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

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

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

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

创建约束数据库

构建约束数据库是通过 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

关节约束

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

  1. 位置 + 单一公差

  2. 位置 + 下限公差

  3. 上限和下限

For example:

- 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

方向约束

可以使用 方向约束 来保持某物直立（或根据某些公差基本直立）。

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

  • 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 类文档。

这样的约束紧凑定义如下：

  • 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 class.

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

  • 约束消息 (moveit_msgs::Constraints)

  • 机器人描述 (std::string)

  • 规划场景 (planning_scene::PlanningScenePtr)

  • 构造选项 (ompl_interface::ConstraintApproximationConstructionOptions)

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

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

约束消息

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

构造选项

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

  • unsigned int samples - size of the approximation graph

  • unsigned int edges_per_sample - degree of the approximation graph

  • double max_edge_length - distance threshold for edge insertion

  • bool explicit_motions - defines if edges should follow constraints

  • double explicit_points_resolution - interpolation resolution of edges for constraint checks

  • unsigned int max_explicit_points - maximum points of an edge to check

图形大小

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

边

向流形添加边是**可选**的，可以通过将**edges_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>”/>

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

对于规划，只需像往常一样初始化约束消息，并将消息名称设置为用于构建数据库的确切名称。

您还需要再次指定相同的值和公差，因为默认情况下，规划器只对状态进行采样，但在路径规划插值期间不一定遵循约束。 没有初始化约束的正确命名的约束消息将使用数据库，但因此无论如何都可能导致无效轨迹。