From 68f2ac81f05f33b86bf34594a94eaaf513df5f3e Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 16:26:40 +0900 Subject: [PATCH 1/4] - modified torque control code --- .../robotis_controller/robotis_controller.h | 4 +- .../robotis_controller/robotis_controller.cpp | 62 ++++++++++++++----- .../devices/dynamixel/H42-20-S300-R.device | 2 +- .../devices/dynamixel/H54-100-S500-R.device | 2 +- .../devices/dynamixel/H54-200-B500-R.device | 2 +- .../devices/dynamixel/H54-200-S500-R.device | 2 +- .../devices/dynamixel/L54-30-S400-R.device | 2 - .../devices/dynamixel/L54-30-S500-R.device | 2 - .../devices/dynamixel/L54-50-S290-R.device | 2 - .../devices/dynamixel/L54-50-S500-R.device | 2 - .../devices/dynamixel/M42-10-S260-R.device | 2 - .../devices/dynamixel/M54-40-S250-R.device | 2 - .../devices/dynamixel/M54-60-S250-R.device | 2 - .../devices/dynamixel/XM-430.device | 2 +- .../include/robotis_device/dynamixel.h | 8 +-- .../include/robotis_device/dynamixel_state.h | 4 +- .../src/robotis_device/dynamixel.cpp | 16 ++--- robotis_device/src/robotis_device/robot.cpp | 8 +-- 18 files changed, 72 insertions(+), 54 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e2c8b64..e6befef 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -115,7 +115,9 @@ public: ros::Publisher present_joint_state_pub_; ros::Publisher current_module_pub_; - std::map gazebo_joint_pub_; + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; static void *timerThread(void *param); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 30b565e..cd8fd0f 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -159,8 +159,8 @@ void RobotisController::initializeSyncWrite() else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_current_ = dxl->convertValue2Current(read_data); - dxl->dxl_state_->goal_current_ = dxl->dxl_state_->present_current_; + dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_current_; } } } @@ -557,8 +557,12 @@ void RobotisController::msgQueueThread() { for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) { - gazebo_joint_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_pos/command", 1); + gazebo_joint_position_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_position/command", 1); + gazebo_joint_velocity_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_effort/command", 1); } } @@ -814,13 +818,13 @@ void RobotisController::process() else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_current_ = dxl->convertValue2Current(data); + dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_current_ = dxl->convertValue2Current(data); + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); else dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; } @@ -982,11 +986,11 @@ void RobotisController::process() } else if ((*module_it)->getControlMode() == CurrentControl) { - dxl_state->goal_current_ = result_state->goal_current_; + dxl_state->goal_torque_ = result_state->goal_torque_; if (gazebo_mode_ == false) { - uint32_t curr_data = dxl->convertCurrent2Value(dxl_state->goal_current_); + uint32_t curr_data = dxl->convertTorque2Value(dxl_state->goal_torque_); uint8_t sync_write_data[2] = { 0 }; sync_write_data[0] = DXL_LOBYTE(curr_data); sync_write_data[1] = DXL_HIBYTE(curr_data); @@ -1041,13 +1045,39 @@ void RobotisController::process() } else if (gazebo_mode_ == true) { - std_msgs::Float64 joint_msg; - - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); - dxl_it++) + for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { - joint_msg.data = dxl_it->second->dxl_state_->goal_position_; - gazebo_joint_pub_[dxl_it->first].publish(joint_msg); + if ((*module_it)->getModuleEnable() == false) + continue; + + std_msgs::Float64 joint_msg; + + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); + dxl_it++) + { + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + DynamixelState *dxl_state = dxl_it->second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + if ((*module_it)->getControlMode() == PositionControl) + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == CurrentControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } } } } @@ -1104,7 +1134,7 @@ void RobotisController::process() goal_state.name.push_back(joint_name); goal_state.position.push_back(dxl->dxl_state_->goal_position_); goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); - goal_state.effort.push_back(dxl->dxl_state_->goal_current_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); } // -> publish present joint_states & goal joint states topic @@ -1510,7 +1540,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } else if (mode == CurrentControl) { - uint32_t curr_data = dxl->convertCurrent2Value(dxl->dxl_state_->goal_current_); + uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 1ed385f..e348f42 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -3,7 +3,7 @@ model_name = H42-20-S300-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 +torque_to_current_value_ratio = 27.15146 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 09ef828..cdf81d0 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-100-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.66026 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 8791d7c..d002f97 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-B500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index dba268c..7e18900 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index 8cdcff3..d684e67 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S400-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -144198 value_of_max_radian_position = 144198 diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index db5a0b4..68848a2 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index cbd0453..3cbd843 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S290-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -103860 value_of_max_radian_position = 103860 diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index d72f2ef..bfbf0c6 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index 8c96988..f21bb44 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -3,8 +3,6 @@ model_name = M42-10-S260-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 - value_of_0_radian_position = 0 value_of_min_radian_position = -131584 value_of_max_radian_position = 131584 diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index 2bc7c1c..d9424ad 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-40-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 72382de..284cac9 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-60-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 7fcec12..3f33b73 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -current_ratio = 2.69 +torque_to_current_value_ratio = 178.842161 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index 916f4c5..349020a 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -56,8 +56,8 @@ public: std::string ctrl_module_name_; DynamixelState *dxl_state_; - double current_ratio_; - double velocity_ratio_; + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; int32_t value_of_0_radian_position_; int32_t value_of_min_radian_position_; @@ -82,8 +82,8 @@ public: double convertValue2Velocity(int32_t value); int32_t convertVelocity2Value(double velocity); - double convertValue2Current(int16_t value); - int16_t convertCurrent2Value(double current); + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); }; } diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index b5f4d4d..833c0f3 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -58,7 +58,7 @@ public: double present_current_; double goal_position_; double goal_velocity_; - double goal_current_; + double goal_torque_; double position_p_gain_; std::map bulk_read_table_; @@ -72,7 +72,7 @@ public: present_current_(0.0), goal_position_(0.0), goal_velocity_(0.0), - goal_current_(0.0), + goal_torque_(0.0), position_p_gain_(0.0), position_offset_(0.0) { diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index c90e4eb..ba316b2 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -41,8 +41,8 @@ using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) : ctrl_module_name_("none"), - current_ratio_(1.0), - velocity_ratio_(1.0), + torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), value_of_min_radian_position_(0), value_of_max_radian_position_(0), @@ -128,20 +128,20 @@ int32_t Dynamixel::convertRadian2Value(double radian) double Dynamixel::convertValue2Velocity(int32_t value) { - return (double) value * velocity_ratio_; + return (double) value / velocity_to_value_ratio_; } int32_t Dynamixel::convertVelocity2Value(double velocity) { - return (int32_t) (velocity / velocity_ratio_);; + return (int32_t) (velocity * velocity_to_value_ratio_);; } -double Dynamixel::convertValue2Current(int16_t value) +double Dynamixel::convertValue2Torque(int16_t value) { - return (double) value * current_ratio_; + return (double) value / torque_to_current_value_ratio_; } -int16_t Dynamixel::convertCurrent2Value(double current) +int16_t Dynamixel::convertTorque2Value(double torque) { - return (int16_t) (current / current_ratio_); + return (int16_t) (torque * torque_to_current_value_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 1ae3fb9..900a9b9 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -366,10 +366,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float if (tokens.size() != 2) continue; - if (tokens[0] == "current_ratio") - dxl->current_ratio_ = std::atof(tokens[1].c_str()); - else if (tokens[0] == "velocity_ratio") - dxl->velocity_ratio_ = std::atof(tokens[1].c_str()); + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); else if (tokens[0] == "value_of_0_radian_position") dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); From 55e2f6e0f8df4648d7c9508920e5052ec93261a1 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 16:35:54 +0900 Subject: [PATCH 2/4] - rename (present_current_ -> present_torque_) --- .../src/robotis_controller/robotis_controller.cpp | 10 +++++----- .../include/robotis_device/dynamixel_state.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index cd8fd0f..479f38b 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -159,8 +159,8 @@ void RobotisController::initializeSyncWrite() else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(read_data); - dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_current_; + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; } } } @@ -818,7 +818,7 @@ void RobotisController::process() else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(data); + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) @@ -1129,7 +1129,7 @@ void RobotisController::process() present_state.name.push_back(joint_name); present_state.position.push_back(dxl->dxl_state_->present_position_); present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); - present_state.effort.push_back(dxl->dxl_state_->present_current_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); goal_state.name.push_back(joint_name); goal_state.position.push_back(dxl->dxl_state_->goal_position_); @@ -1607,7 +1607,7 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: { d_it->second->dxl_state_->present_position_ = msg->position[i]; d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; - d_it->second->dxl_state_->present_current_ = msg->effort[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; } } diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index 833c0f3..a318a1f 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -55,7 +55,7 @@ public: double present_position_; double present_velocity_; - double present_current_; + double present_torque_; double goal_position_; double goal_velocity_; double goal_torque_; @@ -69,7 +69,7 @@ public: : update_time_stamp_(0, 0), present_position_(0.0), present_velocity_(0.0), - present_current_(0.0), + present_torque_(0.0), goal_position_(0.0), goal_velocity_(0.0), goal_torque_(0.0), From 27915069263fe93fc69b2bb26098990aea677a97 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 18:24:33 +0900 Subject: [PATCH 3/4] - rename ControlMode(CurrentControl -> TorqueControl) - rename (port_to_sync_write_torque_ -> port_to_sync_write_current_) --- .../robotis_controller/robotis_controller.h | 2 +- .../robotis_controller/robotis_controller.cpp | 40 +++++++++---------- .../robotis_framework_common/motion_module.h | 2 +- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e6befef..ae91370 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -107,7 +107,7 @@ public: /* sync write */ std::map port_to_sync_write_position_; std::map port_to_sync_write_velocity_; - std::map port_to_sync_write_torque_; + std::map port_to_sync_write_current_; std::map port_to_sync_write_position_p_gain_; /* publisher */ diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 479f38b..dfa8ffe 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -106,8 +106,8 @@ void RobotisController::initializeSyncWrite() if (it->second != NULL) it->second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->clearParam(); @@ -231,7 +231,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: if (default_device->goal_current_item_ != 0) { - port_to_sync_write_torque_[port_name] + port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite(port, default_pkt_handler, default_device->goal_current_item_->address_, @@ -707,8 +707,8 @@ void RobotisController::stopTimer() if (it->second != NULL) it->second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->clearParam(); @@ -984,7 +984,7 @@ void RobotisController::process() port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } - else if ((*module_it)->getControlMode() == CurrentControl) + else if ((*module_it)->getControlMode() == TorqueControl) { dxl_state->goal_torque_ = result_state->goal_torque_; @@ -995,8 +995,8 @@ void RobotisController::process() sync_write_data[0] = DXL_LOBYTE(curr_data); sync_write_data[1] = DXL_HIBYTE(curr_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } } @@ -1036,8 +1036,8 @@ void RobotisController::process() if (it->second != NULL) it->second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->txPacket(); @@ -1071,7 +1071,7 @@ void RobotisController::process() joint_msg.data = dxl_state->goal_velocity_; gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); } - else if ((*module_it)->getControlMode() == CurrentControl) + else if ((*module_it)->getControlMode() == TorqueControl) { joint_msg.data = dxl_state->goal_torque_; gazebo_joint_effort_pub_[joint_name].publish(joint_msg); @@ -1477,8 +1477,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } @@ -1516,8 +1516,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } @@ -1533,12 +1533,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); } - else if (mode == CurrentControl) + else if (mode == TorqueControl) { uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); uint8_t sync_write_data[4] = { 0 }; @@ -1547,8 +1547,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h index c602f34..e85d938 100644 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -53,7 +53,7 @@ enum ControlMode { PositionControl, VelocityControl, - CurrentControl + TorqueControl }; class MotionModule From e8792df69730ff53c02fa4afb638c37de20c9912 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 25 Jul 2016 11:35:12 +0900 Subject: [PATCH 4/4] added XM-430-W210 / XM-430-W350 device file. --- .../robotis_controller/robotis_controller.cpp | 10 +-- .../devices/dynamixel/XM-430-W210.device | 78 +++++++++++++++++++ .../devices/dynamixel/XM-430-W350.device | 78 +++++++++++++++++++ .../devices/dynamixel/XM-430.device | 2 +- 4 files changed, 162 insertions(+), 6 deletions(-) create mode 100644 robotis_device/devices/dynamixel/XM-430-W210.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W350.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index dfa8ffe..292b2ff 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -870,7 +870,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); } // Call SensorModule Process() @@ -889,7 +889,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", time_duration.nsec * 0.000001); } if (controller_mode_ == MotionModuleMode) @@ -1009,7 +1009,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); } // SyncWrite @@ -1117,7 +1117,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); } // publish present & goal position @@ -1144,7 +1144,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); } is_process_running = false; diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device new file mode 100644 index 0000000..178fc4f --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -0,0 +1,78 @@ +[device info] +model_name = XM-430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device new file mode 100644 index 0000000..ed6c17f --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -0,0 +1,78 @@ +[device info] +model_name = XM-430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 3f33b73..67d523f 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -torque_to_current_value_ratio = 178.842161 +torque_to_current_value_ratio = 149.795386991 value_of_0_radian_position = 2048 value_of_min_radian_position = 0