Simbody  3.5
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_
const Vec< N > & getY() const
Return the complete current state as a Vec<N>.
Definition: GeodesicIntegrator.h:230
void initialize(Real t, const Vec< N > &y)
Call this once before taking a series of steps.
Definition: GeodesicIntegrator.h:150
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
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Real getActualInitialStepSizeTaken() const
Return the size of the first accepted step to be taken after the most recent initialize() call...
Definition: GeodesicIntegrator.h:198
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 > & getU() const
Return just the "velocity" variables u from the current state.
Definition: GeodesicIntegrator.h:234
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
#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
void setTimeAndState(Real t, const Vec< N > &y)
Set initial time and state prior to integrating.
Definition: GeodesicIntegrator.h:171
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:593
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
unsigned char square(unsigned char u)
Definition: Scalar.h:351
const Vec< N > & getYDot() const
Return the complete set of time derivatives of the current state.
Definition: GeodesicIntegrator.h:236
const Real & getTime() const
Return the current time.
Definition: GeodesicIntegrator.h:228
int getNumErrorTestFailures() const
How many steps were rejected because they did not satisfy the accuracy requirement, since the most recent initalize() call.
Definition: GeodesicIntegrator.h:211
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
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
Definition: GeodesicIntegrator.h:136
int getNumStepsAttempted() const
Return the total number of steps that were attempted since the most recent initialize() call...
Definition: GeodesicIntegrator.h:206
int getNumProjectionFailures() const
How many steps were rejected because the projectIfNeeded() method was unable to satisfy the constrain...
Definition: GeodesicIntegrator.h:215
float norm(const conjugate< float > &c)
Definition: conjugate.h:775
#define SimTK_ERRCHK3_ALWAYS(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:293
Definition: GeodesicIntegrator.h:136
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
This is the header file that every Simmath compilation unit should include first. ...
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:660
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
int getNumStepsTaken() const
Return the number of successful time steps taken since the most recent initialize() call...
Definition: GeodesicIntegrator.h:202
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
const Vec< NQ > & getQ() const
Return just the "position" variables q from the current state.
Definition: GeodesicIntegrator.h:232
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 getNumInitializations() const
Return the number of calls to initialize() since construction of this integrator object.
Definition: GeodesicIntegrator.h:219
Real getRequiredAccuracy() const
Return the accuracy requirement as set in the constructor.
Definition: GeodesicIntegrator.h:192
This is a stripped-down numerical integrator for small ODE or DAE problems whose size is known at com...
Definition: GeodesicIntegrator.h:134
Definition: GeodesicIntegrator.h:136
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, stride, and packing.
Definition: Vec.h:902
const Vec< NQ > & getUDot() const
Return just the derivatives udot of the "velocity" variables u.
Definition: GeodesicIntegrator.h:240
const Vec< NQ > & getQDot() const
Return just the derivatives qdot of the "position" variables q.
Definition: GeodesicIntegrator.h:238