BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #19 from ROBOTIS-GIT/modify_torque_control

Browse Source 
Modify torque control
This commit is contained in:
ROBOTIS-zerom
2016-07-26 11:56:19 +09:00
committed by GitHub
parent e3381ca424 e8792df697
commit d511493a4c
21 changed files with 258 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -107,7 +107,7 @@ public:
/* sync write */
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_torque_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_current_;
std::map<std::string, dynamixel::GroupSyncWrite *> 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<std::string, ros::Publisher> gazebo_joint_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_velocity_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_effort_pub_;
static void *timerThread(void *param);

114
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -106,8 +106,8 @@ void RobotisController::initializeSyncWrite()
if (it->second != NULL)
it->second->clearParam();
}
for (std::map<std::string, dynamixel::GroupSyncWrite *>::iterator it = port_to_sync_write_torque_.begin();
it != port_to_sync_write_torque_.end(); it++)
for (std::map<std::string, dynamixel::GroupSyncWrite *>::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<std::string, Dynamixel*>::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++)
{
gazebo_joint_pub_[it->first] = ros_node.advertise<std_msgs::Float64>(
"/" + gazebo_robot_name_ + "/" + it->first + "_pos/command", 1);
gazebo_joint_position_pub_[it->first] = ros_node.advertise<std_msgs::Float64>(
"/" + gazebo_robot_name_ + "/" + it->first + "_position/command", 1);
gazebo_joint_velocity_pub_[it->first] = ros_node.advertise<std_msgs::Float64>(
"/" + gazebo_robot_name_ + "/" + it->first + "_velocity/command", 1);
gazebo_joint_effort_pub_[it->first] = ros_node.advertise<std_msgs::Float64>(
"/" + 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<std::string, dynamixel::GroupSyncWrite *>::iterator it = port_to_sync_write_torque_.begin();
it != port_to_sync_write_torque_.end(); it++)
for (std::map<std::string, dynamixel::GroupSyncWrite *>::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<std::string, dynamixel::GroupSyncWrite *>::iterator it = port_to_sync_write_torque_.begin();
it != port_to_sync_write_torque_.end(); it++)
for (std::map<std::string, dynamixel::GroupSyncWrite *>::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<std::string, Dynamixel *>::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end();
dxl_it++)
for (std::list<MotionModule*>::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<std::string, Dynamixel *>::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];
}
}

2
robotis_device/devices/dynamixel/H42-20-S300-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/H54-100-S500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/H54-200-B500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/H54-200-S500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/L54-30-S400-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/L54-30-S500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/L54-50-S290-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/L54-50-S500-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/M42-10-S260-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/M54-40-S250-R.device
View File

@ -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

2
robotis_device/devices/dynamixel/M54-60-S250-R.device
View File

@ -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

78
robotis_device/devices/dynamixel/XM-430-W210.device Normal file
View File

@ -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

78
robotis_device/devices/dynamixel/XM-430-W350.device Normal file
View File

@ -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

2
robotis_device/devices/dynamixel/XM-430.device
View File

@ -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

8
robotis_device/include/robotis_device/dynamixel.h
View File

@ -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);
};
}

8
robotis_device/include/robotis_device/dynamixel_state.h
View File

@ -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<std::string, uint32_t> 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)
{

16
robotis_device/src/robotis_device/dynamixel.cpp
View File

@ -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_);
}

8
robotis_device/src/robotis_device/robot.cpp
View File

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

2
robotis_framework_common/include/robotis_framework_common/motion_module.h
View File

@ -53,7 +53,7 @@ enum ControlMode
{
PositionControl,
VelocityControl,
CurrentControl
TorqueControl
};
class MotionModule