nils/dynamixel_ros2_control
0
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

try restart motor on error

Browse Source
This commit is contained in:
Nils Schulte 2022-09-13 12:15:54 +02:00
parent b8e3f915cf
commit 8d3ff7b5cd
3 changed files with 33 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
include/dynamixel_ros2_control/dynamixel_driver.hpp
View File

@ -54,6 +54,7 @@ class Driver {
//double goal_velocity; //double goal_velocity;
double velocity; double velocity;
double effort; double effort;
bool torque_target;
bool torque; bool torque;
uint8_t hw_error; uint8_t hw_error;
@ -101,6 +102,7 @@ class Driver {
rebooted(false), rebooted(false),
goal_position(std::numeric_limits<double>::quiet_NaN()), goal_position(std::numeric_limits<double>::quiet_NaN()),
position(std::numeric_limits<double>::quiet_NaN()), position(std::numeric_limits<double>::quiet_NaN()),
torque_target(false),
p_gain(std::numeric_limits<double>::quiet_NaN()), p_gain(std::numeric_limits<double>::quiet_NaN()),
i_gain(std::numeric_limits<double>::quiet_NaN()), i_gain(std::numeric_limits<double>::quiet_NaN()),
d_gain(std::numeric_limits<double>::quiet_NaN()) d_gain(std::numeric_limits<double>::quiet_NaN())
@ -127,9 +129,9 @@ class Driver {
command::Torque_Enable, command::Torque_Enable,
[this](uint32_t data) { [this](uint32_t data) {
this->torque = bool(data); this->torque = bool(data);
if (this->torque == false) { //if (this->torque == false) {
goal_position = std::numeric_limits<double>::quiet_NaN(); //goal_position = std::numeric_limits<double>::quiet_NaN();
} //}
}); });
//Present Position //Present Position
@ -321,9 +323,11 @@ class Driver {
* to prepare it for the writeing over the bus. * to prepare it for the writeing over the bus.
*/ */
void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) { void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) {
motor->torque_target = torque;
if(torque && motor->hw_error) { if(torque && motor->hw_error) {
if(motor->rebooting) if(motor->rebooting) {
return; return;
}
throw hardware_error(motor, "Can't enable torque!"); throw hardware_error(motor, "Can't enable torque!");
} }
field command_field; field command_field;
@ -334,10 +338,16 @@ class Driver {
void reboot(dynamixel::Driver::Motor::Ptr& motor) { void reboot(dynamixel::Driver::Motor::Ptr& motor) {
motor->after_reboot_functions.clear(); motor->after_reboot_functions.clear();
//bool old_torque_target = motor->torque_target;
motor->after_reboot_functions.push_back([this, motor](){ motor->after_reboot_functions.push_back([this, motor](){
motor->torque = true;
set_torque(motor, false);
this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target); this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target);
}); });
packetHandler->reboot(portHandler.get(),motor->id); packetHandler->reboot(portHandler.get(),motor->id);
set_torque(motor, false);
motor->torque = false;
write();
} }
hw_error_callback overload_error_callback; hw_error_callback overload_error_callback;
@ -450,6 +460,9 @@ class Driver {
*/ */
void write(bool enable_torque = false) { void write(bool enable_torque = false) {
for (const auto& [joint_name, motor]: motors) { for (const auto& [joint_name, motor]: motors) {
if(motor->torque_target != motor->torque) {
set_torque(motor, motor->torque_target);
}
if(motor->torque || enable_torque) { if(motor->torque || enable_torque) {
if(std::isnan(motor->goal_position)) if(std::isnan(motor->goal_position))
motor->goal_position = motor->position; motor->goal_position = motor->position;
@ -502,6 +515,9 @@ class Driver {
after_reboot_f(); after_reboot_f();
} }
} }
if(motor->rebooted) {
motor->after_reboot_functions.clear();
}
} }
} }
} }
@ -670,9 +686,10 @@ class Driver {
auto motor = it_motor->second; //motors[joint_name]; auto motor = it_motor->second; //motors[joint_name];
if (!motor->torque) { if (!motor->torque) {
if (enable_torque) if (enable_torque) {
set_torque(motor, true); set_torque(motor, true);
else return;
} else
throw torque_not_enabled(motor); throw torque_not_enabled(motor);
} }

3
include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp
View File

@ -18,6 +18,9 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface {
public: public:
RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardwareInterface) RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardwareInterface)
DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
DynamixelHardwareInterface();
DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;

6
src/dynamixel_ros2_control.cpp
View File

@ -47,6 +47,12 @@
#define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0" #define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0"
namespace dynamixel_ros2_control { namespace dynamixel_ros2_control {
DynamixelHardwareInterface::DynamixelHardwareInterface() {
rclcpp::on_shutdown([&](){
dynamixel_driver_->set_torque_all(false);
dynamixel_driver_->write();
});
}
CallbackReturn DynamixelHardwareInterface::on_init( CallbackReturn DynamixelHardwareInterface::on_init(
const hardware_interface::HardwareInfo &info) { const hardware_interface::HardwareInfo &info) {