Ros2 MoveIt2 MoveGroup C++接口

 在 MoveIt 中，最简单的用户界面是通过 MoveGroupInterface 类。 它为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或姿势目标、创建运动计划、移动机器人、将对象添加到环境中以及从机器人上连接/分离对象。 此接口通过 ROS 主题、服务和操作与 MoveGroup 节点进行通信。

观看此快速 YouTube 视频演示，了解 Move Group 界面的强大功能！

开始

如果尚未执行此操作，请确保已完成“入门”中的步骤。

运行代码

打开两个 shell。在第一个 shell 中，启动 RViz 并等待所有内容完成加载：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

片刻之后，RViz 窗口应出现，看起来类似于此页面顶部的窗口。要完成每个演示步骤，请按屏幕底部 RvizVisualToolsGui 面板中的“下一步”按钮，或在屏幕顶部的“工具”面板中选择“键工具”，然后在 RViz 聚焦时按键盘上的 0。

预期输出

请参阅本教程顶部的 YouTube 视频，了解预期的输出。在 RViz 中，我们应该能够看到以下内容：

1. 机器人将其手臂移动到其前方的姿势目标。

2. 机器人将其手臂移动到其侧面的关节目标。

3. 机器人将其手臂移回新的姿势目标，同时保持末端执行器水平。

4. 机器人沿着所需的笛卡尔路径（向下、向右、向上+向左的三角形）移动其手臂。

5. 机器人将其手臂移动到一个简单的目标，没有物体挡路。

6. 一个框对象被添加到手臂右侧的环境中。

7. 机器人将其手臂移动到姿势目标，避免与盒子发生碰撞。

8. 该物体附着在手腕上（其颜色将变为紫色/橙色/绿色）。

9. 机器人将其手臂与附着的物体移动到姿势目标上，避免与盒子发生碰撞。

10. 物体从手腕上分离（其颜色将变回绿色）。

11. 对象将从环境中移除。

整个代码

完整的代码可以在 MoveIt GitHub 项目中查看。接下来，我们将逐步介绍代码以解释其功能。

设置

MoveIt 对称为“计划组”的关节集进行操作，并将它们存储在一个名为JointModelGroup, 在整个MoveIt中，“规划组”和“关节模型组”这两个术语 ,可以互换使用。

static const std::string PLANNING_GROUP = "panda_arm";

只需使用要控制和规划的计划组的名称即可轻松设置 MoveGroupInterface 类。

moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);

我们将使用 PlanningSceneInterface 类在“虚拟世界”场景中添加和删除碰撞对象.

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

原始指针经常用于引用计划组， 以提高性能。

const moveit::core::JointModelGroup* joint_model_group =move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

可视化

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "move_group_tutorial",move_group.getRobotModel());visual_tools.deleteAllMarkers();/* Remote control is an introspection tool that allows users to step through a high level script */
/* via buttons and keyboard shortcuts in RViz */
visual_tools.loadRemoteControl();

RViz 提供了多种类型的标记，在此演示中，我们将使用文本、圆柱体和球体：

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.0;
visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);

批量发布用于减少发送到 RViz 的消息数量，以进行大型可视化：

visual_tools.trigger();

获取基本信息

我们可以打印这个机器人的参考系的名称。

RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());

我们还可以打印此组的末端执行器链接的名称。

RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());

我们可以得到机器人中所有组的列表：

RCLCPP_INFO(LOGGER, "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),std::ostream_iterator<std::string>(std::cout, ", "));

开始演示

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

规划姿势目标

我们可以为这个组计划一个运动，以达到末端执行器一个期望的姿势。

geometry_msgs::msg::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::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

可视化计划

我们还可以将计划可视化为 RViz 中带有标记的线。

RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose_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");

移动到姿势目标

将目标姿态移动到目标位置的过程与前面提到的步骤类似，只是这次我们使用了move()函数。请注意，我们之前设置的目标姿态仍然有效，因此机器人会尝试移动到该目标位置。在本教程中，我们不会使用该函数，因为它是一个阻塞函数，需要一个控制器处于活动状态，并在执行轨迹时报告成功。

/* Uncomment below line when working with a real robot */
/* move_group.move(); */

规划关节空间目标

让我们设定一个关节空间目标，并朝着这个目标迈进。这将替换 我们上面设定的姿态目标。

首先，我们将创建一个指针，该指针引用当前机器人的状态。 RobotState 是包含所有当前位置/速度/加速度数据的对象。

moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);//参数 10 表示等待时间，单位是秒。

