Skip to content

Commit 72463b3

Browse files
hsd-devhsd-dev
committed
enforcing joint limits from urdf / parameter server
1 parent 8828a43 commit 72463b3

File tree

2 files changed

+156
-4
lines changed

2 files changed

+156
-4
lines changed

‎ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,10 @@
4848
#include <urdf/model.h>
4949
#include <ur_controllers/speed_scaling_interface.h>
5050
#include <ur_controllers/scaled_joint_command_interface.h>
51+
#include <joint_limits_interface/joint_limits.h>
52+
#include <joint_limits_interface/joint_limits_interface.h>
53+
#include <joint_limits_interface/joint_limits_rosparam.h>
54+
#include <joint_limits_interface/joint_limits_urdf.h>
5155

5256
#include "ur_robot_driver/ur/ur_driver.h"
5357
#include <ur_robot_driver/ros/dashboard_client_ros.h>
@@ -157,6 +161,12 @@ class HardwareInterface : public hardware_interface::RobotHW
157161
bool shouldResetControllers();
158162

159163
protected:
164+
/*!
165+
* \brief Applies joint limits & soft joint limits on position, velocity & effort defined in URDF / parameter server
166+
*
167+
*/
168+
void registerJointLimits(ros::NodeHandle& robot_hw_nh, const hardware_interface::JointHandle& joint_handle_position,
169+
const hardware_interface::JointHandle& joint_handle_velocity, std::size_t joint_id);
160170

