include/open_manipulator_i2c.h

   1 #include <chrono>
   2 #include <unistd.h>
   3 #include <sys/types.h>
   4 #include <sys/ioctl.h>
   5 #include <sys/stat.h>
   6 #include <fcntl.h>
   7 #include <mutex>
   8 #include <linux/i2c-dev.h>
   9 #include <hardware_interface/joint_command_interface.h>
  10 #include <hardware_interface/joint_state_interface.h>
  11 #include <hardware_interface/robot_hw.h>
  12 #include "std_srvs/SetBool.h"
  13 #include "wt_open_manipulator/GetError.h"
  14
  15 #define I2C_FILE "/dev/i2c-2"
  16 #define I2C_ADDR (0x60>>1)
  17 #define XM_OPERATING_MODE 11
  18 #define XM_HOMING_OFFSET 20
  19 #define XM_CURRENT_LIMIT 38
  20 #define XM_SHUTDOWN 63
  21 #define XM_TORQUE_ENABLE 64
  22 #define XM_HARDWARE_ERROR_STATUS 70
  23 #define XM_GOAL_CURRENT 102
  24 #define XM_PROFILE_ACCELERATION 108
  25 #define XM_PROFILE_VELOCITY 112
  26 #define XM_GOAL_POSITION 116
  27 #define XM_PRESENT_POSITION 132
  28 #define XM_PRESENT_CURRENT 126
  29 #define XM_PRESENT_VELOCITY 128
  30 #define CMD_EOF 0xaa
  31 #define STEPS_TO_RADIAN (2*M_PI/4096)
  32 #define GRIPPER_RADIAN_TO_METER -0.015
  33 #define VELOCITY_STEPS_TO_RADS (0.229*2*M_PI/60)
  34
  35 class WtOpenManipulatorI2C : public hardware_interface::RobotHW
  36 {
  37         public:
  38                 WtOpenManipulatorI2C(ros::NodeHandle nh) {
  39                         uint8_t mode=255;
  40                         int32_t homing_offset=0;
  41                         this->nh = nh;
  42                         torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this);
  43                         get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this);
  44
  45                         // connect and register the joint state interface
  46                         hardware_interface::JointStateHandle state_handle_1("joint1", &pos[0], &vel[0], &eff[0]);
  47                         jnt_state_interface.registerHandle(state_handle_1);
  48                         hardware_interface::JointStateHandle state_handle_2("joint2", &pos[1], &vel[1], &eff[1]);
  49                         jnt_state_interface.registerHandle(state_handle_2);
  50                         hardware_interface::JointStateHandle state_handle_3("joint3", &pos[2], &vel[2], &eff[2]);
  51                         jnt_state_interface.registerHandle(state_handle_3);
  52                         hardware_interface::JointStateHandle state_handle_4("joint4", &pos[3], &vel[3], &eff[3]);
  53                         jnt_state_interface.registerHandle(state_handle_4);
  54                         hardware_interface::JointStateHandle state_handle_gripper("gripper", &pos[4], &vel[4], &eff[4]);
  55                         jnt_state_interface.registerHandle(state_handle_gripper);
  56                         registerInterface(&jnt_state_interface);
  57
  58                         // connect and register the joint position interface
  59                         hardware_interface::JointHandle pos_handle_1(state_handle_1, &cmd[0]);
  60                         jnt_pos_interface.registerHandle(pos_handle_1);
  61                         hardware_interface::JointHandle pos_handle_2(state_handle_2, &cmd[1]);
  62                         jnt_pos_interface.registerHandle(pos_handle_2);
  63                         hardware_interface::JointHandle pos_handle_3(state_handle_3, &cmd[2]);
  64                         jnt_pos_interface.registerHandle(pos_handle_3);
  65                         hardware_interface::JointHandle pos_handle_4(state_handle_4, &cmd[3]);
  66                         jnt_pos_interface.registerHandle(pos_handle_4);
  67                         hardware_interface::JointHandle pos_handle_gripper(state_handle_gripper, &cmd[4]);
  68                         jnt_pos_interface.registerHandle(pos_handle_gripper);
  69                         registerInterface(&jnt_pos_interface);
  70
  71                         // Settings
  72                         if (dynamixel_readdword(11, XM_HOMING_OFFSET, &homing_offset) != 1) {
  73                                 ROS_ERROR("I2C dynamixel id=11 read homing offset error");
  74                                 exit(1);
  75                         }
  76                         if (homing_offset != 64) {
  77                                 if (dynamixel_writedword(11, XM_HOMING_OFFSET, 64) != 1) {
  78                                         ROS_ERROR("I2C dynamixel id=11 write homing offset error");
  79                                         exit(1);
  80                                 }
  81                         }
  82                         if (dynamixel_writeword(15, XM_GOAL_CURRENT, 600) != 1) { // 0..1193, 2.69mA/value
  83                                 ROS_ERROR("I2C dynamixel id=15 write goal current error");
  84                                 exit(1);
  85                         }
  86                         if (dynamixel_readbyte(15, XM_OPERATING_MODE, &mode) != 1) {
  87                                 ROS_ERROR("I2C dynamixel id=15 read operating Mode error");
  88                                 exit(1);
  89                         }
  90                         if (mode != 5) {
  91                                 if (!torque_enable(false)) {
  92                                         exit(1);
  93                                 }
  94                                 if (dynamixel_writebyte(15, XM_OPERATING_MODE, 5) != 1) { // Current-based Position Control Mode
  95                                         ROS_ERROR("I2C dynamixel id=15 write operating Mode error");
  96                                         exit(1);
  97                                 }
  98                         }
  99                         if (dynamixel_writedword(15, XM_PROFILE_ACCELERATION, 20) != 1) {
 100                                 ROS_ERROR("I2C dynamixel id=15 write profile Acceleration error");
 101                                 exit(1);
 102                         }
 103                         if (dynamixel_writedword(15, XM_PROFILE_VELOCITY, 200) != 1) {
 104                                 ROS_ERROR("I2C dynamixel id=15 write profile Velocity error");
 105                                 exit(1);
 106                         }
 107                         if (!torque_enable(true)) {
 108                                 exit(1);
 109                         }
 110                 }
 111
 112                 // Converts pos_encoder from wheels to wheel angles
 113                 void read(ros::Duration period) {
 114                         int32_t value;
 115                         int16_t current;
 116                         auto t1 = std::chrono::high_resolution_clock::now();
 117                         int num_error = 0;
 118
 119                         // arm
 120                         for(int i=0, id=11; id<=14; id++, i++) {
 121                                 if (ignore_ids.count(i)  > 0) {
 122                                         ROS_ERROR_THROTTLE(10, "Ignoring dynamixel %d", id);
 123                                         continue;
 124                                 }
 125
 126                                 if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) == 1) {
 127                                         pos[i] = STEPS_TO_RADIAN*(value-2048);
 128                                 } else {
 129                                         ROS_ERROR("I2C dynamixel id=%d position read error", id);
 130                                         num_error++;
 131                                 }
 132
 133                                 if (dynamixel_readdword(id, XM_PRESENT_VELOCITY, &value) == 1) {
 134                                         vel[i] = value*VELOCITY_STEPS_TO_RADS;
 135                                 } else {
 136                                         ROS_ERROR("I2C dynamixel id=%d velocity read error", id);
 137                                         num_error++;
 138                                 }
 139
 140 #ifdef READ_EFFORT
 141                                 if (dynamixel_readword(id, XM_PRESENT_CURRENT, &current) == 1) {
 142                                         eff[i] = current*2.69e-3;
 143                                 } else {
 144                                         ROS_ERROR("I2C dynamixel id=%d current read error", id);
 145                                         num_error++;
 146                                 }
 147 #endif
 148
 149                                 if (num_error >= 3) {
 150                                         ignore_ids.insert(i);
 151                                 }
 152                         }
 153
 154                         // gripper
 155                         if (dynamixel_readdword(15, XM_PRESENT_POSITION, &value) == 1) {
 156                                 pos[4] = STEPS_TO_RADIAN*(value-2048)*GRIPPER_RADIAN_TO_METER;
 157                         } else {
 158                                 ROS_ERROR("I2C dynamixel id=15 position read error");
 159                         }
 160                         if (dynamixel_readdword(15, XM_PRESENT_VELOCITY, &value) == 1) {
 161                                 vel[4] = value*VELOCITY_STEPS_TO_RADS*GRIPPER_RADIAN_TO_METER;
 162                         } else {
 163                                 ROS_ERROR("I2C dynamixel id=15 velocity read error");
 164                         }
 165 #ifdef READ_EFFORT
 166                         if (dynamixel_readword(15, XM_PRESENT_CURRENT, &current) == 1) {
 167                                 eff[4] = current*2.69e-3;
 168                         } else {
 169                                 ROS_ERROR("I2C dynamixel id=15 current read error");
 170                         }
 171 #endif
 172
 173                         ROS_DEBUG("pos: %.2f %.2f %.2f %.2f %.2f", pos[0]*180/M_PI, pos[1]*180/M_PI, pos[2]*180/M_PI, pos[3]*180/M_PI, pos[4]);
 174                         ROS_DEBUG("velocity: %.2f %.2f %.2f %.2f %.2f", vel[0]*30/M_PI, vel[1]*30/M_PI, vel[2]*30/M_PI, vel[3]*30/M_PI, vel[4]);
 175 #ifdef READ_EFFORT
 176                         ROS_DEBUG("current: %.2f %.2f %.2f %.2f %.2f", eff[0], eff[1], eff[2], eff[3], eff[4]);
 177 #endif
 178                         auto t2 = std::chrono::high_resolution_clock::now();
 179                         int64_t duration = std::chrono::duration_cast<std::chrono::milliseconds>( t2 - t1 ).count();
 180                         ROS_DEBUG("read duration: %lldms", duration);
 181                 }
 182
 183                 // Writes current velocity command to hardware
 184                 void write() {
 185                         int32_t value;
 186
 187                         if (memcmp(cmd, cmd_pre, sizeof(cmd)) != 0) { // Only write new position when changed
 188                                 ROS_DEBUG("cmd: %.2f %.2f %.2f %.2f %.2f", cmd[0]*180/M_PI, cmd[1]*180/M_PI, cmd[2]*180/M_PI, cmd[3]*180/M_PI, cmd[4]);
 189
 190                                 // arm
 191                                 for(int i=0, id=11; id<=14; id++, i++) {
 192                                         if (isnan(cmd[i])) {
 193                                                 ROS_WARN("Desired angle for id=%d is NaN, skipping write.", id);
 194                                                 continue;
 195                                         }
 196
 197                                         value = cmd[i]/STEPS_TO_RADIAN + 2048;
 198
 199                                         if (dynamixel_writedword(id, XM_GOAL_POSITION, value) == 1) {
 200                                                 cmd_pre[i] = cmd[i];
 201                                         } else {
 202                                                 ROS_ERROR("I2C dynamixel id=%d write error", id);
 203                                         }
 204                                 }
 205
 206                                 // gripper
 207                                 if (isnan(cmd[4]) || cmd[4] < -0.010 || cmd[4] > 0.019) {
 208                                         ROS_WARN("Desired angle for id=15 is NaN or outside joint limit, skipping write.");
 209                                 } else {
 210                                         value = cmd[4]/GRIPPER_RADIAN_TO_METER/STEPS_TO_RADIAN + 2048;
 211                                         if (dynamixel_writedword(15, XM_GOAL_POSITION, value) == 1) {
 212                                                 cmd_pre[4] = cmd[4];
 213                                         } else {
 214                                                 ROS_ERROR("I2C dynamixel id=15 write error");
 215                                         }
 216                                 }
 217                         }
 218                 }
 219
 220                 bool torque_enable(bool enable) {
 221                         bool ret = true;
 222                         for(int id=11; id<=15; id++) {
 223                                 if (dynamixel_writebyte(id, XM_TORQUE_ENABLE, enable) != 1) {
 224                                         ROS_ERROR("I2C dynamixel id=%d torque change", id);
 225                                         ret = false;
 226                                 }
 227                         }
 228                         return ret;
 229                 }
 230
 231         private:
 232                 hardware_interface::JointStateInterface jnt_state_interface;
 233                 hardware_interface::PositionJointInterface jnt_pos_interface;
 234                 ros::NodeHandle nh;
 235                 double cmd[5] = {NAN, NAN, NAN, NAN, NAN};
 236                 double cmd_pre[5] = {NAN, NAN, NAN, NAN, NAN}; // determine if cmd changed
 237                 double pos[5] = {0, 0, 0, 0, 0};
 238                 double vel[5] = {0, 0, 0, 0, 0};
 239                 double eff[5] = {0, 0, 0, 0, 0};
 240                 ros::ServiceServer torque_enable_service, get_error_service;
 241                 std::mutex comm_mutex;
 242                 std::set<int> ignore_ids;
 243
 244                 int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
 245                         int file;
 246                         int ret;
 247                         uint8_t read_buf[read_num+1]; // for unknown reason need to read one more byte with USI Slave
 248
 249                         comm_mutex.lock();
 250                         if ((file = open(I2C_FILE, O_RDWR)) < 0) {
 251                                 perror("open");
 252                                 goto error;
 253                         }
 254
 255                         if (ioctl(file, I2C_SLAVE, I2C_ADDR) < 0) {
 256                                 perror("ioctl");
 257                                 goto error;
 258                         }
 259
 260                         if ((ret = ::write(file, write_data, write_num)) != write_num) {
 261                                 perror("i2c write");
 262                                 goto error;
 263                         }
 264
 265                         // wait at least the half duplex transmission time, assume 29 (=14+15) bytes send/received: 1/(115200.0/10/29) <= 2.6ms
 266                         //usleep(3.2*1e3);
 267                         if (read_num > 0) {
 268                                 if ((ret = ::read(file, read_buf, read_num+1)) != read_num+1) {
 269                                         perror("i2c read");
 270                                         goto error;
 271                                 }
 272                                 ret--; // remove extra byte
 273                                 memcpy(read_data, read_buf, ret);
 274                         }
 275
 276                         error:
 277                                 close(file);
 278                                 comm_mutex.unlock();
 279                                 return ret;
 280                 }
 281
 282                 uint8_t dynamixel_writebyte(uint8_t id, uint8_t cmd, uint8_t value) {
 283                         uint8_t write_data[5] = {id, cmd, 1, value, CMD_EOF};
 284                         uint8_t read_data[2];
 285                         int ret = dynamixel_write(write_data, 5, read_data, 2);
 286                         if (ret == 2 && read_data[1] == id) {
 287                                 return read_data[0];
 288                         }
 289                         return 255;
 290                 }
 291
 292                 uint8_t dynamixel_writeword(uint8_t id, uint8_t cmd, int16_t value) {
 293                         uint8_t write_data[6] = {id, cmd, 2, (uint8_t)(value>>8), (uint8_t)value, CMD_EOF};
 294                         uint8_t read_data[2];
 295                         int ret = dynamixel_write(write_data, 6, read_data, 2);
 296                         if (ret == 2 && read_data[1] == id) {
 297                                 return read_data[0];
 298                         }
 299                         return 255;
 300                 }
 301
 302                 uint8_t dynamixel_writedword(uint8_t id, uint8_t cmd, int32_t value) {
 303                         uint8_t write_data[8] = {id, cmd, 4, (uint8_t)(value>>24), (uint8_t)(value>>16), (uint8_t)(value>>8), (uint8_t)value, CMD_EOF};
 304                         uint8_t read_data[2];
 305                         int ret = dynamixel_write(write_data, 8, read_data, 2);
 306                         if (ret == 2 && read_data[1] == id) {
 307                                 return read_data[0];
 308                         }
 309                         return 255;
 310                 }
 311
 312                 uint8_t dynamixel_readbyte(uint8_t id, uint8_t cmd, uint8_t *value) {
 313                         uint8_t write_data[4] = {id, cmd, 0x11, CMD_EOF};
 314                         uint8_t read_data[3];
 315                         int ret = dynamixel_write(write_data, 4, read_data, 3);
 316                         if (ret == 3 && read_data[1] == id) {
 317                                 *value = read_data[2];
 318                                 return read_data[0];
 319                         }
 320                         if (read_data[0] != 1) {
 321                                 ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
 322                         }
 323                         return 255;
 324                 }
 325
 326                 uint8_t dynamixel_readword(uint8_t id, uint8_t cmd, int16_t *value) {
 327                         uint8_t write_data[4] = {id, cmd, 0x12, CMD_EOF};
 328                         uint8_t read_data[4];
 329                         int ret = dynamixel_write(write_data, 4, read_data, 4);
 330                         if (ret == 4 && read_data[1] == id) {
 331                                 *value = (read_data[2]<<8) | read_data[3];
 332                                 return read_data[0];
 333                         }
 334                         return 255;
 335                 }
 336
 337                 uint8_t dynamixel_readdword(uint8_t id, uint8_t cmd, int32_t *value) {
 338                         uint8_t write_data[4] = {id, cmd, 0x14, CMD_EOF};
 339                         uint8_t read_data[6];
 340                         int ret = dynamixel_write(write_data, 4, read_data, 6);
 341                         if (ret == 6 && read_data[0] == 1 && read_data[1] == id) {
 342                                 *value = (read_data[2]<<24) | (read_data[3]<<16) | (read_data[4]<<8) | read_data[5];
 343                                 return read_data[0];
 344                         }
 345                         if (read_data[0] != 1) {
 346                                 ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
 347                         }
 348                         return 255;
 349                 }
 350
 351                 bool cbTorqueEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
 352                         ROS_INFO("Setting torque enable=%d", req.data);
 353                         res.success = torque_enable(req.data);
 354                         return true;
 355                 }
 356
 357                 bool cbGetError(wt_open_manipulator::GetError::Request &req, wt_open_manipulator::GetError::Response &res) {
 358                         ROS_INFO("Reading Hardware Error Status");
 359
 360                         for(int id=11, i=0; id<=15; id++, i++) {
 361                                 wt_open_manipulator::HardwareError hw_error;
 362                                 uint8_t value;
 363                                 if (dynamixel_readbyte(id, XM_HARDWARE_ERROR_STATUS, &value) != 1) {
 364                                         ROS_ERROR("I2C dynamixel id=%d reading hardware error status", id);
 365                                 }
 366                                 hw_error.id = id;
 367                                 hw_error.inputVoltage = value & (1<<0);
 368                                 hw_error.overHeating = value & (1<<2);
 369                                 hw_error.motorEncoder = value & (1<<3);
 370                                 hw_error.electricalShock = value & (1<<4);
 371                                 hw_error.overload = value & (1<<5);
 372                                 res.errors.push_back(hw_error);
 373                         }
 374
 375                         return true;
 376                 }
 377 };