ur_modern_driver: robot_state

Go to the documentation of this file.
00001 /*
00002  * robotStateRT.cpp
00003  *
00004  * Copyright 2015 Thomas Timm Andersen
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  *     http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #include "ur_modern_driver/robot_state_RT.h"
00020 
00021 RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
00022         version_ = 0.0;
00023         time_ = 0.0;
00024         q_target_.assign(6, 0.0);
00025         qd_target_.assign(6, 0.0);
00026         qdd_target_.assign(6, 0.0);
00027         i_target_.assign(6, 0.0);
00028         m_target_.assign(6, 0.0);
00029         q_actual_.assign(6, 0.0);
00030         qd_actual_.assign(6, 0.0);
00031         i_actual_.assign(6, 0.0);
00032         i_control_.assign(6, 0.0);
00033         tool_vector_actual_.assign(6, 0.0);
00034         tcp_speed_actual_.assign(6, 0.0);
00035         tcp_force_.assign(6, 0.0);
00036         tool_vector_target_.assign(6, 0.0);
00037         tcp_speed_target_.assign(6, 0.0);
00038         digital_input_bits_.assign(64, false);
00039         motor_temperatures_.assign(6, 0.0);
00040         controller_timer_ = 0.0;
00041         robot_mode_ = 0.0;
00042         joint_modes_.assign(6, 0.0);
00043         safety_mode_ = 0.0;
00044         tool_accelerometer_values_.assign(3, 0.0);
00045         speed_scaling_ = 0.0;
00046         linear_momentum_norm_ = 0.0;
00047         v_main_ = 0.0;
00048         v_robot_ = 0.0;
00049         i_robot_ = 0.0;
00050         v_actual_.assign(6, 0.0);
00051         data_published_ = false;
00052         controller_updated_ = false;
00053         pMsg_cond_ = &msg_cond;
00054 }
00055 
00056 RobotStateRT::~RobotStateRT() {
00057         /* Make sure nobody is waiting after this thread is destroyed */
00058         data_published_ = true;
00059         controller_updated_ = true;
00060         pMsg_cond_->notify_all();
00061 }
00062 
00063 void RobotStateRT::setDataPublished() {
00064         data_published_ = false;
00065 }
00066 bool RobotStateRT::getDataPublished() {
00067         return data_published_;
00068 }
00069 
00070 void RobotStateRT::setControllerUpdated() {
00071         controller_updated_ = false;
00072 }
00073 bool RobotStateRT::getControllerUpdated() {
00074         return controller_updated_;
00075 }
00076 
00077 double RobotStateRT::ntohd(uint64_t nf) {
00078         double x;
00079         nf = be64toh(nf);
00080         memcpy(&x, &nf, sizeof(x));
00081         return x;
00082 }
00083 
00084 std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
00085                 int nr_of_vals) {
00086         uint64_t q;
00087         std::vector<double> ret;
00088         for (int i = 0; i < nr_of_vals; i++) {
00089                 memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
00090                 ret.push_back(ntohd(q));
00091         }
00092         return ret;
00093 }
00094 
00095 std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
00096         std::vector<bool> ret;
00097         for (int i = 0; i < 64; i++) {
00098                 ret.push_back((data & (1 << i)) >> i);
00099         }
00100         return ret;
00101 }
00102 
00103 void RobotStateRT::setVersion(double ver) {
00104         val_lock_.lock();
00105         version_ = ver;
00106         val_lock_.unlock();
00107 }
00108 
00109 double RobotStateRT::getVersion() {
00110         double ret;
00111         val_lock_.lock();
00112         ret = version_;
00113         val_lock_.unlock();
00114         return ret;
00115 }
00116 double RobotStateRT::getTime() {
00117         double ret;
00118         val_lock_.lock();
00119         ret = time_;
00120         val_lock_.unlock();
00121         return ret;
00122 }
00123 std::vector<double> RobotStateRT::getQTarget() {
00124         std::vector<double> ret;
00125         val_lock_.lock();
00126         ret = q_target_;
00127         val_lock_.unlock();
00128         return ret;
00129 }
00130 std::vector<double> RobotStateRT::getQdTarget() {
00131         std::vector<double> ret;
00132         val_lock_.lock();
00133         ret = qd_target_;
00134         val_lock_.unlock();
00135         return ret;
00136 }
00137 std::vector<double> RobotStateRT::getQddTarget() {
00138         std::vector<double> ret;
00139         val_lock_.lock();
00140         ret = qdd_target_;
00141         val_lock_.unlock();
00142         return ret;
00143 }
00144 std::vector<double> RobotStateRT::getITarget() {
00145         std::vector<double> ret;
00146         val_lock_.lock();
00147         ret = i_target_;
00148         val_lock_.unlock();
00149         return ret;
00150 }
00151 std::vector<double> RobotStateRT::getMTarget() {
00152         std::vector<double> ret;
00153         val_lock_.lock();
00154         ret = m_target_;
00155         val_lock_.unlock();
00156         return ret;
00157 }
00158 std::vector<double> RobotStateRT::getQActual() {
00159         std::vector<double> ret;
00160         val_lock_.lock();
00161         ret = q_actual_;
00162         val_lock_.unlock();
00163         return ret;
00164 }
00165 std::vector<double> RobotStateRT::getQdActual() {
00166         std::vector<double> ret;
00167         val_lock_.lock();
00168         ret = qd_actual_;
00169         val_lock_.unlock();
00170         return ret;
00171 }
00172 std::vector<double> RobotStateRT::getIActual() {
00173         std::vector<double> ret;
00174         val_lock_.lock();
00175         ret = i_actual_;
00176         val_lock_.unlock();
00177         return ret;
00178 }
00179 std::vector<double> RobotStateRT::getIControl() {
00180         std::vector<double> ret;
00181         val_lock_.lock();
00182         ret = i_control_;
00183         val_lock_.unlock();
00184         return ret;
00185 }
00186 std::vector<double> RobotStateRT::getToolVectorActual() {
00187         std::vector<double> ret;
00188         val_lock_.lock();
00189         ret = tool_vector_actual_;
00190         val_lock_.unlock();
00191         return ret;
00192 }
00193 std::vector<double> RobotStateRT::getTcpSpeedActual() {
00194         std::vector<double> ret;
00195         val_lock_.lock();
00196         ret = tcp_speed_actual_;
00197         val_lock_.unlock();
00198         return ret;
00199 }
00200 std::vector<double> RobotStateRT::getTcpForce() {
00201         std::vector<double> ret;
00202         val_lock_.lock();
00203         ret = tcp_force_;
00204         val_lock_.unlock();
00205         return ret;
00206 }
00207 std::vector<double> RobotStateRT::getToolVectorTarget() {
00208         std::vector<double> ret;
00209         val_lock_.lock();
00210         ret = tool_vector_target_;
00211         val_lock_.unlock();
00212         return ret;
00213 }
00214 std::vector<double> RobotStateRT::getTcpSpeedTarget() {
00215         std::vector<double> ret;
00216         val_lock_.lock();
00217         ret = tcp_speed_target_;
00218         val_lock_.unlock();
00219         return ret;
00220 }
00221 std::vector<bool> RobotStateRT::getDigitalInputBits() {
00222         std::vector<bool> ret;
00223         val_lock_.lock();
00224         ret = digital_input_bits_;
00225         val_lock_.unlock();
00226         return ret;
00227 }
00228 std::vector<double> RobotStateRT::getMotorTemperatures() {
00229         std::vector<double> ret;
00230         val_lock_.lock();
00231         ret = motor_temperatures_;
00232         val_lock_.unlock();
00233         return ret;
00234 }
00235 double RobotStateRT::getControllerTimer() {
00236         double ret;
00237         val_lock_.lock();
00238         ret = controller_timer_;
00239         val_lock_.unlock();
00240         return ret;
00241 }
00242 double RobotStateRT::getRobotMode() {
00243         double ret;
00244         val_lock_.lock();
00245         ret = robot_mode_;
00246         val_lock_.unlock();
00247         return ret;
00248 }
00249 std::vector<double> RobotStateRT::getJointModes() {
00250         std::vector<double> ret;
00251         val_lock_.lock();
00252         ret = joint_modes_;
00253         val_lock_.unlock();
00254         return ret;
00255 }
00256 double RobotStateRT::getSafety_mode() {
00257         double ret;
00258         val_lock_.lock();
00259         ret = safety_mode_;
00260         val_lock_.unlock();
00261         return ret;
00262 }
00263 std::vector<double> RobotStateRT::getToolAccelerometerValues() {
00264         std::vector<double> ret;
00265         val_lock_.lock();
00266         ret = tool_accelerometer_values_;
00267         val_lock_.unlock();
00268         return ret;
00269 }
00270 double RobotStateRT::getSpeedScaling() {
00271         double ret;
00272         val_lock_.lock();
00273         ret = speed_scaling_;
00274         val_lock_.unlock();
00275         return ret;
00276 }
00277 double RobotStateRT::getLinearMomentumNorm() {
00278         double ret;
00279         val_lock_.lock();
00280         ret = linear_momentum_norm_;
00281         val_lock_.unlock();
00282         return ret;
00283 }
00284 double RobotStateRT::getVMain() {
00285         double ret;
00286         val_lock_.lock();
00287         ret = v_main_;
00288         val_lock_.unlock();
00289         return ret;
00290 }
00291 double RobotStateRT::getVRobot() {
00292         double ret;
00293         val_lock_.lock();
00294         ret = v_robot_;
00295         val_lock_.unlock();
00296         return ret;
00297 }
00298 double RobotStateRT::getIRobot() {
00299         double ret;
00300         val_lock_.lock();
00301         ret = i_robot_;
00302         val_lock_.unlock();
00303         return ret;
00304 }
00305 std::vector<double> RobotStateRT::getVActual() {
00306         std::vector<double> ret;
00307         val_lock_.lock();
00308         ret = v_actual_;
00309         val_lock_.unlock();
00310         return ret;
00311 }
00312 void RobotStateRT::unpack(uint8_t * buf) {
00313         int64_t digital_input_bits;
00314         uint64_t unpack_to;
00315         uint16_t offset = 0;
00316         val_lock_.lock();
00317         int len;
00318         memcpy(&len, &buf[offset], sizeof(len));
00319 
00320         offset += sizeof(len);
00321         len = ntohl(len);
00322 
00323         //Check the correct message length is received
00324         bool len_good = true;
00325         if (version_ >= 1.6 && version_ < 1.7) { //v1.6
00326                 if (len != 756)
00327                         len_good = false;
00328         } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
00329                 if (len != 764)
00330                         len_good = false;
00331         } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
00332                 if (len != 812)
00333                         len_good = false;
00334         } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
00335                 if (len != 1044)
00336                         len_good = false;
00337         } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
00338                 if (len != 1060)
00339                         len_good = false;
00340         }
00341 
00342         if (!len_good) {
00343                 printf("Wrong length of message on RT interface: %i\n", len);
00344                 val_lock_.unlock();
00345                 return;
00346         }
00347 
00348         memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00349         time_ = RobotStateRT::ntohd(unpack_to);
00350         offset += sizeof(double);
00351         q_target_ = unpackVector(buf, offset, 6);
00352         offset += sizeof(double) * 6;
00353         qd_target_ = unpackVector(buf, offset, 6);
00354         offset += sizeof(double) * 6;
00355         qdd_target_ = unpackVector(buf, offset, 6);
00356         offset += sizeof(double) * 6;
00357         i_target_ = unpackVector(buf, offset, 6);
00358         offset += sizeof(double) * 6;
00359         m_target_ = unpackVector(buf, offset, 6);
00360         offset += sizeof(double) * 6;
00361         q_actual_ = unpackVector(buf, offset, 6);
00362         offset += sizeof(double) * 6;
00363         qd_actual_ = unpackVector(buf, offset, 6);
00364         offset += sizeof(double) * 6;
00365         i_actual_ = unpackVector(buf, offset, 6);
00366         offset += sizeof(double) * 6;
00367         if (version_ <= 1.9) {
00368                 if (version_ > 1.6)
00369                         tool_accelerometer_values_ = unpackVector(buf, offset, 3);
00370                 offset += sizeof(double) * (3 + 15);
00371                 tcp_force_ = unpackVector(buf, offset, 6);
00372                 offset += sizeof(double) * 6;
00373                 tool_vector_actual_ = unpackVector(buf, offset, 6);
00374                 offset += sizeof(double) * 6;
00375                 tcp_speed_actual_ = unpackVector(buf, offset, 6);
00376         } else {
00377                 i_control_ = unpackVector(buf, offset, 6);
00378                 offset += sizeof(double) * 6;
00379                 tool_vector_actual_ = unpackVector(buf, offset, 6);
00380                 offset += sizeof(double) * 6;
00381                 tcp_speed_actual_ = unpackVector(buf, offset, 6);
00382                 offset += sizeof(double) * 6;
00383                 tcp_force_ = unpackVector(buf, offset, 6);
00384                 offset += sizeof(double) * 6;
00385                 tool_vector_target_ = unpackVector(buf, offset, 6);
00386                 offset += sizeof(double) * 6;
00387                 tcp_speed_target_ = unpackVector(buf, offset, 6);
00388         }
00389         offset += sizeof(double) * 6;
00390 
00391         memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
00392         digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
00393         offset += sizeof(double);
00394         motor_temperatures_ = unpackVector(buf, offset, 6);
00395         offset += sizeof(double) * 6;
00396         memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00397         controller_timer_ = ntohd(unpack_to);
00398         if (version_ > 1.6) {
00399                 offset += sizeof(double) * 2;
00400                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00401                 robot_mode_ = ntohd(unpack_to);
00402                 if (version_ > 1.7) {
00403                         offset += sizeof(double);
00404                         joint_modes_ = unpackVector(buf, offset, 6);
00405                 }
00406         }
00407         if (version_ > 1.8) {
00408                 offset += sizeof(double) * 6;
00409                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00410                 safety_mode_ = ntohd(unpack_to);
00411                 offset += sizeof(double);
00412                 tool_accelerometer_values_ = unpackVector(buf, offset, 3);
00413                 offset += sizeof(double) * 3;
00414                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00415                 speed_scaling_ = ntohd(unpack_to);
00416                 offset += sizeof(double);
00417                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00418                 linear_momentum_norm_ = ntohd(unpack_to);
00419                 offset += sizeof(double);
00420                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00421                 v_main_ = ntohd(unpack_to);
00422                 offset += sizeof(double);
00423                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00424                 v_robot_ = ntohd(unpack_to);
00425                 offset += sizeof(double);
00426                 memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
00427                 i_robot_ = ntohd(unpack_to);
00428                 offset += sizeof(double);
00429                 v_actual_ = unpackVector(buf, offset, 6);
00430         }
00431         val_lock_.unlock();
00432         controller_updated_ = true;
00433         data_published_ = true;
00434         pMsg_cond_->notify_all();
00435 
00436 }
00437