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/control/planners/kpiece/KPIECE1.h>
14 #include <ompl/control/planners/rrt/RRT.h>
15 #include <ompl/tools/benchmark/Benchmark.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)
25  base::StateSpacePtr SE2(setup.getStateSpace());
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
62  control::PathControl &path(setup.getSolutionPath());
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 void kinematicCarBenchmark(app::KinematicCarPlanning &setup)
73 {
74  tools::Benchmark::Request request(20., 10000., 10); // runtime (s), memory (MB), run count
75 
76  tools::Benchmark b(setup, setup.getName());
77  b.addPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
78  b.addPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
79  b.benchmark(request);
80  b.saveResultsToFile();
81 }
82 
83 int main(int argc, char ** /*unused*/)
84 {
85  app::KinematicCarPlanning regularCar;
86 
87  kinematicCarSetup(regularCar);
88 
89  // If any command line arguments are given, solve the problem multiple
90  // times with different planners and collect benchmark statistics.
91  // Otherwise, solve the problem once for each car type and print the path.
92  if (argc > 1)
93  kinematicCarBenchmark(regularCar);
94  else
95  kinematicCarDemo(regularCar);
96  return 0;
97 }
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
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
Representation of a benchmark request.
Definition: Benchmark.h:156
The lower and upper bounds for an Rn space.