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<AppType::CONTROL>
37  {
38  public:
39  BlimpPlanning()
40  : AppBase<AppType::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
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156
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:191
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:346
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:141
Main namespace. Contains everything in this library.
Definition: AppBase.h:21