在这里分享了运动规划方面的一些基本的算法原理和伪代码实现，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。

A、OMPL编译与安装

1. 方式一：通过官网下载install-ompl-ubuntu.sh文件，运行后，自动编译安装 https://ompl.kavrakilab.org/installation.html，此方法在使用库文件时，cmakeconfig文件可能有问题，还是建议方法二。
2. 方式二：通过Github下载源码，自己编译安装， https://github.com/ompl/ompl，
3. 检查安装成功 及 ROS中使用 https://blog.csdn.net/zghforever/article/details/106688410
4. 官方教程 https://ompl.kavrakilab.org/tutorials.html

B、OMPL使用

B1、基本定义

1. 求解方式
   求解问题的过程中，根据是否使用ompl::geometric::SimpleSetup类，有两种方式，简单规划和完整规划，见代码部分C1。
2. 求解过程：利用OMPL提供的类和方法来规划，基本过程包括以下几个部分
   ♥ 确认实际问题的状态空间 （如SE(3)）
   ♥ 从OMPL提供的类中构建一个状态空间（如ompl::base::SE3StateSpace is appropriate）
   ♥ 设置状态空间的边界
   ♥ 定义状态有效性的判断方法
   ♥ 定义起点和终点的状态
   ♥ 求解和可视化

3. 状态与状态空间

   //状态与状态空间的基本定义
   ompl::base::StateSpacePtr space(new T());
   ompl::base::ScopedState<> state1(space);    //直接通过状态空间StateSpace初始化
   ompl::base::SpaceInformationPtr si(space);
   ompl::base::ScopedState<T> state2(si);      //通过SpaceInformation初始化
   //举个栗子
   ompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace());
   ompl::base::ScopedState<ompl::base::SE2StateSpace> state(space);
       state->setX(0.1);
       state->setY(0.2);
       state->setYaw(0.0);
   ompl::base::ScopedState<> backup = state;   //state == backup, backup本质是State*, 作用是备份参数， 因此setX()等函数无效
   //状态空间的合并
   //合并方式1
   ompl::base::CompoundStateSpace *cs = new ompl::base::CompoundStateSpace();  //利用CompoundStateSpace类来添加不同状态空间
   cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.0);
   cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO3StateSpace()), 1.0);
   ompl::base::StateSpacePtr space(cs);
   //合并方式2
   // define the individual state spaces
   ompl::base::StateSpacePtr so2(new ompl::base::SO2StateSpace());
   ompl::base::StateSpacePtr so3(new ompl::base::SO3StateSpace());
   ompl::base::StateSpacePtr space = so2 + so3;
   
   //虽然空间可以混合，但对于状态仍然需要指定具体的类型
   ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
   state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

4. 有效性检查
   OMPL不提供具体检查方式，需要根据具体的问题具体确定。用户指定有效性检查的方式有两种，见代码部分C2
   ♥ 继承OMPL定义的两个抽象类
   ♥ 直接定义有效性检查函数

   // 方法2：直接定义判断函数
   bool myStateValidityCheckerFunction(const base::State *state)
   {
    return ...;
   }
   //方法2：使用
   base::SpaceInformationPtr si(space);
   si->setStateValidityChecker(myStateValidityCheckerFunction);
   si->setStateValidityCheckingResolution(0.03); // 3%
   si->setup();

5. 采样
   采样的两种方式：基于ompl::base::StateSampler和基于ompl::base::ValidStateSampler。
   ♥ 基于ompl::base::StateSampler：生成状态空间的一个状态，生成状态附近的一个状态，生成高斯分布的一组状态。
   ♥ 基于ompl::base::ValidStateSampler：以ompl::base::StateSampler为基础，不断采样，直到采到一个有效状态或者到达迭代最大值，有效性通过ompl::base::SpaceInformation::isValid判断。
   ♥ OMPL提供了几个继承自ompl::base::ValidStateSampler的类，如下，可以使用这些类或继承ompl::base::ValidStateSampler重新编写采样器，基本使用方法如下，具体代码与解释见代码部分C3。

