Simbody  3.8
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 
36 namespace SimTK {
37 
133 template <class Eqn>
135 public:
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 
262 private:
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 
290 template <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 
370 template <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< N > & getYDot() const
Return the complete set of time derivatives of the current state.
Definition: GeodesicIntegrator.h:236
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
const Vec< NQ > & getQ() const
Return just the "position" variables q from the current state.
Definition: GeodesicIntegrator.h:232
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
void initialize(Real t, const Vec< N > &y)
Call this once before taking a series of steps.
Definition: GeodesicIntegrator.h:150
const Vec< N > & getY() const
Return the complete current state as a Vec<N>.
Definition: GeodesicIntegrator.h:230
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
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
const Vec< NQ > & getUDot() const
Return just the derivatives udot of the "velocity" variables u.
Definition: GeodesicIntegrator.h:240
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
const Real & getTime() const
Return the current time.
Definition: GeodesicIntegrator.h:228
const Vec< NQ > & getU() const
Return just the "velocity" variables u from the current state.
Definition: GeodesicIntegrator.h:234
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 > & getQDot() const
Return just the derivatives qdot of the "position" variables q.
Definition: GeodesicIntegrator.h:238
Real getConstraintTolerance() const
Return the constraint tolerance as set in the constructor.
Definition: GeodesicIntegrator.h:194
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
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
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:607
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