Ompl 三维刚体的几何规划

本教程展示对于刚体如何在三维空间中进行规划。

我们将以两种方式演示如何进行此操作：使用和不使用ompl::geometric::simplesetup类。

主要区别在于，在后一种情况下，需要显式实例化ompl::base::spaceinformation和ompl::base::problemdefinition。此外，还必须显式地实例化要使用的规划器。推荐的方法是使用ompl::geometric::simplesetup，因为这不太容易出现错误，并且不会以任何方式限制代码的功能。 在三维中设置刚体的几何规划需要以下步骤：

确定我们正在规划的空间：从可用的状态空间中选择相应的状态空间，或创建一个状态空间。对于SE(3)，ompl::base::se3statespace是合适的。由于SE(3)包含一个R3组件，因此我们需要定义边界。定义有效状态（自由状态空间/非障碍区域）。定义开始状态和目标状态。

一旦这些步骤完成，就在概念上完成了对问题的说明。下面显示了一组可以用于实例化此规范的类。

Using the ompl::geometric::SimpleSetup Class

Assuming the following namespace definitions:

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

And a state validity checking function defined like this:

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 return (const void*)rot != (const void*)pos; }

首先创建一个状态空间的实例 (We first create an instance of the state space we are planning in. )

void planWithSimpleSetup() { // construct the state space we are planning in auto space(std::make_shared<ob::SE3StateSpace>());

定义边界 

// set the bounds for the R^3 part of SE(3) ob::RealVectorBounds bounds(3); bounds.setLow(-1); bounds.setHigh(1); space->setBounds(bounds);

创建ompl::geometric::SimpleSetup的实例 ss，ompl::base::spaceinformation和ompl::base::problemdefinition的实例在内部创建。 

// define a simple setup class og::SimpleSetup ss(space);

设置状态有效性检查器

// set state validity checking for this space ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

 设置起始点和目标点，并初始化 SimpleSetup

// create a random start state ob::ScopedState<> start(space); start.random(); // create a random goal state ob::ScopedState<> goal(space); goal.random(); // set the start and goal states ss.setStartAndGoalStates(start, goal); // this call is optional, but we put it in to get more output information ss.setup(); ss.print();

我们现在可以尝试解决规划问题。 这也将触发对ompl :: geometric :: SimpleSetup :: setup() 的调用，并创建一个默认实例，这是因为我们没有指定实例。 此外，会调用ompl :: base :: Planner :: setup() ，依次调用ompl :: base :: SpaceInformation :: setup() 。 此调用链将导致计算运行时参数，例如状态有效性检查。

该调用从ompl :: base :: PlannerStatus返回一个值，该值描述是否在指定的时间段（以秒为单位）内找到了解决方案。 如果可以将此值能够被强制转换为 true，则找到解决方案。

// 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; } void plan() { // construct the state space we are planning in auto space(std::make_shared<ob::SE3StateSpace>()); // set the bounds for the R^3 part of SE(3) ob::RealVectorBounds bounds(3); bounds.setLow(-1); bounds.setHigh(1); space->setBounds(bounds); // construct an instance of space information from this state space auto si(std::make_shared<ob::SpaceInformation>(space)); // set state validity checking for this space si->setStateValidityChecker(isStateValid); // create a random start state ob::ScopedState<> start(space); start.random(); // create a random goal state ob::ScopedState<> goal(space); goal.random(); // create a problem instance auto pdef(std::make_shared<ob::ProblemDefinition>(si)); // set the start and goal states pdef->setStartAndGoalStates(start, goal); // create a planner for the defined space auto planner(std::make_shared<og::RRTConnect>(si)); // set the problem we are trying to solve for the planner planner->setProblemDefinition(pdef); // perform setup steps for the planner planner->setup(); // print the settings for this space si->printSettings(std::cout); // print the problem settings pdef->print(std::cout); // attempt to solve the problem within one second of planning time 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*/) { std::cout << "OMPL version: " << OMPL_VERSION << std::endl; plan(); std::cout << std::endl << std::endl; planWithSimpleSetup(); return 0; }