joint_limits_interface

Specifying a joint limits configuration

A joint limits specification in URDF format looks like:

<joint name="$foo_joint" type="revolute">
  <!-- other joint description elements -->

  <!-- Joint limits -->
  <limit lower="0.0"
         upper="1.0"
         effort="10.0"
         velocity="5.0" />

  <!-- Soft limits -->
  <safety_controller k_position="100"
                     k_velocity="10"
                     soft_lower_limit="0.1"
                     soft_upper_limit="0.9" />
</joint>

Soft joint limits can only be specified through URDF at the moment. In turn, URDF does not support acceleration and jerk limits. These values can be provided by an additional YAML specification.

The YAML specification can thus be used not only to specify acceleration and jerk limits, but also to override the position, velocity and effort values contained in a URDF description (eg. with more conservative ones). A joint limits specification in YAML format that can be loaded to the ROS parameter server looks like:

joint_limits:
  foo_joint:
    has_position_limits: true
    min_position: 0.0
    max_position: 1.0
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 5.0
  bar_joint:
    has_position_limits: false # Continuous joint
    has_velocity_limits: true
    max_velocity: 4.0

Loading a joint limits configuration

The first example shows the different ways of populating joint limits data structures.

#include <ros/ros.h>

#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>

int main(int argc, char** argv)
{
  // Init node handle and URDF model
  ros::NodeHandle nh;
  boost::shared_ptr<urdf::ModelInterface> urdf;
  // ...initialize contents of urdf

  // Data structures
  joint_limits_interface::JointLimits limits;
  joint_limits_interface::SoftJointLimits soft_limits;

  // Manual value setting
  limits.has_velocity_limits = true;
  limits.max_velocity = 2.0;

  // Populate (soft) joint limits from URDF
  // Limits specified in URDF overwrite existing values in 'limits' and 'soft_limits'
  // Limits not specified in URDF preserve their existing values
  boost::shared_ptr<const urdf::Joint> urdf_joint = urdf->getJoint("foo_joint");
  const bool urdf_limits_ok = getJointLimits(urdf_joint, limits);
  const bool urdf_soft_limits_ok = getSoftJointLimits(urdf_joint, soft_limits);

  // Populate (soft) joint limits from the ros parameter server
  // Limits specified in the parameter server overwrite existing values in 'limits' and 'soft_limits'
  // Limits not specified in the parameter server preserve their existing values
  const bool rosparam_limits_ok = getJointLimits("foo_joint", nh, limits);
}

Setting up a joint limits interface

The second example integrates joint limits enforcing into an existing robot hardware implementation.

#include <joint_limits_interface/joint_limits_interface.h>

using namespace hardware_interface;
using joint_limits_interface::JointLimits;
using joint_limits_interface::SoftJointLimits;
using joint_limits_interface::PositionJointSoftLimitsHandle;
using joint_limits_interface::PositionJointSoftLimitsInterface;

class MyRobot
{
public:
  MyRobot() {}

  bool init()
  {
    // Populate pos_cmd_interface_ with joint handles...

    // Get joint handle of interest
    JointHandle joint_handle = pos_cmd_interface_.getHandle("foo_joint");

    JointLimits limits;
    SoftJointLimits soft_limits;
    // Populate with any of the methods presented in the previous example...

    // Register handle in joint limits interface
    PositionJointSoftLimitsHandle handle(joint_handle, // We read the state and read/write the command
                                         limits,       // Limits spec
                                         soft_limits)  // Soft limits spec

    jnt_limits_interface_.registerHandle(handle);
  }

  void read(ros::Time time, ros::Duration period)
  {
    // Read actuator state from hardware...

    // Propagate current actuator state to joints...
  }

  void write(ros::Time time, ros::Duration period)
  {
    // Enforce joint limits for all registered handles
    // Note: one can also enforce limits on a per-handle basis: handle.enforceLimits(period)
    jnt_limits_interface_.enforceLimits(period);

    // Porpagate joint commands to actuators...

    // Send actuator command to hardware...
  }

private:
  PositionJointInterface pos_cmd_interface_;
  PositionJointSoftLimitsInterface jnt_limits_interface_;
};