OMPL学习笔记1

红色是ompl库中RRT搜索路径，蓝色是简化后的路径。

RigidBodyPlanning.cpp

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>
#include <iostream>
#include <fstream>
#include <ostream>
#include "boost/bind.hpp"

using namespace std;

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 ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();

  // extract the first component of the state and cast it to what we expect
  const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

  // extract the second component of the state and cast it to what we expect
  const ob::SO3StateSpace::StateType *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(void)
{
  // construct the state space we are planning in
  ob::StateSpacePtr space(new ob::SE3StateSpace());

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

  space->as<ob::SE3StateSpace>()->setBounds(bounds);

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

  // set state validity checking for this space
  ss.setStateValidityChecker(boost::bind(&isStateValid, _1));

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

  ob::PlannerPtr planner(new og::RRT(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;
    // print the path to screen

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

    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, char **)
{
  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
  planWithSimpleSetup();

  return 0;
}

17-33行是检测当前状态是否可行。ob::SE3StateSpace::StateType *se3state是3维情况，ob::SE2StateSpace::StateType *se2state则表示二维。38-43设置边界范围。54-59为设置初始和目标随机位置。64-65设置用什么算法解决。可用算法参考可用算法。68期望解决的时间。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

ADD_DEFINITIONS(
  -Wall
  -g
  -O2
  -std=c++11
)

project(RigidBodyPlanning)

find_package(OMPL REQUIRED)

include_directories(${OMPL_INCLUDE_DIRS})
link_directories(${OMPL_LIBRARY_DIRS})
add_definitions(${OMPL_DEFINITIONS})

FIND_PACKAGE(Boost COMPONENTS system REQUIRED)

add_executable (RigidBodyPlanning RigidBodyPlanning.cpp)
target_link_libraries (RigidBodyPlanning ${Boost_LIBRARIES} ${OMPL_LIBRARIES})

 

 

 

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