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)，输出状态的维数(此处是1)。
3. 必须实现虚函数ompl::base::Constraint::function()，当然还有其他虚函数可以选择实现

// Constraints must inherit from the constraint base class. By default, a
// numerical approximation to the Jacobian of the constraint function is computed
// using a central finite difference.
class Sphere : public ob::Constraint
{
public:
    // ob::Constraint's constructor takes in two parameters, the dimension of
    // the ambient state space, and the dimension of the real vector space the
    // constraint maps into. For our sphere example, as we are planning in R^3, the
    // dimension of the ambient space is 3, and as our constraint outputs one real
    // value the second parameter is one (this is also the co-dimension of the
    // constraint manifold).
    Sphere() : ob::Constraint(3, 1)
    {
    }
 
    // Here we define the actual constraint function, which takes in some state "x"
    // (from the ambient space) and sets the values of "out" to the result of the
    // constraint function. Note that we are implementing `function` which has this
    // function signature, not the one that takes in ompl::base::State.
    void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
    {
        // The function that defines a sphere is f(q) = || q || - 1, as discussed
        // above. Eigen makes this easy to express:
        out[0] = x.norm() - 1;
    }
};

1.1 Constraint Jacobian

雅各布矩阵约束么？约束函数的雅各布矩阵吧，jacobian也是一个虚函数，用户可以实现

// Implement the Jacobian of the constraint function. The Jacobian for the
// constraint function is an matrix of dimension equal to the co-dimension of the
// constraint by the ambient dimension (in this case, 1 by 3). Similar to
// `function` above, we implement the `jacobian` method with the following
// function signature.
void Sphere::jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
{
    out = x.transpose().normalized();
}

1.2 Projection

投影约束么。。。也是一个虚函数，用户可以实现

One of the primary features of ompl::base::Constraint is the projection function, ompl::base::Constraint::project(). This function takes as input a potential constraint unsatisfying state and maps it onto the constraint manifold, generating a constraint satisfying state. By default, ompl::base::Constraint::project() implements a Newton’s method which performs well in most circumstances.

1.3 Constraint Parameters

约束参数的有两个：tolerance 与maxIterations：

1. tolerance可以通过ompl::base::Constraint::setTolerance() 设置
2. maxIterations可以通过ompl::base::Constraint::setMaxIterations() 设置
3. 两者都服务于ompl::base::Constraint::project() 约束方法
4. 两者的功能：

   tolerance is used in ompl::base::Constraint::project() to determine when a state satisfies the constraints
   maxIterations is used in ompl::base::Constraint::project() to limit the number of iterations the projection routine uses, in the case that a satisfying configuration cannot be found.

2.Defining the Constrained State Space

2.1 Ambient State Space

首先要创建任意的状态空间，这个跟常规状态空间定义一样。

// Create the ambient space state space for the problem.
auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
 
// Set bounds on the space.
ob::RealVectorBounds bounds(3);
bounds.setLow(-2);
bounds.setHigh(2);
 
rvss->setBounds(bounds);

2.2 Constraint Instance

然后就是创建约束：之前的圆形约束

// Create our sphere constraint.
auto constraint = std::make_shared<Sphere>();

2.3 Constrained State Space

最后创建带约束的状态空间：

从下面的代码可以看出：

1. 受约束的状态空间ProjectedStateSpace 以StateSpace与constraint为输入
2. 随后定义带约束的信息状态空间ConstrainedSpaceInformation，其以ProjectedStateSpace为输入
3. 带约束与不带约束的状态空间定义很类似，只不过换了个名字

// Combine the ambient space and the constraint into a constrained state space.
auto css = std::make_shared<ob::ProjectedStateSpace>(rvss, constraint);
 
// Define the constrained space information for this constrained state space.
auto csi = std::make_shared<ob::ConstrainedSpaceInformation>(css);

从下面的话中可以看出：

1. 带约束的状态空间类有三种ProjectedStateSpace，AtlasStateSpace，TangentBundleStateSpace。

Now that we have both the ambient state space and the constraint, we can define the constrained state space. For this example, we will be using ompl::base::ProjectedStateSpace, which implements a projection operator-based methodology for satisfying constraints. However, we could also just as easily use the other constrained state spaces, ompl::base::AtlasStateSpace or ompl::base::TangentBundleStateSpace, for this problem.

3.Defining a Problem

使用SimpleSetup来创建这个带约束的规划问题并使用PRM来求解:
SimpleSetup的创建与之前一样，只不过这里是ConstrainedSpaceInformation作为输入

