本文在Ubuntu18.04 + ROS melodic环境下完成，其他ROS版本类似。
项目github链接：链接: link
一、项目介绍
1、项目内容
机械臂是机器人中运用最广泛、成熟的一种，主要应用于工厂自动化环境。机械臂经过几十年的发展，技术相对成熟，包括运动学正逆解、运动轨迹规划、碰撞检测算法等。随着协作机器人的发展，机械臂也逐渐开始走入人们的生活。ROS提供了不少针对机械臂的功能包，这些功能包集成为一个单独的ROS软件——MoveIt！。
MoveIt！为开发者提供了一个易于使用的集成化开发平台，由一系列移动操作的功能包组成，包含运动规划、操作控制、3D感知、运动学、控制与导航算法等，且提供友好的GUI，可以广泛应用于工业、商业、研发和其他领域。
MoveIt！已经支持几十种常用机器人了，也可以非常灵活地应用到自己的机器人上。详情请查看：链接: link
2、预期目标
学会使用MoveIt！实现机械臂控制的四个步骤：
组装：在控制之前需要有机器人，可以是真实的机械臂，也可以是仿真的机械臂，但都要创建完整的机器人urdf模型。
配置：使用MoveIt！控制机械臂之前，需要根据机器人urdf模型，使用Setup Assistant工具完成自碰撞矩阵、规划组、终端夹具等配置，配置完成后生成一个ROS功能包。
驱动：使用ArbotiX或者ros_control功能包中的控制器插件，实现对机械臂关节的驱动。插件的使用方法一般分为两步：首先创建插件的YAML配置文件；然后通过launch文件启动插件并加载配置参数。
控制：MoveIt！提供了C++、Python、rviz插件等接口，可以实现机器人关节空间和工作空间下的运动规划，规划过程中会综合考虑场景信息，并实现自主避障的优化控制。
二、MoveIt！系统架构
在开始使用MoveIt！之前，我们先来了解一下其主要组成部分。
1、运动组（move_group）
move_group是MoveIt！的核心节点，可以综合其他独立的功能组件为用户提供ROS中的动作指令和服务。
move_group本身并不具备丰富的功能，主要完成各功能包、插件的集成。它通过消息或服务的方式接受机器人发布的点云信息、关节消息状态，以及机器人的TF坐标变换；另外，还需要ROS参数服务器提供机器人的运动学参数，这些参数可根据机器人的urdf模型生成（srdf和配置文件）。
（1）用户接口
MoveIt！提供三种可供调用的接口：
C++：使用move_group_interface包提供的API。
Python：使用moveit_commander包提供的API。
GUI：使用MoveIt！的rviz插件。
（2）ROS参数服务器
ROS参数服务器需要为move_group提供三种信息：
urdf：从ROS参数服务器中查找robot_description参数，获取机器人模型的描述信息。
srdf：从ROS参数服务器中查找robot_description_semantic参数，获取机器人模型的一些配置信息，这些配置信息通过配置工具MoveIt！Setup Assistant生成。
config：机器人的其他配置信息，例如关节限位、运动学插件、运动规划插件等。
（3）机器人
move_group和机器人之间通过Topic和Action通信，机器人传感器和机器人状态发布者将机器人的关节信息和坐标变换关系发送给move_group。如果需要加入机器人外部感知能力，还需要通过机器人3D传感器发布点云数据。另外还有一个很重要的模块——机器人控制器，通过Action的形式接收move_group的规划结果，并且将执行情况反馈给move_group。
move_group结构很容易通过插件的形式进行扩展，已有的功能模块也是通过插件的形式集成在MoveIt！中。
备注：action是ROS下节点间数据通信的一种方式，这种方式本质也是基于ROS topic的。action打包了同一行为下的多个消息，使用action可以将你从繁琐的消息订阅与发布编程中解放出来，并且使节点间的信息交互能力大大提升。
2、运动规划器（motion_planner）
假设已经知道机器人的初始姿态和目标姿态，以及机器人和环境的模型参数，那么可以通过某种算法，在躲避环境障碍物和防止自身碰撞的同时，找到一条到达目标姿态的较优路径，这种算法成为机器人运动规划。机器人和环境的模型静态参数由urdf文件提供，在某些场景下还需要加入3D摄像头、激光雷达来动态检测环境变化，避免与障碍物发生碰撞。
在MoveIt！中，运动规划算法由运动规划器完成，每一个运动规划器都是MoveIt！中的一个插件，开发者可以根据需求选用不同的规划算法，move_group默认使用的是OMPL。
运动规划器的结构如下图所示，首先我们向运动规划器发送一个运动规划请求（比如一个期望的终端姿态），同时我们可以添加以下约束：
位置约束：限制link的运动区域。
方向约束：限制link的运动方向（roll、pitch、yaw）。
可见性约束：限制link上某点在某区域内的可见性（通过视觉传感器）。
joint约束：限制joint的运动范围。
用户自定义约束：用户通过回调函数自定义所需的约束条件。
根据这些约束条件和用户的规划请求，运动规划器通过算法计算得到一条合适的运动轨迹，并发送给机器人的控制器。
运动规划器两侧分别有一个适配器接口（planning request adapters），主要功能是预处理运动规划请求和响应的数据，使之满足规划和使用要求，适配器种类不止一种，如下：
FixStartStateBounds：如果一个joint的状态超出极限，则adapter可以修复joint的初始极限。
FixWorkspaceBounds：设置一个默认尺寸的工作空间。
FixStartStateCollision：如果已有的joint配置文件会导致碰撞，则adapter可以采样新的碰撞配置文件，并且根据jiggle_factor因子修改已有的配置文件。
FixStartStatePathConstraints：如果机器人的初始姿态不满足路径约束，则adapter可以找到附近满足约束的姿态作为机器人的初始姿态。
AddTimeParameterization：运动规划器计算得到的轨迹只是一条空间路径，这个adapter可以为这条空间轨迹进行速度、加速度约束，为每个轨迹点加入速度、加速度、时间等参数。
3、规划场景
规划场景可以为机器人创建一个工作环境，包括外界环境中的桌面、工件等物体，这一主要功能主要由move_group节点中的规划场景监听器（Planning Scene Monitor）实现。
该插件会监听以下几个方面的信息：
状态信息（State Information）：机器人的关节话题joint_states。
传感器信息（Sensor Information）：机器人的传感器数据。
外界环境信息（World Geometry Information）：通过传感器建立的周围环境信息。
4、运动学求解器
运动学算法是机械臂各种算法中的核心，包含正向运动学（Forward Kinematics，FK）和逆向运动学（Inverse Kinematics，IK）等算法。MoveIt！中的运动学插件允许开发者灵活选择多种可供使用的运动学求解器，默认的运动学求解器是从Orocos项目中移植过来的KDL，可以在MoveIt！Setup Assistant工具中进行配置，也可以选择自己的运动学求解器。
5、碰撞检测
MoveIt！使用Collision World对象进行碰撞检测（Collision Checking），采用（Flexible Collision Library）功能包实现。碰撞检测是运动规划中最耗时的运算之一，往往占用90%左右的时间，为了减少计算，可以在MoveIt！Setup Assistant工具中设置免检冲突矩阵（Allowed Collision Matrix，ACM）进行优化，如果两个刚体之间的ACM设置为1，则意味着两个刚体永远不会发生碰撞，即不需要碰撞检测。
三、如何使用MoveIt！
了解系统架构之后，我们对MoveIt！有了大致的了解，接下来需要在实践中加深对MoveIt！的理解。
MoveIt！实现机器人控制的方法总结为以下四个步骤。
组装：创建机器人urdf模型。
配置：使用MoveIt！Setup Assistant工具生成配置文件。
驱动：添加机器人控制器（真实机器人）或控制插件（仿真机器人）。
控制：使用MoveIt！控制机器人运动（仿真算法、物理仿真）。
接下来将以真实机器人为例，从零开始设计一个机器人模型，并且通过MoveIt！控制该仿真模型完成一系列功能。
四、创建机械臂模型
ROS提供了功能丰富的仿真环境，即使没有真实机器人，也可以在仿真环境中学习、研究、开发机械臂。因此，下面就来虚拟一个六轴机械臂——MArm。
首先编写机械臂的urdf文件，为后续MoveIt！和gazebo上的仿真控制做好准备。完整的urdf文件可以参见源码marm_descriptionn/urdf/arm.xacro，这里从三个方面讲解其中的关键内容。
1、声明模型文件中的宏
使用xacro文件描述机械臂的urdf模型，可以通过宏声明来提高模型的灵活性：
<!-- Defining the colors used in this robot -->
<material name="Black">
<color rgba="0 0 0 1"/>
</material>
<material name="White">
<color rgba="1 1 1 1"/>
</material>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
<material name="Red">
<color rgba="1 0 0 1"/>
</material>
<!-- Constants -->
<xacro:property name="M_PI" value="3.14159"/>
<!-- link1 properties -->
<xacro:property name="link1_width" value="0.03" />
<xacro:property name="link1_len" value="0.10" />
<!-- link2 properties -->
<xacro:property name="link2_width" value="0.03" />
<xacro:property name="link2_len" value="0.14" />
<!-- link3 properties -->
<xacro:property name="link3_width" value="0.03" />
<xacro:property name="link3_len" value="0.22" />
<!-- link4 properties -->
<xacro:property name="link4_width" value="0.025" />
<xacro:property name="link4_len" value="0.06" />
<!-- link5 properties -->
<xacro:property name="link5_width" value="0.03" />
<xacro:property name="link5_len" value="0.06" />
<!-- link6 properties -->
<xacro:property name="link6_width" value="0.04" />
<xacro:property name="link6_len" value="0.02" />
<!-- Left gripper -->
<xacro:property name="left_gripper_len" value="0.08" />
<xacro:property name="left_gripper_width" value="0.01" />
<xacro:property name="left_gripper_height" value="0.01" />
<!-- Right gripper -->
<xacro:property name="right_gripper_len" value="0.08" />
<xacro:property name="right_gripper_width" value="0.01" />
<xacro:property name="right_gripper_height" value="0.01" />
<!-- Gripper frame -->
<xacro:property name="grasp_frame_radius" value="0.001" />
<!-- Inertial matrix -->
<xacro:macro name="inertial_matrix" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>
以上是针对机械臂模型定义的一些宏，其中主要包含三个部分。
颜色宏：定义模型中需要使用的外观颜色，设置颜色的RGBA值。
机器人尺寸宏：通过宏属性定义机器人的三维尺寸，便于修改。
惯性矩阵宏：每个link都需要指定惯性矩阵，可以把该模块提取出来，输入质量参数即可。
2、创建六轴机械臂模型
接下来设计机械臂本体的模型。我们的设计目标是六轴机械臂，也就是说，机器人需要六个关节；终端装配一个两指夹爪，使用一个关节驱动。代码如下：
<!-- /// bottom_joint // -->
<joint name="bottom_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_link"/>
<child link="bottom_link"/>
</joint>
<link name="bottom_link">
<visual>
<origin xyz=" 0 0 -0.02" rpy="0 0 0"/>
<geometry>
<box size="1 1 0.02" />
</geometry>
<material name="Brown" />
</visual>
<collision>
<origin xyz=" 0 0 -0.02" rpy="0 0 0"/>
<geometry>
<box size="1 1 0.02" />
</geometry>
</collision>
<xacro:inertial_matrix mass="500"/>
</link>
<!-- / BASE LINK // -->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.04" />
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.04" />
</geometry>
</collision>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.02" rpy="0 ${M_PI/2} 0" />
<axis xyz="-1 0 0" />
<limit effort="300" velocity="1" lower="-2.96" upper="2.96"/>
<dynamics damping="50" friction="1"/>
</joint>
<!-- / LINK1 // -->
<link name="link1" >
<visual>
<origin xyz="-${link1_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link1_width}" length="${link1_len}"/>
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="-${link1_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link1_width}" length="${link1_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="-${link1_len} 0 0.0" rpy="-${M_PI/2} 0 ${M_PI/2}" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.35" upper="2.35" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- /// LINK2 // -->
<link name="link2" >
<visual>
<origin xyz="0 0 ${link2_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link2_width}" length="${link2_len}"/>
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0 0 ${link2_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link2_width}" length="${link2_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint3" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<origin xyz="0 0 ${link2_len}" rpy="0 ${M_PI} 0" />
<axis xyz="-1 0 0" />
<limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- / LINK3 / -->
<link name="link3" >
<visual>
<origin xyz="0 0 -${link3_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link3_width}" length="${link3_len}"/>
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="0 0 -${link3_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link3_width}" length="${link3_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint4" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="0.0 0.0 -${link3_len}" rpy="0 ${M_PI/2} ${M_PI}" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- /// LINK4 -->
<link name="link4" >
<visual>
<origin xyz="${link4_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link4_width}" length="${link4_len}"/>
</geometry>
<material name="Black" />
</visual>
<collision>
<origin xyz="${link4_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link4_width}" length="${link4_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint5" type="revolute">
<parent link="link4"/>
<child link="link5"/>
<origin xyz="${link4_len} 0.0 0.0" rpy="0 ${M_PI/2} 0" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- // LINK5 / -->
<link name="link5">
<visual>
<origin xyz="0 0 ${link4_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link5_width}" length="${link5_len}"/>
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0 0 ${link4_len/2} " rpy="0 0 0" />
<geometry>
<cylinder radius="${link5_width}" length="${link5_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint6" type="revolute">
<parent link="link5"/>
<child link="link6"/>
<origin xyz="0 0 ${link4_len}" rpy="${1.5*M_PI} -${M_PI/2} 0" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-6.28" upper="6.28" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- LINK6 / -->
<link name="link6">
<visual>
<origin xyz="${link6_len/2} 0 0 " rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link6_width}" length="${link6_len}"/>
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="${link6_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link6_width}" length="${link6_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="finger_joint1" type="prismatic">
<parent link="link6"/>
<child link="gripper_finger_link1"/>
<origin xyz="0.0 0 0" />
<axis xyz="0 1 0" />
<limit effort="100" lower="0" upper="0.06" velocity="1.0"/>
<dynamics damping="50" friction="1"/>
</joint>
<!-- // gripper // -->
<!-- LEFT GRIPPER AFT LINK -->
<link name="gripper_finger_link1">
<visual>
<origin xyz="0.04 -0.03 0"/>
<geometry>
<box size="${left_gripper_len} ${left_gripper_width} ${left_gripper_height}" />
</geometry>
<material name="White" />
</visual>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="finger_joint2" type="fixed">
<parent link="link6"/>
<child link="gripper_finger_link2"/>
<origin xyz="0.0 0 0" />
</joint>
<!-- RIGHT GRIPPER AFT LINK -->
<link name="gripper_finger_link2">
<visual>
<origin xyz="0.04 0.03 0"/>
<geometry>
<box size="${right_gripper_len} ${right_gripper_width} ${right_gripper_height}" />
</geometry>
<material name="White" />
</visual>
<xacro:inertial_matrix mass="1"/>
</link>
<!-- Grasping frame -->
<link name="grasping_frame"/>
<joint name="grasping_frame_joint" type="fixed">
<parent link="link6"/>
<child link="grasping_frame"/>
<origin xyz="0.08 0 0" rpy="0 0 0"/>
</joint>
这里主要包含机械臂六个连杆和两指夹爪相关的link和joint设计。
- 模型中设置一个名为“grasping_frame”的link，这个link没有包含任何内容，主要作用是创建一个抓取操作时的参考坐标系。
3、加入gazebo属性
加入gazebo属性的方法与前面移动机器人的相同，需要设置gazebo中的link颜色、每个关节的transmission属性以及ros_control插件，代码如下：
<!-- / Gazebo // -->
<gazebo reference="bottom_link">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="link1">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link2">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="link3">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link4">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="link5">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="link6">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="gripper_finger_link1">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="gripper_finger_link2">
<material>Gazebo/White</material>
</gazebo>
<!-- Transmissions for ROS Control -->
<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:transmission_block joint_name="joint1"/>
<xacro:transmission_block joint_name="joint2"/>
<xacro:transmission_block joint_name="joint3"/>
<xacro:transmission_block joint_name="joint4"/>
<xacro:transmission_block joint_name="joint5"/>
<xacro:transmission_block joint_name="joint6"/>
<xacro:transmission_block joint_name="finger_joint1"/>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/arm</robotNamespace>
</plugin>
</gazebo>
机械臂控制往往更加关注运动过程中的关节位置，一般情况下以位置控制为主，所以这里transmission所使用的接口是PositionJointInterface，相应的控制器ros_cntrol插件是libgazebo_ros_control.so。关于插件的具体配置在yaml配置文件中完成，后续内容会详细讲解。
4、显示机器人模型
首先安装MoveIt！相关依赖：
sudo apt-get install ros-melodic-moveit-*
有些依赖需要从github上下载：
$ git clone https://github.com/ros-interactive-manipulation/manipulation_msgs.git
$ git clone https://github.com/ros-interactive-manipulation/household_objects_database_msgs.git
解压到robot_marm功能包集合下，进而编译工作空间。
在设计过程中，我们将模型放置到rviz中可视化显示，从而验证设计的正确性。可以使用marm_description/launch/view_arm.launch启动rviz并且加载机械臂模型：
<launch>
<arg name="model" />
<!-- 加载机器人模型参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find marm_description)/urdf/arm.xacro" />
<!-- 设置GUI参数，显示关节控制插件 -->
<param name="use_gui" value="true"/>
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_description)/urdf.rviz" required="true" />
</launch>
运行以上launch文件，可以看到设计完成的机器人模型如下图所示：
roslaunch marm_description view_arm.launch
拖动joint控制插件中的滑动条，可以看到对应的机器人关节开始转动。
五、使用Setup Assistant配置机械臂
如果使用自己创建的机器人urdf模型，则使用MoveIt！的第一步就是用Setup Assistant工具完成一些配置工作。Setup Assistant会根据用户导入的机器人urdf模型生成srdf文件，从而创建一个MoveIt！配置的功能包，完成机器人的配置、可视化和仿真等工作。
首先，使用如下命令启动MoveIt Setup Assistant：
rosrun moveit_setup_assistant moveit_setup_assistant
运行成功后，会出现如下图所示的界面。界面左侧的列表就是接下来要配置的项目；点击其中的某项，主界面就会出现相应的配置选项。
1、加载机器人urdf模型
这里有两个选择，一个是新建配置功能包，另一个是使用已有的配置功能包。选择“新建”菜单，在下侧的模型加载窗口中设置模型文件路径为marm_description功能包下的urdf文件arm.xacro，并点击“Load Files”按钮完成模型加载。
模型加载后，可以在右侧的窗口中看到Marm机械臂的模型，如下图所示：
2、配置自碰撞矩阵
点击MoveIt Setup Assistant界面左侧的第二项“Self-Collision”，配置自碰撞矩阵。
MoveIt！允许我们设置一定数量的随机采样点，根据这些点生成碰撞参数，检测永远不会发生碰撞的link。可想而知，点过多会造成运算速度较慢，点过少会导致参数不完善等问题。默认的采样点数量是10 000个，按照这个默认值点击“Generate Collision Matrix”按钮，即可产生碰撞矩阵。
3、配置虚拟关节
虚拟关节主要用来描述机器人在world坐标系下的位置。如果机器人是移动的，则虚拟关节可以与移动基座关联，但这里设计的机械臂是固定不动的，所以无需设置虚拟关节。
4、创建规划组
这一步可以将机器人的多个组成部分（link，joint）集成到一个组中，运动规划器会针对一组link或joint完成规划任务。在配置过程中，还可以选择运动学解析器。这里创建两个组：一个组包含机械臂本体，一个组包含前端夹爪。
首先创建机械臂本体的arm组。点击“Add Group”按钮，按照下图所示进行配置。
Group Name：arm。
Kinematic Solver：kdl_kinematics_plugin/KDLKinematicsPlugin。
Kin. Search Resolution：0.005。
Kin. Search Timeout（sec）：0.05。
然后点击“Add Kin. Chain”按钮，设置运动学计算需要包含的link。点击界面中“Robot Links”旁的下三角，打开所有link；再选中需要的link，点击“Choose Selected”按钮就可以选择该link。如下图所示，这里将机械5的运动学计算所包含的关节设置如下。
Base Link：base_link。
Tip Link：grasping_frame。
接下来还要为机械臂的夹爪添加一个gripper组，运动学解析器不用选择，配置如下图所示。
Gruop Name：gripper。
Kinematic Solver：None。
Kin. Search Resolution：0.005。
Kin. Search Timeout（sec）：0.05。
再点击“Add Links”按钮，会出现如下图所示界面。在左侧选择gripper group需要包含的两个link，在右侧的机器人模型中可以看到选中的link变成了红色，点击向右的箭头将这两个link加入右侧的选中列表中，即可确认关联。
两个group都构建完后，主配置界面中的显示如下图所示。
5、定义机器人位姿
这一步可以设置一些自定义的位姿，比如机器人的初始姿态、指定姿态和位姿等。当然，这些位姿是开发者根据场景自定义的，不一定与机器人本身的位姿相同。这样做的好处是，当使用MoveIt！的API编程时，可以直接通过名称调用这些位姿。这里我们配置两个机器人的位姿。
点击“Add Pose”按钮，在出现的界面中设置第一位姿——home。首先在“Pose Name”输入框中输入位姿名称，再选择对应的规划组为arm。该位姿的机器人姿态是六轴角度都处于0位置，可以理解为是机器人的初始位姿。设置完成后，点击“Save”按钮保存。
然后设置第二个位姿——forward，设置流程与第一个位姿类似，不同的是需要拖动设置界面中的关节控制滑动条，将右侧显示的机器人模型控制到希望的位姿，如下图所示，然后保存该位姿为“forward”。
两个位姿设置完成后，可以看到主界面中的位姿列表如下图所示。
6、配置终端夹爪
机械臂在一些实用场景下会安装夹具等终端结构，可以在这一步中添加。MArm前端装配了一个两指夹爪，这里针对夹爪进行一些配置。
点击“Add End Effector”按钮，按照下图进行配置。
End Effector Name：robot_gripper。
End Effector Group：gripper。
Parent Link (usually part of arm)：grasping_frame。
Parent Group (optional)：可选项，不需要设置。
7、配置无用关节
机器人上的某些关节可能在规划、控制过程中使用不到，可以先声明出来，MArm没有类似joint，这一步不需要配置。
8、设置作者信息
这一步可以设置作者的信息，如下图所示，根据情况填写。
9、生成配置文件
最后一步按照之前的配置，自动生成配置功能包中的所有文件。
点击“Browse”按钮，选择一个存储配置功能包的路径。Setup Assistant会将所有配置文件打包生成一个ROS功能包，一般命名为“RobotName_moveit_config”，这里我们命名为“marm_moveit_config”。
点击“Generate Package”按钮，如果成功生成并保存配置文件，则可以看到“Configuration package generated successfully！”的消息提示。
到目前为止，Setup Assistant的使命终于完成，点击“Exit Setup Assistant”按钮即可退出界面。
六、启动MoveIt！
按照上节的配置步骤完成后，会生成一个marm_moveit_config功能包，它包含大部分MoveIt！启动所需的配置文件和启动文件，以及包含一个简单的演示demo，可以用来测试配置是否成功，使用以下命令即可运行：
roslaunch marm_moveit_congif demo.launch
这个界面在rviz的基础上加入MoveIt！插件，通过左下角的插件窗口可以配置MoveIt相关功能，控制机械臂完成运动规划。例如通过MoveIt！插件，可以控制机械臂完成拖动规划、随机目标规划、初始位姿更新、碰撞检测等功能。
1、拖动规划
拖动机械臂的前端，可以改变机械臂的姿态。然后在Planning标签页中点击“Plan and Execute”按钮，MoveIt！开始规划路径，并且控制机器人向目标位置移动。
2、随即目标规划
在Query工具栏的“Select Goal State”的下拉选项中选择“random valid”，MoveIt！会在机器人的工作范围内随机生成一个目标位姿，接着继续点击“Plan and Exectue”按钮，机器人会自动运动到随机产生的目标姿态。
3、初始位姿更新
很多情况下，机器人运动的初始姿态并不是当前显示的位姿，这时就可以使用“Select Start State”设置机器人的初始位姿。与“Select Goal State”的设置类似，在下拉菜单中选择“random valid”就可以随机生成一个初始位姿，也可以通过鼠标拖动机器人的末端选择一个初始位姿。
确定初始位姿后，还需要像拖动规划和随即规划一样，选择运动的目标姿态，然后点击“Plan”按钮，就可以看到MoveIt！规划出来的路径了。
点击“Execute”按钮，让机器人执行运动命令，但是，因为当前位姿与我们设置的初始位姿相差较大，所以机器人不会开始运动，而且会在终端看到如下图所示的报错。
如果直接点击“Plan and Execute”按钮，MoveIt！仍然会把当前实际的初始位姿作为起点开始规划并运动，如果希望从指定起点运动，则需要在“Select Start State”按钮中更新当前指定的起点作为初始状态。
4、碰撞检测
在MoveIt！框架中有一个planning scene模块，允许我们创建一个虚拟环境。在MotionPlanning插件的“Scene Objects”标签页左下角点击Box“➕”按钮，可以加载场景物体的dae模型文件。例如我们加载一个如下图所示的Box模型，可以通过周围的箭头控制模型的位置和姿态。
加入模型后，MoveIt！在运动规划时会进行碰撞检测。假如机器人的目标姿态和外部模型发生碰撞，进行运动规划时就会提示失败，界面中变成红色的link即为发生碰撞的部分，如下图所示。
七、配置文件
MoveIt！的Setup Assistant工具帮助我们创建了demo模式下所需的全部配置文件，这些配置文件放置在所生成功能包的config文件夹下。下面以配置MArm所生成marm_moveit_
config功能包为例，了解这些配置文件的具体内容。
1、srdf文件
srdf文件储存了Setup Assistant配置的机械臂参数、夹具参数、规划组、自定义位姿等。Marm机械臂的srdf文件在config文件夹下，命名为arm.srdf，该文件同样使用xml格式描述，其中的关键部分如下：
规划组
<group name="arm">
<chain base_link="base_link" tip_link="grasping_frame" />
</group>
<group name="gripper">
<link name="gripper_finger_link1" />
<link name="gripper_finger_link2" />
<joint name="finger_joint1" />
<joint name="finger_joint2" />
</group>
标签用于描述设置的规划组。Marm中包含arm和gripper两个group，可以看到两个规划组的内容与前面在Setup Assistant中的设置相同。
自定义位姿
<group_state name="home" group="arm">
<joint name="joint1" value="0" />
<joint name="joint2" value="0" />
<joint name="joint3" value="0" />
<joint name="joint4" value="0" />
<joint name="joint5" value="0" />
<joint name="joint6" value="0" />
</group_state>
<group_state name="forward" group="arm">
<joint name="joint1" value="0" />
<joint name="joint2" value="0.4132" />
<joint name="joint3" value="0.7486" />
<joint name="joint4" value="0.2015" />
<joint name="joint5" value="-0.4031" />
<joint name="joint6" value="0" />
</group_state>
<group_state>标签描述的是自定义位姿，MArm中设置了“home”和“forward”两个位姿，主要存储每个位姿的六轴角度。
机械臂终端
<end_effector name="robot_gripper" parent_link="grasping_frame" group="gripper" />
<end_effector>标签描述的是机械臂终端的夹爪装置。
碰撞矩阵
<disable_collisions link1="base_link" link2="bottom_link" reason="Adjacent" />
<disable_collisions link1="base_link" link2="link1" reason="Adjacent" />
<disable_collisions link1="bottom_link" link2="link1" reason="Never" />
<disable_collisions link1="bottom_link" link2="link2" reason="Never" />
......
<disable_collisions>标签描述的是碰撞矩阵配置，存储了每两个link之间的碰撞情况，在运动规划过程中，永远不会碰撞的link无需进行碰撞检测，可以节省很多时间。
2、fack_controllers.yaml
在demo中，机械臂可以按照MoveIt！规划出来的轨迹运动。从轨迹到模型，中间需要一个控制器进行连接，这些虚拟的控制器参数在fake_controllers.yaml文件中配置如下：
controller_list:
- name: fake_arm_controller
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
- name: fake_gripper_controller
joints:
- finger_joint1
从上面的配置文件中可以看到，arm和gripper分别有一个控制器，可以控制机器人的多个关节按照运动规划的轨迹进行运动，同时更新/joint_state话题。
如果使用真实机器人，需要实现控制器部分的功能，完成机器人关节的驱动。
3、joint_limits.yaml
该文件设置机器人运动关节的速度限位，可以配置每个关节是否有速度、加速度限制，以及最大速度和最大加速度的数值。
joint_limits:
finger_joint1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint3:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint4:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint5:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint6:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
4、kinematics.yaml
MoveIt！提供多种运动学求解器，相应的配置参数存储在kinematics.yaml文件中，该配置文件会在启动move_group.launch文件时由包含的planning_context.launch文件夹加载。
arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.05
kinematics_solver_attempts: 3
以上运动学求解器的配置包含以下四个方面：
kinematics_solver：设置运动学插件，MoveIt！默认使用KDL插件。
kinematics_solver_search_resolution：设置存在冗余维度逆向运动学求解时的分辨率。
kinematics_solver_timeout：逆向运动学求解超时设置，单位为秒。
kinematics_solver_attempts：逆向运动学求解尝试次数。
5、ompl_planning.yaml
OMPL是MoveIt！中默认使用的运动规划库，ompl_planning.yaml文件中那个存储了运动规划的相关配置。
八、添加ArbotiX关节控制器
MoveIt！默认生成的demo中使用的控制器功能 ，可以使用其他控制器插件实现驱动机器人模型的功能。ArbotiX功能包中提供了Joint Trajectory Action Contorllers插件，可以用来驱动真实机器人的每个舵机关节。由于ArbotiX提供离线模式的支持，所以也可以使用该插件实现对仿真机器人的控制。接下来将介绍如何在MArm中添加这款关节控制器插件，从而驱动机器人模型。
1、添加配置文件
首先新建一个YAML格式的配置文件marm_description/config/arm.yaml，代码如下：
joints: {
joint1: {
id: 1, neutral: 205, max_angle: 169.6, min_angle: -169.6, max_speed: 90},
joint2: {
id: 2, max_angle: 134.6, min_angle: -134.6, max_speed: 90},
joint3: {
id: 3, max_angle: 150.1, min_angle: -150.1, max_speed: 90},
joint4: {
id: 4, max_angle: 150.1, min_angle: -150.1, max_speed: 90},
joint5: {
id: 5, max_angle: 150.1, min_angle: -150.1, max_speed: 90},
joint6: {
id: 6, max_angle: 360, min_angle: -360, max_speed: 90},
finger_joint1: {
id: 7, max_speed: 90},
}
controllers: {
arm_controller: {
type: follow_controller, joints: [joint1, joint2, joint3, joint4, joint5, joint6], action_name: arm_controller/follow_joint_trajectory, onboard: False }
}
arm.yaml配置文件主要分为以下两个部分。
机器人关节属性的设置：包括每个驱动关节的最大、最小角度、最大速度等。
控制器插件的设置：包含机器人六轴本体的控制类型、关节，以及所接受的action消息名称。
2、运行ArbotiX节点
启动ArbotiX中的节点，分别控制机器人的六轴本体和ozh终端夹爪。机器人的启动使用marm_description/launch/fake_arm.launch文件描述，ArbotiX节点部分的代码如下：
<launch>
<!-- 不使用仿真时间 -->
<param name="/use_sim_time" value="false" />
<!-- 启动arbotix driver-->
<arg name="sim" default="true" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find marm_description)/urdf/arm.xacro'" />
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find marm_description)/config/arm.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller">
<rosparam>
model: singlesided
invert: false
center: 0.0
pad_width: 0.004
finger_length: 0.08
min_opening: 0.0
max_opening: 0.06
joint: finger_joint1
</rosparam>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
<!-- 启动rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_description)/urdf.rviz" required="true" />
</launch>
第一个ArbotiX节点会加载上一步中创建的配置文件，并且启动一个控制机器人六轴本体的控制器。ArbotiX还提供了一个夹爪控制器，支持控制一个或多个舵机组成的终端夹爪。MArm夹爪只有一个可动关节，虽然是直线运动，但依然可以使用ArbotiX中的gripper_controller进行控制，且在输入数据上需要将直线运动的长度近似转换成角度旋转。因此第二个ArbotiX节点启动了控制MArm终端夹爪的gripper_controller，同时需要配置一些相关参数。
3、测试例程
在之前的配置中可以看到，无论是机器人的控制还是夹爪额控制，使用的通信机制都是action。所以编写一个例程，通过发布需要的action请求来测试以上配置是否成功。例程代码marm_planning/scripts/demo.py的具体内容如下：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
class TrajectoryDemo():
def __init__(self):
rospy.init_node('trajectory_demo')
# 是否需要回到初始化的位置
reset = rospy.get_param('~reset', False)
# 机械臂中joint的命名
arm_joints = ['joint1',
'joint2',
'joint3',
'joint4',
'joint5',
'joint6']
if reset:
# 如果需要回到初始化位置，需要将目标位置设置为初始化位置的六轴角度
arm_goal = [0, 0, 0, 0, 0, 0]
else:
# 如果不需要回初始化位置，则设置目标位置的六轴角度
arm_goal = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7]
# 连接机械臂轨迹规划的trajectory action server
rospy.loginfo('Waiting for arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# 使用设置的目标位置创建一条轨迹数据
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
rospy.loginfo('Moving the arm to goal position...')
# 创建一个轨迹目标的空对象
arm_goal = FollowJointTrajectoryGoal()
# 将之前创建好的轨迹数据加入轨迹目标对象中
arm_goal.trajectory = arm_trajectory
# 设置执行时间的允许误差值
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# 将轨迹目标发送到action server进行处理，实现机械臂的运动控制
arm_client.send_goal(arm_goal)
# 等待机械臂运动结束
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
if __name__ == '__main__':
try:
TrajectoryDemo()
except rospy.ROSInterruptException:
pass
4、运行结果
首先运行marm_description/launch/fake_arm.launch文件，启动机器人模型、ArbotiX控制器以及rviz：
roslaunch marm_description fake_arm.launch
启动成功后，可以在界面中看到处于初始状态下的机械臂。
然后运行测试例程：
rosrun marm_planning trajectory_demo.py _reset:=False
机器人开始平滑运动，到达指定位姿后停止。
如果想让机器人回到初始位姿，可以使用如下命令：
rosrun marm_planning trajectory_demo.py _reset:=True
可见，这种方法类似于在rviz中搭建了一个机械臂的仿真环境，通过代码实现对机械臂模型的控制。
学术交流坊：
