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/control/planners/est/EST.h>
14 #include <ompl/control/planners/kpiece/KPIECE1.h>
15 #include <ompl/control/planners/pdst/PDST.h>
16 #include <ompl/control/planners/rrt/RRT.h>
17 #include <ompl/tools/benchmark/Benchmark.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
67  control::PathControl &path(setup.getSolutionPath());
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 void dynamicCarBenchmark(app::DynamicCarPlanning &setup)
78 {
79  tools::Benchmark::Request request(100., 10000., 10); // runtime (s), memory (MB), run count
80 
81  setup.setup();
82 
83  tools::Benchmark b(setup, setup.getName());
84  b.addPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
85  b.addPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
86  b.benchmark(request);
87  b.saveResultsToFile();
88 }
89 
90 int main(int argc, char ** /*unused*/)
91 {
93  dynamicCarSetup(car);
94 
95  // If any command line arguments are given, solve the problem multiple
96  // times with different planners and collect benchmark statistics.
97  // Otherwise, solve the problem once and print the path.
98  if (argc > 1)
99  dynamicCarBenchmark(car);
100  else
101  dynamicCarDemo(car);
102  return 0;
103 }
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
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
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
Representation of a benchmark request.
Definition: Benchmark.h:156
The lower and upper bounds for an Rn space.