diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e2c8b64..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 */ @@ -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..292b2ff 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(); @@ -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_torque_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; } } } @@ -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_, @@ -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); } } @@ -703,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(); @@ -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_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_) 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; } @@ -866,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() @@ -885,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) @@ -980,19 +984,19 @@ 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_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); - 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); } } } @@ -1005,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 @@ -1032,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(); @@ -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() == TorqueControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } } } } @@ -1087,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 @@ -1099,12 +1129,12 @@ 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_); 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 @@ -1114,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; @@ -1447,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_); } @@ -1486,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_); } @@ -1503,22 +1533,22 @@ 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->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)); 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_); @@ -1577,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/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-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 7fcec12..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] -current_ratio = 2.69 +torque_to_current_value_ratio = 149.795386991 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..a318a1f 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -55,10 +55,10 @@ public: double present_position_; double present_velocity_; - double present_current_; + double present_torque_; double goal_position_; double goal_velocity_; - double goal_current_; + double goal_torque_; double position_p_gain_; std::map bulk_read_table_; @@ -69,10 +69,10 @@ 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_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()); 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