KoulesSimulator.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Beck Chen, Mark Moll */
36 
37 #include <ompl/control/SpaceInformation.h>
38 #include "KoulesStateSpace.h"
39 #include "KoulesControlSpace.h"
40 #include "KoulesSimulator.h"
41 
42 namespace ob = ompl::base;
43 namespace oc = ompl::control;
44 
45 namespace
46 {
47  inline double normalizeAngle(double theta)
48  {
49  if (theta < -boost::math::constants::pi<double>())
50  return theta + 2. * boost::math::constants::pi<double>();
51  if (theta > boost::math::constants::pi<double>())
52  return theta - 2. * boost::math::constants::pi<double>();
53  return theta;
54  }
55 
56  inline void normalize2(double* v)
57  {
58  double nrm = std::sqrt(v[0] * v[0] + v[1] * v[1]);
59  if (nrm > std::numeric_limits<double>::epsilon())
60  {
61  v[0] /= nrm;
62  v[1] /= nrm;
63  }
64  else
65  {
66  v[0] = 1.;
67  v[1] = 0.;
68  }
69  }
70  inline double dot2(const double* v, const double* w)
71  {
72  return v[0] * w[0] + v[1] * w[1];
73  }
74  inline void vadd2(double* v, double s, const double* w)
75  {
76  v[0] += s * w[0];
77  v[1] += s * w[1];
78  }
79  inline unsigned int ind(unsigned int i)
80  {
81  return i != 0u ? 4 * i + 1 : 0;
82  }
83  inline void ode(const double* y, double* dydx)
84  {
85  dydx[0] = y[2];
86  dydx[1] = y[3];
87  dydx[2] = (.5 * sideLength - y[0]) * lambda_c - y[2] * h;
88  dydx[3] = (.5 * sideLength - y[1]) * lambda_c - y[3] * h;
89  }
90  inline void rungeKutta4(double* y, double h, double* yout)
91  {
92  double hh = h * .5, h6 = h / 6.;
93  static double yt[4], dydx[4], dym[4], dyt[4];
94  ode(y, dydx);
95  for (unsigned int i = 0; i < 4; ++i)
96  yt[i] = y[i] + hh * dydx[i];
97  ode(yt, dyt);
98  for (unsigned int i = 0; i < 4; ++i)
99  yt[i] = y[i] + hh * dyt[i];
100  ode(yt, dym);
101  for (unsigned int i = 0; i < 4; ++i)
102  {
103  yt[i] = y[i] + h * dym[i];
104  dym[i] += dyt[i];
105  }
106  ode(yt, dyt);
107  for (unsigned int i = 0; i < 4; ++i)
108  yout[i] = y[i] + h6 * (dydx[i] + dyt[i] + 2. * dym[i]);
109  }
110 
111  const float eps = std::numeric_limits<float>::epsilon();
112 }
113 
114 KoulesSimulator::KoulesSimulator(const ompl::control::SpaceInformation* si) :
115  si_(si),
116  numDimensions_(si->getStateSpace()->getDimension()),
117  numKoules_((numDimensions_ - 5) / 4),
118  qcur_(numDimensions_),
119  qnext_(numDimensions_),
120  dead_(numKoules_ + 1)
121 {
122 }
123 
124 void KoulesSimulator::updateShip(const oc::Control* control, double t)
125 {
126  const double* cval = control->as<KoulesControlSpace::ControlType>()->values;
127  double v[2] = { cval[0] - qcur_[2], cval[1] - qcur_[3] };
128  double deltaTheta = normalizeAngle(atan2(v[1], v[0]) - qcur_[4]);
129  double accel = 0., omega = 0.;
130 
131  if (std::abs(deltaTheta) < shipEps)
132  {
133  if (v[0] * v[0] + v[1] * v[1] > shipDelta * shipDelta)
134  accel = shipAcceleration;
135  }
136  else
137  omega = deltaTheta > 0. ? shipRotVel : -shipRotVel;
138 
139  if (accel == 0.)
140  {
141  qnext_[0] = qcur_[0] + qcur_[2] * t;
142  qnext_[1] = qcur_[1] + qcur_[3] * t;
143  qnext_[2] = qcur_[2];
144  qnext_[3] = qcur_[3];
145  qcur_[4] = normalizeAngle(qcur_[4] + omega * t);
146  }
147  else // omega == 0.
148  {
149  double ax = accel * cos(qcur_[4]);
150  double ay = accel * sin(qcur_[4]);
151  double t2 = .5 * t * t;
152  qnext_[0] = qcur_[0] + qcur_[2] * t + ax * t2;
153  qnext_[1] = qcur_[1] + qcur_[3] * t + ay * t2;
154  qnext_[2] = qcur_[2] + ax * t;
155  qnext_[3] = qcur_[3] + ay * t;
156  }
157 }
158 
159 void KoulesSimulator::initCollisionEvents()
160 {
161  double r[2], d, bad, ri, rij;
162  unsigned int ii, jj;
163  collisionEvents_ = CollisionEventQueue();
164  for (unsigned int i = 0; i < numKoules_; ++i)
165  if (!dead_[i])
166  {
167  ii = ind(i);
168  ri = si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(i);
169  for (unsigned int j = i + 1; j <= numKoules_; ++j)
170  if (!dead_[j])
171  {
172  jj = ind(j);
173  rij = ri + si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(j);
174  r[0] = qcur_[jj ] - qcur_[ii ];
175  r[1] = qcur_[jj + 1] - qcur_[ii + 1];
176  d = r[0] * r[0] + r[1] * r[1];
177  if (d < rij * rij)
178  {
179  d = std::sqrt(d);
180  bad = rij - d;
181  r[0] /= d;
182  r[0] *= bad * (1. + eps) * .5;
183  r[1] /= d;
184  r[1] *= bad * (1. + eps) * .5;
185  qcur_[ii ] -= r[0];
186  qcur_[ii + 1] -= r[1];
187  qcur_[jj ] += r[0];
188  qcur_[jj + 1] += r[1];
189  }
190  }
191  }
192  for (unsigned int i = 0; i <= numKoules_; ++i)
193  for (unsigned int j = i + 1; j <= numKoules_ + 2; ++j)
194  computeCollisionEvent(i, j);
195 }
196 
197 double KoulesSimulator::wallCollideEvent(unsigned int i, int dim)
198 {
199  double r = si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(i);
200  unsigned int ii = ind(i);
201  if (qcur_[ii + 2 + dim] > 0.)
202  return std::max(0., (sideLength - r - qcur_[ii + dim]) / qcur_[ii + 2 + dim]);
203  if (qcur_[ii + 2 + dim] < 0.)
204  return std::max(0., (r - qcur_[ii + dim]) / qcur_[ii + 2 + dim]);
205  else
206  return -1.;
207 }
208 
209 void KoulesSimulator::computeCollisionEvent(unsigned int i, unsigned int j)
210 {
211  if (dead_[i] || (j <= numKoules_ && dead_[j]))
212  return;
213 
214  double ri = si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(i);
215  double t;
216 
217  if (j == numKoules_ + 1)
218  t = wallCollideEvent(i, 0);
219  else if (j == numKoules_ + 2)
220  t = wallCollideEvent(i, 1);
221  else
222  {
223  unsigned int ii = ind(i), jj = ind(j);
224  double u[2] = { qcur_[ii + 2] - qcur_[jj + 2], qcur_[ii + 3] - qcur_[jj + 3] };
225  double v[2] = { qcur_[ii ] - qcur_[jj ], qcur_[ii + 1] - qcur_[jj + 1] };
226  double a = u[0] * u[0] + u[1] * u[1];
227  if (a == 0.)
228  t = -1.;
229  else
230  {
231  double r = ri + si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(j);
232  double b = 2 * u[0] * v[0] + 2 * u[1] * v[1];
233  double c = v[0] * v[0] + v[1] * v[1] - r * r;
234  double disc = b * b - 4. * a * c;
235  if (std::abs(disc) < std::numeric_limits<float>::epsilon())
236  t = -.5 * b / a;
237  else if (disc < 0.)
238  t = -1.;
239  else
240  {
241  disc = std::sqrt(disc);
242  t = (-b - disc) / (2. * a);
243  if (t < 0.)
244  t = (-b + disc) / (2. * a);
245  }
246  }
247  }
248  t += time_;
249  if (t >= time_ && t <= endTime_)
250  collisionEvents_.emplace(t, i, j);
251 }
252 
253 void KoulesSimulator::elasticCollision(unsigned int i, unsigned int j)
254 {
255  double *a = &qcur_[ind(i)], *b = &qcur_[ind(j)];
256  double d[2] = { b[0] - a[0], b[1] - a[1] };
257  normalize2(d);
258  double va = dot2(a + 2, d), vb = dot2(b + 2, d);
259  if (va - vb <= 0.)
260  return;
261  double ma = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(i);
262  double mb = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(j);
263  double vap = (va * (ma - mb) + 2. * mb * vb) / (ma + mb);
264  double vbp = (vb * (mb - ma) + 2. * ma * va) / (ma + mb);
265  double amag = vap - va, bmag = vbp - vb;
266  if (std::abs(amag) < eps)
267  amag = amag < 0. ? -eps : eps;
268  if (std::abs(bmag) < eps)
269  bmag = bmag < 0. ? -eps : eps;
270 #ifndef NDEBUG
271  double px = ma * a[2] + mb * b[2], py = ma * a[3] + mb * b[3];
272  double k = ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]);
273 #endif
274  vadd2(a + 2, amag, d);
275  vadd2(b + 2, bmag, d);
276 
277 #ifndef NDEBUG
278  // preservation of momemtum
279  assert(std::abs(ma * a[2] + mb * b[2] - px) < 1e-6);
280  assert(std::abs(ma * a[3] + mb * b[3] - py) < 1e-6);
281  // preservation of kinetic energy
282  assert(std::abs(ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]) - k) < 1e-6);
283 #endif
284 }
285 
286 void KoulesSimulator::advance(double t)
287 {
288  double dt = t - time_;
289  qcur_[0] += qcur_[2] * dt;
290  qcur_[1] += qcur_[3] * dt;
291  for (unsigned int i = 5; i < numDimensions_; i += 4)
292  {
293  qcur_[i ] += qcur_[i + 2] * dt;
294  qcur_[i + 1] += qcur_[i + 3] * dt;
295  }
296  time_ = t;
297 }
298 
299 void KoulesSimulator::markAsDead(unsigned int i)
300 {
301  unsigned int ii = ind(i);
302  qcur_[ii] = -2. * kouleRadius;
303  qcur_[ii + 1] = qcur_[ii + 2] = qcur_[ii + 3] = 0.;
304 }
305 
306 void KoulesSimulator::step(const ob::State *start, const oc::Control* control,
307  const double t, ob::State *result)
308 {
309  unsigned int ii;
310 
311  memcpy(&qcur_[0], start->as<KoulesStateSpace::StateType>()->values, numDimensions_ * sizeof(double));
312  time_ = 0.;
313  endTime_ = t;
314  std::fill(dead_.begin(), dead_.end(), false);
315  updateShip(control, t);
316  for (unsigned int i = 0; i <= numKoules_; ++i)
317  {
318  ii = ind(i);
319  dead_[i] = qcur_[ii] == -2. * kouleRadius;
320  if (!dead_[i])
321  {
322  if (i != 0u)
323  rungeKutta4(&qcur_[ii], t, &qnext_[ii]);
324  qcur_[ii + 2] = (qnext_[ii ] - qcur_[ii ]) / t;
325  qcur_[ii + 3] = (qnext_[ii + 1] - qcur_[ii + 1]) / t;
326  }
327  }
328  initCollisionEvents();
329  while (!collisionEvents_.empty())
330  {
331  CollisionEvent event = collisionEvents_.top();
332  double ct = std::get<0>(event);
333  unsigned int i = std::get<1>(event), j = std::get<2>(event);
334 
335  collisionEvents_.pop();
336  advance(ct);
337  if (j <= numKoules_)
338  elasticCollision(i, j);
339  else
340  {
341  markAsDead(i);
342  if (i == 0)
343  {
344  memcpy(result->as<KoulesStateSpace::StateType>()->values, &qcur_[0], numDimensions_ * sizeof(double));
345  return;
346  }
347  }
348 
349  for (unsigned int k = 0; k <= numKoules_ + 2; ++k)
350  {
351  if (k < i)
352  computeCollisionEvent(k, i);
353  else if (k > i && k != j)
354  computeCollisionEvent(i, k);
355  if (k < j && k != i)
356  computeCollisionEvent(k, j);
357  else if (k > j)
358  computeCollisionEvent(j, k);
359  }
360  }
361  advance(t);
362  memcpy(result->as<KoulesStateSpace::StateType>()->values, &qcur_[0], numDimensions_ * sizeof(double));
363 }
ompl::control::Control
Definition of an abstract control.
Definition: Control.h:111
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: ConstrainedSpaceInformation.h:86
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::control
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:76
ompl::control::SpaceInformation
Space information containing necessary information for planning with controls. setup() needs to be ca...
Definition: SpaceInformation.h:134
ompl::base::SpaceInformation::getStateSpace
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Definition: SpaceInformation.h:164
ompl::control::Control::as
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:160