BlimpPlanning.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/BlimpPlanning.h>
17 #include <omplapp/config.h>
18 
19 using namespace ompl;
20 
21 void blimpSetup(app::BlimpPlanning& setup)
22 {
23  base::StateSpacePtr stateSpace(setup.getStateSpace());
24 
25  // set the bounds for the R^3 part of SE(3)
26  base::RealVectorBounds bounds(3);
27  bounds.setLow(-10);
28  bounds.setHigh(10);
29  stateSpace->as<base::CompoundStateSpace>()->as<base::SE3StateSpace>(0)->setBounds(bounds);
30 
31  // define start state
32  base::ScopedState<base::SE3StateSpace> start(setup.getGeometricComponentStateSpace());
33  start->setX(0.);
34  start->setY(0.);
35  start->setZ(0.);
36  start->rotation().setIdentity();
37 
38  // define goal state
39  base::ScopedState<base::SE3StateSpace> goal(setup.getGeometricComponentStateSpace());
40  goal->setX(5.);
41  goal->setY(5.);
42  goal->setZ(5.);
43  goal->rotation().setIdentity();
44 
45  // set the start & goal states
47  setup.getFullStateFromGeometricComponent(start),
48  setup.getFullStateFromGeometricComponent(goal), .5);
49 }
50 
51 void blimpDemo(app::BlimpPlanning& setup)
52 {
53  std::vector<double> coords;
54 
55  std::cout<<"\n\n***** Planning for a " << setup.getName() << " *****\n" << std::endl;
56  setup.setPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
57 
58  // try to solve the problem
59  if (setup.solve(40))
60  {
61  // print the (approximate) solution path: print states along the path
62  // and controls required to get from one state to the next
64  path.interpolate(); // uncomment if you want to plot the path
65  path.printAsMatrix(std::cout);
66 
67  if (!setup.haveExactSolutionPath())
68  {
69  std::cout << "Solution is approximate. Distance to actual goal is " <<
70  setup.getProblemDefinition()->getSolutionDifference() << std::endl;
71  }
72  }
73 }
74 
75 void blimpBenchmark(app::BlimpPlanning& setup)
76 {
77  tools::Benchmark::Request request(100., 10000., 10); // runtime (s), memory (MB), run count
78 
79  setup.setup();
80 
81  tools::Benchmark b(setup, setup.getName());
82  b.addPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
83  b.addPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
84  b.benchmark(request);
85  b.saveResultsToFile();
86 }
87 
88 int main(int argc, char**)
89 {
90  app::BlimpPlanning blimp;
91  blimpSetup(blimp);
92 
93  // If any command line arguments are given, solve the problem multiple
94  // times with different planners and collect benchmark statistics.
95  // Otherwise, solve the problem once and print the path.
96  if (argc>1)
97  blimpBenchmark(blimp);
98  else
99  blimpDemo(blimp);
100  return 0;
101 }
A class to facilitate planning for a simple blimp model.
Definition: BlimpPlanning.h:36
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 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.