ROS学习笔记10 —— ros_control和controller的使用

1. Controllers

sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers

https://github.com/ros-controls/ros_controllers

• effort_controllers：给定期望力矩控制关节
  • joint_effort_controller
  • joint_position_controller
  • joint_velocity_controller
• joint_state_controller：读取关节位置，将注册到hardware_interface :: JointStateInterface的所有资源的状态发布到类型为sensor_msgs / JointState的主题。
  • joint_state_controller
• position_controllers： 一次设置一个或多个关节位置
  • joint_position_controller
  • joint_group_position_controller
• velocity_controllers： 一次设置一个或多个关节速度
  • joint_velocity_controller
  • joint_group_velocity_controller
• joint_trajectory_controllers： 用于为整个轨迹添加附加功能
  • position_controller
  • velocity_controller
  • effort_controller
  • position_velocity_controller
  • position_velocity_acceleration_controller

不同的controller可以完成对不同模块的控制。例如完成对关节力的控制、速度控制等等。请求下层的硬件资源，并提供PID控制器，读取底层硬件资源的状态，发送控制指令。

可根据自己的需求，创建需要的controller，并通过Controller Manager来管理自己创建的controller，具体参考：https://github.com/ros-controls/ros_control/wiki/controller_interface

2. 硬件接口 Hardware Interfaces

具体API参考http://docs.ros.org/melodic/api/hardware_interface/html/c++/namespacehardware__interface.html

ROS control 将硬件接口与ros_controller中的一个结合使用，以向硬件发送和接受命令。

• Joint Command Interface - 支持命令关节阵列的硬件接口。请注意，这些命令可以具有任何语义含义，只要它们每个都可以由单个double表示即可，它们不一定是省力命令。要指定此命令的含义，请参见派生类：

  • Effort Joint Interface - for commanding effort-based joints.
  • Velocity Joint Interface - for commanding velocity-based joints.
  • Position Joint Interface - for commanding position-based joints.

• Joint State Interfaces - 用于支持读取命名关节数组的状态，每个关节都有一些位置，速度和作用力（力或扭矩）。

• Actuator State Interfaces - 用于支持读取命名的执行器阵列的状态，每个执行器都有一定的位置，速度和作用力（力或扭矩）。

• Actuator Command Interfaces

  • Effort Actuator Interface
  • Velocity Actuator Interface
  • Position Actuator Interface

• Force-torque sensor Interface

• IMU sensor Interface

http://docs.ros.org/melodic/api/hardware_interface/html/c++/annotated.html

创建自己的硬件接口可参考：https://github.com/ros-controls/ros_control/wiki/hardware_interface

3. 控制器管理器 Controller Manager

用于管理多个控制器，实现控制器的加载、运行、停止等操作。

命令操作

• 加载、运行、停止等操作：

  rosrun controller_manager controller_manager <command> <controller_name>
  

  where，<command> 为：

  • load
  • unload
  • start
  • stop
  • spawn: load & start
  • kill: stop & unload

  一次操作多个控制器：

  # 加载控制器，但不运行
  rosrun controller_manager spawner [--stopped] name1 name2 name3
  
  # 加载控制器，并运行
  rosrun controller_manager spawner name1 name2 name3
  
  # 停止控制器，但不卸载
  rosrun controller_manager unspawner name1 name2 name3
  
  

• 查看某个控制器状态：

  rosrun controller_manager controller_manager <command>
  

  where，<command> 包括：

  • list
  • list-types
  • reload-libraries
  • reload-libraries --restore

• launch操作：

  <!--加载并启动-->
  <launch>
      <node pkg="controller_manager" 
      type="spawner" 
      args="controller_name1 controller_name2" />
  </launch>
  
  <!--只加载不启动-->
  <launch>
    <node pkg="controller_manager"
      type="spawner"
      args="--stopped controller_name1 controller_name2" />
  </launch>
  

• 管理器可视化：

  rosrun rqt_controller_manager rqt_controller_manager
  

4. 传动系统Transmissions

机器人每个需要运动的关节都需要配置相应的Transmission，可通过代码完成，但多数情况下会在URDF文件中直接添加

