ROS学习笔记（八）—— moveit！概述
参考：
【运动规划】MoveIt!
运动规划 | MoveIt篇
MoveIt Tutorials
github moveit
1. moveit！简介
Easy-to-use open source robotics manipulation platform for developing commercial applications, prototyping designs, and benchmarking algorithms.
MoveIt! 是一个机器人移动操作（mobile manipulation）相关的工具集软件，集成了各种 SOTA 库，包括：
-
运动规划（Motion Planning）：要让一个机器人实现运动规划，需要先将机器人抽象到构形空间（C-Space）。MoveIt就可以帮大家把这些工作给做了，只需提供机器人URDF模型，就可以调用几大运动规划库（坑）的规划算法（如OMPL，SBPL，CHMOP），自动生成机器人运动轨迹。
-
轨迹插值（Trajectory Processing）：由于大多数规划器只能返回一系列路径点，MoveIt可以根据机器人的控制参数（速度、加速度限制等）重新处理路径，生成一条带有时间戳、位置、速度、加速度信息的完整轨迹。
-
操作（Manipulation）：根据识别的物体生成一系列动作抓取物体（pick-and-place），这个目前还比较弱，不涉及任何反馈、动力学、re-grasp等操作问题。
-
3D 感知（Perception）：这个并不是说MoveIt整合了物体识别、环境建模等模块，而是它可以利用传感器采集的信息（点云或深度图像）生成用于碰撞检测的OctoMap。OctoMap就是以八叉树形式表示点云，可以大大降低存储空间，同时，这些3D OctoMap也可以依据贝叶斯准则不断实时更新。这样，机器人就可以避开真实世界的障碍物了.
-
运动学（Kinematics）：目前可以支持多种运动学求解器，如OpenRave的ikfast（封闭解）、Orocos的KDL（数值解）、Trac_ik（考虑关节极限的数值解）、基于service的求解器（用户自己定义）。
-
碰撞检测（Collision Checking）：碰撞检测是运动规划的一大难题，如果采用基于采样的规划算法，那么我们需要对每个采样点做有效性判断，这时候就需要进行碰撞检测。所以，运动规划需要提供一个高效的碰撞检测算法。FCL（Flexible Collision Library），可以非常快速地实现各种几何体（3D面片、OctoMap、基本几何体）的碰撞检测。
-
控制（Control）：这个其实不能算控制，只是一个机器人控制接口问题。由于不同机器人的控制接口都不一样，开发者只需简单修改配置文件，就可以让MoveIt发布出机器人相应的控制指令（只是修改action名字而已）。
-
导航（Navigation）：MoveIt虽然原理上可以进行移动机器人的导航，但是它没提供针对移动机器人的规划器。也就是说目前它的Navigation功能是不能直接使用的。
-
交互（Interaction）：MoveIt给开发者提供了三种方式交互方式，Rviz图形界面（直观）、Python（快速编程）、C++（丰富的高级功能）。
目前而言，MoveIt还只是一个针对机械臂运动规划问题的软件平台，暂时还不是适合用在Navigation、manipulation、control、perception 等领域。
2. moveit！的系统架构
moveit！Concepts
MoveIt! 以 move_group 为核心节点，集成了各种组件，用于提供各种 ROS 动作和服务供用户使用。
MoveIt!中的众多功能都使用插件机制集成，并提供了丰富的接口。这样用户可以方便的覆写插件功能，而无需接触框架的核心。
Plugin Interfaces
2.1 核心节点 move_group
2.1.1 User Interface
用户可以使用 move_group 提供的三种方式来处理 actions 和 services：
- C++：使用 move_group_interface 包提供的 API
- Python：使用 moveit_commander 包提供的 API
- GUI：Rviz 的 Motion Planning 插件
无论是C++还是Python编程，完整的MoveIt!程序流程：
- 确定控制对象的规划组
- 设置目标位姿（关节空间或笛卡尔空间）
- 设置运动约束（可选）
- 使用 Moveit！进行运动规划
- 执行规划轨迹
2.1.2 Robot Interface
move_group 通过 ROS topics 和 actions 与机器人进行通信，可以获取机器人当前的状态（如关节位置）、获取点云或其他感知数据、与机器人控制器通信等。
- Joint State Information: move_group 监听
/joint_states话题来获取当前的状态信息； - Transform Information: move_group 通过ros
TF library监听坐标转换信息。但 move_group 只能监听 TF，若要从机器人发布 TF 信息，必须在机器人上运行robot_state_publisher节点； - Controller Interface: move_group 通过
FollowJointTrajectoryAction接口与机器人控制器进行交互。这是一个 action 接口，move_group 实例化一个客户端与机器人控制器的动作服务器通信。（move_group 仅有 action 客户端，机器人服务端需自行编写） - Planning Scene: move_group 使用 规划场景监听器（Planning Scene Monitor ）来维护一个“规划场景”，该场景描述了机器人所处世界和机器人当前的状态。
- 扩展能力：move_group的结构易于扩展。诸如拾取和放置，运动学，运动规划之类的功能实际上是作为具有通用基类的单独插件实现的。可以使用一组ROS
.yaml参数和ROS pluginlib库来配置插件。大多数用户无需手动配置 move_group 插件，因为它们是在 MoveIt Setup Assistant 生成的launch文件中自动配置的。
2.1.3 Configuration
move_group节点通过参数服务器（param server）获取以下信息：
- URDF：robot_description 参数，获取机器人 URDF 模型信息
- SRDF：robot_description_semantic 参数，获取机器人模型的配置信息，SRDF 通常由用户使用 MoveIt Setup Assistant 创建（一次性）；
- MoveIt configuration：机器人的其他配置信息，如关节限位、运动学插件、运动规划插件等，这些组件的config文件由MoveIt设置助手(MoveIt setup assistant)自动生成，并存储在机器人相应的MoveIt配置包的config目录中。
2.2 运动规划 motion planner 插件
MoveIt! 通过插件机制（plugin interface）与运动规划器插件（motion planner）进行交互，可以使用多个库的不同运动规划器，使得 MoveIt! 扩展性更强。默认使用的运动规划器是 OMPL（Open Motion Planning Library） 库。move_group与motion planner的通信接口是由move_group节点提供的action或service。
2.2.1 运动规划请求（the Motion Plan Request）
做运动规划需要指明你想让运动规划器执行的操作，如将机械臂末端执行器移动到新位姿。默认会进行 collision 检查，包括self-collisions。可以指定一些约束条件让运动规划去 check。MoveIt! 内置的约束为运动学约束（kinematic constraints），包括：
- Position constraints：限制 link 运动的位置
- Orientation constraints：限制 link 的运动姿态（roll, pitch, yaw）
- Visibility constraints：限制 link 上的某点在某区域内的可见性
- Joint constraints：限制 joint 的运动范围
- User-specified constraints：用户通过回调函数自定义所需的约束条件
2.2.2 运动规划应答（The Motion Plan Result）
move_group 节点将会针对运动规划请求生成期望的轨迹（trajectory not just a path）， move_group将使用所需的最大速度和加速度（如果指定）来生成一条轨迹，该轨迹服从关节的速度和加速度约束。
完整的运动规划 pipeline 包含运动规划器（motion planner）和规划请求适配器（Plan Request Adapters），如下图所示。
规划请求适配器（Plan Request Adapters）能够对规划请求做预处理（pre-processing）、对规划响应做后处理（post-processing）。预处理可以应对一些如关节起始状态超出关节限制的场景，后处理可以将生成的路径转成 time-parameterized 轨迹。MoveIt! 提供了一组默认运动规划适配器，如下所示：
- FixStartStateBounds：修复 joint 初始极限
- FixWorkspaceBounds：设置默认尺寸的工作空间
- FixStartStateCollision：修复碰撞配置文件
- FixStartStatePathConstraints：找到满足约束的姿态作为机器人的初始姿态
- AddTimeParameterization：为空间轨迹进行速度、加速度约束，为每个轨迹点加入速度、加速度、时间等参数
2.3 Planning scene
Planning scene 用于表示机器人所处的环境以及机器人本身的状态。由 move_group节点内的规划场景监视器(planning scene monitor)维护。规划场景监测器监听以下话题：
- State Information：joint_states 话题
- Sensor Information：使用用下面描述的世界几何形状监视器 world geometry monitor
- World geometry information：from user input on the planning_scene topic (as a planning scene diff).
- World Geometry Monitor：世界几何图形监视器使用来自机器人上的传感器和用户输入的信息来构建世界几何图形。 它使用 occupancy map monitor 来构建机器人周围环境的3D描述，并使用关于planning_scene topic的信息对其进行扩充，以添加对象信息。
moveit 内置支持两种3D Sensors输入：点云（point cloud occupancy map updater plugin）和深度图（depth image occupancy map updater plugin）。
2.4 Kinematics 插件
MoveIt! 基于插件机制，允许用户添加自己的逆运动学算法。MoveIt! 默认的逆运动学插件为基于雅克比迭代的数值求解器 KDL，可以通过 MoveIt Setup Assistant 自动配置。
通常，用户可以选择编写自己的运动学求解器，一种流行的方法是使用 IKFast 包生成与特定机器人配合使用所需的 C++ 代码，插入到 MoveIt! 中。
2.5 Collision Checking
MoveIt! 中的碰撞检测是通过 Planning Scene 的 CollisionWorld 对象来配置的，主要是通过 FCL（Flexible Collision Library） 库来实现的。通常无需用户手动配置。
MoveIt! 支持以下几种物体的碰撞检测：
- Meshes
- Primitive Shapes，如 boxes, cylinders, cones, spheres 等
- Octomap，可以直接用来做碰撞检测
对于运动规划来说，碰撞检测是非常耗时的，通常要占90%的计算时间。Allowed Collision Matrix (ACM) 使用二值化来处理是否需要做碰撞检测，如两个物体相隔很远，永远不会发生碰撞，则在 ACM 中将相应的设为 1 ，表明两个物体不需要做碰撞检测。
2.6 Trajectory Processing
运动规划器通常只生成没有时间信息的路径，而 MoveIt! 通过 trajectory processing routine 来生成 time-parameterized 的轨迹，满足关节速度和加速度的限制，这些限制可以从 joint_limits.yaml文件中读取。
3. 如何用 MoveIt！控制机器人
3.1 建立机器人URDF模型
URDF（Unified Robot Description Format）是ROS中使用的一种机器人描述文件，它以HTML的形式定义一个机器人。包含的内容有：连杆、关节名称，运动学参数、动力学参数、可视化模型、碰撞检测模型等。后续碰撞检测、运动学求解、规划等都依赖于URDF文件。
-
单臂、串联机器人，可以使用ROS官方发布的 sw_urdf_exporter，从SolidWorks中导出URDF文件。
双臂（双臂机器人用这个插件经常出问题）、并联机器人（需要自己用mimic_joint改成串联形式）可以考虑自己手写URDF或者xacro文件。 -
坐标系设置尽量满足所有关节为0°时候，所有坐标系同姿态（这样可以避免引入pi）。
-
如果想要有颜色的模型，可以自己生成每个零件的dae模型，而不使用stl模型；
-
可视化模型采用漂亮、精细的模型，碰撞模型可以使用简化的模型。
3.2 建立机器人ROS驱动
驱动真实机器人，需要有相应的驱动接口程序。机器人的ROS驱动并没有什么标准的格式或者规定。对于MoveIt而言，只要求你有个ROS node，它有两个功能：
- 发布关节状态
MoveIt需要从机器人当前状态开始规划，因此这个ROS驱动需要能够实时获取机器人的各关节信息（如角度），并用/joint_states消息发布； - 接收规划结果，并下发给机器人
MoveIt 规划的结果会以一个 action 的形式发布，所以我们的ROS驱动就应该提供一个 action server，这个功能就是接收规划结果，下发给机器人，并反馈执行情况。action的类型是control_msgs/FollowJointTrajectory。
具体action的写法可以参照ROS官网。简单而言，一个action有五个部分：
- action_name/goal：这个就是规划的路径，我们需要接收这个路径，并将所有路径点解析成机器人控制器可以识别的形式，之后下发给机器人，必须要有；
- action_name/cancel：这个指令可以随时中断正在执行的动作，但并不是必须的功能；
- action_name/feedback：这个是实时反馈执行状态，最简单的就是将机器人当前关节角度等信息反馈回去，非必须；
- action_name/status：这个用于显示机器人状态，如正在执行动作、等待、执行结束等待，非必须；
- action_name/result：这个就是在动作执行完之后给MoveIt反馈一个执行结果，这个是必须要有的，当然，为了简单，可以已接收到goal就反馈执行成功。
UR 机器人已经写好这部分驱动。
3.3 生成 MoveIt！配置文件
利用MoveIt Setup Assistant，很容易配置。
首先你要有机器人的urdf或xacro 描述文件。配置内容如下：
Generate Self-Collision Matrix，Add Virtual Joints，Add Planning Groups，Add Robot Poses， Label End Effectors，Add Passive Joints，3D Perception， Gazebo Simulation，ROS Control， Add Author Information。
3.4 修改 MoveIt！配置文件与launch文件
前面生成的配置文件都是针对虚拟机器人的，如果需要连接实际机器人，需要修改一些配置文件：
controllers.yaml：这个就是要根据你的ROS驱动中的action来修改，MoveIt可以根据这个配置文件发布出与机器人驱动相匹配的action。简单地说，就是action的名字、类型、关节名字几个信息。robot_moveit_controller_manager.launch：这个额外新增，就是在不适用fake controller的时候能找到上述controller.yaml文件，发布出正确的action类型。sensors.yaml：传感器配置文件需要额外增加，它主要定义了点云的消息名称、OctoMap属性等。moveit_sensor_manager.launch：增加传感器配置文件后，我们也需要在launch文件中增加对配置文件的读取。
其他（可选）：industrial_robot_simulator、warehouse、joystick、规划器、规划算法库……
3.5 机器人，走你！（python）
用 moveit 程序进行机器人轨迹规划，并驱动真实机器人运动。
官方教程：Move Group Python Interface
参考代码：move_group_python_interface。
move_group_python_interface的应用实例：ROS MoveIt! 程序控制真实UR5机器人运动（python）