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());