170110-学习MoveIt!

前言

• 基础而已，一定要学

参考

Mastering ROS for Robotics Programming.2015
moveit! home page

学习记录

概述

• MoveIt!是控制机械臂运动的一系列包和工具，可以完成移动和抓取任务，进行正逆运动学解算（FK&IK）和路径规划（MP）。
• 主要包含的功能有：运动规划，执行器操纵，3D 感知，运动学，障碍检测，控制，导航；而且还有一个非常友好的，基于RViz的前端，当然也有很好的C++ API。
• 学习这个包，首先将在RViz中做运动规划，然后与Gazebo联系做机器人仿真运动

架构

• move_group节点是核心节点，提供ROS服务和动作。
• 向GUI提供的service: Get CartesianPath Service; Get IK Service; Get FK Service; Get Plan Validity Service; Plan Path Service; Execute Path Service; Get Planning Scene Service
• 向GUI提供的Action: MoveGroupAction; PickAction; Place Action
• 向参数服务器索取的参数：URDF; SRDF; Config
• 向机器人传感器索取的消息有：TF；JointState；视觉传感消息；
• 向机器人控制器（ROSController）发送的消息：JointTrajectoryAction
• 请访问这个网站查看更详细情况

move_group node

• move_group node 从参数服务器中获取运动学定于和机器人描述，这些都定义在URDF文件中，另外我们配置完moveit之后会产生两个文件，SRDF和config文件，配置文件包括关节限位，感知信息，运动学，末端执行器等等机械臂配置情况。当配置好机器人描述文件和机器人配置信息后，就可以使用人机界面来发布命令了。可以使用C++或Python API来发布服务请求或动作请求，不过更方便的方式是使用RViz的运动规划插件
• move_group类似于move_base，本身没有实现任何算法，只是一个集合器，把其他组件都以插件的方式组合起来，包括运动学解算器，运动规划器等。插件的软件设计方式意味着，我们可以很容易地用自己设计的解算器，作为插件来部署到moveIt!上。
• 运动规划结束后，生成好的轨迹被发送给controllers，使用FollowJointTrajectoryAction动作接口，动作服务器在机器人上运行，客户端在move_group上运行。也就是说，如果在Gazebo上仿真，那么Gazebo应该负责生成controller。

Motion Planning using MoveIt!

• 运动规划，是针对机器人的起始状态和目标状态，求解出一个最优路径的一种技术，最优条件往往是路径最短或避障。默认的计算器插件是OMPL，OMPL中也含有非常多的规划算法。通常发送一个末端执行器目标位姿消息给运动规划器进行规划。
• 一般我们也需要设置一些额外的运动学约束，内置的几种运动学约束如下
  • 位置约束：某连杆的位置约束
  • 姿态约束：某连杆的朝向约束
  • 是否可见：连杆上的某个点对于传感器是否可见（可见意味着可以被识别为障碍）
  • 关节约束：某关节的角限位
  • 用户自定义约束：使用回调函数来定义一些自己的约束
• 规划器会规划出一条遵守所有约束条件的路径，这个路径会从move_group node发送到轨迹控制器（trajectory controllers）

Motion planning request adapters规划请求适配器

• 适配器会事先处理发送给规划器请求，以及在时候对规划器的结果进行一些后处理。比如，适配器会在前处理中确认关节状态有没有冲突，在后处理中将规划器生成的路径加上时间参数。更多如下
  • FixStartStateBounds: 如果一个关节状态轻微超出限位值，适配器会规整到那个限位值。
  • FixWorkspaceBounds: 将工作空间规定在一个10mX10mX10m的立方空间内
  • FixStartStateCollision: 当当前关机配置在障碍物中时，通过一个参数因子jiggle_factor来改变当前关节配置
  • FixStartStatePathConstraints: 起始配置不符合约束条件时，寻找一个最近的符合约束条件的起始状态
  • AddTimeParameterization: 加上时间参数，来满足速度和加速度约束

MoveIt planning scene，规划场景

• 规划场景，负责展示环境和机器人本身的状态，move_group还有一个模块叫做world geometry monitor，主要负责传感器中的环境描述
• Planning Scene Monitor包含三个子模块：world geometry monitor, scene monitor, state monitor. scene monitor从机器人读取joint_states信息，world scene monitor从视觉传感器读取3D信息，构件octomap，cotomap可以从点云数据生成，也可以从深度信息生成，分别是point cloud occupancy map updater and depth image occupancy map updater.
• 人机界面可以通过Planning Scene Monitor来向环境添加物体，通过界面查看环境。

运动学

• 有多种解算器，默认的IK解算器是一种基于jacobian的数值解算器。
• IKFast包能够生成C++代码使用解析法来进行逆运动学解算，适用于自由度小于6的机械臂。

障碍监测Collision checking

• 使用FCL后端，支持不同种类物体的障碍监测，如meshes, primitive shapes, octomap。一般来讲障碍检测是运动规划时最耗时的步骤。
• 为了减少计算时间，MoveIt!提供一种ACM（Allowed Collision Matrix）允许障碍矩阵，优化这个矩阵可以较少运算时间。可以在物体在远离障碍的情况下将ACM设置为1

