OMPL-constrained planning
有时，一个机器人的运动被它的任务或固有运动学特性所约束。例如，机器人可能想要保持一杯水位、在表面上写字或者遵循与任务相对应的空间中的某些曲线。在上述每种情况下，对机器人运动的约束均由某些 函数 定义，
，它映射了机器人的状态空间
到实向量值
上。只要有约束就认为满足
。例如，为了保持杯子水平，杯子轴与直立的z轴的角距离可能是约束函数。另外，满足运动的约束满足
沿路径的每个点。约束运动计划解决了如何找到满足运动约束的问题，同时仍然避免障碍或实现其他目标的问题。
通常，运动规划器不会意识到约束，因此不会生成满足约束的路径。这是因为约束满足配置的 子流形
与机器人的状态空间
相比，它的尺寸较小，因此几乎无法从中进行采样。为了计划满足运动的约束条件，OMPL提供了一种借助约束条件知识来扩展机器人状态空间的方法，
作为状态空间。使用此扩展状态空间生成的任何运动计划都将满足约束条件，因为运动规划器（例如[ompl::base::StateSpace::interpolate](https://ompl.kavrakilab.org/classompl_1_1base_1_1StateSpace.html#ae64df33ebaaddb1cd64c555be90d099d)）使用的原始操作会自动生成满足条件的约束条件。约束规划框架使任何基于采样的规划器（包括渐近最优规划器）都能在遵守约束功能的情况下进行规划。
您可以使用来表示约束函数[ompl::base::Constraint](https://ompl.kavrakilab.org/classompl_1_1base_1_1Constraint.html)，您必须在其中实现该函数
，以及约束函数的解析雅可比行列式。如果未提供解析雅可比行列式，则使用数值有限的中心差例程来近似雅可比行列式。然而，这是非常耗费计算量的，并且优选提供解析导数。我们提供了一个简单的脚本[ConstraintGeneration.py](https://ompl.kavrakilab.org/ConstraintGeneration_8py_source.html)，该脚本使用SymPy Python库进行约束的符号区分，并可以自动生成可在您的程序中使用的约束代码。还有[ompl::base::ConstraintIntersection](https://ompl.kavrakilab.org/classompl_1_1base_1_1ConstraintIntersection.html)，它可以将必须全部满足的多个约束组合在一起。
当前提供了三个约束状态空间的表示，每个表示都继承自[ompl::base::ConstrainedStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1ConstrainedStateSpace.html)。这些方法中的每一种都实现了一种不同的方式来采样和遍历基础子流形。三个增强状态空间是：
[ompl::base::ProjectedStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1ProjectedStateSpace.html)，一个受约束的状态空间，它使用投影运算符来找到满足运动的约束。[ompl::base::AtlasStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1AtlasStateSpace.html)，一个受约束的状态空间，在规划时会建立的分段线性近似 X使用切线空间。这种近似称为 地图集 ，用于指导规划。[ompl::base::TangentBundleStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1TangentBundleStateSpace.html)，类似于的受约束状态空间[ompl::base::AtlasStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1AtlasStateSpace.html)，但是懒惰地评估atlas。
每个状态空间都有其自己的文档，可以在其页面上查看。OMPL采取的增强状态空间方法已在2019年《国际机器人研究杂志》上发表。在这篇评论文章中提供了有关受限运动计划的更多信息。
局限性
为了使受约束的计划框架起作用，您的基础状态空间和约束功能必须满足一些假设。
连续记忆
作为实现细节，由基础状态空间在状态中分配的内存必须是double值的连续数组。例如，[ompl::base::RealVectorStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1RealVectorStateSpace.html)或运动链基准中实现的状态空间都分配连续的double值数组。各种约束状态空间和约束函数都需要使用此详细信息，以便使用来将它们查看为Eigen::VectorXds Eigen::Map<Eigen::VectorXd>。请注意[ompl::base::ConstrainedStateSpace::StateType](https://ompl.kavrakilab.org/classompl_1_1base_1_1ConstrainedStateSpace_1_1StateType.html)，为方便起见，该类派生自此类。
但是，[ompl::base::CompoundStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1CompoundStateSpace.html)由于状态分配不能保证连续性，因此这种假设会阻止约束规划框架使用它。
约束微分
通常，约束函数应该是机器人状态的 连续 且可 微的 函数。约束函数中的奇异点可能会通过基本的约束满足方法导致不良行为。[ompl::base::AtlasStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1AtlasStateSpace.html)而[ompl::base::TangentBundleStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1TangentBundleStateSpace.html)这两种将把奇点在规划过程中的障碍。
要求插值
如果您想要一个满足约束 且 可能由实际系统执行的路径，则将希望插入找到的任何路径（简化或未简化）。只需调用即可[ompl::geometric::PathGeometric::interpolate()](https://ompl.kavrakilab.org/classompl_1_1geometric_1_1PathGeometric.html#a824b91d04ba886c82c1dda3e356fd1f0)。插补路径将满足约束条件，因为插补例程将使用受约束状态空间提供的原语。但是，此步骤可能会出现问题，因为插值可能会失败。
插值失败
当前，每个受约束状态空间都通过计算两个查询点之间的 离散测地线 在约束子流形上实现插值。离散测地线是两个查询点之间满足状态的接近（近似连续性）约束条件的序列。离散测地线中每个点之间的距离由中的“ delta”参数调整[ompl::base::ConstrainedStateSpace::setDelta()](https://ompl.kavrakilab.org/classompl_1_1base_1_1ConstrainedStateSpace.html#ac4b381611fa84be9e6d0ff2870b11498)。如何计算离散离散测地线是受约束状态空间如何工作的关键，因为在整个代码中都普遍使用了受约束状态空间（例如，插值，碰撞检查，运动验证等）。
由于这些例程的实现方式的性质，离散测地线的计算可能 会失败 ，从而可能由于任何要求离散测地线的上层例程导致潜在的意外结果。这些失败可能是由于约束条件奇异，子流形的高曲率以及各种其他问题造成的。但是，“常规”状态空间中的插值通常不会因为分析而失败，例如[ompl::base::RealVectorStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1RealVectorStateSpace.html);中的线性插值。因此，ompl::base::StateStace::interpolate()假定总是成功的。结果，如果在约束状态空间的规划过程中插值失败，则会看到一些意外的行为。增加或减少“ delta”参数[ompl::base::ConstrainedStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1ConstrainedStateSpace.html)，增加或减少约束满足公差以及其他超参数调整可以解决这些问题。
超参数灵敏度
通常，受约束的状态空间对它们各种超参数的调整很敏感。开始时会设置一些合理的默认值，但是许多受约束的计划问题将具有基础子流形的不同特征，因此可能需要不同的参数。以下提供一些基本的经验法则：
- 对于高维环境状态空间，可以增加许多参数的大小。简短的非详尽清单如下。
- 约束求解容限可以通过
[ompl::base::Constraint::setTolerance()](https://ompl.kavrakilab.org/classompl_1_1base_1_1Constraint.html#a4413eaf904a95e843b85938236a1a2ab) - 有效的步长用于歧管遍历
[ompl::base::ConstrainedStateSpace::setDelta()](https://ompl.kavrakilab.org/classompl_1_1base_1_1ConstrainedStateSpace.html#ac4b381611fa84be9e6d0ff2870b11498) - 的许多图集相关参数
[ompl::base::AtlasStateSpace](https://ompl.kavrakilab.org/classompl_1_1base_1_1AtlasStateSpace.html)，例如ε ，ρ和探索。
- 约束求解容限可以通过
- 通常，流形遍历的步长与基础约束子流形的相对 曲率 有关。弯曲度较小的子流形可以允许较大的步长（即，如果约束定义了一个超平面，则可以使用较大的步长）。如果需要考虑速度，请尝试使用此值，因为步长越大，遍历歧管的时间就越少。
- 对于基于地图集和切线束的空间，依赖于空间均匀采样的规划人员可能需要较高的探索参数，因此约束子流形会更快被覆盖（例如BIT *）。此外，依赖的计划人员
ompl::base::StateSampler::samplerNear()可能还需要进行高度探索或ρ 参数才能有效扩展。 - 还有很多！通常，与基于地图集或切线束的空间相比，基于投影的空间对较差的参数调整较不敏感，因此，这是验证约束规划问题是否可行的良好起点。
附加条款
约束投影与投影评估器
在内[ompl::base::Constraint](https://ompl.kavrakilab.org/classompl_1_1base_1_1Constraint.html)，有一个投影功能[ompl::base::Constraint::project()](https://ompl.kavrakilab.org/classompl_1_1base_1_1Constraint.html#a84203729cf979b398e8e59f92d136ed3)，可将潜在的约束不满意配置映射到约束流形。默认情况下，[ompl::base::Constraint::project()](https://ompl.kavrakilab.org/classompl_1_1base_1_1Constraint.html#a84203729cf979b398e8e59f92d136ed3)实现牛顿方法，该方法在大多数情况下效果良好。请注意，可以使用自己的投影例程（例如，反向运动学）覆盖此方法。
您可能会注意到也存在[ompl::base::ProjectionEvaluator::project()](https://ompl.kavrakilab.org/classompl_1_1base_1_1ProjectionEvaluator.html#aabaf42c4d51b8da6537261e64e9d6996)该方法，一些规划器使用此方法来估计可用空间的覆盖范围（有关更多信息，请参见本教程），因为它是到较低维空间的“投影”。尽管听起来很相似，但该概念与中使用的投影概念正交[ompl::base::Constraint](https://ompl.kavrakilab.org/classompl_1_1base_1_1Constraint.html)，后者将状态投影到低维约束流形中。实际上，可以使用在约束的计划框架中同时使用[ompl::base::ProjectionEvaluator](https://ompl.kavrakilab.org/classompl_1_1base_1_1ProjectionEvaluator.html)（例如[ompl::geometric::KPIECE1](https://ompl.kavrakilab.org/classompl_1_1geometric_1_1KPIECE1.html)，[ompl::geometric::ProjEST](https://ompl.kavrakilab.org/classompl_1_1geometric_1_1ProjEST.html)）的计划者。
默认情况下，约束状态空间将用于[ompl::base::WrapperProjectionEvaluator](https://ompl.kavrakilab.org/classompl_1_1base_1_1WrapperProjectionEvaluator.html)访问基础状态空间的默认投影评估器。但是，如果您对问题的结构一无所知，则应实施自己的投影评估程序以提高性能。每个受约束的计划演示都有一个可以与受约束的计划一起使用的投影评估器的示例（例如SphereProjection，在ConstrainedPlanningSphere中）。
Constrained Planning Tutorial
https://ompl.kavrakilab.org/constrainedPlanningTutorial.html
https://zhuanlan.zhihu.com/p/345982497