// Simple Setup
auto ss = std::make_shared<og::SimpleSetup>(csi);

3.1 State Validity Checker

设置有效状态检测函数，这里的有效状态已经变成了一个圆球

bool obstacle(const ob::State *state)
{
    // As ob::ConstrainedStateSpace::StateType inherits from
    // Eigen::Map<Eigen::VectorXd>, we can grab a reference to it for some easier
    // state access.
    const Eigen::Map<Eigen::VectorXd> &x = *state->as<ob::ConstrainedStateSpace::StateType>();
 
    // Alternatively, we could access the underlying real vector state with the
    // following incantation:
    //   auto x = state->as<ob::ConstrainedStateSpace::StateType>()->getState()->as<ob::RealVectorStateSpace::StateType>();
    // Note the use of "getState()" on the constrained state. This accesss the
    // underlying state that was allocated by the ambient state space.
 
    // Define a narrow band obstacle with a small hole on one side.
    // Z方向-0.1 到 0.1 绝大多是是障碍物，这相当于是一个环带
    if (-0.1 < x[2] && x[2] < 0.1)
    {
        // 环带有缺口
        if (-0.05 < x[0] && x[0] < 0.05)
        // 本来是两个缺口，但这里约束 y < 0,所以只有一个缺口
            return x[1] < 0;
 
        return false;
    }
 
    return true;
}
 
// Set the state validity checker in simple setup.
ss->setStateValidityChecker(obstacle);

下面是示意图，右手定则，右x，平面向内y，上z：

3.2 Start and Goal

接下来设置起点和终点，这就没啥好说的了，这里与Eigen一起使用了一下

// Start and goal vectors.
Eigen::VectorXd sv(3), gv(3);
sv << 0, 0, -1;
gv << 0, 0,  1;
 
// Scoped states that we will add to simple setup.
ob::ScopedState<> start(css);
ob::ScopedState<> goal(css);
 
// Copy the values from the vectors into the start and goal states.
start->as<ob::ConstrainedStateSpace::StateType>()->copy(sv);
goal->as<ob::ConstrainedStateSpace::StateType>()->copy(gv);
 
// If we were using an Atlas or TangentBundleStateSpace, we would also have to anchor these states to charts:
//   css->anchorChart(start.get());
//   css->anchorChart(goal.get());
// Which gives a starting point for the atlas to grow.
 
ss->setStartAndGoalStates(start, goal);

2.3 Planner

设置规划器为PRM

auto pp = std::make_shared<og::PRM>(csi);
ss->setPlanner(pp);

4.Solving a Problem

终于明白这个setup是干啥的了：用来实际设置参数，为规划做准备

ss->setup();

求解过程与之前的基本一致，只不过这里添加了ompl::geometric::PathGeometric::interpolate() 用来进行插值，离散规划形成的路径是不连续的，为了得到“相对连续”并且还满足约束，就需要进行插值。
官方解释：

Note that in general the initial path that you find is not “continuous”. That is, the distance between states in the path (especially after simplification) can be very far apart! If you want a path that has close, intermediate constraint satisfying states, you need to interpolate the path. In the code above, this is achieved with ompl::geometric::PathGeometric::interpolate().

// Solve a problem like normal, for 5 seconds.
ob::PlannerStatus stat = ss->solve(5.);
if (stat)
{
    // Path simplification also works when using a constrained state space!
    ss->simplifySolution(5.);
 
    // Get solution path.
    auto path = ss->getSolutionPath();
 
    // Interpolation also works on constrained state spaces, and is generally required.
    path.interpolate();
 
    // Then do whatever you want with the path, like normal!
}
else
    OMPL_WARN("No solution found!");

最后的结果

带有约束的规划步骤总结：

1. 创建状态空间对象：StateSpace
2. 创建约束对象：Constraint，此过程要实现约束函数.
3. 创建带约束的状态空间：ConstrainedStateSpace(有三种带约束的状态空间)，需要StateSpace与Constraint作为输入
4. 创建带约束的信息状态空间：ConstrainedSpaceInformation，以ConstrainedStateSpace作为输入。
5. 创建SimpleSetup对象，以ConstrainedSpaceInformation作为输入
6. 随后就是以SimpleSetup进行求解的过程：设置起点终点、有效检测、规划器。
7. 最后需要用ompl::geometric::PathGeometric::interpolate() 对输出路径进行插值。