1 #ifndef SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
2 #define SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
136 enum {
NQ = Eqn::NQ,
NC = Eqn::NC,
N = 2*
NQ };
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(); }
152 reinitializeCounters();
153 m_hInit = m_hLast =
NaN;
156 if (!m_eqn.projectIfNeeded(m_consTol, m_t, m_y)) {
158 m_eqn.calcConstraintErrors(m_t, m_y, cerr);
161 "GeodesicIntegrator::initialize()",
162 "Couldn't project constraints to tol=%g;"
163 " largest error was %g.", m_consTol, consErr);
165 m_eqn.calcDerivs(m_t, m_y, m_ydot);
173 m_eqn.calcDerivs(m_t, m_y, m_ydot);
175 m_eqn.calcConstraintErrors(m_t, m_y, cerr);
177 if (consErr > m_consTol)
179 "GeodesicIntegrator::setTimeAndState()",
180 "Supplied state failed to satisfy constraints to tol=%g;"
181 " largest error was %g.", m_consTol, consErr);
246 for (
int i=0; i < Z; ++i) {
257 for (
int i=0; i< Z; ++i)
259 return std::sqrt(
norm/Z);
280 void reinitializeCounters() {
281 m_nStepsTaken=m_nStepsAttempted=0;
282 m_nErrtestFailures=m_nProjectionFailures=0;
285 int m_nStepsAttempted;
286 int m_nErrtestFailures;
287 int m_nProjectionFailures;
290 template <
class Eqn>
void
293 const Real HysteresisLow =
Real(0.9), HysteresisHigh =
Real(1.2);
296 const Real hStretch = MaxStretch*m_hNext;
300 Real t1 = m_t + m_hNext;
303 if (t1 + hStretch > tStop)
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;
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);
326 Real hNew = Safety * h * std::sqrt(std::sqrt(m_accuracy/errNorm));
327 hNew =
clamp(MinShrink*h, hNew, HysteresisLow*h);
332 if (m_eqn.projectIfNeeded(m_consTol, t1, y1))
337 ++m_nProjectionFailures;
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.",
345 const Real hNew = MinShrink*h;
353 if (m_nStepsTaken==1) m_hInit = h;
354 m_t = t1; m_y = y1; m_hLast = h;
355 m_eqn.calcDerivs(m_t, m_y, m_ydot);
362 Real hNew = errNorm == 0 ? MaxGrow*h
363 : Safety * h * std::sqrt(std::sqrt(m_accuracy/errNorm));
364 if (hNew < HysteresisHigh*h) hNew = h;
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;
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);
382 ysave = y0 + h2*(f0 - 3*f2 + 4*f3);
383 m_eqn.calcDerivs(t1, ysave, f4);
389 y1 = y0 + h6*(f0 + 4*f3 + f4);
396 for (
int i=0; i<N; ++i)
397 y1err[i] =
std::abs(y1[i]-ysave[i]) / 5;
#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