ros-controls/ros_control — pull request #76 by adolfo-rt (2013-07-04 09:13:23+00:00)
Title: Joint limits interface

### URDF-agnostic joint limits specification

* **Joint limits** Position, velocity, acceleration, jerk and effort.
* **Soft joint limits** Soft position limits, k_p, k_v (as described [here](http://www.ros.org/wiki/pr2_controller_manager/safety_limits)).
* Utility method for loading **joint limits** information from **URDF** (only position, velocity, effort)
* Utility method for loading **soft joint limits** information from **URDF**
* Utility method for loading **joint limits** from **ROS parameter server** (all values). Parameter specification is the same used in MoveIt, with the addition that we also parse jerk and effort limits.

**Note** Joint limits specification should maybe live outside ros_control. I don't like that there's some code duplication between this pull request and MoveIt.

**Note** Maybe utilities for loading values should live elsewhere, to not impose the URDF dependency, or make it optional. For now it's required.

### Joint limits interface
* *ros_control* interface for enforcing joint limits.
* For *effort-controlled* joints, the soft-limits implementation from the PR2 has been ported.
* For *position-controlled* joints, a modified version of the PR2 soft limits has been implemented.
* For *velocity-controlled* joints, simple saturation based on acceleration and velocity limits has been implemented.

```c++
using namespace joint_limits_interface;

// Populate joint limits from URDF
UrdfJointConstPtr urdf_joint = urdf_->getJoint(name);
JointLimits limits;
SoftJointLimits soft_limits;
const bool limits_ok = getJointLimits(urdf_joint, limits) && getSoftJointLimits(urdf_joint, soft_limits);

// Register handle for enforcing limits of a position-controlled joint
JointHandle joint_handle; // Populate values...
PositionJointSoftLimitsInterface limits_iface_;
if (limits_ok)
{
PositionJointSoftLimitsHandle handle(joint_handle, // We read the state and read/write the command
limits, // Limits spec
soft_limits) // Soft limits spec
limits_iface_.registerHandle(handle);
}

// Populate with other handles...

// Enforce joint limits
ros::Duration period(0.001); // Control period
limits_iface_.enforceLimits(period); // Apply to all registered handles

// One can also enforce limits on a per-handle basis
limits_iface_.getHandle(joint_handle.getName()).enforceLimits(period);
```

**Note** Joint limits enforcing for effort-controlled joints has **not yet** been unit tested. Position- and velocity-controlled joint cases do have tests in place.