From b0e769624053ba459105a8d9c6f225b56cbac668 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Fri, 9 Feb 2024 17:59:32 +0100 Subject: [PATCH 1/6] Swap Position to ExtendedPosition mode --- dynamixel_hardware/src/dynamixel_hardware.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 474f7b1..a5646a6 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -102,7 +102,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo } enable_torque(false); - set_control_mode(ControlMode::Position, true); + set_control_mode(ControlMode::ExtendedPosition, true); set_joint_params(); enable_torque(true); @@ -305,7 +305,7 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc // 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); + set_control_mode(ControlMode::ExtendedPosition); if(mode_changed_) { set_joint_params(); @@ -327,7 +327,7 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc set_joint_velocities(); return return_type::OK; break; - case ControlMode::Position: + case ControlMode::ExtendedPosition: set_joint_positions(); return return_type::OK; break; @@ -396,23 +396,23 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const return return_type::OK; } - if (mode == ControlMode::Position && (force_set || control_mode_ != ControlMode::Position)) { + if (mode == ControlMode::ExtendedPosition && (force_set || control_mode_ != ControlMode::ExtendedPosition)) { bool torque_enabled = torque_enabled_; if (torque_enabled) { enable_torque(false); } for (uint i = 0; i < joint_ids_.size(); ++i) { - if (!dynamixel_workbench_.setPositionControlMode(joint_ids_[i], &log)) { + if (!dynamixel_workbench_.setExtendedPositionControlMode(joint_ids_[i], &log)) { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); return return_type::ERROR; } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); - if(control_mode_ != ControlMode::Position) + if(control_mode_ != ControlMode::ExtendedPosition) { mode_changed_ = true; - control_mode_ = ControlMode::Position; + control_mode_ = ControlMode::ExtendedPosition; } if (torque_enabled) { @@ -421,7 +421,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const return return_type::OK; } - if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) { + if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::ExtendedPosition) { RCLCPP_FATAL( rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented"); return return_type::ERROR; From adb4faf88e25099348e8a5968bb194de22ab00d3 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Thu, 15 Feb 2024 15:21:54 +0000 Subject: [PATCH 2/6] Add current control to hardware interface --- .../dynamixel_hardware/dynamixel_hardware.hpp | 3 +- dynamixel_hardware/src/dynamixel_hardware.cpp | 88 +++++++++++++++++-- 2 files changed, 81 insertions(+), 10 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index 0eaf026..f447a89 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -51,7 +51,7 @@ enum class ControlMode { Position, Velocity, Torque, - Currrent, + Current, ExtendedPosition, MultiTurn, CurrentBasedPosition, @@ -94,6 +94,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 a5646a6..50d1808 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"; @@ -103,6 +105,10 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo enable_torque(false); set_control_mode(ControlMode::ExtendedPosition, true); + // if (set_control_mode(ControlMode::ExtendedPosition, true) == return_type::ERROR) + // { + // return CallbackReturn::ERROR; + // } set_joint_params(); enable_torque(true); @@ -121,6 +127,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) { @@ -147,6 +159,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; @@ -165,6 +178,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 + @@ -203,6 +223,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; @@ -316,9 +338,14 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc // 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; + joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != j.prev_command.effort; })) { + set_control_mode(ControlMode::Current); + if(mode_changed_) + { + set_joint_params(); + } + set_joint_currents(); + return return_type::OK; } // if all command values are unchanged, then remain in existing control mode and set corresponding command values @@ -331,7 +358,11 @@ 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; + default: // other RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Control mode not implemented"); return return_type::ERROR; break; @@ -420,16 +451,36 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } return return_type::OK; } - - if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::ExtendedPosition) { - RCLCPP_FATAL( - rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented"); - return return_type::ERROR; + + if (mode == ControlMode::Current && (force_set || control_mode_ != ControlMode::Current)) { + bool torque_enabled = torque_enabled_; + if (torque_enabled) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setCurrentControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Current control"); + if(control_mode_ != ControlMode::Current) + { + mode_changed_ = true; + control_mode_ = ControlMode::Current; + } + + if (torque_enabled) { + enable_torque(true); + } + return return_type::OK; } return return_type::OK; } + return_type DynamixelHardware::reset_command() { for (uint i = 0; i < joints_.size(); i++) { @@ -482,6 +533,25 @@ 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++) { + joints_[i].prev_command.effort = joints_[i].command.effort; + 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 081f76c524d40155849924474a8803a73fc8c01c Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Tue, 20 Feb 2024 15:54:59 +0100 Subject: [PATCH 3/6] Add velocity and effort update to dummy mode --- dynamixel_hardware/src/dynamixel_hardware.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 50d1808..d5e88d1 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -308,6 +308,10 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc for (auto & joint : joints_) { joint.prev_command.position = joint.command.position; joint.state.position = joint.command.position; + joint.prev_command.velocity = joint.command.velocity; + joint.state.velocity = joint.command.velocity; + joint.prev_command.effort = joint.command.effort; + joint.state.effort = joint.command.effort; } return return_type::OK; } From a9340cb9641816bed7bfba871a269c8a4acad1d3 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Thu, 21 Mar 2024 16:50:59 +0000 Subject: [PATCH 4/6] Implement the on_configure() callback --- .../dynamixel_hardware/dynamixel_hardware.hpp | 5 +- dynamixel_hardware/src/dynamixel_hardware.cpp | 75 ++++++++++++------- 2 files changed, 50 insertions(+), 30 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index f447a89..f894eba 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -67,6 +67,9 @@ class DynamixelHardware DYNAMIXEL_HARDWARE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + DYNAMIXEL_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -102,7 +105,7 @@ class DynamixelHardware std::vector joints_; std::vector joint_ids_; bool torque_enabled_{false}; - ControlMode control_mode_{ControlMode::Position}; + ControlMode control_mode_{ControlMode::ExtendedPosition}; 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 d5e88d1..dfc1243 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -52,7 +52,7 @@ constexpr const char * const kExtraJointParameters[] = { CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo & info) { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "configure"); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "init"); if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -103,15 +103,6 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo } } - enable_torque(false); - set_control_mode(ControlMode::ExtendedPosition, true); - // if (set_control_mode(ControlMode::ExtendedPosition, true) == return_type::ERROR) - // { - // return CallbackReturn::ERROR; - // } - set_joint_params(); - enable_torque(true); - const ControlItem * goal_position = dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalPositionItem); if (goal_position == nullptr) { @@ -198,9 +189,42 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo return CallbackReturn::SUCCESS; } +CallbackReturn DynamixelHardware::on_configure(const rclcpp_lifecycle::State & /* previous_state */) +{ + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "configure"); + + for (uint i = 0; i < joints_.size(); i++) { + if (use_dummy_ && std::isnan(joints_[i].state.position)) { + joints_[i].state.position = 0.0; + joints_[i].state.velocity = 0.0; + joints_[i].state.effort = 0.0; + } + } + + if (read(rclcpp::Time{}, rclcpp::Duration(0, 0)) == return_type::ERROR) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Read failed in on_configure"); + return CallbackReturn::ERROR; + } + + // for (uint i = 0; i < joints_.size(); i++) { + // RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "In on_configure, joint%d pos: %f", i, joints_[i].state.position); + // } + + enable_torque(false); + set_control_mode(ControlMode::ExtendedPosition, true); + set_joint_params(); + // Ideally torque should be enabled in on_activate(), but this appears to cause issues + // due to conflict with RT loop read/write calls, so it is here instead + enable_torque(true); + + return CallbackReturn::SUCCESS; +} + +// TODO - add on_cleanup to disable torque + std::vector DynamixelHardware::export_state_interfaces() { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_state_interfaces"); + RCLCPP_INFO(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( @@ -216,7 +240,7 @@ std::vector DynamixelHardware::export_state_ std::vector DynamixelHardware::export_command_interfaces() { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_command_interfaces"); + RCLCPP_INFO(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( @@ -232,24 +256,16 @@ std::vector DynamixelHardware::export_comm CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* previous_state */) { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start"); - for (uint i = 0; i < joints_.size(); i++) { - if (use_dummy_ && std::isnan(joints_[i].state.position)) { - joints_[i].state.position = 0.0; - joints_[i].state.velocity = 0.0; - joints_[i].state.effort = 0.0; - } - } - read(rclcpp::Time{}, rclcpp::Duration(0, 0)); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "activate"); + reset_command(); - write(rclcpp::Time{}, rclcpp::Duration(0, 0)); return CallbackReturn::SUCCESS; } CallbackReturn DynamixelHardware::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop"); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "deactivate"); return CallbackReturn::SUCCESS; } @@ -269,7 +285,8 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp if (!dynamixel_workbench_.syncRead( kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "[in syncRead() in read()] %s", log); + return return_type::ERROR; } if (!dynamixel_workbench_.getSyncReadData( @@ -277,21 +294,21 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp 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_[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_[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]); @@ -443,7 +460,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const return return_type::ERROR; } } - RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Extended Position control"); if(control_mode_ != ControlMode::ExtendedPosition) { mode_changed_ = true; @@ -558,7 +575,7 @@ CallbackReturn DynamixelHardware::set_joint_currents() CallbackReturn DynamixelHardware::set_joint_params() { - const char * log = nullptr; + 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()) { From 93d1fd2b7b900b2858bbd42716aa8e69b0e14c07 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Wed, 3 Apr 2024 09:33:24 +0100 Subject: [PATCH 5/6] Remove some debug printing --- dynamixel_hardware/src/dynamixel_hardware.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index dfc1243..ce122a9 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -206,10 +206,6 @@ CallbackReturn DynamixelHardware::on_configure(const rclcpp_lifecycle::State & / return CallbackReturn::ERROR; } - // for (uint i = 0; i < joints_.size(); i++) { - // RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "In on_configure, joint%d pos: %f", i, joints_[i].state.position); - // } - enable_torque(false); set_control_mode(ControlMode::ExtendedPosition, true); set_joint_params(); @@ -285,7 +281,7 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp if (!dynamixel_workbench_.syncRead( kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { - RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "[in syncRead() in read()] %s", log); + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); return return_type::ERROR; } From 0b6e261eb327c55fbaeff7bc27c87e3c5ee55a87 Mon Sep 17 00:00:00 2001 From: Seb Date: Sun, 26 Jan 2025 12:10:14 +0100 Subject: [PATCH 6/6] Merge and test (dummy mode only) changes from upstream --- .github/dependabot.yml | 11 ++ .github/workflows/build_and_test_foxy.yaml | 4 +- .github/workflows/build_and_test_humble.yaml | 4 +- .github/workflows/build_and_test_rolling.yaml | 8 +- dynamixel_hardware/CHANGELOG.rst | 29 ++++ dynamixel_hardware/CMakeLists.txt | 2 +- .../dynamixel_hardware/dynamixel_hardware.hpp | 12 +- dynamixel_hardware/package.xml | 2 +- dynamixel_hardware/src/dynamixel_hardware.cpp | 137 +++++++++++------- 9 files changed, 137 insertions(+), 72 deletions(-) create mode 100644 .github/dependabot.yml create mode 100644 dynamixel_hardware/CHANGELOG.rst diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..e644b37 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,11 @@ +# Set update schedule for GitHub Actions +# (https://docs.github.com/en/code-security/dependabot/working-with-dependabot/keeping-your-actions-up-to-date-with-dependabot) + +version: 2 +updates: + + - package-ecosystem: "github-actions" + directory: "/" + schedule: + # Check for updates to GitHub Actions every week + interval: "weekly" diff --git a/.github/workflows/build_and_test_foxy.yaml b/.github/workflows/build_and_test_foxy.yaml index 2860543..e655ccf 100644 --- a/.github/workflows/build_and_test_foxy.yaml +++ b/.github/workflows/build_and_test_foxy.yaml @@ -23,10 +23,10 @@ jobs: # Steps represent a sequence of tasks that will be executed as part of the job steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@v0.7 with: # The testing repository will be used use-ros2-testing: true - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: foxy diff --git a/.github/workflows/build_and_test_humble.yaml b/.github/workflows/build_and_test_humble.yaml index 63bd52f..f49863f 100644 --- a/.github/workflows/build_and_test_humble.yaml +++ b/.github/workflows/build_and_test_humble.yaml @@ -22,10 +22,10 @@ jobs: image: ubuntu:jammy steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@v0.7 with: # The testing repository will be used use-ros2-testing: true - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: humble diff --git a/.github/workflows/build_and_test_rolling.yaml b/.github/workflows/build_and_test_rolling.yaml index 30a8540..df74787 100644 --- a/.github/workflows/build_and_test_rolling.yaml +++ b/.github/workflows/build_and_test_rolling.yaml @@ -16,16 +16,16 @@ on: jobs: # This workflow contains a single job called "build" build: - # The runner will be ubuntu-latest, with a clean ubuntu jammy container + # The runner will be ubuntu-latest, with a clean ubuntu noble container runs-on: ubuntu-latest container: - image: ubuntu:jammy + image: ubuntu:noble steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@v0.7 with: # The testing repository will be used use-ros2-testing: true - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: rolling diff --git a/dynamixel_hardware/CHANGELOG.rst b/dynamixel_hardware/CHANGELOG.rst new file mode 100644 index 0000000..95aeb51 --- /dev/null +++ b/dynamixel_hardware/CHANGELOG.rst @@ -0,0 +1,29 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package dynamixel_hardware +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.0 (2024-04-24) +------------------ +* Adhere to style guide (`#73 `_) +* Revised control mode changes, added set_joint_params (`#65 `_) + * revised control mode changes, added set_params + * removed unnecessary comment +* Missing comma for setting Position_D_Gain (`#56 `_) + * comment out unused paramter + * A comma is missing for setting the Position_D_Gain +* Contributors: Geoff Sokoll, Kenji Brameld, Lass6230, Yutaka Kondo + +0.3.1 (2022-11-17) +------------------ +* Merge pull request `#39 `_ from ijnek/ijnek-unused-parameters +* Merge pull request `#31 `_ from ijnek/ijnek-unused-parameter-2 +* Merge pull request `#25 `_ from ijnek/ijnek-new-hardware-interface +* Merge pull request `#24 `_ from ijnek/ijnek-add-callbackreturn-reference +* Merge pull request `#20 `_ from pdenes/add-extra-params +* Merge pull request `#19 `_ from pdenes/add-dependencies +* Merge pull request `#17 `_ from youtalk/galactic +* Merge pull request `#16 `_ from Schnilz/main +* Merge pull request `#13 `_ from youtalk/change-loglevel +* Merge pull request `#10 `_ from youtalk/joint-id +* Merge pull request `#1 `_ from youtalk/velocity-control +* Contributors: Kenji Brameld, Nils Schulte, Pal Denes, Yutaka Kondo diff --git a/dynamixel_hardware/CMakeLists.txt b/dynamixel_hardware/CMakeLists.txt index 1d15981..d97ca69 100644 --- a/dynamixel_hardware/CMakeLists.txt +++ b/dynamixel_hardware/CMakeLists.txt @@ -42,7 +42,7 @@ ament_target_dependencies( hardware_interface pluginlib dynamixel_workbench_toolbox - ) +) pluginlib_export_plugin_description_file(hardware_interface dynamixel_hardware.xml) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index f894eba..d5f219d 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -17,14 +17,14 @@ #include +#include +#include + #include #include #include #include -#include -#include - #include "dynamixel_hardware/visiblity_control.h" #include "rclcpp/macros.hpp" @@ -47,7 +47,8 @@ struct Joint JointValue prev_command{}; }; -enum class ControlMode { +enum class ControlMode +{ Position, Velocity, Torque, @@ -58,8 +59,7 @@ enum class ControlMode { PWM, }; -class DynamixelHardware -: public hardware_interface::SystemInterface +class DynamixelHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardware) diff --git a/dynamixel_hardware/package.xml b/dynamixel_hardware/package.xml index 787424d..cab2424 100644 --- a/dynamixel_hardware/package.xml +++ b/dynamixel_hardware/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware - 0.0.0 + 0.6.0 ros2_control hardware for ROBOTIS Dynamixel Yutaka Kondo Apache 2.0 diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index ce122a9..e430631 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -77,7 +77,8 @@ 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") + { use_dummy_ = true; RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode"); return CallbackReturn::SUCCESS; @@ -156,15 +157,17 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo control_items_[kPresentCurrentItem] = present_current; if (!dynamixel_workbench_.addSyncWriteHandler( - control_items_[kGoalPositionItem]->address, control_items_[kGoalPositionItem]->data_length, - &log)) { + control_items_[kGoalPositionItem]->address, control_items_[kGoalPositionItem]->data_length, + &log)) + { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); return CallbackReturn::ERROR; } if (!dynamixel_workbench_.addSyncWriteHandler( - control_items_[kGoalVelocityItem]->address, control_items_[kGoalVelocityItem]->data_length, - &log)) { + control_items_[kGoalVelocityItem]->address, control_items_[kGoalVelocityItem]->data_length, + &log)) + { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); return CallbackReturn::ERROR; } @@ -179,8 +182,8 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo uint16_t start_address = std::min( control_items_[kPresentPositionItem]->address, control_items_[kPresentCurrentItem]->address); uint16_t read_length = control_items_[kPresentPositionItem]->data_length + - control_items_[kPresentVelocityItem]->data_length + - control_items_[kPresentCurrentItem]->data_length + 2; + control_items_[kPresentVelocityItem]->data_length + + control_items_[kPresentCurrentItem]->data_length + 2; if (!dynamixel_workbench_.addSyncReadHandler(start_address, read_length, &log)) { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); return CallbackReturn::ERROR; @@ -223,12 +226,15 @@ std::vector DynamixelHardware::export_state_ RCLCPP_INFO(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)); + 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)); } return state_interfaces; @@ -239,11 +245,14 @@ std::vector DynamixelHardware::export_comm RCLCPP_INFO(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( + 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)); } @@ -259,13 +268,16 @@ CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* return CallbackReturn::SUCCESS; } -CallbackReturn DynamixelHardware::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) +CallbackReturn DynamixelHardware::on_deactivate( + const rclcpp_lifecycle::State & /* previous_state */) { RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "deactivate"); return CallbackReturn::SUCCESS; } -return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +return_type DynamixelHardware::read( + const rclcpp::Time & /* time */, + const rclcpp::Duration & /* period */) { if (use_dummy_) { return return_type::OK; @@ -280,29 +292,33 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp const char * log = nullptr; if (!dynamixel_workbench_.syncRead( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) { + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) + { RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); return return_type::ERROR; } if (!dynamixel_workbench_.getSyncReadData( - kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), - control_items_[kPresentCurrentItem]->address, - control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) { + 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_[kPresentVelocityItem]->address, - control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) { + 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_[kPresentPositionItem]->address, - control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) { + 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); } @@ -315,7 +331,9 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp return return_type::OK; } -return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +return_type DynamixelHardware::write( + const rclcpp::Time & /* time */, + const rclcpp::Duration & /* period */) { if (use_dummy_) { for (auto & joint : joints_) { @@ -331,22 +349,26 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc // Velocity control if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != j.prev_command.velocity; })) { + joints_.cbegin(), joints_.cend(), [](auto j) { + return j.command.velocity != j.prev_command.velocity; + })) + { set_control_mode(ControlMode::Velocity); - if(mode_changed_) - { + 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; })) { + joints_.cbegin(), joints_.cend(), [](auto j) { + return j.command.position != j.prev_command.position; + })) + { set_control_mode(ControlMode::ExtendedPosition); - if(mode_changed_) - { + if (mode_changed_) { set_joint_params(); } set_joint_positions(); @@ -355,17 +377,19 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc // Effort control if (std::any_of( - joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != j.prev_command.effort; })) { - set_control_mode(ControlMode::Current); - if(mode_changed_) - { - set_joint_params(); - } - set_joint_currents(); - return return_type::OK; - } + joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != j.prev_command.effort; })) + { + set_control_mode(ControlMode::Current); + if(mode_changed_) + { + set_joint_params(); + } + set_joint_currents(); + return return_type::OK; + } - // if all command values are unchanged, then remain in existing control mode and set corresponding command values + // 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(); @@ -384,7 +408,6 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc return return_type::ERROR; break; } - } return_type DynamixelHardware::enable_torque(const bool enabled) @@ -432,8 +455,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); - if(control_mode_ != ControlMode::Velocity) - { + if (control_mode_ != ControlMode::Velocity) { mode_changed_ = true; control_mode_ = ControlMode::Velocity; } @@ -457,8 +479,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const } } RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Extended Position control"); - if(control_mode_ != ControlMode::ExtendedPosition) - { + if (control_mode_ != ControlMode::ExtendedPosition) { mode_changed_ = true; control_mode_ = ControlMode::ExtendedPosition; } @@ -525,7 +546,8 @@ CallbackReturn DynamixelHardware::set_joint_positions() ids[i], static_cast(joints_[i].command.position)); } if (!dynamixel_workbench_.syncWrite( - kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { + kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) + { RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); } return CallbackReturn::SUCCESS; @@ -544,7 +566,8 @@ CallbackReturn DynamixelHardware::set_joint_velocities() ids[i], static_cast(joints_[i].command.velocity)); } if (!dynamixel_workbench_.syncWrite( - kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log)) { + kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log)) + { RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); } return CallbackReturn::SUCCESS; @@ -580,7 +603,9 @@ CallbackReturn DynamixelHardware::set_joint_params() 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); + RCLCPP_INFO( + rclcpp::get_logger( + kDynamixelHardware), "%s set to %d for joint %d", paramName, value, i); } } }