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  odeSolver(std::make_shared<control::ODEBasicSolver<>>(si_, [this](const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
42  {
43  ode(q, ctrl, qdot);
44  }))
45  {
46  name_ = std::string("Blimp");
47  setDefaultBounds();
48 
49  si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver,
50  [this](const base::State* state, const control::Control* control, const double duration, base::State* result)
51  {
52  postPropagate(state, control, duration, result);
53  }));
54  }
55  ~BlimpPlanning() override = default;
56 
57  bool isSelfCollisionEnabled() const override
58  {
59  return false;
60  }
61  unsigned int getRobotCount() const override
62  {
63  return 1;
64  }
65  base::ScopedState<> getDefaultStartState() const override;
66  base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const override;
67  const base::StateSpacePtr& getGeometricComponentStateSpace() const override
68  {
69  return getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(0);
70  }
71 
72  virtual void setDefaultBounds();
73 
74  protected:
75 
76  const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int /*index*/) const override
77  {
78  return state->as<base::CompoundState>()->components[0];
79  }
80 
81  void postPropagate(const base::State* state, const control::Control* control, double duration, base::State* result);
82 
83  virtual void ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot);
84 
85  static control::ControlSpacePtr constructControlSpace()
86  {
87  return std::make_shared<control::RealVectorControlSpace>(constructStateSpace(), 3);
88  }
89  static base::StateSpacePtr constructStateSpace();
90 
91  double timeStep_{1e-2};
92  control::ODESolverPtr odeSolver;
93  };
94 
95  }
96 }
97 
98 #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