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