使用Setup Assistant tool

概述

• 该工具产生SRDF，配置文件，启动文件，其他脚本文件。
• SRDF文件包含机械臂关节，最终效应器关节，虚拟关节，障碍连杆对，的一些细节
• 配置文件包括运动学解算器，关节限位，控制器的一些细节。
• 使用这些文件和包，我们能够直接在RViz中进行仿真。

启动

roslaunch moveit_setup_assistant setup_assistant.launch

生成Selt-Collision matrix自碰撞矩阵

• 其实就是定义哪些连杆应该作为障碍物，哪些连杆永远不会被碰撞，不用在碰撞检测算法中进行计算。
• This tool analyses each link pair and categorizes the links as always in
  collision, never in collision, default in collision, adjacent links disabled, and
  sometimes in collision, and it disables the pair of links which makes any kind
  of collision.
• sampling density，是进行障碍检测算法的采样密度，默认为10000

添加虚拟关节

• 当机械臂的基座不是固定的时候，就需要添加虚拟关节，例如，如果机械臂是在一个移动机器人上，那么需要在odom坐标系上定义一个虚拟关节

添加规划集合（planning groups）

• 一个规划集合是要在一起规划的关节集合，一般要定义两个规划集合，一个是机械臂规划集合arm，一个是抓手规划集合griper
• 对于每一个集合，需要定义运动学解算器，kinematics_plugin/KDLKinematicsPlugin, which is the default numerical
  IK solver with MoveIt!. We can keep the other parameters as the default
  values
• 定义运动链，chain的起点和终点，如base_link->grasping_frame
• 对于gripper group，不需要定义运动学解算器，我们可以添加关节和连杆

添加robot pose

• 添加一些预定义的姿态，如home，pick/place点，这样在编程过程中我们可以直接调用API来使用这些预定义的点。

设置末端执行器eef

• 这一步，我们命名，添加效应器规划集合，规定父连杆，父规划集合。
• 我们可以定义很多EEF，典型情况是一个抓手

添加被动关节

• 被动关节是没有马达或激励的，定滑轮就是一个例子，规划器会忽略这一关节。
• 一般不用定义这个关节

产生配置文件

• 这一步，会产生一个包含各种配置文件的包,称作moveit_config
• 将这个包放在catki_ws下，编译后就可以使用了。

在RViz中使用配置包

• 启动配置包中的例子程序

roslaunch seven_dof_arm_config demo.launch

RViz插件详解

context选项卡

planning选项卡

• By default, execution is done on fake controllers.We can change these controllers into trajectory controllers for executing the planned trajectory in Gazebo or the real robot.
• 可以使用交互式标记，绿色是机械臂的起始位姿，橘红色是目标位姿，点击规划按钮后，会开始规划，如果某姿态是红色，以为着发生了自碰撞。
• 还可以随机定义目标位姿

连接到Gazebo

概述

• 需要在Gazebo中定义一个FollowJointTrajectoryAction动作服务器
• 步骤1和2为gazebo创建控制器文件
• 之后的步骤为gazebo创建文件

Step 1 – Writing the controller configuration file for MoveIt!

• 与moveit的trajectory controllers通信。称作controllers.yaml，在./config/文件夹下。
• 典型的配置文件需要定义两个控制器接口，arm和gripper
• The default: true indicates that it will use the default controller, which is the primary controller in MoveIt! for communicating with the set of joints.

Step 2 – Creating the controller launch files

• seven_dof_arm_moveit_controller_manager.launch来启动trajectory controllers
• The name of the file starts with the robot name, which is added with _moveit_controller_manager.

Step 3 – Creating the controller configuration file for Gazebo

• Create a new file called trajectory_control.yaml which contains the list of the Gazebo ROS controllers that need to be loaded along with Gazebo
• chapter_4_codes/seven_dof_arm_gazebo/config

Step 4 – Creating the launch file for Gazebo trajectory controllers

• we can load the controllers along with Gazebo. We have to create a launch file which launches Gazebo, the trajectory controllers, and the MoveIt! interface in a single command.

<launch>
<!-- Launch Gazebo -->
<include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_
world.launch" />
<!-- ros_control seven dof arm launch file -->
<include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_
gazebo_states.launch" />
<!-- ros_control position control dof arm launch file -->
<!--<include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_
arm_gazebo_position.launch" /> -->
<!-- ros_control trajectory control dof arm launch file -->
<include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_
trajectory_controller.launch" />
<!-- moveit launch file -->
<include file="$(find seven_dof_arm_config)/launch/moveit_planning_
execution.launch" />
</launch>

• This launch file spawns the robot model in Gazebo, publishes the joint states,
  attaches the position controller, attaches the trajectory controller, and at last launches
  moveit_planning_execution.launch inside the MoveIt! package for starting the
  MoveIt! nodes along with RViz. We may need to load the MotionPlanning plugin in
  RViz if it is not loaded by default.
• 上面的启动文件会启动gazebo和RViz。

roslaunch seven_dof_arm_config seven_dof_arm_bringup_moveit.launch

• This will launch RViz and Gazebo, and we can do motion planning inside RViz. After motion planning, click on the Execute button to send the trajectory to the Gazebo controllers.