使用 MoveIt 任务构造器进行拾取和放置

本教程将引导您创建一个使用 MoveIt 任务构造器 规划拾取和放置操作的包。MoveIt 任务构造器提供了一种规划由多个不同子任务（称为阶段）组成的任务的方法。如果您只想运行本教程，您可以按照 Docker 指南 启动包含完整教程的容器。

1 基本概念

MTC 的基本思想是，复杂的运动规划问题可以组合成一组更简单的子问题。 顶层规划问题被指定为**任务**，而所有子问题则由**阶段**指定。 阶段可以按任意顺序排列，层次结构仅受各个阶段类型的限制。 阶段的排列顺序受结果传递方向的限制。 与结果流相关的可能阶段有三个：生成器、传播器和连接器阶段：

**生成器**独立于其相邻阶段计算其结果，并向后和向前两个方向传递它们。 一个例子是用于几何姿势的 IK 采样器，其中接近和离开运动（相邻阶段）取决于解决方案。

**传播器**接收一个相邻阶段的结果，解决子问题，然后将其结果传播到对面站点的邻居。 根据实现方式，传播阶段可以分别向前、向后或双向传递解决方案。 一个例子是基于起始状态或目标状态计算笛卡尔路径的阶段。

连接器 不传播任何结果，而是试图弥合两个邻居的结果状态之间的差距。 一个例子是从一个给定状态到另一个给定状态的自由运动计划的计算。

除了顺序类型之外，还有不同的层次结构类型允许封装下级阶段。 没有下级阶段的阶段称为 原始阶段，更高级别的阶段称为 容器阶段。 有三种容器类型：

包装器 封装单个下级阶段并修改或过滤结果。 例如，仅接受满足特定约束的子阶段解决方案的过滤阶段可以实现为包装器。 此类型的另一个标准用途包括 IK 包装器阶段，它基于用姿势目标属性注释的规划场景生成逆运动学解决方案。

串行容器 包含一系列从属阶段，并且仅将端到端解决方案视为结果。 一个例子是拾取动作，它由一系列连贯的步骤组成。

并行容器 结合了一组从属阶段，可用于传递最佳替代结果、运行后备求解器或合并多个独立解决方案。 例如，运行自由运动计划的替代规划器、用右手或左手拾取物体作为后备，或者同时移动手臂和打开夹持器。

../../../_images/mtc_stage_types.png

阶段不仅支持解决运动规划问题。 它们还可用于各种状态转换，例如修改规划场景。 结合使用类继承的可能性，可以构建非常复杂的行为，同时仅依赖一组结构良好的原始阶段。

有关 MTC 的更多详细信息，请参阅：doc:MoveIt 任务构造器概念页面

2 入门

如果您尚未完成，请确保您已完成：doc:`入门 </doc/tutorials/getting_started/getting_started>`中的步骤。

移动到您的 colcon 工作区并拉取 MoveIt 任务构造器源，其中 <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 release

3 试用

MoveIt 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

在右侧，您应该会看到**运动规划任务**面板，其中概述了任务的层次结构。 当您选择特定阶段时，成功和失败的解决方案列表将显示在最右侧的窗口中。选择其中一个解决方案将开始其可视化。

../../../_images/mtc_show_stages1.gif

4 使用 MoveIt 任务构造器设置项目

本节介绍使用 MoveIt 任务构造器构建简单任务所需的步骤。

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 代码

在您选择的编辑器中打开“mtc_node.cpp”，并粘贴以下代码。

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#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 代码分解

代码顶部包含此包使用的 ROS 和 MoveIt 库。

  • rclcpp/rclcpp.hpp 包含核心 ROS2 功能

  • moveit/planning_scene/planning_scene.h 和 moveit/planning_scene_interface/planning_scene_interface.h 包含与机器人模型和碰撞对象交互的功能

  • moveit/task_constructor/task.h、moveit/task_constructor/solvers.h 和 moveit/task_constructor/stages.h 包含示例中使用的 MoveIt Task Constructor 的不同组件

  • tf2_geometry_msgs/tf2_geometry_msgs.hpp 和 tf2_eigen/tf2_eigen.hpp 不会在此初始示例中使用，但当我们向 MoveIt Task Constructor 任务添加更多阶段时，它们将用于姿势生成。

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#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

