KinematicCarPlanning.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Mark Moll */
12 
13 #include <ompl/tools/benchmark/Benchmark.h>
14 #include <ompl/control/planners/rrt/RRT.h>
15 #include <ompl/control/planners/kpiece/KPIECE1.h>
16 #include <omplapp/apps/KinematicCarPlanning.h>
17 #include <omplapp/config.h>
18 #include <boost/math/constants/constants.hpp>
19 
20 using namespace ompl;
21 
22 void kinematicCarSetup(app::KinematicCarPlanning& setup)
23 {
24  // plan for kinematic car in SE(2)
26 
27  // set the bounds for the R^2 part of SE(2)
28  base::RealVectorBounds bounds(2);
29  bounds.setLow(-10);
30  bounds.setHigh(10);
31  SE2->as<base::SE2StateSpace>()->setBounds(bounds);
32 
33  // define start state
35  start->setX(0);
36  start->setY(0);
37  start->setYaw(0);
38 
39  // define goal state
41  goal->setX(2);
42  goal->setY(2);
43  goal->setYaw(boost::math::constants::pi<double>());
44 
45  // set the start & goal states
46  setup.setStartAndGoalStates(start, goal, .1);
47 }
48 
49 void kinematicCarDemo(app::KinematicCarPlanning& setup)
50 {
51  setup.setPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
52  std::vector<double> cs(2);
53  cs[0] = cs[1] = 0.1;
54  setup.setup();
55  setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);
56 
57  // try to solve the problem
58  if (setup.solve(20))
59  {
60  // print the (approximate) solution path: print states along the path
61  // and controls required to get from one state to the next
63  //path.interpolate(); // uncomment if you want to plot the path
64  path.printAsMatrix(std::cout);
65  if (!setup.haveExactSolutionPath())
66  {
67  std::cout << "Solution is approximate. Distance to actual goal is " <<
68  setup.getProblemDefinition()->getSolutionDifference() << std::endl;
69  }
70  }
71 
72 }
73 void kinematicCarBenchmark(app::KinematicCarPlanning& setup)
74 {
75  tools::Benchmark::Request request(20., 10000., 10); // runtime (s), memory (MB), run count
76 
77  tools::Benchmark b(setup, setup.getName());
78  b.addPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
79  b.addPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
80  b.benchmark(request);
81  b.saveResultsToFile();
82 }
83 
84 int main(int argc, char**)
85 {
86  app::KinematicCarPlanning regularCar;
87 
88  kinematicCarSetup(regularCar);
89 
90  // If any command line arguments are given, solve the problem multiple
91  // times with different planners and collect benchmark statistics.
92  // Otherwise, solve the problem once for each car type and print the path.
93  if (argc>1)
94  kinematicCarBenchmark(regularCar);
95  else
96  kinematicCarDemo(regularCar);
97  return 0;
98 }
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.cpp:90
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:87
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
Definition: Benchmark.h:253
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:98
void setPlanner(const base::PlannerPtr &planner)
Set the planner to use. If the planner is not set, an attempt is made to use the planner allocator...
Definition: SimpleSetup.h:208
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:75
Definition of a control path.
Definition: PathControl.h:60
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A class to facilitate planning for a generic kinematic car model.
A state space representing SE(2)
Definition: SE2StateSpace.h:49
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:81
Representation of a benchmark request.
Definition: Benchmark.h:156
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
The lower and upper bounds for an Rn space.