OMPL-constrained planning

有时，一个机器人的运动被它的任务或固有运动学特性所约束。例如，机器人可能想要保持一杯水位、在表面上写字或者遵循与任务相对应的空间中的某些曲线。在上述每种情况下，对机器人运动的约束均由某些函数定义，，它映射了机器人的状态空间 到实向量值上。只要有约束就认为满足。例如，为了保持杯子水平，杯子轴与直立的z轴的角距离可能是约束函数。另外，满足运动的约束满足沿路径的每个点。约束运动计划解决了如何找到满足运动约束的问题，同时仍然避免障碍或实现其他目标的问题。

通常，运动规划器不会意识到约束，因此不会生成满足约束的路径。这是因为约束满足配置的子流形与机器人的状态空间相比，它的尺寸较小，因此几乎无法从中进行采样。为了计划满足运动的约束条件，OMPL提供了一种借助约束条件知识来扩展机器人状态空间的方法，作为状态空间。使用此扩展状态空间生成的任何运动计划都将满足约束条件，因为运动规划器（例如ompl::base::StateSpace::interpolate）使用的原始操作会自动生成满足条件的约束条件。约束规划框架使任何基于采样的规划器（包括渐近最优规划器）都能在遵守约束功能的情况下进行规划。

您可以使用来表示约束函数ompl::base::Constraint，您必须在其中实现该函数，以及约束函数的解析雅可比行列式。如果未提供解析雅可比行列式，则使用数值有限的中心差例程来近似雅可比行列式。然而，这是非常耗费计算量的，并且优选提供解析导数。我们提供了一个简单的脚本ConstraintGeneration.py&#