DynamicCarPlanning.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/est/EST.h>
15 #include <ompl/control/planners/rrt/RRT.h>
16 #include <ompl/control/planners/kpiece/KPIECE1.h>
17 #include <ompl/control/planners/pdst/PDST.h>
18 #include <omplapp/apps/DynamicCarPlanning.h>
19 #include <omplapp/config.h>
20 
21 using namespace ompl;
22 
23 void dynamicCarSetup(app::DynamicCarPlanning& setup)
24 {
25  // plan for dynamic car in SE(2)
26  base::StateSpacePtr stateSpace(setup.getStateSpace());
27 
28  // set the bounds for the R^2 part of SE(2)
29  base::RealVectorBounds bounds(2);
30  bounds.setLow(-10);
31  bounds.setHigh(10);
32  stateSpace->as<base::CompoundStateSpace>()->as<base::SE2StateSpace>(0)->setBounds(bounds);
33 
34  // define start state
35  base::ScopedState<> start(stateSpace);
36  start[0] = start[1] = start[2] = start[3] = start[4] = 0.;
37 
38  // define goal state
39  base::ScopedState<> goal(stateSpace);
40  goal[0] = goal[1] = 8.;
41  goal[2] = 0;
42  goal[3] = goal[4] = 0.;
43 
44  // set the start & goal states
45  setup.setStartAndGoalStates(start, goal, .5);
46 
47  // optionally, set a planner
48  //setup.setPlanner(std::make_shared<control::EST>(setup.getSpaceInformation()));
49  //setup.setPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
50  //setup.setPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
51  //setup.setPlanner(std::make_shared<control::PDST>(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 
58 void dynamicCarDemo(app::DynamicCarPlanning& setup)
59 {
60  std::cout<<"\n\n***** Planning for a " << setup.getName() << " *****\n" << std::endl;
61 
62  // try to solve the problem
63  if (setup.solve(40))
64  {
65  // print the (approximate) solution path: print states along the path
66  // and controls required to get from one state to the next
68  //path.interpolate(); // uncomment if you want to plot the path
69  path.printAsMatrix(std::cout);
70  if (!setup.haveExactSolutionPath())
71  {
72  std::cout << "Solution is approximate. Distance to actual goal is " <<
73  setup.getProblemDefinition()->getSolutionDifference() << std::endl;
74  }
75  }
76 
77 }
78 void dynamicCarBenchmark(app::DynamicCarPlanning& setup)
79 {
80  tools::Benchmark::Request request(100., 10000., 10); // runtime (s), memory (MB), run count
81 
82  setup.setup ();
83 
84  tools::Benchmark b(setup, setup.getName());
85  b.addPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
86  b.addPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
87  b.benchmark(request);
88  b.saveResultsToFile();
89 }
90 
91 int main(int argc, char** /*unused*/)
92 {
94  dynamicCarSetup(car);
95 
96  // If any command line arguments are given, solve the problem multiple
97  // times with different planners and collect benchmark statistics.
98  // Otherwise, solve the problem once and print the path.
99  if (argc>1)
100  dynamicCarBenchmark(car);
101  else
102  dynamicCarDemo(car);
103  return 0;
104 }
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
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 setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.cpp:90
A class to facilitate planning for a generic second-order car model.
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
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 space to allow the composition of state spaces.
Definition: StateSpace.h:573
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.