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$圆面约束

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

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

1.3 Constraint Parameters

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

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

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。

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() 用来进行插值，离散规划形成的路径是不连续的，为了得到“相对连续”并且还满足约束，就需要进行插值。
官方解释：

// 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() 对输出路径进行插值。