前言：
ROS 提供了针对机械臂的功能包，这些功能包在2012年集成为个单独的ROS软件“MoveIt!”。
MoveIt!为开发者提供了一个易于使用的集成化开发平台，由一系列移动操作的功能包组成，包含运动规划、操作控制、3D感知、运动学、控制与导航算法等，且提供友好的 GUI ，可以广泛应用于工业、商业、研发和其他领域。
MoveIt!实现机械臂控制的四个步骤如下：
1）组装：在控制机器人之前需要有机器人，可以是真实的机械臂，也可以是仿真的机械臂，但都要创建完整的机器人URDF模型。
2）配置：使用MoveIt!控制机械臂之前，需要根据机器人的URDF模型，再使用Setup Assistant工具完成自碰撞矩阵、规划组、终端夹具等配置，配置完成后生成一个ROS功能包。
3）驱动：使用ArbotiX或者ros_control功能包中的控制器插件，实现对机械臂关节的驱动。插件的使用方法一般分为两步：首先创建插件的YAML配置文件，然后通过launch文件启动插件并加载配置参数。
4）控制：MoveIt!提供了C++、Python、rviz插件等接口，可以实现机器人关节空间和工作空间下的运动规划，规划过程中会综合考虑场景信息，并实现自主避障的优化控制。
一、MoveIt!系统架构：
1.运动组：
move_group是MoveIt!的核心节点，可以综合其他独立的功能组件为用户提供ROS中的动作指令和服务，如下图所示：
move_group本身不具备丰富的功能，主要完成各功能包、插件的集成。它通过消息或服务的方式接收机器人的发布的点云信息、关节状态信息，以及机器人的TF坐标变换；另外，还需要ROS参数服务器提供机器人的运动学参数，这些参数可根据机器人的URDF模型生成。
move_group的结构很容易通过插件形式扩展，已有的功能模块也是通过插件的形式集成在MoveIt!中。
（1）用户接口：
MoveIt!提供三种可供调用的接口：
1）C++：使用move_group_interface包提供的API。
2）Python：使用moveit_commander包提供的API。
3）GUI：使用Movelt!的rviz插件。
（2）ROS参数服务器：
ROS的参数服务器需要为move_group提供三种信息：
1）URDF：robot_description 参数，获取机器人URDF模型的描述信息。
2）SRDF：robot_description_semantic参数，获取机器人模型的配置信息。
3）config：机器人的其他配置信息，例如关节限位、运动学插件、运动规划插件等。
（3）机器人：
move_group和机器人之间通过Topic和Action通信。
2.运动规划器：
（1）运动规划：
已知机器人的初始姿态和目标姿态，以及机器人和环境的模型参数，那么可以通过某种算法，在躲避障碍物和防止自身碰撞的同时，找到一条到达目标姿态的较优路径，这种算法称为机器人的运动规划。
在MoveIt!中，运动规划算法由运动规划器完成。
（2）运动规划器的结构：
（3）约束条件：
运动规划不能随意计算，可以根据实际情况设置一些约束条件。
1）位置约束：限制 link 的运动区域；
2）方向约束：限制 link 的运动方向（roll、pitch 和yaw）；
3）可见性约束：限制 link 上的某点在某区域内的可见性（通过视觉传感器）；
4）joint 约束：限制 joint 的运动范围；
5）用户定义约束：用户通过回调函数自定义所需的约束条件。
（4）适配器：
MoveIt!提供以下适配器：
1）FixStartStateBounds：修复 joint 的初始极限；
2）FixWorkspaceBounds：设置一个默认尺寸的工作空间；
3）FixStartStateColision：修复碰撞配置文件；
4）FixStartStatePathConstraints：找到满足约束的姿态作为机器人的初始姿态；
5）AddTimeParameterization：为空间轨迹进行速度、加速度约束，为毎个轨迹点加入速度、加速度、时间等参数。
3.规划场景：
规划场景可以为机器人创建一个工作环境，包括外界环境中的桌面、工件等物体。这一功能主要由move_group节点中的规划场景监听器(Planning Scene Monitor)实现。
该插件会监听以下信息：
（1）状态信息(State Information)：机器人的关节话题 joint_states ；
（2）传感器信息(Sensor Information)：机器人的传感器数据；
（3）外界环境信息(World geometryinformation)：通过传感器建立的周围环境信息。
4.运动学求解器：
MoveIt!中的运动学插件允许开发者灵活选择多种可供使用的运动学求解器，可以在MoveIt! Setup Assistant工具中进行配置。
5.碰撞检测：
MoveIt!使用Collision World对象进行碰撞检测，采用FCL功能包实现。碰撞检测是运动规划中最耗时的运算之一，为减少计算量，可以在MoveIt! Setup Assistant工具中设置免检冲突矩阵（ACM）进行优化。如果两个刚体之间ACM设置为1，则意味着两个刚体永远不会发生碰撞，即不需要碰撞检测。
二、创建机械臂模型与配置文件：
下面先虚拟一个六自由度机械臂——MArm。
功能包资料链接：
链接: https://pan.baidu.com/s/1xu-FfbJSS0brF1pKeZxw0A 提取码: 1234 复制这段内容后打开百度网盘手机App，操作更方便哦
1.创建机械臂urdf文件：
机械臂的urdf文件如下：
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- 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>
<!-- /// 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="Black" />
</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>
<!--<xacro:inertial_matrix mass="1"/>-->
</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>
<!-- / 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>
</robot>
下面是程序段解读：
（1）宏定义：
1）材料定义：
2）属性定义：
3）惯性矩阵定义：
（2）link与joint：
1）为底盘（bottom_link）设计一个较大的质量，防止机械臂倾倒：
2）名为“grasping_frame”的link不包含任何内容，主要作用是创建一个抓取操作时的参考坐标系：
（3）Gazebo属性：
2.编写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" type="joint_state_publisher" />
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_description)/urdf.rviz" required="true" />
</launch>
3.对工作空间进行编译：
编译过程中可能因为ROS noetic缺少某些必要的包和依赖，导致在编译时报错，解决办法如下：
（1）下载以下功能包，解压后去掉后缀，复制到src中：
https://github.com/ros-interactive-manipulation/manipulation_msgs/
https://github.com/ros-interactive-manipulation/household_objects_database_msgs
（2）使用命令行安装以下功能包：
sudo apt-get install ros-noetic-object-recognition-msgs
sudo apt-get install ros-noetic-moveit4.完成编译后运行launch文件：
5.创建配置文件——MoveIt! Setup Assistant：
启动MoveIt! Setup Assistant：
rosrun moveit_setup_assistant moveit_setup_assistant
（1）加载机器人URDF模型（Start）：
（2）配置自碰撞矩阵（Self-Collisions）：
（3）配置虚拟关节（Virtual Joints）：
（4）创建规划组（Planning Groups）：
（5）定义机器人位姿（Robot Poses）：
（6）配置终端夹爪（End Effectors）：
（7）设置作者信息（Author Information）：
（8）生成配置文件（Configuration Files）：
生成完毕，生成的配置文件如下：
6.启动MoveIt!：
（1）启动MoveIt!：
roslaunch marm_moveit_config demo.launch
（2）拖动规划：
（3）随机目标规划：
（4）碰撞检测：
三、Moveit!编程基础：
在实际应用中，在实际应用中，GUI提供的功能毕竟有限，很多实现还要再代码中完成。MoveIt!的move_group提供了丰富的C++和Python的编程API，可以完成更多的运动、控制相关功能。
准备工作：
将marm_planning文件夹复制到arm-of-robot-using-Moveit-in-ros-gazebo-rviz-master文件夹下。
将manipulation_msgs功能包放到src目录下。（这个功能包无法在noetic版本下载，只能拷贝）
功能包资料链接：
链接: https://pan.baidu.com/s/1rnSX1sScHkyvgp8oZq6iFA 提取码: 1234 复制这段内容后打开百度网盘手机App，操作更方便哦
链接: https://pan.baidu.com/s/1yQhuC1b1mIseNEtVwhQmTA 提取码: 1234 复制这段内容后打开百度网盘手机App，操作更方便哦
1.关节空间规划（Python）：
以关节角度为控制量的机器人运动；机器人状态使用各轴位置来描述，指定运动目标的机器人状态后，通过控制各轴运动来到达目标位姿。
启动文件：
roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_fk_demo.py基本流程：
（1）首先创建规划组的控制对象；
（2）然后设置关节空间运动的目标位姿；
（3）最后控制机械臂完成运动。
2.工作空间规划：
以关节角度为控制量的机器人运动；机器人状态使用各轴位置来描述，指定运动目标的机器人状态后，通过控制各轴运动来到达目标位姿。
启动文件：
roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_ik_demo.py基本流程：
（1）首先创建规划组的控制对象；
（2）然后获取机器人的终端link名称；
（3）其次设置目标位姿对应的参考坐标系、起始位姿和终止位姿；
（4）最后进行规划并控制机器人运动。
3.笛卡尔运动规划（约束运动规划中机器人的终端轨迹）：
（1）规划出直线轨迹：
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=True
（2）不再以直线规划：
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=False
4.避障规划：
MoveIt!中默认使用的运动规划库OMPL支持避障规划。
启动文件：
roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_obstacles_demo.py（本例程核心在于如何创建场景对象）
基本流程：
（1）初始化场景，设置参数；
（2）在可视化环境中加入障碍物模型；
（3）设置机器人的起始位姿和目标位姿；
（4）进行规划。
5.抓取和放置物体：
四、Gazebo机械臂仿真：
准备工作：
功能包资料链接：
链接: https://pan.baidu.com/s/1lSfZWSKdzW4_XdflOmAWEQ 提取码: 1234 复制这段内容后打开百度网盘手机App，操作更方便哦
1.Gazebo中的机械臂仿真：
（1）创建配置文件（配置controller插件）：
（2）创建launch文件，加载所配置的控制器：
（3）创建顶层launch文件，包含加载控制器的launch文件，启动Gazebo仿真环境：
（4）开始仿真：
（5）查看当前话题列表，发布控制运动的话题消息：
2.使用MoveIt!控制Gazebo中的机械臂仿真：
在Gazebo中完成更加复杂的运动规划控制，可使用MoveIt!，核心在于实现MoveIt!和Gazebo之间的通信；Movelt !完 成 运 动 规 划后 的输 出 接 口 是 一个 命 名 为“FollowJointTrajectory”的action, 其中包含一系列规划好的路径点轨迹；ros control提供了一个名为“Joint Trajectory Controller ”的控制器插件，将这些信息转换成Gazebo中机器人需要输人的joint位置。
五、ROS-I框架介绍：
1.ROS-I(ROS-Industrial) 的目标是：
（1）将ROS强大的功能应用到工业生产的过程中；
（2）为工业机器人的研究与应用提供快捷有效的开发途径；
（3）为工业机器人创建一个强大的社区支持；
（4）为工业机器人提供一站式的工业级ROS应用开发支持。
2.总体架构：
（1）GUl : ROS 中现在已有的 UI 工具和专门针对工业机器人通用的 UI 工具（规划中）;
（2）ROS Layer : ROS 基础框架，提供核心通信机制；
（3）Movelt! Layer :为工业机器人提供规划、运动学等核心功能的解决方案；
（4）ROS-I Application Layer:处理工业生产的具体应用，目前也是针对未来的规划；
（5）ROS-I Interface Layer:接口层，工业机器人的客户端，通过 simple message 协议与机器人的控制器通信；
（6）ROS-I Simple Message Layer：通信层，定义了通信的协议，打包和解析通信数据；
（7）ROS-I Controller Layer:机器人厂商开发的工业机器人控制器。
