This is a stripped-down numerical integrator for small ODE or DAE problems whose size is known at compile time, with no provision for discrete variables, event detection, or interpolation. More...

#include <GeodesicIntegrator.h>

Public Types
enum	{ NQ = Eqn::NQ, NC = Eqn::NC, N = 2*NQ }
Public Member Functions
	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 accuracy, with constraints maintained to within the given constraintTol.
void	initialize (Real t, const Vec< N > &y)
	Call this once before taking a series of steps.
void	setTimeAndState (Real t, const Vec< N > &y)
	Set initial time and state prior to integrating.
void	setNextStepSizeToTry (Real h)
	Use this if you think you know a better initial step size to try than the default.
Real	getNextStepSizeToTry () const
	Return the size of the next time step the integrator will attempt on the next call to takeOneStep().
Real	getRequiredAccuracy () const
	Return the accuracy requirement as set in the constructor.
Real	getConstraintTolerance () const
	Return the constraint tolerance as set in the constructor.
Real	getActualInitialStepSizeTaken () const
	Return the size of the first accepted step to be taken after the most recent initialize() call.
int	getNumStepsTaken () const
	Return the number of successful time steps taken since the most recent initialize() call.
int	getNumStepsAttempted () const
	Return the total number of steps that were attempted since the most recent initialize() call.
int	getNumErrorTestFailures () const
	How many steps were rejected because they did not satisfy the accuracy requirement, since the most recent initalize() call.
int	getNumProjectionFailures () const
	How many steps were rejected because the projectIfNeeded() method was unable to satisfy the constraint tolerance (since the most recent initialize() call).
int	getNumInitializations () const
	Return the number of calls to initialize() since construction of this integrator object.
void	takeOneStep (Real tStop)
	Advance time and state by one error-controlled step and return, but in no case advance past t=tStop.
const Real &	getTime () const
	Return the current time.
const Vec< N > &	getY () const
	Return the complete current state as a Vec<N>.
const Vec< NQ > &	getQ () const
	Return just the "position" variables q from the current state.
const Vec< NQ > &	getU () const
	Return just the "velocity" variables u from the current state.
const Vec< N > &	getYDot () const
	Return the complete set of time derivatives of the current state.
const Vec< NQ > &	getQDot () const
	Return just the derivatives qdot of the "position" variables q.
const Vec< NQ > &	getUDot () const
	Return just the derivatives udot of the "velocity" variables u.
Static Public Member Functions
template<int Z>
static Real	calcNormInf (const Vec< Z > &v)
	This is a utility routine that returns the infinity norm (maximum absolute value) contained in a fixed-size, scalar Vec.
template<int Z>
static Real	calcNormRMS (const Vec< Z > &v)
	This is a utility routine that returns the RMS norm of a fixed-size, scalar Vec.

Detailed Description

template<class Eqn>
class SimTK::GeodesicIntegrator< Eqn >

This is a stripped-down numerical integrator for small ODE or DAE problems whose size is known at compile time, with no provision for discrete variables, event detection, or interpolation.

You cannot use this integrator to advance a Simbody System; see Integrator instead. Everything is defined in this header file so that the integration can proceed with virtually no overhead. Templates are used rather than run-time polymorphism, so there are no virtual function calls. The system of equations is given as a template object that must implement particular methods which the compiler may inline if they are simple enough.

Class Equations

This integrator is instantiated with a class that encapsulates the system of equations to be solved, and must provide compile time constants and methods with the following signatures:

class MyEquations {
public:
    enum { NQ=5,       // number of 2nd order equations
           NC=3,       // total number of position & velocity constraints
           N = 2*NQ }; // total number of states y

    // Calculate state derivatives ydot given time and state y.
    void calcDerivs(Real t, const Vec<N>& y, Vec<N>& ydot) const;

    // Calculate amount by which the given time and state y violate the
    // constraints and return the constraint errors in cerr.
    void calcConstraintErrors(Real t, const Vec<N>& y, Vec<NC>& cerr) const;

