OMPL官方教程学习Constrained Planning Tutorial

Constrained Planning Tutorial

带约束的规划，这个应用场景就很多了

从下面这段话中可以看出，使用带约束的规划：

1. 首先要定义一个约束constraint
2. 其次要在受约束的空间进行规划constrained state space
3. 约束方程满足$f(q):Q\to R^n$，$f(q)=0$.
4. 本次教程使用的约束是：$f(q)=\Vert q\Vert -1$, 即：$(x^2+y^2+z^2)=1$圆面约束

Defining a constrained motion planning problem is easy and very similar to defining an unconstrained planning problem. The primary difference is the need to define a constraint, and the use of a constrained state space, which wraps around an ambient state space. In this example, we will walk through defining a simple constrained planning problem: a point in R3 that is constrained to be on the surface of a sphere, giving a constraint function f(q)=||q||−1.

1.Defining the Constraint

下面代码是官方给出的圆面约束代码，从中可以看出：

1. 自定义的圆面约束方程Sphere需要继承自 Constraint
2. 构造函数需要传递两个参数，输入状态的维数(此处是3)，输出状态的维数(此处是