#ifndef dynamixel_hardware_interface
#define dynamixel_hardware_interface
#include <limits>
#include <math.h>
#include <stdexcept>
#include <unordered_map>
// ROS
#include <ros/ros.h>
#include <urdf/model.h>
// ROS-related: to parse parameters
#include <XmlRpcException.h>
#include <XmlRpcValue.h>
// ROS control
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <joint_limits_interface/joint_limits_urdf.h>
// Library for access to the dynamixels
#include <dynamixel/dynamixel.hpp>
namespace dynamixel {
/** Hardware interface for a set of dynamixel actuators.
This class fits in the ros_control framework for robot control.
Warning: this code is currently limited to joint-mode dynamixel actuators
**/
template <class Protocol>
class DynamixelHardwareInterface : public hardware_interface::RobotHW {
public:
// Actuator's id type
using id_t = typename Protocol::id_t;
DynamixelHardwareInterface(){};
~DynamixelHardwareInterface();
/** Initialise the whole hardware interface.
Set the essential parameters for communication with the hardware
and find all connected devices and register those referred in
dynamixel_map in the hardware interface.
**/
bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);
/** Copy joint's information to memory
firstly queries the information from the dynamixels, then put it in
private attributes, for use by a controller.
Warning: do not get any information on torque
**/
virtual void read(const ros::Time& time, const ros::Duration& elapsed_time);
/** Send new joint's target position to dynamixels
takes the target position from memory (given by a controller) and sends
them to the dynamixels.
**/
virtual void write(const ros::Time& time, const ros::Duration& elapsed_time);
private:
using dynamixel_servo = std::shared_ptr<dynamixel::servos::BaseServo<Protocol>>;
DynamixelHardwareInterface(DynamixelHardwareInterface<Protocol> const&) = delete;
DynamixelHardwareInterface& operator=(DynamixelHardwareInterface<Protocol> const&) = delete;
// Methods used for initialisation
bool _get_ros_parameters(ros::NodeHandle& root_nh,
ros::NodeHandle& robot_hw_nh);
dynamixel::OperatingMode _str2mode(const std::string& mode_string,
std::string name);
bool _load_urdf(ros::NodeHandle& nh, std::string param_name);
bool _find_servos();
void _enable_and_configure_servo(dynamixel_servo servo, OperatingMode mode);
void _register_joint_limits(const hardware_interface::JointHandle& cmd_handle,
id_t id);
void _enforce_limits(const ros::Duration& loop_period);
// ROS's hardware interface instances
hardware_interface::JointStateInterface _jnt_state_interface;
hardware_interface::PositionJointInterface _jnt_pos_interface;
hardware_interface::VelocityJointInterface _jnt_vel_interface;
// Joint limits (hard and soft)
joint_limits_interface::PositionJointSoftLimitsInterface _jnt_pos_lim_interface;
joint_limits_interface::VelocityJointSoftLimitsInterface _jnt_vel_lim_interface;
joint_limits_interface::PositionJointSaturationInterface _jnt_pos_sat_interface;
joint_limits_interface::VelocityJointSaturationInterface _jnt_vel_sat_interface;
// Memory space shared with the controller
// It reads here the latest robot's state and put here the next desired values
std::vector<double> _prev_commands;
std::vector<double> _joint_commands; // target joint angle or speed
std::vector<double> _joint_angles; // actual joint angle
std::vector<double> _joint_velocities; // actual joint velocity
std::vector<double> _joint_efforts; // compulsory but not used
// USB to serial connection settings
std::string _usb_serial_interface;
int _baudrate;
float _read_timeout, _scan_timeout; // in seconds
// Dynamixel's low level controller
dynamixel::controllers::Usb2Dynamixel _dynamixel_controller;
// List of actuators (collected at init. from the actuators)
std::vector<dynamixel_servo> _servos;
// Map from dynamixel ID to actuator's name
std::unordered_map<id_t, std::string> _dynamixel_map;
// Map from dynamixel ID to actuator's command interface (ID: velocity/position)
std::unordered_map<id_t, OperatingMode> _c_mode_map;
// Map for servos in velocity mode which speed needs to be inverted
std::unordered_map<id_t, bool> _invert;
// Map for max speed (ID: max speed)
std::unordered_map<id_t, double> _dynamixel_max_speed;
// Map for angle offsets (ID: correction in radians)
std::unordered_map<id_t, double> _dynamixel_corrections;
// To get joint limits from the parameter server
ros::NodeHandle _nh;
// URDF model of the robot, for joint limits
std::shared_ptr<urdf::Model> _urdf_model;
};
template <class Protocol>
DynamixelHardwareInterface<Protocol>::~DynamixelHardwareInterface()
{
// stop all actuators
try {
for (auto dynamixel_servo : _servos) {
dynamixel::StatusPacket<Protocol> status;
_dynamixel_controller.send(dynamixel_servo->set_torque_enable(0));
_dynamixel_controller.recv(status);
}
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while trying to "
<< "power them off:\n"
<< e.msg());
}
}
template <class Protocol>
bool DynamixelHardwareInterface<Protocol>::init(
ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
{
// Get the relevant parameters from rosparam
// Search for the servos declared bu the user, in the parameters
if (!_get_ros_parameters(root_nh, robot_hw_nh) || !_find_servos())
return false;
// declare all available actuators to the control manager, provided a
// name has been given for them
// also enable the torque output on the actuators (sort of power up)
try {
for (unsigned i = 0; i < _servos.size(); i++) {
id_t id = _servos[i]->id();
// check that the actuator's name is in the map
typename std::unordered_map<id_t, std::string>::iterator dynamixel_iterator
= _dynamixel_map.find(id);
if (dynamixel_iterator != _dynamixel_map.end()) {
// tell ros_control the in-memory addresses where to read the
// information on joint angle, velocity and effort
hardware_interface::JointStateHandle state_handle(
dynamixel_iterator->second,
&_joint_angles[i],
&_joint_velocities[i],
&_joint_efforts[i]);
_jnt_state_interface.registerHandle(state_handle);
// check that the actuator control mode matches the declared
// command interface (position/velocity)
OperatingMode hardware_mode
= operating_mode<Protocol>(_dynamixel_controller, id);
typename std::unordered_map<id_t, OperatingMode>::iterator c_mode_map_i
= _c_mode_map.find(id);
if (c_mode_map_i != _c_mode_map.end()) {
if (c_mode_map_i->second != hardware_mode) {
ROS_ERROR_STREAM("The command interface declared "
<< mode2str(c_mode_map_i->second)
<< " for joint " << dynamixel_iterator->second
<< " but is set to " << mode2str(hardware_mode)
<< " in hardware. Disabling this joint.");
_c_mode_map[id] = OperatingMode::unknown;
}
}
else {
ROS_ERROR_STREAM("The command interface was not defined "
<< "for joint " << dynamixel_iterator->second
<< ". Disabling this joint.");
_c_mode_map[id] = OperatingMode::unknown;
}
// tell ros_control the in-memory address to change to set new
// position or velocity goal for the actuator (depending on
// hardware_mode)
hardware_interface::JointHandle cmd_handle(
_jnt_state_interface.getHandle(dynamixel_iterator->second),
&_joint_commands[i]);
if (OperatingMode::joint == hardware_mode) {
_jnt_pos_interface.registerHandle(cmd_handle);
}
else if (OperatingMode::wheel == hardware_mode) {
_jnt_vel_interface.registerHandle(cmd_handle);
}
else if (OperatingMode::unknown != hardware_mode) {
ROS_ERROR_STREAM("Servo " << id << " was not initialised "
<< "(operating mode "
<< mode2str(hardware_mode)
<< " is not supported)");
_c_mode_map[id] = OperatingMode::unknown;
}
// Enable servos that were properly configured
if (OperatingMode::unknown != _c_mode_map[id]) {
// Set joint limits (saturation or soft for the joint)
_register_joint_limits(cmd_handle, id);
// enable torque output on the servo and set its configuration
// including max speed
_enable_and_configure_servo(_servos[i], hardware_mode);
}
else {
// remove this servo
_servos.erase(_servos.begin() + i);
--i;
}
}
else {
ROS_WARN_STREAM("Servo " << id << " was not initialised "
<< "(not found in the parameters)");
// remove this servo
_servos.erase(_servos.begin() + i);
--i;
}
}
// register the hardware interfaces
registerInterface(&_jnt_state_interface);
registerInterface(&_jnt_pos_interface);
registerInterface(&_jnt_vel_interface);
}
catch (const ros::Exception& e) {
// TODO: disable actuators that were enabled ?
ROS_FATAL_STREAM("Error during initialisation. BEWARE: some "
<< "actuators might have already been started.");
return false;
}
// At startup robot should keep the pose it has
ros::Duration period(0);
read(ros::Time::now(), period);
for (unsigned i = 0; i < _servos.size(); i++) {
OperatingMode mode = _c_mode_map[_servos[i]->id()];
if (OperatingMode::joint == mode)
_joint_commands[i] = _joint_angles[i];
else if (OperatingMode::wheel == mode)
_joint_commands[i] = 0;
}
return true;
}
template <class Protocol>
void DynamixelHardwareInterface<Protocol>::read(const ros::Time& time,
const ros::Duration& elapsed_time)
{
for (unsigned i = 0; i < _servos.size(); i++) {
dynamixel::StatusPacket<Protocol> status;
try {
// current position
_dynamixel_controller.send(_servos[i]->get_present_position_angle());
_dynamixel_controller.recv(status);
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while getting "
<< _dynamixel_map[_servos[i]->id()] << "'s position\n"
<< e.msg());
}
if (status.valid()) {
try {
_joint_angles[i] = _servos[i]->parse_present_position_angle(status);
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Unpack exception while getting "
<< _dynamixel_map[_servos[i]->id()] << "'s position\n"
<< e.msg());
}
// Invert the orientation, if configured
typename std::unordered_map<id_t, bool>::iterator
invert_iterator
= _invert.find(_servos[i]->id());
if (invert_iterator != _invert.end()) {
_joint_angles[i] = 2 * M_PI - _joint_angles[i];
}
// Apply angle correction to joint, if any
typename std::unordered_map<id_t, double>::iterator
dynamixel_corrections_iterator
= _dynamixel_corrections.find(_servos[i]->id());
if (dynamixel_corrections_iterator != _dynamixel_corrections.end()) {
_joint_angles[i] -= dynamixel_corrections_iterator->second;
}
}
else {
ROS_WARN_STREAM("Did not receive any data when reading "
<< _dynamixel_map[_servos[i]->id()] << "'s position");
}
dynamixel::StatusPacket<Protocol> status_speed;
try {
// current speed
_dynamixel_controller.send(_servos[i]->get_present_speed());
_dynamixel_controller.recv(status_speed);
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while getting "
<< _dynamixel_map[_servos[i]->id()] << "'s velocity\n"
<< e.msg());
}
if (status_speed.valid()) {
try {
_joint_velocities[i]
= _servos[i]->parse_joint_speed(status_speed);
typename std::unordered_map<id_t, bool>::iterator
invert_iterator
= _invert.find(_servos[i]->id());
if (invert_iterator != _invert.end()
&& invert_iterator->second)
_joint_velocities[i] = -_joint_velocities[i];
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Unpack exception while getting "
<< _dynamixel_map[_servos[i]->id()] << "'s velocity\n"
<< e.msg());
}
}
else {
ROS_WARN_STREAM("Did not receive any data when reading "
<< _dynamixel_map[_servos[i]->id()] << "'s velocity");
}
}
}
template <class Protocol>
void DynamixelHardwareInterface<Protocol>::write(const ros::Time& time,
const ros::Duration& loop_period)
{
// ensure that the joints limits are respected
_enforce_limits(loop_period);
for (unsigned int i = 0; i < _servos.size(); i++) {
// Sending commands only when needed
if (std::abs(_joint_commands[i] - _prev_commands[i]) < std::numeric_limits<double>::epsilon())
continue;
_prev_commands[i] = _joint_commands[i];
try {
dynamixel::StatusPacket<Protocol> status;
double command = _joint_commands[i];
OperatingMode mode = _c_mode_map[_servos[i]->id()];
if (OperatingMode::joint == mode) {
typename std::unordered_map<id_t, double>::iterator
dynamixel_corrections_iterator
= _dynamixel_corrections.find(_servos[i]->id());
if (dynamixel_corrections_iterator != _dynamixel_corrections.end()) {
command += dynamixel_corrections_iterator->second;
}
// Invert the orientation, if configured
typename std::unordered_map<id_t, bool>::iterator
invert_iterator
= _invert.find(_servos[i]->id());
if (invert_iterator != _invert.end()) {
command = 2 * M_PI - command;
}
ROS_DEBUG_STREAM("Setting position for joint "
<< _dynamixel_map[_servos[i]->id()] << " to " << command
<< " rad.");
_dynamixel_controller.send(
_servos[i]->reg_goal_position_angle(command));
_dynamixel_controller.recv(status);
}
else if (OperatingMode::wheel == mode) {
// Invert the orientation, if configured
const short sign = 1; // formerly: _invert[_servos[i]->id()] ? -1 : 1;
typename std::unordered_map<id_t, bool>::iterator
invert_iterator
= _invert.find(_servos[i]->id());
if (invert_iterator != _invert.end()
&& invert_iterator->second) {
command = -command;
}
ROS_DEBUG_STREAM("Setting velocity for joint "
<< _servos[i]->id() << " to " << command);
_dynamixel_controller.send(
_servos[i]->reg_moving_speed_angle(command, mode));
_dynamixel_controller.recv(status);
}
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while sending "
<< "new commands:\n"
<< e.msg());
}
}
try {
_dynamixel_controller.send(dynamixel::instructions::Action<Protocol>(Protocol::broadcast_id));
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while sending "
<< "new commands:\n"
<< e.msg());
}
}
/** Retrieve all needed ROS parameters
@return true if and only if all went well
**/
template <class Protocol>
bool DynamixelHardwareInterface<Protocol>::_get_ros_parameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
{
bool got_all_params = true;
got_all_params &= robot_hw_nh.getParam("serial_interface", _usb_serial_interface); // path to the device
int baudrate;
got_all_params &= robot_hw_nh.getParam("baudrate", baudrate); // in bauds
_baudrate = get_baudrate(baudrate); // convert to OS-defined values
got_all_params &= robot_hw_nh.getParam("read_timeout", _read_timeout); // in seconds
bool dyn_scan = robot_hw_nh.getParam("scan_timeout", _scan_timeout); // in seconds
if (!dyn_scan) {
ROS_WARN_STREAM("Dynamixel scanning timeout parameter was not found. "
<< "Setting to default: 0.05s.");
_scan_timeout = 0.05;
}
std::string default_command_interface;
bool has_default_command_interface = robot_hw_nh.getParam(
"default_command_interface", default_command_interface);
// Retrieve the servo parameter and fill maps above with its content.
XmlRpc::XmlRpcValue servos_param; // temporary map, from parameter server
if (got_all_params &= robot_hw_nh.getParam("servos", servos_param)) {
ROS_ASSERT(servos_param.getType() == XmlRpc::XmlRpcValue::TypeStruct);
try {
for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = servos_param.begin(); it != servos_param.end(); ++it) {
ROS_DEBUG_STREAM("servo: " << (std::string)(it->first));
id_t id;
if (it->second.hasMember("id")) {
id = static_cast<int>(servos_param[it->first]["id"]);
ROS_DEBUG_STREAM("\tid: " << (int)id);
_dynamixel_map[id] = it->first;
}
else {
ROS_ERROR_STREAM("Actuator " << it->first
<< " has no associated servo ID.");
continue;
}
if (it->second.hasMember("offset")) {
_dynamixel_corrections[id]
= static_cast<double>(servos_param[it->first]["offset"]);
ROS_DEBUG_STREAM("\toffset: "
<< _dynamixel_corrections[id]);
}
if (it->second.hasMember("command_interface")) {
std::string mode_string
= static_cast<std::string>(
servos_param[it->first]["command_interface"]);
ROS_DEBUG_STREAM("\tcommand_interface: " << mode_string);
_c_mode_map[id] = _str2mode(mode_string, it->first);
}
else if (has_default_command_interface) {
_c_mode_map[id]
= _str2mode(default_command_interface, it->first);
ROS_DEBUG_STREAM("\tcommand_interface: "
<< default_command_interface << " (default)");
}
else {
ROS_ERROR_STREAM("A command interface (speed or position) "
<< "should be declared for the actuator " << it->first
<< " or a default one should be defined with the parameter "
<< "'default_command_interface'.");
}
if (it->second.hasMember("reverse")) {
_invert[id] = servos_param[it->first]["reverse"];
}
}
}
catch (XmlRpc::XmlRpcException& e) {
ROS_FATAL_STREAM("Exception raised by XmlRpc while reading the "
<< "configuration: " << e.getMessage() << ".\n"
<< "Please check the configuration, particularly parameter types.");
return false;
}
}
if (!got_all_params) {
std::string sub_namespace = robot_hw_nh.getNamespace();
std::string error_message = "One or more of the following parameters "
"were not set:\n"
+ sub_namespace + "/serial_interface\n"
+ sub_namespace + "/baudrate\n"
+ sub_namespace + "/read_timeout\n"
+ sub_namespace + "/servos";
ROS_FATAL_STREAM(error_message);
return false;
}
// Get joint limits from the URDF or the parameter server
// ------------------------------------------------------
std::string urdf_param_name("robot_description");
// TODO: document this feature
if (robot_hw_nh.hasParam("urdf_param_name"))
robot_hw_nh.getParam("urdf_param_name", urdf_param_name);
if (!_load_urdf(root_nh, urdf_param_name))
ROS_INFO_STREAM("Unable to find a URDF model.");
else
ROS_DEBUG_STREAM("Received the URDF from param server.");
return true;
}
/** Convert a string to an operating mode for a Dynamixel servo
@param mode_string name of the operating mode (either velocity or position)
@param nam name of the joint
@return dynamixel::OperatingMode::unknown if mode_string is not recognized
**/
template <class Protocol>
dynamixel::OperatingMode DynamixelHardwareInterface<Protocol>::_str2mode(
const std::string& mode_string, std::string name)
{
dynamixel::OperatingMode mode;
if ("velocity" == mode_string)
mode = dynamixel::OperatingMode::wheel;
else if ("position" == mode_string)
mode = dynamixel::OperatingMode::joint;
else {
mode = dynamixel::OperatingMode::unknown;
ROS_ERROR_STREAM("The command mode " << mode_string
<< " is not available (actuator "
<< name << ")");
}
return mode;
}
/** Search for the robot's URDF model on the parameter server and parse it
@param nh NodeHandle used to query for the URDF
@param param_name name of the ROS parameter holding the robot model
@param urdf_model pointer to be populated by this function
**/
template <class Protocol>
bool DynamixelHardwareInterface<Protocol>::_load_urdf(ros::NodeHandle& nh,
std::string param_name)
{
std::string urdf_string;
if (_urdf_model == nullptr)
_urdf_model = std::make_shared<urdf::Model>();
// get the urdf param on param server
nh.getParam(param_name, urdf_string);
return !urdf_string.empty() && _urdf_model->initString(urdf_string);
}
/** Search for the requested servos
Servos that were not requested are ignored and the software complain if
any required one misses.
@return true if and only if there was no error
**/
template <class Protocol>
bool DynamixelHardwareInterface<Protocol>::_find_servos()
{
// extract servo IDs from _dynamixel_map
std::vector<typename Protocol::id_t> ids(_dynamixel_map.size());
using dm_iter_t = typename std::unordered_map<id_t, std::string>::iterator;
for (dm_iter_t dm_iter = _dynamixel_map.begin(); dm_iter != _dynamixel_map.end(); ++dm_iter) {
ids.push_back(dm_iter->first);
}
// get the list of available actuators using the vector of IDs
try {
// small recv timeout for auto_detect
_dynamixel_controller.set_recv_timeout(_scan_timeout);
_dynamixel_controller.open_serial(_usb_serial_interface, _baudrate);
_servos = dynamixel::auto_detect<Protocol>(_dynamixel_controller, ids);
// restore recv timeout
_dynamixel_controller.set_recv_timeout(_read_timeout);
}
catch (dynamixel::errors::Error& e) {
ROS_FATAL_STREAM("Caught a Dynamixel exception while trying to "
<< "initialise them:\n"
<< e.msg());
return false;
}
// remove servos that are not in the _dynamixel_map (i.e. that are not used)
using servo = dynamixel::DynamixelHardwareInterface<Protocol>::dynamixel_servo;
typename std::vector<servo>::iterator servo_it;
for (servo_it = _servos.begin(); servo_it != _servos.end();) {
typename std::unordered_map<id_t, std::string>::iterator dynamixel_iterator
= _dynamixel_map.find((*servo_it)->id());
// the actuator's name is not in the map
if (dynamixel_iterator == _dynamixel_map.end())
servo_it = _servos.erase(servo_it);
else
++servo_it;
}
// Check that no actuator was declared by user but not found
int missing_servos = _dynamixel_map.size() - _servos.size();
if (missing_servos > 0) {
ROS_ERROR_STREAM(
missing_servos
<< " servo"
<< (missing_servos > 1 ? "s were" : " was")
<< " declared to the hardware interface but could not be found");
return false;
}
_prev_commands.resize(_servos.size(), 0.0);
_joint_commands.resize(_servos.size(), 0.0);
_joint_angles.resize(_servos.size(), 0.0);
_joint_velocities.resize(_servos.size(), 0.0);
_joint_efforts.resize(_servos.size(), 0.0);
return true;
}
/** Enable torque output for a joint and send it the relevant settings.
For now, these settings are only the speed limit.
@param servo the actuator concerned
@param mode operating mode of the actuator (e.g. position or velocity,
in dynamixel speech, joint, wheel, etc.)
**/
template <class Protocol>
void DynamixelHardwareInterface<Protocol>::_enable_and_configure_servo(
dynamixel_servo servo, OperatingMode mode)
{
try {
// Enable the actuator
dynamixel::StatusPacket<Protocol> status;
ROS_DEBUG_STREAM("Enabling servo " << servo->id());
_dynamixel_controller.send(servo->set_torque_enable(1));
_dynamixel_controller.recv(status);
// Set max speed for actuators in position mode
typename std::unordered_map<id_t, double>::iterator
dynamixel_max_speed_iterator
= _dynamixel_max_speed.find(servo->id());
if (dynamixel_max_speed_iterator != _dynamixel_max_speed.end()) {
if (OperatingMode::joint == mode) {
ROS_DEBUG_STREAM("Setting velocity limit of servo "
<< _dynamixel_map[servo->id()] << " to "
<< dynamixel_max_speed_iterator->second << " rad/s.");
_dynamixel_controller.send(
servo->set_moving_speed_angle(
dynamixel_max_speed_iterator->second));
_dynamixel_controller.recv(status);
}
else {
ROS_WARN_STREAM("A \"max speed\" was defined for servo "
<< servo->id() << " but it is currently only supported for "
<< "servos in position mode. Ignoring the speed limit.");
}
}
else if (OperatingMode::joint == mode) {
ROS_DEBUG_STREAM("Resetting velocity limit of servo "
<< _dynamixel_map[servo->id()] << ".");
_dynamixel_controller.send(servo->set_moving_speed_angle(0));
_dynamixel_controller.recv(status);
}
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while "
<< "initializing:\n"
<< e.msg());
}
}
template <class Protocol>
void DynamixelHardwareInterface<Protocol>::_register_joint_limits(
const hardware_interface::JointHandle& cmd_handle,
id_t id)
{
// Limits datastructures
joint_limits_interface::JointLimits joint_limits; // Position
joint_limits_interface::SoftJointLimits soft_limits; // Soft Position
bool has_joint_limits = false;
bool has_soft_limits = false;
// Get limits from URDF
if (_urdf_model == NULL) {
ROS_WARN_STREAM("No URDF model loaded, cannot be used to load joint"
" limits");
}
else {
// Get limits from URDF
urdf::JointConstSharedPtr urdf_joint
= _urdf_model->getJoint(cmd_handle.getName());
// Get main joint limits
if (urdf_joint == nullptr) {
ROS_ERROR_STREAM("URDF joint not found "
<< cmd_handle.getName());
return;
}
// Get limits from URDF
if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
has_joint_limits = true;
ROS_DEBUG_STREAM("Joint " << cmd_handle.getName()
<< " has URDF position limits ["
<< joint_limits.min_position << ", "
<< joint_limits.max_position << "]");
if (joint_limits.has_velocity_limits)
ROS_DEBUG_STREAM("Joint " << cmd_handle.getName()
<< " has URDF velocity limit ["
<< joint_limits.max_velocity << "]");
}
else {
if (urdf_joint->type != urdf::Joint::CONTINUOUS)
ROS_WARN_STREAM("Joint " << cmd_handle.getName()
<< " does not have a URDF "
"position limit");
}
// Get soft limits from URDF
if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
has_soft_limits = true;
ROS_DEBUG_STREAM("Joint " << cmd_handle.getName()
<< " has soft joint limits.");
}
else {
ROS_DEBUG_STREAM("Joint " << cmd_handle.getName()
<< " does not have soft joint "
"limits");
}
}
// Get limits from ROS param
if (joint_limits_interface::getJointLimits(cmd_handle.getName(), _nh, joint_limits)) {
has_joint_limits = true;
ROS_DEBUG_STREAM("Joint " << cmd_handle.getName()
<< " has rosparam position limits ["
<< joint_limits.min_position << ", "
<< joint_limits.max_position << "]");
if (joint_limits.has_velocity_limits)
ROS_DEBUG_STREAM("Joint " << cmd_handle.getName()
<< " has rosparam velocity limit ["
<< joint_limits.max_velocity << "]");
} // the else debug message provided internally by joint_limits_interface
// Slightly reduce the joint limits to prevent floating point errors
if (joint_limits.has_position_limits) {
joint_limits.min_position += std::numeric_limits<double>::epsilon();
joint_limits.max_position -= std::numeric_limits<double>::epsilon();
}
// Save the velocity limit for later if the joint is in position mode
// it is going to be sent to the servo-motor which will enforce it.
if (joint_limits.has_velocity_limits
&& OperatingMode::joint == _c_mode_map[id]) {
_dynamixel_max_speed[id] = joint_limits.max_velocity;
}
if (has_soft_limits) // Use soft limits
{
ROS_DEBUG_STREAM("Using soft saturation limits");
if (OperatingMode::joint == _c_mode_map[id]) {
const joint_limits_interface::PositionJointSoftLimitsHandle
soft_handle_position(
cmd_handle, joint_limits, soft_limits);
_jnt_pos_lim_interface.registerHandle(soft_handle_position);
}
else if (OperatingMode::wheel == _c_mode_map[id]) {
const joint_limits_interface::VelocityJointSoftLimitsHandle
soft_handle_velocity(
cmd_handle, joint_limits, soft_limits);
_jnt_vel_lim_interface.registerHandle(soft_handle_velocity);
}
}
else if (has_joint_limits) // Use saturation limits
{
ROS_DEBUG_STREAM("Using saturation limits (not soft limits)");
if (OperatingMode::joint == _c_mode_map[id]) {
const joint_limits_interface::PositionJointSaturationHandle
sat_handle_position(cmd_handle, joint_limits);
_jnt_pos_sat_interface.registerHandle(sat_handle_position);
}
else if (OperatingMode::wheel == _c_mode_map[id]) {
const joint_limits_interface::VelocityJointSaturationHandle
sat_handle_velocity(cmd_handle, joint_limits);
_jnt_vel_sat_interface.registerHandle(sat_handle_velocity);
}
}
} // namespace dynamixel
template <class Protocol>
void DynamixelHardwareInterface<Protocol>::_enforce_limits(const ros::Duration& loop_period)
{
// enforce joint limits
_jnt_pos_lim_interface.enforceLimits(loop_period);
_jnt_vel_lim_interface.enforceLimits(loop_period);
_jnt_pos_sat_interface.enforceLimits(loop_period);
_jnt_vel_sat_interface.enforceLimits(loop_period);
}
} // namespace dynamixel
#endif