OMPL官方教程学习Optimal Planning Tutorial
Optimal Planning Tutorial
Optimal Planning 通常指最优规划，在这一领域中存在多种算法设计思路。其中一类算法展现出显著的优化性能，在某些特定条件下甚至接近全局最优解的表现。例如RRT*等算法在复杂环境中展现出卓越的路径规划能力
官方教程中说明要实施Optimal Planning的方式与一般的规划方法流程大体相同,只是增加了几项步骤
- 应将其优化目标字段设为优化目标。
- 建议采用优化规划器而非通用规划器。
感到困惑的是我对一般规划器的操作流程尚不清楚。建议参考官方的第一篇教程获取简要说明，并作为补充说明。
- 一般的规划的步骤：
1. 使用SimpleSetup 对象则：
应该使用setPlanner() 完成
不用SimpleSetup对象时：
建立SpaceInformation对象。
根据SpaceInformation对象创建ProblemDefinition对象。
配置ProblemDefinition对象的起始点和终点位置。
初始化planner对象，并基于SpaceInformation以及所采用的规划算法配置其参数。
将ProblemDefinition对象赋值给planner以完成路径规划。
请调用planner的对象方法进行初始化操作。
请调用planner的对象方法进行路径求解。
- 最优规划的具体流程如下：
- 教程中通常未涉及SimpleSetup的使用情况。
- 具体区别如下：
- 首先初始化并获取SpaceInformation变量。
- 基于上述初始化结果生成完整的ProblemDefinition模型。
- 在完成Model Definition后设定规划问题的关键参数——起点与终点位置信息。
- 接下来构建并配置优化目标(OptimizationObjective)模块。
- 将该优化目标模块整合到ProblemDefinition模型中进行统一配置与求解准备阶段。
- 然后根据所选最优规划算法的特点与约束条件需求初始化相应的planner对象，并完成必要的参数配置工作。
- 最后依次调用planner->setup()函数以启动规划计算流程，并随后执行planner->solve()操作以获得最终规划结果数据集.
1.Finding the shortest path
官方提供的场景：从(0,0)到(1,1)的一个二维区域，障碍物在(0.5,0.5)半径为0.25
We will showcase OMPL's efficient motion planning framework through an illustrative example. The robot will be modeled as a (x,y) point within a square, with its lower-left corner at position (0, 0) and upper-right corner at position (1, 1). Within this confined space lies an obstacle that must be navigated around; specifically positioned as a circle with radius 0.25 centered at coordinates (0.5, 0.5). To model this scenario accurately, we employ a two-dimensional ompl::base::RealVectorStateSpace and outline how our state validity checker operates.
官方专门开发了一个_state validity checker_ ，我们StateValidityChecker与StateSpace对象是必须负责实现的两个核心类。该类的主要职责是负责实现两个虚函数，在基类中已有默认实现：为此类提供了特殊的接口以确保状态的有效性检查功能得以可靠地执行。这两个虚函数在基类中已有默认实现：为此类提供了特殊的接口以确保状态的有效性检查功能得以可靠地执行。
官方专门开发了一个_state validity checker_ ，我们StateValidityChecker与StateSpace对象是必须负责实现的两个核心类。该类的主要职责是负责实现两个虚函数，在基类中已有默认实现了：为此类提供了特殊的接口以确保状态的有效性检查功能得以可靠地执行。
- isValid：用于判定给定的状态的有效性
- clearance：该参数表示在当前状态下与最近的有效状态之间的距离
Compute the value of distance to the nearest forbidden state when beginning with a given state. If this computed distance is negative, then set the clearance value as 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框架中定义了用于优化规划的目标函数，并通过创建ompl::base::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 进行问题定义设置
// 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
从下面这句话可以看出：
- 存在一个满足最优函数的门槛。
- 当将阈值设定为零点零时意味着：希望寻找到一条最佳路径。
- 如果未设定该阈值，则默认其数值为零点零。
- 我们可以通过\texttt{OptimizationObjective}对象的方法\texttt{setCostThreshold}来进行相应的参数配置。
The act of satisfying an optimization objective denotes achieving a specific goal within a defined framework. This behavior can be customized, yet by default, it implies identifying a trajectory that meets certain quality benchmarks. For shortest path planning purposes, this implies identifying a route whose length is less than a specified measure. It's important to note that we do not specify this benchmark; instead, the default behavior for ompl::base::PathLengthOptimizationObjective sets the threshold to 0.0 when unspecified. Consequently, this objective is only deemed satisfied by paths measuring less than 0.0 units in length—a condition that effectively makes this objective unsatisfiable! The rationale behind adopting this approach is rooted in our assumption that users will opt for the planner to generate the most optimal path within allocated time constraints if no threshold is provided. By setting a threshold of 0.0, we ensure that this objective will never be met unless such constraints are explicitly defined. This design choice guarantees that the planner operates until the designated time limit has elapsed and delivers the most optimal solution achievable under those conditions.
这段代码仅负责设置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 using this approach as the objective for the ompl::base::ProblemDefinition, you will quickly realize that once a shorter path is identified, planning ceases immediately. Recall that when setting up an optimization goal with ompl::base::OptimizationObjective, we encapsulate our threshold value into an instance of ompl::base::Cost. For now, consider these objects simply containers for numerical values representing costs; we'll delve deeper into their structure later.