MoveIt-tutorial笔记

简介

MoveIt 的系统框架及介绍如下所示：
附上Moveit quick tutorial. Melodic-level, Moveit tutorials Kinetic-level下面进行的例程和该tuttorial的内容可能有些差异，因为原turorial安装的是git clone https://github.com/ros-planning/moveit_tutorials.git -b master版本，而我在下面安装的是git clone https://github.com/ros-planning/moveit_tutorials.git -b kinetic-devel版本。
附上:lauch file理解

1.安装

#Once you have ROS installed, make sure you have the most up to date packages

#update：当执行apt-get update时，update重点更新的是来自软件源的软件包的索引记录（即index files）。
#upgrade：当执行apt-get upgrade时，upgrade是根据update更新的索引记录来下载并更新软件包。
#dist-upgrade:当执行apt-get dist-upgrade时，除了拥有upgrade的全部功能外，dist-upgrade会比upgrade更智能地处理需要更新的软件包的依赖关系。

#rosdep update作用是同步/etc/ros/rosdep/sources.list.d和/etc/ros/rosdep/sources.list.d中列出的源的索引，以获取最新的包
rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get install ros-kinetic-catkin python-catkin-tools

#安装moveit

#默认安装至/etc/ros/kinetic处
sudo apt-get install ros-kinetic-moveit
#创建一个moveit的工作空间
mkdir -p ~/ws_moveit/src
#安装例程包
cd ~/ws_moveit/src
#建议运行下面这句，若失败才运行kinetic-level版本的语句
#git clone https://github.com/ros-planning/moveit_tutorials.git -b master
git clone https://github.com/ros-planning/moveit_tutorials.git -b kinetic-devel
git clone https://github.com/ros-planning/panda_moveit_config.git -b kinetic-devel

#安装依赖
#rosdep install -y --from-paths . --ignore-src --rosdistro kinetic
#1.若出现ERROR: the following rosdeps failed to install apt: command [sudo -H apt-get install -y ros-kinetic-franka-description] failed
#则可从source安装: git clone https://github.com/frankaemika/franka_ros.git -b kinetic-devel
#2.E: There were unauthenticated packages and -y was used without --allow-unauthenticated
#第二个问题不知如何解决，于是换了一个命令进行安装，结果可行
cd ~/ws_moveit
rosdep install --from-paths . --ignore-src --rosdistro=kinetic
rosdep install --from-paths src --ignore-src --rosdistro kinetic -y

#配置工作空间
cd ~/ws_moveit
#--extend: extended的工作空间相当于一个"父空间"，待会儿所创建的工作空间会依赖于此工作空间
#--cmake-args:cmake参数设置，D的意思是"定义，"这里将CMAKE_BUILD_TYPE"定义"为Release，可直接运行
#若"定义"为Debug，则执行文件可进行调试
catkin config --extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
#catkin build与catkin_make不一样,它会使得src中的每个包的编译都是独立的
catkin build

source ~/ws_moveit/devel/setup.bash

ROBOT_moveit_config package是机器人的相关配置文件包，是必须要有的，于此处使用的是panda_moveit_config,稍晚会介绍如何制作自己的机器人配置文件包。

#另一种安装方法
#moveit源安装
cd ~/ws_ooveit/src
wstool init .
wstool merge https://raw.githubusercontent.com/ros-planning/moveit/kinetic-devel/moveit.rosinstall
wstool update
rosdep install --from-paths . --ignore-src --rosdistro kinetic
cd ..
catkin config --extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build

2.RViz中的按钮效果

启动demo:

roslaunch panda_moveit_config demo.launch rviz_tutorial:=true

接下就是一下按钮的介绍了，第一步设置接口文件：robot Description、Planning Scene Topic、Planning Request的Planning Group、Planned Path的Trajectory Topic；第二步就是可视化机器人：Scene Robot、Planned Path中的Show Robot Visual ，Planning Request 的Query Start State、Query Goal State ；第三步就是与panda互动：改变那个坐标轴小球，可以改变机器人的初始和最终位置。
motion plannning的planning中的Use Collision-Aware IK勾选框勾选之后，可以避免碰撞的姿态，若不勾选，则在碰撞时候会显现红色。
点击motion planning的planning中的plan，则会出现Start到Goal的轨迹（确保Planned Path的Show Trail checkbox有勾选）
在窗口坐上边缘的"Panels"中有个motion planning-slider，可以分帧查看轨迹

3.Move Group C++ Interface

用户接口通常通过MoveGroupIterface class进行配置，其中有大多数常用的操作：specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot. 此接口通过MoveGroup Node通讯。

实验：

#启动RVIZ
roslaunch panda_moveit_config demo.launch

#写好的一些应用
roslaunch moveit_tutorials move_group_interface_tutorial.launch

press Next or use key N,来观察机器人的变化：

1. The robot moves its arm to the pose goal to its front.
2. The robot moves its arm to the joint goal at its side.
3. The robot moves its arm back to a new pose goal while maintaining
   the end-effector level.
4. The robot moves its arm along the desired Cartesian path (a triangle
   down, right, up+left).
5. A box object is added into the environment to the right of the arm.
   B
6. The robot moves its arm to the pose goal, avoiding collision with
   the box.
7. The object is attached to the wrist (its color will change to
   purple/orange/green).