接下来，获取运动组的当前关节值集。

std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

现在，让我们修改其中一个关节，计划到新的关节空间目标，并可视化计划。

joint_group_positions[0] = -1.0;  // radians
bool within_bounds = move_group.setJointValueTarget(joint_group_positions);
if (!within_bounds)
{RCLCPP_WARN(LOGGER, "Target joint position(s) were outside of limits, but we will plan and clamp to the limits ");
}

我们将允许的最大速度和加速度降低到其最大值的 5%。默认值是 10%（0.1）。如果你需要你的机器人移动得更快，可以在机器人的 moveit_config 中的 joint_limits.yaml 文件中设置你偏好的默认值，或者在代码中设置明确的因子。

move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

visual_tools.deleteAllMarkers();
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");

使用路径约束进行规划

可以很容易地为机器人上的链路指定路径约束。 让我们为我们的组指定一个路径约束和一个姿势目标。 首先定义路径约束。

moveit_msgs::msg::OrientationConstraint ocm;// 用于设置一个方向约束（OrientationConstraint）
ocm.link_name = "panda_link7";//设置方向约束适用于机器人模型中的某个链接
ocm.header.frame_id = "panda_link0";//设置约束的坐标系。"panda_link0" 是这个约束的参考坐标系，表示方向约束是相对于这个坐标系来定义的。
ocm.orientation.w = 1.0;//设置期望的目标方向的四元数（Quaternion）中的 w 分量。w 是四元数的一个分量，用于表示旋转的角度。1.0 表示没有旋转（即单位四元数），即期望的方向是这个坐标系的对齐方向。
ocm.absolute_x_axis_tolerance = 0.1;//设置 X 轴方向的绝对容忍度。0.1 表示在 X 轴方向上，允许的最大偏差为 0.1 弧度。
ocm.absolute_y_axis_tolerance = 0.1;//设置 Y 轴方向的绝对容忍度。0.1 表示在 Y 轴方向上，允许的最大偏差为 0.1 弧度。
ocm.absolute_z_axis_tolerance = 0.1;//设置 Z 轴方向的绝对容忍度。0.1 表示在 Z 轴方向上，允许的最大偏差为 0.1 弧度。
ocm.weight = 1.0;//设置约束的权重。1.0 表示这个约束的优先级较高，权重值范围通常是 0 到 1，值越大，约束的优先级越高。

现在，将其设置为组的路径约束。

moveit_msgs::msg::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);

在关节空间中实施规划

根据规划问题，MoveIt 在 ompl_planning.yaml 文件中选择并使用不同的表示方法。设置 group 参数为 true 时，将强制所有计划使用关节空间 (joint space)。

默认情况下，具有方向路径约束的规划请求会在笛卡尔空间 (cartesian space) 中进行采样，以便调用逆运动学 (IK) 作为生成采样器。

通过强制使用关节空间 (joint space)，规划过程将使用拒绝采样 (rejection sampling) 来寻找有效的请求。请注意，这可能会显著增加规划时间。

我们将重用之前设定的目标并进行规划。请注意，这只有在当前状态已经满足路径约束的情况下才会有效。因此，我们需要将起始状态设置为新的姿势。

moveit::core::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::msg::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);

现在，我们将从刚刚创建的新起始状态规划到之前的姿势目标。

move_group.setPoseTarget(target_pose1);

由于每个样本都必须调用逆运动学求解器，因此带有约束的规划可能会很慢。为了确保规划器有足够的时间成功，我们将规划时间从默认的 5 秒增加。

move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "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("Press 'next' in the RvizVisualToolsGui window to continue the demo");

处理完路径约束后，请务必清除它。

move_group.clearPathConstraints();

笛卡尔路径

您可以通过指定航点列表来直接规划笛卡尔路径 ，让末端执行器通过。注意，我们是从上面提到的新起始状态开始的。初始姿态（起始状态）不需要添加到路径点列表中，但将其添加进去有助于可视化。

 

std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(start_pose2);geometry_msgs::msg::Pose target_pose3 = start_pose2;target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3);  // down

target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3);  // right

target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // up and left

我们希望笛卡尔路径以1厘米的分辨率进行插值，因此我们将指定0.01作为笛卡尔平移的最大步长。我们将把跳跃阈值指定为0.0，从而有效地禁用它。警告 - 在操作真实硬件时禁用跳跃阈值可能会导致冗余关节发生大的不可预测运动，并可能存在安全隐患。

moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

在RViz中可视化：

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian_Path", 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");

笛卡尔运动通常应该缓慢进行，例如接近物体时。目前无法通过 maxVelocityScalingFactor 设置笛卡尔规划的速度，而是需要手动调整轨迹时间，如[此处]所述。欢迎提交拉取请求。

您可以像这样执行轨迹：

/* move_group.execute(trajectory); */

向环境中添加对象

首先，让我们计划另一个没有物体阻碍的简单目标。

move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::msg::Pose another_pose;
another_pose.orientation.w = 0;
another_pose.orientation.x = -1.0;
another_pose.position.x = 0.7;
another_pose.position.y = 0.0;
another_pose.position.z = 0.59;
move_group.setPoseTarget(another_pose);success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(another_pose, "goal");
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");

结果可能如下所示：

 现在，让我们为机器人定义一个要避免的碰撞对象 ROS 消息。

moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();

对象的 ID 用于标识对象。

collision_object.id = "box1";

定义一个要添加到World的框。

shape_msgs::msg::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.1;
primitive.dimensions[primitive.BOX_Y] = 1.5;
primitive.dimensions[primitive.BOX_Z] = 0.5;

定义框的姿势（相对于frame_id指定）。

geometry_msgs::msg::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.48;
box_pose.position.y = 0.0;
box_pose.position.z = 0.25;collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

现在，让我们将碰撞对象添加到世界中 （使用可能包含其他对象的向量）

RCLCPP_INFO(LOGGER, "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

在状态的 RViz 中显示文本，并等待 MoveGroup 接收和处理碰撞对象消息

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

现在，当我们规划一条轨迹时，它将避开障碍物。

success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
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("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

结果可能如下所示：

 

将物体连接到机器人上

您可以将对象连接到机器人上，以便它随着机器人的几何形状移动。 这模拟了为了操纵对象而拾取对象。 运动规划也应避免对象之间的碰撞。

moveit_msgs::msg::CollisionObject object_to_attach;
object_to_attach.id = "cylinder1";shape_msgs::msg::SolidPrimitive cylinder_primitive;
cylinder_primitive.type = primitive.CYLINDER;
cylinder_primitive.dimensions.resize(2);
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;

我们定义这个圆柱体的框架/姿势，使其出现在夹持器中。

object_to_attach.header.frame_id = move_group.getEndEffectorLink();
geometry_msgs::msg::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;

首先，我们将对象添加到世界中（不使用向量）。

object_to_attach.primitives.push_back(cylinder_primitive);
object_to_attach.primitive_poses.push_back(grab_pose);
object_to_attach.operation = object_to_attach.ADD;
planning_scene_interface.applyCollisionObject(object_to_attach);

然后，我们将物体“附着”到机器人上。它使用frame_id来确定它连接到哪个机器人链接。 我们还需要告诉 MoveIt，允许物体与抓手的手指链接发生碰撞。 您还可以使用 applyAttachedCollisionObject 将对象直接附加到机器人。

RCLCPP_INFO(LOGGER, "Attach the object to the robot");
std::vector<std::string> touch_links;
touch_links.push_back("panda_rightfinger");
touch_links.push_back("panda_leftfinger");
move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");

重新计划，但现在手头有对象。

move_group.setStartStateToCurrentState();
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

结果可能如下所示：

 

分离和移除对象

现在，让我们将气缸从机器人的夹持器上拆下。

RCLCPP_INFO(LOGGER, "Detach the object from the robot");
move_group.detachObject(object_to_attach.id);

在状态的 RViz 中显示文本

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");

现在，让我们从世界中移除对象。

RCLCPP_INFO(LOGGER, "Remove the objects from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
object_ids.push_back(object_to_attach.id);
planning_scene_interface.removeCollisionObjects(object_ids);

启动文件

整个启动文件位于 GitHub 上。本教程中的所有代码都可以从作为 MoveIt 设置的一部分提供的 moveit2_tutorials 包中运行。

关于设置公差的注意事项

请注意，MoveGroupInterface 的 setGoalTolerance（） 和相关方法设置了规划的容差，而不是执行的容差。

 

如果要配置执行容差，则必须编辑 controller.yaml 文件（如果使用 FollowJointTrajectory 控制器），或者手动将其添加到规划器生成的轨迹消息中。