    // Given a time and state y, ensure that the state satisfies the constraints
    // to within the indicated absolute tolerance, by performing the shortest
    // (i.e. least squares) projection of the state back to the constraint
    // manifold. Return false if the desired tolerance cannot be achieved. 
    // Otherwise (true return), a subsequent call to calcConstraintErrors() 
    // would return each |cerr[i]|<=consTol.
    bool projectIfNeeded(Real consTol, Real t, Vec<N>& y) const;
};

Usage

// Create an object of a type that satisfies the Equations description above.
MyEquations eqns(...); // ... is whatever arguments you require.
// Instantiate an integrator for this system of equations and specify the
// integration accuracy and constraint tolerance.
GeodesicIntegrator<MyEquations> integ(eqns, accuracy, constraintTol);
// Initialize; will project constraints if necessary.
integ.initialize(t0, y0);
// Integrate to time finalTime, getting output every completed step.
while (true) {
    std::cout << "t=" << integ.getTime() << " y=" << integ.getY() << "\n";
    if (integ.getTime() == finalTime)
        break;
    integ.takeOneStep(finalTime);
}

Mathematical Overview

This is an explicit, variable-step integrator solving a 2nd-order DAE structured as an ODE-on-a-manifold system[1] like this:

        (1)  udot = f(t,q,u)      NQ dynamic differential equations
        (2)  qdot = u             NQ kinematic differential equations
        (3)  0    = c(t,q,u)      NC constraints

Here the "dot" suffix indicates differentiation with respect to the independent variable t which we'll refer to as time here although it can be anything (for geodesic calculations it is arc length). We'll call the second order variables q the "position variables", and their time derivatives u the "velocity variables". Collected together we call the state y={q,u}. At the beginning of a step, we expect to have been given initial conditions t0,q0,u0 such that |c(t0,q0,u0)|<=tol. The user provides the accuracy requirement and constraint tolerance. We solve the system to that accuracy while keeping the constraints within tolerance. The integrator returns after taking a successful step which may involve trial evaluations that are retracted.

By "ODE on a manifold" we mean that the ODE (1,2) automatically satisfies the condition that IF c==0, THEN cdot=0, where

    cdot=Dc/Dt + Dc/Dq*qdot + Dc/Du*udot

This means that satisfaction of the acceleration-level constraints is built into the dynamic differential equations (1) so that we need only deal with relatively slow drift of the solution away from the position and velocity constraint manifolds.

To handle the constraint drift we use the method of coordinate projection and expect the supplied Equations object to be able to perform a least-squares projection of a state (q,u) to move it onto the constraint manifolds.

[1] Hairer, Lubich, Wanner, "Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations", 2nd ed., section IV.4, pg 109ff, Springer, 2006.

Member Enumeration Documentation

template<class Eqn >

anonymous enum

Enumerator:

NQ
NC
N

Constructor & Destructor Documentation

template<class Eqn >

SimTK::GeodesicIntegrator< Eqn >::GeodesicIntegrator	(	const Eqn &	eqn,
		Real	accuracy,
		Real	constraintTol
	)		`[inline]`

Construct an integrator for the given set of equations eqn, which are to be solved to the given accuracy, with constraints maintained to within the given constraintTol.

Member Function Documentation

template<class Eqn >

void SimTK::GeodesicIntegrator< Eqn >::initialize	(	Real	t,
		const Vec< N > &	y
	)		`[inline]`

Call this once before taking a series of steps.

This sets the initial conditions, and calculates the starting derivatives and constraint errors. The constraints must be satisfied already by the given state; an error is thrown if not.

template<class Eqn >

void SimTK::GeodesicIntegrator< Eqn >::setTimeAndState	(	Real	t,
		const Vec< N > &	y
	)		`[inline]`

Set initial time and state prior to integrating.

State derivatives and constraint errors are calculated and an error is thrown if the constraints are not already satisifed to the required tolerance.

template<class Eqn >

void SimTK::GeodesicIntegrator< Eqn >::setNextStepSizeToTry ( Real h ) [inline]

Use this if you think you know a better initial step size to try than the default.

template<class Eqn >

Real SimTK::GeodesicIntegrator< Eqn >::getNextStepSizeToTry ( ) const [inline]

Return the size of the next time step the integrator will attempt on the next call to takeOneStep().

template<class Eqn >

