ur5在gazebo中仿真的官方源码浅析

一 复现

好久之前初学ros+gazebo机械臂仿真的时候总有些懵，用的是ur5机械臂，现在回过头来看好像看懂了一些，故重新理清了一下功能包的逻辑，方便查阅。
官方源码
本文参考
ubuntu16.04安装UR3/UR5/UR10机械臂的ROS驱动并实现gazebo下Moveit运动规划仿真以及真实UR3机械臂的运动控制(1)

1.1 roslaunch ur_gazebo ur5.launch

<launch>
  <arg name="limited" default="false"  doc="If true, limits joint range [-PI, PI] on all joints." />
  <arg name="paused" default="false" doc="Starts gazebo in paused mode" />
  <arg name="gui" default="true" doc="Starts gazebo gui" />
  
  
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  include>


  
  <include file="$(find ur_description)/launch/ur5_upload.launch">
    <arg name="limited" value="$(arg limited)"/>
  include>

  
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />

  <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>

  
  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>

  
  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" />

launch>

1.1.1 找gazebo_ros功能包（该包是安装了gazebo默认有的，会自动索引找到系统中该功能包的位置），并打开empty_world。
1.1.2 找ur_description功能包，并打开ur5_upload.launch，主要是对机械臂模型的各个参数进行初始化，在以下目录找到：

<launch>
  <arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" />

  <param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_robot.urdf.xacro' transmission_hw_interface:=$(arg transmission_hw_interface)" />
  <param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_joint_limited_robot.urdf.xacro' transmission_hw_interface:=$(arg transmission_hw_interface)" />
launch>

这里的"limited"参数arg默认值是false，所以下面的两句话执行unless那句，那么ur5_upload.launch做了一件事，打开了ur5_robot.urdf.xacro（或者ur5_joint_limited_robot.urdf.xacr）。
“param unless=“$(0 or 1)” name=“robot_description” command=……”将ur5_robot.urdf.xacro模型文件作为参数robot_description上传至参数服务器。

<robot xmlns:xacro="http://wiki.ros.org/xacro"
       name="ur5" >

  <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>

  
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  
  <xacro:ur5_robot prefix="" joint_limited="false"
    transmission_hw_interface="$(arg transmission_hw_interface)"
  />

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  joint>

robot>

1.1.2.1 打开common.gazebo.xacro
libgazebo_ros_control.so提供了一些ROS控制器，如关节控制器和力/扭矩控制器，可以与Gazebo仿真环境中的机器人模型进行交互。

<robot xmlns:xacro="http://wiki.ros.org/xacro">

  <gazebo>
    <plugin name="ros_control" filename="libgazebo_ros_control.so">
      
      
    plugin>


  gazebo>

robot>

1.1.2.2 打开ur5.urdf.xacro

<robot xmlns:xacro="http://wiki.ros.org/xacro">

  <xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/ur.gazebo.xacro" />

  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
    <inertial>
      <mass value="${mass}" />
      <xacro:insert_block name="origin" />
      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
        izz="${0.5 * mass * radius * radius}" />
    inertial>
  xacro:macro>

  <xacro:macro name="ur5_robot" params="prefix joint_limited
    shoulder_pan_lower_limit:=${-pi}    shoulder_pan_upper_limit:=${pi}
    shoulder_lift_lower_limit:=${-pi}    shoulder_lift_upper_limit:=${pi}
    elbow_joint_lower_limit:=${-pi}    elbow_joint_upper_limit:=${pi}
    wrist_1_lower_limit:=${-pi}    wrist_1_upper_limit:=${pi}
    wrist_2_lower_limit:=${-pi}    wrist_2_upper_limit:=${pi}
    wrist_3_lower_limit:=${-pi}    wrist_3_upper_limit:=${pi}
    transmission_hw_interface:=hardware_interface/PositionJointInterface
    safety_limits:=false safety_pos_margin:=0.15
    safety_k_position:=20">
    ……

• ur.transmission.xacro 在仿真中模拟关节电机
• ur.gazebo.xacro 使能自碰撞检测
• 定义了圆柱体惯性矩阵的计算公式，后面各个轴的惯性矩阵直接将其参数代入该公式即可。
• 定义了ur5_robot电机旋转限制
• 省略部分还定义了一些宏定义和运动学参数相关
• 最后就是构建关节和轴之间的连接特性并且导入模型，dae和stl后缀文件可以用SolidWorks打开和绘制导出。

