joint_limits_interface

Joint limits representation

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);
}

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: 20.0
  bar_joint:
    has_position_limits: false # Continuous joint
    has_velocity_limits: true
    max_velocity: 4.0
    has_jerk_limits: false

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_;
};