552 lines
14 KiB
C++
552 lines
14 KiB
C++
/*
|
|
* robotStateRT.cpp
|
|
*
|
|
* Copyright 2015 Thomas Timm Andersen
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include "robot_state_RT.h"
|
|
|
|
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
|
|
version_ = 0.0;
|
|
time_ = 0.0;
|
|
q_target_.assign(6, 0.0);
|
|
qd_target_.assign(6, 0.0);
|
|
qdd_target_.assign(6, 0.0);
|
|
i_target_.assign(6, 0.0);
|
|
m_target_.assign(6, 0.0);
|
|
q_actual_.assign(6, 0.0);
|
|
qd_actual_.assign(6, 0.0);
|
|
i_actual_.assign(6, 0.0);
|
|
i_control_.assign(6, 0.0);
|
|
tool_vector_actual_.assign(6, 0.0);
|
|
tcp_speed_actual_.assign(6, 0.0);
|
|
tcp_force_.assign(6, 0.0);
|
|
tool_vector_target_.assign(6, 0.0);
|
|
tcp_speed_target_.assign(6, 0.0);
|
|
//digital_input_bits_.assign(64, false);
|
|
digital_input_bits_ = 0;
|
|
motor_temperatures_.assign(6, 0.0);
|
|
controller_timer_ = 0.0;
|
|
robot_mode_ = 0.0;
|
|
joint_modes_.assign(6, 0.0);
|
|
safety_mode_ = 0.0;
|
|
tool_accelerometer_values_.assign(3, 0.0);
|
|
speed_scaling_ = 0.0;
|
|
linear_momentum_norm_ = 0.0;
|
|
v_main_ = 0.0;
|
|
v_robot_ = 0.0;
|
|
i_robot_ = 0.0;
|
|
v_actual_.assign(6, 0.0);
|
|
data_published_ = false;
|
|
controller_updated_ = false;
|
|
pMsg_cond_ = &msg_cond;
|
|
}
|
|
|
|
RobotStateRT::~RobotStateRT() {
|
|
/* Make sure nobody is waiting after this thread is destroyed */
|
|
data_published_ = true;
|
|
controller_updated_ = true;
|
|
pMsg_cond_->notify_all();
|
|
}
|
|
|
|
void RobotStateRT::setDataPublished() {
|
|
data_published_ = false;
|
|
}
|
|
bool RobotStateRT::getDataPublished() {
|
|
return data_published_;
|
|
}
|
|
|
|
void RobotStateRT::setControllerUpdated() {
|
|
controller_updated_ = false;
|
|
}
|
|
bool RobotStateRT::getControllerUpdated() {
|
|
return controller_updated_;
|
|
}
|
|
|
|
double RobotStateRT::ntohd(uint64_t nf) {
|
|
double x;
|
|
nf = be64toh(nf);
|
|
memcpy(&x, &nf, sizeof(x));
|
|
return x;
|
|
}
|
|
|
|
std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
|
|
int nr_of_vals) {
|
|
uint64_t q;
|
|
std::vector<double> ret;
|
|
for (int i = 0; i < nr_of_vals; i++) {
|
|
memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
|
|
ret.push_back(ntohd(q));
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
|
|
//cout << "Data: " << data << endl;
|
|
std::vector<bool> ret;
|
|
for (int i = 0; i < 64; i++) {
|
|
cout << "Pin: " << i << " state: " << ((data & (1 << i)) >> i) << endl;
|
|
ret.push_back((data & (1 << i)) >> i);
|
|
}
|
|
return ret;
|
|
}*/
|
|
|
|
int RobotStateRT::unpackDigitalInputBits(int64_t data){
|
|
int ret;
|
|
ret = ((data & (1 << 31)) >> 31);
|
|
return ret;
|
|
}
|
|
|
|
void RobotStateRT::setVersion(double ver) {
|
|
val_lock_.lock();
|
|
version_ = ver;
|
|
val_lock_.unlock();
|
|
}
|
|
|
|
double RobotStateRT::getVersion() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = version_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getTime() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = time_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getQTarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = q_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getQdTarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = qd_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getQddTarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = qdd_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getITarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = i_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getMTarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = m_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getQActual() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = q_actual_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getQdActual() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = qd_actual_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getIActual() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = i_actual_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getIControl() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = i_control_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getToolVectorActual() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = tool_vector_actual_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getTcpSpeedActual() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = tcp_speed_actual_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getTcpForce() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = tcp_force_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getToolVectorTarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = tool_vector_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getTcpSpeedTarget() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = tcp_speed_target_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<bool> RobotStateRT::getDigitalInputBits() {
|
|
std::vector<bool> ret;
|
|
val_lock_.lock();
|
|
//ret = digital_input_bits_;
|
|
//cout << "Digital Bits: " << digital_input_bits_ << endl;
|
|
switch(digital_input_bits_){ //Pin 0 1 2 3 Binair
|
|
case 2145386496: //1000 0001
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2147483648: //0100 0010
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2148532224: //1100 0011
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2149580800: //1100 0011
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2150105088: //1010 0101
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2150629376: //0110 0110
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2151153664: //1110 0111
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
break;
|
|
case 2151677952: //0001 1000
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2151940096: //1001 1001
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2152202240: //0101 1010
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2152464384: //1101 1011
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2152726528: //0011 1100
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2152988672: //1011 1101
|
|
ret.push_back(true);
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2153250816: //0111 1110
|
|
ret.push_back(false);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
break;
|
|
case 2153512960: //1111 1111
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
ret.push_back(true);
|
|
break;
|
|
default :
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
ret.push_back(false);
|
|
break;
|
|
}
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getMotorTemperatures() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = motor_temperatures_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getControllerTimer() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = controller_timer_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getRobotMode() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = robot_mode_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getJointModes() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = joint_modes_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getSafety_mode() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = safety_mode_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getToolAccelerometerValues() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = tool_accelerometer_values_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getSpeedScaling() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = speed_scaling_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getLinearMomentumNorm() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = linear_momentum_norm_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getVMain() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = v_main_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getVRobot() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = v_robot_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
double RobotStateRT::getIRobot() {
|
|
double ret;
|
|
val_lock_.lock();
|
|
ret = i_robot_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
std::vector<double> RobotStateRT::getVActual() {
|
|
std::vector<double> ret;
|
|
val_lock_.lock();
|
|
ret = v_actual_;
|
|
val_lock_.unlock();
|
|
return ret;
|
|
}
|
|
void RobotStateRT::unpack(uint8_t * buf) {
|
|
//cout << "UNPAAAACKKK" << endl;
|
|
int64_t digital_input_bits;
|
|
uint64_t unpack_to;
|
|
uint16_t offset = 0;
|
|
val_lock_.lock();
|
|
int len;
|
|
memcpy(&len, &buf[offset], sizeof(len));
|
|
|
|
offset += sizeof(len);
|
|
len = ntohl(len);
|
|
//cout << "PACK 1 " << endl;
|
|
//Check the correct message length is received
|
|
bool len_good = true;
|
|
if (version_ >= 1.6 && version_ < 1.7) { //v1.6
|
|
if (len != 756)
|
|
len_good = false;
|
|
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
|
|
if (len != 764)
|
|
len_good = false;
|
|
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
|
|
if (len != 812)
|
|
len_good = false;
|
|
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
|
|
if (len != 1044)
|
|
len_good = false;
|
|
} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
|
|
if (len != 1060)
|
|
len_good = false;
|
|
}
|
|
//cout << "PACK 2 " << endl;
|
|
if (!len_good) {
|
|
printf("Wrong length of message on RT interface: %i\n", len);
|
|
val_lock_.unlock();
|
|
return;
|
|
}
|
|
//cout << "PACK 3 " << endl;
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
time_ = RobotStateRT::ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
q_target_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
qd_target_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
qdd_target_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
i_target_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
m_target_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
q_actual_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
qd_actual_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
i_actual_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
//cout << "PACK 4 " << endl;
|
|
if (version_ <= 1.9) {
|
|
if (version_ > 1.6)
|
|
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
|
|
offset += sizeof(double) * (3 + 15);
|
|
tcp_force_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tool_vector_actual_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tcp_speed_actual_ = unpackVector(buf, offset, 6);
|
|
} else {
|
|
i_control_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tool_vector_actual_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tcp_speed_actual_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tcp_force_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tool_vector_target_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
tcp_speed_target_ = unpackVector(buf, offset, 6);
|
|
}
|
|
offset += sizeof(double) * 6;
|
|
//cout << "PACK 5 " << endl;
|
|
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
|
|
digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
|
|
offset += sizeof(double);
|
|
motor_temperatures_ = unpackVector(buf, offset, 6);
|
|
offset += sizeof(double) * 6;
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
controller_timer_ = ntohd(unpack_to);
|
|
if (version_ > 1.6) {
|
|
offset += sizeof(double) * 2;
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
robot_mode_ = ntohd(unpack_to);
|
|
if (version_ > 1.7) {
|
|
offset += sizeof(double);
|
|
joint_modes_ = unpackVector(buf, offset, 6);
|
|
}
|
|
}
|
|
//cout << "PACK 6 " << endl;
|
|
if (version_ > 1.8) {
|
|
offset += sizeof(double) * 6;
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
safety_mode_ = ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
|
|
offset += sizeof(double) * 3;
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
speed_scaling_ = ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
linear_momentum_norm_ = ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
v_main_ = ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
v_robot_ = ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
i_robot_ = ntohd(unpack_to);
|
|
offset += sizeof(double);
|
|
v_actual_ = unpackVector(buf, offset, 6);
|
|
}
|
|
|
|
//cout << "PACK 7 " << endl;
|
|
val_lock_.unlock();
|
|
controller_updated_ = true;
|
|
data_published_ = true;
|
|
//cout << "PACK 8 " << endl;
|
|
//pMsg_cond_->notify_all();
|
|
//cout << "PACK DONE " << endl;
|
|
|
|
}
|