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 }
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Definition of an abstract control.
Definition: Control.h:48
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
Space information containing necessary information for planning with controls. setup() needs to be ca...
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:45