MoveIt 任务构造器

什么是 MoveIt 任务构造器？

MoveIt 任务构造器 (MTC) 框架有助于将复杂的规划任务分解为多个相互依赖的子任务。

MTC 使用 MoveIt 来解决子任务。子任务的信息通过 InterfaceState 对象传递。

MTC 阶段

MTC 阶段是指任务执行管道中的组件或步骤。
阶段可以按任意顺序排列，其层次结构仅受各个阶段类型的限制。
阶段的排列顺序受结果传递方向的限制。

与结果流相关的可能阶段有三个：

生成器
传播器
连接器

生成器阶段

生成器阶段不从相邻阶段获取输入。它们计算结果并将它们传递到两个方向 - 向前和向后。
MTC 任务的执行从生成器阶段开始。
最重要的生成器阶段是 CurrentState，它将当前机器人状态作为规划管道的起点。

监控生成器是一个监控另一个阶段（不相邻）的解决方案以使用该解决方案进行规划的阶段。

监控生成器的示例 - GeneratePose。它通常监控 CurrentState 或 ModifyPlanningScene 阶段。通过监控 CurrentState 的解决方案，GeneratePose 阶段可以找到它应该围绕其生成姿势的对象或框架。

有关 MTC 提供的生成器阶段的更多信息，请参见此处 - Generating Stages。

传播阶段

传播器从一个邻居状态接收解决方案，解决问题，然后将结果传播到另一侧的邻居。
根据实现方式，此阶段可以向前、向后或双向传递解决方案。
传播阶段的示例 - 相对于姿势“移动”。此阶段通常用于接近要拾取的物体。

有关 MTC 提供的传播阶段的更多信息，请参见此处 - 传播阶段。

连接阶段

连接器不传播任何结果，但尝试连接相邻阶段提供的起始和目标输入。

连接阶段通常解决起始状态和目标状态之间的可行轨迹。

有关 MTC 提供的连接阶段的更多信息，请参见此处 - 连接阶段。

包装器

包装器封装另一个阶段以修改或过滤结果。

包装器示例 - 计算 IK 用于``生成抓取姿势`` 阶段。生成抓取姿势 阶段将产生笛卡尔姿势解决方案。通过将``计算 IK`` 阶段包装在``生成姿势`` 阶段周围，来自``生成姿势`` 阶段的笛卡尔姿势解决方案可用于生成 IK 解决方案（即）生成机器人的关节状态配置以达到姿势。

|有关 MTC 提供的包装器的更多信息，请参见此处 - 包装器。

MTC 容器

MTC 框架使用容器实现阶段的分层组织，允许顺序和并行组合。
MTC 容器有助于组织阶段的执行顺序。
以编程方式，可以在一个容器中添加另一个容器。

当前可用的容器：

串行
并行

串行容器

串行容器以线性方式组织阶段，仅将端到端解决方案视为结果。

默认情况下，MTC 任务存储为串行容器。

并行容器

并行容器组合了一组阶段，允许规划替代解决方案。

有关并行容器的更多信息，请参见此处 - 并行容器。

初始化 MTC 任务

顶层规划问题被指定为 MTC 任务，由阶段指定的子问题被添加到 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”。

请参阅：moveit_msgs_codedir:here 以识别不同的错误类型。

如果规划成功，您可以预期计划函数将返回“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());

附加信息的链接

这里有一个教程，介绍如何使用 MTC 创建拾取和放置管道。

下面列出的链接包含有关 MTC 提供的阶段和容器的更多信息