默认规划路径算法和RRTConnet路径规划算法生成路径

1.  源代码

#include
#include
#include
#include

#include
#include

#include
#include

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();

// 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;

}

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

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

space->setBounds(bounds);

// 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); });

// 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();

// set planner
ob::PlannerPtr planner(new og::RRTConnect(ss.getSpaceInformation()));
ss.setPlanner(planner);

// 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;

    std::ofstream ofs0("../plot/path0.dat");
    ss.getSolutionPath().printAsMatrix(ofs0);

    // print the path to screen
    ss.simplifySolution();
    ss.getSolutionPath().print(std::cout);

    std::ofstream ofs("../plot/path.dat");
    ss.getSolutionPath().printAsMatrix(ofs);
}
else
    std::cout << "No solution found" << std::endl;

}

int main(int /*argc*/, char ** /*argv*/)
{
std::cout << "OMPL version: " << OMPL_VERSION << std::endl;

planWithSimpleSetup();

return 0;

}

2. Python可视化生成的原始路径和简化路径

from mpl_toolkits.mplot3d import Axes3D
import numpy
import matplotlib.pyplot as plt
data = numpy.loadtxt('path.dat')
data1 = numpy.loadtxt('path0.dat')
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot(data[:,0],data[:,1],data[:,2],'.-')
plt.hold('on')
plt.grid('on')
ax.plot(data1[:,0],data1[:,1],data1[:,2],'r-')
plt.show()

路径可视化方法可以参考官网 http://ompl.kavrakilab.org/pathVisualization.html

OMPL 参考列表

1. http://ompl.kavrakilab.org/group__demos.html

2. http://ompl.kavrakilab.org/tutorials.html

3. http://ompl.kavrakilab.org/gui.html#gui_paths