QuadrotorPlanning.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/QuadrotorPlanning.h"
14 
15 ompl::base::ScopedState<> ompl::app::QuadrotorPlanning::getDefaultStartState() const
16 {
17  base::ScopedState<base::SE3StateSpace> s(getGeometricComponentStateSpace());
18  aiVector3D c = getRobotCenter(0);
19 
20  s->setXYZ(c.x, c.y, c.z);
21  s->rotation().setIdentity();
22  return getFullStateFromGeometricComponent(s);
23 }
24 
25 
26 ompl::base::ScopedState<> ompl::app::QuadrotorPlanning::getFullStateFromGeometricComponent(
27  const base::ScopedState<> &state) const
28 {
29  base::ScopedState<> s(getStateSpace());
30  std::vector <double> reals = state.reals ();
31 
32  s = 0.0;
33  for (size_t i = 0; i < reals.size (); ++i)
34  s[i] = reals[i];
35  return s;
36 }
37 
38 void ompl::app::QuadrotorPlanning::ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
39 {
40  const double *u = ctrl->as<control::RealVectorControlSpace::ControlType>()->values;
41 
42  // zero out qdot
43  qdot.resize (q.size (), 0);
44 
45  // derivative of position
46  qdot[0] = q[7];
47  qdot[1] = q[8];
48  qdot[2] = q[9];
49 
50  // derivative of orientation
51  // 1. First convert omega to quaternion: qdot = omega * q / 2
53  qomega.w = 0;
54  qomega.x = .5*q[10];
55  qomega.y = .5*q[11];
56  qomega.z = .5*q[12];
57 
58  // 2. We include a numerical correction so that dot(q,qdot) = 0. This constraint is
59  // obtained by differentiating q * q_conj = 1
60  double delta = q[3] * qomega.x + q[4] * qomega.y + q[5] * qomega.z;
61 
62  // 3. Finally, set the derivative of orientation
63  qdot[3] = qomega.x - delta * q[3];
64  qdot[4] = qomega.y - delta * q[4];
65  qdot[5] = qomega.z - delta * q[5];
66  qdot[6] = qomega.w - delta * q[6];
67 
68  // derivative of velocity
69  // the z-axis of the body frame in world coordinates is equal to
70  // (2(wy+xz), 2(yz-wx), w^2-x^2-y^2+z^2).
71  // This can be easily verified by working out q * (0,0,1).
72  qdot[7] = massInv_ * (-2*u[0]*(q[6]*q[4] + q[3]*q[5]) - beta_ * q[7]);
73  qdot[8] = massInv_ * (-2*u[0]*(q[4]*q[5] - q[6]*q[3]) - beta_ * q[8]);
74  qdot[9] = massInv_ * ( -u[0]*(q[6]*q[6]-q[3]*q[3]-q[4]*q[4]+q[5]*q[5]) - beta_ * q[9]) - 9.81;
75 
76  // derivative of rotational velocity
77  qdot[10] = u[1];
78  qdot[11] = u[2];
79  qdot[12] = u[3];
80 }
81 
82 void ompl::app::QuadrotorPlanning::postPropagate(const base::State* /*state*/, const control::Control* /*control*/, const double /*duration*/, base::State* result)
83 {
84  const base::CompoundStateSpace* cs = getStateSpace()->as<base::CompoundStateSpace>();
85  const base::SO3StateSpace* SO3 = cs->as<base::SE3StateSpace>(0)->as<base::SO3StateSpace>(1);
87  base::SO3StateSpace::StateType& so3State = csState.as<base::SE3StateSpace::StateType>(0)->rotation();
88 
89  // Normalize the quaternion representation for the quadrotor
90  SO3->enforceBounds(&so3State);
91  // Enforce velocity bounds
92  cs->getSubspace(1)->enforceBounds(csState[1]);
93 }
94 
95 ompl::base::StateSpacePtr ompl::app::QuadrotorPlanning::constructStateSpace()
96 {
97  auto stateSpace(std::make_shared<base::CompoundStateSpace>());
98 
99  stateSpace->addSubspace(std::make_shared<base::SE3StateSpace>(), 1.);
100  stateSpace->addSubspace(std::make_shared<base::RealVectorStateSpace>(6), .3);
101  stateSpace->lock();
102  return stateSpace;
103 }
104 
105 void ompl::app::QuadrotorPlanning::setDefaultBounds()
106 {
107  base::RealVectorBounds velbounds(6), controlbounds(4);
108 
109  velbounds.setLow(-1);
110  velbounds.setHigh(1);
111  getStateSpace()->as<base::CompoundStateSpace>()->as<base::RealVectorStateSpace>(1)->setBounds(velbounds);
112  controlbounds.setLow(-1);
113  controlbounds.setHigh(1);
114  controlbounds.setLow(0, 5.);
115  controlbounds.setHigh(0, 15.);
116  getControlSpace()->as<control::RealVectorControlSpace>()->setBounds(controlbounds);
117 }
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
A shared pointer wrapper for ompl::base::StateSpace.
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:97
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot&#39;s center (average of all the vertices of all its parts)
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:73