Loading...
Searching...
No Matches
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
42namespace ob = ompl::base;
43namespace oc = ompl::control;
44
45namespace
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
114KoulesSimulator::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
124void 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
159void 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
197double 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
209void 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
253void 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
286void 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
299void 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
306void 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}
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
ompl::control::Control ControlType
Define the type of control allocated by this control space.
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