下一行获取新节点的记录器。为了方便起见，我们还为“moveit::task_constructor”创建了一个命名空间别名。

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
namespace mtc = moveit::task_constructor;

我们首先定义一个包含主要 MoveIt 任务构造函数功能的类。我们还将 MoveIt 任务构造函数任务对象声明为我们类的成员变量：这对于给定的应用程序来说并不是绝对必要的，但它有助于保存任务以供以后可视化。我们将在下面分别探讨每个函数。

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 函数来获取节点基接口，稍后将用于执行器。

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

此函数与 MoveIt Task Constructor 任务对象交互。它首先创建一个任务，其中包括设置一些属性和添加阶段。这将在 createTask 函数定义中进一步讨论。接下来，task.init() 初始化任务，task.plan(5) 生成计划，在找到 5 个成功计划后停止。下一行发布要在 RViz 中可视化的解决方案 - 如果您不关心可视化，可以删除此行。最后，task.execute() 执行该计划。执行通过与 RViz 插件的操作服务器接口进行。

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;
}

如上所述，此函数创建一个 MoveIt Task Constructor 对象并设置一些初始属性。在本例中，我们将任务名称设置为“demo_task”，加载机器人模型，定义一些有用帧的名称，并使用“task.setProperty(property_name, value)”将这些帧名称设置为任务的属性。接下来的几个代码块将填充此函数体。

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”；这会创建一个指向阶段的指针，以便我们可以在特定场景中重用阶段信息。此行目前未使用，但稍后在向任务添加更多阶段时将使用。接下来，我们创建一个“current_state”阶段（生成器阶段）并将其添加到我们的任务中 - 这将以当前状态启动机器人。现在我们已经创建了“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));

解算器用于定义机器人运动的类型。MoveIt Task Constructor 有三个解算器选项：

PipelinePlanner uses MoveIt’s planning pipeline, which typically defaults to OMPL.

auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);

JointInterpolation is a simple planner that interpolates between the start and goal joint states. It is typically used for simple motions as it computes quickly but doesn’t support complex motions.

auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();

CartesianPath is used to move the end effector in a straight line in Cartesian space.

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``阶段（传播器阶段）。由于张开手是一个相对简单的动作，我们可以使用关节插值规划器。 此阶段计划移动到“张开手”姿势，这是在 SRDF 中为 Panda 机器人定义的命名姿势。 我们返回任务并使用``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 运行演示

5.1 启动文件

我们需要一个启动文件来启动 move_group、ros2_control、static_tf、robot_state_publisher 和 rviz 节点，这些节点为我们提供运行演示的环境。我们将在本例中使用的文件可以在 here 找到。

