From b44fcaa315cb91aad107c6bbcaa22694d5e54334 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Mon, 26 Feb 2024 10:28:53 +0000 Subject: [PATCH 01/11] [Remove]previous joint commands --- .../dynamixel_hardware/dynamixel_hardware.hpp | 1 - dynamixel_hardware/src/dynamixel_hardware.cpp | 40 ------------------- 2 files changed, 41 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index 0eaf026..be22395 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -44,7 +44,6 @@ struct Joint { JointValue state{}; JointValue command{}; - JointValue prev_command{}; }; enum class ControlMode { diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 474f7b1..a9dbda4 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -67,9 +67,6 @@ 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]); } @@ -284,43 +281,11 @@ 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; } - // Velocity control - if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != j.prev_command.velocity; })) { - set_control_mode(ControlMode::Velocity); - if(mode_changed_) - { - set_joint_params(); - } - 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; - } - - // Effort control - if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != 0.0; })) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Effort control is not implemented"); - return return_type::ERROR; - } - // if all command values are unchanged, then remain in existing control mode and set corresponding command values switch (control_mode_) { case ControlMode::Velocity: @@ -436,9 +401,6 @@ 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; @@ -452,7 +414,6 @@ 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; commands[i] = dynamixel_workbench_.convertRadian2Value( ids[i], static_cast(joints_[i].command.position)); } @@ -471,7 +432,6 @@ 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; commands[i] = dynamixel_workbench_.convertVelocity2Value( ids[i], static_cast(joints_[i].command.velocity)); } From 85df0b974e2daa29fe9aa51480d29211bd59a878 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Mon, 26 Feb 2024 10:41:48 +0000 Subject: [PATCH 02/11] [Fix]a typo --- .../include/dynamixel_hardware/dynamixel_hardware.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index be22395..739b1b6 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -50,7 +50,7 @@ enum class ControlMode { Position, Velocity, Torque, - Currrent, + Current, ExtendedPosition, MultiTurn, CurrentBasedPosition, From b59cdc4f3df7b176f1524d8c4e7c0adaf47aca35 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Mon, 26 Feb 2024 12:06:23 +0000 Subject: [PATCH 03/11] [Add]effort command interface --- dynamixel_hardware/src/dynamixel_hardware.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index a9dbda4..df89598 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -200,6 +200,8 @@ std::vector DynamixelHardware::export_comm info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].command.position)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].command.effort)); } return command_interfaces; From c06216f1eed03124ba4bc1d409a03b0acc5b0129 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Mon, 26 Feb 2024 12:20:36 +0000 Subject: [PATCH 04/11] [Add]command interfaces switching using ros2_control lifecycle --- .../dynamixel_hardware/dynamixel_hardware.hpp | 9 +- dynamixel_hardware/src/dynamixel_hardware.cpp | 82 ++++++++++++++++++- 2 files changed, 89 insertions(+), 2 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index 739b1b6..b6fbd64 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -55,6 +55,7 @@ enum class ControlMode { MultiTurn, CurrentBasedPosition, PWM, + NoControl, }; class DynamixelHardware @@ -75,6 +76,12 @@ class DynamixelHardware DYNAMIXEL_HARDWARE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + DYNAMIXEL_HARDWARE_PUBLIC + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + DYNAMIXEL_HARDWARE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; @@ -100,7 +107,7 @@ class DynamixelHardware std::vector joints_; std::vector joint_ids_; bool torque_enabled_{false}; - ControlMode control_mode_{ControlMode::Position}; + ControlMode control_mode_{ControlMode::NoControl}; bool mode_changed_{false}; bool use_dummy_{false}; }; diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index df89598..64b207b 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -99,7 +99,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo } enable_torque(false); - set_control_mode(ControlMode::Position, true); + set_control_mode(control_mode_, true); set_joint_params(); enable_torque(true); @@ -207,6 +207,86 @@ std::vector DynamixelHardware::export_comm return command_interfaces; } + +return_type DynamixelHardware::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + std::vector new_modes; + if(control_mode_ == ControlMode::CurrentBasedPosition){ + new_modes.push_back(ControlMode::Position); + new_modes.push_back(ControlMode::Current); + } else if(control_mode_ != ControlMode::NoControl){ + new_modes.push_back(control_mode_); + } else { //cotrol_mode_ == ControlMode::NoControl + } + + std::vector> new_joint_modes(info_.joints.size()); + for (std::size_t i = 0; i < info_.joints.size(); i++){ + for (std::string key : start_interfaces){ + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ + new_joint_modes[i].push_back(ControlMode::Position); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ + new_joint_modes[i].push_back(ControlMode::Velocity); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT){ + new_joint_modes[i].push_back(ControlMode::Current); + } + } + } + if (std::all_of( + new_joint_modes.begin() + 1, new_joint_modes.end(), + [&](const std::vector& mode) { return mode == new_joint_modes[0]; })){ + for(auto mode:new_joint_modes[0]){ + new_modes.push_back(mode); + } + } else { + return return_type::ERROR; + } + + std::vector> stop_joint_modes(info_.joints.size()); + for (std::string key : stop_interfaces){ + for (std::size_t i = 0; i < info_.joints.size(); i++){ + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ + stop_joint_modes[i].push_back(ControlMode::Position); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ + stop_joint_modes[i].push_back(ControlMode::Velocity); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT){ + stop_joint_modes[i].push_back(ControlMode::Current); + } + } + } + if (std::all_of( + stop_joint_modes.begin() + 1, stop_joint_modes.end(), + [&](const std::vector& mode) { return mode == stop_joint_modes[0];})){ + for(auto mode:stop_joint_modes[0]){ + auto new_end = std::remove(new_modes.begin(), new_modes.end(), mode); + new_modes.erase(new_end, new_modes.end()); + } + } else { + return return_type::ERROR; + } + + if(new_modes.size() == 1){ + control_mode_ = new_modes[0]; + }else if( + std::find(new_modes.begin(), new_modes.end(), ControlMode::Position) != new_modes.end() && + std::find(new_modes.begin(), new_modes.end(), ControlMode::Current) != new_modes.end()){ + control_mode_ = ControlMode::CurrentBasedPosition; + }else if(new_modes.size() == 0){ + RCLCPP_WARN(rclcpp::get_logger(kDynamixelHardware), "No Cotrol Interface, Torque disabled"); + control_mode_ = ControlMode::NoControl; + } else { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Not implemented hardware interfaces"); + return return_type::ERROR; + } + + return return_type::OK; +} + CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* previous_state */) { RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start"); From 86da4367412476249e2bdaabd6d00a922e3355b6 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 03:06:18 +0000 Subject: [PATCH 05/11] [Change]Execute 'set_control_mode' function only on comannd interfaces switch --- .../dynamixel_hardware/dynamixel_hardware.hpp | 3 +- dynamixel_hardware/src/dynamixel_hardware.cpp | 67 +++++++++++-------- 2 files changed, 41 insertions(+), 29 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index b6fbd64..a37101a 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -94,7 +94,7 @@ class DynamixelHardware private: return_type enable_torque(const bool enabled); - return_type set_control_mode(const ControlMode & mode, const bool force_set = false); + return_type set_control_mode(const ControlMode & mode); return_type reset_command(); @@ -108,6 +108,7 @@ class DynamixelHardware std::vector joint_ids_; bool torque_enabled_{false}; ControlMode control_mode_{ControlMode::NoControl}; + ControlMode prev_control_mode_{ControlMode::NoControl}; bool mode_changed_{false}; bool use_dummy_{false}; }; diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 64b207b..b0705eb 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -99,7 +99,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo } enable_torque(false); - set_control_mode(control_mode_, true); + set_control_mode(control_mode_); set_joint_params(); enable_torque(true); @@ -367,9 +367,17 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc } return return_type::OK; } + + if(control_mode_ != prev_control_mode_){ + set_control_mode(control_mode_); + set_joint_params(); + prev_control_mode_ = control_mode_; + } - // if all command values are unchanged, then remain in existing control mode and set corresponding command values switch (control_mode_) { + case ControlMode::NoControl: + return return_type::OK; + break; case ControlMode::Velocity: set_joint_velocities(); return return_type::OK; @@ -413,39 +421,48 @@ return_type DynamixelHardware::enable_torque(const bool enabled) return return_type::OK; } -return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const bool force_set) +return_type DynamixelHardware::set_control_mode(const ControlMode & mode) { const char * log = nullptr; mode_changed_ = false; - if (mode == ControlMode::Velocity && (force_set || control_mode_ != ControlMode::Velocity)) { - bool torque_enabled = torque_enabled_; - if (torque_enabled) { + if (mode == ControlMode::NoControl) { + if (torque_enabled_) { enable_torque(false); } for (uint i = 0; i < joint_ids_.size(); ++i) { - if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) { + if (!dynamixel_workbench_.setPositionControlMode(joint_ids_[i], &log)) { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); return return_type::ERROR; } } - RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); - if(control_mode_ != ControlMode::Velocity) - { - mode_changed_ = true; - control_mode_ = ControlMode::Velocity; + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control,but no torque mode"); + mode_changed_ = true; + + return return_type::OK; + } + + if (mode == ControlMode::Velocity) { + if (torque_enabled_) { + enable_torque(false); } - if (torque_enabled) { - enable_torque(true); + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); + mode_changed_ = true; + + enable_torque(true); return return_type::OK; } - if (mode == ControlMode::Position && (force_set || control_mode_ != ControlMode::Position)) { - bool torque_enabled = torque_enabled_; - if (torque_enabled) { + if (mode == ControlMode::Position) { + if (torque_enabled_) { enable_torque(false); } @@ -456,21 +473,15 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); - if(control_mode_ != ControlMode::Position) - { - mode_changed_ = true; - control_mode_ = ControlMode::Position; - } + mode_changed_ = true; - if (torque_enabled) { - enable_torque(true); - } + enable_torque(true); return return_type::OK; } - - if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) { + + if (mode != ControlMode::Velocity && mode != ControlMode::Position && mode != ControlMode::Current && mode != ControlMode::CurrentBasedPosition) { RCLCPP_FATAL( - rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented"); + rclcpp::get_logger(kDynamixelHardware), "Only position/velocity/current/current based position control are implemented"); return return_type::ERROR; } From 9186d0603c8580bb9e1ad51028bd6bb38f5568e6 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 03:11:15 +0000 Subject: [PATCH 06/11] [Remove]unused variable. --- .../include/dynamixel_hardware/dynamixel_hardware.hpp | 1 - dynamixel_hardware/src/dynamixel_hardware.cpp | 4 ---- 2 files changed, 5 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index a37101a..bf9c50c 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -109,7 +109,6 @@ class DynamixelHardware bool torque_enabled_{false}; ControlMode control_mode_{ControlMode::NoControl}; ControlMode prev_control_mode_{ControlMode::NoControl}; - 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 b0705eb..036de09 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -424,7 +424,6 @@ return_type DynamixelHardware::enable_torque(const bool enabled) return_type DynamixelHardware::set_control_mode(const ControlMode & mode) { const char * log = nullptr; - mode_changed_ = false; if (mode == ControlMode::NoControl) { if (torque_enabled_) { @@ -438,7 +437,6 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode) } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control,but no torque mode"); - mode_changed_ = true; return return_type::OK; } @@ -455,7 +453,6 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode) } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); - mode_changed_ = true; enable_torque(true); return return_type::OK; @@ -473,7 +470,6 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode) } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); - mode_changed_ = true; enable_torque(true); return return_type::OK; From faed640e35536898300a8e3c882e048896255ce4 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 03:22:24 +0000 Subject: [PATCH 07/11] [Add]current control, current based position control mode --- .../dynamixel_hardware/dynamixel_hardware.hpp | 1 + dynamixel_hardware/src/dynamixel_hardware.cpp | 79 ++++++++++++++++++- 2 files changed, 79 insertions(+), 1 deletion(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index bf9c50c..c221bff 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -100,6 +100,7 @@ class DynamixelHardware CallbackReturn set_joint_positions(); CallbackReturn set_joint_velocities(); + CallbackReturn set_joint_currents(); CallbackReturn set_joint_params(); DynamixelWorkbench dynamixel_workbench_; diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 036de09..0dfa8e7 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -29,9 +29,11 @@ namespace dynamixel_hardware constexpr const char * kDynamixelHardware = "DynamixelHardware"; constexpr uint8_t kGoalPositionIndex = 0; constexpr uint8_t kGoalVelocityIndex = 1; +constexpr uint8_t kGoalCurrentIndex = 2; constexpr uint8_t kPresentPositionVelocityCurrentIndex = 0; constexpr const char * kGoalPositionItem = "Goal_Position"; constexpr const char * kGoalVelocityItem = "Goal_Velocity"; +constexpr const char * kGoalCurrentItem = "Goal_Current"; constexpr const char * kMovingSpeedItem = "Moving_Speed"; constexpr const char * kPresentPositionItem = "Present_Position"; constexpr const char * kPresentVelocityItem = "Present_Velocity"; @@ -118,6 +120,12 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo return CallbackReturn::ERROR; } + const ControlItem * goal_current = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalCurrentItem); + if (goal_current == nullptr) { + return CallbackReturn::ERROR; + } + const ControlItem * present_position = dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentPositionItem); if (present_position == nullptr) { @@ -144,6 +152,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo control_items_[kGoalPositionItem] = goal_position; control_items_[kGoalVelocityItem] = goal_velocity; + control_items_[kGoalCurrentItem] = goal_current; control_items_[kPresentPositionItem] = present_position; control_items_[kPresentVelocityItem] = present_velocity; control_items_[kPresentCurrentItem] = present_current; @@ -162,6 +171,13 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo return CallbackReturn::ERROR; } + if (!dynamixel_workbench_.addSyncWriteHandler( + control_items_[kGoalCurrentItem]->address, control_items_[kGoalCurrentItem]->data_length, + &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + uint16_t start_address = std::min( control_items_[kPresentPositionItem]->address, control_items_[kPresentCurrentItem]->address); uint16_t read_length = control_items_[kPresentPositionItem]->data_length + @@ -386,7 +402,16 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc set_joint_positions(); return return_type::OK; break; - default: // effort, etc + case ControlMode::Current: + set_joint_currents(); + return return_type::OK; + break; + case ControlMode::CurrentBasedPosition: + set_joint_currents(); + set_joint_positions(); + return return_type::OK; + break; + default: // torque, etc RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Control mode not implemented"); return return_type::ERROR; break; @@ -475,6 +500,40 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode) return return_type::OK; } + if (mode == ControlMode::Current) { + if (torque_enabled_) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setTorqueControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Current control"); + + enable_torque(true); + return return_type::OK; + } + + if (mode == ControlMode::CurrentBasedPosition) { + if (torque_enabled_) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setCurrentBasedPositionControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "CurrentBasedPosition control"); + + enable_torque(true); + return return_type::OK; + } + if (mode != ControlMode::Velocity && mode != ControlMode::Position && mode != ControlMode::Current && mode != ControlMode::CurrentBasedPosition) { RCLCPP_FATAL( rclcpp::get_logger(kDynamixelHardware), "Only position/velocity/current/current based position control are implemented"); @@ -531,6 +590,24 @@ CallbackReturn DynamixelHardware::set_joint_velocities() return CallbackReturn::SUCCESS; } +CallbackReturn DynamixelHardware::set_joint_currents() +{ + 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++) { + commands[i] = dynamixel_workbench_.convertCurrent2Value( + ids[i], static_cast(joints_[i].command.effort)); + } + if (!dynamixel_workbench_.syncWrite( + kGoalCurrentIndex, 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; From c95ce5c12015ab17eeb6826ea91bf3600e791063 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 04:16:58 +0000 Subject: [PATCH 08/11] [Improve]format code --- .../include/dynamixel_hardware/dynamixel_hardware.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index c221bff..776338e 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -73,14 +73,13 @@ class DynamixelHardware DYNAMIXEL_HARDWARE_PUBLIC std::vector export_command_interfaces() override; - DYNAMIXEL_HARDWARE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - DYNAMIXEL_HARDWARE_PUBLIC return_type prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) override; - + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; DYNAMIXEL_HARDWARE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; From 193dc8512ad0ded41da339707ccc96db1c2eee2d Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 04:33:31 +0000 Subject: [PATCH 09/11] [Add]export command/state interfaces using joints info --- dynamixel_hardware/src/dynamixel_hardware.cpp | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 0dfa8e7..90f5207 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -196,12 +196,20 @@ std::vector DynamixelHardware::export_state_ RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_state_interfaces"); std::vector state_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].state.position)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].state.velocity)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].state.effort)); + for(auto joint_interface:info_.joints[i].state_interfaces){ + if(joint_interface.name == hardware_interface::HW_IF_POSITION){ + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].state.position)); + } + if(joint_interface.name == hardware_interface::HW_IF_VELOCITY){ + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].state.velocity)); + } + if(joint_interface.name == hardware_interface::HW_IF_EFFORT){ + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].state.effort)); + } + } } return state_interfaces; @@ -212,12 +220,20 @@ std::vector DynamixelHardware::export_comm RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_command_interfaces"); std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].command.position)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].command.effort)); + for(auto joint_interface:info_.joints[i].command_interfaces){ + if(joint_interface.name == hardware_interface::HW_IF_POSITION){ + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].command.position)); + } + if(joint_interface.name == hardware_interface::HW_IF_VELOCITY){ + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity)); + } + if(joint_interface.name == hardware_interface::HW_IF_EFFORT){ + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].command.effort)); + } + } } return command_interfaces; From 3eb921d7cb2f669eece591addb83d6564798e757 Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 09:16:57 +0000 Subject: [PATCH 10/11] [Add]enable control of TTL and RS485 motors a via a single U2D2 --- .../dynamixel_hardware/dynamixel_hardware.hpp | 2 + dynamixel_hardware/src/dynamixel_hardware.cpp | 119 +++++++++++++----- 2 files changed, 89 insertions(+), 32 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index 776338e..bb9710f 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -106,6 +106,8 @@ class DynamixelHardware std::map control_items_; std::vector joints_; std::vector joint_ids_; + std::vector joint_ids_ttl_; + std::vector joint_ids_rs_; bool torque_enabled_{false}; ControlMode control_mode_{ControlMode::NoControl}; ControlMode prev_control_mode_{ControlMode::NoControl}; diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 90f5207..df28cf1 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -70,6 +70,12 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo joints_[i].command.velocity = std::numeric_limits::quiet_NaN(); joints_[i].command.effort = std::numeric_limits::quiet_NaN(); RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); + + if(info_.joints[i].parameters.at("protocol") == "TTL"){ + joint_ids_ttl_.push_back(joint_ids_[i]); + }else if(info_.joints[i].parameters.at("protocol") == "RS"){ + joint_ids_rs_.push_back(joint_ids_[i]); + } } if ( @@ -347,47 +353,96 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp if (use_dummy_) { return return_type::OK; } + + if(!joint_ids_ttl_.empty() && !joint_ids_rs_.empty()){ + std::vector* ids_each[] = {&joint_ids_ttl_, &joint_ids_rs_}; + for(auto& idt: ids_each){ + std::vector ids(idt->size(), 0); + std::vector positions(idt->size(), 0); + std::vector velocities(idt->size(), 0); + std::vector currents(idt->size(), 0); + std::copy(idt->begin(), idt->end(), ids.begin()); + const char * log = nullptr; + + if (!dynamixel_workbench_.syncRead( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - std::vector ids(info_.joints.size(), 0); - std::vector positions(info_.joints.size(), 0); - std::vector velocities(info_.joints.size(), 0); - std::vector currents(info_.joints.size(), 0); + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentCurrentItem]->address, + control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); - const char * log = nullptr; + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentVelocityItem]->address, + control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.syncRead( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentPositionItem]->address, + control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentCurrentItem]->address, - control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + for(uint i = 0; i < ids.size(); i++){ + auto it = std::find(joint_ids_.begin(), joint_ids_.end(), ids[i]); + if(it != joint_ids_.end()){ + int index = std::distance(joint_ids_.begin(), it); + joints_[index].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); + joints_[index].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); + joints_[index].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]); + } + } + } + }else{//When TTL and RS485 Protocols are not used at the same time + std::vector ids(info_.joints.size(), 0); + std::vector positions(info_.joints.size(), 0); + std::vector velocities(info_.joints.size(), 0); + std::vector currents(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + const char * log = nullptr; + + if (!dynamixel_workbench_.syncRead( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentVelocityItem]->address, - control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentCurrentItem]->address, + control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentPositionItem]->address, - control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); - } + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentVelocityItem]->address, + control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } - 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]); + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentPositionItem]->address, + control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + 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]); + } } + return return_type::OK; } From 909f7de545d57e2af19b857c2604f0d5fb06781e Mon Sep 17 00:00:00 2001 From: Yoshimura Kouki Date: Tue, 27 Feb 2024 09:38:32 +0000 Subject: [PATCH 11/11] [Add]allow passing parameter via xacro args --- dynamixel_hardware/src/dynamixel_hardware.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index df28cf1..c15dc78 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -80,7 +80,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo if ( info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() && - info_.hardware_parameters.at("use_dummy") == "true") { + (info_.hardware_parameters.at("use_dummy") == "true" || info_.hardware_parameters.at("use_dummy") == "True")) { use_dummy_ = true; RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode"); return CallbackReturn::SUCCESS;