From 4873e736fd755172ae1252806b29f1112551acda Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Fri, 27 Oct 2023 15:09:02 -0400 Subject: [PATCH 1/7] added mech reduction to write command and reading joint states --- include/interbotix_xs_driver/xs_common.hpp | 2 ++ include/interbotix_xs_driver/xs_driver.hpp | 3 +++ src/xs_driver.cpp | 22 ++++++++++++++++++---- 3 files changed, 23 insertions(+), 4 deletions(-) diff --git a/include/interbotix_xs_driver/xs_common.hpp b/include/interbotix_xs_driver/xs_common.hpp index 39a0f98..7776877 100644 --- a/include/interbotix_xs_driver/xs_common.hpp +++ b/include/interbotix_xs_driver/xs_common.hpp @@ -138,6 +138,8 @@ struct MotorState { // Dynamixel ID of the motor uint8_t motor_id; +// // Mech Red of the motor +// uint32_t mech_red; // Operating Mode of the motor std::string mode; // Profile Type ('velocity' or 'time') for the motor diff --git a/include/interbotix_xs_driver/xs_driver.hpp b/include/interbotix_xs_driver/xs_driver.hpp index 790a4ea..0863851 100644 --- a/include/interbotix_xs_driver/xs_driver.hpp +++ b/include/interbotix_xs_driver/xs_driver.hpp @@ -373,6 +373,9 @@ class InterbotixDriverXS // Dictionary mapping the name of a joint with its position in the JointState 'name' list std::unordered_map js_index_map; + // Dictionary mapping the ID of a joint with its mechanical reduction + std::unordered_map js_mech_reduction_map; + // Vector containing the robot joint positions in [rad] at the last update std::vector robot_positions; diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index b4f5c35..eacb2d8 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -414,10 +414,15 @@ bool InterbotixDriverXS::write_commands( get_group_info(name)->joint_names.at(i), commands.at(i)); } - // translate from position to command value + + // get mech red value +// int32_t j_reduction = motor_map[get_group_info(name)->joint_names.at(i)].mech_red; + int32_t j_reduction = js_mech_reduction_map[get_group_info(name)->joint_ids.at(i)]; + + // translate from position to command value - INCLUDE MECH REDUCTION dynamixel_commands[i] = dxl_wb.convertRadian2Value( get_group_info(name)->joint_ids.at(i), - commands.at(i)); + (commands.at(i) * j_reduction)); XSLOG_DEBUG( "ID: %d, writing %s command %d.", get_group_info(name)->joint_ids.at(i), @@ -835,8 +840,14 @@ bool InterbotixDriverXS::retrieve_motor_configs( YAML::Node single_motor = all_motors[motor_name]; // extract ID from node uint8_t id = (uint8_t)single_motor["ID"].as(); + + // extract mech reduction from node + uint32_t mech_red = (uint32_t)single_motor["Mech_Reduction"].as(); + js_mech_reduction_map[id] = mech_red; + // add the motor to the motor_map with it's ID, pos as the default opmode, vel as default - // profile + // profile and mechanical reduction +// motor_map.insert({motor_name, {id, mech_red, mode::POSITION, profile::VELOCITY}}); motor_map.insert({motor_name, {id, mode::POSITION, profile::VELOCITY}}); for ( YAML::const_iterator info_itr = single_motor.begin(); @@ -846,7 +857,7 @@ bool InterbotixDriverXS::retrieve_motor_configs( // iterate through the single_motor node // save all registers that are not ID or Baud_Rate std::string reg = info_itr->first.as(); - if (reg != "ID" && reg != "Baud_Rate") { + if (reg != "ID" && reg != "Baud_Rate" && reg != "Mech_Reduction") { int32_t value = info_itr->second.as(); MotorRegVal motor_info = {id, reg, value}; motor_info_vec.push_back(motor_info); @@ -1377,6 +1388,9 @@ void InterbotixDriverXS::read_joint_states() float velocity = dxl_wb.convertValue2Velocity(id, velocity_raw); float position = dxl_wb.convertValue2Radian(id, position_raw); + // Apply mechanical reduction + position = position / js_mech_reduction_map[id]; + robot_efforts.push_back(effort); robot_velocities.push_back(velocity); robot_positions.push_back(position); From 973cd5c93d06426b29fbe585af90bc39776c0a01 Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Fri, 27 Oct 2023 15:36:15 -0400 Subject: [PATCH 2/7] cleaned up old implementation --- include/interbotix_xs_driver/xs_common.hpp | 2 -- src/xs_driver.cpp | 7 ++++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/interbotix_xs_driver/xs_common.hpp b/include/interbotix_xs_driver/xs_common.hpp index 7776877..39a0f98 100644 --- a/include/interbotix_xs_driver/xs_common.hpp +++ b/include/interbotix_xs_driver/xs_common.hpp @@ -138,8 +138,6 @@ struct MotorState { // Dynamixel ID of the motor uint8_t motor_id; -// // Mech Red of the motor -// uint32_t mech_red; // Operating Mode of the motor std::string mode; // Profile Type ('velocity' or 'time') for the motor diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index eacb2d8..b5c94d3 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -416,7 +416,6 @@ bool InterbotixDriverXS::write_commands( } // get mech red value -// int32_t j_reduction = motor_map[get_group_info(name)->joint_names.at(i)].mech_red; int32_t j_reduction = js_mech_reduction_map[get_group_info(name)->joint_ids.at(i)]; // translate from position to command value - INCLUDE MECH REDUCTION @@ -844,10 +843,12 @@ bool InterbotixDriverXS::retrieve_motor_configs( // extract mech reduction from node uint32_t mech_red = (uint32_t)single_motor["Mech_Reduction"].as(); js_mech_reduction_map[id] = mech_red; + XSLOG_FATAL( + "reading mech reduction, motor id: %i, red: %i", + id, mech_red); // add the motor to the motor_map with it's ID, pos as the default opmode, vel as default - // profile and mechanical reduction -// motor_map.insert({motor_name, {id, mech_red, mode::POSITION, profile::VELOCITY}}); + // profile motor_map.insert({motor_name, {id, mode::POSITION, profile::VELOCITY}}); for ( YAML::const_iterator info_itr = single_motor.begin(); From 59aaad76bb8e0ae966bb8f50afd77b44062221a4 Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Fri, 27 Oct 2023 15:46:03 -0400 Subject: [PATCH 3/7] added change to single joint command --- src/xs_driver.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index b5c94d3..5fb7a8f 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -535,6 +535,11 @@ bool InterbotixDriverXS::write_joint_command( // convert from linear position if necessary command = convert_linear_position_to_radian(name, command); } + + // get mech red value + int32_t j_reduction = js_mech_reduction_map[get_group_info(name)->joint_ids.at(i)]; + command = command * j_reduction; + XSLOG_DEBUG( "ID: %d, writing %s command %f.", motor_map[name].motor_id, mode.c_str(), command); From 03c6e7328480f112f915c9855f1f108f2f45838e Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Fri, 27 Oct 2023 15:47:07 -0400 Subject: [PATCH 4/7] added change to single joint command --- src/xs_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index 5fb7a8f..904c00a 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -537,7 +537,7 @@ bool InterbotixDriverXS::write_joint_command( } // get mech red value - int32_t j_reduction = js_mech_reduction_map[get_group_info(name)->joint_ids.at(i)]; + int32_t j_reduction = js_mech_reduction_map[motor_map[name].motor_id]; command = command * j_reduction; XSLOG_DEBUG( From d97ea72b448f836816abacdbe299ded7afaa2440 Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Wed, 17 Jan 2024 12:38:43 -0500 Subject: [PATCH 5/7] added catch exception to reading mech reduction --- src/xs_driver.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index 904c00a..4e4bad4 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -846,11 +846,14 @@ bool InterbotixDriverXS::retrieve_motor_configs( uint8_t id = (uint8_t)single_motor["ID"].as(); // extract mech reduction from node - uint32_t mech_red = (uint32_t)single_motor["Mech_Reduction"].as(); - js_mech_reduction_map[id] = mech_red; - XSLOG_FATAL( - "reading mech reduction, motor id: %i, red: %i", - id, mech_red); + try { + uint32_t mech_red = (uint32_t)single_motor["Mech_Reduction"].as(); + js_mech_reduction_map[id] = mech_red; + XSLOG_DEBUG("Reading mech reduction, motor id: %i, red: %i", id, mech_red); + } + catch (const std::exception& e) { + XSLOG_ERROR("Could not read the Mech_Reduction field in the motor configs. Motor ID: %i.", id) + } // add the motor to the motor_map with it's ID, pos as the default opmode, vel as default // profile From 69e1955e6be2bead7551e2a9eec5054cacc00ee1 Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Wed, 17 Jan 2024 13:04:07 -0500 Subject: [PATCH 6/7] throw the error properly --- src/xs_driver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index 4e4bad4..eba8198 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -852,7 +852,9 @@ bool InterbotixDriverXS::retrieve_motor_configs( XSLOG_DEBUG("Reading mech reduction, motor id: %i, red: %i", id, mech_red); } catch (const std::exception& e) { - XSLOG_ERROR("Could not read the Mech_Reduction field in the motor configs. Motor ID: %i.", id) + XSLOG_ERROR("Could not read the Mech_Reduction field in the motor configs. Motor ID: %i.", id); + XSLOG_FATAL("YAML Error: '%s'", e.what()); + return false; } // add the motor to the motor_map with it's ID, pos as the default opmode, vel as default From c7324419033395a96fb8a4dda7c5202afd9f1718 Mon Sep 17 00:00:00 2001 From: Shreeyak Date: Wed, 28 Feb 2024 17:49:27 -0500 Subject: [PATCH 7/7] corrected gear ratio to a float --- include/interbotix_xs_driver/xs_driver.hpp | 2 +- src/xs_driver.cpp | 29 ++++++++++++++-------- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/include/interbotix_xs_driver/xs_driver.hpp b/include/interbotix_xs_driver/xs_driver.hpp index 0863851..6aaa592 100644 --- a/include/interbotix_xs_driver/xs_driver.hpp +++ b/include/interbotix_xs_driver/xs_driver.hpp @@ -374,7 +374,7 @@ class InterbotixDriverXS std::unordered_map js_index_map; // Dictionary mapping the ID of a joint with its mechanical reduction - std::unordered_map js_mech_reduction_map; + std::unordered_map js_mech_reduction_map; // Vector containing the robot joint positions in [rad] at the last update std::vector robot_positions; diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index eba8198..141b5c8 100644 --- a/src/xs_driver.cpp +++ b/src/xs_driver.cpp @@ -415,13 +415,16 @@ bool InterbotixDriverXS::write_commands( commands.at(i)); } - // get mech red value - int32_t j_reduction = js_mech_reduction_map[get_group_info(name)->joint_ids.at(i)]; + // Apply mechanical reduction to the angular values + float j_reduction = js_mech_reduction_map[get_group_info(name)->joint_ids.at(i)]; + for (size_t i{0}; i < commands.size(); i++) { + commands.at(i) = commands.at(i) / j_reduction; + } - // translate from position to command value - INCLUDE MECH REDUCTION + // translate from position to command value dynamixel_commands[i] = dxl_wb.convertRadian2Value( get_group_info(name)->joint_ids.at(i), - (commands.at(i) * j_reduction)); + commands.at(i)); XSLOG_DEBUG( "ID: %d, writing %s command %d.", get_group_info(name)->joint_ids.at(i), @@ -536,9 +539,9 @@ bool InterbotixDriverXS::write_joint_command( command = convert_linear_position_to_radian(name, command); } - // get mech red value - int32_t j_reduction = js_mech_reduction_map[motor_map[name].motor_id]; - command = command * j_reduction; + // Apply mechanical reduction to the angular values + float j_reduction = js_mech_reduction_map[motor_map[name].motor_id]; + command = command / j_reduction; XSLOG_DEBUG( "ID: %d, writing %s command %f.", @@ -847,7 +850,7 @@ bool InterbotixDriverXS::retrieve_motor_configs( // extract mech reduction from node try { - uint32_t mech_red = (uint32_t)single_motor["Mech_Reduction"].as(); + float mech_red = (float)single_motor["Mech_Reduction"].as(); js_mech_reduction_map[id] = mech_red; XSLOG_DEBUG("Reading mech reduction, motor id: %i, red: %i", id, mech_red); } @@ -1368,6 +1371,11 @@ void InterbotixDriverXS::read_joint_states() } velocity = dxl_wb.convertValue2Velocity(id, get_velocity.at(index)); position = dxl_wb.convertValue2Radian(id, get_position.at(index)); + + // Apply mechanical reduction (protocol 2.0, syncRead) + float j_reduction = js_mech_reduction_map[id] + position = position * j_reduction; + robot_efforts.push_back(effort); robot_velocities.push_back(velocity); robot_positions.push_back(position); @@ -1399,8 +1407,9 @@ void InterbotixDriverXS::read_joint_states() float velocity = dxl_wb.convertValue2Velocity(id, velocity_raw); float position = dxl_wb.convertValue2Radian(id, position_raw); - // Apply mechanical reduction - position = position / js_mech_reduction_map[id]; + // Apply mechanical reduction (protocol 1.0) + float j_reduction = js_mech_reduction_map[id] + position = position * j_reduction; robot_efforts.push_back(effort); robot_velocities.push_back(velocity);