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 b763432b4..f3a9875c2 100644 --- a/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp +++ b/mep3_controllers/include/mep3_controllers/joint_position_controller.hpp @@ -13,13 +13,19 @@ namespace mep3_controllers { + 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; @@ -27,9 +33,11 @@ namespace mep3_controllers double target_position; double max_velocity; + double max_effort; double tolerance; double timeout; - double max_effort; + 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 04655ec28..9e0bc7ba9 100644 --- a/mep3_controllers/src/joint_position_controller.cpp +++ b/mep3_controllers/src/joint_position_controller.cpp @@ -13,33 +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 %lf (velocity %lf, tolerance %lf)", + "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->tolerance, + 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 = 99999; - if (goal->tolerance != 0) + double tolerance = std::numeric_limits::max(); + if (goal->tolerance != 0) { tolerance = goal->tolerance; + } - double timeout = 99999; - if (goal->timeout != 0) + double timeout = std::numeric_limits::max(); + if (goal->timeout != 0) { timeout = goal->timeout; + } - double max_effort = 99999; - if (goal->max_effort != 0) + double max_effort = std::numeric_limits::max(); + 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; + } + + int8_t recovery_mode = (int8_t)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; @@ -99,6 +116,9 @@ 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"); + conf_names.push_back(joint->name + "/recovery_mode"); } return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names}; @@ -110,33 +130,39 @@ 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"); + 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 }" for (std::shared_ptr joint : joints_) { if (joint->active) - { + { 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(); 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); @@ -155,14 +181,31 @@ 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->effort_handle->get().get_value() > joint->max_effort) + 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) + { + 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; } else { @@ -193,7 +236,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,11 +251,71 @@ 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); + // 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); + + // 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(), + [&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); + + // 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(), @@ -223,7 +326,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 +341,26 @@ 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); + + // 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/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 9c70269cf..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 @@ -24,8 +24,12 @@ #include #include +#include +#include +#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; @@ -33,17 +37,45 @@ using hardware_interface::return_type; namespace dynamixel_hardware { -struct JointValue + +enum RecoveryState { + OFF = 0, + PENDING = 1, + ACTIVE = 2 +}; + +struct JointState { double position{0.0}; double velocity{0.0}; double effort{0.0}; + double voltage{0.0}; + double temperature{0.0}; + 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> recovery_pending_start_{}; + std::optional> recovery_off_start_{}; +}; + +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 + uint8_t recovery_mode{mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY}; }; struct Joint { - JointValue state{}; - JointValue command{}; + JointState state{}; + JointCommand command{}; }; enum class ControlMode { @@ -92,11 +124,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 int8_t mode); + static std::string recovery_state(const enum RecoveryState state); + static int8_t 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_; @@ -106,6 +146,9 @@ class DynamixelHardware bool use_dummy_{false}; 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 c9a904519..c55531ae5 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; @@ -59,15 +59,40 @@ namespace dynamixel_hardware joints_.resize(info_.joints.size(), Joint()); joint_ids_.resize(info_.joints.size(), 0); + try { + 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_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")); - 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(); + // 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(); + 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 = 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(); + 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.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.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]); } @@ -89,6 +114,20 @@ 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()); + } + + 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); @@ -225,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; @@ -232,7 +273,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++) { @@ -240,6 +280,14 @@ 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)); + 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; @@ -247,7 +295,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)) @@ -258,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; @@ -269,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)); } }); @@ -282,7 +331,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; } @@ -293,6 +342,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)); @@ -300,6 +360,11 @@ namespace dynamixel_hardware void DynamixelHardware::write_to_hardware() { + if (use_dummy_) + { + return; + } + std::vector ids(info_.joints.size(), 0); std::vector commands(info_.joints.size(), 0); @@ -398,6 +463,152 @@ 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 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 ""; + } + } + + 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 ""; + } + } + + int8_t DynamixelHardware::to_recovery_mode(const double mode) { + if (mode > -0.5 && mode < 0.5) { + return mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY; + } + if (mode > 0.5 && mode < 1.5) { + return mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN; + } + // Default + return mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_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_; + } + + 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 || max_effort_reached) { + // 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 mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY: + joint->command.position = joint->state.previous_safe_position_; + break; + case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_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 */ { @@ -412,32 +623,78 @@ 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+1+1; + 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 = (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; + 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.effort = dynamixel_workbench_.convertValue2Current(load); + joints_[i].state.voltage = voltage / 10.0; + joints_[i].state.temperature = temperature; + joints_[i].state.overloaded = 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(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; + } - // RCLCPP_WARN(rclcpp::get_logger(kDynamixelHardware), "position: %d\nspeed: %d\nload: %d\neffort: %X\n", position, speed, load, load); + // 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 + // ); } } diff --git a/mep3_hardware/test/dynamixel/controllers.yaml b/mep3_hardware/test/dynamixel/controllers.yaml index 6dd271449..0fcba9a43 100644 --- a/mep3_hardware/test/dynamixel/controllers.yaml +++ b/mep3_hardware/test/dynamixel/controllers.yaml @@ -6,20 +6,7 @@ controller_manager: joint_position_controller: type: mep3_controllers/JointPositionController -joint_trajectory_controller: - ros__parameters: - joints: - - joint1 - - command_interfaces: - - position - - velocity - - state_interfaces: - - position - 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..9a40f88bd 100644 --- a/mep3_hardware/test/dynamixel/description.urdf +++ b/mep3_hardware/test/dynamixel/description.urdf @@ -1,60 +1,26 @@ + dynamixel_hardware/DynamixelHardware /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 115200 - false - 2.618 + false + 0.0 + 2 + 0.8 + 500 - - 3 - - - - - - - - 7 - - - - - - - - 8 - - - - - - - - 14 - - - - - - - - 9 - - - - - - - - 55 + + 5 + + - + \ No newline at end of file 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( diff --git a/mep3_msgs/action/JointPositionCommand.action b/mep3_msgs/action/JointPositionCommand.action index fb00a6e6b..00e96a7b6 100644 --- a/mep3_msgs/action/JointPositionCommand.action +++ b/mep3_msgs/action/JointPositionCommand.action @@ -2,17 +2,23 @@ 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 +float64 recovery_position # rad or m +uint8 recovery_mode --- float64 last_position float64 last_effort uint8 result --- float64 position # rad or m -float64 effort +float64 effort # mA