ROS学习笔记17：MoveIt!机械臂控制
前言：
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文件如下：
Gazebo/White
Gazebo/White
Gazebo/Blue
Gazebo/White
Gazebo/Blue
Gazebo/Black
Gazebo/White
Gazebo/Blue
Gazebo/White
Gazebo/White
transmission_interface/SimpleTransmission
hardware_interface/PositionJointInterface
hardware_interface/PositionJointInterface
1
/arm
下面是程序段解读：
（1）宏定义：
1）材料定义：
2）属性定义：
3）惯性矩阵定义：
（2）link与joint：
1）为底盘（bottom_link）设计一个较大的质量，防止机械臂倾倒：
2）名为“grasping_frame”的link不包含任何内容，主要作用是创建一个抓取操作时的参考坐标系：
（3）Gazebo属性：
2.编写launch文件启动rviz显示机械臂模型：
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:机器人厂商开发的工业机器人控制器。