+#include <chrono>
#include <unistd.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <hardware_interface/robot_hw.h>
#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);
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
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<std::chrono::milliseconds>( 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
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);
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];
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];
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);
projects / wt_open_manipulator.git / commitdiff