161171
/*!
162172
* \brief Loads URDF model from robot_description parameter
@@ -231,6 +241,18 @@ class HardwareInterface : public hardware_interface::RobotHW
231241
ur_controllers::ScaledVelocityJointInterface svj_interface_;
232242
hardware_interface::ForceTorqueSensorInterface fts_interface_;
233243

244+
// Joint limits interfaces - Saturation
245+
joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_;
246+
joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_;
247+
248+
// Joint limits interfaces - Soft limits
249+
joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_interface_;
250+
joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_interface_;
251+
252+
// Copy of limits, in case we need them later in our control stack
253+
std::vector<double> joint_position_lower_limits_;
254+
std::vector<double> joint_position_upper_limits_;
255+
std::vector<double> joint_velocity_limits_;
234256

235257
urdf::Model* urdf_model_;
236258

‎ur_robot_driver/src/ros/hardware_interface.cpp‎

Lines changed: 134 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -298,14 +298,18 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
298298
&joint_velocities_[i], &joint_efforts_[i]));
299299

300300
// Create joint position control interface
301-
pj_interface_.registerHandle(
302-
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
303-
vj_interface_.registerHandle(
304-
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
301+
hardware_interface::JointHandle joint_handle_position =
302+
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]);
303+
pj_interface_.registerHandle(joint_handle_position);
304+
hardware_interface::JointHandle joint_handle_velocity =
305+
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]);
306+
vj_interface_.registerHandle(joint_handle_velocity);
305307
spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
306308
js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
307309
svj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
308310
js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i], &speed_scaling_combined_));
311+
312+
registerJointLimits(robot_hw_nh, joint_handle_position, joint_handle_velocity, i);
309313
}
310314

311315
speedsc_interface_.registerHandle(
@@ -385,6 +389,128 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
385389
return true;
386390
}
387391

392+
void HardwareInterface::registerJointLimits(ros::NodeHandle& robot_hw_nh,
393+
const hardware_interface::JointHandle& joint_handle_position,
394+
const hardware_interface::JointHandle& joint_handle_velocity,
395+
std::size_t joint_id)
396+
{
397+
// Default values
398+
joint_position_lower_limits_[joint_id] = -std::numeric_limits<double>::max();
399+
joint_position_upper_limits_[joint_id] = std::numeric_limits<double>::max();
400+
joint_velocity_limits_[joint_id] = std::numeric_limits<double>::max();
401+
402+
// Limits datastructures
403+
joint_limits_interface::JointLimits joint_limits; // Position
404+
joint_limits_interface::SoftJointLimits soft_limits; // Soft Position
405+
bool has_joint_limits = false;
406+
bool has_soft_limits = false;
407+
408+
// Get limits from URDF
409+
if (urdf_model_ == NULL)
410+
{
411+
ROS_WARN_STREAM("No URDF model loaded, unable to get joint limits");
412+
return;
413+
}
414+
415+
// Get limits from URDF
416+
urdf::JointConstSharedPtr urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]);
417+
418+
// Get main joint limits
419+
if (urdf_joint == NULL)
420+
{
421+
ROS_ERROR_STREAM("URDF joint not found " << joint_names_[joint_id]);
422+
return;
423+
}
424+
425+
// Get limits from URDF
426+
if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits))
427+
{
428+
has_joint_limits = true;
429+
ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has URDF position limits [" << joint_limits.min_position
430+
<< ", " << joint_limits.max_position << "]");
431+
if (joint_limits.has_velocity_limits)
432+
ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has URDF velocity limit [" << joint_limits.max_velocity
433+
<< "]");
434+
}
435+
else
436+
{
437+
if (urdf_joint->type != urdf::Joint::CONTINUOUS)
438+
ROS_WARN_STREAM("Joint " << joint_names_[joint_id]
439+
<< " does not have a URDF "
440+
"position limit");
441+
}
442+
443+
// Get limits from ROS param
444+
if (joint_limits_interface::getJointLimits(joint_names_[joint_id], robot_hw_nh, joint_limits))
445+
{
446+
has_joint_limits = true;
447+
ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has rosparam position limits ["
448+
<< joint_limits.min_position << ", " << joint_limits.max_position << "]");
449+
if (joint_limits.has_velocity_limits)
450+
ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has rosparam velocity limit ["
451+
<< joint_limits.max_velocity << "]");
452+
} // the else debug message provided internally by joint_limits_interface
453+
454+
// Get soft limits from URDF
455+
if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
456+
{
457+
has_soft_limits = true;
458+
ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has soft joint limits.");
459+
}
460+
else
461+
{
462+
ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id]
463+
<< " does not have soft joint "
464+
"limits");
465+
}
466+
467+
// Quit we we haven't found any limits in URDF or rosparam server
468+
if (!has_joint_limits)
469+
{
470+
return;
471+
}
472+
473+
// Copy position limits if available
474+
if (joint_limits.has_position_limits)
475+
{
476+
// Slighly reduce the joint limits to prevent floating point errors
477+
joint_limits.min_position += std::numeric_limits<double>::epsilon();
478+
joint_limits.max_position -= std::numeric_limits<double>::epsilon();
479+
480+
joint_position_lower_limits_[joint_id] = joint_limits.min_position;
481+
joint_position_upper_limits_[joint_id] = joint_limits.max_position;
482+
}
483+
484+
// Copy velocity limits if available
485+
if (joint_limits.has_velocity_limits)
486+
{
487+
joint_velocity_limits_[joint_id] = joint_limits.max_velocity;
488+
}
489+
490+
if (has_soft_limits) // Use soft limits
491+
{
492+
ROS_DEBUG_STREAM("Using soft saturation limits");
493+
const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position,
494+
joint_limits, soft_limits);
495+
pos_jnt_soft_interface_.registerHandle(soft_handle_position);
496+
const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity,
497+
joint_limits, soft_limits);
498+
vel_jnt_soft_interface_.registerHandle(soft_handle_velocity);
499+
}
500+
else // Use saturation limits
501+
{
502+
ROS_DEBUG_STREAM("Using saturation limits (not soft limits)");
503+
504+
const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position,
505+
joint_limits);
506+
pos_jnt_sat_interface_.registerHandle(sat_handle_position);
507+
508+
const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity,
509+
joint_limits);
510+
vel_jnt_sat_interface_.registerHandle(sat_handle_velocity);
511+
}
512+
}
513+
388514
template <typename T>
389515
void HardwareInterface::readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg,
390516
const std::string& var_name, T& data)
@@ -522,10 +648,14 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
522648
{
523649
if (position_controller_running_)
524650
{
651+
pos_jnt_soft_interface_.enforceLimits(period);
652+
pos_jnt_sat_interface_.enforceLimits(period);
525653
ur_driver_->writeJointCommand(joint_position_command_, comm::ControlMode::MODE_SERVOJ);
526654
}
527655
else if (velocity_controller_running_)
528656
{
657+
vel_jnt_soft_interface_.enforceLimits(period);
658+
vel_jnt_sat_interface_.enforceLimits(period);
529659
ur_driver_->writeJointCommand(joint_velocity_command_, comm::ControlMode::MODE_SPEEDJ);
530660
}
531661
else

0 commit comments

Comments
 (0)