void planWithSimpleSetupPen(std::string title = "Default")
{
// x, theta, velocity, angular veloctiy
ompl::base::StateSpacePtr space;
ompl::base::StateSpacePtr pos(new ompl::base::RealVectorStateSpace(1));
ompl::base::StateSpacePtr theta(new ompl::base::SO2StateSpace());
ompl::base::StateSpacePtr vel(new ompl::base::RealVectorStateSpace(1));
ompl::base::StateSpacePtr omega(new ompl::base::SO2StateSpace());
ompl::base::RealVectorBounds position_limit(1);
position_limit.setLow(-xaxis_limit);
position_limit.setHigh(xaxis_limit);
pos->as<ompl::base::RealVectorStateSpace>()->setBounds(position_limit);
ompl::base::RealVectorBounds vel_limit(1);
vel_limit.setLow(-std::numeric_limits<double>::infinity());
vel_limit.setHigh(std::numeric_limits<double>::infinity());
vel->as<ompl::base::RealVectorStateSpace>()->setBounds(vel_limit);
space = pos + theta + vel + omega;
// create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 1));
// set the bounds for the control space
//ob::RealVectorBounds cbounds(1);
//cbounds.setLow(0);
//cbounds.setHigh(acceleration_limit);
//cspace->as<RealVectorControlSpace>()->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
ss.setStateValidityChecker(isStateValidPen);
// Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
// when integration has finished to normalize the orientation values.
oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &PendulumODE));
ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver, PendulumPostIntegration));
/// create a start state
ob::ScopedState<> start(space);
start[0] = 0.0;
start[1] = -0.35;
start[2] = 0.0;
start[3] = 0.0;
/// create a goal state; use the hard way to set the elements
ob::ScopedState<> goal(space);
goal[1] = 0.0;
goal[3] = 0.0;
/// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
ss.getSpaceInformation()->setMinMaxControlDuration(1, 10);
ss.getSpaceInformation()->setPropagationStepSize(0.01);
ompl::base::PlannerPtr planner(new ompl::control::PID(ss.getSpaceInformation()));
ss.setPlanner(planner);
ss.setup();
/// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(10);
/*
if (solved)
{
ompl::control::PathControl& path = ss.getSolutionPath();
path.printAsMatrix(std::cout);
// print path to file
std::ofstream fout("path.txt");
path.printAsMatrix(fout);
fout.close();
}
*/
}
void planWithSimpleSetup(double start_x, double start_y, double start_theta,
double goal_x, double goal_y, double goal_theta)
{
/// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
/// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// create a control space
oc::ControlSpacePtr cspace(new DemoControlSpace(space));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-0.3);
cbounds.setHigh(0.3);
cspace->as<DemoControlSpace>()->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
// Setting the propagation routine for this space:
// KinematicCarModel does NOT use ODESolver
//ss.setStatePropagator(oc::StatePropagatorPtr(new KinematicCarModel(ss.getSpaceInformation())));
// Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
// when integration has finished to normalize the orientation values.
// oc::ODEBasicSolver<> odeSolver(ss.getSpaceInformation(), &KinematicCarODE);
// ss.setStatePropagator(odeSolver.getStatePropagator(&KinematicCarPostIntegration));
oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &KinematicCarODE));
ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver/*, &KinematicCarPostIntegration*/));
/// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(start_x);
start->setY(start_y);
start->setYaw(start_theta);
/// create a goal state; use the hard way to set the elements
ob::ScopedState<ob::SE2StateSpace> goal(space);
goal->setX(goal_x);
goal->setY(goal_y);
goal->setYaw(goal_theta);
/// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
/// we want to have a reasonable value for the propagation step size
ss.setup();
/// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(10.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
/// print the path to screen
ss.getSolutionPath().asGeometric().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
