From 90d80c38960656e56f441bf53556faec282476ef Mon Sep 17 00:00:00 2001 From: Geoff Sokoll Date: Tue, 25 Jul 2023 16:51:41 +1000 Subject: [PATCH] Revised control mode changes, added set_joint_params (#64) * revised control mode changes, added set_params * removed unnecessary comment --- .../dynamixel_hardware/dynamixel_hardware.hpp | 6 + dynamixel_hardware/src/dynamixel_hardware.cpp | 164 +++++++++++++----- 2 files changed, 125 insertions(+), 45 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index 2af549d..0eaf026 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -44,6 +44,7 @@ struct Joint { JointValue state{}; JointValue command{}; + JointValue prev_command{}; }; enum class ControlMode { @@ -91,12 +92,17 @@ class DynamixelHardware return_type reset_command(); + CallbackReturn set_joint_positions(); + CallbackReturn set_joint_velocities(); + CallbackReturn set_joint_params(); + DynamixelWorkbench dynamixel_workbench_; std::map control_items_; std::vector joints_; std::vector joint_ids_; bool torque_enabled_{false}; ControlMode control_mode_{ControlMode::Position}; + bool mode_changed_{false}; bool use_dummy_{false}; }; } // namespace dynamixel_hardware diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 3815adf..474f7b1 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -43,7 +43,7 @@ constexpr const char * const kExtraJointParameters[] = { "Profile_Acceleration", "Position_P_Gain", "Position_I_Gain", - "Position_D_Gain" + "Position_D_Gain", "Velocity_P_Gain", "Velocity_I_Gain", }; @@ -67,6 +67,9 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo joints_[i].command.position = std::numeric_limits::quiet_NaN(); joints_[i].command.velocity = std::numeric_limits::quiet_NaN(); joints_[i].command.effort = std::numeric_limits::quiet_NaN(); + 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; RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); } @@ -100,18 +103,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo enable_torque(false); set_control_mode(ControlMode::Position, true); - for (uint i = 0; i < info_.joints.size(); ++i) { - for (auto paramName : kExtraJointParameters) { - if (info_.joints[i].parameters.find(paramName) != info_.joints[i].parameters.end()) { - auto value = std::stoi(info_.joints[i].parameters.at(paramName)); - if (!dynamixel_workbench_.itemWrite(joint_ids_[i], paramName, value, &log)) { - RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); - return CallbackReturn::ERROR; - } - RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "%s set to %d for joint %d", paramName, value, i); - } - } - } + set_joint_params(); enable_torque(true); const ControlItem * goal_position = @@ -292,50 +284,59 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc { if (use_dummy_) { for (auto & joint : joints_) { + joint.prev_command.position = joint.command.position; joint.state.position = joint.command.position; } - return return_type::OK; } - std::vector ids(info_.joints.size(), 0); - std::vector commands(info_.joints.size(), 0); - - std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); - const char * log = nullptr; - + // Velocity control if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != 0.0; })) { - // Velocity control + joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != j.prev_command.velocity; })) { set_control_mode(ControlMode::Velocity); - for (uint i = 0; i < ids.size(); i++) { - commands[i] = dynamixel_workbench_.convertVelocity2Value( - ids[i], static_cast(joints_[i].command.velocity)); + if(mode_changed_) + { + set_joint_params(); } - if (!dynamixel_workbench_.syncWrite( - kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + set_joint_velocities(); + return return_type::OK; + } + + // Position control + if (std::any_of( + joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.position != j.prev_command.position; })) { + set_control_mode(ControlMode::Position); + if(mode_changed_) + { + set_joint_params(); } + set_joint_positions(); return return_type::OK; - } else if (std::any_of( + } + + // Effort control + if (std::any_of( joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != 0.0; })) { - // Effort control RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Effort control is not implemented"); return return_type::ERROR; } - // Position control - set_control_mode(ControlMode::Position); - for (uint i = 0; i < ids.size(); i++) { - commands[i] = dynamixel_workbench_.convertRadian2Value( - ids[i], static_cast(joints_[i].command.position)); - } - if (!dynamixel_workbench_.syncWrite( - kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + // if all command values are unchanged, then remain in existing control mode and set corresponding command values + switch (control_mode_) { + case ControlMode::Velocity: + set_joint_velocities(); + return return_type::OK; + break; + case ControlMode::Position: + set_joint_positions(); + return return_type::OK; + break; + default: // effort, etc + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Control mode not implemented"); + return return_type::ERROR; + break; } - return return_type::OK; } return_type DynamixelHardware::enable_torque(const bool enabled) @@ -368,6 +369,7 @@ return_type DynamixelHardware::enable_torque(const bool enabled) return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const bool force_set) { const char * log = nullptr; + mode_changed_ = false; if (mode == ControlMode::Velocity && (force_set || control_mode_ != ControlMode::Velocity)) { bool torque_enabled = torque_enabled_; @@ -382,13 +384,19 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); - control_mode_ = ControlMode::Velocity; + if(control_mode_ != ControlMode::Velocity) + { + mode_changed_ = true; + control_mode_ = ControlMode::Velocity; + } if (torque_enabled) { enable_torque(true); } - } else if ( - mode == ControlMode::Position && (force_set || control_mode_ != ControlMode::Position)) { + return return_type::OK; + } + + if (mode == ControlMode::Position && (force_set || control_mode_ != ControlMode::Position)) { bool torque_enabled = torque_enabled_; if (torque_enabled) { enable_torque(false); @@ -401,12 +409,19 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); - control_mode_ = ControlMode::Position; + if(control_mode_ != ControlMode::Position) + { + mode_changed_ = true; + control_mode_ = ControlMode::Position; + } if (torque_enabled) { enable_torque(true); } - } else if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) { + return return_type::OK; + } + + if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) { RCLCPP_FATAL( rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented"); return return_type::ERROR; @@ -421,11 +436,70 @@ return_type DynamixelHardware::reset_command() joints_[i].command.position = joints_[i].state.position; joints_[i].command.velocity = 0.0; joints_[i].command.effort = 0.0; + 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; } return return_type::OK; } +CallbackReturn DynamixelHardware::set_joint_positions() +{ + const char * log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + 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; + commands[i] = dynamixel_workbench_.convertRadian2Value( + ids[i], static_cast(joints_[i].command.position)); + } + if (!dynamixel_workbench_.syncWrite( + kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::set_joint_velocities() +{ + const char * log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + 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; + commands[i] = dynamixel_workbench_.convertVelocity2Value( + ids[i], static_cast(joints_[i].command.velocity)); + } + if (!dynamixel_workbench_.syncWrite( + kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::set_joint_params() +{ + const char * log = nullptr; + for (uint i = 0; i < info_.joints.size(); ++i) { + for (auto paramName : kExtraJointParameters) { + if (info_.joints[i].parameters.find(paramName) != info_.joints[i].parameters.end()) { + auto value = std::stoi(info_.joints[i].parameters.at(paramName)); + if (!dynamixel_workbench_.itemWrite(joint_ids_[i], paramName, value, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "%s set to %d for joint %d", paramName, value, i); + } + } + } + return CallbackReturn::SUCCESS; +} + } // namespace dynamixel_hardware #include "pluginlib/class_list_macros.hpp"