ROS control-----joint_limits_interface简介

   joint_limits_interface 包含表示joint limits的数据结构，以及从URDF文件或者ROS参数服务器读取数据来填充这些结构的函数，以及用于不同特定硬件接口实现加强限制的函数。一般controller本身不会操作 joint_limits_interface ， 而是在controller update之后操作它（例如：在机器人硬件抽象的write或等效其它函数中操作它）。强制约束一旦触发将会强制修正controller设置的command。

  配置joint limits接口主要包括以下2方面：

• Joint limits representation

  • JointLimits Position, velocity, acceleration, jerk and effort.
  • SoftJointLimits Soft position limits, k_p, k_v (as described here).
  • Loading from URDF 用于从URDF文件加载joint limits information(position, velocity and effort)和soft joint limits信息
    
  • Loading from ROS params  用于从ROS参数服务器加载joint limits(position, velocity, acceleration, jerk and effort). 参数规范同 MoveIt定义
    

• Joint limits interface

  • 用于effort-controlled joints, soft-limits实现方式来自于PR2
    
  • 用于position-controlled joints, 修订版的PR2 soft limits
    
  • 用于 velocity-controlled joints, 基于加速度和速度limits的简单实

1 重要数据结构

1.1 handle

1.2 interface

1.3 其它数据结构

2 用例分析

2.1 joint limits配置

Soft joint limits的配置只能在URDF文件定义，但是URDF文件不能定义acceleration and jerk limits，这些必须在YAML文件定义。

<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>

YAML规范不仅可以定义acceleration and jerk limits, 而且可以重定义覆盖URDF文件中的position, velocity and effort值设定。 在YAML文件中的joint limits定义可以被加载到ROS 参数服务器：

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

2.2 加载joint limits configuration

如下用例中展示了填充joint limits数据结构的不同实现方式,

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

2.3 配置joint limits interface

   以下例中将joint limits应用到了机器人硬件中，

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