include/open_manipulator_i2c.h

   1 #include <unistd.h>
   2 #include <sys/types.h>
   3 #include <sys/ioctl.h>
   4 #include <sys/stat.h>
   5 #include <fcntl.h>
   6 #include <mutex>
   7 #include <linux/i2c-dev.h>
   8 #include <hardware_interface/joint_command_interface.h>
   9 #include <hardware_interface/joint_state_interface.h>
  10 #include <hardware_interface/robot_hw.h>
  11 #include "std_srvs/SetBool.h"
  12
  13 /* http://wiki.ros.org/joint_trajectory_controller?distro=melodic
  14  *
  15  * arm:
  16  * - Gripper Goal Current(102)=200, Operating Mode(11)=Current-based Position Control Mode, Profile_Acceleration=20, Profile_Velocity=200
  17  * - Read: Present Current(126), Present Velocity(128), Present Position(132)
  18  * - Gripper joint limits:
  19  *             0.010,      // max gripper limit (0.01 m)
  20  *             -0.010,     // min gripper limit (-0.01 m)
  21  *             -0.015,     // Change unit from `meter` to `radian`
  22  */
  23
  24 #define I2C_FILE "/dev/i2c-2"
  25 #define I2C_ADDR (0x60>>1)
  26 #define XM_TORQUE_ENABLE 64
  27 #define XM_GOAL_POSITION 116
  28 #define XM_PRESENT_POSITION 132
  29 #define CMD_EOF 0xaa
  30
  31 class WtOpenManipulatorI2C : public hardware_interface::RobotHW
  32 {
  33         public:
  34                 WtOpenManipulatorI2C(ros::NodeHandle nh) {
  35                         this->nh = nh;
  36                         torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this);
  37
  38                         // connect and register the joint state interface
  39                         hardware_interface::JointStateHandle state_handle_1("joint1", &pos[0], &vel[0], &eff[0]);
  40                         jnt_state_interface.registerHandle(state_handle_1);
  41                         hardware_interface::JointStateHandle state_handle_2("joint2", &pos[1], &vel[1], &eff[1]);
  42                         jnt_state_interface.registerHandle(state_handle_2);
  43                         hardware_interface::JointStateHandle state_handle_3("joint3", &pos[2], &vel[2], &eff[2]);
  44                         jnt_state_interface.registerHandle(state_handle_3);
  45                         hardware_interface::JointStateHandle state_handle_4("joint4", &pos[3], &vel[3], &eff[3]);
  46                         jnt_state_interface.registerHandle(state_handle_4);
  47                         registerInterface(&jnt_state_interface);
  48
  49                         // connect and register the joint position interface
  50                         hardware_interface::JointHandle pos_handle_1(state_handle_1, &cmd[0]);
  51                         jnt_pos_interface.registerHandle(pos_handle_1);
  52                         hardware_interface::JointHandle pos_handle_2(state_handle_2, &cmd[1]);
  53                         jnt_pos_interface.registerHandle(pos_handle_2);
  54                         hardware_interface::JointHandle pos_handle_3(state_handle_3, &cmd[2]);
  55                         jnt_pos_interface.registerHandle(pos_handle_3);
  56                         hardware_interface::JointHandle pos_handle_4(state_handle_4, &cmd[3]);
  57                         jnt_pos_interface.registerHandle(pos_handle_4);
  58                         registerInterface(&jnt_pos_interface);
  59
  60                         torque_enable(0x1);
  61                 }
  62
  63                 // Converts pos_encoder from wheels to wheel angles
  64                 void read(ros::Duration period) {
  65                         for(int i=0, id=11; id<=14; id++, i++) {
  66                                 int32_t value;
  67                                 if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) != 1) {
  68                                         ROS_ERROR("I2C dynamixel id=%d read error", id);
  69                                         return;
  70                                 }
  71                                 pos[i] = 0.088*M_PI/180*(value-2048);
  72                         }
  73                         ROS_DEBUG("pos: %.2f %.2f %.2f %.2f", pos[0]*180/M_PI, pos[1]*180/M_PI, pos[2]*180/M_PI, pos[3]*180/M_PI);
  74                 }
  75
  76                 // Writes current velocity command to hardware
  77                 void write() {
  78                         ROS_DEBUG("cmd: %.2f %.2f %.2f %.2f", cmd[0]*180/M_PI, cmd[1]*180/M_PI, cmd[2]*180/M_PI, cmd[3]*180/M_PI);
  79
  80                         for(int i=0, id=11; id<=14; id++, i++) {
  81                                 if (isnan(cmd[i])) {
  82                                         ROS_WARN("Desired angle for id=%d is NaN, skipping write.", id);
  83                                         continue;
  84                                 }
  85
  86                                 int32_t value = cmd[i]/(0.088*M_PI/180) + 2048;
  87
  88                                 if (dynamixel_writedword(id, XM_GOAL_POSITION, value) != 1) {
  89                                         ROS_ERROR("I2C dynamixel id=%d write error", id);
  90                                 }
  91                         }
  92                 }
  93
  94         private:
  95                 hardware_interface::JointStateInterface jnt_state_interface;
  96                 hardware_interface::PositionJointInterface jnt_pos_interface;
  97                 ros::NodeHandle nh;
  98                 double cmd[4] = {NAN, NAN, NAN, NAN};
  99                 double pos[4] = {0, 0, 0, 0};
 100                 double vel[4] = {0, 0, 0, 0};
 101                 double eff[4] = {0, 0, 0, 0};
 102                 ros::ServiceServer torque_enable_service;
 103                 std::mutex comm_mutex;
 104
 105                 int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
 106                         int file;
 107                         uint8_t buf[32];
 108                         int ret;
 109                         uint8_t read_buf[read_num+1]; // for unknown reason need to read one more byte with USI Slave
 110
 111                         comm_mutex.lock();
 112                         if ((file = open(I2C_FILE, O_RDWR)) < 0) {
 113                                 perror("open");
 114                                 goto error;
 115                         }
 116
 117                         if (ioctl(file, I2C_SLAVE, I2C_ADDR) < 0) {
 118                                 perror("ioctl");
 119                                 goto error;
 120                         }
 121
 122                         if ((ret = i2c_smbus_write_i2c_block_data(file, 0x0, write_num, write_data)) != 0) {
 123                                 perror("i2c_smbus_write_block_data");
 124                                 goto error;
 125                         }
 126
 127                         for(int i=0; i<10; i++) {
 128                                 usleep(10*1000);
 129
 130                                 if ((ret = i2c_smbus_read_i2c_block_data(file, 0x0, read_num+1, read_buf)) != read_num+1) {
 131                                         perror("i2c_smbus_write_block_data");
 132                                         goto error;
 133                                 }
 134                                 if (read_data[0] == 1) {
 135                                         break;
 136                                 }
 137                         }
 138                         ret--; // remove extra byte
 139                         memcpy(read_data, read_buf, ret);
 140
 141                         error:
 142                                 close(file);
 143                                 comm_mutex.unlock();
 144                                 return ret;
 145                 }
 146
 147                 uint8_t dynamixel_writebyte(uint8_t id, uint8_t cmd, uint8_t value) {
 148                         uint8_t write_data[5] = {id, cmd, 1, value, CMD_EOF};
 149                         uint8_t read_data[2];
 150                         int ret = dynamixel_write(write_data, 5, read_data, 2);
 151                         if (ret == 2 && read_data[1] == id) {
 152                                 return read_data[0];
 153                         }
 154                         return 255;
 155                 }
 156
 157                 uint8_t dynamixel_writedword(uint8_t id, uint8_t cmd, int32_t value) {
 158                         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};
 159                         uint8_t read_data[2];
 160                         int ret = dynamixel_write(write_data, 8, read_data, 2);
 161                         if (ret == 2 && read_data[1] == id) {
 162                                 return read_data[0];
 163                         }
 164                         return 255;
 165                 }
 166
 167                 uint8_t dynamixel_readdword(uint8_t id, uint8_t cmd, int32_t *value) {
 168                         uint8_t write_data[4] = {id, cmd, 0x14, CMD_EOF};
 169                         uint8_t read_data[6];
 170                         int ret = dynamixel_write(write_data, 4, read_data, 6);
 171                         if (ret == 6 && read_data[1] == id) {
 172                                 *value = (read_data[2]<<24) | (read_data[3]<<16) | (read_data[4]<<8) | read_data[5];
 173                                 return read_data[0];
 174                         }
 175                         return 255;
 176                 }
 177
 178                 bool torque_enable(uint8_t enable) {
 179                         bool ret = true;
 180                         for(int id=11; id<=14; id++) {
 181                                 if (dynamixel_writebyte(id, XM_TORQUE_ENABLE, enable) != 1) {
 182                                         ROS_ERROR("I2C dynamixel id=%d torque change", id);
 183                                         ret = false;
 184                                 }
 185                         }
 186                         return ret;
 187                 }
 188
 189                 bool cbTorqueEnable(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res) {
 190                         ROS_INFO("Setting torque enable=%d", req.data);
 191                         res.success = torque_enable(req.data);
 192                         return true;
 193                 }
 194 };