Optimal Planning Tutorial

Optimal Planning应该是最优规划吧，规划算法在某种程度上是最优，比如RRT*等等。

官方教程中说使用要进行Optimal Planning 与一般的规划方法使用流程基本一样，只不过多了一下两点：

需要为ompl::base::ProblemDefinition 对象设置目标函数optimization objective
需要使用最有规划器optimizing planner，而不是一般的规划器。

尴尬我都不知道一般的规划器使用流程。。。。。官方在第一篇教程中有简单介绍，这里写一下：

一般的规划的步骤：
1. 使用SimpleSetup 对象则：
  1. 应该使用setPlanner() 完成
2. 不使用SimpleSetup对象则：
  1. 创建SpaceInformation
  2. 创建ProblemDefinition 根据SpaceInformation
  3. 为ProblemDefinition 设置起点终点
  4. 创建planner，根据SpaceInformation 以及所使用的规划方法
  5. 为planner 设置ProblemDefinition
  6. planner->setup()，启动？
  7. planner->slove()，求解.
最优规划的步骤：
1. 教程中只给了不使用SimpleSetup 的情况，下面列出不同之处：
  1. 创建SpaceInformation
  2. 创建ProblemDefinition 根据SpaceInformation
  3. 为ProblemDefinition 设置起点终点
  4. 创建OptimizationObjective
  5. 为ProblemDefinition 设置OptimizationObjective
  6. 创建planner，根据SpaceInformation 以及所使用的最优规划方法
  7. 为planner 设置ProblemDefinition
  8. planner->setup()，启动？
  9. planner->slove()，求解.

1.Finding the shortest path

官方提供的场景：从(0,0)到(1,1)的一个二维区域，障碍物在(0.5,0.5)半径为0.25

We’ll demonstrate OMPL’s optimal planning framework with an example. In this example, our robot is represented as a (x,y) coordinate on a square, where (0,0) is the square’s bottom-left corner and (1,1) is the square’s top-right corner. There is also an obstacle in this square that the robot cannot pass through; this obstacle is a circle of radius 0.25 centered at (0.5,0.5). To reflect this environment, we use a two-dimensional ompl::base::RealVectorStateSpace and define our state validity checker as follows:

官方自定义了一个state validity checker，我们StateValidityChecker与 StateSpace 对象是必须实现的两个对象。实现StateValidityChecker 主要就是为了实现两个虚函数，不过这两个虚函数在基类也有实现，这里重新实现了：

isValid：判断状态是否有效
clearance：当前状态距离最近的无效状态之间的距离

Report the distance to the nearest invalid state when starting from state. If the distance is negative, the value of clearance is the penetration depth.

// Our collision checker. For this demo, our robot's state space
// lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
// centered at (0.5,0.5). Any states lying in this circular region are
// considered "in collision".
class ValidityChecker : public ob::StateValidityChecker
{
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {}
 
    // Returns whether the given state's position overlaps the
    // circular obstacle
    bool isValid(const ob::State* state) const
    {
        return this->clearance(state) > 0.0;
    }
 
    // Returns the distance from the given state's position to the
    // boundary of the circular obstacle.
    // 这个函数就是计算状态点离圆形障碍物的圆心的距离
    double clearance(const ob::State* state) const
    {
        // We know we're working with a RealVectorStateSpace in this
        // example, so we downcast state into the specific type.
        const ob::RealVectorStateSpace::StateType* state2D =
            state->as<ob::RealVectorStateSpace::StateType>();
 
        // Extract the robot's (x,y) position from its state
        double x = state2D->values[0];
        double y = state2D->values[1];
 
        // Distance formula between two points, offset by the circle's
        // radius
        return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
    }
};

随后就是进行规划解算的准备：StateSpace，设置边界，SpaceInformation，ValidityChecker，start&goal，ProblemDefinition。注意这里没有使用SimpleSetup类对象进行。

// Construct the robot state space in which we're planning. We're
// planning in [0,1]x[0,1], a subset of R^2.
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
 
// Set the bounds of space to be in [0,1].
space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
 
// Construct a space information instance for this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
 
// Set the object used to check which states in the space are valid
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
 
si->setup();
 
// Set our robot's starting state to be the bottom-left corner of
// the environment, or (0,0).
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
 
// Set our robot's goal state to be the top-right corner of the
// environment, or (1,1).
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
 
// Create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
 
// Set the start and goal states
pdef->setStartAndGoalStates(start, goal);

接下来就是使用最优规划需要使用的过程：
为最优规划设置一个目标函数：ompl::base::OptimizationObjective对象的创建。
从下面代码中可以看出：

OptimizationObjective 应该是优化函数的基类表示
使用过程中用OptimizationObjective对不同的优化函数进行统一表示
使用函数进行封装，我觉得应该可以不封装
创建OptimizationObjective 输入是SpaceInformation。

// Returns a structure representing the optimization objective to use
// for optimal motion planning. This method returns an objective which
// attempts to minimize the length in configuration space of computed
// paths.
ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
{
  
    return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si));
}

然后就是为ProblemDefinition 设置目标函数，从官方提供的API关系图中可以会好的理解。
这里用一个OptimizationObjective对象应该也可以。

pdef->setOptimizationObjective(getPathLengthObjective(si));

最后就是设置最优规划器，这里使用的是RRT*算法
从代码可以看出：

创建的对象依然是Planner，只不过规划算法是一种最优算法
为Planner 设置ProblemDefinition

// Construct our optimizing planner using the RRTstar algorithm.
ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));
 
// Set the problem instance for our planner to solve
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
 
// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

至此就完成了最优规划，流程总结在前面吧。

2.Specifying an optimality threshold

从下面这句话可以看出：

最优函数的满足存在一个阈值
阈值设置为0.0的时候表示：希望找到能够找到的最好路线。
如果不设置阈值，则默认为0.0
我们可以通过OptimizationObjective 的setCostThreshold 来设置阈值

What does satisfying an optimization objective mean? This behaviour can be customized, but by default, it means finding a path that passes some quality threshold. For shortest path planning, it means finding a path that is shorter than some given length. You’ll notice we never specified this threshold; the default behaviour for ompl::base::PathLengthOptimizationObjective is to set the threshold to 0.0 if a threshold wasn’t specified. This means that the objective is only satisfied by paths of length less than 0.0, which means this objective will never be satisfied! Our reasoning for doing this is that, if you don’t specify a threshold, we assume that you want the planner to return the best possible path it can find in the time limit. Therefore, setting a threshold of 0.0 means that the objective will never be satisfied, and guarantee that the planner runs all the way until the specified time limit and returns the best path it could find.

下面的代码只创建OptimizationObjective过程中进行了设置，这也说明为什么要把一个对象的创建过程写成函数了，因为这要做以后，只要涉及到该对象的设置，都可以在写到函数中，封装完整。

ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
{
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    obj->setCostThreshold(ob::Cost(1.51));
    return obj;
}

从下面这段话中可以看出：

阈值的的设置将极大的影响规划器求解的速度。
OptimizationObjective::setCostThreshold 实际上在调用Cost对象

If you set this as the objective for the ompl::base::ProblemDefinition you’ll find that the planner will terminate far more quickly, since it stops planning as soon as it has found a path shorter than the given threshold. Note that when calling ompl::base::OptimizationObjective::setCostThreshold, we wrap our threshold value in an ompl::base::Cost object. We’ll talk more about the ompl::base::Cost object later, but for now you can think of them as a wrapper for double values that represent the cost of a path.