BlimpPlanning.h
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 #ifndef OMPLAPP_BLIMP_PLANNING_
14 #define OMPLAPP_BLIMP_PLANNING_
15 
16 #include "omplapp/apps/AppBase.h"
17 #include <ompl/base/spaces/SE3StateSpace.h>
18 #include <ompl/control/ODESolver.h>
19 #include <ompl/control/spaces/RealVectorControlSpace.h>
20 
21 namespace ompl
22 {
23  namespace app
24  {
36  class BlimpPlanning : public AppBase<CONTROL>
37  {
38  public:
40  : AppBase<CONTROL>(constructControlSpace(), Motion_3D),
41  timeStep_(1e-2),
42  odeSolver(std::make_shared<control::ODEBasicSolver<>>(si_, [this](const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
43  {
44  ode(q, ctrl, qdot);
45  }))
46  {
47  name_ = std::string("Blimp");
48  setDefaultBounds();
49 
50  si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver,
51  [this](const base::State* state, const control::Control* control, const double duration, base::State* result)
52  {
53  postPropagate(state, control, duration, result);
54  }));
55  }
56  ~BlimpPlanning() override = default;
57 
58  bool isSelfCollisionEnabled() const override
59  {
60  return false;
61  }
62  unsigned int getRobotCount() const override
63  {
64  return 1;
65  }
66  base::ScopedState<> getDefaultStartState() const override;
67  base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const override;
68  const base::StateSpacePtr& getGeometricComponentStateSpace() const override
69  {
70  return getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(0);
71  }
72 
73  virtual void setDefaultBounds();
74 
75  protected:
76 
77  const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int /*index*/) const override
78  {
79  return state->as<base::CompoundState>()->components[0];
80  }
81 
82  void postPropagate(const base::State* state, const control::Control* control, const double duration, base::State* result);
83 
84  virtual void ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot);
85 
86  static control::ControlSpacePtr constructControlSpace()
87  {
88  return std::make_shared<control::RealVectorControlSpace>(constructStateSpace(), 3);
89  }
90  static base::StateSpacePtr constructStateSpace();
91 
92  double timeStep_;
93  control::ODESolverPtr odeSolver;
94  };
95 
96  }
97 }
98 
99 #endif
A class to facilitate planning for a simple blimp model.
Definition: BlimpPlanning.h:36
Definition of a compound state.
Definition: State.h:86
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
Definition of an abstract control.
Definition: Control.h:47
A shared pointer wrapper for ompl::base::StateSpace.
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:274
A shared pointer wrapper for ompl::control::ODESolver.
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=nullptr)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
Definition: ODESolver.h:123
A shared pointer wrapper for ompl::control::ControlSpace.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:73
Definition of an abstract state.
Definition: State.h:49
Basic solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current sta...
Definition: ODESolver.h:195