Real SimTK::GeodesicIntegrator< Eqn >::getRequiredAccuracy ( ) const [inline]

Return the accuracy requirement as set in the constructor.

template<class Eqn >

Real SimTK::GeodesicIntegrator< Eqn >::getConstraintTolerance ( ) const [inline]

Return the constraint tolerance as set in the constructor.

template<class Eqn >

Real SimTK::GeodesicIntegrator< Eqn >::getActualInitialStepSizeTaken ( ) const [inline]

Return the size of the first accepted step to be taken after the most recent initialize() call.

template<class Eqn >

int SimTK::GeodesicIntegrator< Eqn >::getNumStepsTaken ( ) const [inline]

Return the number of successful time steps taken since the most recent initialize() call.

template<class Eqn >

int SimTK::GeodesicIntegrator< Eqn >::getNumStepsAttempted ( ) const [inline]

Return the total number of steps that were attempted since the most recent initialize() call.

In general this will be more than the number of steps taken since some will be rejected.

template<class Eqn >

int SimTK::GeodesicIntegrator< Eqn >::getNumErrorTestFailures ( ) const [inline]

How many steps were rejected because they did not satisfy the accuracy requirement, since the most recent initalize() call.

This is common but for non-stiff systems should be only a modest fraction of the number of steps taken.

template<class Eqn >

int SimTK::GeodesicIntegrator< Eqn >::getNumProjectionFailures ( ) const [inline]

How many steps were rejected because the projectIfNeeded() method was unable to satisfy the constraint tolerance (since the most recent initialize() call).

This should be very rare.

template<class Eqn >

int SimTK::GeodesicIntegrator< Eqn >::getNumInitializations ( ) const [inline]

Return the number of calls to initialize() since construction of this integrator object.

template<class Eqn >

void SimTK::GeodesicIntegrator< Eqn >::takeOneStep ( Real tStop )

Advance time and state by one error-controlled step and return, but in no case advance past t=tStop.

The integrator's internal time, state, and state derivatives are advanced to the end of the step. If this step reaches tStop, the returned time will be exactly tStop.

template<class Eqn >

const Real& SimTK::GeodesicIntegrator< Eqn >::getTime ( ) const [inline]

Return the current time.

template<class Eqn >

const Vec<N>& SimTK::GeodesicIntegrator< Eqn >::getY ( ) const [inline]

Return the complete current state as a Vec<N>.

template<class Eqn >

const Vec<NQ>& SimTK::GeodesicIntegrator< Eqn >::getQ ( ) const [inline]

Return just the "position" variables q from the current state.

template<class Eqn >

const Vec<NQ>& SimTK::GeodesicIntegrator< Eqn >::getU ( ) const [inline]

Return just the "velocity" variables u from the current state.

template<class Eqn >

const Vec<N>& SimTK::GeodesicIntegrator< Eqn >::getYDot ( ) const [inline]

Return the complete set of time derivatives of the current state.

template<class Eqn >

const Vec<NQ>& SimTK::GeodesicIntegrator< Eqn >::getQDot ( ) const [inline]

Return just the derivatives qdot of the "position" variables q.

template<class Eqn >

const Vec<NQ>& SimTK::GeodesicIntegrator< Eqn >::getUDot ( ) const [inline]

Return just the derivatives udot of the "velocity" variables u.

template<class Eqn >

template<int Z>

static Real SimTK::GeodesicIntegrator< Eqn >::calcNormInf ( const Vec< Z > & v ) [inline, static]

This is a utility routine that returns the infinity norm (maximum absolute value) contained in a fixed-size, scalar Vec.

template<class Eqn >

template<int Z>

static Real SimTK::GeodesicIntegrator< Eqn >::calcNormRMS ( const Vec< Z > & v ) [inline, static]

This is a utility routine that returns the RMS norm of a fixed-size, scalar Vec.

The documentation for this class was generated from the following file:

GeodesicIntegrator.h

Public Types

Public Member Functions

Static Public Member Functions

Detailed Description

template<class Eqn> class SimTK::GeodesicIntegrator< Eqn >

Class Equations

Usage

Mathematical Overview

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

template<class Eqn>
class SimTK::GeodesicIntegrator< Eqn >