< >
Home » ROS2与MoveIt2教程 » ROS2与MoveIt2入门教程-Move Group C++接口

ROS2与MoveIt2入门教程-Move Group C++接口

说明：

  • 介绍Move Group C++接口

概述：

  • 在MoveIt2中，最简单的用户界面可以通过MoveGroupInterface类来建立。该类为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或位姿目标、创建运动规划、移动机器人、将物体添加到环境中以及将物体连接到机器人或者将物体从机器人上分离出来。该接口通过ROS话题、服务和动作与MoveGroup节点进行通信。

运行代码

  • 打开两个终端。在第一个终端中启动RViz，然后等待加载完所有项目：
ros2 launch moveit2_tutorials move_group.launch.py
  • 在第二个终端中运行启动文件：
ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

片刻之后，RViz窗口应该会出现，并且看起来与本节教程开头的图片类似。要完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的“下一步（Next）”按钮，或者在光标聚焦RViz时选择屏幕顶部“工具（Tools）”面板中的“关键工具（Key Tool）”后按键盘上的N字母键。

预期输出

  • 要查看预期输出，请观看本节教程顶部的YouTube视频。在RViz中，我们应该能够看到以下输出：

    • 机器人将其手臂移动到其前方的位姿目标处。

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

    • 机器人将其手臂移回到一个新的位姿目标，同时保持末端执行器处于水平状态。

    • 机器人沿着期望的笛卡尔路径（一个三角形向下、向右、向上+向左）移动其手臂。

    • 一个盒子物体被添加到环境中位于机器人手臂右侧位置上。

      请输入图片描述

    • 机器人将手臂移动到位姿目标，避免与该盒子发生碰撞。

    • 该物体被连接到机械臂手腕上（其颜色会变成紫色/橙色/绿色）。

    • 该物体从机械臂手腕上分离开来（其颜色会变回绿色）。

    • 该物体从环境中移除。

完整代码及其说明

完整代码可以在此处的MoveIt GitHub项目中看到。接下来将逐段解释各代码段的功能。

  • MoveIt2对称为“规划组（planning group）”的关节集合进行操作，并将它们存储在一个称为JointModelGroup的对象中。在整个MoveIt2中，术语“规划组（planning group）”和“关节模型组（joint model group）”是可以互换使用的。
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;
  • 裸指针（raw pointers）频繁用于引用规划组以提高性能：
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::planning_interface::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);
  • 接着来获取该规划组的当前关节值集合。
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
  • 现在，让我们来修改其中一个关节，规划至新的关节空间目标并可视化该规划。
joint_group_positions[0] = -1.0;  // radians
move_group.setJointValueTarget(joint_group_positions);
  • 我们将允许的最大速度和加速度减小到其最大值的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::planning_interface::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
  • 最后在RViz中可视化该规划结果：
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;
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::msg::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);

  • 在关节空间中执行规划

    取决于规划问题，MoveIt2会为问题表示在joint space和cartesian space之间作出选择。可以在ompl_planning.yaml文件中设置规划组参数enforce_joint_model_state_space:true来强制所有规划使用joint space。

    默认情况下，对具有方位路径约束的规划请求会在笛卡尔空间中进行采样，以便调用IK作为生成采样器。

    通过强制使用joint space，规划过程将会使用拒绝抽样来查找有效请求。请注意，这可能会大大增加规划时间。

  • 这里我们将会重用已有的旧目标并规划至该目标。请注意，这仅在当前状态已经满足路径约束时才有效。因此需要将起始状态设置为一个新的位姿。

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::planning_interface::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%% acheived)", 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::planning_interface::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";
  • 定义一个要添加到虚拟世界中的盒子：
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::planning_interface::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来确定该物体连接到机器人的哪个链接上，而且还需告诉MoveIt2允许该物体与抓手的手指链接发生碰撞。也可以用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::planning_interface::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);
  • 然后再在Rviz中显示状态文本：
visual_tools.publishText(text_pose, "Objects_removed", 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 to once the collision object disapears");

  • 启动文件。整个启动文件放在GitHub网站上的此处。本教程中的所有代码都可以从作为MoveIt2安装一个组成部分的moveit_tutorials软件包中运行。

  • 容差设置的注意事项

请注意，MoveGroupInterface的setGoalTolerance()和相关方法都是为规划设置容差，而不是为执行设置容差。

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

英语原文地址：https://moveit2_tutorials.picknik.ai/doc/move_group_interface/move_group_interface_tutorial.html

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与moveit2入门教程