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;
402 #endif // SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_ #define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
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 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
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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 > & getQDot() const
Return just the derivatives qdot of the "position" variables q.
Definition: GeodesicIntegrator.h:238
const Vec< NQ > & getUDot() const
Return just the derivatives udot of the "velocity" variables u.
Definition: GeodesicIntegrator.h:240
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
#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
Real getRequiredAccuracy() const
Return the accuracy requirement as set in the constructor.
Definition: GeodesicIntegrator.h:192
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
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:349
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.
int getNumInitializations() const
Return the number of calls to initialize() since construction of this integrator object.
Definition: GeodesicIntegrator.h:219
Definition: GeodesicIntegrator.h:136
#define SimTK_ERRCHK3_ALWAYS(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:293
Real getActualInitialStepSizeTaken() const
Return the size of the first accepted step to be taken after the most recent initialize() call...
Definition: GeodesicIntegrator.h:198
const Real & getTime() const
Return the current time.
Definition: GeodesicIntegrator.h:228
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
float norm(const conjugate< float > &c)
Definition: conjugate.h:486
int getNumErrorTestFailures() const
How many steps were rejected because they did not satisfy the accuracy requirement, since the most recent initialize() call.
Definition: GeodesicIntegrator.h:211
Definition: GeodesicIntegrator.h:136
const Vec< NQ > & getQ() const
Return just the "position" variables q from the current state.
Definition: GeodesicIntegrator.h:232
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:634
const Vec< NQ > & getU() const
Return just the "velocity" variables u from the current state.
Definition: GeodesicIntegrator.h:234
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
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
int getNumStepsTaken() const
Return the number of successful time steps taken since the most recent initialize() call...
Definition: GeodesicIntegrator.h:202
This is a stripped-down numerical integrator for small ODE or DAE problems whose size is known at com...
Definition: GeodesicIntegrator.h:134
int getNumProjectionFailures() const
How many steps were rejected because the projectIfNeeded() method was unable to satisfy the constrain...
Definition: GeodesicIntegrator.h:215
Real getConstraintTolerance() const
Return the constraint tolerance as set in the constructor.
Definition: GeodesicIntegrator.h:194
const Vec< N > & getY() const
Return the complete current state as a Vec<N>.
Definition: GeodesicIntegrator.h:230
Definition: GeodesicIntegrator.h:136
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:904