1.1.3 告诉Gazebo从上个步骤上传到参数服务器的robot_description读取urdf模型文件。

  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />

1.1.4 引入controller_utils.launch文件，启动robot_state_publisher和joint_state_controller节点

robot_state_publisher从/joint_states话题中获取机器人joint角度作为输入, 使用机器人的运动学树模型计算出机器人link的3D姿态, 然后将其发布到话题/tf和 /tf_static. joint_state_publisher从ROS参数服务器中读取robot_description参数, 找到所有non-fixed joint, 发布他们的JointState消息到/joint_states话题.
rosparam load load parameters from file 从文件读取参数
把关节控制的配置信息读取到参数服务器

<launch>

  
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" type="string" value="" />
  node>

  
  <node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
        args="pub /calibrated std_msgs/Bool true" />
  
  
  <rosparam file="$(find ur_gazebo)/controller/joint_state_controller.yaml" command="load"/>
  <node name="joint_state_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>
  
launch>

上传数据到参数服务器有两种写法，
一种是前面提到的

<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_joint_limited_robot.urdf.xacro' transmission_hw_interface:=$(arg transmission_hw_interface)" />

另一种是加载yaml
最后发布joint_state_controller节点。

<rosparam file="$(find ur_gazebo)/controller/joint_state_controller.yaml" command="load"/>

controller_manager参考 ROS机械臂开发：Moveit + Gazebo仿真/Gazebo配置
功能和joint_state_publisher这个节点是一样的，只是数据输入不一样，输出都是话题形式
应该是等同于

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

1.1.5 启动joint_position_controller（arm_controller）节点
controller概念详见：
http://wiki.ros.org/controller_manager

1.2 roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true

rviz相关初始化。打开move_group.launch

<launch>
  <arg name="sim" default="false" />
  <arg name="limited" default="false"/>
  <arg name="debug" default="false" />
    
  
  <remap if="$(arg sim)" from="/follow_joint_trajectory" to="/arm_controller/follow_joint_trajectory"/>
  
  
  <include file="$(find ur5_moveit_config)/launch/move_group.launch">
    <arg name="limited" default="$(arg limited)"/>
    <arg name="debug" default="$(arg debug)" />
  include>
launch>

又打开了几个文件
1.2.1 planning_context.launch

<launch>
  
  <arg name="load_robot_description" default="false"/>
  <arg name="limited" default="false"/>

  
  <arg name="robot_description" default="robot_description"/>

  
  <group if="$(arg load_robot_description)">
    <param unless="$(arg limited)" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_robot.urdf.xacro'" />
    <param if="$(arg limited)" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_joint_limited_robot.urdf.xacro'" />
  group>

  
  <param name="$(arg robot_description)_semantic" textfile="$(find ur5_moveit_config)/config/ur5.srdf" />
  
  
  <group ns="$(arg robot_description)_planning">
    <rosparam command="load" file="$(find ur5_moveit_config)/config/joint_limits.yaml"/>
  group>

  
  <group ns="$(arg robot_description)_kinematics">
    <rosparam command="load" file="$(find ur5_moveit_config)/config/kinematics.yaml"/>
  group>
  
launch>

• 导入ur机械臂模型ur5_robot.urdf.xacro
• ur5.srdf
• 设置关节速度、加速度等限制joint_limits.yaml
• kinematics.yaml

1.2.2 planning_pipeline.launch.xml

<launch>

    

  <arg name="pipeline" default="ompl" />

  <include file="$(find ur5_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />

launch>

打开了ompl_planning_pipeline.launch.xml，使用ompl规划库。
1.2.3 trajectory_execution.launch.xml
打开了ur5_moveit_controller_manager.launch.xml，又打开了controllers.yaml

controller_list:
  - name: ""
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

节点名称为follow_joint_trajectory，联系到上文move_group.launch的
如果我们使能sim，那么就会讲上图发布的/follow_joint_trajectory映射到/arm_controller/follow_joint_trajectory。
move_group并不发布机器人的关节信息，这必须在机器人上实现。move_group只监听tf，tf信息由机器人发布，需要在机器人上运行robot_state_publisher节点。
1.2.4 sensor_manager.launch.xml 初始化传感器控制器
1.2.5 启用move_group这个action

  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">

1.3 roslaunch ur3_moveit_config moveit_rviz.launch config:=true

打开moveit.rviz。