From dadc88ebde0518dc077cc7a26ee89613d48b7070 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Fri, 23 Feb 2024 19:44:46 +0100 Subject: [PATCH 01/19] read robot_description file --- .../test/dynamixel/dynamixel_joint_trajectory_launch.py | 2 ++ mep3_hardware/test/dynamixel/dynamixel_launch.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/mep3_hardware/test/dynamixel/dynamixel_joint_trajectory_launch.py b/mep3_hardware/test/dynamixel/dynamixel_joint_trajectory_launch.py index 7386acb44..7890de2a8 100644 --- a/mep3_hardware/test/dynamixel/dynamixel_joint_trajectory_launch.py +++ b/mep3_hardware/test/dynamixel/dynamixel_joint_trajectory_launch.py @@ -8,6 +8,8 @@ def generate_launch_description(): controller_params_file = os.path.join( package_dir, 'controllers.yaml') robot_description = os.path.join(package_dir, 'description.urdf') + with open(robot_description, 'r') as file: + robot_description = file.read() return LaunchDescription([ Node( diff --git a/mep3_hardware/test/dynamixel/dynamixel_launch.py b/mep3_hardware/test/dynamixel/dynamixel_launch.py index 401b8b570..a930e86aa 100644 --- a/mep3_hardware/test/dynamixel/dynamixel_launch.py +++ b/mep3_hardware/test/dynamixel/dynamixel_launch.py @@ -8,6 +8,8 @@ def generate_launch_description(): controller_params_file = os.path.join( package_dir, 'controllers.yaml') robot_description = os.path.join(package_dir, 'description.urdf') + with open(robot_description, 'r') as file: + robot_description = file.read() return LaunchDescription([ Node( From 02bbec86f8735a04e35a544e8a0c112d9ec4dfd5 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Fri, 23 Feb 2024 19:46:29 +0100 Subject: [PATCH 02/19] update test joints --- mep3_hardware/test/dynamixel/controllers.yaml | 3 +- mep3_hardware/test/dynamixel/description.urdf | 51 +++---------------- 2 files changed, 8 insertions(+), 46 deletions(-) diff --git a/mep3_hardware/test/dynamixel/controllers.yaml b/mep3_hardware/test/dynamixel/controllers.yaml index 6dd271449..2ec3dc05c 100644 --- a/mep3_hardware/test/dynamixel/controllers.yaml +++ b/mep3_hardware/test/dynamixel/controllers.yaml @@ -17,9 +17,10 @@ joint_trajectory_controller: state_interfaces: - position + - velocity + - effort joint_position_controller: ros__parameters: joints: - joint1 - - joint2 diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index 6c1d4d892..8b343d030 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -1,55 +1,16 @@ + dynamixel_hardware/DynamixelHardware /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 115200 - false - 2.618 + false + 2.618 - - 3 - - - - - - - - 7 - - - - - - - - 8 - - - - - - - - 14 - - - - - - - - 9 - - - - - - - - 55 + + 5 @@ -57,4 +18,4 @@ - + \ No newline at end of file From 87b9e9447b1a10dc15e74fc77543e56ff741ff0f Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Fri, 23 Feb 2024 21:37:08 +0100 Subject: [PATCH 03/19] effort filtering and overload detection --- .../dynamixel_hardware.hpp | 4 ++ .../dynamixel_hardware.cpp | 61 ++++++++++++++----- mep3_hardware/test/dynamixel/description.urdf | 3 +- 3 files changed, 52 insertions(+), 16 deletions(-) diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index 9c70269cf..b8a83551e 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -24,6 +24,7 @@ #include #include +#include #include "mep3_hardware/dynamixel_hardware_interface/visibility_control.hpp" #include "rclcpp/macros.hpp" @@ -38,6 +39,8 @@ struct JointValue double position{0.0}; double velocity{0.0}; double effort{0.0}; + bool overloaded; + std::deque previous_efforts_{}; }; struct Joint @@ -106,6 +109,7 @@ class DynamixelHardware bool use_dummy_{false}; double offset_{0}; bool keep_read_write_thread_{true}; + unsigned int effort_filter_ {0}; }; } // namespace dynamixel_hardware diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index c9a904519..24d6ec861 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -59,12 +59,21 @@ namespace dynamixel_hardware joints_.resize(info_.joints.size(), Joint()); joint_ids_.resize(info_.joints.size(), 0); + try { + effort_filter_ = std::stoi(info_.hardware_parameters.at("effort_filter")); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_filter: %d", effort_filter_); + } catch (const std::out_of_range& e) { + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_filter: %d [default]", effort_filter_); + } + for (uint i = 0; i < info_.joints.size(); i++) { joint_ids_[i] = std::stoi(info_.joints[i].parameters.at("id")); joints_[i].state.position = std::numeric_limits::quiet_NaN(); joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); joints_[i].state.effort = std::numeric_limits::quiet_NaN(); + joints_[i].state.previous_efforts_ = std::deque(); + joints_[i].state.previous_efforts_.resize(effort_filter_); 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(); @@ -412,32 +421,54 @@ namespace dynamixel_hardware for (uint i = 0; i < ids.size(); i++) { // https://emanual.robotis.com/docs/en/dxl/ax/ax-12a/ - // ax12 present position address: 36 - // ax12 present speed address: 38 - // ax12 present load address: 40 - - unsigned int data[6]; - if (!dynamixel_workbench_.readRegister(ids[i], 36, 6, data, &log)) { + // ax12 present position address: 36 [2B] + // ax12 present speed address: 38 [2B] + // ax12 present load address: 40 [2B] + // ax12 present voltage address: 42 [1B] + // ax12 present temperature address: 43 [1B] + + const unsigned PRESENT_DATA_ADDRESS = 36; + const unsigned PRESENT_DATA_BYTES = 2+2+2; + const unsigned TORQUE_LOAD_MAX = 1023; + + unsigned int present_data[PRESENT_DATA_BYTES]; + if (!dynamixel_workbench_.readRegister(ids[i], PRESENT_DATA_ADDRESS, PRESENT_DATA_BYTES, present_data, &log)) { RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "read0: %s", log); - dynamixel_workbench_.itemWrite(ids[i], "Torque_Limit", (int32_t)1023, &log); + dynamixel_workbench_.itemWrite(ids[i], "Torque_Limit", (int32_t)TORQUE_LOAD_MAX, &log); dynamixel_workbench_.itemWrite(ids[i], "Torque_Enable", (int32_t)1, &log); } - int32_t position = data[0] | (data[1] << 8); - int32_t speed = (data[2] | ((0x3 & data[3]) << 8)); - int32_t load = (data[4] | ((0x3 & data[5]) << 8)); + int16_t position = present_data[0] | (present_data[1] << 8); + + int16_t speed = (present_data[2] | ((0x3 & present_data[3]) << 8)); // data[3] third bit determines speed sign - if (data[3] & 0x4) + if (present_data[3] & 0x4) speed = -speed; - // data[5] third bit determines load sign - if (data[5] & 0x4) + + int16_t load = (present_data[4] | ((0x3 & present_data[5]) << 8)); + bool overload = load > TORQUE_LOAD_MAX; + // data[5] third bit determines effort sign + if (present_data[5] & 0x4) load = -load; joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], position) + offset_; joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], speed); - joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(load); + joints_[i].state.overloaded = overload; - // RCLCPP_WARN(rclcpp::get_logger(kDynamixelHardware), "position: %d\nspeed: %d\nload: %d\neffort: %X\n", position, speed, load, load); + if (!overload) { + double effort = dynamixel_workbench_.convertValue2Current(load); + if (joints_[i].state.previous_efforts_.size() >= effort_filter_) { + joints_[i].state.previous_efforts_.pop_front(); + } + joints_[i].state.previous_efforts_.push_back(effort); + joints_[i].state.effort = 0; + for (unsigned int j = 0; j < joints_[i].state.previous_efforts_.size(); ++j) { + joints_[i].state.effort += joints_[i].state.previous_efforts_[j]; + } + joints_[i].state.effort /= joints_[i].state.previous_efforts_.size(); + } else { + joints_[i].state.effort = std::numeric_limits::infinity(); + } } } diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index 8b343d030..618b8222a 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -7,7 +7,8 @@ /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 115200 false - 2.618 + 0.0 + 2 5 From e4b5f3ea33f271bdbf60f11f073227e9edd3f81d Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Fri, 23 Feb 2024 21:37:42 +0100 Subject: [PATCH 04/19] read1 temperature and voltage --- .../dynamixel_hardware.hpp | 2 ++ .../dynamixel_hardware.cpp | 22 +++++++++++++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index b8a83551e..b550181b2 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -39,6 +39,8 @@ struct JointValue double position{0.0}; double velocity{0.0}; double effort{0.0}; + double voltage{0.0}; + double temperature{0.0}; bool overloaded; std::deque previous_efforts_{}; }; diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index 24d6ec861..c77dec3f1 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -72,6 +72,9 @@ namespace dynamixel_hardware joints_[i].state.position = std::numeric_limits::quiet_NaN(); joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); joints_[i].state.effort = std::numeric_limits::quiet_NaN(); + joints_[i].state.voltage = std::numeric_limits::quiet_NaN(); + joints_[i].state.temperature = std::numeric_limits::quiet_NaN(); + joints_[i].state.overloaded = false; joints_[i].state.previous_efforts_ = std::deque(); joints_[i].state.previous_efforts_.resize(effort_filter_); joints_[i].command.position = std::numeric_limits::quiet_NaN(); @@ -428,7 +431,7 @@ namespace dynamixel_hardware // ax12 present temperature address: 43 [1B] const unsigned PRESENT_DATA_ADDRESS = 36; - const unsigned PRESENT_DATA_BYTES = 2+2+2; + const unsigned PRESENT_DATA_BYTES = 2+2+2+1+1; const unsigned TORQUE_LOAD_MAX = 1023; unsigned int present_data[PRESENT_DATA_BYTES]; @@ -446,13 +449,18 @@ namespace dynamixel_hardware speed = -speed; int16_t load = (present_data[4] | ((0x3 & present_data[5]) << 8)); - bool overload = load > TORQUE_LOAD_MAX; + bool overload = (unsigned) load > TORQUE_LOAD_MAX; // data[5] third bit determines effort sign if (present_data[5] & 0x4) load = -load; + uint8_t voltage = present_data[6]; + uint8_t temperature = present_data[7]; + joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], position) + offset_; joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], speed); + joints_[i].state.voltage = voltage / 10.0; + joints_[i].state.temperature = temperature; joints_[i].state.overloaded = overload; if (!overload) { @@ -469,6 +477,16 @@ namespace dynamixel_hardware } else { joints_[i].state.effort = std::numeric_limits::infinity(); } + + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "DYNAMIXEL [ position: %6.2f rad | speed: %6.2f rad/s | load: %6.2f mA | voltage: %6.2f V | temperature: %6.2f C ]", + // joints_[i].state.position, + // joints_[i].state.velocity, + // joints_[i].state.effort, + // joints_[i].state.voltage, + // joints_[i].state.temperature + // ); } } From 481ce37ddd3ff2519a3aaa5078355b26411baf47 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Fri, 23 Feb 2024 22:04:32 +0100 Subject: [PATCH 05/19] joint position controller overloaded check --- .../src/joint_position_controller.cpp | 17 +++++++++-------- mep3_msgs/action/JointPositionCommand.action | 4 ++-- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 04655ec28..3874de1bb 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -23,15 +23,15 @@ namespace mep3_controllers if (goal->max_velocity != 0) max_velocity = goal->max_velocity; - double tolerance = 99999; + double tolerance = std::numeric_limits::max(); if (goal->tolerance != 0) tolerance = goal->tolerance; - double timeout = 99999; + double timeout = std::numeric_limits::max(); if (goal->timeout != 0) timeout = goal->timeout; - double max_effort = 99999; + double max_effort = std::numeric_limits::max(); if (goal->max_effort != 0) max_effort = goal->max_effort; @@ -157,7 +157,7 @@ namespace mep3_controllers joint->active = false; RCLCPP_ERROR(get_node()->get_logger(), "Joint %s timeout", joint->name.c_str()); } - else if (joint->effort_handle->get().get_value() > joint->max_effort) + else if (std::isinf(joint->effort_handle->get().get_value()) || joint->effort_handle->get().get_value() > joint->max_effort) { result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_OVERLOAD); joint->action_server->terminate_current(result); @@ -193,7 +193,7 @@ namespace mep3_controllers }); if (position_command_handle == command_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint position command handle for %s", joint->name.c_str()); return controller_interface::CallbackReturn::FAILURE; } joint->position_command_handle = std::ref(*position_command_handle); @@ -208,7 +208,7 @@ namespace mep3_controllers }); if (velocity_command_handle == command_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint velocity command handle for %s", joint->name.c_str()); return controller_interface::CallbackReturn::FAILURE; } joint->velocity_command_handle = std::ref(*velocity_command_handle); @@ -223,7 +223,7 @@ namespace mep3_controllers }); if (position_handle == state_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint position state handle for %s", joint->name.c_str()); return controller_interface::CallbackReturn::FAILURE; } joint->position_handle = std::ref(*position_handle); @@ -238,10 +238,11 @@ namespace mep3_controllers }); if (effort_handle == state_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort state handle for %s", joint->name.c_str()); return controller_interface::CallbackReturn::FAILURE; } joint->effort_handle = std::ref(*effort_handle); + } return controller_interface::CallbackReturn::SUCCESS; } diff --git a/mep3_msgs/action/JointPositionCommand.action b/mep3_msgs/action/JointPositionCommand.action index fb00a6e6b..f8b115309 100644 --- a/mep3_msgs/action/JointPositionCommand.action +++ b/mep3_msgs/action/JointPositionCommand.action @@ -8,11 +8,11 @@ float64 max_velocity # rad/s or m/s float64 max_acceleration # rad/s^2 or m/s^2 float64 tolerance # rad or m float64 timeout # s -float64 max_effort +float64 max_effort # mA --- float64 last_position float64 last_effort uint8 result --- float64 position # rad or m -float64 effort +float64 effort # mA From 31b923d14b16079656613cc649971fbe1edbd2bf Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Fri, 23 Feb 2024 22:19:29 +0100 Subject: [PATCH 06/19] reorder joint status state checking --- mep3_controllers/src/joint_position_controller.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 3874de1bb..3f03a4475 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -130,13 +130,7 @@ namespace mep3_controllers auto result = std::make_shared(); result->set__last_effort(joint->effort_handle->get().get_value()); result->set__last_position(joint->position_handle->get().get_value()); - if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance) - { - result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS); - joint->action_server->succeeded_current(result); - joint->active = false; - } - else if (joint->action_server->is_cancel_requested()) + if (joint->action_server->is_cancel_requested()) { result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_PREEMPTED); joint->action_server->terminate_current(result); @@ -164,6 +158,12 @@ namespace mep3_controllers joint->active = false; RCLCPP_ERROR(get_node()->get_logger(), "Joint %s overload", joint->name.c_str()); } + else if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance) + { + result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS); + joint->action_server->succeeded_current(result); + joint->active = false; + } else { // TODO: If the action is terminated (with the feedback) then it crashes the whole BT. From b176fe254808c7eb8f77c9b9a269059c3dca63ce Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Sat, 24 Feb 2024 00:03:31 +0100 Subject: [PATCH 07/19] stuck joint recovery --- .../dynamixel_hardware.hpp | 20 +++++-- .../dynamixel_hardware.cpp | 55 ++++++++++++++++--- mep3_hardware/test/dynamixel/description.urdf | 3 +- 3 files changed, 65 insertions(+), 13 deletions(-) diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index b550181b2..3e6c03ad9 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include "mep3_hardware/dynamixel_hardware_interface/visibility_control.hpp" #include "rclcpp/macros.hpp" @@ -34,7 +36,7 @@ using hardware_interface::return_type; namespace dynamixel_hardware { -struct JointValue +struct JointState { double position{0.0}; double velocity{0.0}; @@ -43,12 +45,21 @@ struct JointValue double temperature{0.0}; bool overloaded; std::deque previous_efforts_{}; + std::optional> last_max_effort_{}; +}; + +struct JointCommand +{ + double position{0.0}; + double velocity{0.0}; + double effort{0.0}; + double initial_position_{0.0}; }; struct Joint { - JointValue state{}; - JointValue command{}; + JointState state{}; + JointCommand command{}; }; enum class ControlMode { @@ -111,7 +122,8 @@ class DynamixelHardware bool use_dummy_{false}; double offset_{0}; bool keep_read_write_thread_{true}; - unsigned int effort_filter_ {0}; + unsigned int effort_average_ {0}; + std::chrono::milliseconds recovery_timeout_{250}; }; } // namespace dynamixel_hardware diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index c77dec3f1..3ec3188bb 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -60,26 +60,31 @@ namespace dynamixel_hardware joint_ids_.resize(info_.joints.size(), 0); try { - effort_filter_ = std::stoi(info_.hardware_parameters.at("effort_filter")); - RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_filter: %d", effort_filter_); + effort_average_ = std::stoi(info_.hardware_parameters.at("effort_average")); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_average: %d samples", effort_average_); } catch (const std::out_of_range& e) { - RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_filter: %d [default]", effort_filter_); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_average: %d samples [default]", effort_average_); } for (uint i = 0; i < info_.joints.size(); i++) { joint_ids_[i] = std::stoi(info_.joints[i].parameters.at("id")); + // Command + 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(); + // State joints_[i].state.position = std::numeric_limits::quiet_NaN(); joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); joints_[i].state.effort = std::numeric_limits::quiet_NaN(); joints_[i].state.voltage = std::numeric_limits::quiet_NaN(); joints_[i].state.temperature = std::numeric_limits::quiet_NaN(); + // Bookkeeping + joints_[i].command.initial_position_ = std::numeric_limits::quiet_NaN(); joints_[i].state.overloaded = false; joints_[i].state.previous_efforts_ = std::deque(); - joints_[i].state.previous_efforts_.resize(effort_filter_); - 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].state.previous_efforts_.resize(effort_average_); + joints_[i].state.last_max_effort_.reset(); RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); } @@ -101,6 +106,13 @@ namespace dynamixel_hardware RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "usb_port: %s", usb_port.c_str()); RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "baud_rate: %d", baud_rate); + try { + recovery_timeout_ = std::chrono::milliseconds(stoi(info_.hardware_parameters.at("recovery_timeout"))); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "recovery_timeout: %ld ms", recovery_timeout_.count()); + } catch (const std::out_of_range& e) { + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "recovery_timeout: %ld ms [default]", recovery_timeout_.count()); + } + if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log)) { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); @@ -271,6 +283,11 @@ namespace dynamixel_hardware } read_from_hardware(); + for (uint i = 0; i < joints_.size(); i++) + { + // Save initial position for recovery + joints_[i].command.initial_position_ = joints_[i].state.position; + } reset_command(); write_to_hardware(); @@ -450,6 +467,7 @@ namespace dynamixel_hardware int16_t load = (present_data[4] | ((0x3 & present_data[5]) << 8)); bool overload = (unsigned) load > TORQUE_LOAD_MAX; + bool max_torque = (unsigned) load == TORQUE_LOAD_MAX; // data[5] third bit determines effort sign if (present_data[5] & 0x4) load = -load; @@ -465,7 +483,7 @@ namespace dynamixel_hardware if (!overload) { double effort = dynamixel_workbench_.convertValue2Current(load); - if (joints_[i].state.previous_efforts_.size() >= effort_filter_) { + if (joints_[i].state.previous_efforts_.size() >= effort_average_) { joints_[i].state.previous_efforts_.pop_front(); } joints_[i].state.previous_efforts_.push_back(effort); @@ -478,6 +496,27 @@ namespace dynamixel_hardware joints_[i].state.effort = std::numeric_limits::infinity(); } + if (max_torque) { + if (joints_[i].state.last_max_effort_.has_value()) { + const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.last_max_effort_.value(); + const auto duration = std::chrono::duration_cast(time_delta); + if (duration > recovery_timeout_) { + RCLCPP_WARN( + rclcpp::get_logger(kDynamixelHardware), + "Recovering stuck joint %s to initial position %.3lf", + info_.joints[i].name.c_str(), + joints_[i].command.initial_position_ + ); + joints_[i].command.position = joints_[i].command.initial_position_; + joints_[i].state.effort = std::numeric_limits::infinity(); + } + } else { + joints_[i].state.last_max_effort_ = std::chrono::system_clock::now(); + } + } else { + joints_[i].state.last_max_effort_.reset(); + } + // RCLCPP_WARN( // rclcpp::get_logger(kDynamixelHardware), // "DYNAMIXEL [ position: %6.2f rad | speed: %6.2f rad/s | load: %6.2f mA | voltage: %6.2f V | temperature: %6.2f C ]", diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index 618b8222a..9df2180cd 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -8,7 +8,8 @@ 115200 false 0.0 - 2 + 2 + 100 5 From 3f0185110e5c8fa739ea37276307ef98fe688b28 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Sat, 24 Feb 2024 01:54:56 +0100 Subject: [PATCH 08/19] high torque threshold --- .../dynamixel_hardware.hpp | 5 ++- .../dynamixel_hardware.cpp | 37 +++++++++++-------- mep3_hardware/test/dynamixel/description.urdf | 1 + 3 files changed, 26 insertions(+), 17 deletions(-) diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index 3e6c03ad9..3a2c8a843 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -44,8 +44,9 @@ struct JointState double voltage{0.0}; double temperature{0.0}; bool overloaded; + double recovery_position_{0.0}; std::deque previous_efforts_{}; - std::optional> last_max_effort_{}; + std::optional> high_torque_start{}; }; struct JointCommand @@ -53,7 +54,6 @@ struct JointCommand double position{0.0}; double velocity{0.0}; double effort{0.0}; - double initial_position_{0.0}; }; struct Joint @@ -123,6 +123,7 @@ class DynamixelHardware double offset_{0}; bool keep_read_write_thread_{true}; unsigned int effort_average_ {0}; + double torque_threshold_ {0.9}; std::chrono::milliseconds recovery_timeout_{250}; }; } // namespace dynamixel_hardware diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index 3ec3188bb..44a97596a 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -50,7 +50,7 @@ namespace dynamixel_hardware CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo &info) { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "configure"); + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "Configure"); if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -80,11 +80,11 @@ namespace dynamixel_hardware joints_[i].state.voltage = std::numeric_limits::quiet_NaN(); joints_[i].state.temperature = std::numeric_limits::quiet_NaN(); // Bookkeeping - joints_[i].command.initial_position_ = std::numeric_limits::quiet_NaN(); + joints_[i].state.recovery_position_ = std::numeric_limits::quiet_NaN(); joints_[i].state.overloaded = false; joints_[i].state.previous_efforts_ = std::deque(); joints_[i].state.previous_efforts_.resize(effort_average_); - joints_[i].state.last_max_effort_.reset(); + joints_[i].state.high_torque_start.reset(); RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); } @@ -113,6 +113,13 @@ namespace dynamixel_hardware RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "recovery_timeout: %ld ms [default]", recovery_timeout_.count()); } + try { + torque_threshold_ = stof(info_.hardware_parameters.at("torque_threshold")); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "torque_threshold: %.2f", torque_threshold_); + } catch (const std::out_of_range& e) { + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "torque_threshold: %.2f [default]", torque_threshold_); + } + if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log)) { RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); @@ -271,7 +278,7 @@ namespace dynamixel_hardware CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* previous_state */) { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start"); + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "Start"); for (uint i = 0; i < joints_.size(); i++) { if (use_dummy_ && std::isnan(joints_[i].state.position)) @@ -286,7 +293,7 @@ namespace dynamixel_hardware for (uint i = 0; i < joints_.size(); i++) { // Save initial position for recovery - joints_[i].command.initial_position_ = joints_[i].state.position; + joints_[i].state.recovery_position_ = joints_[i].state.position; } reset_command(); write_to_hardware(); @@ -311,7 +318,7 @@ namespace dynamixel_hardware CallbackReturn DynamixelHardware::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) { keep_read_write_thread_ = false; - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop"); + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "Stop"); return CallbackReturn::SUCCESS; } @@ -467,7 +474,7 @@ namespace dynamixel_hardware int16_t load = (present_data[4] | ((0x3 & present_data[5]) << 8)); bool overload = (unsigned) load > TORQUE_LOAD_MAX; - bool max_torque = (unsigned) load == TORQUE_LOAD_MAX; + bool high_torque = (double) load >= TORQUE_LOAD_MAX * torque_threshold_; // data[5] third bit determines effort sign if (present_data[5] & 0x4) load = -load; @@ -496,25 +503,25 @@ namespace dynamixel_hardware joints_[i].state.effort = std::numeric_limits::infinity(); } - if (max_torque) { - if (joints_[i].state.last_max_effort_.has_value()) { - const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.last_max_effort_.value(); + if (high_torque) { + if (joints_[i].state.high_torque_start.has_value()) { + const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.high_torque_start.value(); const auto duration = std::chrono::duration_cast(time_delta); if (duration > recovery_timeout_) { RCLCPP_WARN( rclcpp::get_logger(kDynamixelHardware), - "Recovering stuck joint %s to initial position %.3lf", + "Recovering stuck joint %s to position %.3lf", info_.joints[i].name.c_str(), - joints_[i].command.initial_position_ + joints_[i].state.recovery_position_ ); - joints_[i].command.position = joints_[i].command.initial_position_; + joints_[i].command.position = joints_[i].state.recovery_position_; joints_[i].state.effort = std::numeric_limits::infinity(); } } else { - joints_[i].state.last_max_effort_ = std::chrono::system_clock::now(); + joints_[i].state.high_torque_start = std::chrono::system_clock::now(); } } else { - joints_[i].state.last_max_effort_.reset(); + joints_[i].state.high_torque_start.reset(); } // RCLCPP_WARN( diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index 9df2180cd..dad42bf61 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -9,6 +9,7 @@ false 0.0 2 + 0.8 100 From ea9c8754b598355bf1cb5bf98074f8dbc1426112 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Sat, 24 Feb 2024 02:31:09 +0100 Subject: [PATCH 09/19] effort command handle --- .../joint_position_controller.hpp | 1 + .../src/joint_position_controller.cpp | 21 +++++++++++++++++-- .../dynamixel_hardware.cpp | 19 +++++++++++++++++ mep3_hardware/test/dynamixel/description.urdf | 1 + 4 files changed, 40 insertions(+), 2 deletions(-) diff --git a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp index b763432b4..1be7b355d 100644 --- a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp +++ b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp @@ -18,6 +18,7 @@ namespace mep3_controllers Joint(){}; std::optional> position_command_handle; std::optional> velocity_command_handle; + std::optional> effort_command_handle; std::optional> position_handle; std::optional> effort_handle; std::string name; diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 3f03a4475..829ea0b37 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -13,11 +13,12 @@ namespace mep3_controllers auto goal = joint->action_server->get_current_goal(); RCLCPP_INFO( get_node()->get_logger(), - "Motor %s action called to position %lf (velocity %lf, tolerance %lf)", + "Motor %s action called to position %.4lf (velocity %.2lf, tolerance %.3lf, max_effort %.2lf", joint->name.c_str(), goal->position, goal->max_velocity, - goal->tolerance); + goal->tolerance, + goal->max_effort); double max_velocity = 1.0; if (goal->max_velocity != 0) @@ -125,6 +126,7 @@ namespace mep3_controllers { joint->position_command_handle->get().set_value(joint->target_position); joint->velocity_command_handle->get().set_value(joint->max_velocity); + joint->effort_command_handle->get().set_value(joint->max_effort); // Return the result auto result = std::make_shared(); @@ -213,6 +215,21 @@ namespace mep3_controllers } joint->velocity_command_handle = std::ref(*velocity_command_handle); + // Effort command + const auto effort_command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&joint](const auto &interface) + { + return interface.get_prefix_name() == joint->name && + interface.get_interface_name() == hardware_interface::HW_IF_EFFORT; + }); + if (effort_command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort command handle for %s", joint->name.c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + joint->effort_command_handle = std::ref(*effort_command_handle); + // Position state const auto position_handle = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index 44a97596a..6917ed525 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -271,6 +271,8 @@ namespace dynamixel_hardware 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; @@ -503,6 +505,17 @@ namespace dynamixel_hardware joints_[i].state.effort = std::numeric_limits::infinity(); } + RCLCPP_WARN( + rclcpp::get_logger(kDynamixelHardware), + "EFFORT %.2lf %.2lf", + joints_[i].state.effort, + joints_[i].command.effort + ); + + // if (joints_[i].state.effort > joints_[i].command.effort) { + // continue; + // } + if (high_torque) { if (joints_[i].state.high_torque_start.has_value()) { const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.high_torque_start.value(); @@ -518,6 +531,12 @@ namespace dynamixel_hardware joints_[i].state.effort = std::numeric_limits::infinity(); } } else { + RCLCPP_WARN( + rclcpp::get_logger(kDynamixelHardware), + "Torque on joint %s is high %.2lf%%", + info_.joints[i].name.c_str(), + (double) load / (double) TORQUE_LOAD_MAX * 100 + ); joints_[i].state.high_torque_start = std::chrono::system_clock::now(); } } else { diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index dad42bf61..a52e7028e 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -16,6 +16,7 @@ 5 + From 6060b3494c32ca4302240b5b51ba379d529e275d Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Sat, 2 Mar 2024 17:14:54 +0100 Subject: [PATCH 10/19] comment out effort command --- .../src/joint_position_controller.cpp | 28 +++++++++---------- mep3_hardware/test/dynamixel/controllers.yaml | 1 + 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 829ea0b37..c7f8ddf87 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -215,20 +215,20 @@ namespace mep3_controllers } joint->velocity_command_handle = std::ref(*velocity_command_handle); - // Effort command - const auto effort_command_handle = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&joint](const auto &interface) - { - return interface.get_prefix_name() == joint->name && - interface.get_interface_name() == hardware_interface::HW_IF_EFFORT; - }); - if (effort_command_handle == command_interfaces_.end()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort command handle for %s", joint->name.c_str()); - return controller_interface::CallbackReturn::FAILURE; - } - joint->effort_command_handle = std::ref(*effort_command_handle); + // // Effort command + // const auto effort_command_handle = std::find_if( + // command_interfaces_.begin(), command_interfaces_.end(), + // [&joint](const auto &interface) + // { + // return interface.get_prefix_name() == joint->name && + // interface.get_interface_name() == hardware_interface::HW_IF_EFFORT; + // }); + // if (effort_command_handle == command_interfaces_.end()) + // { + // RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort command handle for %s", joint->name.c_str()); + // return controller_interface::CallbackReturn::FAILURE; + // } + // joint->effort_command_handle = std::ref(*effort_command_handle); // Position state const auto position_handle = std::find_if( diff --git a/mep3_hardware/test/dynamixel/controllers.yaml b/mep3_hardware/test/dynamixel/controllers.yaml index 2ec3dc05c..572fb06e2 100644 --- a/mep3_hardware/test/dynamixel/controllers.yaml +++ b/mep3_hardware/test/dynamixel/controllers.yaml @@ -14,6 +14,7 @@ joint_trajectory_controller: command_interfaces: - position - velocity + - effort state_interfaces: - position From c120bbaaea1085ed32a14514bd6cf59ba52898a0 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 4 Mar 2024 22:57:43 +0100 Subject: [PATCH 11/19] clean --- mep3_hardware/test/dynamixel/controllers.yaml | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/mep3_hardware/test/dynamixel/controllers.yaml b/mep3_hardware/test/dynamixel/controllers.yaml index 572fb06e2..0fcba9a43 100644 --- a/mep3_hardware/test/dynamixel/controllers.yaml +++ b/mep3_hardware/test/dynamixel/controllers.yaml @@ -6,21 +6,6 @@ controller_manager: joint_position_controller: type: mep3_controllers/JointPositionController -joint_trajectory_controller: - ros__parameters: - joints: - - joint1 - - command_interfaces: - - position - - velocity - - effort - - state_interfaces: - - position - - velocity - - effort - joint_position_controller: ros__parameters: joints: From 22d4ec2d5b7d79cf6d18295be7a7860ee47a3c8a Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 4 Mar 2024 22:58:01 +0100 Subject: [PATCH 12/19] fix dummy mode --- .../dynamixel_hardware.cpp | 18 +++++++++++++++++- mep3_hardware/test/dynamixel/description.urdf | 1 + 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index 6917ed525..e65260e51 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -263,7 +263,6 @@ namespace dynamixel_hardware std::vector DynamixelHardware::export_command_interfaces() { - RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "export_command_interfaces"); std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { @@ -331,6 +330,17 @@ namespace dynamixel_hardware void DynamixelHardware::read_from_hardware() { + if (use_dummy_) + { + for (uint i = 0; i < joints_.size(); i++) + { + joints_[i].state.position = joints_[i].command.position; + joints_[i].state.velocity = joints_[i].command.velocity; + joints_[i].state.effort = joints_[i].command.effort; + } + return; + } + dynamixel_workbench_.getProtocolVersion() > 1.5 ? read2(rclcpp::Time{}, rclcpp::Duration(0, 0)) : read1(rclcpp::Time{}, rclcpp::Duration(0, 0)); @@ -338,6 +348,12 @@ namespace dynamixel_hardware void DynamixelHardware::write_to_hardware() { + if (use_dummy_) + { + std::cout << "Effort " << joints_[0].command.effort << std::endl; + return; + } + std::vector ids(info_.joints.size(), 0); std::vector commands(info_.joints.size(), 0); diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index a52e7028e..a914f14df 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -11,6 +11,7 @@ 2 0.8 100 + true 5 From 0a4390cad056d007b9265273afa3fa6aa580f60f Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 4 Mar 2024 22:58:21 +0100 Subject: [PATCH 13/19] fix effort command --- .../src/joint_position_controller.cpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index c7f8ddf87..12a227c4f 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -100,6 +100,7 @@ namespace mep3_controllers { conf_names.push_back(joint->name + "/position"); conf_names.push_back(joint->name + "/velocity"); + conf_names.push_back(joint->name + "/effort"); } return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names}; @@ -215,20 +216,20 @@ namespace mep3_controllers } joint->velocity_command_handle = std::ref(*velocity_command_handle); - // // Effort command - // const auto effort_command_handle = std::find_if( - // command_interfaces_.begin(), command_interfaces_.end(), - // [&joint](const auto &interface) - // { - // return interface.get_prefix_name() == joint->name && - // interface.get_interface_name() == hardware_interface::HW_IF_EFFORT; - // }); - // if (effort_command_handle == command_interfaces_.end()) - // { - // RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort command handle for %s", joint->name.c_str()); - // return controller_interface::CallbackReturn::FAILURE; - // } - // joint->effort_command_handle = std::ref(*effort_command_handle); + // Effort command + const auto effort_command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&joint](const auto &interface) + { + return interface.get_prefix_name() == joint->name && + interface.get_interface_name() == hardware_interface::HW_IF_EFFORT; + }); + if (effort_command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort command handle for %s", joint->name.c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + joint->effort_command_handle = std::ref(*effort_command_handle); // Position state const auto position_handle = std::find_if( From d670bff2aebc4fe71f1a2a0541e1807f74d05c0a Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 4 Mar 2024 23:00:33 +0100 Subject: [PATCH 14/19] clean --- .../src/dynamixel_hardware_interface/dynamixel_hardware.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index e65260e51..f50c0d4df 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -350,7 +350,6 @@ namespace dynamixel_hardware { if (use_dummy_) { - std::cout << "Effort " << joints_[0].command.effort << std::endl; return; } From 38f51025a4c8a39c2b8398d62592ced81c269eee Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Tue, 5 Mar 2024 23:46:32 +0100 Subject: [PATCH 15/19] respect max_torque --- .../joint_position_controller.hpp | 1 + .../src/joint_position_controller.cpp | 25 ++++++++++- .../dynamixel_hardware.hpp | 3 +- .../dynamixel_hardware.cpp | 45 +++++++++---------- mep3_hardware/test/dynamixel/description.urdf | 4 +- 5 files changed, 49 insertions(+), 29 deletions(-) diff --git a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp index 1be7b355d..9b9934c96 100644 --- a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp +++ b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp @@ -17,6 +17,7 @@ namespace mep3_controllers { Joint(){}; std::optional> position_command_handle; + std::optional> recovery_position_command_handle; std::optional> velocity_command_handle; std::optional> effort_command_handle; std::optional> position_handle; diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 12a227c4f..6f9bf0e36 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -101,6 +101,7 @@ namespace mep3_controllers conf_names.push_back(joint->name + "/position"); conf_names.push_back(joint->name + "/velocity"); conf_names.push_back(joint->name + "/effort"); + conf_names.push_back(joint->name + "/recovery_position"); } return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names}; @@ -112,6 +113,7 @@ namespace mep3_controllers for (std::shared_ptr joint : joints_) { conf_names.push_back(joint->name + "/position"); + conf_names.push_back(joint->name + "/velocity"); conf_names.push_back(joint->name + "/effort"); } return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names}; @@ -124,7 +126,10 @@ namespace mep3_controllers for (std::shared_ptr joint : joints_) { if (joint->active) - { + { + if (std::isnan(joint->recovery_position_command_handle->get().get_value())) { + joint->recovery_position_command_handle->get().set_value(0); // Default recovery position + } joint->position_command_handle->get().set_value(joint->target_position); joint->velocity_command_handle->get().set_value(joint->max_velocity); joint->effort_command_handle->get().set_value(joint->max_effort); @@ -163,6 +168,9 @@ namespace mep3_controllers } else if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance) { + joint->recovery_position_command_handle->get().set_value(joint->target_position); + RCLCPP_ERROR(get_node()->get_logger(), "Last good position: %f\n", joint->target_position); + result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS); joint->action_server->succeeded_current(result); joint->active = false; @@ -231,6 +239,21 @@ namespace mep3_controllers } joint->effort_command_handle = std::ref(*effort_command_handle); + // Recovery position command + const auto recovery_position_command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&joint](const auto &interface) + { + return interface.get_prefix_name() == joint->name && + interface.get_interface_name() == "recovery_position"; + }); + if (recovery_position_command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint recovery position command handle for %s", joint->name.c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + joint->recovery_position_command_handle = std::ref(*recovery_position_command_handle); + // Position state const auto position_handle = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index 3a2c8a843..2c2d7147c 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -36,6 +36,7 @@ using hardware_interface::return_type; namespace dynamixel_hardware { + struct JointState { double position{0.0}; @@ -44,7 +45,6 @@ struct JointState double voltage{0.0}; double temperature{0.0}; bool overloaded; - double recovery_position_{0.0}; std::deque previous_efforts_{}; std::optional> high_torque_start{}; }; @@ -54,6 +54,7 @@ struct JointCommand double position{0.0}; double velocity{0.0}; double effort{0.0}; + double recovery_position{0.0}; }; struct Joint diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index f50c0d4df..bd32967c8 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -73,6 +73,7 @@ namespace dynamixel_hardware 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].command.recovery_position = std::numeric_limits::quiet_NaN(); // State joints_[i].state.position = std::numeric_limits::quiet_NaN(); joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); @@ -80,7 +81,6 @@ namespace dynamixel_hardware joints_[i].state.voltage = std::numeric_limits::quiet_NaN(); joints_[i].state.temperature = std::numeric_limits::quiet_NaN(); // Bookkeeping - joints_[i].state.recovery_position_ = std::numeric_limits::quiet_NaN(); joints_[i].state.overloaded = false; joints_[i].state.previous_efforts_ = std::deque(); joints_[i].state.previous_efforts_.resize(effort_average_); @@ -272,6 +272,8 @@ namespace dynamixel_hardware 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)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "recovery_position", &joints_[i].command.recovery_position)); } return command_interfaces; @@ -291,11 +293,6 @@ namespace dynamixel_hardware } read_from_hardware(); - for (uint i = 0; i < joints_.size(); i++) - { - // Save initial position for recovery - joints_[i].state.recovery_position_ = joints_[i].state.position; - } reset_command(); write_to_hardware(); @@ -511,41 +508,31 @@ namespace dynamixel_hardware joints_[i].state.previous_efforts_.pop_front(); } joints_[i].state.previous_efforts_.push_back(effort); - joints_[i].state.effort = 0; + double new_effort_average = 0; for (unsigned int j = 0; j < joints_[i].state.previous_efforts_.size(); ++j) { - joints_[i].state.effort += joints_[i].state.previous_efforts_[j]; + new_effort_average += joints_[i].state.previous_efforts_[j]; } - joints_[i].state.effort /= joints_[i].state.previous_efforts_.size(); + new_effort_average /= joints_[i].state.previous_efforts_.size(); + joints_[i].state.effort = new_effort_average; } else { joints_[i].state.effort = std::numeric_limits::infinity(); } - RCLCPP_WARN( - rclcpp::get_logger(kDynamixelHardware), - "EFFORT %.2lf %.2lf", - joints_[i].state.effort, - joints_[i].command.effort - ); - - // if (joints_[i].state.effort > joints_[i].command.effort) { - // continue; - // } - if (high_torque) { - if (joints_[i].state.high_torque_start.has_value()) { + if (joints_[i].state.high_torque_start.has_value()) { const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.high_torque_start.value(); const auto duration = std::chrono::duration_cast(time_delta); if (duration > recovery_timeout_) { RCLCPP_WARN( rclcpp::get_logger(kDynamixelHardware), - "Recovering stuck joint %s to position %.3lf", + "Recovering stuck joint %s to recovery position %.3lf", info_.joints[i].name.c_str(), - joints_[i].state.recovery_position_ + joints_[i].command.recovery_position ); - joints_[i].command.position = joints_[i].state.recovery_position_; + joints_[i].command.position = joints_[i].command.recovery_position; joints_[i].state.effort = std::numeric_limits::infinity(); } - } else { + } else { RCLCPP_WARN( rclcpp::get_logger(kDynamixelHardware), "Torque on joint %s is high %.2lf%%", @@ -554,6 +541,14 @@ namespace dynamixel_hardware ); joints_[i].state.high_torque_start = std::chrono::system_clock::now(); } + } else if (std::abs(joints_[i].state.effort) > joints_[i].command.effort) { + RCLCPP_WARN( + rclcpp::get_logger(kDynamixelHardware), + "Stopping joint %s in position %.3lf", + info_.joints[i].name.c_str(), + joints_[i].state.position + ); + joints_[i].command.position = joints_[i].state.position; } else { joints_[i].state.high_torque_start.reset(); } diff --git a/mep3_hardware/test/dynamixel/description.urdf b/mep3_hardware/test/dynamixel/description.urdf index a914f14df..9a40f88bd 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -10,12 +10,12 @@ 0.0 2 0.8 - 100 - true + 500 5 + From 3f5112a98c4dec58acb84c4593fe141d636a1d41 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Tue, 5 Mar 2024 23:51:42 +0100 Subject: [PATCH 16/19] hide logs --- .../src/joint_position_controller.cpp | 4 +-- .../dynamixel_hardware.cpp | 36 +++++++++---------- 2 files changed, 19 insertions(+), 21 deletions(-) diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 6f9bf0e36..7640150da 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -168,10 +168,8 @@ namespace mep3_controllers } else if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance) { - joint->recovery_position_command_handle->get().set_value(joint->target_position); - RCLCPP_ERROR(get_node()->get_logger(), "Last good position: %f\n", joint->target_position); - result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS); + joint->recovery_position_command_handle->get().set_value(joint->target_position); joint->action_server->succeeded_current(result); joint->active = false; } diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index bd32967c8..838753cff 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -523,31 +523,31 @@ namespace dynamixel_hardware const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.high_torque_start.value(); const auto duration = std::chrono::duration_cast(time_delta); if (duration > recovery_timeout_) { - RCLCPP_WARN( - rclcpp::get_logger(kDynamixelHardware), - "Recovering stuck joint %s to recovery position %.3lf", - info_.joints[i].name.c_str(), - joints_[i].command.recovery_position - ); + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Recovering stuck joint %s to recovery position %.3lf", + // info_.joints[i].name.c_str(), + // joints_[i].command.recovery_position + // ); joints_[i].command.position = joints_[i].command.recovery_position; joints_[i].state.effort = std::numeric_limits::infinity(); } } else { - RCLCPP_WARN( - rclcpp::get_logger(kDynamixelHardware), - "Torque on joint %s is high %.2lf%%", - info_.joints[i].name.c_str(), - (double) load / (double) TORQUE_LOAD_MAX * 100 - ); + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Torque on joint %s is high %.2lf%%", + // info_.joints[i].name.c_str(), + // (double) load / (double) TORQUE_LOAD_MAX * 100 + // ); joints_[i].state.high_torque_start = std::chrono::system_clock::now(); } } else if (std::abs(joints_[i].state.effort) > joints_[i].command.effort) { - RCLCPP_WARN( - rclcpp::get_logger(kDynamixelHardware), - "Stopping joint %s in position %.3lf", - info_.joints[i].name.c_str(), - joints_[i].state.position - ); + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Stopping joint %s in position %.3lf", + // info_.joints[i].name.c_str(), + // joints_[i].state.position + // ); joints_[i].command.position = joints_[i].state.position; } else { joints_[i].state.high_torque_start.reset(); From 485db60b8496206fb0d907ef9cb95676d54c657c Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Thu, 14 Mar 2024 22:26:30 +0100 Subject: [PATCH 17/19] goal recovery position --- .../joint_position_controller.hpp | 13 +- .../src/joint_position_controller.cpp | 103 +++++++- .../dynamixel_hardware.hpp | 32 ++- .../dynamixel_hardware.cpp | 225 ++++++++++++++---- mep3_msgs/action/JointPositionCommand.action | 8 +- 5 files changed, 318 insertions(+), 63 deletions(-) diff --git a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp index 9b9934c96..740121586 100644 --- a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp +++ b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp @@ -13,15 +13,24 @@ namespace mep3_controllers { + + enum RecoveryMode { + STAY = 0, + RETURN = 1 + }; + struct Joint { Joint(){}; std::optional> position_command_handle; std::optional> recovery_position_command_handle; + std::optional> recovery_mode_command_handle; std::optional> velocity_command_handle; std::optional> effort_command_handle; + std::optional> timeout_command_handle; std::optional> position_handle; std::optional> effort_handle; + std::optional> recovery_state_handle; std::string name; uint64_t start_time_ns; @@ -29,9 +38,11 @@ namespace mep3_controllers double target_position; double max_velocity; + double max_effort; double tolerance; double timeout; - double max_effort; + enum RecoveryMode recovery_mode; + double recovery_position; bool active; }; diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 7640150da..53996a278 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -13,34 +13,50 @@ namespace mep3_controllers auto goal = joint->action_server->get_current_goal(); RCLCPP_INFO( get_node()->get_logger(), - "Motor %s action called to position %.4lf (velocity %.2lf, tolerance %.3lf, max_effort %.2lf", + "Motor %s action called to position %.4lf (velocity %.2lf, tolerance %.3lf, max_effort %.2lf, recovery_position = %.2lf, recovery_mode = %i", joint->name.c_str(), goal->position, goal->max_velocity, goal->tolerance, - goal->max_effort); + goal->max_effort, + goal->recovery_position, + goal->recovery_mode + ); double max_velocity = 1.0; - if (goal->max_velocity != 0) + if (goal->max_velocity != 0) { max_velocity = goal->max_velocity; + } double tolerance = std::numeric_limits::max(); - if (goal->tolerance != 0) + if (goal->tolerance != 0) { tolerance = goal->tolerance; + } double timeout = std::numeric_limits::max(); - if (goal->timeout != 0) + if (goal->timeout != 0) { timeout = goal->timeout; + } double max_effort = std::numeric_limits::max(); - if (goal->max_effort != 0) + if (goal->max_effort != 0) { max_effort = goal->max_effort; + } + + double recovery_position = std::numeric_limits::quiet_NaN(); + if (goal->recovery_position != 0) { + recovery_position = goal->recovery_position; + } + + enum RecoveryMode recovery_mode = (enum RecoveryMode)goal->recovery_mode; joint->target_position = goal->position; joint->max_velocity = max_velocity; joint->tolerance = tolerance; joint->timeout = timeout; joint->max_effort = max_effort; + joint->recovery_mode = recovery_mode; + joint->recovery_position = recovery_position; joint->start_time_ns = get_node()->now().nanoseconds(); joint->active = true; @@ -102,6 +118,7 @@ namespace mep3_controllers conf_names.push_back(joint->name + "/velocity"); conf_names.push_back(joint->name + "/effort"); conf_names.push_back(joint->name + "/recovery_position"); + conf_names.push_back(joint->name + "/recovery_mode"); } return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names}; @@ -115,11 +132,12 @@ namespace mep3_controllers conf_names.push_back(joint->name + "/position"); conf_names.push_back(joint->name + "/velocity"); conf_names.push_back(joint->name + "/effort"); + conf_names.push_back(joint->name + "/recovery_state"); } return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names}; } - controller_interface::return_type JointPositionController::update(const rclcpp::Time &time, const rclcpp::Duration & /* period */) + controller_interface::return_type JointPositionController::update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /* period */) { // ros2 action send_goal /big/joint_position_command/m6 mep3_msgs/action/JointPositionCommand "{ position: -1.57 }" @@ -127,12 +145,18 @@ namespace mep3_controllers { if (joint->active) { - if (std::isnan(joint->recovery_position_command_handle->get().get_value())) { - joint->recovery_position_command_handle->get().set_value(0); // Default recovery position - } joint->position_command_handle->get().set_value(joint->target_position); joint->velocity_command_handle->get().set_value(joint->max_velocity); joint->effort_command_handle->get().set_value(joint->max_effort); + joint->timeout_command_handle->get().set_value(joint->timeout); + joint->recovery_mode_command_handle->get().set_value(joint->recovery_mode); + if (!std::isnan(joint->recovery_position)) { + joint->recovery_position_command_handle->get().set_value(joint->recovery_position); + } else if (std::isnan(joint->recovery_position_command_handle->get().get_value())) { + joint->recovery_position_command_handle->get().set_value(std::numeric_limits::quiet_NaN()); // Default recovery position + } + + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint recovery state handle for %s", joint->name.c_str()); // Return the result auto result = std::make_shared(); @@ -157,14 +181,24 @@ namespace mep3_controllers result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_TIMEOUT); joint->action_server->terminate_current(result); joint->active = false; - RCLCPP_ERROR(get_node()->get_logger(), "Joint %s timeout", joint->name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Joint %s timed out", joint->name.c_str()); + } + else if (joint->recovery_state_handle->get().get_value() != 0) + { + result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_RECOVERY); + joint->action_server->terminate_current(result); + joint->active = false; + RCLCPP_ERROR(get_node()->get_logger(), "Joint %s performed recovery", joint->name.c_str()); + joint->recovery_position_command_handle->get().set_value( + joint->position_handle->get().get_value() + ); } else if (std::isinf(joint->effort_handle->get().get_value()) || joint->effort_handle->get().get_value() > joint->max_effort) { result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_OVERLOAD); joint->action_server->terminate_current(result); joint->active = false; - RCLCPP_ERROR(get_node()->get_logger(), "Joint %s overload", joint->name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Joint %s overloaded", joint->name.c_str()); } else if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance) { @@ -237,6 +271,21 @@ namespace mep3_controllers } joint->effort_command_handle = std::ref(*effort_command_handle); + // Timeout command + const auto timeout_command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&joint](const auto &interface) + { + return interface.get_prefix_name() == joint->name && + interface.get_interface_name() == hardware_interface::HW_IF_EFFORT; + }); + if (timeout_command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint timeout command handle for %s", joint->name.c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + joint->timeout_command_handle = std::ref(*timeout_command_handle); + // Recovery position command const auto recovery_position_command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), @@ -252,6 +301,21 @@ namespace mep3_controllers } joint->recovery_position_command_handle = std::ref(*recovery_position_command_handle); + // Recovery mode command + const auto recovery_mode_command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&joint](const auto &interface) + { + return interface.get_prefix_name() == joint->name && + interface.get_interface_name() == "recovery_mode"; + }); + if (recovery_mode_command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint recovery mode command handle for %s", joint->name.c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + joint->recovery_mode_command_handle = std::ref(*recovery_mode_command_handle); + // Position state const auto position_handle = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), @@ -281,6 +345,21 @@ namespace mep3_controllers return controller_interface::CallbackReturn::FAILURE; } joint->effort_handle = std::ref(*effort_handle); + + // Recovery state + const auto recovery_state_handle = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [&joint](const auto &interface) + { + return interface.get_prefix_name() == joint->name && + interface.get_interface_name() == "recovery_state"; + }); + if (recovery_state_handle == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint recovery state handle for %s", joint->name.c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + joint->recovery_state_handle = std::ref(*recovery_state_handle); } return controller_interface::CallbackReturn::SUCCESS; diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index 2c2d7147c..ed2a41a0d 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -37,6 +37,18 @@ using hardware_interface::return_type; namespace dynamixel_hardware { +enum RecoveryMode { + STAY = 0, + RETURN = 1 +}; + +enum RecoveryState { + OFF = 0, + PENDING = 1, + ACTIVE = 2 +}; + + struct JointState { double position{0.0}; @@ -44,9 +56,14 @@ struct JointState double effort{0.0}; double voltage{0.0}; double temperature{0.0}; - bool overloaded; + bool overloaded{false}; + bool high_torque{false}; + enum RecoveryState recovery_state{OFF}; + double recovery_state_{0.0}; // needed for exported interface + double previous_safe_position_{0.0}; std::deque previous_efforts_{}; - std::optional> high_torque_start{}; + std::optional> recovery_pending_start_{}; + std::optional> recovery_off_start_{}; }; struct JointCommand @@ -54,7 +71,10 @@ struct JointCommand double position{0.0}; double velocity{0.0}; double effort{0.0}; + double timeout{0.0}; double recovery_position{0.0}; + double recovery_mode_{0.0}; // needed for exported interface + enum RecoveryMode recovery_mode{STAY}; }; struct Joint @@ -109,11 +129,19 @@ class DynamixelHardware return_type reset_command(); void read_from_hardware(); + void update_command(); void write_to_hardware(); void read1(const rclcpp::Time & time, const rclcpp::Duration & period); void read2(const rclcpp::Time & time, const rclcpp::Duration & period); + bool timeout_passed(std::chrono::time_point & start_time, double joint_timeout); + + static std::string recovery_mode(const enum RecoveryMode mode); + static std::string recovery_state(const enum RecoveryState state); + static enum RecoveryMode to_recovery_mode(const double mode); + static double from_recovery_state(const enum RecoveryState state); + DynamixelWorkbench dynamixel_workbench_; std::map control_items_; std::vector joints_; diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index 838753cff..41d43f0e4 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -73,18 +73,26 @@ namespace dynamixel_hardware 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].command.timeout = std::numeric_limits::quiet_NaN(); joints_[i].command.recovery_position = std::numeric_limits::quiet_NaN(); + joints_[i].command.recovery_mode_ = std::numeric_limits::quiet_NaN(); + joints_[i].command.recovery_mode = STAY; // State joints_[i].state.position = std::numeric_limits::quiet_NaN(); joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); joints_[i].state.effort = std::numeric_limits::quiet_NaN(); joints_[i].state.voltage = std::numeric_limits::quiet_NaN(); joints_[i].state.temperature = std::numeric_limits::quiet_NaN(); - // Bookkeeping joints_[i].state.overloaded = false; + joints_[i].state.high_torque = false; + joints_[i].state.recovery_state_ = std::numeric_limits::quiet_NaN(); + joints_[i].state.recovery_state = OFF; + // Bookkeeping joints_[i].state.previous_efforts_ = std::deque(); joints_[i].state.previous_efforts_.resize(effort_average_); - joints_[i].state.high_torque_start.reset(); + joints_[i].state.previous_safe_position_ = std::numeric_limits::quiet_NaN(); + joints_[i].state.recovery_pending_start_.reset(); + joints_[i].state.recovery_off_start_.reset(); RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]); } @@ -256,6 +264,8 @@ namespace dynamixel_hardware 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, "recovery_state", &joints_[i].state.recovery_state_)); } return state_interfaces; @@ -272,8 +282,12 @@ namespace dynamixel_hardware 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)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "timeout", &joints_[i].command.timeout)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "recovery_position", &joints_[i].command.recovery_position)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "recovery_mode", &joints_[i].command.recovery_mode_)); } return command_interfaces; @@ -292,9 +306,9 @@ namespace dynamixel_hardware } } - read_from_hardware(); - reset_command(); - write_to_hardware(); + this->read_from_hardware(); + this->reset_command(); + this->write_to_hardware(); // Start read/write thread keep_read_write_thread_ = true; @@ -303,8 +317,9 @@ namespace dynamixel_hardware { while (rclcpp::ok() && keep_read_write_thread_) { - read_from_hardware(); - write_to_hardware(); + this->read_from_hardware(); + this->update_command(); + this->write_to_hardware(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } }); @@ -448,6 +463,150 @@ namespace dynamixel_hardware } } + bool DynamixelHardware::timeout_passed(std::chrono::time_point & start_time, double joint_timeout) { + const auto time_delta = std::chrono::system_clock::now() - start_time; + const auto duration = std::chrono::duration_cast(time_delta); + const auto joint_duration = std::chrono::milliseconds((int) (joint_timeout * 1000.0)); + return duration > recovery_timeout_ || duration > joint_duration; + } + + std::string DynamixelHardware::recovery_mode(const enum RecoveryMode mode) { + switch (mode) + { + case STAY: + return "STAY"; + case RETURN: + return "RETURN"; + default: + return ""; + } + } + + std::string DynamixelHardware::recovery_state(const enum RecoveryState state) { + switch (state) + { + case OFF: + return "OFF"; + case PENDING: + return "PENDING"; + case ACTIVE: + return "ACTIVE"; + default: + return ""; + } + } + + enum RecoveryMode DynamixelHardware::to_recovery_mode(const double mode) { + if (mode > -0.5 && mode < 0.5) { + return STAY; + } + if (mode > 0.5 && mode < 1.5) { + return RETURN; + } + // Default + return STAY; + } + + double DynamixelHardware::from_recovery_state(const enum RecoveryState state) { + switch (state) + { + case OFF: + return 0.0; + case PENDING: + return 1.0; + case ACTIVE: + return 2.0; + default: + return std::numeric_limits::quiet_NaN(); + } + } + + void DynamixelHardware::update_command() { + + for (auto&& joint = joints_.begin(); joint != joints_.end(); ++joint) + { + // Cast double recieved from controller into enum + joint->command.recovery_mode = to_recovery_mode(joint->command.recovery_mode_); + joint->state.recovery_state_ = from_recovery_state(joint->state.recovery_state); + + // Set recovery position to safe position if NaN + if (std::isnan(joint->command.recovery_position)) { + joint->command.recovery_position = joint->state.previous_safe_position_; + } + + // Joint is under high torque, overloaaded or has reached maximum torque from command + if (joint->state.high_torque || joint->state.overloaded || joint->state.effort > joint->command.effort) { + // Reset recovery off counter + joint->state.recovery_off_start_.reset(); + // Encountered high torque, set recovery state to pending + if (joint->state.recovery_state == OFF) { + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Joint enter PENDING recovery state" + // ); + joint->state.recovery_state = PENDING; + joint->state.recovery_pending_start_ = std::chrono::system_clock::now(); + } + // Enter active recovery state if pending reaches timeout + else if (joint->state.recovery_state == PENDING) { + if (!joint->state.recovery_pending_start_.has_value()) { + RCLCPP_FATAL( + rclcpp::get_logger(kDynamixelHardware), + "Recovery timeout start time for joint is missing" + ); + continue; + } + if (this->timeout_passed(joint->state.recovery_pending_start_.value(), joint->command.timeout)) { + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Joint enter ACTIVE recovery state" + // ); + joint->state.recovery_state = ACTIVE; + } + } + } else { + // Set prevoious safe position to current one (effort is tolerable) + joint->state.previous_safe_position_ = joint->state.position; + // Return from recovery if necessary + if (joint->state.recovery_state != OFF) { + if (joint->state.recovery_off_start_.has_value()) { + if (this->timeout_passed(joint->state.recovery_off_start_.value(), joint->command.timeout)) { + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Joint enter OFF recovery state" + // ); + joint->state.recovery_state = OFF; + joint->state.recovery_pending_start_.reset(); + joint->state.recovery_off_start_.reset(); + } + } else { + joint->state.recovery_off_start_ = std::chrono::system_clock::now(); + } + } + } + + // Recover to position set by controller + if (joint->state.recovery_state == ACTIVE) { + switch (joint->command.recovery_mode) { + case STAY: + joint->command.position = joint->state.previous_safe_position_; + break; + case RETURN: + joint->command.position = joint->command.recovery_position; + break; + } + const double RECOVERY_TOLERANCE = 0.1; + if (abs(joint->state.position - joint->command.position) < RECOVERY_TOLERANCE) { + joint->state.recovery_state = OFF; + // RCLCPP_WARN( + // rclcpp::get_logger(kDynamixelHardware), + // "Joint enter OFF recovery state" + // ); + } + } + } + } + void DynamixelHardware::read1(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) /* Read function for protocol 1.0 */ { @@ -501,56 +660,28 @@ namespace dynamixel_hardware joints_[i].state.voltage = voltage / 10.0; joints_[i].state.temperature = temperature; joints_[i].state.overloaded = overload; - - if (!overload) { + joints_[i].state.high_torque = high_torque; + + // Read average effort from joint + if (overload) { + RCLCPP_WARN( + rclcpp::get_logger(kDynamixelHardware), + "Joint %s is overloaded", + info_.joints[i].name.c_str() + ); + joints_[i].state.effort = std::numeric_limits::infinity(); + } else { double effort = dynamixel_workbench_.convertValue2Current(load); if (joints_[i].state.previous_efforts_.size() >= effort_average_) { joints_[i].state.previous_efforts_.pop_front(); } - joints_[i].state.previous_efforts_.push_back(effort); + joints_[i].state.previous_efforts_.push_back(abs(effort)); double new_effort_average = 0; for (unsigned int j = 0; j < joints_[i].state.previous_efforts_.size(); ++j) { new_effort_average += joints_[i].state.previous_efforts_[j]; } new_effort_average /= joints_[i].state.previous_efforts_.size(); joints_[i].state.effort = new_effort_average; - } else { - joints_[i].state.effort = std::numeric_limits::infinity(); - } - - if (high_torque) { - if (joints_[i].state.high_torque_start.has_value()) { - const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.high_torque_start.value(); - const auto duration = std::chrono::duration_cast(time_delta); - if (duration > recovery_timeout_) { - // RCLCPP_WARN( - // rclcpp::get_logger(kDynamixelHardware), - // "Recovering stuck joint %s to recovery position %.3lf", - // info_.joints[i].name.c_str(), - // joints_[i].command.recovery_position - // ); - joints_[i].command.position = joints_[i].command.recovery_position; - joints_[i].state.effort = std::numeric_limits::infinity(); - } - } else { - // RCLCPP_WARN( - // rclcpp::get_logger(kDynamixelHardware), - // "Torque on joint %s is high %.2lf%%", - // info_.joints[i].name.c_str(), - // (double) load / (double) TORQUE_LOAD_MAX * 100 - // ); - joints_[i].state.high_torque_start = std::chrono::system_clock::now(); - } - } else if (std::abs(joints_[i].state.effort) > joints_[i].command.effort) { - // RCLCPP_WARN( - // rclcpp::get_logger(kDynamixelHardware), - // "Stopping joint %s in position %.3lf", - // info_.joints[i].name.c_str(), - // joints_[i].state.position - // ); - joints_[i].command.position = joints_[i].state.position; - } else { - joints_[i].state.high_torque_start.reset(); } // RCLCPP_WARN( diff --git a/mep3_msgs/action/JointPositionCommand.action b/mep3_msgs/action/JointPositionCommand.action index f8b115309..00e96a7b6 100644 --- a/mep3_msgs/action/JointPositionCommand.action +++ b/mep3_msgs/action/JointPositionCommand.action @@ -2,13 +2,19 @@ uint8 RESULT_SUCCESS = 0 uint8 RESULT_TIMEOUT = 1 uint8 RESULT_OVERLOAD = 2 uint8 RESULT_PREEMPTED = 3 +uint8 RESULT_RECOVERY = 4 +uint8 RECOVERY_STAY = 0 +uint8 RECOVERY_RETURN = 1 + float64 position # rad or m float64 max_velocity # rad/s or m/s float64 max_acceleration # rad/s^2 or m/s^2 +float64 max_effort # mA float64 tolerance # rad or m float64 timeout # s -float64 max_effort # mA +float64 recovery_position # rad or m +uint8 recovery_mode --- float64 last_position float64 last_effort From e5b7834afe41f21f0857b1df3da73b0fa8a34afe Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Thu, 14 Mar 2024 23:14:34 +0100 Subject: [PATCH 18/19] add recovery to behavior tree action --- .../joint_position_command_action.hpp | 96 +++++++++++++++++-- .../joint_position_controller.hpp | 7 +- .../src/joint_position_controller.cpp | 2 +- mep3_hardware/CMakeLists.txt | 1 + .../dynamixel_hardware.hpp | 13 +-- .../dynamixel_hardware.cpp | 20 ++-- 6 files changed, 105 insertions(+), 34 deletions(-) diff --git a/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp b/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp index d63269a52..db2d24463 100644 --- a/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp +++ b/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp @@ -26,6 +26,18 @@ namespace mep3_behavior { + std::string recovery_mode(const int8_t mode) { + switch (mode) + { + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY: + return "STAY"; + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN: + return "RETURN"; + default: + return ""; + } + } + class JointPositionCommandAction : public BT::RosActionNode { @@ -37,18 +49,63 @@ namespace mep3_behavior : BT::RosActionNode(name, conf, params, action_client) { if (!getInput("position", position_)) - throw BT::RuntimeError( - "Dynamixel action requires 'position' argument"); + throw BT::RuntimeError("Dynamixel action requires 'position' argument"); if (!getInput("max_velocity", max_velocity_)) - max_velocity_ = 99999; + max_velocity_ = std::numeric_limits::max(); // 99999 if (!getInput("max_acceleration", max_acceleration_)) - max_acceleration_ = 99999; + max_acceleration_ = std::numeric_limits::max(); // 99999 if (!getInput("tolerance", tolerance_)) - tolerance_ = 9; + tolerance_ = std::numeric_limits::max(); // 9 if (!getInput("timeout", timeout_)) - timeout_ = 5; + timeout_ = std::numeric_limits::max(); // 5 if (!getInput("max_effort", max_effort_)) - max_effort_ = 99999; + max_effort_ = std::numeric_limits::max(); // 99999 + + std::string recovery_combo; + if (getInput("recovery", recovery_combo)) { + if (recovery_combo == "stay:fail") { + // Stay but fail on recovery + fail_on_recovery_ = true; + recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; + recovery_position_ = std::numeric_limits::quiet_NaN(); + } else if (recovery_combo == "stay:ok") { + // Stay and succeed on recovery + fail_on_recovery_ = false; + recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; + recovery_position_ = std::numeric_limits::quiet_NaN(); + } else if (recovery_combo == "return:fail") { + // Return but fail on recovery + fail_on_recovery_ = true; + recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN; + recovery_position_ = std::numeric_limits::quiet_NaN(); + } else if (recovery_combo == "return:ok") { + // Return and succeed on recovery + fail_on_recovery_ = false; + recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN; + recovery_position_ = std::numeric_limits::quiet_NaN(); + } else if (recovery_combo.rfind("goto:", 0) == 0) { + // Recover to position... + recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN; + if (std::sscanf(recovery_combo.c_str(),"goto:%lf:fail", &position_) != EOF) { + // ...and fail + fail_on_recovery_ = true; + } else if (std::sscanf(recovery_combo.c_str(),"goto:%lf:ok", &position_) != EOF) { + // ...and succeed + fail_on_recovery_ = false; + } else { + // Parsing error + throw BT::RuntimeError("Dynamixel action parsing error for 'recovery' argument"); + } + } else { + // Unknown argument value + throw BT::RuntimeError("Dynamixel action unknown value for 'recovery' argument"); + } + } else { + // Default recovery behavior + fail_on_recovery_ = true; + recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; + recovery_position_ = std::numeric_limits::quiet_NaN(); + } std::string table = this->config().blackboard->get("table"); double position_offset; @@ -63,7 +120,10 @@ namespace mep3_behavior std::cout << "Dynamixel desired position to θ=" << position_ << " max_velocity=" << max_velocity_ << " max effort=" << max_effort_ - << " max_acceleration=" << max_acceleration_ << std::endl; + << " max_acceleration=" << max_acceleration_ << std::endl + << " recovery_mode=" << recovery_mode(recovery_mode_) << std::endl + << " recovery_position=" << recovery_position_ << std::endl + << " fail_on_recovery_=" << fail_on_recovery_ << std::endl; goal.position = position_ * M_PI / 180; goal.max_velocity = max_velocity_ * M_PI / 180; @@ -71,6 +131,8 @@ namespace mep3_behavior goal.tolerance = tolerance_ * M_PI / 180; goal.timeout = timeout_; goal.max_effort = max_effort_; + goal.recovery_mode = recovery_mode_; + goal.recovery_position = recovery_position_; return true; } @@ -85,6 +147,7 @@ namespace mep3_behavior BT::InputPort("max_effort"), BT::InputPort("tolerance"), BT::InputPort("timeout"), + BT::InputPort("recovery"), BT::OutputPort("feedback_effort"), BT::OutputPort("feedback_position"), BT::OutputPort("result")}; @@ -105,6 +168,20 @@ namespace mep3_behavior setOutput("feedback_position", wr.result->last_position); std::cout << "Last result: " << (double)wr.result->result << "; last effort: " << (double)wr.result->last_effort << "; last position: " << (double)wr.result->last_position << std::endl; + switch (wr.result->result) + { + case mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS: + return BT::NodeStatus::SUCCESS; + case mep3_msgs::action::JointPositionCommand::Goal::RESULT_TIMEOUT: + case mep3_msgs::action::JointPositionCommand::Goal::RESULT_OVERLOAD: + case mep3_msgs::action::JointPositionCommand::Goal::RESULT_PREEMPTED: + return BT::NodeStatus::FAILURE; + case mep3_msgs::action::JointPositionCommand::Goal::RESULT_RECOVERY: + return fail_on_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS; + default: + throw BT::RuntimeError("Dynamixel action got invalid result number"); + } + return BT::NodeStatus::SUCCESS; } @@ -125,6 +202,9 @@ namespace mep3_behavior double tolerance_; double timeout_; double max_effort_; + bool fail_on_recovery_; + double recovery_position_; + int8_t recovery_mode_; }; } // namespace mep3_behavior diff --git a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp index 740121586..f3a9875c2 100644 --- a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp +++ b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp @@ -14,11 +14,6 @@ namespace mep3_controllers { - enum RecoveryMode { - STAY = 0, - RETURN = 1 - }; - struct Joint { Joint(){}; @@ -41,7 +36,7 @@ namespace mep3_controllers double max_effort; double tolerance; double timeout; - enum RecoveryMode recovery_mode; + int8_t recovery_mode; double recovery_position; bool active; }; diff --git a/mep3_controllers/src/joint_position_controller.cpp b/mep3_controllers/src/joint_position_controller.cpp index 53996a278..9e0bc7ba9 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -48,7 +48,7 @@ namespace mep3_controllers recovery_position = goal->recovery_position; } - enum RecoveryMode recovery_mode = (enum RecoveryMode)goal->recovery_mode; + int8_t recovery_mode = (int8_t)goal->recovery_mode; joint->target_position = goal->position; joint->max_velocity = max_velocity; diff --git a/mep3_hardware/CMakeLists.txt b/mep3_hardware/CMakeLists.txt index 350b4bb15..66ac6ef04 100644 --- a/mep3_hardware/CMakeLists.txt +++ b/mep3_hardware/CMakeLists.txt @@ -93,6 +93,7 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle dynamixel_workbench_toolbox + mep3_msgs ) pluginlib_export_plugin_description_file(hardware_interface dynamixel_hardware.xml) install( diff --git a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp index ed2a41a0d..3efecb3c4 100644 --- a/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp +++ b/mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp @@ -29,6 +29,7 @@ #include #include "mep3_hardware/dynamixel_hardware_interface/visibility_control.hpp" +#include "mep3_msgs/action/joint_position_command.hpp" #include "rclcpp/macros.hpp" using hardware_interface::CallbackReturn; @@ -37,18 +38,12 @@ using hardware_interface::return_type; namespace dynamixel_hardware { -enum RecoveryMode { - STAY = 0, - RETURN = 1 -}; - enum RecoveryState { OFF = 0, PENDING = 1, ACTIVE = 2 }; - struct JointState { double position{0.0}; @@ -74,7 +69,7 @@ struct JointCommand double timeout{0.0}; double recovery_position{0.0}; double recovery_mode_{0.0}; // needed for exported interface - enum RecoveryMode recovery_mode{STAY}; + uint8_t recovery_mode{mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY}; }; struct Joint @@ -137,9 +132,9 @@ class DynamixelHardware bool timeout_passed(std::chrono::time_point & start_time, double joint_timeout); - static std::string recovery_mode(const enum RecoveryMode mode); + static std::string recovery_mode(const int8_t mode); static std::string recovery_state(const enum RecoveryState state); - static enum RecoveryMode to_recovery_mode(const double mode); + static int8_t to_recovery_mode(const double mode); static double from_recovery_state(const enum RecoveryState state); DynamixelWorkbench dynamixel_workbench_; diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index 41d43f0e4..ad5848cfb 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -76,7 +76,7 @@ namespace dynamixel_hardware joints_[i].command.timeout = std::numeric_limits::quiet_NaN(); joints_[i].command.recovery_position = std::numeric_limits::quiet_NaN(); joints_[i].command.recovery_mode_ = std::numeric_limits::quiet_NaN(); - joints_[i].command.recovery_mode = STAY; + joints_[i].command.recovery_mode = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; // State joints_[i].state.position = std::numeric_limits::quiet_NaN(); joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); @@ -470,12 +470,12 @@ namespace dynamixel_hardware return duration > recovery_timeout_ || duration > joint_duration; } - std::string DynamixelHardware::recovery_mode(const enum RecoveryMode mode) { + std::string DynamixelHardware::recovery_mode(const int8_t mode) { switch (mode) { - case STAY: + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY: return "STAY"; - case RETURN: + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN: return "RETURN"; default: return ""; @@ -496,15 +496,15 @@ namespace dynamixel_hardware } } - enum RecoveryMode DynamixelHardware::to_recovery_mode(const double mode) { + int8_t DynamixelHardware::to_recovery_mode(const double mode) { if (mode > -0.5 && mode < 0.5) { - return STAY; + return mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; } if (mode > 0.5 && mode < 1.5) { - return RETURN; + return mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN; } // Default - return STAY; + return mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; } double DynamixelHardware::from_recovery_state(const enum RecoveryState state) { @@ -588,10 +588,10 @@ namespace dynamixel_hardware // Recover to position set by controller if (joint->state.recovery_state == ACTIVE) { switch (joint->command.recovery_mode) { - case STAY: + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY: joint->command.position = joint->state.previous_safe_position_; break; - case RETURN: + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN: joint->command.position = joint->command.recovery_position; break; } From 4c886e800e7c450f9dfcd872723de2b0bf5d83d1 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Thu, 14 Mar 2024 23:27:44 +0100 Subject: [PATCH 19/19] keep position on startup --- .../src/dynamixel_hardware_interface/dynamixel_hardware.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp index ad5848cfb..c55531ae5 100644 --- a/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp +++ b/mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp @@ -534,8 +534,10 @@ namespace dynamixel_hardware joint->command.recovery_position = joint->state.previous_safe_position_; } + const bool max_effort_reached = joint->command.effort > 0 && joint->state.effort > joint->command.effort; + // Joint is under high torque, overloaaded or has reached maximum torque from command - if (joint->state.high_torque || joint->state.overloaded || joint->state.effort > joint->command.effort) { + if (joint->state.high_torque || joint->state.overloaded || max_effort_reached) { // Reset recovery off counter joint->state.recovery_off_start_.reset(); // Encountered high torque, set recovery state to pending