使用 MoveIt Task Constructor 实现抓取与放置
本教程将带你逐步创建一个包，使用 MoveIt Task Constructor 来规划抓取与放置（pick and place）任务。MoveIt Task Constructor（MTC，MoveIt 任务构造器）提供了一种灵活的方式来规划由多个不同子任务（称为阶段，stage）组成的复杂任务。如果你只想运行本教程而不想从头搭建，可以按照 Docker 指南 启动一个已完成教程的容器。
1 基本概念
Section titled “1 基本概念”MTC 的基本思想是：复杂的运动规划问题可以被分解为一组更简单的子问题。顶层规划问题被定义为一个任务（Task），所有子问题则被定义为阶段（Stage）。阶段可以按任意顺序和层级排列，但排列顺序受到结果传递方向的约束，同时也受各阶段类型的限制。根据结果传递方式的不同，阶段可以分为三种类型：生成器（generator）、传播器（propagator）和连接器（connector）。
生成器（Generator） 独立于相邻阶段计算结果，并将结果同时向前和向后两个方向传递。例如，用于几何位姿的 IK 采样器就是一个生成器——其前后的接近和离开运动都依赖于它生成的解。
传播器（Propagator） 接收某一侧相邻阶段的结果，求解一个子问题，然后将解传播到另一侧的相邻阶段。根据具体实现，传播器可以向前、向后，或同时向两个方向传递解。例如，一个基于起始状态或目标状态计算笛卡尔路径的阶段就是传播器。
连接器（Connector） 不传播任何结果，而是尝试在两个相邻阶段的结果状态之间建立衔接。例如，计算从一个给定状态到另一个状态的自由运动规划。
除了上述顺序类型，阶段还有不同的层级类型，可以嵌套包含子阶段。不含子阶段的阶段称为原始阶段（primitive stage），能够包含子阶段的阶段称为容器阶段（container stage）。容器又分为三种类型：
包装器（Wrapper） 封装单个子阶段，并修改或过滤其结果。例如，一个只保留子阶段中满足特定约束的解的过滤阶段，就可以用包装器来实现。该类型的另一个常见用途是 IK 包装器阶段，它根据带有位姿目标（pose target）属性的规划场景来生成逆运动学解。
串行容器（Serial Container） 持有一系列顺序执行的子阶段，只输出能够从头到尾贯穿所有阶段的完整解。例如，由一系列连贯步骤组成的抓取运动。
并行容器（Parallel Container） 组合一组子阶段，可以用于：从多个备选结果中选取最佳结果、运行后备求解器，或合并多个独立的解。例如，同时运行多个备选的运动规划器、用右手或左手（作为后备方案）抓取物体，或同时移动手臂和打开夹爪。
阶段不仅用于解决运动规划问题，还可以执行各种状态转换操作，例如修改规划场景。借助面向对象的继承机制，即使只使用一组结构良好的原始阶段，也能构建出非常复杂的行为。
关于 MTC 更详细的信息，请参阅 MoveIt Task Constructor 概念。
如果你还没有完成入门指南中的步骤，请先确保完成。
进入你的 colcon 工作区，拉取 MoveIt Task Constructor 源码。其中 <branch> 可以是 humble（适用于 ROS Humble），也可以是 ros2（与 MoveIt 2 main 分支兼容的最新版本）：
cd ~/ws_moveit/src git clone -b <branch> https://github.com/moveit/moveit_task_constructor.git用 rosdep 安装缺失的包：
rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO构建工作区：
cd ~/ws_moveit colcon build --mixin releaseMoveIt Task Constructor 包含几个基本示例和一个抓取与放置演示。运行任何演示之前，都需要先启动基础环境：
ros2 launch moveit_task_constructor_demo demo.launch.py然后即可运行各个演示：
ros2 launch moveit_task_constructor_demo run.launch.py exe:=cartesian ros2 launch moveit_task_constructor_demo run.launch.py exe:=modular ros2 launch moveit_task_constructor_demo run.launch.py exe:=pick_place_demo在 RViz 右侧，你会看到 Motion Planning Tasks 面板，其中展示了任务的层级阶段结构。当你选择某个阶段时，最右侧的窗口会显示成功和失败的解列表。选择其中任意一个解即可启动可视化。
4 使用 MoveIt Task Constructor 搭建项目
Section titled “4 使用 MoveIt Task Constructor 搭建项目”本节将逐步介绍如何使用 MoveIt Task Constructor 构建一个简单的任务。
4.1 创建新包
Section titled “4.1 创建新包”使用以下命令创建一个新包：
ros2 pkg create \ --build-type ament_cmake \ --dependencies moveit_task_constructor_core rclcpp \ --node-name mtc_node mtc_tutorial这会创建一个名为 mtc_tutorial 的新包和对应文件夹。该包依赖于 moveit_task_constructor_core，并在 src/mtc_node 中生成一个 hello world 示例。
4.2 代码
Section titled “4.2 代码”在你喜欢的编辑器中打开 mtc_node.cpp，粘贴以下代码。
#include <rclcpp/rclcpp.hpp> #include <moveit/planning_scene/planning_scene.hpp> #include <moveit/planning_scene_interface/planning_scene_interface.hpp> #include <moveit/task_constructor/task.h> #include <moveit/task_constructor/solvers.h> #include <moveit/task_constructor/stages.h> #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>) #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> #else #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #endif #if __has_include(<tf2_eigen/tf2_eigen.hpp>) #include <tf2_eigen/tf2_eigen.hpp> #else #include <tf2_eigen/tf2_eigen.h> #endif
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial"); namespace mtc = moveit::task_constructor;
class MTCTaskNode { public: MTCTaskNode(const rclcpp::NodeOptions& options);
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
void doTask();
void setupPlanningScene();
private: // Compose an MTC task from a series of stages. mtc::Task createTask(); mtc::Task task_; rclcpp::Node::SharedPtr node_; };
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options) : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) } { }
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface() { return node_->get_node_base_interface(); }
void MTCTaskNode::setupPlanningScene() { moveit_msgs::msg::CollisionObject object; object.id = "object"; object.header.frame_id = "world"; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; object.primitives[0].dimensions = { 0.1, 0.02 };
geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; pose.orientation.w = 1.0; object.pose = pose;
moveit::planning_interface::PlanningSceneInterface psi; psi.applyCollisionObject(object); }
void MTCTaskNode::doTask() { task_ = createTask();
try { task_.init(); } catch (mtc::InitStageException& e) { RCLCPP_ERROR_STREAM(LOGGER, e); return; }
if (!task_.plan(5)) { RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); return; } task_.introspection().publishSolution(*task_.solutions().front());
auto result = task_.execute(*task_.solutions().front()); if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed"); return; }
return; }
mtc::Task MTCTaskNode::createTask() { mtc::Task task; task.stages()->setName("demo task"); task.loadRobotModel(node_);
const auto& arm_group_name = "panda_arm"; const auto& hand_group_name = "hand"; const auto& hand_frame = "panda_hand";
// Set task properties task.setProperty("group", arm_group_name); task.setProperty("eef", hand_group_name); task.setProperty("ik_frame", hand_frame);
// Disable warnings for this line, as it's a variable that's set but not used in this example #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-but-set-variable" mtc::Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator #pragma GCC diagnostic pop
auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current"); current_state_ptr = stage_state_current.get(); task.add(std::move(stage_state_current));
auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_); auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>(); cartesian_planner->setMaxVelocityScalingFactor(1.0); cartesian_planner->setMaxAccelerationScalingFactor(1.0); cartesian_planner->setStepSize(.01);
auto stage_open_hand = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); stage_open_hand->setGroup(hand_group_name); stage_open_hand->setGoal("open"); task.add(std::move(stage_open_hand));
return task; }
int main(int argc, char** argv) { rclcpp::init(argc, argv);
rclcpp::NodeOptions options; options.automatically_declare_parameters_from_overrides(true);
auto mtc_task_node = std::make_shared<MTCTaskNode>(options); rclcpp::executors::MultiThreadedExecutor executor;
auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() { executor.add_node(mtc_task_node->getNodeBaseInterface()); executor.spin(); executor.remove_node(mtc_task_node->getNodeBaseInterface()); });
mtc_task_node->setupPlanningScene(); mtc_task_node->doTask();
spin_thread->join(); rclcpp::shutdown(); return 0; }4.3 代码解析
Section titled “4.3 代码解析”代码顶部包含了本包所需的 ROS 和 MoveIt 库的头文件。
rclcpp/rclcpp.hpp：ROS 2 核心功能moveit/planning_scene/planning_scene.hpp和moveit/planning_scene_interface/planning_scene_interface.hpp：与机器人模型和碰撞对象交互moveit/task_constructor/task.h、moveit/task_constructor/solvers.h和moveit/task_constructor/stages.h：MTC 的核心组件tf2_geometry_msgs/tf2_geometry_msgs.hpp和tf2_eigen/tf2_eigen.hpp：在初始示例中暂不使用，但后续添加位姿生成阶段时会用到
#include <rclcpp/rclcpp.hpp> #include <moveit/planning_scene/planning_scene.hpp> #include <moveit/planning_scene_interface/planning_scene_interface.hpp> #include <moveit/task_constructor/task.h> #include <moveit/task_constructor/solvers.h> #include <moveit/task_constructor/stages.h> #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>) #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> #else #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #endif #if __has_include(<tf2_eigen/tf2_eigen.hpp>) #include <tf2_eigen/tf2_eigen.hpp> #else #include <tf2_eigen/tf2_eigen.h> #endif下一行为新节点创建了一个 logger。为方便起见，我们还为 moveit::task_constructor 定义了命名空间别名 mtc。
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial"); namespace mtc = moveit::task_constructor;接下来定义一个类来封装 MTC 的主要功能。我们将 MTC 任务对象声明为类的成员变量——这不是必须的，但这样做可以保存任务以供后续可视化。下面逐一分析每个函数。
class MTCTaskNode { public: MTCTaskNode(const rclcpp::NodeOptions& options);
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
void doTask();
void setupPlanningScene();
private: // Compose an MTC task from a series of stages. mtc::Task createTask(); mtc::Task task_; rclcpp::Node::SharedPtr node_; };以下是 MTCTaskNode 类的构造函数，使用指定选项初始化节点。
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options) : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) } { }接下来定义一个 getter 函数，用于获取节点基接口（NodeBaseInterface），后续的执行器（executor）会用到它。
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface() { return node_->get_node_base_interface(); }这个类方法用于设置示例中的规划场景。它创建一个圆柱体，尺寸由 object.primitives[0].dimensions 指定，位置由 pose.position.x 和 pose.position.y 指定。你可以尝试修改这些数值来调整圆柱体的大小和位置——如果把圆柱体放到机器人够不到的地方，规划将会失败。
void MTCTaskNode::setupPlanningScene() { moveit_msgs::msg::CollisionObject object; object.id = "object"; object.header.frame_id = "world"; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; object.primitives[0].dimensions = { 0.1, 0.02 };
geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; object.pose = pose;
moveit::planning_interface::PlanningSceneInterface psi; psi.applyCollisionObject(object); }此函数与 MTC 任务对象交互，完成任务的创建、规划与执行。首先，它调用 createTask() 创建任务（包括设置属性和添加阶段），后面会详细讨论这个函数。然后，task.init() 初始化任务，task.plan(5) 尝试规划，最多生成 5 个成功的解后停止。接着，task.introspection().publishSolution() 发布一个解供 RViz 可视化——如果你不需要可视化，可以删除这一行。最后，task.execute() 通过 RViz 插件的 action server 接口执行该解。
void MTCTaskNode::doTask() { task_ = createTask();
try { task_.init(); } catch (mtc::InitStageException& e) { RCLCPP_ERROR_STREAM(LOGGER, e); return; }
if (!task_.plan(5)) { RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); return; } task_.introspection().publishSolution(*task_.solutions().front());
auto result = task_.execute(*task_.solutions().front()); if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed"); return; }
return; }如前所述，此函数创建一个 MTC 任务对象并设置初始属性。在本例中，我们将任务命名为 “demo task”，加载机器人模型，定义一些有用的坐标系名称，然后用 task.setProperty(property_name, value) 将规划组、末端执行器和 IK 坐标系设置为任务属性。接下来的几个代码块将逐步填充此函数体。
mtc::Task MTCTaskNode::createTask() { moveit::task_constructor::Task task; task.stages()->setName("demo task"); task.loadRobotModel(node_);
const auto& arm_group_name = "panda_arm"; const auto& hand_group_name = "hand"; const auto& hand_frame = "panda_hand";
// Set task properties task.setProperty("group", arm_group_name); task.setProperty("eef", hand_group_name); task.setProperty("ik_frame", hand_frame);现在向任务添加第一个阶段。首先声明一个 current_state_ptr 指针并初始化为 nullptr，用于保存阶段信息以便后续复用——这一行目前不会用到，但在添加更多阶段时会用到。接下来创建一个 CurrentState 阶段（生成器阶段），让机器人从当前状态开始，并将其添加到任务中。最后，将该阶段的指针保存到 current_state_ptr 中以备后用。
mtc::Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current"); current_state_ptr = stage_state_current.get(); task.add(std::move(stage_state_current));求解器（solver）用于定义机器人的运动方式。MTC 提供了三种求解器：
PipelinePlanner 使用 MoveIt 的规划管线，通常默认基于 OMPL。
auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);JointInterpolationPlanner 是一个简单的规划器，在起始和目标关节状态之间进行插值。它计算速度快，适合简单运动，但不支持复杂运动。
auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();CartesianPath 用于在笛卡尔空间中沿直线移动末端执行器。
auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();你可以尝试不同的求解器，观察机器人运动方式的变化。笛卡尔规划器需要设置以下属性：
auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>(); cartesian_planner->setMaxVelocityScalingFactor(1.0); cartesian_planner->setMaxAccelerationScalingFactor(1.0); cartesian_planner->setStepSize(.01);求解器准备好之后，我们可以添加一个实际移动机器人的阶段了。下面的代码使用 MoveTo 阶段（一种传播器阶段）来打开手。由于张开手是比较简单的运动，这里选用关节插值规划器即可。该阶段规划到 “open” 位姿的运动，这是为 Panda 机器人在 SRDF 中定义的命名位姿。最后返回任务对象，createTask() 函数到此结束。
auto stage_open_hand = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); stage_open_hand->setGroup(hand_group_name); stage_open_hand->setGoal("open"); task.add(std::move(stage_open_hand));
return task; }最后是 main 函数。以下代码使用上面定义的类创建节点，并调用类方法来设置规划场景和执行 MTC 任务。在本示例中，任务执行完成后我们没有立即关闭执行器，而是让节点保持存活状态，方便你在 RViz 中检查解。
int main(int argc, char** argv) { rclcpp::init(argc, argv);
rclcpp::NodeOptions options; options.automatically_declare_parameters_from_overrides(true);
auto mtc_task_node = std::make_shared<MTCTaskNode>(options); rclcpp::executors::MultiThreadedExecutor executor;
auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() { executor.add_node(mtc_task_node->getNodeBaseInterface()); executor.spin(); executor.remove_node(mtc_task_node->getNodeBaseInterface()); });
mtc_task_node->setupPlanningScene(); mtc_task_node->doTask();
spin_thread->join(); rclcpp::shutdown(); return 0; }5 运行演示
Section titled “5 运行演示”5.1 启动文件
Section titled “5.1 启动文件”我们需要一个启动文件来启动 move_group、ros2_control、static_tf、robot_state_publisher 和 rviz 等节点，以搭建运行演示所需的环境。本示例使用的启动文件可以在这里找到。
为了运行 MTC 节点，我们还需要第二个启动文件，以正确的参数启动 mtc_tutorial 可执行文件。你可以在其中直接加载 URDF、SRDF 和 OMPL 参数，也可以使用 MoveIt Configs Utils 来完成。你的启动文件应该类似于本教程包中的这个文件（请注意下方代码中的 package 和 executable 参数与所链接的文件不同）：
from launch import LaunchDescription from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description(): moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()
# MTC Demo node pick_place_demo = Node( package="mtc_tutorial", executable="mtc_node", output="screen", parameters=[ moveit_config, ], )
return LaunchDescription([pick_place_demo])将启动文件保存为 pick_place_demo.launch.py 并放入包的 launch 目录。同时，编辑 CMakeLists.txt，添加以下内容以安装 launch 文件夹：
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} )然后构建并 source colcon 工作区。
cd ~/ws_moveit colcon build --mixin release source ~/ws_moveit/install/setup.bash首先启动第一个启动文件。如果你想使用教程提供的版本：
ros2 launch moveit2_tutorials mtc_demo.launch.pyRViz 将随即启动。如果你使用自己的启动文件，且没有包含这样的 RViz 配置，那么需要先手动配置 RViz 才能看到显示效果。如果你使用的是教程包中的启动文件，RViz 已经配置好了，可以直接跳到下一节末尾。
5.2 RViz 配置
Section titled “5.2 RViz 配置”如果你没有使用教程提供的 RViz 配置，需要对 RViz 做一些调整，才能看到机器人和 MTC 的解。以下步骤将介绍如何配置 RViz 来可视化 MTC 的解。
- 如果 MotionPlanning 显示处于启用状态，取消勾选以暂时隐藏它。
- 在 Global Options 下，如果 Fixed Frame 还不是
panda_link0，请将其从map改为panda_link0。 - 在窗口左下角，单击 Add 按钮。
- 在
moveit_task_constructor_visualization下选择 Motion Planning Tasks，单击 OK。Motion Planning Tasks 显示应出现在左下角。 - 在 Displays 中的 Motion Planning Tasks 下，将 Task Solution Topic 更改为
/solution。
此时你应该会在主视图中看到 Panda 机械臂，左下角的 Motion Planning Tasks 面板是空的。启动 mtc_tutorial 节点后，你的 MTC 任务就会出现在这个面板中。如果你使用的是教程提供的 mtc_demo.launch.py，可以从这里继续。
5.3 启动演示
Section titled “5.3 启动演示”使用以下命令启动 mtc_tutorial 节点：
ros2 launch mtc_tutorial pick_place_demo.launch.py你应该会看到机械臂执行”打开手”这个单一阶段的任务，前方有一个绿色的圆柱体。效果大致如下：
如果你没有创建自己的包，但想看看效果，可以直接启动教程提供的文件：
ros2 launch moveit2_tutorials mtc_demo_minimal.launch.py6 添加阶段
Section titled “6 添加阶段”到目前为止，我们已经创建并运行了一个简单的任务。它能工作，但功能有限。接下来，我们将向任务中添加抓取与放置阶段。下图展示了任务中将要用到的所有阶段。
我们将在现有的 open hand 阶段之后依次添加新的阶段。打开 mtc_node.cpp，找到以下代码：
auto stage_open_hand = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); stage_open_hand->setGroup(hand_group_name); stage_open_hand->setGoal("open"); task.add(std::move(stage_open_hand)); // Add the next lines of codes to define more stages here6.1 抓取阶段
Section titled “6.1 抓取阶段”首先，我们需要将机械臂移动到可以拾取物体的位置。这通过 Connect 阶段来实现——顾名思义，它是一个连接器阶段，负责在其前后相邻阶段的结果之间搭建桥梁。该阶段以名称 “move to pick” 和一个指定规划组与规划器的 GroupPlannerVector 进行初始化。然后设置超时时间，配置阶段属性，并将其添加到任务中。
auto stage_move_to_pick = std::make_unique<mtc::stages::Connect>( "move to pick", mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner } }); stage_move_to_pick->setTimeout(5.0); stage_move_to_pick->properties().configureInitFrom(mtc::Stage::PARENT); task.add(std::move(stage_move_to_pick));接下来，创建一个指向 MTC 阶段对象的指针，暂时设为 nullptr，稍后用于保存某个阶段。
mtc::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator下一段代码创建了一个串行容器（SerialContainer）。串行容器可以容纳多个顺序执行的子阶段。在本例中，我们将所有与抓取动作相关的阶段放入这个串行容器中，而不是直接添加到任务里。接着，用 exposeTo() 将父任务的属性暴露给串行容器，并用 configureInitFrom() 初始化这些属性，使容器内的子阶段能够访问它们。
{ auto grasp = std::make_unique<mtc::SerialContainer>("pick object"); task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" }); grasp->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" });接下来创建一个接近物体的阶段。这里使用 MoveRelative 阶段，它允许指定相对于当前位置的运动。MoveRelative 是一个传播器阶段：它接收相邻阶段的解，将自身的解传播到另一侧。配合 cartesian_planner，可以生成末端执行器的直线运动。我们设置阶段属性以及移动的最小和最大距离，然后创建一个 Vector3Stamped 消息来指定运动方向——这里设为沿手部坐标系的 Z 轴方向。最后，将此阶段添加到串行容器中。
{ auto stage = std::make_unique<mtc::stages::MoveRelative>("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); stage->properties().set("link", hand_frame); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.15);
// Set hand forward direction geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = hand_frame; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); }现在，创建一个生成抓取位姿的阶段。这是一个生成器阶段，它独立计算结果，不依赖前后的阶段。第一个阶段 CurrentState 也是生成器——两个生成器不能直接相连，必须通过连接器衔接，我们前面已经创建了这个连接器。
这段代码设置了阶段属性、预抓取位姿（pre-grasp pose）、角度增量和受监控阶段。角度增量是 GenerateGraspPose 阶段的一个属性，用于控制要生成的位姿数量——MTC 会尝试从许多不同的朝向来抓取物体，相邻朝向之间的角度差就是角度增量。增量越小，候选抓取朝向就越密集。在前面定义 CurrentState 阶段时，我们保存了 current_state_ptr，现在用它将物体的位姿和形状信息转发给逆运动学求解器。
这个阶段不会直接添加到串行容器中，因为我们还需要对它生成的位姿进行逆运动学计算。
{ // Sample grasp pose auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose"); stage->properties().configureInitFrom(mtc::Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); stage->setPreGraspPose("open"); stage->setObject("object"); stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(current_state_ptr); // Hook into current state在对生成的位姿计算逆运动学之前，首先需要定义坐标系变换。这可以通过 geometry_msgs 的 PoseStamped 消息来完成；在本例中，我们使用 Eigen 变换矩阵和相关连杆的名称来定义。变换矩阵的定义如下：
Eigen::Isometry3d grasp_frame_transform; Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()); grasp_frame_transform.linear() = q.matrix(); grasp_frame_transform.translation().z() = 0.1;现在，创建 ComputeIK 阶段，命名为 “grasp pose IK”，并将上面定义的 GenerateGraspPose 阶段传给它作为子阶段。有些机器人对给定位姿有多个逆运动学解——这里我们将求解数量上限设为 8。同时设置最小解距离（min solution distance）：这是一个阈值，如果某个解的关节位置与已有解过于相似，就会被标记为无效。接着配置一些附加属性，并将 ComputeIK 阶段添加到串行容器中。
// Compute IK auto wrapper = std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); wrapper->setIKFrame(grasp_frame_transform, hand_frame); wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); }要拾取物体，必须允许手和物体之间存在碰撞。这可以通过 ModifyPlanningScene 阶段来完成。allowCollisions 函数让我们指定需要禁用碰撞的对象，它接受一个名称列表，因此我们可以用 getLinkModelNamesWithCollisionGeometry 获取手部规划组中所有具有碰撞几何体的连杆名称，一次性禁用它们与物体之间的碰撞。
{ auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object)"); stage->allowCollisions("object", task.getRobotModel() ->getJointModelGroup(hand_group_name) ->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); }允许碰撞之后，就可以合拢手了。同样使用 MoveTo 阶段，与前面的 open hand 阶段类似，只是目标改为 SRDF 中定义的 close 位置。
{ auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner); stage->setGroup(hand_group_name); stage->setGoal("close"); grasp->insert(std::move(stage)); }接下来再次使用 ModifyPlanningScene 阶段，这次调用 attachObject 将物体附着到手部。和之前处理 current_state_ptr 一样，我们保存此阶段的指针，供后续生成放置位姿时使用。
{ auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object"); stage->attachObject("object", hand_frame); attach_object_stage = stage.get(); grasp->insert(std::move(stage)); }接下来，用 MoveRelative 阶段抬起物体，与前面的 approach object 阶段类似。
{ auto stage = std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.3); stage->setIKFrame(hand_frame); stage->properties().set("marker_ns", "lift_object");
// Set upward direction geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = "world"; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); }至此，拾取物体所需的全部阶段都已定义完毕。现在将整个串行容器（及其所有子阶段）添加到任务中。此时构建并运行这个包，你就能看到机器人规划拾取物体的过程。
task.add(std::move(grasp)); }要测试新创建的阶段，请构建代码并执行：
ros2 launch mtc_tutorial pick_place_demo.launch.py6.2 放置阶段
Section titled “6.2 放置阶段”抓取阶段定义完毕，现在来定义放置物体的阶段。接着上次的代码位置，先添加一个 Connect 阶段来连接抓取和放置两个部分，因为接下来要使用生成器阶段来生成放置位姿，而生成器之间需要连接器来衔接。
{ auto stage_move_to_place = std::make_unique<mtc::stages::Connect>( "move to place", mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner }, { hand_group_name, interpolation_planner } }); stage_move_to_place->setTimeout(5.0); stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT); task.add(std::move(stage_move_to_place)); }同样，我们为放置阶段创建一个串行容器，结构与抓取的串行容器类似。后续的阶段都将添加到这个串行容器中，而非直接添加到任务里。
{ auto place = std::make_unique<mtc::SerialContainer>("place object"); task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" }); place->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" });下一个阶段用于生成放置位姿并计算逆运动学，逻辑与抓取串行容器中的 GenerateGraspPose 阶段类似。首先创建一个 GeneratePlacePose 阶段，继承任务属性。然后使用 PoseStamped 消息指定放置物体的目标位姿——这里选择 "object" 坐标系中的 y = 0.5，并通过 setPose 将目标位姿传递给该阶段。接着用 setMonitoredStage 将前面 attach_object 阶段的指针传入，使该阶段能够获知物体的附着方式。最后创建 ComputeIK 阶段，将 GeneratePlacePose 阶段传给它——其余配置与抓取阶段完全相同。
{ // Sample place pose auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose"); stage->properties().configureInitFrom(mtc::Stage::PARENT); stage->properties().set("marker_ns", "place_pose"); stage->setObject("object");
geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = "object"; target_pose_msg.pose.position.y = 0.5; target_pose_msg.pose.orientation.w = 1.0; stage->setPose(target_pose_msg); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
// Compute IK auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage)); wrapper->setMaxIKSolutions(2); wrapper->setMinSolutionDistance(1.0); wrapper->setIKFrame("object"); wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); }准备好放置物体后，用 MoveTo 阶段配合关节插值规划器张开手。
{ auto stage = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); stage->setGroup(hand_group_name); stage->setGoal("open"); place->insert(std::move(stage)); }物体已经放下，不再需要握住，因此可以重新启用手与物体之间的碰撞检测。操作方式与之前禁用碰撞几乎完全相同，只需将最后一个参数从 true 改为 false。
{ auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision (hand,object)"); stage->allowCollisions("object", task.getRobotModel() ->getJointModelGroup(hand_group_name) ->getLinkModelNamesWithCollisionGeometry(), false); place->insert(std::move(stage)); }接下来，调用 detachObject 将物体从手部分离。
{ auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object"); stage->detachObject("object", hand_frame); place->insert(std::move(stage)); }然后用 MoveRelative 阶段从物体处后退，与前面的 approach object 和 lift object 阶段类似。
{ auto stage = std::make_unique<mtc::stages::MoveRelative>("retreat", cartesian_planner); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.3); stage->setIKFrame(hand_frame); stage->properties().set("marker_ns", "retreat");
// Set retreat direction geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = "world"; vec.vector.x = -0.5; stage->setDirection(vec); place->insert(std::move(stage)); }至此，放置串行容器完成，将其添加到任务中。
task.add(std::move(place)); }最后一步是返回原点：使用 MoveTo 阶段，将 ready 目标位姿传给它，这是 Panda SRDF 中定义的命名位姿。
{ auto stage = std::make_unique<mtc::stages::MoveTo>("return home", interpolation_planner); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setGoal("ready"); task.add(std::move(stage)); }所有这些阶段都应该添加在以下代码行之前。
// Stages all added to the task above this line
return task; }恭喜！你已经使用 MoveIt Task Constructor 完成了一个完整的抓取与放置任务！构建代码并执行来试试看：
ros2 launch mtc_tutorial pick_place_demo.launch.py7 进一步讨论
Section titled “7 进一步讨论”任务及其包含的每个阶段都会显示在 Motion Planning Tasks 面板中。单击某个阶段，右侧会显示该阶段的详细信息，包括不同的解及其关联成本。根据阶段类型和机器人配置，可能只会显示一个解。
单击某个解的成本，可以看到机器人按该解运动的动画。单击面板右上角的 “Exec” 按钮即可实际执行该运动。
要运行 MoveIt 教程附带的完整 MTC 示例：
ros2 launch moveit2_tutorials mtc_demo.launch.py然后在第二个终端中：
ros2 launch moveit2_tutorials pick_place_demo.launch.py7.1 打印到终端的调试信息
Section titled “7.1 打印到终端的调试信息”运行 MTC 时，终端会打印类似这样的图表：
[demo_node-1] 1 - ← 1 → - 0 / initial_state [demo_node-1] - 0 → 0 → - 0 / move_to_home上面的示例显示两个阶段。第一个阶段（“initial_state”）是 CurrentState 类型，它会初始化一个规划场景并捕获当前存在的所有碰撞物体。指向该阶段的指针可用于获取机器人的状态。由于 CurrentState 继承自 Generator，它会向前和向后两个方向传播解，这由双向箭头表示。
- 最左边的
1表示一个解被成功向后传播到前一个阶段。 - 中间的
1表示生成了一个解。 - 最右边的
0表示解没有成功向前传播到下一个阶段，因为下一个阶段失败了。
第二个阶段（“move_to_home”）是 MoveTo 类型。它继承前一个阶段的传播方向，因此两个箭头都指向前方。这一行全是 0，表示该阶段完全失败。从左到右，这三个 0 的含义分别是：
- 该阶段没有从前一个阶段接收到解
- 该阶段没有生成解
- 该阶段没有将解向前传播到下一个阶段
由此可以判断，“move_to_home” 就是失败的根源。问题在于原点（home）状态本身处于碰撞中。定义一个无碰撞的原点位置即可解决这个问题。
7.2 阶段
Section titled “7.2 阶段”单个阶段的信息也可以从任务中获取。例如，以下代码获取某个阶段的唯一 ID：
uint32_t const unique_stage_id = task_.stages()->findChild(stage_name)->introspectionId();CurrentState 类型的阶段不仅用于获取机器人的当前状态，还会初始化一个规划场景对象，捕获当前存在的所有碰撞物体。
MTC 阶段可以向前和向后两个方向传播解。你可以通过 RViz 界面中的箭头直观地查看传播方向。需要注意的是，当解向后传播时，许多操作的逻辑会反转。例如，要在 ModifyPlanningScene 阶段中允许与物体碰撞，你应该调用 allowCollisions(false) 而不是 allowCollisions(true)。相关讨论可以阅读这里。