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

PID configurable

Browse Source
This commit is contained in:
Nils Schulte 2022-02-13 16:51:13 +01:00
parent f38fcac529
commit b8e3f915cf
3 changed files with 184 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
README.md
View File

@ -26,6 +26,9 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
</hardware>
<joint name="joint1">
<param name="id">10</param>
<param name="p_gain">1120.0</param>
<param name="i_gain">170.0</param>
<param name="d_gain">3600.0</param>
<!--param name="reboot_on_error">true</param-->
<command_interface name="position"/>
<!--command_interface name="velocity"/-->
@ -35,7 +38,9 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
</joint>
...
```
### Optional parameters
When `reboot_on_error` is "true" the motor automaticaly reboots on mechanical overload error (it still loses torque for a brief moment, but that should be enogh to let the motor recover the fault).
`p_gain`, `i_gain` and `d_gain` set the PID gain values of the motor.
## Run a test
You need the urdf of the robot with the ros2_control xml added.

140
include/dynamixel_ros2_control/dynamixel_driver.hpp
View File

@ -9,6 +9,7 @@
#include <map>
#include <memory>
#include <vector>
#include <limits>
#include "dynamixel_sdk/dynamixel_sdk.h"
#include "iostream"
@ -20,9 +21,11 @@ class Driver {
public:
typedef std::function<void(const std::string& joint_name)> hw_error_callback;
private:
class Motor;
struct GroupSyncReader {
GroupSyncRead syncRead;
std::vector<std::function<void(GroupSyncRead&)>> read_functions;
std::vector<Motor*> motors;
GroupSyncReader(
std::unique_ptr<PortHandler>& port,
std::unique_ptr<PacketHandler>& ph,
@ -42,6 +45,7 @@ class Driver {
bool led;
bool rebooting;
bool rebooted;
double goal_position;
@ -49,13 +53,16 @@ class Driver {
//double goal_velocity;
double velocity;
double effort;
bool torque;
uint8_t hw_error;
double p_gain, i_gain, d_gain;
double p_gain_target, i_gain_target, d_gain_target;
std::vector<std::function<void()>> after_reboot_functions;
typedef std::function<void(dynamixel::hardware_status)> hw_error_callback;
//hw_error_callback error_callback;
@ -87,7 +94,17 @@ class Driver {
void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); }
void set_effort_from_raw(uint32_t raw) { effort = double(static_cast<int16_t>(raw))*0.1; }
Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), rebooting(false), goal_position(std::nan("")), position(std::nan("")) {
Motor(const motor_id p_id, std::string p_name, const model_t type) :
id(p_id),
name(p_name),
rebooting(true),
rebooted(false),
goal_position(std::numeric_limits<double>::quiet_NaN()),
position(std::numeric_limits<double>::quiet_NaN()),
p_gain(std::numeric_limits<double>::quiet_NaN()),
i_gain(std::numeric_limits<double>::quiet_NaN()),
d_gain(std::numeric_limits<double>::quiet_NaN())
{
try {
model = &model_infos.at(type);
} catch (const std::exception &e) {
@ -111,7 +128,7 @@ class Driver {
[this](uint32_t data) {
this->torque = bool(data);
if (this->torque == false) {
goal_position = std::nan("");
goal_position = std::numeric_limits<double>::quiet_NaN();
}
});
@ -176,10 +193,38 @@ class Driver {
//https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status
} else {
this->rebooting = false;
this->rebooted = true;
}
});
}
}
void setup_init_sync_readers(
std::map<field, std::unique_ptr<GroupSyncReader>>& initSyncReaders,
std::unique_ptr<PortHandler>& portHandler,
std::unique_ptr<PacketHandler>& packetHandler)
{
//Load PID gains if available
try {
setup_sync_reader(
initSyncReaders, portHandler, packetHandler,
command::Position_P_Gain,
[this](uint32_t raw) {
this->p_gain = double(static_cast<int16_t>(raw));
});
setup_sync_reader(
initSyncReaders, portHandler, packetHandler,
command::Position_I_Gain,
[this](uint32_t raw) {
this->i_gain = double(static_cast<int16_t>(raw));
});
setup_sync_reader(
initSyncReaders, portHandler, packetHandler,
command::Position_D_Gain,
[this](uint32_t raw) {
this->d_gain = double(static_cast<int16_t>(raw));
});
} catch (command_type_not_implemented& e) {}
}
void setup_sync_reader(
@ -199,6 +244,7 @@ class Driver {
std::make_unique<GroupSyncReader>(portHandler, packetHandler, sr_field)});
}
sync_reader_it->second->syncRead.addParam(this->id);
sync_reader_it->second->motors.push_back(this);
sync_reader_it->second->read_functions.push_back(
[this, sr_field, callback](GroupSyncRead& read) {
if (read.isAvailable(this->id, sr_field.address, sr_field.data_length)) {
@ -216,6 +262,7 @@ class Driver {
std::unique_ptr<PortHandler> portHandler;
std::unique_ptr<PacketHandler> packetHandler;
std::map<field, std::unique_ptr<GroupSyncReader>> initSyncReaders;
std::map<field, std::unique_ptr<GroupSyncReader>> syncReaders;
std::map<field, std::unique_ptr<GroupSyncWrite>> syncWrites;
@ -285,7 +332,11 @@ class Driver {
add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&p));
}
void reboot(const dynamixel::Driver::Motor::Ptr& motor) {
void reboot(dynamixel::Driver::Motor::Ptr& motor) {
motor->after_reboot_functions.clear();
motor->after_reboot_functions.push_back([this, motor](){
this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target);
});
packetHandler->reboot(portHandler.get(),motor->id);
}
@ -367,6 +418,8 @@ class Driver {
auto motor = std::make_shared<Motor>(id, joint_name, type);
motors[joint_name] = motor;
std::weak_ptr<Motor> weak_motor(motors[joint_name]);
motor->setup_init_sync_readers(initSyncReaders, portHandler, packetHandler);
motor->setup_sync_readers(syncReaders, portHandler, packetHandler,
[this, weak_motor](dynamixel::hardware_status s)
{
@ -415,6 +468,22 @@ class Driver {
}
void read() {
bool init_done = true;
for (auto it = initSyncReaders.begin(); it != initSyncReaders.end(); std::advance(it,1)) {
const auto& [field, syncReader] = *it;
(void)field;
/*auto result = */ syncReader->syncRead.txRxPacket();
//std::cout<<packetHandler->getTxRxResult(result)<<std::endl;
for (auto read_f : syncReader->read_functions) {
read_f(syncReader->syncRead);
}
// If no motor is rebooting then the initial read was succesfull.
if (!std::all_of(syncReader->motors.cbegin(), syncReader->motors.cend(),
[](const auto& m){ return !m->rebooting; })) {
init_done = false;
}
}
for (const auto& [field, syncReader] : syncReaders) {
(void)field;
/*auto result = */ syncReader->syncRead.txRxPacket();
@ -423,6 +492,18 @@ class Driver {
read_f(syncReader->syncRead);
}
}
if(init_done) {
initSyncReaders.clear();
for (const auto& [joint_name, motor]: motors) {
if(motor->rebooted) {
motor->rebooted = false;
for(auto after_reboot_f:motor->after_reboot_functions) {
after_reboot_f();
}
}
}
}
}
/**
@ -579,7 +660,6 @@ class Driver {
* @param joint_name the name of the joint
* @param position the position in radians the actuator should move to
* @param enable_torque enable the torque if not enabled already
* @return true if successfull, false otherwise (e.g. joint_name is not available)
*/
void set_position(const std::string& joint_name, double position, bool enable_torque = false) {
if (std::isnan(position))
@ -603,6 +683,54 @@ class Driver {
add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&p));
}
/**
* Sets the pid gains for a motor. This is only possible after the first read
* as the initial values need to be read from the motor.
* @param joint_name the name of the joint
*/
void pid_gains(const std::string& joint_name, double& p, double& i, double& d) {
auto it_motor = motors.find(joint_name);
if (it_motor == motors.end())
throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!");
auto motor = it_motor->second;
//std::cout <<joint_name<<" target p:"<< p<<"\ti:"<< i<<"\td:" << d << std::endl;
//std::cout <<joint_name<<" current p:"<< motor->p_gain<<"\ti:"<< motor->i_gain<<"\td:" << motor->d_gain << std::endl;
if(std::isnan( motor->p_gain) || std::isnan( motor->i_gain) || std::isnan( motor->d_gain)) {
throw std::invalid_argument("Cant set pid gains for \"" + joint_name + "\" because motor doesn't have pid control!");
}
if(std::isnan(p)) {
p = motor->p_gain;
}
if(std::isnan(i)) {
i = motor->i_gain;
}
if(std::isnan(d)) {
d = motor->d_gain;
}
if( p != motor->p_gain) {
field command_field;
motor->get_command(command::Position_P_Gain, command_field);
uint32_t data = static_cast<uint32_t>(p);
motor->p_gain_target = p;
add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&data));
}
if( i != motor->i_gain) {
field command_field;
motor->get_command(command::Position_I_Gain, command_field);
uint32_t data = static_cast<uint32_t>(i);
motor->i_gain_target = i;
add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&data));
}
if( d != motor->d_gain) {
field command_field;
motor->get_command(command::Position_D_Gain, command_field);
uint32_t data = static_cast<uint32_t>(d);
motor->d_gain_target = d;
add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&data));
}
}
/**
* reboots a joint.
* @param joint_name the name of the joint

49
src/dynamixel_ros2_control.cpp
View File

@ -22,6 +22,7 @@
#include <thread>
#include <utility>
#include <vector>
#include <limits>
#include "dynamixel_ros2_control/dynamixel_driver.hpp"
@ -38,6 +39,9 @@
#define DYN_PORTNAME_PARAM_STR "serial_port"
#define DYN_ENABLE_TORQUE_ON_START_PARAM_STR "enable_torque_on_startup"
#define DYN_AUTO_REBOOT_PARAM_STR "reboot_on_error"
#define DYN_P_GAIN_PARAM_STR "p_gain"
#define DYN_I_GAIN_PARAM_STR "i_gain"
#define DYN_D_GAIN_PARAM_STR "d_gain"
#define DYN_ID_PARAM_STR "id"
#define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0"
@ -111,9 +115,43 @@ CallbackReturn DynamixelHardwareInterface::on_init(
(std::string("Motors count: ") +
std::to_string(dynamixel_driver_->get_motor_count()))
.c_str());
dynamixel_driver_->read();
for (const auto &joint : info_.joints) {
auto p_it = joint.parameters.find(DYN_P_GAIN_PARAM_STR);
double p = (p_it != joint.parameters.end()) ? std::stod(p_it->second)
: std::numeric_limits<double>::quiet_NaN();
auto i_it = joint.parameters.find(DYN_I_GAIN_PARAM_STR);
double i = (i_it != joint.parameters.end()) ? std::stod(i_it->second)
: std::numeric_limits<double>::quiet_NaN();
auto d_it = joint.parameters.find(DYN_D_GAIN_PARAM_STR);
double d = (d_it != joint.parameters.end()) ? std::stod(d_it->second)
: std::numeric_limits<double>::quiet_NaN();
try {
dynamixel_driver_->pid_gains(joint.name, p, i, d);
if (!std::isnan(p) || !std::isnan(i) || !std::isnan(d)) {
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
("Joint-Actuator \"" + std::string(joint.name) +
"\" p:%f\t i:%f\t d:%f")
.c_str(),
float(p), float(i), float(d));
}
} catch (const std::exception &e) {
if (p_it != joint.parameters.end() || i_it != joint.parameters.end() ||
d_it != joint.parameters.end()) {
RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
("Joint-Actuator \"" + std::string(joint.name) +
"\" Cant set PID gains: " + e.what())
.c_str());
}
}
}
if (failure) {
return CallbackReturn::ERROR;
}
RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
"init success");
return CallbackReturn::SUCCESS;
}
@ -158,7 +196,7 @@ void DynamixelHardwareInterface::overload_error_callback(
std::vector<hardware_interface::StateInterface>
DynamixelHardwareInterface::export_state_interfaces() {
RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
"export_state_interfaces");
std::vector<hardware_interface::StateInterface> state_interfaces;
dynamixel_driver_->for_each_joint([&](const std::string &joint_name) {
@ -177,7 +215,7 @@ DynamixelHardwareInterface::export_state_interfaces() {
std::vector<hardware_interface::CommandInterface>
DynamixelHardwareInterface::export_command_interfaces() {
RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
"export_command_interfaces");
std::vector<hardware_interface::CommandInterface> command_interfaces;
dynamixel_driver_->for_each_joint([&](const std::string &joint_name) {
@ -193,7 +231,7 @@ CallbackReturn DynamixelHardwareInterface::on_activate(
RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "on_activate");
try {
dynamixel_driver_->ping_all();
RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
"Pinging dynamixels was successfull");
} catch (const dynamixel::Driver::dynamixel_bus_error &e) {
RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what());
@ -211,6 +249,8 @@ CallbackReturn DynamixelHardwareInterface::on_activate(
RCLCPP_ERROR(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what());
return CallbackReturn::ERROR;
}
RCLCPP_DEBUG(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
"activate success");
return CallbackReturn::SUCCESS;
}
@ -221,7 +261,8 @@ CallbackReturn DynamixelHardwareInterface::on_deactivate(
dynamixel_driver_->read();
dynamixel_driver_->set_torque_all(false);
dynamixel_driver_->write();
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "Motors deaktivated. Torque disabled !");
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
"Motors deaktivated. Torque disabled !");
} catch (std::invalid_argument &e) {
RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), e.what());
return CallbackReturn::ERROR;