00001 /* 00002 * robotStateRT.h 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 #ifndef ROBOT_STATE_RT_H_ 00020 #define ROBOT_STATE_RT_H_ 00021 00022 #include <inttypes.h> 00023 #include <vector> 00024 #include <stdlib.h> 00025 #include <string.h> 00026 #include <mutex> 00027 #include <netinet/in.h> 00028 #include <condition_variable> 00029 00030 class RobotStateRT { 00031 private: 00032 double version_; //protocol version 00033 00034 double time_; //Time elapsed since the controller was started 00035 std::vector<double> q_target_; //Target joint positions 00036 std::vector<double> qd_target_; //Target joint velocities 00037 std::vector<double> qdd_target_; //Target joint accelerations 00038 std::vector<double> i_target_; //Target joint currents 00039 std::vector<double> m_target_; //Target joint moments (torques) 00040 std::vector<double> q_actual_; //Actual joint positions 00041 std::vector<double> qd_actual_; //Actual joint velocities 00042 std::vector<double> i_actual_; //Actual joint currents 00043 std::vector<double> i_control_; //Joint control currents 00044 std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation 00045 std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates 00046 std::vector<double> tcp_force_; //Generalised forces in the TC 00047 std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation 00048 std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates 00049 std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high 00050 std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius 00051 double controller_timer_; //Controller realtime thread execution time 00052 double robot_mode_; //Robot mode 00053 std::vector<double> joint_modes_; //Joint control modes 00054 double safety_mode_; //Safety mode 00055 std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) 00056 double speed_scaling_; //Speed scaling of the trajectory limiter 00057 double linear_momentum_norm_; //Norm of Cartesian linear momentum 00058 double v_main_; //Masterboard: Main voltage 00059 double v_robot_; //Matorborad: Robot voltage (48V) 00060 double i_robot_; //Masterboard: Robot current 00061 std::vector<double> v_actual_; //Actual joint voltages 00062 00063 std::mutex val_lock_; // Locks the variables while unpack parses data; 00064 00065 std::condition_variable* pMsg_cond_; //Signals that new vars are available 00066 bool data_published_; //to avoid spurious wakes 00067 bool controller_updated_; //to avoid spurious wakes 00068 00069 std::vector<double> unpackVector(uint8_t * buf, int start_index, 00070 int nr_of_vals); 00071 std::vector<bool> unpackDigitalInputBits(int64_t data); 00072 double ntohd(uint64_t nf); 00073 00074 public: 00075 RobotStateRT(std::condition_variable& msg_cond); 00076 ~RobotStateRT(); 00077 double getVersion(); 00078 double getTime(); 00079 std::vector<double> getQTarget(); 00080 std::vector<double> getQdTarget(); 00081 std::vector<double> getQddTarget(); 00082 std::vector<double> getITarget(); 00083 std::vector<double> getMTarget(); 00084 std::vector<double> getQActual(); 00085 std::vector<double> getQdActual(); 00086 std::vector<double> getIActual(); 00087 std::vector<double> getIControl(); 00088 std::vector<double> getToolVectorActual(); 00089 std::vector<double> getTcpSpeedActual(); 00090 std::vector<double> getTcpForce(); 00091 std::vector<double> getToolVectorTarget(); 00092 std::vector<double> getTcpSpeedTarget(); 00093 std::vector<bool> getDigitalInputBits(); 00094 std::vector<double> getMotorTemperatures(); 00095 double getControllerTimer(); 00096 double getRobotMode(); 00097 std::vector<double> getJointModes(); 00098 double getSafety_mode(); 00099 std::vector<double> getToolAccelerometerValues(); 00100 double getSpeedScaling(); 00101 double getLinearMomentumNorm(); 00102 double getVMain(); 00103 double getVRobot(); 00104 double getIRobot(); 00105 00106 void setVersion(double ver); 00107 00108 void setDataPublished(); 00109 bool getDataPublished(); 00110 bool getControllerUpdated(); 00111 void setControllerUpdated(); 00112 std::vector<double> getVActual(); 00113 void unpack(uint8_t * buf); 00114 }; 00115 00116 #endif /* ROBOT_STATE_RT_H_ */