8. The object is detached from the wrist (its color will change back to
   green). The object is removed from the environment.

相关类(moveit, planning_interface, robot_state)的成员变量与函数的接口介绍可以在roswiki找到，代码的解释在:

~/ws_moveit/src/moveit_tutorials/doc/move_group_interface/src/move_group_interface_tutorial.cpp

还有launch file:

#move_group_interface_tutorial.launch
<launch>

  <node name="move_group_interface_tutorial" pkg="moveit_tutorials" type="move_group_interface_tutorial" respawn="false" output="screen">
  </node>

</launch>

4.MoveIt Commander Scripting

moveit_commander python package包含了许多封装好的Moveit功能，如运动规划，笛卡尔轨迹的计算，抓取与放置。

#启动RVIZ
roslaunch panda_moveit_config demo.launch

rosrun moveit_commander moveit_commander_cmdline.py

使用示例：

use <group name>#指定操作的节点, 此处为panda_arm

current#显示group的状态

rec c#用变量c存储current状态

goal = c#复制c的值给goal
goal[0] = 0.2#修改关节1的值
go goal#规划运动，并且执行

goal[0] = 0.2
goal[1] = 0.2
plan goal#可视化轨迹
execute#执行轨迹运动

help#查询指令
quit#退出指令操作

5.Robot Model and Robot State

这一节主要介绍MoveIt中用于运动学规划的C++ API

RobotModel and RobotState 类是核心类。

RobotModel 包含了从URDF加载的机器人关节joints和连接件links的相互关系，它还将机器人的joints和links分解为SRDF所定义的planning groups。URDF和SRDF的分解教程如下所示：URDF and SRDF Tutorial

RobotState 包含了某一时刻的机器人状态：一个包含了关节的位置的向量，以及可选择地根据当前状态（末端执行器的Jacobian）获取关节的速度和加速度。它还包含了一些函数：基于末端执行器的位置(笛卡尔位姿)来设定机器手的位置，并计算运动轨迹（笛卡尔轨迹）。

程序实现的功能：

1. 读取机器人模型RobotModel，获取该机器人的RobotState
2. 读取panda_arm组的关节名字及关节值
3. 赋值给panda_arm的joints, 检测其是否超出关节限制
4. Enforce the joint limits for this state，矫正关节值，再检测是否超出关节限制
5. 正运动学实现：给定关节值，计算末端执行器的姿态
6. 逆运动学实现：给定末端执行器的姿态，计算各关节值
7. 获取雅可比矩阵

相关代码解释：

~/ws_moveit/src/moveit_tutorials/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp

Launch File有两件事需要完成：

1. 加载Panda机器人的URDF和SRDF文件至参数服务器
2. 将moveit安装助手生成的运动学解算器配置放到本教程中实例化类的节点的命名空间中的ros参数服务器上。

<launch>
  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <node name="robot_model_and_robot_state_tutorial"
        pkg="moveit_tutorials"
        type="robot_model_and_robot_state_tutorial"
        respawn="false" output="screen">
    <rosparam command="load"
              file="$(find panda_moveit_config)/config/kinematics.yaml"/>
  </node>
</launch>

调试Robot State：

rosrun moveit_ros_planning moveit_print_planning_model_info

6.Planning Scene

Planning Scene类提供了主要的接口：用于碰撞检测和约束检测。
运行程序：

roslaunch moveit_tutorials planning_scene_tutorial.launch

程序实现的功能：

1. 自我碰撞检测，重要的类(collision_detection::CollisionRequest, collision_detection::CollisionResult)，重要的方法planning_scene.checkSelfCollision(collision_request, collision_result)
2. 通过设定collision_request.group_name = "hand"，来检测特定组的是否有自我碰撞
3. 获取接触信息，即谁和谁相互碰撞
4. 利用AllowedCollisionMatrix来设定忽略碰撞的对象
5. 完整的碰撞检测planning_scene.checkCollision(collision_request, collision_result, copied_state, acm)，还会检测机器人与交互环境是否有碰撞，并且于此检测中会有一个安全距离，小于安全距离则为碰撞
6. 约束检测，包括运动学约束(关节、位置、朝向、可视化)和自定义的约束

程序解释：

~/ws_moveit/src/moveit_tutorials/doc/planning_scene/src/planning_scene_tutorial.cpp

Launch File:

<launch>
  <!-- send Panda urdf to parameter server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" />

  <include file="$(find panda_moveit_config)/launch/planning_context.launch"/>

  <node name="planning_scene_tutorial" pkg="moveit_tutorials" type="planning_scene_tutorial" respawn="false" output="screen">
    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
  </node>
</launch>

7. Planning Scene ROS API

此前的planning scene是在没有物体交互的环境下进行的，此处将介绍两种操作：

• 向环境中加入和移除物体(Adding and removing)
• 将物体附加和分离至机器人(Attaching and detaching)

程序运行：

roslaunch panda_moveit_config demo.launch

roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch

程序功能：

1. 将物体添加至环境(添加至planning_scene集合planning_scene.world.collision_objects.push_back(attached_object.object), 设定planning_scene.is_diff = true, 然后发布信息planning_scene_diff_publisher.publish(planning_scene)即可)
2. 将物体“附在”机器人身上(因为当机器人拾起物体时，需要让该该物体成为机器人的一部分，如此在碰撞检测中才可以一同考虑进去)(两步走：将物体从环境中移除，将物体附在机器人身上)
3. 将物体与机器人“分离”(两步走：分离机器人身上的物体，将移除的物体添加至环境中)
4. 将物体从环境中移除(planning_scene.world.collision_objects.push_back(remove_object)，发布信息即可)

