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/control/planners/kpiece/KPIECE1.h>
14 #include <ompl/control/planners/rrt/RRT.h>
15 #include <ompl/tools/benchmark/Benchmark.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
46  setup.setStartAndGoalStates(setup.getFullStateFromGeometricComponent(start),
47  setup.getFullStateFromGeometricComponent(goal), .5);
48 }
49 
50 void blimpDemo(app::BlimpPlanning &setup)
51 {
52  std::vector<double> coords;
53 
54  std::cout << "\n\n***** Planning for a " << setup.getName() << " *****\n" << std::endl;
55  setup.setPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
56 
57  // try to solve the problem
58  if (setup.solve(40))
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 
66  if (!setup.haveExactSolutionPath())
67  {
68  std::cout << "Solution is approximate. Distance to actual goal is "
69  << setup.getProblemDefinition()->getSolutionDifference() << std::endl;
70  }
71  }
72 }
73 
74 void blimpBenchmark(app::BlimpPlanning &setup)
75 {
76  tools::Benchmark::Request request(100., 10000., 10); // runtime (s), memory (MB), run count
77 
78  setup.setup();
79 
80  tools::Benchmark b(setup, setup.getName());
81  b.addPlanner(std::make_shared<control::RRT>(setup.getSpaceInformation()));
82  b.addPlanner(std::make_shared<control::KPIECE1>(setup.getSpaceInformation()));
83  b.benchmark(request);
84  b.saveResultsToFile();
85 }
86 
87 int main(int argc, char ** /*unused*/)
88 {
89  app::BlimpPlanning blimp;
90  blimpSetup(blimp);
91 
92  // If any command line arguments are given, solve the problem multiple
93  // times with different planners and collect benchmark statistics.
94  // Otherwise, solve the problem once and print the path.
95  if (argc > 1)
96  blimpBenchmark(blimp);
97  else
98  blimpDemo(blimp);
99  return 0;
100 }
A class to facilitate planning for a simple blimp model.
Definition: BlimpPlanning.h:36
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 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.