Simbody 3.7
Loading...
Searching...
No Matches
GeodesicIntegrator.h
Go to the documentation of this file.
1#ifndef SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
2#define SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
3
4/* -------------------------------------------------------------------------- *
5 * Simbody(tm): SimTKmath *
6 * -------------------------------------------------------------------------- *
7 * This is part of the SimTK biosimulation toolkit originating from *
8 * Simbios, the NIH National Center for Physics-Based Simulation of *
9 * Biological Structures at Stanford, funded under the NIH Roadmap for *
10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11 * *
12 * Portions copyright (c) 2012 Stanford University and the Authors. *
13 * Authors: Michael Sherman *
14 * Contributors: *
15 * *
16 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17 * not use this file except in compliance with the License. You may obtain a *
18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19 * *
20 * Unless required by applicable law or agreed to in writing, software *
21 * distributed under the License is distributed on an "AS IS" BASIS, *
22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23 * See the License for the specific language governing permissions and *
24 * limitations under the License. *
25 * -------------------------------------------------------------------------- */
26
33#include "SimTKcommon.h"
35
36namespace SimTK {
37
133template <class Eqn>
135public:
136 enum { NQ = Eqn::NQ, NC = Eqn::NC, N = 2*NQ };
137
141 GeodesicIntegrator(const Eqn& eqn, Real accuracy, Real constraintTol)
142 : m_eqn(eqn), m_accuracy(accuracy), m_consTol(constraintTol),
143 m_hInit(NaN), m_hLast(NaN), m_hNext(Real(0.1)), m_nInitialize(0)
144 { reinitializeCounters(); }
145
150 void initialize(Real t, const Vec<N>& y) {
151 ++m_nInitialize;
152 reinitializeCounters();
153 m_hInit = m_hLast = NaN;
154 m_hNext = Real(0.1); // override if you have a better idea
155 m_t = t; m_y = y;
156 if (!m_eqn.projectIfNeeded(m_consTol, m_t, m_y)) {
157 Vec<NC> cerr;
158 m_eqn.calcConstraintErrors(m_t, m_y, cerr);
159 const Real consErr = calcNormInf(cerr);
160 SimTK_ERRCHK2_ALWAYS(!"projection failed",
161 "GeodesicIntegrator::initialize()",
162 "Couldn't project constraints to tol=%g;"
163 " largest error was %g.", m_consTol, consErr);
164 }
165 m_eqn.calcDerivs(m_t, m_y, m_ydot);
166 }
167
171 void setTimeAndState(Real t, const Vec<N>& y) {
172 m_t = t; m_y = y;
173 m_eqn.calcDerivs(m_t, m_y, m_ydot);
174 Vec<NC> cerr;
175 m_eqn.calcConstraintErrors(m_t, m_y, cerr);
176 const Real consErr = calcNormInf(cerr);
177 if (consErr > m_consTol)
178 SimTK_ERRCHK2_ALWAYS(!"constraints not satisfied",
179 "GeodesicIntegrator::setTimeAndState()",
180 "Supplied state failed to satisfy constraints to tol=%g;"
181 " largest error was %g.", m_consTol, consErr);
182 }
183
186 void setNextStepSizeToTry(Real h) {m_hNext=h;}
189 Real getNextStepSizeToTry() const {return m_hNext;}
190
192 Real getRequiredAccuracy() const {return m_accuracy;}
194 Real getConstraintTolerance() const {return m_consTol;}
195
198 Real getActualInitialStepSizeTaken() const {return m_hInit;}
199
202 int getNumStepsTaken() const {return m_nStepsTaken;}
206 int getNumStepsAttempted() const {return m_nStepsAttempted;}
211 int getNumErrorTestFailures() const {return m_nErrtestFailures;}
215 int getNumProjectionFailures() const {return m_nProjectionFailures;}
216
219 int getNumInitializations() const {return m_nInitialize;}
220
225 void takeOneStep(Real tStop);
226
228 const Real& getTime() const {return m_t;}
230 const Vec<N>& getY() const {return m_y;}
232 const Vec<NQ>& getQ() const {return Vec<NQ>::getAs(&m_y[0]);}
234 const Vec<NQ>& getU() const {return Vec<NQ>::getAs(&m_y[NQ]);}
236 const Vec<N>& getYDot() const {return m_ydot;}
238 const Vec<NQ>& getQDot() const {return Vec<NQ>::getAs(&m_ydot[0]);}
240 const Vec<NQ>& getUDot() const {return Vec<NQ>::getAs(&m_ydot[NQ]);}
241
244 template <int Z> static Real calcNormInf(const Vec<Z>& v) {
245 Real norm = 0;
246 for (int i=0; i < Z; ++i) {
247 Real aval = std::abs(v[i]);
248 if (aval > norm) norm = aval;
249 }
250 return norm;
251 }
252
255 template <int Z> static Real calcNormRMS(const Vec<Z>& v) {
256 Real norm = 0;
257 for (int i=0; i< Z; ++i)
258 norm += square(v[i]);
259 return std::sqrt(norm/Z);
260 }
261
262private:
263 void takeRKMStep(Real h, Vec<N>& y1, Vec<N>& y1err) const;
264
265 const Eqn& m_eqn; // The DAE system to be solved.
266 Real m_accuracy; // Absolute accuracy requirement for y.
267 Real m_consTol; // Absolute tolerance for constraint errors.
268
269 Real m_t; // Current value of the independent variable.
270 Vec<N> m_y; // Current q,u in that order.
271
272 Real m_hInit; // Actual initial step taken.
273 Real m_hLast; // Last step taken.
274 Real m_hNext; // max step size to try next
275
276 Vec<N> m_ydot; // ydot(t,y)
277
278 // Counters.
279 int m_nInitialize; // zeroed on construction only
280 void reinitializeCounters() {
281 m_nStepsTaken=m_nStepsAttempted=0;
282 m_nErrtestFailures=m_nProjectionFailures=0;
283 }
284 int m_nStepsTaken;
285 int m_nStepsAttempted;
286 int m_nErrtestFailures;
287 int m_nProjectionFailures;
288};
289
290template <class Eqn> void
292 const Real Safety = Real(0.9), MinShrink = Real(0.1), MaxGrow = Real(5);
293 const Real HysteresisLow = Real(0.9), HysteresisHigh = Real(1.2);
294 const Real MaxStretch = Real(0.1);
295 const Real hMin = m_t <= 1 ? SignificantReal : SignificantReal*m_t;
296 const Real hStretch = MaxStretch*m_hNext;
297
298 // Figure out the target ending time for the next step. Choosing time
299 // rather than step size lets us end at exactly tStop.
300 Real t1 = m_t + m_hNext; // this is the usual case
301 // If a small stretching of the next step would get us to tStop, try
302 // to make it all the way in one step.
303 if (t1 + hStretch > tStop)
304 t1 = tStop;
305
306 Real h, errNorm;
307 Vec<N> y1, y1err;
308
309 // Try smaller and smaller step sizes if necessary until we get one
310 // that satisfies the error requirement, and for which projection
311 // succeeds.
312 while (true) {
313 ++m_nStepsAttempted;
314 h = t1 - m_t; assert(h>0);
315 takeRKMStep(h, y1, y1err);
316 errNorm = calcNormInf(y1err);
317 if (errNorm > m_accuracy) {
318 ++m_nErrtestFailures;
319 // Failed to achieve required accuracy at this step size h.
320 SimTK_ERRCHK4_ALWAYS(h > hMin,
321 "GeodesicIntegrator::takeOneStep()",
322 "Accuracy %g worse than required %g at t=%g with step size"
323 " h=%g; can't go any smaller.", errNorm, m_accuracy, m_t, h);
324
325 // Shrink step by (acc/err)^(1/4) for 4th order.
326 Real hNew = Safety * h * std::sqrt(std::sqrt(m_accuracy/errNorm));
327 hNew = clamp(MinShrink*h, hNew, HysteresisLow*h);
328 t1 = m_t + hNew;
329 continue;
330 }
331 // Accuracy achieved. Can we satisfy the constraints?
332 if (m_eqn.projectIfNeeded(m_consTol, t1, y1))
333 break; // all good
334
335 // Constraint projection failed. Hopefully that's because the
336 // step was too big.
337 ++m_nProjectionFailures;
338
339 SimTK_ERRCHK3_ALWAYS(h > hMin,
340 "GeodesicIntegrator::takeOneStep()",
341 "Projection failed to reach constraint tolerance %g at t=%g "
342 "with step size h=%g; can't shrink step further.",
343 m_consTol, m_t, h);
344
345 const Real hNew = MinShrink*h;
346 t1 = m_t + hNew;
347 }
348
349 // We achieved desired accuracy at step size h, and satisfied the
350 // constraints. (t1,y1) is now the final time and state; calculate
351 // state derivatives which will be used at the start of the next step.
352 ++m_nStepsTaken;
353 if (m_nStepsTaken==1) m_hInit = h; // that was the initial step
354 m_t = t1; m_y = y1; m_hLast = h;
355 m_eqn.calcDerivs(m_t, m_y, m_ydot);
356
357 // If the step we just took ended right at tStop, don't use it to
358 // predict a new step size; instead we'll just use the same hNext we
359 // would have used here if it weren't for tStop.
360 if (t1 < tStop) {
361 // Possibly grow step for next time.
362 Real hNew = errNorm == 0 ? MaxGrow*h
363 : Safety * h * std::sqrt(std::sqrt(m_accuracy/errNorm));
364 if (hNew < HysteresisHigh*h) hNew = h; // don't bother
365 hNew = std::min(hNew, MaxGrow*h);
366 m_hNext = hNew;
367 }
368}
369
370template <class Eqn> void
372 const Real h2=h/2, h3=h/3, h6=h/6, h8=h/8;
373 const Real t0=m_t, t1=m_t+h;
374 const Vec<N>& y0 = m_y;
375 const Vec<N>& f0 = m_ydot;
376 Vec<N> f1, f2, f3, f4, ysave;
377 m_eqn.calcDerivs(t0+h3, y0 + h3* f0, f1);
378 m_eqn.calcDerivs(t0+h3, y0 + h6*(f0 + f1), f2);
379 m_eqn.calcDerivs(t0+h2, y0 + h8*(f0 + 3*f2), f3);
380
381 // We'll need this for error estimation.
382 ysave = y0 + h2*(f0 - 3*f2 + 4*f3);
383 m_eqn.calcDerivs(t1, ysave, f4);
384
385 // Final value. This is the 4th order accurate estimate for
386 // y1=y(t0+h)+O(h^5): y1 = y0 + (h/6)*(f0 + 4 f3 + f4).
387 // Don't evaluate here yet because we might reject this step or we
388 // might need to do a projection.
389 y1 = y0 + h6*(f0 + 4*f3 + f4);
390
391 // This is an embedded 3rd-order estimate y1hat=y(t0+h)+O(h^4).
392 // y1hat = y0 + (h/10)*(f0 + 3 f2 + 4 f3 + 2 f4)
393 // We don't actually have any need for y1hat, just its 4th-order
394 // error estimate y1hat-y1=(1/5)(y1-ysave) (easily verified from the above).
395
396 for (int i=0; i<N; ++i)
397 y1err[i] = std::abs(y1[i]-ysave[i]) / 5;
398}
399
400} // namespace SimTK
401
402#endif // SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
#define SimTK_ERRCHK3_ALWAYS(cond, whereChecked, fmt, a1, a2, a3)
Definition ExceptionMacros.h:293
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition ExceptionMacros.h:289
#define SimTK_ERRCHK4_ALWAYS(cond, whereChecked, fmt, a1, a2, a3, a4)
Definition ExceptionMacros.h:297
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
This is a stripped-down numerical integrator for small ODE or DAE problems whose size is known at com...
Definition GeodesicIntegrator.h:134
const Vec< NQ > & getQ() const
Return just the "position" variables q from the current state.
Definition GeodesicIntegrator.h:232
const Real & getTime() const
Return the current time.
Definition GeodesicIntegrator.h:228
const Vec< NQ > & getUDot() const
Return just the derivatives udot of the "velocity" variables u.
Definition GeodesicIntegrator.h:240
GeodesicIntegrator(const Eqn &eqn, Real accuracy, Real constraintTol)
Construct an integrator for the given set of equations eqn, which are to be solved to the given accur...
Definition GeodesicIntegrator.h:141
int getNumStepsTaken() const
Return the number of successful time steps taken since the most recent initialize() call.
Definition GeodesicIntegrator.h:202
Real getRequiredAccuracy() const
Return the accuracy requirement as set in the constructor.
Definition GeodesicIntegrator.h:192
const Vec< NQ > & getQDot() const
Return just the derivatives qdot of the "position" variables q.
Definition GeodesicIntegrator.h:238
const Vec< N > & getYDot() const
Return the complete set of time derivatives of the current state.
Definition GeodesicIntegrator.h:236
void initialize(Real t, const Vec< N > &y)
Call this once before taking a series of steps.
Definition GeodesicIntegrator.h:150
void setNextStepSizeToTry(Real h)
Use this if you think you know a better initial step size to try than the default.
Definition GeodesicIntegrator.h:186
static Real calcNormInf(const Vec< Z > &v)
This is a utility routine that returns the infinity norm (maximum absolute value) contained in a fixe...
Definition GeodesicIntegrator.h:244
static Real calcNormRMS(const Vec< Z > &v)
This is a utility routine that returns the RMS norm of a fixed-size, scalar Vec.
Definition GeodesicIntegrator.h:255
void takeOneStep(Real tStop)
Advance time and state by one error-controlled step and return, but in no case advance past t=tStop.
Definition GeodesicIntegrator.h:291
int getNumStepsAttempted() const
Return the total number of steps that were attempted since the most recent initialize() call.
Definition GeodesicIntegrator.h:206
const Vec< N > & getY() const
Return the complete current state as a Vec<N>.
Definition GeodesicIntegrator.h:230
Real getActualInitialStepSizeTaken() const
Return the size of the first accepted step to be taken after the most recent initialize() call.
Definition GeodesicIntegrator.h:198
void setTimeAndState(Real t, const Vec< N > &y)
Set initial time and state prior to integrating.
Definition GeodesicIntegrator.h:171
Real getNextStepSizeToTry() const
Return the size of the next time step the integrator will attempt on the next call to takeOneStep().
Definition GeodesicIntegrator.h:189
int getNumErrorTestFailures() const
How many steps were rejected because they did not satisfy the accuracy requirement,...
Definition GeodesicIntegrator.h:211
@ N
Definition GeodesicIntegrator.h:136
@ NC
Definition GeodesicIntegrator.h:136
@ NQ
Definition GeodesicIntegrator.h:136
int getNumInitializations() const
Return the number of calls to initialize() since construction of this integrator object.
Definition GeodesicIntegrator.h:219
int getNumProjectionFailures() const
How many steps were rejected because the projectIfNeeded() method was unable to satisfy the constrain...
Definition GeodesicIntegrator.h:215
const Vec< NQ > & getU() const
Return just the "velocity" variables u from the current state.
Definition GeodesicIntegrator.h:234
Real getConstraintTolerance() const
Return the constraint tolerance as set in the constructor.
Definition GeodesicIntegrator.h:194
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
static const Vec & getAs(const ELT *p)
Recast an ordinary C++ array E[] to a const Vec<M,E,S>; assumes compatible length,...
Definition Vec.h:904
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
float norm(const conjugate< float > &c)
Definition conjugate.h:486
unsigned char square(unsigned char u)
Definition Scalar.h:349
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition SimTKcommon/include/SimTKcommon/internal/common.h:606
double clamp(double low, double v, double high)
If v is in range low <= v <= high then return v, otherwise return the nearest bound; this function do...
Definition Scalar.h:634