QuadrotorPlanning.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_QUADROTOR_PLANNING_
14 #define OMPLAPP_QUADROTOR_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 QuadrotorPlanning : public AppBase<CONTROL>
37  {
38  public:
40  : AppBase<CONTROL>(constructControlSpace(), Motion_3D), timeStep_(1e-2), massInv_(1.), beta_(1.),
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("Quadrotor");
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  ~QuadrotorPlanning() 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  double getMass()
72  {
73  return 1./massInv_;
74  }
75  void setMass(double mass)
76  {
77  massInv_ = 1./mass;
78  }
79  double getDampingCoefficient()
80  {
81  return beta_;
82  }
83  void setDampingCoefficient(double beta)
84  {
85  beta_ = beta;
86  }
87  virtual void setDefaultBounds();
88 
89  protected:
90 
91  const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int /*index*/) const override
92  {
93  return state->as<base::CompoundState>()->components[0];
94  }
95 
96  virtual void ode(const control::ODESolver::StateType& q, const control::Control* ctrl, control::ODESolver::StateType& qdot);
97 
98  virtual void postPropagate(const base::State* state, const control::Control* control, const double duration, base::State* result);
99 
100  static control::ControlSpacePtr constructControlSpace()
101  {
102  return std::make_shared<control::RealVectorControlSpace>(constructStateSpace(), 4);
103  }
104  static base::StateSpacePtr constructStateSpace();
105 
106  double timeStep_;
107  double massInv_;
108  double beta_;
109  control::ODESolverPtr odeSolver;
110  };
111 
112  }
113 }
114 
115 #endif
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.
A class to facilitate planning for a simple quadrotor model.
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