open_manipulator: increase torque to gripper

diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h
index f46045c7af1cc68c67f8dd91d4be7ff6cdf5f49f..4694242dc97dd13a2e7d58cb4e33c42ea93c4670 100644 (file)
--- a/include/open_manipulator_i2c.h
+++ b/include/open_manipulator_i2c.h
@@ -15,6 +15,8 @@
 #define I2C_FILE "/dev/i2c-2"
 #define I2C_ADDR (0x60>>1)
 #define XM_OPERATING_MODE 11
+#define XM_CURRENT_LIMIT 38
+#define XM_SHUTDOWN 63
 #define XM_TORQUE_ENABLE 64
 #define XM_HARDWARE_ERROR_STATUS 70
 #define XM_GOAL_CURRENT 102
@@ -35,6 +37,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                WtOpenManipulatorI2C(ros::NodeHandle nh) {
                        uint8_t mode=255;
                        this->nh = nh;
+                       int16_t current;
                        torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this);
                        get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this);
 
@@ -65,7 +68,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                        registerInterface(&jnt_pos_interface);
 
                        // Settings
-                       if (dynamixel_writeword(15, XM_GOAL_CURRENT, 200) != 1) {
+                       if (dynamixel_writeword(15, XM_GOAL_CURRENT, 600) != 1) { // 0..1193, 2.69mA/value
                                ROS_ERROR("I2C dynamixel id=15 write goal current error");
                                exit(1);
                        }
@@ -100,19 +103,27 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                        int32_t value;
                        int16_t current;
                        auto t1 = std::chrono::high_resolution_clock::now();
+                       int num_error = 0;
 
                        // arm
                        for(int i=0, id=11; id<=14; id++, i++) {
+                               if (ignore_ids.count(i)  > 0) {
+                                       ROS_ERROR_THROTTLE(10, "Ignoring dynamixel %d", id);
+                                       continue;
+                               }
+
                                if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) == 1) {
                                        pos[i] = STEPS_TO_RADIAN*(value-2048);
                                } else {
                                        ROS_ERROR("I2C dynamixel id=%d position read error", id);
+                                       num_error++;
                                }
 
                                if (dynamixel_readdword(id, XM_PRESENT_VELOCITY, &value) == 1) {
                                        vel[i] = value*VELOCITY_STEPS_TO_RADS;
                                } else {
                                        ROS_ERROR("I2C dynamixel id=%d velocity read error", id);
+                                       num_error++;
                                }
 
 #ifdef READ_EFFORT
@@ -120,8 +131,13 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                                        eff[i] = current*2.69e-3;
                                } else {
                                        ROS_ERROR("I2C dynamixel id=%d current read error", id);
+                                       num_error++;
                                }
 #endif
+
+                               if (num_error >= 3) {
+                                       ignore_ids.insert(i);
+                               }
                        }
 
                        // gripper
@@ -212,6 +228,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                double eff[5] = {0, 0, 0, 0, 0};
                ros::ServiceServer torque_enable_service, get_error_service;
                std::mutex comm_mutex;
+               std::set<int> ignore_ids;
 
                int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
                        int file;
@@ -289,6 +306,9 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                                *value = read_data[2];
                                return read_data[0];
                        }
+                       if (read_data[0] != 1) {
+                               ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
+                       }
                        return 255;
                }
 
@@ -307,10 +327,13 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                        uint8_t write_data[4] = {id, cmd, 0x14, CMD_EOF};
                        uint8_t read_data[6];
                        int ret = dynamixel_write(write_data, 4, read_data, 6);
-                       if (ret == 6 && read_data[1] == id) {
+                       if (ret == 6 && read_data[0] == 1 && read_data[1] == id) {
                                *value = (read_data[2]<<24) | (read_data[3]<<16) | (read_data[4]<<8) | read_data[5];
                                return read_data[0];
                        }
+                       if (read_data[0] != 1) {
+                               ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
+                       }
                        return 255;
                }
 
@@ -328,7 +351,6 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
                                uint8_t value;
                                if (dynamixel_readbyte(id, XM_HARDWARE_ERROR_STATUS, &value) != 1) {
                                        ROS_ERROR("I2C dynamixel id=%d reading hardware error status", id);
-                                       return false;
                                }
                                hw_error.id = id;
                                hw_error.inputVoltage = value & (1<<0);