X-Git-Url: https://defiant.homedns.org/gitweb/?a=blobdiff_plain;ds=sidebyside;f=include%2Fopen_manipulator_i2c.h;h=3d041c7f49fdfdcd2d5c7696aa1051252690cd22;hb=548a3f79f594a3b2c3c817bffd1e028b601bc150;hp=80bfdedbb80db8f4beccb4dd075b869df0a6a64f;hpb=183c6cd62538abe4945aebdb2bcdccbbeb98888a;p=wt_open_manipulator.git diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h index 80bfded..3d041c7 100644 --- a/include/open_manipulator_i2c.h +++ b/include/open_manipulator_i2c.h @@ -1,3 +1,4 @@ +#include #include #include #include @@ -9,31 +10,37 @@ #include #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` - */ +#include "wt_open_manipulator/GetError.h" #define I2C_FILE "/dev/i2c-2" #define I2C_ADDR (0x60>>1) +#define XM_OPERATING_MODE 11 +#define XM_HOMING_OFFSET 20 +#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 +#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; + int32_t homing_offset=0; this->nh = nh; torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this); + get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this); // connect and register the joint state interface hardware_interface::JointStateHandle state_handle_1("joint1", &pos[0], &vel[0], &eff[0]); @@ -44,6 +51,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 +64,185 @@ 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_readdword(11, XM_HOMING_OFFSET, &homing_offset) != 1) { + ROS_ERROR("I2C dynamixel id=11 read homing offset error"); + exit(1); + } + if (homing_offset != 64) { + if (dynamixel_writedword(11, XM_HOMING_OFFSET, 64) != 1) { + ROS_ERROR("I2C dynamixel id=11 write homing offset error"); + exit(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); + } + 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(); + int num_error = 0; + + // 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 (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 + 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); + num_error++; + } +#endif + + if (num_error >= 3) { + ignore_ids.insert(i); } - pos[i] = 0.088*M_PI/180*(value-2048); } - 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); + + // 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"); + } +#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.019) { + 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}; - ros::ServiceServer torque_enable_service; + 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, get_error_service; std::mutex comm_mutex; + std::set ignore_ids; 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 +257,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 +289,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,31 +309,69 @@ 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]; + } + if (read_data[0] != 1) { + ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, 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]; 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; } - 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; + 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); + return true; + } + + bool cbGetError(wt_open_manipulator::GetError::Request &req, wt_open_manipulator::GetError::Response &res) { + ROS_INFO("Reading Hardware Error Status"); + + for(int id=11, i=0; id<=15; id++, i++) { + wt_open_manipulator::HardwareError hw_error; + uint8_t value; + if (dynamixel_readbyte(id, XM_HARDWARE_ERROR_STATUS, &value) != 1) { + ROS_ERROR("I2C dynamixel id=%d reading hardware error status", id); } + hw_error.id = id; + hw_error.inputVoltage = value & (1<<0); + hw_error.overHeating = value & (1<<2); + hw_error.motorEncoder = value & (1<<3); + hw_error.electricalShock = value & (1<<4); + hw_error.overload = value & (1<<5); + res.errors.push_back(hw_error); } - 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); return true; } };