Skip to content

Commit

Permalink
stuck joint recovery
Browse files Browse the repository at this point in the history
  • Loading branch information
@filiparag
filiparag committed Feb 23, 2024
1 parent 31b923d commit b176fe2
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 13 deletions.
20 changes: 16 additions & 4 deletions mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
#include <map>
#include <vector>
#include <deque>
#include <optional>
#include <chrono>

#include "mep3_hardware/dynamixel_hardware_interface/visibility_control.hpp"
#include "rclcpp/macros.hpp"
Expand All @@ -34,7 +36,7 @@ using hardware_interface::return_type;

namespace dynamixel_hardware
{
struct JointValue
struct JointState
{
double position{0.0};
double velocity{0.0};
Expand All @@ -43,12 +45,21 @@ struct JointValue
double temperature{0.0};
bool overloaded;
std::deque<double> previous_efforts_{};
std::optional<std::chrono::time_point<std::chrono::system_clock>> 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 {
Expand Down Expand Up @@ -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

Expand Down
55 changes: 47 additions & 8 deletions mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::quiet_NaN();
joints_[i].command.velocity = std::numeric_limits<double>::quiet_NaN();
joints_[i].command.effort = std::numeric_limits<double>::quiet_NaN();
// State
joints_[i].state.position = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.velocity = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.effort = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.voltage = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.temperature = std::numeric_limits<double>::quiet_NaN();
// Bookkeeping
joints_[i].command.initial_position_ = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.overloaded = false;
joints_[i].state.previous_efforts_ = std::deque<double>();
joints_[i].state.previous_efforts_.resize(effort_filter_);
joints_[i].command.position = std::numeric_limits<double>::quiet_NaN();
joints_[i].command.velocity = std::numeric_limits<double>::quiet_NaN();
joints_[i].command.effort = std::numeric_limits<double>::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]);
}

Expand All @@ -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);
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -478,6 +496,27 @@ namespace dynamixel_hardware
joints_[i].state.effort = std::numeric_limits<double>::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<std::chrono::milliseconds>(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<double>::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 ]",
Expand Down
3 changes: 2 additions & 1 deletion mep3_hardware/test/dynamixel/description.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,8 @@
<param name="baud_rate">115200</param>
<param name="use_dummy">false</param>
<param name="offset">0.0</param>
<param name="effort_filter">2</param>
<param name="effort_average">2</param>
<param name="recovery_timeout">100</param>
</hardware>
<joint name="joint1">
<param name="id">5</param>
Expand Down

0 comments on commit b176fe2

Please sign in to comment.