//基本使用方式
//***************************定义状态分配器（函数）***************************
ompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)
{
    // we can perform any additional setup / configuration of a sampler here,
    // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
    return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);

B2、路径可视化

1. OMPL不提供路径可视化工具，直接一点的方式是作为矩阵打印输出，保存成文本文件。
2. OMPL提供两种路径：ompl::geometric::PathGeometric 和 ompl::control::PathControl，两个类可用成员函数printAsMatrix()打印，前者每行打印一个状态，后者每行除了状态，还有控制指令和控制间隔时间。

3. 打印方式如下：

   bool solved = ss.solve(20.0);
   if (solved)
   {
   // if ss is a ompl::geometric::SimpleSetup object
   ss.getSolutionPath().printAsMatrix(std::cout);
   
   // if ss is a ompl::control::SimpleSetup object
   ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
   }

B3、API Overview

C、代码附录

C1、Geometric Planning for a Rigid Body in 3D

//注：这是在ROS里面写的，虽然这一部分跟ROS暂时没关系。
#include <ros/ros.h>
#include <iostream>

#include <ompl/base/spaces/SE3StateSpace.h>

//使用ompl::geometric::SimpleSetup类
#include <ompl/geometric/SimpleSetup.h>
//不使用ompl::geometric::SimpleSetup类
#include <ompl/base/SpaceInformation.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

namespace ob = ompl::base;
namespace og = ompl::geometric;

//状态检查函数
bool isStateValid(const ob::State *state)
{
    // cast the abstract state type to the type we expect
    // 将抽象状态类型转换为我们期望的类型
    const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

    // extract the first component of the state and cast it to what we expect 
    // 提取状态的第一个组件并将其转换为我们所期望的
    const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

    // extract the second component of the state and cast it to what we expect
    // 提取状态的第二个组件并将其转换为我们所期望的
    const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

    // check validity of state defined by pos & rot

    // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
    // 返回一个始终为true但使用我们定义的两个变量的值
    return (const void*)rot != (const void*)pos;
}

//简单规划：使用ompl::geometric::SimpleSetup类
void planWithSimpleSetup()
{
    // 状态空间：构建
    auto space(std::make_shared<ob::SE3StateSpace>());

    // 状态空间：边界
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    space->setBounds(bounds);

    // 配置类：构建
    og::SimpleSetup ss(space);

    // 配置类：设置状态有效性检查器
    ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

    // 配置类：设置起终点 
    ob::ScopedState<> start(space);
    start.random();
    ob::ScopedState<> goal(space);
    goal.random();
    ss.setStartAndGoalStates(start, goal);

    // this call is optional, but we put it in to get more output information
    ss.setup();
    ss.print();

    // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = ss.solve(1.0);

    if (solved)
    {
        std::cout << "Found solution:" << std::endl;
        // print the path to screen
        ss.simplifySolution();
        ss.getSolutionPath().print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

//完整规划：不使用ompl::geometric::SimpleSetup类
void plan()
{
    // 状态空间：构建
    auto space(std::make_shared<ob::SE3StateSpace>());

    // 状态空间：边界
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    space->setBounds(bounds);

    // 空间信息：构建
    auto si(std::make_shared<ob::SpaceInformation>(space));

    // 空间信息：设置状态有效性检查器
    si->setStateValidityChecker(isStateValid);

    // 问题实例：构建
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));

    // 问题实例：设置起终点
    ob::ScopedState<> start(space);
    start.random();
    ob::ScopedState<> goal(space);
    goal.random();
    pdef->setStartAndGoalStates(start, goal);

    // 规划方法：构建
    auto planner(std::make_shared<og::RRTConnect>(si));

    // 规划方法：实际问题
    planner->setProblemDefinition(pdef);

    // 规划方法：初始化
    planner->setup();

    // 打印空间信息和问题实例
    si->printSettings(std::cout);
    pdef->print(std::cout);

    // 求解
    ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);

    if (solved)
    {
        // get the goal representation from the problem definition (not the same as the goal state)
        // and inquire about the found path
        ob::PathPtr path = pdef->getSolutionPath();
        std::cout << "Found solution:" << std::endl;

        // print the path to screen
        path->print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "rigid_body_planning");
    ros::NodeHandle nh;

    std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
    plan();
    std::cout << std::endl << std::endl;
    planWithSimpleSetup();  

    return 0;
}

