ROS2 - Moveit2 - Planning with Approximated Constraint Manifolds(使用近似约束流形进行规划)

使用近似约束流形进行规划

OMPL 支持自定义约束，以使规划轨迹遵循所需的行为。约束可以在关节空间和笛卡尔空间中定义，后者基于方向或位置。在规划轨迹时，每个关节状态都需要遵循所有设置的约束，默认情况下，这是通过拒绝采样来执行的。然而，这可能会导致非常长的规划时间，特别是当约束非常严格且拒绝率相应较高时。

Sucan 等人提出了一种方法，他们预先计算约束流形的近似值，并在其中执行轨迹规划。OMPL 插件包含针对给定的一组约束执行此操作并将其保存在数据库中的功能。在以后的实例中，可以加载数据库以使用任何 OMPL 规划器进行约束规划，从而大大缩短规划时间。

本教程包括如何构建约束近似数据库以及如何将其用于约束轨迹规划的步骤。有关如何使用路径约束进行规划的更多信息，请查看路径约束教程。

创建约束数据库

构建约束数据库是通过generate_state_database可执行文件完成的。这会从 ROS 参数服务器加载约束定义（格式如下所述），并将状态数据库输出到给定目录。

定义约束

generate_state_database 可执行文件从 ROS 参数 /constraints 中读取约束，这些约束的格式比完整的 ROS 消息更紧凑。你可以在 rosparam 中定义这些约束，以便一起加载到一个文件中，例如 X_moveit_config/config/constraints.yaml。

path_constraint:
  name: some_constraints
  constraints:
  - type: orientation
    frame_id: world
    # etc, as described below

JointConstraint

JointConstraint限制关节可以采取的位置。有三种方法可以紧凑地指定这一点。

1. 位置 + 单一公差

2. 位置 + 公差下限和上限

3. 位置上限和下限

例如：

- type: joint
  joint_name: first_joint
  position: 0.1
  tolerance: 0.2
  weight: 1.0
- type: joint
  joint_name: second_joint
  position: 0.1
  tolerances: [0.1, 0.2]
  weight: 1.0
- type: joint
  joint_name: third_joint
  bounds: [-0.5, 1.0]
  weight: 1.0

位置约束

PositionConstraint 约束了一个（相对于某个）链接所允许的笛卡尔位置。target_offset 是相对于链接的那个位置，例如末端执行器的尖端相对于其安装点或其他原点定义的位置。这个区域（在这种紧凑定义中仅为盒子）通过指定沿每个轴的上下界来紧凑地定义。

例如：

- type: position
  frame_id: base_link
  link_name: gripper_link
  target_offset: [0.01, 0.01, 0.01]
  region:
    x: [0, 1.0] # [start, end]
    y: [0, 1.0] # [start, end]
    z: [0, 1.0] # [start, end]
  weight: 1.0

方向约束

例如，OrientationConstraint可以用来保持某物直立（或者在一定的公差范围内基本直立）。

它以滚动、俯仰、偏航列表和每个轴的公差列表紧凑地表示，例如：

- type: orientation
  frame_id: base_link
  link_name: gripper_link
  orientation: [-3.1415269, -1.57079632, 0] #RPY
  tolerances: [6.28318531, 0.2, 6.28318531]
  weight: 1.0

可见性约束

例如VisibilityConstraint，允许指定摄像机应始终能够看到夹持器。

VisibilityConstraint类文档最好地解释了如何实现这一点。

此类约束的紧凑定义如下：

- type: visibility
  target_radius: 0.5
  target_pose:
    frame_id: 'base_link'
    position: [1.2, 3.4, 5.6]
    orientation: [-3.1415269, -1.57079632, 0] #RPY
  cone_sides: 4
  sensor_pose:
    frame_id: 'gripper_cam_link'
    position: [1.2, 3.4, 5.6]
    orientation: [-3.1415269, -1.57079632, 0] #RPY
  max_view_angle: 1.1
  max_range_angle: 0.55
  weight: 1.0

运行数据库生成器

假设 MoveIt 已经启动（例如通过 roslaunch X_moveit_config demo.launch），你可以使用类似 generate_state_database.launch 的启动文件。

约束定义文件可以作为参数传递给启动文件：

roslaunch ompl_interface generate_state_database.launch constraints_file:=$(rospack find X_moveit_config)/config/constraints.yaml planning_group:=arm

内部实现

主要功能在ConstraintsLibrary类中实现。

通过调用添加约束addConstraintApproximation()，随后可以调用该函数以在近似值中包含多个约束。该函数需要四个参数：

• 约束消息（moveit_msgs::Constraints）

• 机器人描述（std::string）

• 规划场景（planning_scene::PlanningScenePtr）

• 构造选项（ompl_interface::ConstraintApproximationConstructionOptions）

机器人描述是移动组的名称，规划场景应像往常一样使用相应的机器人模型进行初始化。

下面解释约束消息和选项的初始化。

限制消息

限制消息对象可以像往常一样使用任何类型和所需的容差进行初始化。关键点是name消息的 应该是描述性的并且对于约束是唯一的。name稍后在使用近似数据库进行规划时，这将用于引用正确的约束。

构造选项

该ompl_interface::ConstraintApproximationConstructionOptions对象指定近似流形的各种特征，例如大小、密度、空间参数化类型等。以下是每个选项的概述：

• unsigned int samples - 近似图的大小

• unsigned int edges_per_sample - 近似图的程度

• double max_edge_length - 边插入的距离阈值

• bool explicit_motions - 定义边缘是否应遵循约束

• double explicit_points_resolution - 约束检查的边缘插值分辨率

• unsigned int max_explicit_points - 要检查的边缘的最大点数

图表大小

