From 553ea1fb16565c11c5d85f77058252fdd56bfd02 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sat, 4 Jan 2020 10:26:22 +0100 Subject: [PATCH] Added gripper --- config/control.yaml | 6 + include/open_manipulator_i2c.h | 235 +++++++++++++++++++++++-------- src/wt_open_manipulator_node.cpp | 4 +- 3 files changed, 188 insertions(+), 57 deletions(-) diff --git a/config/control.yaml b/config/control.yaml index 4aeee8c..75e7d3b 100644 --- a/config/control.yaml +++ b/config/control.yaml @@ -9,3 +9,9 @@ arm_controller: - joint2 - joint3 - joint4 + constraints: + goal_time: 0.3 + +gripper_controller: + type: "position_controllers/GripperActionController" + joint: gripper diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h index 80bfded..5a9040c 100644 --- a/include/open_manipulator_i2c.h +++ b/include/open_manipulator_i2c.h @@ -1,3 +1,4 @@ +#include #include #include #include @@ -10,28 +11,27 @@ #include #include "std_srvs/SetBool.h" -/* http://wiki.ros.org/joint_trajectory_controller?distro=melodic - * - * arm: - * - Gripper Goal Current(102)=200, Operating Mode(11)=Current-based Position Control Mode, Profile_Acceleration=20, Profile_Velocity=200 - * - Read: Present Current(126), Present Velocity(128), Present Position(132) - * - Gripper joint limits: - * 0.010, // max gripper limit (0.01 m) - * -0.010, // min gripper limit (-0.01 m) - * -0.015, // Change unit from `meter` to `radian` - */ - #define I2C_FILE "/dev/i2c-2" #define I2C_ADDR (0x60>>1) +#define XM_OPERATING_MODE 11 #define XM_TORQUE_ENABLE 64 +#define XM_GOAL_CURRENT 102 +#define XM_PROFILE_ACCELERATION 108 +#define XM_PROFILE_VELOCITY 112 #define XM_GOAL_POSITION 116 #define XM_PRESENT_POSITION 132 +#define XM_PRESENT_CURRENT 126 +#define XM_PRESENT_VELOCITY 128 #define CMD_EOF 0xaa +#define STEPS_TO_RADIAN (2*M_PI/4096) +#define GRIPPER_RADIAN_TO_METER -0.015 +#define VELOCITY_STEPS_TO_RADS (0.229*2*M_PI/60) class WtOpenManipulatorI2C : public hardware_interface::RobotHW { public: - WtOpenManipulatorI2C(ros::NodeHandle nh) { + WtOpenManipulatorI2C(ros::NodeHandle nh) { + uint8_t mode=255; this->nh = nh; torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this); @@ -44,6 +44,8 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW jnt_state_interface.registerHandle(state_handle_3); hardware_interface::JointStateHandle state_handle_4("joint4", &pos[3], &vel[3], &eff[3]); jnt_state_interface.registerHandle(state_handle_4); + hardware_interface::JointStateHandle state_handle_gripper("gripper", &pos[4], &vel[4], &eff[4]); + jnt_state_interface.registerHandle(state_handle_gripper); registerInterface(&jnt_state_interface); // connect and register the joint position interface @@ -55,56 +57,161 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW jnt_pos_interface.registerHandle(pos_handle_3); hardware_interface::JointHandle pos_handle_4(state_handle_4, &cmd[3]); jnt_pos_interface.registerHandle(pos_handle_4); + hardware_interface::JointHandle pos_handle_gripper(state_handle_gripper, &cmd[4]); + jnt_pos_interface.registerHandle(pos_handle_gripper); registerInterface(&jnt_pos_interface); - torque_enable(0x1); + // Settings + if (dynamixel_writeword(15, XM_GOAL_CURRENT, 200) != 1) { + ROS_ERROR("I2C dynamixel id=15 write goal current error"); + exit(1); + } + if (dynamixel_readbyte(15, XM_OPERATING_MODE, &mode) != 1) { + ROS_ERROR("I2C dynamixel id=15 read operating Mode error"); + exit(1); + } + if (mode != 5) { + if (!torque_enable(false)) { + exit(1); + } + if (dynamixel_writebyte(15, XM_OPERATING_MODE, 5) != 1) { // Current-based Position Control Mode + ROS_ERROR("I2C dynamixel id=15 write operating Mode error"); + exit(1); + } + } + if (dynamixel_writedword(15, XM_PROFILE_ACCELERATION, 20) != 1) { + ROS_ERROR("I2C dynamixel id=15 write profile Acceleration error"); + exit(1); + } + if (dynamixel_writedword(15, XM_PROFILE_VELOCITY, 200) != 1) { + ROS_ERROR("I2C dynamixel id=15 write profile Velocity error"); + exit(1); + } + if (!torque_enable(true)) { + exit(1); + } } // Converts pos_encoder from wheels to wheel angles void read(ros::Duration period) { + int32_t value; + int16_t current; + auto t1 = std::chrono::high_resolution_clock::now(); + + // arm for(int i=0, id=11; id<=14; id++, i++) { - int32_t value; - if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) != 1) { - ROS_ERROR("I2C dynamixel id=%d read error", id); - return; + 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); + } + + 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); } - pos[i] = 0.088*M_PI/180*(value-2048); + +#ifdef READ_EFFORT + if (dynamixel_readword(id, XM_PRESENT_CURRENT, ¤t) == 1) { + eff[i] = current*2.69e-3; + } else { + ROS_ERROR("I2C dynamixel id=%d current read error", id); + } +#endif + } + + // gripper + if (dynamixel_readdword(15, XM_PRESENT_POSITION, &value) == 1) { + pos[4] = STEPS_TO_RADIAN*(value-2048)*GRIPPER_RADIAN_TO_METER; + } else { + ROS_ERROR("I2C dynamixel id=15 position read error"); + } + if (dynamixel_readdword(15, XM_PRESENT_VELOCITY, &value) == 1) { + vel[4] = value*VELOCITY_STEPS_TO_RADS*GRIPPER_RADIAN_TO_METER; + } else { + ROS_ERROR("I2C dynamixel id=15 velocity read error"); } - 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); +#ifdef READ_EFFORT + if (dynamixel_readword(15, XM_PRESENT_CURRENT, ¤t) == 1) { + eff[4] = current*2.69e-3; + } else { + ROS_ERROR("I2C dynamixel id=15 current read error"); + } +#endif + + 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]); + 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]); +#ifdef READ_EFFORT + ROS_DEBUG("current: %.2f %.2f %.2f %.2f %.2f", eff[0], eff[1], eff[2], eff[3], eff[4]); +#endif + auto t2 = std::chrono::high_resolution_clock::now(); + int64_t duration = std::chrono::duration_cast( t2 - t1 ).count(); + ROS_DEBUG("read duration: %lldms", duration); } // Writes current velocity command to hardware void write() { - 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); + int32_t value; - for(int i=0, id=11; id<=14; id++, i++) { - if (isnan(cmd[i])) { - ROS_WARN("Desired angle for id=%d is NaN, skipping write.", id); - continue; + if (memcmp(cmd, cmd_pre, sizeof(cmd)) != 0) { // Only write new position when changed + 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]); + + // arm + for(int i=0, id=11; id<=14; id++, i++) { + if (isnan(cmd[i])) { + ROS_WARN("Desired angle for id=%d is NaN, skipping write.", id); + continue; + } + + value = cmd[i]/STEPS_TO_RADIAN + 2048; + + if (dynamixel_writedword(id, XM_GOAL_POSITION, value) == 1) { + cmd_pre[i] = cmd[i]; + } else { + ROS_ERROR("I2C dynamixel id=%d write error", id); + } } - int32_t value = cmd[i]/(0.088*M_PI/180) + 2048; + // gripper + if (isnan(cmd[4]) || cmd[4] < -0.010 || cmd[4] > 0.010) { + ROS_WARN("Desired angle for id=15 is NaN or outside joint limit, skipping write."); + } else { + value = cmd[4]/GRIPPER_RADIAN_TO_METER/STEPS_TO_RADIAN + 2048; + if (dynamixel_writedword(15, XM_GOAL_POSITION, value) == 1) { + cmd_pre[4] = cmd[4]; + } else { + ROS_ERROR("I2C dynamixel id=15 write error"); + } + } + } + } - if (dynamixel_writedword(id, XM_GOAL_POSITION, value) != 1) { - ROS_ERROR("I2C dynamixel id=%d write error", id); + bool torque_enable(bool enable) { + bool ret = true; + for(int id=11; id<=15; id++) { + if (dynamixel_writebyte(id, XM_TORQUE_ENABLE, enable) != 1) { + ROS_ERROR("I2C dynamixel id=%d torque change", id); + ret = false; } } + return ret; } private: hardware_interface::JointStateInterface jnt_state_interface; hardware_interface::PositionJointInterface jnt_pos_interface; ros::NodeHandle nh; - double cmd[4] = {NAN, NAN, NAN, NAN}; - double pos[4] = {0, 0, 0, 0}; - double vel[4] = {0, 0, 0, 0}; - double eff[4] = {0, 0, 0, 0}; + double cmd[5] = {NAN, NAN, NAN, NAN, NAN}; + double cmd_pre[5] = {NAN, NAN, NAN, NAN, NAN}; // determine if cmd changed + double pos[5] = {0, 0, 0, 0, 0}; + double vel[5] = {0, 0, 0, 0, 0}; + double eff[5] = {0, 0, 0, 0, 0}; ros::ServiceServer torque_enable_service; std::mutex comm_mutex; int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) { int file; - uint8_t buf[32]; int ret; uint8_t read_buf[read_num+1]; // for unknown reason need to read one more byte with USI Slave @@ -119,24 +226,21 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW goto error; } - if ((ret = i2c_smbus_write_i2c_block_data(file, 0x0, write_num, write_data)) != 0) { - perror("i2c_smbus_write_block_data"); + if ((ret = ::write(file, write_data, write_num)) != write_num) { + perror("i2c write"); goto error; } - for(int i=0; i<10; i++) { - usleep(10*1000); - - if ((ret = i2c_smbus_read_i2c_block_data(file, 0x0, read_num+1, read_buf)) != read_num+1) { - perror("i2c_smbus_write_block_data"); + // wait at least the half duplex transmission time, assume 29 (=14+15) bytes send/received: 1/(115200.0/10/29) <= 2.6ms + usleep(3.2*1e3); + if (read_num > 0) { + if ((ret = ::read(file, read_buf, read_num+1)) != read_num+1) { + perror("i2c read"); goto error; } - if (read_data[0] == 1) { - break; - } + ret--; // remove extra byte + memcpy(read_data, read_buf, ret); } - ret--; // remove extra byte - memcpy(read_data, read_buf, ret); error: close(file); @@ -154,6 +258,16 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW return 255; } + uint8_t dynamixel_writeword(uint8_t id, uint8_t cmd, int16_t value) { + uint8_t write_data[6] = {id, cmd, 2, (uint8_t)(value>>8), (uint8_t)value, CMD_EOF}; + uint8_t read_data[2]; + int ret = dynamixel_write(write_data, 6, read_data, 2); + if (ret == 2 && read_data[1] == id) { + return read_data[0]; + } + return 255; + } + uint8_t dynamixel_writedword(uint8_t id, uint8_t cmd, int32_t value) { 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}; uint8_t read_data[2]; @@ -164,6 +278,28 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW return 255; } + uint8_t dynamixel_readbyte(uint8_t id, uint8_t cmd, uint8_t *value) { + uint8_t write_data[4] = {id, cmd, 0x11, CMD_EOF}; + uint8_t read_data[3]; + int ret = dynamixel_write(write_data, 4, read_data, 3); + if (ret == 3 && read_data[1] == id) { + *value = read_data[2]; + return read_data[0]; + } + return 255; + } + + uint8_t dynamixel_readword(uint8_t id, uint8_t cmd, int16_t *value) { + uint8_t write_data[4] = {id, cmd, 0x12, CMD_EOF}; + uint8_t read_data[4]; + int ret = dynamixel_write(write_data, 4, read_data, 4); + if (ret == 4 && read_data[1] == id) { + *value = (read_data[2]<<8) | read_data[3]; + return read_data[0]; + } + return 255; + } + uint8_t dynamixel_readdword(uint8_t id, uint8_t cmd, int32_t *value) { uint8_t write_data[4] = {id, cmd, 0x14, CMD_EOF}; uint8_t read_data[6]; @@ -175,17 +311,6 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW return 255; } - bool torque_enable(uint8_t enable) { - bool ret = true; - for(int id=11; id<=14; id++) { - if (dynamixel_writebyte(id, XM_TORQUE_ENABLE, enable) != 1) { - ROS_ERROR("I2C dynamixel id=%d torque change", id); - ret = false; - } - } - return ret; - } - bool cbTorqueEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { ROS_INFO("Setting torque enable=%d", req.data); res.success = torque_enable(req.data); diff --git a/src/wt_open_manipulator_node.cpp b/src/wt_open_manipulator_node.cpp index b20f13c..7200361 100644 --- a/src/wt_open_manipulator_node.cpp +++ b/src/wt_open_manipulator_node.cpp @@ -5,9 +5,9 @@ int main(int argc, char **argv) { ros::init(argc, argv, "wt_open_manipulator_i2c"); - ros::NodeHandle n; + ros::NodeHandle nh("~"); - WtOpenManipulatorI2C robot(n); + WtOpenManipulatorI2C robot(nh); controller_manager::ControllerManager cm(&robot); ros::AsyncSpinner spinner(1); -- 2.39.5