DynamicCarPlanning.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_DYNAMIC_CAR_PLANNING_
14 #define OMPLAPP_DYNAMIC_CAR_PLANNING_
15 
16 #include "omplapp/apps/AppBase.h"
17 #include <ompl/base/spaces/SE2StateSpace.h>
18 #include <ompl/control/ODESolver.h>
19 #include <ompl/control/spaces/RealVectorControlSpace.h>
20 
21 namespace ompl
22 {
23  namespace app
24  {
42  class DynamicCarPlanning : public AppBase<CONTROL>
43  {
44  public:
46  : AppBase<CONTROL>(constructControlSpace(), Motion_2D),
47  timeStep_(1e-2), lengthInv_(1.), mass_(1.),
48  odeSolver(std::make_shared<control::ODEBasicSolver<>>(si_, [this](const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
49  {
50  ode(q, ctrl, qdot);
51  }))
52  {
53  name_ = std::string("Dynamic car");
54  setDefaultBounds();
55 
56  si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver,
57  [this](const base::State* state, const control::Control* control, const double duration, base::State* result)
58  {
59  postPropagate(state, control, duration, result);
60  }));
61  }
62  ~DynamicCarPlanning() override = default;
63 
64  bool isSelfCollisionEnabled() const override
65  {
66  return false;
67  }
68  unsigned int getRobotCount() const override
69  {
70  return 1;
71  }
72  base::ScopedState<> getDefaultStartState() const override;
73  base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const override
74  {
76  r = 0.0;
77  r[0] = state[0];
78  r[1] = state[1];
79  r[2] = state[2];
80  return r;
81  }
82 
83  const base::StateSpacePtr& getGeometricComponentStateSpace() const override
84  {
85  return getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(0);
86  }
87 
88  double getVehicleLength()
89  {
90  return 1./lengthInv_;
91  }
92  void setVehicleLength(double length)
93  {
94  lengthInv_ = 1./length;
95  }
96  double getMass()
97  {
98  return mass_;
99  }
100  void setMass(double mass)
101  {
102  mass_ = mass;
103  }
104  virtual void setDefaultBounds();
105 
106  protected:
107 
108  const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int /*index*/) const override
109  {
110  return state->as<base::CompoundState>()->components[0];
111  }
112 
113  virtual void ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot);
114 
115  virtual void postPropagate(const base::State* state, const control::Control* control, const double duration, base::State* result);
116 
117  static control::ControlSpacePtr constructControlSpace()
118  {
119  return std::make_shared<control::RealVectorControlSpace>(constructStateSpace(), 2);
120  }
121  static base::StateSpacePtr constructStateSpace()
122  {
123  auto stateSpace(std::make_shared<base::CompoundStateSpace>());
124  stateSpace->addSubspace(std::make_shared<base::SE2StateSpace>(), 1.);
125  stateSpace->addSubspace(std::make_shared<base::RealVectorStateSpace>(2), .3);
126  stateSpace->lock();
127  return stateSpace;
128  }
129 
130  double timeStep_;
131  double lengthInv_;
132  double mass_;
133  control::ODESolverPtr odeSolver;
134  };
135 
136  }
137 }
138 
139 #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.
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