使用近似约束流形规划

OMPL 支持自定义约束，从而可以规划遵循期望行为的轨迹。约束可以在关节空间和笛卡尔空间中定义，其中后者可以基于方向（orientation）或位置（position）。在规划轨迹时，每个关节状态都需要满足所有已设置的约束，默认情况下这是通过拒绝采样（rejection sampling）完成的。然而，这可能会导致非常长的规划时间，尤其是当约束非常严格且拒绝率相应较高时。

Sucan et al 提出了一种方法：预先计算约束流形（constraint manifold）的近似，并在该近似流形中执行轨迹规划。OMPL 插件包含针对给定约束集执行此操作并将结果保存到数据库中的功能。之后可以加载该数据库，与任何 OMPL 规划器一起用于约束规划，这可以大幅缩短规划时间。

本教程包含构建约束近似数据库以及将其用于约束轨迹规划的步骤。有关使用路径约束进行规划的一般性信息，请参阅路径约束教程。

创建约束数据库

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

定义约束

generate_state_database 可执行文件从 /constraints 上的 ROS 参数中读取约束，其格式比完整的 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

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

OrientationConstraint 可用于保持某物直立（或在某个容差范围内基本直立）。

它通过一个 roll、pitch、yaw 列表以及每个轴的容差列表来紧凑地表示，例如：

 - 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 可用于指定例如相机应始终能够看到夹爪（gripper）。

其实现方式最好通过 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）

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

下面说明约束消息和选项的初始化。

约束消息

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

构建选项

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

• unsigned int samples - 近似图的大小
• unsigned int edges_per_sample - 近似图的度数（degree）
• double max_edge_length - 边插入的距离阈值
• bool explicit_motions - 定义边是否应遵循约束
• double explicit_points_resolution - 用于约束检查的边的插值分辨率
• unsigned int max_explicit_points - 要检查的边的最大点数

图大小

显然，稳定的规划结果需要细致的近似，因此 samples 越高，性能越可靠。然而，更高的值会导致数据库的构建时间线性增加。为流形找到合适的大小高度依赖约束的限制程度。对于大多数约束，使用 1000 到 10000 范围内的值应该就够了，如论文中所建议的，更高的值不会带来明显改进。

边

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

添加边会极大地增加数据库的构建时间，而增加显式运动（explicit motion）检查还会对此产生额外的影响。在试验边时，请记住应调整 edges_per_sample 和 max_edge_length 的值，以便始终有足够多的状态彼此足够接近而能够被连接。这需要从密度角度分析近似空间的大小，并适应状态之间的实际距离。

数据库构建

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

数据库加载与使用

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

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

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

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