显然，稳定的规划结果需要详细的近似值，因此样本越多，性能就越可靠。然而，更高的值会导致数据库的构建时间线性延长。找到合适的流形大小是一个高度依赖于约束条件限制的问题。对于大多数约束，使用 1000 到 10000 范围内的值就足够了，而更高的值不会带来明显的改进，正如本文所建议的那样。

边

向流形添加边是可选的，可以通过将edge_per_sample设置为 0 来禁用。大多数情况下，轨迹规划在没有边的情况下也能正常工作，因为采样过程只需要状态即可运行。max_edge_length 定义允许在两个状态之间添加边的最大距离。通过将explicit_motions设置为true，边也会被强制匹配约束，使它们表示相邻状态之间的有效路径。这在非常稀疏的近似值中尤其有利，因为绝对距离不是可达性的必要度量，因为许多区域非常难以到达。通过测试状态对之间的线性插值点来检查边是否与约束匹配。这些插值点的数量设置为explicit_points_resolution乘以边长度，并受max_explicit_points限制。

添加边缘会极大地增加数据库的构建时间，而增加显式运动检查甚至会对此产生额外的影响。在对边缘进行实验时，请记住应调整Edges_per_sample和Max_edge_length值，以便始终有足够多的状态足够接近以进行连接。这需要分析近似空间的密度大小并适应状态之间的实际距离。

数据库构建

在将约束添加到 ConstraintsLibrary 对象后，可以通过调用 saveApproximationConstraints() 来构建数据库，该函数只需要指定数据库保存的相对目录。

数据库加载和使用

必须通过设置 ros 参数在启动移动组节点时加载约束数据库： 

<param name="move_group/constraint_approximations_path" value="<path_to_database>"/>

要验证数据库是否已成功加载，请检查日志中是否显示命名的约束。

在规划时，像往常一样初始化约束消息，并将消息名称设置为与构建数据库时使用的名称完全一致。此外，你还需要再次指定相同的值和公差，因为默认情况下，规划器只是对状态进行采样，但在路径规划的插值过程中不一定遵循约束。一个正确命名但没有初始化约束的约束消息会使用数据库，但仍然可能导致无效的轨迹。

这句话的意思是，在路径规划时，除了使用正确命名的约束消息外，你还需要重新指定与构建数据库时相同的值和公差。这是因为规划器在生成路径时会对状态进行采样，但在插值路径时可能不会自动应用这些约束，除非你明确指定这些值和公差。

具体含义和举例说明

假设你在构建约束数据库时定义了一个位置约束，要求末端执行器的某个位置在 xxx 轴上的范围为 0 到 1 米，公差为 0.1 米。这些约束在数据库中被保存下来。

当你进行路径规划时，你需要做的是：

1. 初始化约束消息：创建一个新的约束消息对象，并确保它的名称与数据库中的约束名称匹配。例如，消息名称可能是 end_effector_position_constraint。

2. 指定相同的值和公差：在新的约束消息中再次明确设置这些值和公差。例如，设置 xxx 轴的范围为 0 到 1 米，公差为 0.1 米。这是为了确保规划器在路径规划过程中能够应用这些约束。

举例说明

假设你有一个名为 end_effector_position_constraint 的约束，数据库中保存了末端执行器在 xxx 轴上的位置范围是 [0, 1] 米，公差是 0.1 米。

1. 构建数据库时：

   • 约束定义：end_effector_position_constraint
   • 范围：xxx 轴 [0, 1] 米
   • 公差：0.1 米

2. 路径规划时：

   • 初始化约束消息为 end_effector_position_constraint
   • 重新指定约束值：xxx 轴范围 [0, 1] 米
   • 重新指定公差：0.1 米

如果你在路径规划时没有重新指定这些值和公差，即使消息名称正确，规划器可能不会在路径生成过程中遵循这些约束，导致生成的路径可能不符合约束条件，从而可能导致无效的轨迹。

 

如果在路径规划时只指定约束，而不预先将这些约束保存在数据库中，会有以下几个可能的影响和问题：

1. 约束不被持久化

• 数据库作用：约束数据库的作用是持久化存储约束信息，以便在多个路径规划任务中重用。没有数据库的约束信息在路径规划结束后不会被保存，下一次规划时需要重新定义这些约束。

2. 路径规划的性能和效率

• 实时计算：如果不使用预先存储的数据库，路径规划器必须在每次规划时重新计算所有约束。这可能会增加计算开销，导致规划过程变得更慢，特别是在约束复杂或有大量状态时。
• 约束应用：有些规划器在插值或采样过程中可能不会有效地应用动态指定的约束。如果约束在每次规划时都需要计算，可能会出现不一致的应用情况。

3. 约束一致性

• 约束配置：如果在每次路径规划时都重新指定约束，可能会引入人为错误或不一致。如果约束定义发生变化，需要确保所有规划任务中都使用相同的约束配置，否则可能会得到不同的结果。
• 数据库的作用：预先定义并保存约束到数据库中可以确保一致性和准确性，避免在每次路径规划时重复定义相同的约束。

4. 潜在的无效路径

• 约束不应用：路径规划器可能会在路径生成过程中忽略动态指定的约束，尤其是在没有使用数据库的情况下。这样，即使你在路径规划时指定了约束，如果这些约束没有正确应用，也可能生成无效的轨迹。

总结

在路径规划时仅指定约束而不使用预先保存的数据库可能导致以下问题：

• 增加计算负担和规划时间。
• 可能出现约束不一致或应用不准确的情况。
• 增加了人为错误的风险，特别是在频繁变更约束时。

为了提高路径规划的效率和准确性，建议在实际使用中预先定义和保存约束数据库，并在路径规划时正确应用这些约束。