MoveIt2——3.Move Group C++ Interface

最新推荐文章于 2022-07-15 17:25:07 发布

babyrtsh .

最新推荐文章于 2022-07-15 17:25:07 发布

阅读量967

点赞数 1

分类专栏： Moveit2 ROS2 文章标签： c++ ubuntu

本文链接：https://blog.csdn.net/Avada_C_L/article/details/125649740

版权

Moveit2 同时被 2 个专栏收录

10 篇文章 11 订阅

订阅专栏

ROS2

8 篇文章 1 订阅

订阅专栏

Move Group C++ Interface

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

运行代码

打开两个终端。在第一个终端打开RViz并且等待所有东西加载完成：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个终端中运行launch文件

ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

片刻之后，RViz窗口应该会出现，并且看起来与本页顶部的窗口类似。要继续完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的Next按钮，或在屏幕顶部的"Tools"面板中选择“Key Tools”，然后在 RViz 聚焦时按键盘上的 0。

期望输出

参阅YouTube视频了解期望输出。在 RViz 中，我们应该能够看到以下内容：

1. 机器人将手臂移动到前方的姿势目标。
2. 机器人将手臂移动到其侧面的联合目标。
3. 机器人将其手臂移回新的姿势目标，同时保持末端执行器水平。
4. 机器人沿着所需的笛卡尔路径（向下，向右，向上+向左的三角形）移动手臂。
5. 机器人将手臂移动到一个简单的目标，没有物体挡路。
6. 一个 box 对象将添加到手臂右侧的环境中。

在这里插入图片描述
7. 机器人将其手臂移动到姿势目标，避免与盒子发生碰撞。
8. 物体附着在手腕上（其颜色将变为紫色/橙色/绿色）。
9. 机器人将带有附加物体的手臂移动到姿势目标，避免与盒子发生碰撞。
10. 对象与手腕分离（其颜色将变回绿色）。
11. 该对象将从环境中移除。

整个代码

整个代码可以在MoveIt GitHub项目中看到。接下来，我们逐段介绍代码以解释其功能。

设置

MoveIt对一组叫做“planning groups”关节进行操作，并将他们存储在称为关节模型组的对象中。通过MoveIt"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;

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

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

/* 远程控制是一种内省工具，允许用户单步执行高级脚本 */
/* 通过 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()函数。请注意，我们之前设置的姿势目标仍然处于活动状态，因此机器人将尝试移动到该目标。在本教程中，我们不会使用该函数，因为它是一个阻塞函数，并且需要控制器处于活动状态并在执行轨迹时报告成功。

/* 使用真实机器人时取消行下方注释 */
/* 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);

在关节空间中执行规划

根据规划问题，MoveIt会为问题在joint space和cartesian space之间做出选择。可以在ompl_planning.yaml文件中设置规划组参数enforce_joint_model_state_space:true来强制所有规划使用joint space。
默认情况下，对具有方位路径约束的规划请求会在cartesian 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");