<transmission name="simple_trans">
	 <!--指定传动类型-->
     <type>transmission_interface/SimpleTransmission</type>
	 <!--传动系统所连接的关节-->
     <joint name="foo_joint">
     	  <!--指定硬件接口，注意，当在Gazebo中加载此传动系统是，值应为EffortJointInterface-->
     	  <!--而在RobotHW中加载此传动系统时，值应为hardware_interface/EffortJointInterface-->
          <hardwareInterface>EffortJointInterface</hardwareInterface>
     </joint>
     <actuator name="foo_motor">
     	  <!--可选，指定关节制动器之间机械减速比，并非所有传动系统都需要此标签-->
          <mechanicalReduction>50</mechanicalReduction>
          <!--可选，指定硬件接口-->
          <hardwareInterface>EffortJointInterface</hardwareInterface>
     </actuator>
</transmission>

5. 关节约束Joint Limits

Joint Limits是硬件抽象层中的一块，维护一个关节限位的数据结构，这些限位数据可以从机器人的URDF文件中加载，也可以ROS的参数服务器上加载（先用YAML配置文件导入ROS parameter server）.

限位数据包括：关节速度、位置、加速度、加加速度、力矩等方面的限位；

还包含安全作用的位置软限位、速度边界（k_v）和位置边界（k_p）等等。

• URDF文件：

  <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文件：

  joints_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
          has_velocity_limits: true
          max_velocity: 4.0
  

• URDF和YAML两种描述方式的区别：

① 目前只能通过URDF来指定软限位，URDF不支持加速度和加加速度限制，这些可通过YAML提供。
② YAML可覆盖URDF中描述的值
③ PID增益和控制器设置必须保存在yaml文件中，该文件通过roslaunch文件加载到参数服务器中

• 加载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);
  }
  

• 配置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_;
  };
  

6. gazebo_ros_control

添加传动系统

<transmission name="simple_trans">
	<type>transmission_interface/SimpleTransmission</type>
	<joint name="foo_joint">
		<hardwareInterface>EffortJointInterface</hardwareInterface>
	</joint>
	<actuator name="foo_motor">
 		<mechanicalReduction>50</mechanicalReduction>
		<hardwareInterface>EffortJointInterface</hardwareInterface>
	</actuator>
</transmission>

注意：

• <joint name="">：必须对应URDF中定义的关节名称
• <type>：传输类型，当前仅实现transmission_interface /SimpleTransmission
• <hardwareInterface>：在<actuator>和<joint>标签中的，告诉gazebo_ros_control插件要加载的硬件接口（位置，速度或力矩接口）。当前仅实现了EffortJointInterface这一功能，当前仅支持Effort接口

添加gazebo_ros_control插件

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
  </plugin>
</gazebo>

gazebo_ros_control<plugin>标签还有以下可选元素：

• <robotNamespace>：用于此插件实例化的ROS命名空间，默认为URDF / SDF中机器人的名称
• <controlPeriod>：控制器的更新周期（以秒为单位），默认为gazebo的周期
• <robotParam>：URDF文件在参数服务器上的位置，默认为/ robot_description
• <robotSimType>：要使用的自定义机器人仿真接口的pluginlib名称，默认为DefaultRobotHWSim

默认gazebo_ros_control行为

默认情况下，如果没有<robotSimType> 标签，gazebo_ros_control 将尝试从URDF中获取与基于ros_control的控制器接口所需的所有信息。

默认行为提供以下ros_control接口：

• hardware_interface::JointStateInterface
• hardware_interface::EffortJointInterface
• hardware_interface::VelocityJointInterface（未全实现）

自定义gazebo_ros_control插件

gazebo_ros_control插件还提供了一个基于pluginlib的接口，用于实现Gazebo和ros_control之间的自定义接口，以模拟更复杂的机制（非线性弹簧，连杆等）。

这些插件必须继承gazebo_ros_control :: RobotHWSim，该插件实现了模拟的ros_control ``hardware_interface :: RobotHW。RobotHWSim提供API级访问，以在Gazebo仿真器中读取和命令关节属性。

相应的RobotHWSim子类在URDF模型中指定，并在加载机器人模型时加载。例如，以下XML将加载默认插件（与不使用<robotSimType>标签时的行为相同）：

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
</gazebo>

---

参考文献：

• http://wiki.ros.org/ros_control
• https://wiki.ros.org/urdf/XML/Transmission
• https://github.com/ros-controls/ros_control/wiki/joint_limits_interface
• http://gazebosim.org/tutorials/?tut=ros_control
• https://github.com/ros-simulation/gazebo_ros_demos
• https://wiki.ros.org/dynamic_reconfigure