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