KinematicCarPlanning.cpp
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 #include "omplapp/apps/KinematicCarPlanning.h"
14 #include <boost/math/constants/constants.hpp>
15 
16 ompl::app::KinematicCarPlanning::KinematicCarPlanning()
17  : AppBase<CONTROL>(constructControlSpace(), Motion_2D), odeSolver(std::make_shared<control::ODEBasicSolver<>>(si_, [this](const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
18  {
19  ode(q, ctrl, qdot);
20  }))
21 {
22  name_ = std::string("Kinematic car");
23  setDefaultControlBounds();
24 
25  si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver,
26  [this](const base::State* state, const control::Control* control, const double duration, base::State* result)
27  {
28  postPropagate(state, control, duration, result);
29  }));
30 }
31 
32 ompl::app::KinematicCarPlanning::KinematicCarPlanning(const control::ControlSpacePtr &controlSpace)
33  : AppBase<CONTROL>(controlSpace, Motion_2D), timeStep_(1e-2), lengthInv_(1.), odeSolver(std::make_shared<control::ODEBasicSolver<>>(si_, [this](const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
34  {
35  ode(q, ctrl, qdot);
36  }))
37 {
38  setDefaultControlBounds();
39 
40  si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver,
41  [this](const base::State* state, const control::Control* control, const double duration, base::State* result)
42  {
43  postPropagate(state, control, duration, result);
44  }));
45 }
46 
47 ompl::base::ScopedState<> ompl::app::KinematicCarPlanning::getDefaultStartState() const
48 {
49  base::ScopedState<base::SE2StateSpace> sSE2(getStateSpace());
50  aiVector3D s = getRobotCenter(0);
51 
52  sSE2->setX(s.x);
53  sSE2->setY(s.y);
54  sSE2->setYaw(0.);
55  return sSE2;
56 }
57 
58 void ompl::app::KinematicCarPlanning::setDefaultControlBounds()
59 {
60  base::RealVectorBounds cbounds(2);
61  cbounds.low[0] = -5.;
62  cbounds.high[0] = 5.;
63  cbounds.low[1] = -boost::math::constants::pi<double>() * 30. / 180.;
64  cbounds.high[1] = boost::math::constants::pi<double>() * 30. / 180.;
65  getControlSpace()->as<control::RealVectorControlSpace>()->setBounds(cbounds);
66 }
67 
68 void ompl::app::KinematicCarPlanning::ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
69 {
70  const double *u = ctrl->as<control::RealVectorControlSpace::ControlType>()->values;
71 
72  // zero out qdot
73  qdot.resize (q.size (), 0);
74 
75  qdot[0] = u[0] * cos(q[2]);
76  qdot[1] = u[0] * sin(q[2]);
77  qdot[2] = u[0] * lengthInv_ * tan(u[1]);
78 }
79 
80 void ompl::app::KinematicCarPlanning::postPropagate(const base::State* /*state*/, const control::Control* /*control*/, const double /*duration*/, base::State* result)
81 {
82  // Normalize orientation value between 0 and 2*pi
83  const base::SO2StateSpace* SO2 = getStateSpace()->as<base::SE2StateSpace>()
84  ->as<base::SO2StateSpace>(1);
85  auto* so2 = result->as<base::SE2StateSpace::StateType>()
86  ->as<base::SO2StateSpace::StateType>(1);
87  SO2->enforceBounds(so2);
88 }
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
STL namespace.
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:274
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
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot&#39;s center (average of all the vertices of all its parts)
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:73