Movelt为使用者提供了一个最通用且简单的接口 MoveGroupInterface 类，这个接口提供了很多控制机器人的常用基本操作，如：
- 设置机械臂的位姿
- 进行运动规划
- 移动机器人本体
- 将物品添加到环境 / 从环境移除
- 将物体绑定到机器人 / 从机器人解绑
这个接口通过ROS话题topic、服务service和动作action等机制与 MoveGroup 节点进行通信。
1. 执行示例代码
1.1 运行过程
cd ~/ARM/ws_moveit/
source devel/setup.bash
roslaunch panda_moveit_config demo.launch
新开一个终端，在相同目录下执行
source devel/setup.bash
roslaunch moveit_tutorials move_group_interface_tutorial.launch
1.2 debug 过程
1.2.1 visual_tools 缺包
在运行上面的第二个命令时，我发生报错：
[move_group_interface_tutorial-1] process has died [pid 10609, exit code 127, cmd /home/zzrs234/ARM/ws_moveit/devel/lib/moveit_tutorials/move_group_interface_tutorial __name:=move_group_interface_tutorial __log:=/home/zzrs234/.ros/log/fe0cbdfa-08cc-11ed-8c8a-bc6ee239de01/move_group_interface_tutorial-1.log].
log file: /home/zzrs234/.ros/log/fe0cbdfa-08cc-11ed-8c8a-bc6ee239de01/move_group_interface_tutorial-1*.log
因为是 devel 包中的问题，所以，我重新编译：
zzrs234@zzrs234:~/ARM/ws_moveit$ catkin build
发现果然有错误:
Could not find a package configuration file provided by
"moveit_visual_tools" with any of the following names:
moveit_visual_toolsConfig.cmake
moveit_visual_tools-config.cmake
这种错误很经典，就是缺包，执行以下命令进行安装：
sudo apt-get install ros-melodic-moveit-visual-tools# ubuntu18.04
再重新编译，运行上面 1.1 的命令即可。
1.2.2 只显示路径而机械臂不到位
在初步执行时，发现机械臂只运行一次，留下一个绿色带点的路径，就又复归原来位置，对于将物品附加到机械臂的效果体现不明显（因为机械臂回到了原位置）。
这是因为只是规划而不是执行，只会留下路径。
1.3 预期效果
在RViz中，我们应该能够看到以下效果：
机器人将其手臂移动到其前方的目标姿态。
机器人将其手臂移动到其侧面的目标姿态。
机器人将其手臂移到一个新的目标姿态，同时保持末端执行器水平。
机器人沿着设置好的笛卡尔路径（一个三角形，下、右、上+左）移动手臂。
将长方体对象添加到手臂右侧的环境中。
机器人将其手臂移动到目标位姿，越过箱子，避免与箱子碰撞。
该对象附加到腕部（末端爪的侧面），并将其颜色将更改为紫色/橙色/绿色。
物体从腕部分离（其颜色将变回绿色）。
将箱子将从环境中删除。
2. 代码分析
实现这个功能的代码在~/ARM/ws_moveit/src/moveit_tutorials/doc/move_group_interface/src下，Github地址为：完整代码地址。
下面我们一步一步分析一下 Movelt 是如何实现这些操作的。
2.1 Setup 初始化
这部分是对 Movelt 的几个接口类的初始化。
2.1.1 MoveGroupInterface
MoveIt编程 是对 规划组 planning groups 进行操作的。
//创建接口对象 PLANNING_GROUP
static const std::string PLANNING_GROUP = "panda_arm";
MoveGroupInterface类只需放入 想要控制和规划的 planning group 的名称 即可设置。
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
2.1.2 PlanningSceneInterface
PlanningSceneInterface 类将被用来在 "virtual world" 场景中添加和删除碰撞对象 :
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
2.1.3 JointModelGroup
创建 MoveGroupInterface 对象后，Planning Group 会被保存到 JointModelGroup 对象。所以在 MoveIt 编程中通常会把 planning group 叫做 joint model group。
构建一个指针指向 JointModelGroup 对象，有些情况下会直接对 joint model group 进行操作。
// 获取当前关节状态
//调用 getJointModelGroup 将位置姿态 放入 joint_model_group，这个后续2.6会用。
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
2.2 Visualization 可视化
MoveItVisualTools包是一个在 Rviz 中可视化的工具包。比如：
- 物体 | 机器人 | 轨迹 | 调试信息
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
远程控制 Remote control 是一种自省工具 (introspection tool )，可以通过RViz中的按钮和键盘快捷键逐步完成高级功能。
visual_tools.loadRemoteControl();
RViz提供了多种类型的标记markers，用于记录历史轨迹和表征信息。在本演示中，我们将使用文本 test、圆柱体 cylinders,和球体 spheres：
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
批量发布trigger 可以减少发送到RViz进行可视化的消息数量：
visual_tools.trigger();
2.3 打印基本信息
我们可以打印一些基本信息到终端里显示出来：
参考系的名字
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());机器人的末端执行器
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());机器人的规划组
ROS_INFO_NAMED("tutorial", "Available Planning Groups:"); std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));
2.4 Start the demo
调用 visual_tools 的 prompt 提示函数 来开始操作：
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
后续执行完一次规划都会来这么一句，在 RViz 中进行交互。
2.5 Planning to a Pose goal
2.5.1 设置目标末端姿态
可以为规划组(上面指定过为 panda_arm) 指定一个末端执行器的目标位姿（就是机械爪，end-effector）：
//设置目标位姿
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);
接下来，就可以调用规划器来计算、规划并将其可视化。请注意，我们只是在规划，而不是调用 move_group 实际移动机器人。
//调用规划器进行规划
//创建规划器
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
//成功标志位
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
//输出
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
2.5.2 规划可视化
还记得上面 2.2 中的 markers 吗？
我们还可以在RViz 中将规划轨迹用 markers 展示出来：
//可视化规划轨迹
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
//markers创建
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
//画线
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
//完成一次规划，与RViz 界面进行交互，使其点击 next 再继续执行。
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
2.5.3 移动到目标姿态
移动到姿势目标与上述步骤类似，只是我们现在使用move()函数。请注意，之前设置的目标姿势仍处于活动状态，因此机器人将尝试移动到该目标。
本教程中不会使用该函数，因为它是一个阻塞函数，需要 控制器controller（也就是真实硬件） 处于活动状态并报告轨迹执行成功。
/* Uncomment below line when working with a real robot */
/* move_group.move(); */
//这里注释掉因为是模拟，没有在硬件上执行，调用的话会阻塞。
2.6 Planning to a joint-space goal
除了上面提到的 Pose Goal，还有 Joint-space Goal ，这种方式通过在关节空间定义一个目标关节状态的方式进行控制。
2.6.1 设置目标关节姿态
首先，创建一个指向 RobotState 的指针 current_state 。RobotState 记录了 Planning Group 中各个关节的 位置 / 速度 / 加速度数据
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();接下来，获取当前状态下的关节位置，关节位置是由 弧度radians 来表征的：
std::vector<double> joint_group_positions; //调用copyJointGroupPositions将之前的位姿拷贝到关节数组中。 current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);现在，让我们修改数组中其中一个关节的值，规划到新的关节空间目标 joint-space goal，并将规划可视化。
joint_group_positions[0] = -1.0; // radians，弧度制 //将新得到的关节数组设为目标姿态 move_group.setJointValueTarget(joint_group_positions); //规划，返回标志位 success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
2.6.2 规划可视化
//删除之前的markers
visual_tools.deleteAllMarkers();
//新画一个start、target、trajectory
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
//批量发布
visual_tools.trigger();
//交互
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
2.7 路径约束下的规划
Planning with Path Constraints.
2.7.1 设置路径约束
在实际情况中，机械臂的路径常常是有约束的，下面使用 OrientationConstraint 为机械臂添加约束：
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
将上面规定好的约束添加到规划组：
moveit_msgs::Constraints test_constraints;
//将约束添加到约束条件
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
2.7.2 设置目标姿态
下一步进行规划，我们这里再次使用之前定义的目标姿态target_pose1。
因为在路径约束下，仅当当前状态满足路径约束时，操作才有效。因此，我们需要将开始状态设置为一个新的姿态来确保符合约束。
robot_state::RobotState start_state(*move_group.getCurrentState());
//设置一个新的初始姿态
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);
2.7.3 规划可视化
现在，我们将从上面新的start state开始规划之前的target_pose1。
move_group.setPoseTarget(target_pose1);
2.7加入了路径约束的规划会比较慢，因为每个样本都必须调用逆运动学解算器。所以把规划时间从默认的5秒增加到10秒，确保规划器有足够的时间规划成功。
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
在RViz 中可视化表达。
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
2.8 执行笛卡尔路径
笛卡尔路径 Cartesian Paths.
2.8.1 规划路径
以 **2.5节 的规划方法 **，通过指定末端效应器要通过的路径点 waypoints 的列表 list，可以直接规划一段笛卡尔路径。
注意，我们从上面的新开始状态开始。初始位姿（start state）不需要添加到 waypoints 列表中，但把它加进去就可以在RViz 中显示出来：
//waypoints 是一个变长的路点数组，数组中每个点的类型是 geometry_msgs::Pose 类型
std::vector<geometry_msgs::Pose> waypoints;
//起始点
waypoints.push_back(start_pose2);
//初始化 目标点
geometry_msgs::Pose target_pose3 = start_pose2;
//第一段
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3); // 向下
//第二段
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); //向右
//第三段
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // 左上
//这样waypoints 就存入了一个三段两折的轨迹
2.8.2 调节关节速度
对于 接近物体 和 后退抓取 这些动作，笛卡尔运动通常较慢。我们通过降低每个关节最大速度的比例因子来降低机器人手臂的速度：
move_group.setMaxVelocityScalingFactor(0.1);
2.8.3 设置差值步长
如果我们要让笛卡尔路径以1厘米的分辨率插值，就将笛卡尔平移中的最大步长指定为0.01。具体操作为：调用computeCartesianPath()按一定步长插补得到一个plan。
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
//这里的步长eef_step设置为0.01，路径的分辨率就是1cm。
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
2.8.4 规划可视化
在RViz 中显示出来上面的规划：
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
2.9 添加/去除 物体 && 附着/解绑 物体
以代码顺序介绍添加物体、避障规划、将物体附着在机械臂上，解绑，删除物体的操作实现。
2.9.1 设置物体
利用 moveit_msgs::CollisionObject 定义一个物体 collision object
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
给物体起一个名字：
collision_object.id = "box1";
描述这个物体的大小形状：
shape_msgs::SolidPrimitive primitive;
//规定它是一个盒子（立方体）
primitive.type = primitive.BOX;
//长宽高
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
设置这个盒子的姿态：
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
//把上面设置的大小和姿态都放入 collision_object的属性
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
//用CollisionObject 类中的 operation属性add表示添加到场景中，此外还有 REMOVE删除，APPEND附加，MOVE移动
collision_object.operation = collision_object.ADD;
//把这个物体放入一个数组，便于后续增添
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
2.9.2 加入环境并显示
添加到环境中
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
RViz 显示结果：
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
2.9.3 避障规划
这样我们规划一个运动时，解算器就会把这个物体考虑进去，生成避开这个物体的路径：
调用的函数与此前的路径规划相同。
//以当前状态作为start state
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
//以2.5 中的规划设置为 target state
move_group.setPoseTarget(another_pose);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");
2.9.4 规划可视化
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
2.9.5 附着在机械臂后解绑
使用 MoveGroupInterface 类 attachObject() 函数将物体附着在机械臂上，参数是物体的名字 id。
ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group.attachObject(collision_object.id);
显示在 RViz 中：
visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
"robot");
解绑操作使用 MoveGroupInterface 类 的detachObject() 函数，参数也是物体名：
ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
move_group.detachObject(collision_object.id);
显示在 RViz 中：
visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
"robot");
2.9.6 删除物体
将盒子box1从环境移除，调用planning_scene_interface 类的removeCollisionObjects()
ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
RViz 显示：
visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
3. 总结
运行的程序已经讲解完毕，下面简单总结一下：
3.1 机械臂规划的两种方式
对机械臂规划有两种方式：
2.5 中通过设置末端执行器的位置作为target；
输入初始姿态：
move_group.setStartState(*move_group.getCurrentState());设置目标姿态：
move_group.setPoseTarget(target_pose1);target_pose1 是 描述末端执行器姿态的数组。附注：
设置初始姿态也可以向设置目标姿态一样给数组赋值：
geometry_msgs::Pose start_pose2; start_pose2.orientation.w = 1.0; start_pose2.position.x = 0.55; start_pose2.position.y = -0.05; start_pose2.position.z = 0.8; //setFromIK() 是一个检验函数 start_state.setFromIK(joint_model_group, start_pose2); move_group.setStartState(start_state);
2.6 中设置各个关节的姿态数组作为target。
设置初始姿态：
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();修改姿态：
std::vector<double> joint_group_positions; //调用copyJointGroupPositions将之前的位姿拷贝到关节数组中。 current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); //然后修改joint_group_positions数组即可修改设置目标姿态：
move_group.setJointValueTarget(joint_group_positions);
3.2 其他函数
- MoveGroupInterface 类的绑定与解绑函数：
- attachObject()
- detachObject()
- PlanningSceneInterface 类的添加物体删除物体：
- addCollisionObjects(collision_objects)
- removeCollisionObjects(object_ids)
参考资料：