DynamicCarPlanning.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/DynamicCarPlanning.h"
14 #include <boost/math/constants/constants.hpp>
15 
16 ompl::base::ScopedState<> ompl::app::DynamicCarPlanning::getDefaultStartState() const
17 {
18  base::ScopedState<base::CompoundStateSpace> s(getStateSpace());
21 
22  aiVector3D c = getRobotCenter(0);
23  pose.setX(c.x);
24  pose.setY(c.y);
25  pose.setYaw(0.);
26  vel.values[0] = 0.;
27  vel.values[1] = 0.;
28  return s;
29 }
30 
31 void ompl::app::DynamicCarPlanning::ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
32 {
33  // Retrieving control inputs
34  const double *u = ctrl->as<control::RealVectorControlSpace::ControlType>()->values;
35 
36  // zero out qdot
37  qdot.resize (q.size (), 0);
38 
39  qdot[0] = q[3] * cos(q[2]);
40  qdot[1] = q[3] * sin(q[2]);
41  qdot[2] = q[3] * mass_ * lengthInv_ * tan(q[4]);
42 
43  qdot[3] = u[0];
44  qdot[4] = u[1];
45 }
46 
47 void ompl::app::DynamicCarPlanning::postPropagate(const base::State* /*state*/, const control::Control* /*control*/, const double /*duration*/, base::State* result)
48 {
49  // Normalize orientation value between 0 and 2*pi
50  const base::SO2StateSpace* SO2 = getStateSpace()->as<base::CompoundStateSpace>()
51  ->as<base::SE2StateSpace>(0)->as<base::SO2StateSpace>(1);
52  auto* so2 = result->as<base::CompoundStateSpace::StateType>()
53  ->as<base::SE2StateSpace::StateType>(0)->as<base::SO2StateSpace::StateType>(1);
54  SO2->enforceBounds(so2);
55 }
56 
57 void ompl::app::DynamicCarPlanning::setDefaultBounds()
58 {
59  base::RealVectorBounds bounds(2);
60  bounds.low[0] = -1.;
61  bounds.high[0] = 1.;
62  bounds.low[1] = -boost::math::constants::pi<double>() * 30. / 180.;
63  bounds.high[1] = boost::math::constants::pi<double>() * 30. / 180.;
64  getStateSpace()->as<base::CompoundStateSpace>()->as<base::RealVectorStateSpace>(1)->setBounds(bounds);
65  bounds.low[0] = -.5;
66  bounds.high[0] = .5;
67  bounds.low[1] = -boost::math::constants::pi<double>() * 2. / 180.;
68  bounds.high[1] = boost::math::constants::pi<double>() * 2. / 180.;
69  getControlSpace()->as<control::RealVectorControlSpace>()->setBounds(bounds);
70 }
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
ompl::control::Control ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:131
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot'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:641
Definition of a scoped state.
Definition: ScopedState.h:120
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:141