详细的程序解释：

~/ws_moveit/src/moveit_tutorials/doc/planning_scene_ros_api/src/planning_scene_ros_api_tutorial.cpp

Launch File：

<launch>

  <node name="planning_scene_ros_api_tutorial" pkg="moveit_tutorials" type="planning_scene_ros_api_tutorial" respawn="false" output="screen">
  </node>

</launch>

Debugging the Planning Scene Monitor：

rosrun moveit_ros_planning moveit_print_planning_scene_info

8.Motion Planning API

在MoveIt中，运动规划是利用插件加载的。这样使得MoveIt实时加载运动规划，此处将运行C++代码来加载运动规划。主要利用planning_interface类
程序运行：

roslaunch panda_moveit_config demo.launch

roslaunch moveit_tutorials motion_planning_api_tutorial.launch

程序功能：
加载RobotModel和PlanningScene

1. 从ros参数服务器上获取planner运动规划器的planner_plugin_name接口名字，然后以该名字激活planner_plugin_loader运动规划加载器，最后用该加载器加载运动规划指针planning_interface::PlannerManagerPtr
   planner_instance，该指针便是后续主要调用的东西。
2. 利用moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");进行可视化，开启Next控制visual_tools.loadRemoteControl()
3. 设定pose goal与末端执行器的允许误差范围, 计算轨迹, 显示轨迹, 更新RobotModel和PlanningScene
4. 设定joint goal, 计算轨迹, 显示轨迹
5. 设定joint goal，设定路径约束req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion)，计算轨迹，显示轨迹

详细的程序解释：

~/ws_moveit/src/moveit_tutorials/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp

Launch File:

<launch>

  <node name="motion_planning_api_tutorial" pkg="moveit_tutorials" type="motion_planning_api_tutorial" respawn="false" output="screen">
    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
    <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
    <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
  </node>

</launch>

9.Motion Planning Pipeline

根据官方文档说是：在使用运动规划时，我们经常需要预处理Motion planning request和后期处理规划的路径(例如, 时间参数化)。于是Planning Pipeline应运而生啦，它可以帮我们将预处理, 运动规划, 后期处理串连起来，就像是一个Pipeline管道。而预处理和后期处理被称作planning request adapters运动规划适配器，可以由ROS参数服务器通过名字调用进行设置（adapters后面章节会讲到）。

程序运行：

roslaunch panda_moveit_config demo.launch
#in another shell
roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch

由于kinetic版本的LaunchFile与例程的Master版本不一样，它里面加了GUI进行joint调节等等，与本章内容想要传达的意思不太一致。于是我将kinetic版本的motion_planning_pipeline_tutorial.cpp和motion_planning_pipeline_tutorial.launch内容改成了master版本的内容，运行之后，一切ok.

程序功能：
实现的功能与上一章类似，它的主要改变在于：

1. 不同于上一章繁琐的初始化，此处用planning_pipeline进行初始化：