C2、有效性检查方式

//方法1：继承OMPL定义的抽象类 base::StateValidityChecker
class myStateValidityCheckerClass : public base::StateValidityChecker
{
public:
     myStateValidityCheckerClass(const base::SpaceInformationPtr &si) :
       base::StateValidityChecker(si)
        {
     }

     virtual bool isValid(const base::State *state) const
     {
             return ...;
     }
};
//方法1：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(std::make_shared<myStateValidityCheckerClass>(si));
si->setup();

//方法1：继承OMPL定义的抽象类 base::MotionValidator
class myMotionValidator : public base::MotionValidator
{
public:
    // implement checkMotion()
};
//方法1：使用
base::SpaceInformationPtr si(space);
si->setMotionValidator(std::make_shared<myMotionValidator>(si));
si->setup();

// 方法2：直接定义判断函数
bool myStateValidityCheckerFunction(const base::State *state)
{
     return ...;
}
//方法2：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(myStateValidityCheckerFunction);
si->setStateValidityCheckingResolution(0.03); // 3%
si->setup();

C3、采样方式

1 已有采样器

不能直接在SimpleSetup或者SpaceInformation类中配置采样方式，需要通过定义下面类型的函数，输入ompl::base::SpaceInformation，返回ompl::base::ValidStateSamplerPtr。

//***************************定义状态分配器（函数）***************************
ompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)
{
    // we can perform any additional setup / configuration of a sampler here,
    // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
    return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);

2 自定义采样器

继承ompl::base::ValidStateSampler重新编写采样器，继承后的类与OMPL提供的类的使用方法相同。

//***************************定义子类***************************
namespace ob = ompl::base;
namespace og = ompl::geometric;

// This is a problem-specific sampler that automatically generates valid
// states; it doesn't need to call SpaceInformation::isValid. This is an
// example of constrained sampling. If you can explicitly describe the set valid
// states and can draw samples from it, then this is typically much more
// efficient than generating random samples from the entire state space and
// checking for validity.
class MyValidStateSampler : public ob::ValidStateSampler
{
public:
    MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
    {
        name_ = "my sampler";
    }
    // Generate a sample in the valid part of the R^3 state space
    // Valid states satisfy the following constraints:
    // -1<= x,y,z <=1
    // if .25 <= z <= .5, then |x|>.8 and |y|>.8
    bool sample(ob::State *state) override
    {
        double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
        double z = rng_.uniformReal(-1,1);

        if (z>.25 && z<.5)
        {
            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
            switch(rng_.uniformInt(0,3))
            {
                case 0: val[0]=x-1;  val[1]=y-1;  break;
                case 1: val[0]=x-.8; val[1]=y+.8; break;
                case 2: val[0]=y-1;  val[1]=x-1;  break;
                case 3: val[0]=y+.8; val[1]=x-.8; break;
            }
        }
        else
        {
            val[0] = rng_.uniformReal(-1,1);
            val[1] = rng_.uniformReal(-1,1);
        }
        val[2] = z;
        assert(si_->isValid(state));
        return true;
    }
    // We don't need this in the example below.
    bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override
    {
        throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
        return false;
    }
protected:
    ompl::RNG rng_;
};
//***************************定义状态分配器（函数）***************************
ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
{
    return std::make_shared<MyValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);