ROS机械臂 Movelt 学习笔记2 | Move Group 接口 C++

Movelt为使用者提供了一个最通用且简单的接口 MoveGroupInterface 类，这个接口提供了很多控制机器人的常用基本操作，如：

设置机械臂的位姿
进行运动规划
移动机器人本体
将物品添加到环境 / 从环境移除
将物体绑定到机器人 / 从机器人解绑

这个接口通过ROS话题topic、服务service和动作action等机制与 MoveGroup 节点进行通信。

有关ROS节点相关知识以后会介绍，参见ROS Nodes

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

注意：例程使用 RvizVisualToolsGui 面板逐步完成演示。默认应当是直接出现的。

如果没有，要将此面板添加到 RViz，参考基础准备。

程序执行过程中的 rviz 如下图所示，跟随第二个终端里的提示，在屏幕底部 RvizVisualToolsGui 面板中按下 Next 按钮。

或选择屏幕顶部工具面板 (Interact那一行) 中的Key Tool 工具，然后按键盘上的 N 来进行下一步。

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 的名称 即可设置。

规划组planning group：

是我们设置的，设置过程会在后续讲解倒入机器人模型时讲解。

例如 panda机械臂，在Rviz的 Planning Request 中可以看到它有 3 个 Planning Group 规划组：

hand \ panda_arm \ panda_arm_hand

决定我们操作的机器人对象。

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);

Eigen 参见相关博客

rviz 中的 markers参见：

各种marker的详细介绍

RVIZ中利用markers标记历史路径

Rviz官网

批量发布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，并将规划可视化。

这里调用了 setJointValueTarget()函数。

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);

start_state.setFromIK()解释：
start_state.setFromIK(joint_model_group, start_pose2);
含义是，如果 start_pose2 对应的规划组joint_model_group 是链，并且解算器可用（实际上是可用的），则可通过 moveit 中自带的逆运动学求解器进行计算得到规划组中各关节位置值，前提是设置的start_pose2要在运动模型的参考系中。

参考：https://www.freesion.com/article/8268839796/

回头我也想总结一下常用函数和常用ROS消息。

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。

computeCartesianPath()

该函数用来计算笛卡尔路径，该路径会经过之前设置好的 waypoints，步长为 eef_step，误差不超过 jump_threshold ，将该值设置为0可以有效禁用该变化

但是在操作真实机械臂的时候 jump_threshold 设为0，可能导致冗余连接，发生大量不可预测的动作。

返回值是0.0~1.0之间的一个数值，表示成功规划了给定路径点的比例（1.0表示都能成功经过）。

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);