From feb2ef8ef184e1ec847cd5fe3d13c1f84c9e46c6 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Thu, 16 Jan 2025 14:21:16 +0900 Subject: [PATCH] Add reverse param --- .../dynamixel_hardware/dynamixel_hardware.hpp | 1 + dynamixel_hardware/src/dynamixel_hardware.cpp | 19 ++++++++++++++----- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index d3d5746..bb6cabb 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -45,6 +45,7 @@ struct Joint JointValue state{}; JointValue command{}; JointValue prev_command{}; + bool reverse{false}; }; enum class ControlMode diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 720db9e..04bd2e6 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -69,6 +69,10 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo joints_[i].prev_command.position = joints_[i].command.position; joints_[i].prev_command.velocity = joints_[i].command.velocity; joints_[i].prev_command.effort = joints_[i].command.effort; + if (info_.joints[i].parameters.find("reverse") != info_.joints[i].parameters.end()) { + auto reverse_str = info_.joints[i].parameters.at("reverse"); + joints_[i].reverse = reverse_str == "true" || reverse_str == "1"; + } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); } @@ -286,9 +290,12 @@ return_type DynamixelHardware::read( } for (uint i = 0; i < ids.size(); i++) { - joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); - joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); - joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]); + auto sign = joints_[i].reverse ? -1.0 : 1.0; + joints_[i].state.position = + dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]) * sign; + joints_[i].state.velocity = + dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]) * sign; + joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]) * sign; } return return_type::OK; @@ -472,8 +479,9 @@ CallbackReturn DynamixelHardware::set_joint_positions() std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); for (uint i = 0; i < ids.size(); i++) { joints_[i].prev_command.position = joints_[i].command.position; + auto sign = joints_[i].reverse ? -1.0 : 1.0; commands[i] = dynamixel_workbench_.convertRadian2Value( - ids[i], static_cast(joints_[i].command.position)); + ids[i], static_cast(joints_[i].command.position * sign)); } if (!dynamixel_workbench_.syncWrite( kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) @@ -492,8 +500,9 @@ CallbackReturn DynamixelHardware::set_joint_velocities() std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); for (uint i = 0; i < ids.size(); i++) { joints_[i].prev_command.velocity = joints_[i].command.velocity; + auto sign = joints_[i].reverse ? -1.0 : 1.0; commands[i] = dynamixel_workbench_.convertVelocity2Value( - ids[i], static_cast(joints_[i].command.velocity)); + ids[i], static_cast(joints_[i].command.velocity * sign)); } if (!dynamixel_workbench_.syncWrite( kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log))