在 ROS 中添加自定义 URDF 机械臂
所属知识点：Robotics: ROS，URDF，MoveIt
代码地址：cougarbot_urdf_moveit 视频演示：
归纳和总结机器人技术的库：ViolinLee/roboticsCV。该库挑选和收集各类与机器人技术相关的高质量资源，欢迎大家关注和Star！会一直更新下去。
内容：
从零开始制作一个全新的使用 URDF 定义的机械臂，并为其配置用于路径规划的 Moveit 软件包。原内容来自中文翻译版书籍《ROS 机器人编程实践》第18章：添加你自己的机械臂，非常适合作为入门 ROS 机械臂的教程。为了描述准确和容易理解，我翻阅了英文原版，修改了一些术语和表达，同时增加了一些图来解释。
此次仿真试验是在 Ubuntu 16.04 虚拟机上运行的，MoveIt 版本比原著中使用的版本要高，新版本 MoveIt 中 moveit_setup_assistant 增加了一些更方便的选项，例如控制器的配置也可以使用该配置助手完成。但为了更深入地认识 MoveIt 中的控制器，我并没有使用这些新增加的特性，而是尽可能地根据书本上的步骤来做。
制作自定义 URDF 机械臂的步骤与小龟机器人差不多，大致如下：
1.确定 ROS 消息接口；
2.确定机器人电动机的驱动；
3.编写机器人物理结构模型；
4.为步骤3中编写的模型增加物理特性，用于在Gazebo中进行仿真；
5.增加 tf 对外发布坐标变换数据，并使用 rviz 进行可视化（这一步中，机器人只是显示在 rviz 中，没有反馈状态，也不能被控制）；
6.增加传感器，注意要带有驱动和仿真的支持；
7.添加一些标准的机器人算法，如路径规划等。
移动机器人使用 cmd_vel/odom 标准的 ROS 接口，这样就可以同时发送速度指令，更新里程计数据。对机械臂而言，类似功能的消息接口如下：
control_msgs/FollowJointTrajectory ( follow-joint-trajectroy 动作 )
指定机械臂的运动轨迹，并监控其运动过程sensor_msgs/JointState ( joint_state 话题 )
发布机械臂上每个关节的当前状态通过 follow-joint-trajectory/joint-state ROS 接口，可以灵活地控制和监控机械臂的关节。先来看看可以向 follow-joint-trajectroy 动作发送哪些全局消息：
容易发现，control_msgs/FollowJointTrajectoryGoal 是一个复合消息类型：大方向看包含：
-
一个 trajectroy_msgs/JointTrajectroy 消息
-
两个 control_msgs/JointTolerance[] 消息
-
一个 duration goal_time_tolerance 消息
robond@udacity:~/catkin_ws$ rosmsg show control_msgs/FollowJointTrajectoryGoal
trajectory_msgs/JointTrajectory trajectory
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start
control_msgs/JointTolerance[] path_tolerance
string name
float64 position
float64 velocity
float64 acceleration
control_msgs/JointTolerance[] goal_tolerance
string name
float64 position
float64 velocity
float64 acceleration
duration goal_time_tolerance看起来相当多！大致来说，我们需要将位置、速度、加速度、力度（前面四个是数组）、目标、时间参数和公差组合成一条轨迹，从而控制机械臂，这很麻烦。但后面会讲到一种方法，可以在不填写任何其他字段的情况下，构造出一条简单的轨迹！
再看看 joint_states:
robond@udacity:~/catkin_ws$ rosmsg show sensor_msgs/JointState
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort这个消息很直观，只报告所有关节（所以必须是数组）的当前位置、速度和力度。
关于硬件驱动的说明
为了将 follow_joint_trajectory/joint_states 接口应用在实体机器人上，我们需要编写一个与机器人硬件通信的节点。具体的实现细节取决于硬件本身和采用的通信方式。与移动机器人类似，机械臂通常会对外提供一些物理接口，一般是串行或网络接口，还会有配套的数据通信协议。理想情况下，或许能找到一个实现协议的可重用库，减少不必要的工作量。
虽然这里不能提供通用的机械臂驱动代码，但是 ROS 中已有许多样例，可供编写参考。为方便起见，我们假定已经写好了一个支持 follow_joint_trajectory/joint_states 接口的驱动节点，然后继续讲解 ROS 集成的剩余步骤。（接下来的任务就是对机器人进行建模，并将模型用于仿真，而不使用硬件或驱动）。
对机器人建模： 使用URDF
URDF模型文件作用：
-
可以被 rviz 用来对机器人的状态进行可视化；
-
可以用作 Gazebo 仿真的参考（包括机器人模型、Gazebo插件）；
-
可能被 Moveit 拿来做运动规划（配置 MoveIt Configuration Package 时需要 URDF 模型文件）。
从模型的运动学部分开始，机械臂的一些关键特性如下：
- 机械臂底座（base）被刚性连接在工作平面上。
- 底座之上的是相对旋转“torso”旋转的“hip”关节。
- 接下来的三个关节分别为“shoulder”“elbow”“wrist”。机械臂的“upper arm”“lower arm”和“hand”分别可以相对这三个关节上下摆动。
总的来说，机械臂模型需要5个杆件（base、torso、upper arm、lower arm、hand），彼此之间通过4个关节相连（hip、shoulder、elbow、wrist）。所以说，这是一个四轴机械臂。为简便起见，模型中的连接一律使用圆柱体。不过，使用更精确的模型可以提高仿真准确度和视觉真实性！
建模从底盘开始。由于底盘固定在工作平面上，我们需要额外创建一个名为 world 的特殊连接，并将圆柱形的底盘用一个 fixed 类型的关节与之连接。具体的URDF代码：
说明：
1.包含的重要标签：
2.为额外创建的特殊连接。
3.注意
我们可以使用 rosluanch urdf_tutorial display.launch 来进行可视化
roslaunch urdf_tutorial dispaly.launch model:=cougarbot.urdf让我们看一下 display.launch 的代码：
display.launch 启用了三个 ROS 节点：
-
joint_state_publisher：为 URDF（机器人）设置和发布 sensor_msgs/JointState 消息。可与 robot_state_publisher 节点一起使用，发布所有关节状态的 transform。（设置以更新和可视化，发布以计算正运动学）
-
robot_state_publisher：使用参数 robot_description 指定的 urdf 和 joint_states 话题的关节位置来计算机器人的正向运动学，并通过 tf 发布结果。
-
rviz
运行后，可以在 RVIZ 中看到下图所示模型（注意 world 和 base_link 坐标重叠）。
添加 torso 杆件和 hip关节，下面是代码段：
注意：我们在 URDF 中设置的坐标轴都是初始位置，后面改动机器人动作改变的时候，这些坐标轴位置和姿态都会发生改变，这时候就不用我们管了，因为我们定义玩 URDF 后，它就是一个完整的机器人了。后续我们只要使用 robot_state_publisher ，就会根据关节状态（joint_state）自动计算正运动学，从而知道各坐标的位置和姿态。
以同样的方式进行可视化：
注意：hip 是一个 continuous 类型关节，子杆件 torso 可以相对父杆件 base_link 绕 z（blue axis）任意旋转。可以使用 joint state publisher GUI 来设置该 URDF。
接来是 upper arm 和 lower arm。杆件 upper arm 通过关节 shoulder 连接在 torso 的右侧（外侧）；杆件 lower arm 通过 elbow 连接在 upper arm 的左侧（内测）。以下是代码段：
添加 upper arm 和 shoulder：
添加 lower arm 和 elbow：
以同样的方式进行可视化：
注意，我们没有必要将这里的坐标轴规范到 DH 参数的坐标轴定义方式，程序内部的运动学计算已经考虑了这一点。但是，我们在定义 joint 的时候必须指定旋转轴！
接下来是最后一个运动学元素 hand，hand 通过 wrist 连接在 lower arm 的末端。我们盒子来表示。以下是代码段：
同样进行可视化：
到目前为止，机械臂的整体结构已初见雏形。下面我开始对其进行仿真（前面的步骤只完成在 RVIZ 中可视化，还没到仿真的地步！）。
在 Gazebo 中进行仿真
上一节，我们创建了机械臂的运动学模型，包括部件的外形尺寸（visual 标签）、相对位置（joint 标签），这些信息对于在 RVIZ 中可视化已经足够，但如果要仿真，还要补充每个连接的碰撞几何（geometry 标签）和惯性特性（inertial 标签）。
碰撞几何部分的代码可以直接由已有的外形尺寸代码复制粘贴（注意删除 matierial 标签部分）。惯性特性需要测量每个杆件的质量和转动惯量（为了简便起见，每个杆件的质量设置为 1Kg，转动惯量使用圆柱体和立方体对应公式计算）。
以下是 world 和 baselink 杆件部分的代码段。相比之前，多了仿真使用的
注意：对于任何在
现在可以把所有代码打包成一个 ROS package 了，名为 cougarbot （推荐通过 catkin-create-pkg）：
catkin_create_pkg cougarbot rospy然后是一个 launch 文件，用来自动打开 Gazebo。让我来看看 cougarbot.launch 的代码：
此时，运行 cougarbot.launch 后显示如图所示：
机械臂是出现了，但样子很古怪——杆件散落在地上。原因在于我们在 Gazebo 中对机械臂进行仿真时，没有施加任何控制，即每个关节没有施加力矩。因此在重力的作用下，机械臂自然就会像破玩偶一样耸拉下来，但这完全符合机器人的运动学和动力学模型。
回忆移动机器人部分：通过（在 Gazebo 中）使用差分驱动插件（plugin），我们可以经由 cmd_vel/odom 接口对移动机器人进行差分运动控制。而针对该本次的 cougarbot 机器人，我们需要的是能够操作 follow_joint_trajectory/joint_states 接口的插件，我们需要用到两个插件：
-
从 follow_joint_trajectory 接受轨迹并产生控制的 ros_control
-
对外发布 joint_states 数据的 ros_joint_state_publisher
注意：事实上仅有 ros_control 就能够保证机器人以正常状态站立，而且能够使用命令行进行关节角的简单控制。但是仅有这些还是不足以配置 MoveIt 进行路径规划的。路径规划器需要机器人的关节角信息（或用关节角信息进行正运动学计算出的连杆坐标信息），而这部分就需要节点 joint_state_publisher 和节点 robot_state_publisher。前者通常作为 Gazebo 插件在 URDF 中配置；后者则直接在 launch 中定义。
先看 ros_control 插件。添加这个插件的过程稍显繁琐。主要原因是，我们需要让用于仿真的控制代码能同时工作在硬件实体上！因此需要对控制器和其他基础组件进行很多额外的抽象和配置，这都会给插件的添加过程增加一定的复杂度。
添加 ros_control 插件 （Gazebo 插件在 URDF 中配置！）
首先，对于 URDF 模型中的每个关节，都需要定义相应的 transmission。对于一个关节而言，假设其上安装了一个电动机，则 transmission 描述了该关节的运动与电动机转动输出之间的关系。transmission 通常会包含一个减速比（代表了电动机齿轮箱降低的速度，具有增大电动机扭矩的作用，因为电动机通常是一个速度高转矩小的设备）。除此之外，transmission还可能包含其他更复杂的内容，如关节之间的机械耦合等。关于 transmission 的更多细节，参阅相关文档（http://wiki.ros.org/urdf/XML/Transmission?distro）。
以下是为 hip 关节定义的一个 SimpleTransmission 的代码段，主要标签为
transmission_interface/SimpleTransmission
PositionJointInterface
PositionJointInterface
1
上述代码定义了一个减速比为 1 的空的 transmission。这是不现实的，但对于仿真来说却是足够的。同样，可以对其他三个关节定义类似的 transmission。
接着，加载 ros_control 插件：
将上述这段代码插入到 cougarbot.urdf 中（Gazebo 插件在 URDF 中配置）。
注意：至此，再次运行 cougarbot.launch 机械臂杆件已经不会再掉落了，但是我们还没有选择和配置可用于路径规划的控制器类型。
接下来，要从 ros_control 提供的控制器中选择一个进行配置。考虑到我们的目的，我们需要接受由关节位置组成的轨迹（而不是速度，加速度或其他目标、约束）。在 cougarbot 中创建一个名为 controllers.yaml 的新文件，并添加一下代码：
arm_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- hip
- shoulder
- elbow
- wrist该文件定义了一个名为 arm_controller、类型为 position_controllers/JointTrajectoryController 的新控制器（参数），用来控制机械臂的关节（这里的关节名称是 URDF 中定义的）。该配置文件可以通过 rosparam 加载到 ROS 参数服务器中，以便其他工具访问。
上述代码需要添加到 cougarbot.launch 中。
现在需要实例化新配置的控制器。由于 ros_control 启动时默认不运行任何控制器，因此我们需要使用 controller_manager/spawer 工具来实例化新创建的 arm_controller。相应的 XML 代码如下：
上述代码同样添加到 cougarbot.launch中。至此我们就能够启动仿真了，这次结果如下图所示：
现在机器人不再散落在地上，因为控制器默认会将各个关节保持在 0 位置！现在可以向 follow_joint_trajectory 接口发送指令，来控制机械臂向某个方向运动。先看看当前生效的 topic：
~$ rostopic list
/arm_controller/command
/arm_controller/follow_joint_trajectory/cancel
/arm_controller/follow_joint_trajectory/feedback
/arm_controller/follow_joint_trajectory/goal
/arm_controller/follow_joint_trajectory/result
/arm_controller/follow_joint_trajectory/status
/arm_controller/state
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/rosout
/rosout_agg可以看到，/arm_controller 名称空间下包含了许多有意思的话题。进一步看，该名称空间下 follow_joint_trajectory 这一次级名称所包含的话题直接组成了一个常用于控制器的动作接口。不过，鉴于 /arm_controller 下还提供了 command 这个话题，可以直接向其发送控制指令。我们先看看 command 话题的细节：
~$ rostopic info /arm_controller/command
Type: trajectory_msgs/JointTrajectory
Publishers: None
Subscribers:
* /gazebo (http://192.168.**.***:*****/)注意 command 的消息类型：trajectory_msgs/JointTrajectory。在一开始，我们查看 follow_joint_trajectory 动作能接受的控制目标时曾见过它。因此，只要构造并发布该类型的消息即可控制机械臂运动。要构造该消息，要提供的信息最少也需要包括待控制的关节名字列表，以及一条至少由一个点组成的轨迹。对轨迹上的每个点，还要包含到达该点时的时间（从执行运动轨迹的时刻开始计时）和所有关节的位置。这些数据还不算太多，我们可以用 rostopic 发布。一下命令让每个关节移动到一个新的角度，并要求所有的移动在1秒内完成：
~$ rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory \
'{joint_names: ["hip", "shouler", "elbow", "wrist"], points: [{positions: [0.1, -0.5, 0.5, 0.75], time_from_start: [1.0 0.0]}]}' -1运动效果如下图所示。我们看到机械臂平滑地移动到了一个新的位置。在这个过程中，我们仅指定了机械臂的最终目标位置，而把中间地路径规划交给控制器去完成：
但是，使用命令进行控制机械臂并不是一个好办法。我们还需要对机械臂进行改善，直至它可以自行完成规划路径的任务。
验证坐标变换信息
在上面，我们使用 ros_control 插件对外提供了 follow_joint_trajectory 接口，进而实现对机械臂的控制。而对于 joint_state 接口，则需要用到 ros_joint_state_publisher 插件。该插件会提供 joint_state 接口，并向这个接口发布机械臂的当前状态。
添加 ros_joint_state_publisher 插件要相对简单一点，只需告诉它该汇报哪些关节的状态即可。在 cougarbot 上，就是 hip、shouler、elbow 和 wrist 这四个关节。代码如下：
hip, shoulder, elbow, wrist
将上述这段代码添加到 cougarbot.urdf 中。
现在可以启动 cougarbot.urdf，然后用 rostopic 检查一下 joint_states 话题的数据，看看插件是否已经添加正确添加。若工作正常，就能看到反应各个关节位置信息的数据流：
~$ rostopic echo /joint_states
...
header:
seq: 4069
stamp:
secs: 41
nsec: 40000000
frame_id: ' '
name: [hip, shoulder, elbow, wrist]
position: [0.00017596620518745, -1.439691312743463e-05, -5.112913754867776e-05, 1.1855589603371186e-06]
velocity: [-2.8469657251642077e-10, 0.0, 0.0, 0.0]
effort: [ ]在控制器的作用下，关节的位置会非常接近0，同时还会不停地 0 附近波动，就像真实地机械臂在不断地对抗重力，保持位置不变一样。
接下来需要添加的是 robot_state_publisher，它会拿 joint_states 消息和机器人模型做前向运动学解算，产生 tf 消息，并发布出去。下面的代码用来启动 robot_state_publisher，将其插入到 cougarbot.launch 即可：
重新启动 cougarbot.launch，打开 rviz，选择 base_link 为 "fixed frame"，并在 Displays 栏添加 RobotModel 和 TF。这样就能看到带 TF 帧的机器人了，效果如下：
还可以用 rqt_plot 将 joint-states 的数据绘制出来：
~$ rqt_plot '/joint_states/position[0]' '/joint_states/position[1]' '/joint_states/position[2]' '/joint_states/position[3]'你将会看到一张由四个关节位置组成的实时数据图，而且每个关节的位置都很接近0。
再次向 command 话题发送之前的简单轨迹，让机械臂动起来：机械臂顺利地移动到了新位置，于此同时，rqt_plot 窗口中地位置数据也从 0 变化为各自的新角度，如图所示。
配置 Moveit
Moveit 是一个用来做动作规划和控制的工具库。虽然在设计理念上与导航程序栈很像，但是要更加复杂，拥有更丰富的配置选项。为了方便进行配置，Moveit 提供了一个叫做 Setup Assistant 的图形工具。让我们来打开它：
~$ roslaunch moveit_setup_assistant setup_assistant.launch正常情况下会弹出一个如下图所示的引导界面（注意：不同版本的 MoveIt 界面和选项可能存在差异）。
单击”Create New Moveit Configuration Package“按钮，在文件选择栏找到 cougarbot.urdf，然后单击”Load Files“按钮。然后就能在右侧看到 cougarbot 模型了。
接下来就可以逐一对 Setup Assistant 窗口左侧的配置项进行配置了：
Self-Collisions
单击”Generate Collision Matrix“按钮，Moveit 会自动对机器人模型进行检查，并随机采样可能的碰撞组合，从而帮助决定哪些碰撞检测。这样做的意义在于减少不必要的碰撞检测，从而提高 Moveit 规划的效率。
Visual Joints 不做任何修改
Planning Groups
我们需要创建一个包含整个机械臂的 planning group。单击”Add Group“按钮，在”Group Name“一项中填入”arm“（可任取），在”Kinematic Solver“中选择”kdl_kinematics_plugin/KDLKinematicsPlugin“（该插件提供了通用的逆运动学求解器，虽效率不是最高，但已够用）。然后单击”Add Joints“，选中所有的 5 个关节，最后”save“。
Robot Poses 不做任何修改
End Effectors
对于 cargoubot，与 hand 连杆的物体才应该是末端执行器（实际上此时的 cargoubot 并没有安装末端执行器），所以我们在”Parent Link“中选择”hand“，然后”save“。
Passive Joints 不做任何修改
Configuration Files
我们需要告诉 Moveit 该在哪个路径下创建一个包含新配置文件的 ROS 包。在”Configuration Package Save Path“中填入稍后要自动创建的 cougarbot_moveit_config 文件夹路径，单击”Generate Package“创建该 ROS 包。
完成上述各项配置后，单击”Exit Setup Assistant“退出配置工具。经过上面对 Moveit 的配置，得到了一个名为 cougarbot_moveit_config 的 ROS 包，其中包含许多 launch 文件和 YAML 文件。可以查阅 Moveit 的文档来深入了解它们。
注意：较新版本的 Moveit 存在更多配置选项，如上面的 3D Perception、Simulation、ROS Control 在旧版本中是没有的。对于此次的 cougarbot ，我们先遵从旧版本的配置操作——手动配置控制器信息，而不是依靠 Assistant 的 ROS Control 自动配置。
配置控制器信息，我们需要告诉 MoveIt 机械臂控制器的配置情况。在 cougarbot_moveit_config 包中，创建一个名为 config/controller.yaml 的文件（里面的参数供 moveit_congroller_manager），并插入以下代码：
controller_manager_ns: /
controller_list:
- name: arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- hip
- shoulder
- elbow
- wrist在这个配置文件中，我们将 ros_control 插件（Gazebo中的）提供的 follow_joint_trajectory 动作服务器配置为 Moveit 的控制器接口，同时将 MoveIt 的控制对象设置为 hip、shoulder、elbow、wrist 这四个关节。除此之外，还要修改 cougarbot_moveit_config 中的另一个文件：打开 launch/cougarbot_moveit_controller_manager.launch.xml （初始为空），插入以下代码：
这个文件设置了很多参数，其中就包括将刚才创建的 controller.yaml 加载到参数服务器中。到此为止，MoveIt 的配置全部完成。接下来将使用它。
使用 RVIZ 控制机械臂
启动对 cargoubot 机械臂的仿真：
~$ roslaunch cougarbot cougarbot.launch于此同时，使用刚才创建的配置文件启动 MoveIt：
~$ roslaunch cougarbot_moveit_config move_group.launch现在 cargoubot 处于仿真状态，MoveIt 也准备好接受目标位姿。接下来使用 MoveIt 提供的配置文件打开 RVIZ (当然也可以直接打开 RVIZ 再自行设置)：
~$ roslaunch cougarbot_moveit_config moveit_rviz.launch config:=True以上三个步骤可以合在一个 launch 文件里，创建一个 all.launch 文件：
运用 RVIZ Motion Planning 控制机械臂的过程见视频演示。
注意：由于 cougarbot 的腕（wrist）关节是单自由度的，因而很难在 RVIZ 中以交互式的方法指定精确的目标位姿（欠自由度机械臂的特性）。也正是这个原因，机械臂的腕关节一般会有 2 或 3 个自由度。
最后总结：cougarbot 还远没有完成。首先，没有添加任何传感器。至此看来路径规划还不错，但是在存在障碍物的情况下，使用传感器来避障就十分必要了。MoveIt支持障碍物感知下的路径规划，我们只需要向 cougarbot 模型中添加一个传感器（如类似 Kinect 的深度相机），然后在 MoveIt 的配置中订阅该传感器的数据流即可。具体细节参见 MoveIt 文档。