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"
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
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)
35 class WtOpenManipulatorI2C : public hardware_interface::RobotHW
38 WtOpenManipulatorI2C(ros::NodeHandle nh) {
40 int32_t homing_offset=0;
42 torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this);
43 get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this);
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);
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);
72 if (dynamixel_readdword(11, XM_HOMING_OFFSET, &homing_offset) != 1) {
73 ROS_ERROR("I2C dynamixel id=11 read homing offset error");
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");
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");
86 if (dynamixel_readbyte(15, XM_OPERATING_MODE, &mode) != 1) {
87 ROS_ERROR("I2C dynamixel id=15 read operating Mode error");
91 if (!torque_enable(false)) {
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");
99 if (dynamixel_writedword(15, XM_PROFILE_ACCELERATION, 20) != 1) {
100 ROS_ERROR("I2C dynamixel id=15 write profile Acceleration error");
103 if (dynamixel_writedword(15, XM_PROFILE_VELOCITY, 200) != 1) {
104 ROS_ERROR("I2C dynamixel id=15 write profile Velocity error");
107 if (!torque_enable(true)) {
112 // Converts pos_encoder from wheels to wheel angles
113 void read(ros::Duration period) {
116 auto t1 = std::chrono::high_resolution_clock::now();
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);
126 if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) == 1) {
127 pos[i] = STEPS_TO_RADIAN*(value-2048);
129 ROS_ERROR("I2C dynamixel id=%d position read error", id);
133 if (dynamixel_readdword(id, XM_PRESENT_VELOCITY, &value) == 1) {
134 vel[i] = value*VELOCITY_STEPS_TO_RADS;
136 ROS_ERROR("I2C dynamixel id=%d velocity read error", id);
141 if (dynamixel_readword(id, XM_PRESENT_CURRENT, ¤t) == 1) {
142 eff[i] = current*2.69e-3;
144 ROS_ERROR("I2C dynamixel id=%d current read error", id);
149 if (num_error >= 3) {
150 ignore_ids.insert(i);
155 if (dynamixel_readdword(15, XM_PRESENT_POSITION, &value) == 1) {
156 pos[4] = STEPS_TO_RADIAN*(value-2048)*GRIPPER_RADIAN_TO_METER;
158 ROS_ERROR("I2C dynamixel id=15 position read error");
160 if (dynamixel_readdword(15, XM_PRESENT_VELOCITY, &value) == 1) {
161 vel[4] = value*VELOCITY_STEPS_TO_RADS*GRIPPER_RADIAN_TO_METER;
163 ROS_ERROR("I2C dynamixel id=15 velocity read error");
166 if (dynamixel_readword(15, XM_PRESENT_CURRENT, ¤t) == 1) {
167 eff[4] = current*2.69e-3;
169 ROS_ERROR("I2C dynamixel id=15 current read error");
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]);
176 ROS_DEBUG("current: %.2f %.2f %.2f %.2f %.2f", eff[0], eff[1], eff[2], eff[3], eff[4]);
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);
183 // Writes current velocity command to hardware
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]);
191 for(int i=0, id=11; id<=14; id++, i++) {
193 ROS_WARN("Desired angle for id=%d is NaN, skipping write.", id);
197 value = cmd[i]/STEPS_TO_RADIAN + 2048;
199 if (dynamixel_writedword(id, XM_GOAL_POSITION, value) == 1) {
202 ROS_ERROR("I2C dynamixel id=%d write error", id);
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.");
210 value = cmd[4]/GRIPPER_RADIAN_TO_METER/STEPS_TO_RADIAN + 2048;
211 if (dynamixel_writedword(15, XM_GOAL_POSITION, value) == 1) {
214 ROS_ERROR("I2C dynamixel id=15 write error");
220 bool torque_enable(bool enable) {
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);
232 hardware_interface::JointStateInterface jnt_state_interface;
233 hardware_interface::PositionJointInterface jnt_pos_interface;
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;
244 int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
247 uint8_t read_buf[read_num+1]; // for unknown reason need to read one more byte with USI Slave
250 if ((file = open(I2C_FILE, O_RDWR)) < 0) {
255 if (ioctl(file, I2C_SLAVE, I2C_ADDR) < 0) {
260 if ((ret = ::write(file, write_data, write_num)) != write_num) {
265 // wait at least the half duplex transmission time, assume 29 (=14+15) bytes send/received: 1/(115200.0/10/29) <= 2.6ms
268 if ((ret = ::read(file, read_buf, read_num+1)) != read_num+1) {
272 ret--; // remove extra byte
273 memcpy(read_data, read_buf, ret);
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) {
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) {
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) {
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];
320 if (read_data[0] != 1) {
321 ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
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];
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];
345 if (read_data[0] != 1) {
346 ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
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);
357 bool cbGetError(wt_open_manipulator::GetError::Request &req, wt_open_manipulator::GetError::Response &res) {
358 ROS_INFO("Reading Hardware Error Status");
360 for(int id=11, i=0; id<=15; id++, i++) {
361 wt_open_manipulator::HardwareError hw_error;
363 if (dynamixel_readbyte(id, XM_HARDWARE_ERROR_STATUS, &value) != 1) {
364 ROS_ERROR("I2C dynamixel id=%d reading hardware error status", 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);