planning_pipeline::PlanningPipelinePtr planning_pipeline(
    new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

2. 轨迹计算也有变化：

//上一章的内容
//planning_interface::PlanningContextPtr context =
//    planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
//context->solve(res);//计算运动轨迹, 计算结果存放于res中

//本章的内容
// Before planning, we will need a Read Only lock on the planning scene so that it does not modify the world
// representation while planning
{
  planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
  /* Now, call the pipeline and check whether planning was successful. */
  planning_pipeline->generatePlan(lscene, req, res);
}

程序解释：

~/ws_moveit/src/moveit_tutorials/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp

Launch File:

<launch>
  <!-- Debug Info -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints" />

  <node name="motion_planning_pipeline_tutorial" pkg="moveit_tutorials" type="motion_planning_pipeline_tutorial" respawn="false" launch-prefix="$(arg launch_prefix)" output="screen">
    <param name="planning_plugin" value="$(arg planning_plugin)" />
    <param name="request_adapters" value="$(arg planning_adapters)" />
    <param name="start_state_max_bounds_error" value="0.1" />
  </node>
</launch>

10.Visualizing Collisions

本章将介绍在Rviz中移动和操作机器人机械臂时，如何可视化机器人自身, 物体和世界的碰撞接触点.

程序运行：

roslaunch moveit_tutorials visualizing_collisions_tutorial.launch

程序功能：

1. 作者使用了InteractiveRobot ,
   包含了robot模型, 立方体, 交互用的marker(用以移动物体或机器人)
2. 创建planning_scene用于碰撞检测
3. 提取InteractiveRobot的cube信息, 添加至planning_scene
4. 在Rviz中用交互marker移动机器人或者cube, 会产生消息类型, 发布至"interactive_robot_marray"话题, 该节点订阅该消息, 进入回调函数
5. 在回调函数中, 用新的InteractiveRobot信息更新planning_scene, 碰撞检测, 计算碰撞点,
   发布信息至InteractiveRobot进行Rviz显示。

程序解释：

~/ws_moveit/src/moveit_tutorials/doc/visualizing_collisions/src/visualizing_collisions_tutorial.cpp

Launch File:

<launch>

  <!-- load URDF -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>

  <!-- load SRDF -->
  <param name="robot_description_semantic" command="$(find xacro)/xacro --inorder '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'"/>

  <!-- Run RViz with a custom config -->
  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
	args="-d $(find moveit_tutorials)/doc/visualizing_collisions/launch/moveit.rviz" output="screen">
    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
  </node>

  <!-- If needed, broadcast static tf2 for robot root -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 1 /world/panda_link0 /panda_link0 10" />

  <!-- launch interactivity_tutorial -->
  <node name="visualizing_collisions_tutorial" pkg="moveit_tutorials" type="visualizing_collisions_tutorial" respawn="false" output="screen">
    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
  </node>

</launch>

11.Time Parameterization

MoveIt是一个运动学运动规划框架–它主要规划关节和末端执行器的位置，而不是速度或者加速度。但是，MoveIt确实使用了后期处理，对轨迹进行了time parameterization时间参数化处理。通俗来讲，时间参数化就是将一段运动路径转化为对于机器人而言可行的运动轨迹(可行意味着：考虑机器人的物理限制, 如速度, 加速度, 力矩限制，还有最小化特定的误差(如执行时间))

速度控制
From File：
MoveIt是根据机器人的URDF或者joit_limits.yaml设置关节运动速度和加速度的。 joint_limits.yaml是由Setup Assistant生成，并且初始化值由URDF提供。使用者可以自己设置它们的值，通过关键词max_position, min_position, max_velocity, max_acceleration设置关节属性，Joint limits关节限制可以通过has_velocity_limits, has_acceleration_limits进行开闭。
During Runtime：
参数化的运动轨迹在运行期间是可以修改的，修改方式是通过一个最大速度和加速度的比例(0-1) 来修改。用以改变的消息如 MotionPlanRequest.msg所示；还可以用Spinboxes进行设置，如MoveIt MotionPlanning RViz plugin所示。

时间参数化算法
MoveIt支持用不同的算法来添加时间戳和设置速度/加速度值，以对运动轨迹进行后期处理。通常有以下方法：

• Iterative Parabolic Time Parameterization
• Iterative Spline Parameterization
• Time-optimal Trajectory Generation

12.Planning with Approximated Constraint Manifolds

在前面章节我们知道，我们可以施加约束给关节空间或者是笛卡尔坐标空间。当规划路径时，每个关节状态必须通过rejection sampling拒绝采样(一种仿真方法, 用以从分布中产生观察) 来遵从约束。如此将导致过长的规划时间，尤其当约束十分严重并且rejection rate相对比较高的时候，规划所需的时间更长。

sucan等人提出了一种预先计算constraint manifold约束流形的近似值并进行轨迹规划的方法。ompl插件包含对给定约束集执行此操作并将其保存到数据库中的功能。在以后的实例中，数据库可以被加载到用于约束规划的任何OMPL规划器中，这大大减少了规划时间。

这个例程包含了如何构建constraint approximation database约束估计数据库，以及如何使用它来规划带约束的轨迹路径。

(有待补充)

13.Pick and Place

为了抓取一个物体，我需要创建moveit_msgs::Grasp消息来定义各种各样的抓取姿态和姿势。

程序运行：

roslaunch panda_moveit_config demo.launch
rosrun moveit_tutorials pick_place_tutorial

程序功能：

1. 添加collision障碍物std::vector collision_objects
2. 抓取操作: 设定夹持器的目标位姿(在物体后面), 在执行预抓取路径前打开夹持器, 设定预抓取路径(向前移动), 关闭夹持器, 设定抓取完成后的运动路径(向上移动)。以上操作都是通过设定std::vector grasps其中的属性来完成的，然后利用planning_interface:: MoveGroupInterface& move_group进行运动规划, 执行抓取操作:

// Set support surface as table1.
move_group.setSupportSurfaceName("table1");
// Call pick to pick up the object using the grasps given
move_group.pick("object", grasps);

3. 放置操作: 同样通过设定std::vector grasps其中的属性进行操作，最后利用place进行运动规划，执行放置操作:

// Set support surface as table2.
group.setSupportSurfaceName("table2");
// Call place to place the object using the place locations given.
group.place("object", place_location);

程序解释：

~/ws_moveit/src/moveit_tutorials/doc/pick_place/src/pick_place_tutorial.cpp

Launch File:
这个是一个执行文件, 没有launch file, 执行文件在:

~/ws_moveit/devel/lib/moveit_tutorials/pick_place_tutorial

14.MoveIt Grasps

MoveIt Grasps是一个抓取生成器，可以适用于块状, 圆柱体一些规则的物体抓取，也可以用来替代MoveIt的pick and place pipeline. MoveIt Groups提供了许多抓取功能，包括通过可达性和笛卡尔路径规划, 夹起和恢复运动来选择合适的抓取操作。 抓取生成算法仅仅考虑物体的形状, 而不考虑摩擦力和一些抓取动力学问题。
MoveIt Grasps可以被用于平行的手指夹具或者是吸盘型夹具

下载安装Moveit Grasps包:

cd ~/ws_moveit/src
git clone -b $ROS_DISTRO-devel https://github.com/ros-planning/moveit_grasps.git
rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO
cd ..
catkin build

程序运行:

1. The Entire MoveIt Grasps Pipeline

#before any of the Demos, start Rviz first
roslaunch moveit_grasps rviz.launch

2. Visualize Gripper Parameters

roslaunch moveit_grasps grasp_poses_visualizer_demo.launch

• retreat and pregrasp:即机械手预抓取和恢复时候的姿态位置
• Grasp Pose and lifted grasp: 即末端执行器执行闭合抓取时候的姿态
• Object Pose and EE(end effector) pose: 即物体的真实姿态和末端执行器的姿态

3. Visualize Grasp Generation

roslaunch moveit_grasps grasp_generator_demo.launch

这个程序展示了MoveitGrasp在生成抓取时，是如何进行选择的，几个方法如下：

• Ideal TCP(robot Tool Center Point) Grasp Pose: 抓取生成器会偏重选择用户更钟意的抓取姿态, 例如用户会说: “我想让末端执行器指向左上方.”
• Pose-Based Grasp Score Weighting: 通过对特定坐标轴进行加权, 使得特定的平移或者旋转轴所占的得分权重更高
• Depth-Based Grasp Score Weighting: 加权使夹持器更深位置的权重
• Width-Based Grasp Score Weighting: 加权使得夹持器手指和物体距离更宽位置的权重
• Overhang Grasp Score Weighting: (针对吸盘式抓取器)加权吸取器与物体的覆盖面积权重.这个面积由它们重叠百分比来表示.

4. Grasp Filter

roslaunch moveit_grasps grasp_filter_demo.launch

抓取过滤器的颜色表示什么:

• RED- ik(逆运动学)过滤抓取
• PINK- collision(碰撞)过滤抓取
• MAGENTA紫红色- cutting plane(切割平面法)过滤抓取(切割平面法其实就是增加不等式约束, 为"候选抓取"这个可行集增添不等式约束, 相当于在可行集这个"平面"上画上几条"斜线"进行"分割")
• YELLOW- orientation(朝向)过滤抓取
• BLUE- ik(逆运动学)过滤预抓取
• CYAN青色- collision(碰撞)过滤预抓取
• GREEN- 有效

概念综述:

MoveIt Grasps有三个重要的组成成分 :

• Grasp Generator: 利用末端执行器的运动学和物体形状来采样抓取姿态, 并且利用几何学评价方法来优化这些抓取姿态
• Grasp Filter: 利用IK逆运动学对候选抓取姿态进行求解, 验证此些姿态的有效性
• Grasp Planner: 计算笛卡尔路径, 抓起和恢复轨迹来形成一段完整的抓取运动

注意:Grasp Planner将会逐步淘汰, 因为MoveIt Task Constructor 的兴起
另外, Grasp Generator还用了Grasp Scorer来提供一些先验, 来选择用户钟意的抓取姿态。
整个 generating, filtering and planning grasp motions的pipeline:

~/ws_moveit/src/moveit_grasps/src/demo/grasp_pipeline_demo.cpp

Robot-Agnostic Configuration机器人配置 ：

MoveIt Grasps需要两个配置文件:

• ROBOT_grasp_data.yaml ： 描述了末端执行器的几何特征, 每个机器人都不一样
• *moveit_grasps_config.yaml * ： 配置 Grasp Generator, Grasp Filter and Grasp Planner, 该文件还包含了许多debugging and visualization选项(可以使得MoveIt Grasps的使用更加快速便捷)

例程中Franka Emika’s Panda的配置文件:

~/ws_moveit/src/moveit_grasps/config_robot/panda_grasp_data.yaml
~/ws_moveit/src/moveit_grasps/config/moveit_grasps_config.yaml

Applying Robot-Agnostic Configuration应用机器人配置 ：

为了应用yaml配置文件, 需要将它们用应用程序的节点加载至ros参数服务器, 以下是~/ws_moveit/src/moveit_grasps/launch/grasp_pipeline_demo.launch的一部分程序:

<!-- Start the demo -->
<node name="moveit_grasps_demo" launch-prefix="$(arg launch_prefix)" pkg="moveit_grasps"
type="moveit_grasps_grasp_pipeline_demo" output="screen" args="$(arg command_args)">
  <param name="ee_group_name" value="hand"/>
  <param name="planning_group_name" value="panda_arm"/>
  <rosparam command="load" file="$(find moveit_grasps)/config_robot/panda_grasp_data.yaml"/>
  <rosparam command="load" file="$(find moveit_grasps)/config/moveit_grasps_config.yaml"/>
</node>

注意！机器人的planning group和end effector group必须额外指定, 使用ee_group_name and planning_group_name.

Additional Configuration Notes其它配置注意事项 :

tcp_to_eef_mount_transform:

tcp_to_eef_mount_transform代表了工具中心点到末端执行器的转换. 这个参数可以帮助不同URDF末端执行器共同工作, 而不用重新编译代码.
在MoveIt当中, 末端执行器应该有一个父节点, 可能是手腕关节或者手掌关节。这个关节的Z轴应当指向物体, 这样才能够成功抓取。
这个设定应当是墨守成规的, 但是许多URDFs没有遵从这个准则, 所以tcp_to_eef_mount_transform经常用于解决这个问题。另外, Y轴应当指向夹持器其中一个手指.

Switch from Bin to Shelf Picking：

GraspGenerator中的方法setIdealGraspPoseRPY()和setIdealGraspPose()可以被用来选择一个理想的抓取朝向，这些方法通过评分候选区域来选择理想的抓取姿态。
这些方法对于在垃圾桶和货架进行抓取十分有效,** 因为我们可能想要垂直对齐垃圾桶或者水平对齐货架, 来进行抓取。**

15.Subframes

Subframes由CollisionObjects所定义,它们可用于定义放置在场景中的对象上的兴趣点，例如瓶子的开口、螺丝刀的尖端或螺钉的头部。 它们可用于规划和编写机器人指令，如“拿起瓶子，然后移动水龙头喷嘴下的开口”或“拿起螺丝刀，然后将其放在螺钉头上方”。

这章教程主要介绍如何在物体上定义subframes, 并将其发布至planning scene场景规划中, 以及利用它们进行运动规划。

Kinect版本中无此教程

程序运行:

roslaunch panda_moveit_config demo.launch
rosrun moveit_tutorials subframes_tutorial

16.MoveIt Setup Assistant

MoveIt! Setup Assistant是一个图形用户界面, 用来配置所有将要使用MoveIt的机器人。它的主要作用就是生成一个所用机器人的Semantic Robot Description Format (SRDF) file.详细操作可参照此处, 该笔记仅仅记录功能。

程序运行：

sudo apt-get install ros-kinetic-franka-description
#删除之前安装的 panda_moveit_config,此处将教你如何从头建立
cd ~/ws_moveit/src
rm -rf panda_moveit_config
catkin clean panda_moveit_config

Step1：Start

#To start the MoveIt! Setup Assistant:
roslaunch moveit_setup_assistant setup_assistant.launch

选择Create New MoveIt! Configuration Package, 再选择/opt/ros/kinetic/share/franka_description/robots/panda_arm_hand.urdf.xacro, Load Files

Step2: Generate Self-Collision Matrix

• 默认的碰撞免检矩阵生成器搜索机器人所有关节，这个碰撞免检矩阵是可以安全地关闭检查，从而减少行动规划的处理时间

• 在某些关节会关闭碰撞检查，比如总是碰撞，从不碰撞，在默认的位置碰撞，或在运动学链条上的相邻处。

• 采样密度指定了多少个随机机器人位置来检查碰撞。更高的密度需要更多的计算时间，而较低的密度就要减少关闭的检查节点

• 默认值是10000个碰撞检查。碰撞检查是并行完成，以减少处理时间。

Step3: Add Virtual Joints

• 虚拟关节主要用于将机器人连接到世界上。

• 我们将为PR2定义了一个虚拟节点连接PR2的base_footprint和odom_combined世界框架。

• 这个虚拟的关节表示在一个平面上的机器人基座的运动。

Step4: Add planning Groups

就是描述机器人不同部分的名字ID, 如手臂ID, 末端执行器ID。还可以设置它们的运动学解算器。然后添加joints或者links.

Step5: Add Robot Poses
用来设置一个固定姿态。例如，可以给机器人手臂定义一个特定的姿态，并命名为Home, 则可以通过调用这个ID来调整机器人姿态。
Tips: 试着移动所有关节。如果您的urdf中的联合限制有问题，您应该能够立即在这里看到它。

Step6: Label End Effectors
指定了末端执行器后，可以允许它们进行一些特殊的内部操作.

Step7: Add Passive Joints

• 被动关节标签意味着允许任何存在于机器人的被动关节的规范
• 这告诉规划者不能规划这些被动节点。
• 被动关节的例子包括被动脚轮
• Panda没有被动关节，所以我们将跳过这一步。

Step8: 3D Perception
用来设置YAML配置文件，来配置3D传感器sensors_3d.yaml

Step9: Gazebo Simulation
这个tab可以帮助你通过生成一个新的Gazebo compatible urdf，来在gazebo中进行机器人仿真, 绿色部分是需要被修改的部分。
可以使用生成的robot urdf在gazebo中启动robot:

roslaunch gazebo_ros empty_world.launch paused:=true use_sim_time:=false gui:=true throttled:=false recording:=false debug:=true

rosrun gazebo_ros spawn_model -file </path_to_new_urdf/file_name.urdf> -urdf -x 0 -y 0 -z 1 -model panda

Step10: ROS Control
这个是一系列包，包含了: controller interfaces控制器界面, controller managers控制器管理器, transmissions and hardware_interfaces发送和硬件接口。文档
ROS control tab可以用来自动生成仿真器来驱动机器人的关节

Step 11:Add Author Information

Step12: Generate Configuration Files

17.URDF and SRDF

URDF
URDF名为Universal Robot Description Format, ROS中用于描述机器人的文件。
urdf由许多不同的功能包和组件构成。它们的关系如下图所示：

URDF Resources:
URDF ROS Wiki Page
URDF Tutorials
SOLIDWORKS URDF Plugin
深蓝学院简易教程

Tips:

• Joints名称中不能够有-, [, ], (, )这些符号
• 有些URDFs有针对机器人关节joints的安全距离设置, 针对Panda head pan joint的一个例子:
  
  soft_lower_limit和soft_upper_limit就像是丝杠导轨机构中的软行程, 由软件编程规定的一个行程, 硬行程则由UEDF文件中所定义, 两者比较取最小值。(所以若软行程设为0的话, 则关节将不能运动)
• Moveit利用URDF中定义的mesh来检测碰撞, URDF可以定义两种meshes, 一种用来可视化,
  一种用来碰撞检测，通常来讲，可视化的mesh应该更加详细，用于碰撞检测的mesh应该不那么详细，应为mesh中的三角形数量将会影响碰撞检测的运行时间。
• 检测URDF是否正确:check_urdf file.urdf

SRDF

SRDF用以完善URDF并且可以指定关节组, 设定默认的机器人配置, 额外的碰撞检测以及额外的姿态转换关系。仔细的功能如上一章所述。

18. Low Level Controllers

这章将介绍moveit中的controllers, 与机器人的硬件层通信，用以控制机器人的运动。本章假设机器人的手臂有 FollowJointTrajectoryaction service, (optionally)机器人的夹具有GripperCommandservice. 若没有, 可以通过ROS control架构简单地添加至硬件通信层。ROS control的架构如下所示：
在robot_moveit_config/config中加入controllers.yaml:

controller_list:
 - name: panda_arm_controller #controller的名字
   action_ns: follow_joint_trajectory #action命名空间
   type: FollowJointTrajectory #action类型
   default: true #default controller就是由MoveIt首选的控制器, 用来与特定的关节进行通信
   joints: #所有受控制的关节名字
     - panda_joint1
     - panda_joint2
     - panda_joint3
     - panda_joint4
     - panda_joint5
     - panda_joint6
     - panda_joint7
 - name: hand_controller
   action_ns: gripper_action
   type: GripperCommand
   default: true
   parallel: true # 这个设置为true后, joints必须是两个, 而且命令将会两个joints的总和 
   joints:
     - panda_finger_joint1
     - panda_finger_joint2

可选择的参数: 设置合法轨迹执行的持续时间：

controller_list:
 - name: arm_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   #这两个参数重载了全局变量trajectory_execution/allowed_execution_duration_scaling and trajectory_execution/allowed_goal_duration_margin
   #与全局变量不同, 于此处设置之后则不能动态地改变
   #这两个参数的作用是调整轨迹执行持续时间，若时间超出限制则轨迹会被取消
   allowed_execution_duration_scaling: 1.2
   allowed_goal_duration_margin: 0.5

Create the Controller launch file
设置ros参数服务器，让trajectory_execution_manager找得到控制器接口controllers.yaml

#robot_moveit_controller_manager.launch.xml
<launch>
 <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
 <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 <!-- load controller_list -->
 <rosparam file="$(find robot_moveit_config)/config/controllers.yaml"/>
</launch>

Debugging Information
在上述例子中，所创建的topics应该是/name/action_ns, 如下所示：

/panda_arm_controller/follow_joint_trajectory/goal
/panda_arm_controller/follow_joint_trajectory/feedback
/panda_arm_controller/follow_joint_trajectory/result
/hand_controller/gripper_action/goal
/hand_controller/gripper_action/feedback
/hand_controller/gripper_action/result

通过rostopic info topic_name应该可以看到topics被机器人的controllers和move_group node published/subscribed.

Remapping /joint_states topic
当运行[move group node]时候，需要将话题/joint remap重新映射给/robot/joint, 否则Moveit不能够得到joints的反馈。可以写一个简单的Launch file如下：

<node pkg="moveit_ros_move_group" type="move_group" name="any_name" output="screen">
  <remap from="joint_states" to="robot/joint_states"/>
</node>

或者可以让move group订阅该话题的信息，在回调函数中更新move group的robot state.

Trajectory Execution Manager Options
有几种选择来调整Moveit中执行pipeline的表现和安全检测, 在robot_moveit_config/launch/trajectory_execution.launch.xml中修改以下参数:
execution_duration_monitoring: 如果设置为false, 则无论轨迹执行的持续时间多长都不会报错
allowed_goal_duration_margin: 超过规定时间多少，才取消轨迹执行
allowed_start_tolerance: 允许的关节公差(轨迹的第一个点和机器人的当前状态)，如果设置为零，将跳过执行轨迹后等待robot停止的过程.

19.Perception Pipeline Tutorial

Moveit可以通过使用Octomap来无缝连接3D传感器的数据。这一章将介绍如何配置机器人的3D传感器。moveit中处理3D perception的主要组件是Occupancy Map Updater, 这个Updater使用了一个接口结构来处理不同类型的输入，接口如下：

• The PointCloud Occupancy Map Updater: which can take as input point
  clouds (sensor_msgs/PointCloud2)
• The Depth Image Occupancy Map Updater: which can take as input Depth
  Images (sensor_msgs/Image)

Configuration

YAML Configuration file (Point Cloud)
3D传感器的配置需要一个YAML配置文件，针对Kinect的点云配置文件如下:

#sensors_kinect_depthmap.yaml
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater#接口名字
    point_cloud_topic: /camera/depth_registered/points#得到点云数据的话题命
    max_range: 5.0#in m, 超过这个范围的点不会被使用
    point_subsample: 1# Choose one of every point_subsample points.
    padding_offset: 0.1#The size of the padding (in cm).
    padding_scale: 1.0#The scale of the padding.
    max_update_rate: 1.0#octomap更新的最大频率
    filtered_cloud_topic: filtered_cloud#The topic on which the filtered cloud will be published (mainly for debugging). The filtering cloud is the resultant cloud after self-filtering has been performed.

YAML Configuration file (Depth Map)
针对KInect的深度图：

#sensors_kinect_depthmap.yaml
sensors:
  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
    image_topic: /camera/depth_registered/image_raw
    queue_size: 5#允许排队的图片数量
    near_clipping_plane_distance: 0.3#The minimum distance before lack of visibility.
    far_clipping_plane_distance: 5.0#The maximum distance before lack of visibility.
    shadow_threshold: 0.2#The minimum brightness of the shadow map below an entity for its dynamic shadow to be visible
    padding_scale: 4.0#The scale of the padding.
    padding_offset: 0.03#The size of the padding (in cm).
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

Update the launch file
Add the YAML file to the launch script
向之前生成的SRDF-panda_moveit_config的launch中添加该句(若使用point cloud)

Octomap Configuration
加入至sensor_manager.launch:

#MoveIt使用一个基于八叉树的结构来表示的世界。以下是Octomap的配置参数意义
#1.指定一个参考坐标系，如果是移动机器人，则这个frame最好是固定在世界中的
#2.octomap的分辨率(m)
#3.传至该节点的传感器输入数据的最大值范围
<param name="octomap_frame" type="string" value="odom_combined" />
<param name="octomap_resolution" type="double" value="0.05" />
<param name="max_range" type="double" value="5.0" />

Obstacle Avoidance
如果设置了机器人位置的起点和终点，机器人会根据planner自动规避octomap，生成理想的路径。

程序运行:

roslaunch moveit_tutorials obstacle_avoidance_demo.launch
roslaunch moveit_tutorials detect_and_add_cylinder_collision_object_demo.launch

遇到如下错误：

ros.moveit_ros_planning.planning_scene_monitor: Transform error: Lookup would require extrapolation into the future.  Requested time 1527473962.793050157 but the latest data is at time 1527473962.776993978, when looking up transform from frame [panda_link2] to frame [camera_rgb_optical_frame]
ros.moveit_ros_perception: Transform cache was not updated. Self-filtering may fail.

PCL tutorial
(未完待续)

20.IKFast Kinematics Solver

IKFast: 来自维基百科解释：ikfast，机器人运动学的编译器，由Rosen Diankov OpenRAVE运动规划软件提供，是一个强大的逆运动学求解器。不像大多数的逆运动学求解，ikfast可以求解任意复杂运动链的运动学方程，并产生特定语言的文件（如C++）后供使用。最终的结果是非常稳定的解决方案，在最新的处理器能以5微秒的速度完成运算。

MoveIt! IKFast：一种可利用OpenRAVE生成的cpp文件来生成 IKFast 运动学插件的工具，MoveIt! IKFast测试环境是ROS Kinetic，使用catkin编译，使用带有6dof和7dof的机器臂的OpenRave 0.8 理论上可以工作在任何自动度的机械臂，但目前 IKFast插件生成器不能工作在大于7自动度的机械臂上

程序运行：

#Make sure you have these programs installed:
sudo apt-get install cmake g++ git ipython minizip python-dev python-h5py python-numpy python-scipy qt4-dev-tools
#You may also need the following libraries:
sudo apt-get install libassimp-dev libavcodec-dev libavformat-dev libavformat-dev libboost-all-dev libboost-date-time-dev libbullet-dev libfaac-dev libglew-dev libgsm1-dev liblapack-dev liblog4cxx-dev libmpfr-dev libode-dev libogg-dev libpcrecpp0v5 libpcre3-dev libqhull-dev libqt4-dev libsoqt-dev-common libsoqt4-dev libswscale-dev libswscale-dev libvorbis-dev libx264-dev libxml2-dev libxvidcore-dev
#To enable the OpenRAVE viewer you may also need to install OpenSceneGraph-3.4 from source:
sudo apt-get install libcairo2-dev libjasper-dev libpoppler-glib-dev libsdl2-dev libtiff5-dev libxrandr-dev
git clone https://github.com/openscenegraph/OpenSceneGraph.git --branch OpenSceneGraph-3.4
cd OpenSceneGraph
mkdir build; cd build
cmake .. -DDESIRED_QT_VERSION=4
make -j$(nproc)
sudo make install
#For IkFast to work correctly, you must have the correct version of sympy installed:
pip install --upgrade --user sympy==0.7.1
#You should not have mpmath installed:
sudo apt remove python-mpmath

MoveIt! IKFast Installation
Binary install:

sudo apt-get install ros-kinetic-moveit-kinematics

或者从source安装, 在catkin workspace:

#这个包里包含了kinematics
git clone https://github.com/ros-planning/moveit.git

OpenRAVE Installation
Source Install:

#安装第一句时候就fatal: unable to access 'https://github.com/rdiankov/openrave.git/': Operation timed out after 0 milliseconds with 0 out of 0 bytes received
git clone --branch latest_stable https://github.com/rdiankov/openrave.git
cd openrave && mkdir build && cd build
cmake -DODE_USE_MULTITHREAD=ON -DOSG_DIR=/usr/local/lib64/ ..
make -j$(nproc)
sudo make install

于是从ubuntu的官方源进行安装

sudo add-apt-repository ppa:openrave/release
sudo apt-get update
sudo apt-get install openrave

未完待续…