系列文章目录
前言
一、运动学
1.1 运动学插件
MoveIt 使用了一个插件基础架构，专门用于允许用户编写自己的逆运动学算法。正向运动学和寻找雅各布因子集成在机器人状态类中。MoveIt 的默认逆运动学插件是使用 KDL 数值雅各布解算器配置的。MoveIt 设置助手会自动配置该插件。
1.1.1 碰撞检查
MoveIt 中的碰撞检查是在规划场景中使用 CollisionWorld 对象配置的。幸运的是，MoveIt 的设置使用户不必真正担心碰撞检查是如何进行的。MoveIt 中的碰撞检查主要是通过 FCL 包（MoveIt 的主要碰撞检查库）进行的。
1.2 碰撞对象
MoveIt 支持不同类型对象的碰撞检查，包括
- 网格（Meshes） - 您可以使用 .stl（标准三角形语言）或 .dae（数字资产交换）格式来描述机器人链接等对象。
- 原始形状（Primitive Shapes）--如方形、圆柱形、圆锥形、球形和平面
- 八方位图（Octomap） - 八方位图对象可直接用于碰撞检测
1.3 允许碰撞矩阵 (ACM)
碰撞检查是一项非常昂贵的操作，通常占运动规划过程中计算费用的近 90%。允许碰撞矩阵（ACM）编码了一个二进制值，该值与是否需要检查一对物体（可能在机器人上，也可能在世界中）之间的碰撞相对应。如果 ACM 中对应两个物体的值设置为 “true”，则不会对这两个物体进行碰撞检查。例如，如果两个物体始终相距甚远，永远不会发生碰撞，那么就不需要进行碰撞检查。或者，默认情况下两个物体是相互接触的，在这种情况下，应在 ACM 中禁用这对物体的碰撞检测。
二、运动规划
2.1 运动规划插件
MoveIt 通过一个插件接口与运动规划器协同工作。这使得 MoveIt 能够与多个库中的不同运动规划器进行通信并使用它们，从而使 MoveIt 易于扩展。与运动规划器的接口是通过 ROS 操作或服务（由 move_group 节点提供）。Move_group 的默认运动规划器是通过 OMPL 和 MoveIt 设置助手的 MoveIt 接口来配置的。其他默认的运动规划器包括 Pilz 工业运动规划器和 CHOMP。
2.2 运动规划请求
运动规划请求指定了您希望运动规划器做的事情。通常情况下，您会要求运动规划器将手臂移动到不同位置（关节空间），或将末端执行器移动到新姿势。默认情况下会检查碰撞（包括自碰撞和附加对象）。您还可以通过 planning_pipeline 和 planner_id 参数指定规划器，以及运动规划器需要检查的约束条件--MoveIt 提供的内置约束条件是运动学约束条件：
- 位置限制：将部件的位置限制在空间区域内。
- 方向约束：将部件的方向限制在指定的滚动、俯仰或偏航范围内。
- 可见度约束：限制部件上的点位于特定传感器的可见度锥范围内。
- 关节约束：限制一个关节位于两个值之间。
- 用户指定的约束：也可以通过用户定义的回调来指定自己的约束。
2.3 运动规划结果
move_group 节点将根据您的运动规划请求生成所需的轨迹。该轨迹将把手臂（或任意一组关节）移动到所需位置。请注意，move_group 生成的结果是一条轨迹，而不仅仅是一条路径。move_group 将使用所需的最大速度和加速度（如果已指定）来生成一条轨迹，该轨迹在关节级别上遵守速度和加速度约束。
2.4 运动规划适配器
完整的运动规划流水线将运动规划器与其他称为规划请求适配器的组件连接在一起。规划请求适配器可对运动规划请求进行预处理，并对运动规划响应进行后处理。预处理在多种情况下都很有用，例如当机器人的起始状态略微超出指定的关节限制时。其他一些操作也需要后处理，例如将为机器人生成的路径转换为时间参数化轨迹。MoveIt 提供了一组默认的运动规划适配器，每个适配器都执行非常具体的功能。
2.4.1 检查起始状态边界
固定起始状态边界适配器可将起始状态固定在 URDF 中指定的关节限制范围内。当物理机器人的关节限制配置不当时，就需要使用该适配器。这样，机器人最终可能会处于一个或多个关节略微超出其关节限制的配置中。在这种情况下，运动规划器将无法进行规划，因为它会认为起始状态超出了关节限制。检查起始状态边界（CheckStartStateBounds）“规划请求适配器会通过将起始状态移至关节极限来 ”修复 "起始状态。然而，这显然不是每次都能正确解决的问题，例如，当关节确实超出其关节限制很多时。适配器的一个参数规定了关节超出限制的程度，这样才是 “可修复 ”的。
2.4.2 验证工作区边界
固定工作区边界适配器将为规划指定一个默认工作区：大小为 10 m x 10 m x 10 m 的立方体。只有在向规划器发出的规划请求中没有填写这些字段时，才会指定该工作区。
2.4.3 检查起始状态碰撞
固定起始状态碰撞适配器将通过少量扰动关节值，尝试在指定配置（碰撞中）附近采样新的无碰撞配置。扰动值的大小由 jiggle_fraction 参数指定，该参数控制的扰动量占关节总运动范围的百分比。该适配器的另一个参数用于指定适配器在放弃之前将采样多少次随机扰动。
2.4.4 添加时间参数化
运动规划器通常会生成 “运动路径”，即不服从任何速度或加速度约束且没有时间参数化的路径。该适配器将通过应用速度和加速度约束对运动规划进行 “时间参数化”。
2.4.5 解析约束坐标系
目标约束可以使用子坐标系来设置（例如，在杯子/手柄坐标系中设置姿势目标，其中手柄是物体杯子上的一个子坐标系）。该适配器可将约束的坐标系改为对象或机器人坐标系（如杯子）。
2.5 OMPL
OMPL（开放运动规划库）是一个开源运动规划库，主要实现随机运动规划。MoveIt 直接与 OMPL 集成，并使用该库中的运动规划器作为其主要/默认规划器集。OMPL 中的规划器是抽象的，即 OMPL 没有机器人的概念。相反，MoveIt 对 OMPL 进行配置，并为 OMPL 处理机器人学问题提供后端。
三、混合规划
MoveIt 的运动规划架构遵循 “感知-规划-执行 ”的方法。为了规划和执行运动，首先要感知环境和机器人状态（“感知”），然后由规划器计算机器人轨迹（“规划”），最后使用轨迹控制器一次性执行（“执行”）。
虽然这种解决方案在众所周知的静态环境中能很好地进行全局运动规划，但在现实世界的许多应用中，尤其是在不稳定或动态环境中，这种方法并不适用。像为桌子上的人端一杯水或在不平整的黑板上写字这样的任务，需要更复杂的方法来应对不可预知的变化。例如，机器人所处的环境可能会发生动态变化，或者任务本身可能存在某些不确定性，如用粉笔写字需要调整对黑板的压力，而粉笔也会因为用完而变短。
要解决这些难题，就需要一种方法，能够使执行的运动适应当前的条件，甚至在环境发生不可预见的变化时通过重新规划做出反应。混合规划架构试图通过结合一对循环的全局和局部规划器来解决这一问题。
3.1 什么是混合规划？
混合规划是运动规划方法的一个术语，它将异构运动规划器结合起来，以产生更稳健或反应更快的解决方案。这种通用方法在导航领域已经非常成熟，并在导航2等流行项目中成功实施。
MoveIt 的混合规划架构结合了一对全局规划器和局部规划器，它们以不同的规划速度和问题范围并行、并发运行。
全局规划器的任务是解决全局运动规划问题，这与 “感知-规划-行动 ”应用中使用的规划器非常相似。所使用的规划器算法应该是完整的，因此假定其计算时间相对较慢。此外，全局规划器不要求实时安全，这意味着无法保证规划器在特定期限内找到解决方案。根据规划器的实现，全局规划器可能会在执行过程中生成一个初始解决方案或迭代优化解决方案。
局部规划器在执行过程中会持续运行，并产生迭代机器人指令，以遵循全局轨迹。在某种程度上，本地规划器类似于控制器，只是在架构上允许解决更复杂的问题和约束条件。我们的想法是，规划器能够对世界进行推理，并具有内部状态。这一特性使其用途非常广泛，因为它可以用于解决任意组合的局部规划问题，如
- 后续全局参考轨迹的开卷、混合或拼接
- 在遵循全局路径时动态避免近距离碰撞
- 根据局部约束条件调整全局轨迹（例如，不平表面上所需的力压，根据视觉反馈重新调整工具）
- 局部轨迹优化和时间参数化（在局部环境中优化轨迹的计算成本更低，速度更快）
为了能够解决这些局部问题，局部规划器必须快速，能够对传感器反馈做出反应，而且在很多情况下必须是实时安全的。此外，它还应具有确定性，以避免出现生涩或不可预测的运动。
一般来说，局部规划器依赖于全局规划器生成的参考轨迹，以避免陷入局部最小值。由于有时仍无法排除局部最小值，因此可能需要触发全局规划器进行重新规划，以便仍能达到预期目标。这种行为需要一种特定的方式来交流规划器事件，并对其进行相应处理。为此，混合规划架构允许实施基于事件的逻辑，该逻辑可根据特定用例和规划器类型进行定制。
| Global Planner | Local Planner |
|---|---|
|
|
|
|
|
|
混合规划可以在广泛的用例中发挥作用。大多数应用可分为以下三种情况。
- 在线运动规划： 全局规划器创建初始全局解决方案，并不断对其进行优化。与此同时，局部规划器执行参考轨迹，并将更新的轨迹段融合到其中。
- 反应式运动： 全局规划器用于修复失效的解决方案（重新规划），而局部规划器则在碰撞前减速或停止。
- 自适应运动： 本地规划器用于调整全局解决方案，以适应动态条件，如保持工具与不平表面的稳定接触。
3.2 混合规划架构
下图描述了构成混合规划架构的基本插件类型和 ROS 接口。
该架构由三个 ROS 组件节点组成：
- 混合规划管理器
- 为混合规划请求提供 ROS 操作
- 运行规划逻辑并协调规划人员
- 全局规划器
- 解决全局规划问题并发布解决方案轨迹
- 本地规划器
- 处理传入的全局轨迹更新
- 根据机器人状态、世界和参考轨迹解决本地规划问题
- 向机器人驱动器发送位置/速度指令
架构组件的设计具有通用性和高度可定制性。由于这些组件仅通过 ROS 2 消息接口进行交互，因此很容易替换每个架构组件或插件的实现。插件接口的设计力求最小化，并尽可能从实际算法实现中抽象出来。这样，开发人员就可以完全专注于孤立的逻辑或求解器，而无需实现基础架构的任何部分。这也允许在不同的设置或规划问题中重复使用相同的组件。
3.2.1 混合规划管理器
该组件是架构的 “大脑”。它的主要目的是处理 HybridPlanner 的操作请求，并根据规划逻辑插件协调运动规划和执行过程。规划逻辑在 PlanningLogic 插件中实现，在设计上是事件驱动的。事件由字符串标识符定义，可触发针对全局或本地规划器的动作调用或取消。下图是一个简单规划逻辑的事件日志示例：
事件由混合规划操作请求以及全局和本地规划器操作反馈消息触发。在本例中，混合规划管理器在收到混合规划请求后启动全局规划器。全局轨迹到达后，本地规划器启动，本地规划器完成后，混合规划管理器返回混合规划响应。
规划逻辑插件的定制实现支持将通用事件映射到架构提供的可用操作，如 “启动全局规划”、“停止轨迹执行 ”或 “切换到本地规划器约束 x”。这样，运动规划行为就具有了高度的可定制性和适应性。
3.2.2 全局规划器
全局规划器是架构中最简单的组件。它提供了一个可处理 GlobalPlanner 请求的动作服务器。这些请求包括由全局规划器插件处理的普通 MotionPlanRequests。默认情况下，这只是 MoveIt 的规划管道，但从技术上讲，任何类型的规划器甚至 MTC 都可以在这里使用。规划结果将使用动作反馈进行报告，解决方案轨迹将发布到本地规划器进行进一步处理。
3.2.3 局部规划器
局部规划器还运行一个动作服务器，处理来自混合规划管理器的请求。该操作用于启动和停止执行，也可以配置运行时参数，如约束条件或求解器类型。
本地规划器的实现基于两个插件：
- 轨迹操作器： 该插件维护全局参考轨迹，处理来自全局规划器的轨迹更新，并监控当前机器人状态的进程。
- 局部约束求解器： 该插件实现迭代求解器算法，根据参考轨迹和本地约束条件生成机器人指令。它可能包括用于动态处理传感器输入或事件更新的附加接口。
下图显示了本地规划器在收到混合规划管理器的操作请求后的循环示例：
每次迭代，本地规划器都会请求当前规划场景，并在参考轨迹内匹配当前机器人状态。如果目标达成，则本地规划行动成功结束。否则，将根据当前的机器人状态确定当前的本地规划问题，之后进行求解。最后，将得到的控制指令发布给机器人控制器。
3.2.4 它是如何工作的？
通过绘制工作流程图，将不同组件的通信渠道和事件可视化，可以最好地理解混合规划器的运行时行为。
下图展示了成功执行轨迹的运行逻辑。
规划器由混合规划请求调用，这也是混合规划管理器响应的第一个事件。在本例中，规划器逻辑只是依次运行两个规划器。初始混合规划请求发出后，混合规划管理器会调用全局规划器。全局规划器计算并发布一条轨迹，混合规划管理器和本地规划器组件会收到这条轨迹。需要注意的是，本地规划组件只是处理新轨迹，在混合规划管理器调用之前不会开始执行。一旦混合规划管理器发出请求，本地规划组件就会开始展开参考轨迹，并在到达最终状态时成功返回动作响应。之后，混合规划管理器返回成功的混合规划响应（HybridPlanningResponse）。
现在让我们考虑一个更困难的场景，即混合规划器在执行过程中通过重新规划来避开障碍物。下面的动画展示了一个简单的运动，由于碰撞对象的变化，该运动在运行时被固定。
在这里，全局规划过程中出现的碰撞对象会在计算出全局轨迹后消失。取而代之的是两个新的碰撞对象，它们使初始全局轨迹失效。本地规划器会检测到即将发生的碰撞，并暂停执行，直到全局规划器提供更新的无碰撞轨迹。
下面是所述行为的工作流程。
启动过程与第一个示例相同，但在展开参考轨迹的过程中，本地规划器检测到了碰撞。在这种情况下，规划器逻辑的反应是重新调用全局规划器。在计算新的全局解决方案时，本地规划器必须防止机器人与碰撞对象发生碰撞，即保持机器人当前的位置。在全局规划器完成计算后，新的全局解决方案将发布到本地规划器，本地规划器的轨迹运算器插件将更新内容融合到参考轨迹中。之后，本地规划器组件将继续遵循参考轨迹，因为更新后的解决方案使其能够绕着碰撞对象转向。
如果您想在自己的应用程序中使用混合规划，或者只是想尝试一下，请查看混合规划示例教程。
四、move_group 节点
下图展示了 MoveIt 提供的一个名为 move_group 的关键节点的高层系统架构。该节点的作用是集成：将所有单独的组件整合在一起，为用户提供一系列 ROS 操作和服务。
4.1 用户接口
用户可通过两种方式访问 move_group 提供的操作和服务：
- 通过 C++ - 使用 move_group_interface 软件包，它为 move_group 提供了一个易于设置的 C++ 接口
- 通过图形用户界面--使用 Rviz（ROS 可视化器）的运动规划插件
4.2 配置
move_group 是一个 ROS 节点。它使用 ROS param 服务器获取三种信息：
- URDF move_group 会查找 robot_description_semantic 参数，以获取机器人的 SRDF。SRDF 通常由用户使用 MoveIt 设置助手创建（一次）。
- SRDF move_group 会查找 robot_description 参数，以获取机器人的 URDF。
- MoveIt 配置 move_group 会查找 MoveIt 特有的其他配置，包括关节限制、运动学、运动规划、感知和其他信息。这些组件的配置文件由 MoveIt 设置助手自动生成，并存储在机器人相应 MoveIt 配置包的配置目录中。
4.3 机器人界面
move_group 通过 ROS 的话题和动作与机器人对话。它通过与机器人通信来获取当前状态信息（关节位置等），从机器人传感器获取点云和其他传感器数据，并与机器人上的控制器进行对话。
4.4 关节状态信息
move_group 可监听 /joint_states 话题，以确定当前状态信息，即确定机器人每个关节的位置。move_group 可监听此话题上的多个发布者，即使它们发布的只是机器人状态的部分信息（例如，机器人的手臂和移动底座可使用不同的发布者）。请注意，move_group 不会建立自己的关节状态发布器，这需要在每个机器人上实现。
4.5 变换信息
move_group 使用 ROS TF 库监控变换信息。这样，节点就能获得有关机器人姿势的全局信息（以及其他信息）。例如，ROS 导航堆栈会向 TF 发布机器人的地图坐标系和基准坐标系之间的变换。move_group 可以使用 TF 计算出该变换，供内部使用。请注意，move_group 只监听 TF。要发布机器人的 TF 信息，需要在机器人上运行 robot_state_publisher 节点。
4.6 控制器接口
move_group 使用 FollowJointTrajectoryAction 接口与机器人上的控制器对话。这是一个 ROS 动作接口。机器人上的服务器需要为该动作提供服务，而 move_group 本身并不提供该服务器。move_group 只会实例化一个客户端，以便与机器人上的控制器动作服务器进行对话。
4.7 规划场景
move_group 使用 “规划场景监控器”（Planning Scene Monitor）来维护规划场景，该场景是世界和机器人当前状态的表示。机器人的状态可以包括任何附着在机器人上（由机器人携带）的物体，这些物体被认为是刚性附着在机器人上的。有关维护和更新规划场景架构的更多详情，请参阅下文的 “规划场景 ”部分。
4.8 可扩展功能
move_group 的结构易于扩展。拾取和放置、运动学、运动规划等单项功能实际上是作为具有共同基类的独立插件来实现的。这些插件可通过一组 ROS yaml 参数和 ROS pluginlib 库使用 ROS 进行配置。大多数用户无需配置 move_group 插件，因为 MoveIt 安装助手生成的启动文件会自动配置这些插件。
五、规划场景监控器
规划场景是一个用于存储机器人周围世界表示法和机器人自身状态的对象。规划场景对象的内部状态通常由规划场景监控器（planning_scene_monitor）组件来维护，该组件能以线程安全的方式读写规划场景对象的状态。
5.1 世界几何监控器
世界几何监控器利用机器人上的传感器（如激光雷达或深度摄像头）和用户输入的信息构建世界几何。它使用下面描述的占用地图监控器来构建机器人周围环境的三维表示法，并使用 planning_scene 话题上的信息对其进行扩充，以添加对象信息。
5.2 三维感知
MoveIt 中的三维感知由占用图监控器处理。如上图所示，占用图监视器使用插件架构来处理不同类型的传感器输入。尤其是，MoveIt 内置支持处理两种输入：
- 点云：由 PointCloudOccupancyMapUpdater 插件处理。
- 深度图像：由 DepthImageOccupancyMapUpdater 插件处理。
请注意，您可以将自己类型的更新器作为插件添加到占用图监视器中。
5.3 八方图
占用图监控器使用 Octomap 来维护环境的占用图。Octomap 实际上可以编码单个单元的概率信息，尽管 MoveIt 目前还没有使用这些信息。Octomap 可以直接传入 FCL（MoveIt 使用的碰撞检查库）。
5.3.1 深度图像占用图更新器
深度图占位图更新器包含自己的自滤波器，即它会从深度图中移除机器人的可见部分。它使用机器人的当前信息（机器人状态）来执行此操作。
六、轨迹处理
6.1 时间参数化
运动规划器通常只生成 “路径”，即没有与路径相关的时间信息。MoveIt 包含几种轨迹处理算法，可以处理这些路径，并根据施加在各个关节上的最大速度和加速度限制生成适当时间参数化的轨迹。这些限制是从为每个机器人指定的特殊 joint_limits.yaml 配置文件中读取的。配置文件是可选的，它会覆盖 URDF 中的任何速度或加速度限制。截至 2023 年 1 月的推荐算法是时间最优轨迹生成算法（TimeOptimalTrajectoryGeneration，TOTG）。该算法的一个注意事项是，机器人必须以静止状态开始和结束。默认情况下，TOTG 的时间步长为 0.1 秒。
七、MoveIt 任务构造器
7.1 什么是 MoveIt 任务构造器？
MoveIt Task Constructor（MTC）框架有助于将复杂的规划任务分解为多个相互依存的子任务。
MTC 使用 MoveIt 解决子任务。来自子任务的信息通过 InterfaceState 对象传递。
7.2 MTC 阶段
MTC 阶段是指任务执行流水线中的一个组件或步骤。
阶段可按任意顺序排列，其层次结构仅受各个阶段类型的限制。
阶段的排列顺序受结果传递方向的限制。
有三种可能的阶段与结果流有关：
- 生成器
- 传播者
- 连接器
7.2.1 生成器阶段
生成器级不从相邻级获得输入。它们计算结果，并向前和向后两个方向传递。
MTC 任务的执行从生成器阶段开始。
最重要的生成器阶段是当前状态（CurrentState），它获取当前的机器人状态，作为规划流水线的起点。
监控生成器是一个阶段，它监控另一个阶段（不相邻）的解决方案，以便将解决方案用于规划。
监控生成器的示例--GeneratePose。它通常监控一个 CurrentState 或 ModifyPlanningScene 阶段。通过监控 CurrentState 的解决方案，GeneratePose 阶段可以找到它应围绕其生成姿势的对象或坐标系。
有关 MTC 提供的生成阶段的更多信息，请点击此处--生成阶段。
7.2.2 传播阶段
传播器从一个相邻状态接收解决方案，解决问题，然后将结果传播给对面的相邻状态。
根据实现方式的不同，这一阶段可以向前、向后或双向传递解法。
传播阶段示例--相对于姿势移动。该阶段通常用于靠近物体进行拾取。
有关 MTC 提供的传播阶段的更多信息，请点击此处 - 传播阶段。
7.2.3 连接阶段
连接阶段不传播任何结果，但试图连接相邻阶段提供的起始和目标输入。
连接阶段通常会求解起始和目标状态之间的可行轨迹。
有关 MTC 所提供的连接阶段的更多信息，请点击此处 - 连接阶段。
7.2.4 封装器
封装器封装另一个阶段，以修改或过滤结果。
封装器示例--为 “生成抓握姿势 ”阶段计算 IK。生成抓取姿势 "阶段将生成笛卡尔姿势解决方案。通过在 “生成抓取姿势 ”阶段周围包装 “计算 IK ”阶段，可以利用 “生成抓取姿势 ”阶段的笛卡尔姿势解决方案来生成 IK 解决方案（即生成机器人的关节状态配置，以达到姿势）。
有关 MTC 提供的封装器的更多信息，请点击此处 - 封装器。
7.3 MTC 容器
MTC 框架可使用容器对阶段进行分层组织，允许顺序和并行组合。
MTC 容器有助于组织各阶段的执行顺序。
通过编程，可以在另一个容器中添加一个容器。
目前可用的容器有
- 串行
- 并行
7.3.1 串行容器
串行容器以线性方式组织阶段，只将端到端的解决方案视为结果。
MTC 任务默认存储为串行容器。
7.3.2 并行容器
并行容器结合了一组阶段，允许规划备用解决方案。
有关并行容器的更多信息，请点击此处--并行容器。
7.4 初始化 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");7.5 为 MTC 任务添加容器和阶段
在 MTC 任务中添加阶段
auto current_state = std::make_unique<moveit::task_constructor::stages::CurrentState>("current_state");
task->add(std::move(current_state));容器源于阶段，因此容器也可以类似地添加到 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));7.6 设置规划求解器
进行运动规划的阶段需要求解器信息。
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 等阶段。
7.7 设置属性
每个 MTC 阶段都有可配置的属性。例如 - 规划组、超时、目标状态等。
不同类型的属性可以使用下面的函数进行设置。
void setProperty(const std::string& name, const boost::any& value);子阶段可轻松继承父阶段的属性，从而减少配置开销。
7.8 阶段成本计算器
CostTerm 是计算 MTC 阶段解决方案成本的基本接口。
MTC 中可用的 CostTerm 实现
- 常数 - 为每个解决方案添加一个常数成本
- PathLength - 成本取决于轨迹长度，不同关节的权重可选
- 轨迹持续时间 - 成本取决于整个轨迹的执行持续时间
- TrajectoryCostTerm - 仅适用于子轨迹解决方案的成本项
- LambdaCostTerm - 通过 lambda 表达式计算成本
- DistanceToReference - 成本取决于加权关节空间到参考点的距离
- 部件运动 - 成本取决于部件的笛卡尔轨迹长度
- 间隙 - 成本是碰撞距离的倒数
如何使用 LambdaCostTerm 设置 CostTerm 的示例代码
stage->setCostTerm(moveit::task_constructor::LambdaCostTerm(
[](const moveit::task_constructor::SubTrajectory& traj) { return 100 * traj.cost(); }));MTC 提供的所有阶段都有默认的成本项。生成轨迹作为解决方案的阶段通常使用路径长度来计算成本。
7.9 规划和执行 MTC 任务
规划 MTC 任务将返回 MoveItErrorCode。请在此查看不同的错误类型。如果规划成功，则计划函数将返回 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());7.10 其他信息链接
以下是如何使用 MTC 创建拾取和放置流水线的教程。
以下链接包含有关 MTC 提供的阶段和容器的更多信息
- 生成阶段
- 传播阶段
- 连接阶段
- 包装器
- 并行容器
- 调试 MTC 任务
1823
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
