MoveIt 任务构造器

MoveIt 任务构造器（MoveIt Task Constructor，MTC）框架旨在将复杂的规划任务分解为多个相互依赖的子任务，并使用 MoveIt 来求解这些子任务。子任务之间的信息通过 InterfaceState 对象传递。

MTC 阶段（Stages）

MTC 阶段（Stage）是任务执行流水线中的一个组件或步骤。阶段可以按任意顺序排列，其层级结构仅受各阶段类型的限制，且排列顺序还受结果传递方向的约束。

根据结果的流向，阶段分为三种类型：

Generators
Propagators
Connectors

生成器阶段（Generator Stage）

生成器阶段不从相邻阶段接收输入，而是自行计算结果，并沿两个方向——向前和向后——传递结果。MTC 任务的执行从生成器阶段开始。最重要的生成器阶段是 CurrentState，它获取当前机器人状态作为规划流水线的起点。

监控生成器（Monitoring Generator）是一种特殊的生成器，它会监控另一个（不相邻的）阶段的解，并利用这些解进行规划。一个典型的例子是 GeneratePose：它通常监控 CurrentState 或 ModifyPlanningScene 阶段。通过监控 CurrentState 的解，GeneratePose 阶段可以找到需要在其周围生成位姿的对象或坐标系。

关于 MTC 提供的生成器阶段的更多信息，请参阅生成阶段。

传播阶段（Propagating Stage）

传播器（Propagator）从一个相邻状态接收解，求解问题，然后将结果传播给另一侧的相邻状态。根据实现的不同，该阶段可以向前、向后或双向传递解。传播阶段的一个典型例子是相对于某个位姿移动（MoveRelative），通常用于接近要抓取的物体。

关于 MTC 提供的传播阶段的更多信息，请参阅传播阶段。

连接阶段（Connecting Stage）

连接器（Connector）不传播任何结果，而是尝试连接由相邻阶段提供的起始输入和目标输入。连接阶段通常用于求解起始状态与目标状态之间的可行轨迹。

关于 MTC 提供的连接阶段的更多信息，请参阅连接阶段。

包装器（Wrapper）

包装器封装另一个阶段，以修改或过滤其结果。一个典型的例子是将 ComputeIK 包裹在 GenerateGraspPose 阶段外层：GenerateGraspPose 阶段会产生笛卡尔位姿解，而在其外层包裹 ComputeIK 后，即可利用这些笛卡尔位姿解来计算逆运动学解（即生成机器人到达这些位姿所需的关节状态配置）。

关于 MTC 提供的包装器的更多信息，请参阅包装器。

MTC 容器（Containers）

MTC 框架支持使用容器对阶段进行层级化组织，既支持串行组合，也支持并行组合。MTC 容器有助于组织阶段的执行顺序。从编程角度讲，可以在一个容器内再嵌套另一个容器。

当前可用的容器类型：

Serial
Parallel

串行容器（Serial Container）

串行容器将阶段线性组织起来，仅将端到端的完整解视为最终结果。MTC 任务默认以串行容器存储。

并行容器（Parallel Container）

并行容器将一组阶段组合起来，以规划备选解决方案。

关于并行容器的更多信息，请参阅并行容器。

初始化 MTC 任务

顶层规划问题对应一个 MTC 任务（Task），由阶段（Stage）指定的子问题则被添加到 MTC 任务对象中。

  auto node = std::make_shared<rclcpp::Node>();
  auto task = std::make_unique<moveit::task_constructor::Task>();
  task->loadRobotModel(node);
  // Set controllers used to execute robot motion. If not set, MoveIt has controller discovery logic.
  task->setProperty("trajectory_execution_info", "joint_trajectory_controller gripper_controller");

向 MTC 任务添加容器和阶段

向 MTC 任务添加阶段：

  auto current_state = std::make_unique<moveit::task_constructor::stages::CurrentState>("current_state");
  task->add(std::move(current_state));

容器派生自 Stage，因此可以用同样的方式向 MTC 任务添加容器：

  auto container = std::make_unique<moveit::task_constructor::SerialContainer>("Pick Object");
  // TODO: Add stages to the container before adding the container to MTC task
  task->add(std::move(container));

设置规划求解器

涉及运动规划的阶段需要配置求解器。

MTC 中可用的求解器：

PipelinePlanner - 使用 MoveIt 的规划流水线
JointInterpolation - 在起始与目标关节状态之间进行插值，不支持复杂运动。
CartesianPath - 使末端执行器在笛卡尔空间中沿直线运动。

初始化求解器的代码示例：

  const auto mtc_pipeline_planner = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(
      node, "ompl", "RRTConnectkConfigDefault");
  const auto mtc_joint_interpolation_planner =
      std::make_shared<moveit::task_constructor::solvers::JointInterpolationPlanner>();
  const auto mtc_cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();

这些求解器将被传入 MoveTo、MoveRelative 和 Connect 等阶段。

设置属性

每个 MTC 阶段都有可配置的属性，例如规划组、超时时间、目标状态等。可以使用以下函数设置不同类型的属性：

  void setProperty(const std::string& name, const boost::any& value);

子阶段可以轻松继承父阶段的属性，从而减少配置开销。

阶段的成本计算器

CostTerm 是计算 MTC 阶段解决方案成本的基础接口。

MTC 中可用的 CostTerm 实现：

Constant - 为每个解决方案添加一个恒定成本
PathLength - 成本取决于路径长度，可为不同关节设置权重
TrajectoryDuration - 成本取决于整条轨迹的执行时长
TrajectoryCostTerm - 仅作用于 SubTrajectory 解决方案的成本项
LambdaCostTerm - 传入 lambda 表达式来计算成本
DistanceToReference - 成本取决于到参考点的加权关节空间距离
LinkMotion - 成本取决于某个连杆的笛卡尔轨迹长度
Clearance - 成本为到碰撞距离的倒数

使用 LambdaCostTerm 设置 CostTerm 的示例代码：

  stage->setCostTerm(moveit::task_constructor::LambdaCostTerm(
        [](const moveit::task_constructor::SubTrajectory& traj) { return 100 * traj.cost(); }));

MTC 提供的所有阶段都有默认的成本项。产生轨迹作为解决方案的阶段通常使用路径长度来计算成本。

规划并执行 MTC 任务

规划 MTC 任务将返回一个 MoveItErrorCode。请参阅 MoveItErrorCodes.msg 了解不同的错误类型。如果规划成功，plan 函数将返回 moveit_msgs::msg::MoveItErrorCodes::SUCCESS。

  auto error_code = task.plan()

规划完成后，提取第一个成功的解决方案并将其传递给执行函数。这将创建一个 execute_task_solution 动作客户端。动作服务器位于 MTC 提供的 execute_task_solution_capability 插件中，该插件扩展了 MoveGroupCapability：它根据 MTC 解决方案构建一个 MotionPlanRequest，并使用 MoveIt 的 PlanExecution 来驱动机器人。

  auto result = task.execute(*task.solutions().front());