MoveIt! 学习笔记10- Planning with Approximated Constraint Manifolds

本博文转载自https://blog.csdn.net/ssw_1990/article/details/104048403；

由于目前正处在学习阶段，对教程中提到的知识不是很确定含义，待以后用到的时候再添加补充。

OMPL支持自定义约束，以支持遵循期望行为的规划轨迹。约束可以在关节空间和笛卡儿空间中定义，笛卡儿空间可以是基于方向的，也可以是基于位置的。在规划轨迹时，每个关节状态都需要遵循所有设置的约束，默认情况下通过拒绝采样来执行。然而，这可能会导致非常长的计划时间，特别是当约束非常严格且废品率相应很高时。
Sucan等人提出了一种方法，他们预先计算约束流形的近似值，并据此进行轨迹规划。OMPL插件包含对给定约束集执行此操作并将其保存到数据库中的功能。在以后的实例中，可以使用任何OMPL计划器加载数据库来进行约束规划，这大大减少了规划时间。
本教程包括如何构建约束近似数据库以及如何将其用于约束轨迹规划的步骤。有关如何使用路径约束进行总体规划的更多信息，请参见此处。

一.Creating the Constraint Database
在demo_construct_state_database.cpp中可以找到如何从约束构造近似数据库的示例实现。主要的功能是在ConstraintsLibrary类中实现的。
通过调用addConstraintApproximation()来添加约束，随后可以调用它来在逼近中包含多个约束。该函数需要四个参数：

• 约束消息(moveit_msgs:: constraints)
• 机器人描述(std::string)
• 规划场景(planning_scene::PlanningScenePtr)
• 构造选项(ompl_interface:: constraintationconstructionoptions)

机器人描述是移动组的名称，规划场景与相应的机器人模型一起初始化。约束消息和选项的初始化将在下面进行说明。
1.Constraints message
可以使用任何类型和所需的容差像往常一样初始化constraints消息对象。关键的一点是，消息的名称应该是描述性的，并且对于约束是惟一的。在使用逼近数据库进行规划时，将使用该名称来引用正确的约束。
2.Construction Options
constraintationconstructionoptions对象指定逼近流形的各种特性，例如实例大小、密度、空间参数化类型和其它。以下是每个选项的概述：

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

[1]Graph size
显然，稳定的规划结果需要详细的近似，因此，样本越高，性能越可靠。然而，数值越大，数据库的构建时间越长。找到合适的流形尺寸是一个高度依赖于约束条件的限制性的问题。对于大多数约束来说，使用范围在1000到10000之间的值就足够了，而不需要像本文中建议的那样对更高的值进行明显的改进。
[2]Edges
向流形添加边是可选的，可以通过将edges_per_sample设置为0来禁用它。轨迹规划在大多数情况下都是可行的，因为采样过程只需要状态就可以了。max_edge_length定义了两个状态之间允许添加边缘的最大距离。通过将explicit_motion设置为true，还可以强制边缘匹配约束，使它们表示相邻状态之间的有效路径。这是有利的，特别是在非常稀疏的逼近，许多区域很难达到，因为绝对距离不是一个必要的可达性的措施。通过测试状态对之间的线性内插点来检查边缘是否与约束匹配。这些内插点的数量被设置为explicit_points_resolution乘以边缘长度，并受到max_explicit_points的限制。
添加边缘极大地增加了数据库的构建时间，而增加显式动作检查甚至会对其产生额外的影响。在对边进行实验时，请记住，edges_per_sample和max_edge_length值应该进行调整，以便总是有足够多的状态足够接近以进行连接。这需要根据密度分析近似空间的大小，并适应状态之间的实际距离。
3.Database Construction
将约束添加到ConstraintsLibrary对象后，可以通过调用saveApproximationConstraints()来构造数据库，该约束只接受应该保存数据库的相对目录。

二.Database Loading and Usage
通过设置ros参数，必须在move group节点启动时加载constraints数据库：

• 1

要验证数据库是否已被找到，请检查指定的约束是否显示在日志中。
对于规划，只需像往常一样初始化约束消息，并将消息名称设置为用于构造数据库的确切名称。此外，还需要再次指定相同的值和容差，因为在默认情况下，规划器只是在状态上采样，而不必在路径规划的插值期间遵循约束。没有初始化约束的正确命名的约束消息将使用数据库，但因此可能导致无效的轨迹。

参考文献：
[1]Planning with Approximated Constraint Manifolds：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html