KinematicCarPlanning.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_KINEMATIC_CAR_PLANNING_
14 #define OMPLAPP_KINEMATIC_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  {
39  class KinematicCarPlanning : public AppBase<AppType::CONTROL>
40  {
41  public:
42  KinematicCarPlanning();
43  KinematicCarPlanning(const control::ControlSpacePtr &controlSpace);
44  ~KinematicCarPlanning() override = default;
45 
46  bool isSelfCollisionEnabled() const override
47  {
48  return false;
49  }
50  unsigned int getRobotCount() const override
51  {
52  return 1;
53  }
54  base::ScopedState<> getDefaultStartState() const override;
55  base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const override
56  {
57  return state;
58  }
59  const base::StateSpacePtr& getGeometricComponentStateSpace() const override
60  {
61  return getStateSpace();
62  }
63  double getVehicleLength()
64  {
65  return 1./lengthInv_;
66  }
67  void setVehicleLength(double length)
68  {
69  lengthInv_ = 1./length;
70  }
71  virtual void setDefaultControlBounds();
72 
73  protected:
74 
75  const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int /*index*/) const override
76  {
77  return state;
78  }
79 
80  virtual void ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot);
81 
82  virtual void postPropagate(const base::State* state, const control::Control* control, double duration, base::State* result);
83 
84  static control::ControlSpacePtr constructControlSpace()
85  {
86  return std::make_shared<control::RealVectorControlSpace>(constructStateSpace(), 2);
87  }
88  static base::StateSpacePtr constructStateSpace()
89  {
90  return std::make_shared<base::SE2StateSpace>();
91  }
92 
93  double timeStep_{1e-2};
94  double lengthInv_{1.};
95  control::ODESolverPtr odeSolver;
96  };
97  }
98 }
99 
100 #endif
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156
Definition of a scoped state.
Definition: ScopedState.h:120
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