【MoveIt 2】示例:移动组 C++ 接口

最新推荐文章于 2025-04-09 22:28:35 发布

十年一梦实验室

最新推荐文章于 2025-04-09 22:28:35 发布

阅读量1.3k

点赞数 19

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/140942395

版权

在 MoveIt 中，最简单的用户界面是通过 MoveGroupInterface 类https://github.com/moveit/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h 。它为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或姿态目标、创建运动计划、移动机器人、向环境中添加对象以及从机器人附加/分离对象。该接口通过 ROS 主题、服务和操作与 MoveGroup 节点https://github.com/moveit/moveit2/blob/main/moveit_ros/move_group/src/move_group.cpp 进行通信。

观看这个快速的 YouTube 视频https://www.youtube.com/watch?v=_5siHkFQPBQ&feature=youtu.be 演示，了解移动组界面的强大功能！

视频演示

入门

如果您还没有这样做，请确保您已完成入门中的步骤。https://moveit.picknik.ai/main/doc/tutorials/getting_started/getting_started.html

运行代码

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

ros2 launch moveit2_tutorials move_group.launch.py

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

ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

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

预期输出

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

机器人将其手臂移动到其前方的姿势目标。
机器人将其手臂移动到其侧面的关节目标。
机器人在保持末端执行器水平的同时，将手臂移回到一个新的姿态目标。
机器人沿着所需的笛卡尔路径移动其手臂（一个三角形向下，向右，向上+向左）。
机器人将其手臂移动到一个没有障碍物的简单目标。
一个箱子对象被添加到环境中，位于机械臂的右侧。
机器人将其手臂移动到目标姿势，避免与箱子碰撞。
物体附着在手腕上（其颜色将变为紫色/橙色/绿色）。
机器人移动其带有附加物的手臂到达目标姿势，避免与箱子碰撞。
物体从手腕上脱落（其颜色将变回绿色）。
对象已从环境中移除。

整个代码

整个代码可以在 MoveIt GitHub 项目中https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp 看到。接下来，我们逐步解释代码的功能。

设置

MoveIt 在称为“规划组 planning groups ”的关节集上运行，并将它们存储在称为 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);

接下来获取该组的当前关节值集。

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

在 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.t