要运行 MoveIt Task Constructor 节点，我们将使用第二个启动文件以适当的参数启动 mtc_tutorial 可执行文件。在这里，我们可以加载 URDF、SRDF 和 OMPL 参数，或者使用 MoveIt Configs Utils 来执行此操作。您的启动文件应类似于本教程包中的启动文件：codedir:此处 <tutorials/pick_and_place_with_moveit_task_constructor/launch/pick_place_demo.launch.py​​>`（请密切注意下面的 ``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​​”或将其下载到包的启动目录中。确保编辑“CMakeLists.txt”，使其包含启动文件夹，方法是添加以下几行：:

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
  )

现在我们可以构建并获取 colcon 工作区。:

cd ~/ws_moveit
colcon build --mixin release
source ~/ws_moveit/install/setup.bash

首先启动第一个启动文件。如果您想使用教程提供的文件：:

ros2 launch moveit2_tutorials mtc_demo.launch.py

RViz 现在将加载。如果您使用的是自己的启动文件，并且没有包含 rviz 配置 例如此，则需要先配置 RViz，然后才能看到任何显示内容。如果您使用的是教程包中的启动文件，则 RViz 将已为您配置，您可以跳到下一节的末尾。

5.2 RViz 配置

如果您不使用提供的 RViz 配置，我们将不得不对 RViz 配置进行一些更改，以查看您的机器人和 MoveIt 任务构造器解决方案。首先，启动 RViz。以下步骤将介绍如何设置 RViz 以实现 MoveIt 任务构造器解决方案可视化。

  1. 如果 MotionPlanning 显示处于活动状态，请取消选中它以暂时隐藏它。

  2. 在 Global Options 下，将 Fixed Frame 从 map 更改为 ``panda_link0``（如果尚未完成）。

  3. 在窗口左下方，单击 Add 按钮。

  4. 在 moveit_task_constructor_visualization 下，选择 Motion Planning Tasks 并单击 OK。Motion Planning Tasks 显示应出现在左下方。

  5. 在 Displays 中的 Motion Planning Tasks 下，将 Task Solution Topic 更改为 /solution

您应该在主视图中看到熊猫手臂，左下方打开 Motion Planning Tasks 显示，但其中没有任何内容。启动 mtc_tutorial 节点后，您的 MTC 任务将显示在此面板中。如果您正在使用教程中的“mtc_demo.launch.py​​”，请跳转回此处。

5.3 启动演示

使用 :: 启动“mtc_tutorial”节点

ros2 launch mtc_tutorial pick_place_demo.launch.py

您应该看到手臂以单级方式执行任务以张开手，其前面有一个绿色圆柱体。它看起来应该像这样：

../../../_images/first_stages.png

如果您还没有制作自己的包，但仍想看看它是什么样子，您可以从教程中启动此文件：:

ros2 launch moveit2_tutorials mtc_demo_minimal.launch.py

6 添加阶段

到目前为止，我们已经完成了创建和执行简单任务的演示，该任务可以运行，但作用不大。现在，我们将开始向任务中添加拾取和放置阶段。下图显示了我们将在任务中使用的阶段的概述。

../../../_images/stages.png

我们将在现有的开放手阶段之后开始添加阶段。打开“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 here

6.1 拾取阶段

我们需要将手臂移动到可以拾取物体的位置。这通过“连接”阶段完成，顾名思义，这是一个连接器阶段。这意味着它试图在前后阶段的结果之间架起桥梁。此阶段使用名称“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));

接下来，我们创建一个指向 MoveIt 任务构造函数阶段对象的指针，并暂时将其设置为“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”也是一个生成器阶段 - 要连接第一阶段和此阶段，必须使用我们已经在上面创建的连接阶段。 此代码设置阶段属性，设置抓取前的姿势、角度增量和监控阶段。 角度增量是“GenerateGraspPose”阶段的一个属性，用于确定要生成的姿势数量；在生成解决方案时，MoveIt 任务构造器将尝试从许多不同的方向抓取物体，角度增量指定方向之间的差异。增量越小，抓取方向就越接近。在定义当前阶段时，我们设置“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::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”阶段，并将其命名为“生成姿势 IK”以及上面定义的“生成抓取姿势”阶段。一些机器人对于给定的姿势有多个逆运动学解决方案 - 我们将要解决的解决方案数量限制为最多 8 个。我们还设置了最小解决方案距离，这是不同解决方案必须有多大的阈值：如果解决方案中的关节位置与前一个解决方案太相似，它将被标记为无效。接下来，我们配置一些附加属性，并将“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``函数允许我们指定要禁用哪些碰撞。 ``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”阶段完成的，类似于上面的“张开手”阶段，只是移动到 SRDF 中定义的“合拢”位置。

{
  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.py

6.2 放置阶段

现在定义拾取的阶段已经完成，我们继续定义放置对象的阶段。接着我们上次停止的地方，我们添加一个“连接”阶段来连接这两个阶段，因为我们很快就会使用生成器阶段来生成放置对象的姿势。

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

下一个阶段生成用于放置物体的姿势并计算这些姿势的逆运动学 - 它有点类似于 pick 串行容器中的“生成抓取姿势”阶段。 我们首先创建一个阶段来生成姿势并继承任务属性。 我们使用来自“geometry_msgs”的“PoseStamped”消息指定我们想要放置物体的姿势 - 在本例中，我们在“object”框架中选择“y = 0.5”。 然后我们使用“setPose”将目标姿势传递给阶段。 接下来，我们使用“setMonitoredStage”并将指针传递给之前的“attach_object”阶段。 这允许阶段知道物体是如何附着的。 然后我们创建一个“ComputeIK”阶段并将我们的“GeneratePlacePose”阶段传递给它 - 其余部分遵循与上面 pick 阶段相同的逻辑。

{
  // 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));
}

既然我们不再需要握住物体，我们也可以重新启用与物体的碰撞。 使用“allowCollisions”完成此操作几乎与禁用碰撞完全相同，只是将最后一个参数设置为“false”而不是“true”。

{
  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”阶段从物体处撤退，这与“接近物体”和“抬起物体”阶段类似。

{
  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.py

7 进一步讨论

每个组成阶段的任务都显示在运动规划任务窗格中。单击某个阶段，右侧将显示有关该阶段的其他信息。右侧窗格显示不同的解决方案及其相关成本。根据阶段类型和机器人配置，可能只显示一个解决方案。

单击其中一个解决方案成本可查看机器人按照该阶段的计划进行的动画。单击窗格右上角的“执行”按钮以执行运动。

要运行 MoveIt 教程中包含的完整 MoveIt 任务构造器示例：:

ros2 launch moveit2_tutorials mtc_demo.launch.py

在第二个终端中：:

ros2 launch moveit2_tutorials pick_place_demo.launch.py

7.1 打印到终端的调试信息

运行 MTC 时，它会将如下图表打印到终端：

[demo_node-1]     1  - ←   1 →   -  0 / initial_state
[demo_node-1]     -  0 →   0 →   -  0 / move_to_home

此示例^显示了两个阶段。第一阶段（“initial_state”）是“CurrentState”类型的阶段，它初始化“PlanningScene”并捕获当时存在的任何碰撞对象。 指向此阶段的指针可用于检索机器人的状态。 由于“CurrentState”继承自“Generator”，因此它会向前和向后传播解决方案。 这由两个方向的箭头表示。

  • 第一个“1”表示一个解决方案已成功向后传播到上一阶段。

  • 箭头之间的第二个“1”表示生成了一个解决方案。

  • “0”表示解决方案未成功向前传播到下一阶段，因为下一阶段失败了。

第二阶段（“move_to_home”）是“MoveTo”类型的阶段。它从上一阶段继承了传播方向，因此两个箭头都指向前方。 “0” 表示此阶段完全失败。从左到右，“0” 表示：

  • 该阶段未从上一阶段收到解决方案

  • 该阶段未生成解决方案

  • 该阶段未将解决方案传播到下一阶段

在这种情况下，我们可以说“move_to_home”是失败的根本原因。问题是发生碰撞的 home 状态。定义一个新的、无碰撞的 home 位置解决了这个问题。

7.2 阶段

可以从任务中检索有关各个阶段的信息。例如，我们在这里检索阶段的唯一 ID：:

uint32_t const unique_stage_id = task_.stages()->findChild(stage_name)->introspectionId();

CurrentState 类型阶段不仅仅检索机器人的当前状态。 它还初始化 PlanningScene 对象，捕获当时存在的任何碰撞对象。

MTC 阶段可以正向和反向传播。 您可以通过 RViz GUI 中的箭头轻松检查阶段传播的方向。 当向后传播时，许多操作的逻辑会被逆转。 例如，要允许与 ModifyPlanningScene 阶段中的对象发生碰撞，您可以调用 allowCollisions(false) 而不是 allowCollisions(true)。这里有一个讨论可以阅读。<https://github.com/moveit/moveit_task_constructor/issues/349>`_