#include <Constraint.h>

Inheritance diagram for SimTK::Constraint::Custom::Implementation:

Public Member Functions
virtual	~Implementation ()
	Destructor is virtual so derived classes get a chance to clean up if necessary.
virtual Implementation *	clone () const =0
	This method should produce a deep copy identical to the concrete derived Implementation object underlying this Implementation base class object.
	Implementation (SimbodyMatterSubsystem &, int mp, int mv, int ma)
	This Implementation base class constructor sets the topological defaults for the number of position level (holonomic), velocity level (nonholonomic), and acceleration-only constraint equations to be generated.
	Implementation (SimbodyMatterSubsystem &)
	The default constructor for the Implementation base class sets the number of generated equations to zero for this constraint, meaning the Constraint won't do anything by default.
const SimbodyMatterSubsystem &	getMatterSubsystem () const
void	invalidateTopologyCache () const
	Call this if you want to make sure that the next realizeTopology() call does something.
Implementation &	setDefaultNumConstraintEquations (int mp, int mv, int ma)
	This is an alternate way to set the default number of equations to be generated if you didn't specify them in the base class constructor.
Implementation &	setDisabledByDefault (bool shouldBeDisabled)
	Normally Constraints are enabled when defined and can be disabled later.
ConstrainedBodyIndex	addConstrainedBody (const MobilizedBody &)
	Call this during construction phase to add a body to the topological structure of this Constraint.
ConstrainedMobilizerIndex	addConstrainedMobilizer (const MobilizedBody &)
	Call this during construction phase to add a mobilizer to the topological structure of this Constraint.
MobilizedBodyIndex	getMobilizedBodyIndexOfConstrainedBody (ConstrainedBodyIndex) const
MobilizedBodyIndex	getMobilizedBodyIndexOfConstrainedMobilizer (ConstrainedMobilizerIndex) const
Real	getOneQ (const State &, ConstrainedMobilizerIndex, MobilizerQIndex) const
	Extract from the State the value of a single generalized coordinate q from one of this Constraint's ConstrainedMobilizers.
Real	getOneU (const State &, ConstrainedMobilizerIndex, MobilizerUIndex) const
	Extract from the State the value of a single generalized speed (mobility) u from one of this Constraint's ConstrainedMobilizers.
Real	getOneQDot (const State &, ConstrainedMobilizerIndex, MobilizerQIndex, bool realizingVelocity=false) const
	Extract from the State cache the value of a single generalized coordinate time derivative qdot.
Real	getOneQDotDot (const State &, ConstrainedMobilizerIndex, MobilizerQIndex, bool realizingAcceleration=false) const
	Extract from the State cache the value of a single generalized coordinate second time derivative qdotdot.
Real	getOneUDot (const State &, ConstrainedMobilizerIndex, MobilizerUIndex, bool realizingAcceleration=false) const
	Extract from the State cache the value of a single generalized speed time derivative udot.
void	addInOneMobilityForce (const State &, ConstrainedMobilizerIndex, MobilizerUIndex whichU, Real force, Vector &mobilityForces) const
	Apply a scalar generalized (mobility) force `force` to a particular mobility of one of this Constraint's Constrained Mobilizers, adding it in to the appropriate slot of the mobilityForces vector, which is of length getNumConstrainedU() for this Constraint.
const Transform &	getBodyTransform (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the spatial transform X_AB giving the location and orientation of a Constrained Body B's body frame in this Constraint's Ancestor frame A.
const SpatialVec &	getBodyVelocity (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the spatial velocity V_AB giving the linear and angular velocity of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.
const SpatialVec &	getBodyAcceleration (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the spatial acceleration A_AB giving the linear and angular acceleration of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.
const Rotation &	getBodyRotation (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the rotation matrix R_AB giving the orientation of a Constrained Body B's body frame in this Constraint's Ancestor frame A.
const Vec3 &	getBodyAngularVelocity (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the angular velocity w_AB giving the angular velocity of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.
const Vec3 &	getBodyAngularAcceleration (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the angular acceleration b_AB giving the angular acceleration of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.
const Vec3 &	getBodyOriginLocation (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the position vector p_AB (or more explicitly, p_OA_OB) giving the location of a Constrained Body B's body frame origin OB relative to this Constraint's Ancestor (A) frame origin OA.
const Vec3 &	getBodyOriginVelocity (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the linear velocity v_AB (or more explicitly, v_A_OB) giving the linear velocity of a Constrained Body B's body frame origin OB measured and expressed in this Constraint's Ancestor frame A.
const Vec3 &	getBodyOriginAcceleration (const State &s, ConstrainedBodyIndex B) const
	Extract from the State cache the linear acceleration a_AB (or more explicitly, a_A_OB) giving the linear acceleration of a Constrained Body B's body frame origin OB, measured and expressed in this Constraint's Ancestor frame A.
Vec3	findStationLocation (const State &s, ConstrainedBodyIndex B, const Vec3 &p_BS) const
	Calculate the location p_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame.
Vec3	findStationVelocity (const State &s, ConstrainedBodyIndex B, const Vec3 &p_BS) const
	Calculate the velocity v_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame.
Vec3	findStationAcceleration (const State &s, ConstrainedBodyIndex B, const Vec3 &p_BS) const
	Calculate the acceleration a_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame.
void	addInStationForce (const State &s, ConstrainedBodyIndex B, const Vec3 &p_BS, const Vec3 &forceInA, Vector_< SpatialVec > &bodyForcesInA) const
	Apply an Ancestor-frame force to a B-frame station S given by the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame, adding to the appropriate `bodyForcesInA` entry for this ConstrainedBody B.
void	addInBodyTorque (const State &s, ConstrainedBodyIndex B, const Vec3 &torqueInA, Vector_< SpatialVec > &bodyForcesInA) const
	Apply an Ancestor-frame torque to body B, adding to the appropriate `bodyForcesInA` entry for this ConstrainedBody B.
Protected Member Functions
virtual void	calcDecorativeGeometryAndAppend (const State &s, Stage stage, Array_< DecorativeGeometry > &geom) const
	Implement this optional method if you would like your constraint to generate any suggestions for geometry that could be used as default visualization as an aid to understanding a system containing this constraint.
Optional realize() virtual methods
Provide implementations of these methods if you want to allocate State variables (such as modeling options or parameters) or want to pre-calculate some expensive quantities and store them in the State cache for your future use. Note that the Position, Velocity, and Acceleration-stage realize methods will be called before the constraint error calculating methods associated with this Constraint's constraint equations. That means, for example, you can calculate some distances in realizePosition() and then use them in realizePositionErrors().
virtual void	realizeTopology (State &) const
	The Matter Subsystem's realizeTopology() method will call this method after all MobilizedBody topology has been processed.
virtual void	realizeModel (State &) const
	The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Model-stage processing has been done.
virtual void	realizeInstance (const State &) const
	The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instance-stage processing has been done.
virtual void	realizeTime (const State &) const
	The Matter Subsystem's realizeTime() method will call this method after any MobilizedBody Time-stage processing has been done.
virtual void	realizePosition (const State &) const
	The Matter Subsystem's realizePosition() method will call this method after any MobilizedBody Position-stage processing has been done.
virtual void	realizeVelocity (const State &) const
	The Matter Subsystem's realizeVelocity() method will call this method after any MobilizedBody Velocity-stage processing has been done.
virtual void	realizeDynamics (const State &) const
	The Matter Subsystem's realizeDynamics() method will call this method after any MobilizedBody Dynamics-stage processing has been done.
virtual void	realizeAcceleration (const State &) const
	The Matter Subsystem's realizeAcceleration() method will call this method after any MobilizedBody Acceleration-stage processing has been done.
virtual void	realizeReport (const State &) const
	The Matter Subsystem's realizeReport() method will call this method after any MobilizedBody Report-stage processing has been done.
Position (Holonomic) Constraint Virtuals
These must be defined if there are any position (holonomic) constraints defined. Note that the number mp of expected position constraints is passed in by Simbody for redundancy; this should agree with what you are expecting and defines the number of Real values Simbody expects you to write on or view at the address it provides.
virtual void	realizePositionErrors (const State &, int mp, Real *perr) const
	During realizePosition(), calculate the `mp` position-constraint errors due to the position-level specification of a holonomic constraint and write them to `perr`.
virtual void	realizePositionDotErrors (const State &, int mp, Real *pverr) const
	During realizeVelocity(), calculate the `mp` errors arising from the first time derivative of the position-level specification of a holonomic constraint and write them to `pverr`.
virtual void	realizePositionDotDotErrors (const State &, int mp, Real *paerr) const
	During realizeAcceleration(), calculate the `mp` errors arising from the second time derivative of the position-level specification of a holonomic constraint and write them to `paerr`.
virtual void	applyPositionConstraintForces (const State &, int mp, const Real *multipliers, Vector_< SpatialVec > &bodyForces, Vector &mobilityForces) const
	From the `mp` supplied Lagrange multipliers provided in `multipliers`, calculate the forces produced by this Constraint on its ConstrainedBodies and ConstrainedMobilizers.
Velocity (Nonholonomic) Constraint Virtuals
These must be defined if there are any velocity (nonholonomic) constraints defined.
virtual void	realizeVelocityErrors (const State &, int mv, Real *verr) const
	During realizeVelocity(), calculate the `mv` errors arising from the initial velocity-level specification of a nonholonomic constraint and write them to `verr`.
virtual void	realizeVelocityDotErrors (const State &, int mv, Real *vaerr) const
	During realizeAcceleration(), calculate the `mv` errors arising from the first time derivative of the velocity-level specification of a nonholonomic constraint and write them to `vaerr`.
virtual void	applyVelocityConstraintForces (const State &, int mv, const Real *multipliers, Vector_< SpatialVec > &bodyForces, Vector &mobilityForces) const
	From the `mv` supplied Lagrange multipliers provided in `multipliers`, calculate the forces produced by this Constraint on its ConstrainedBodies and ConstrainedMobilizers due to its velocity-level (nonholonomic) constraints.
Acceleration-Only Constraint Virtuals
These must be defined if there are any acceleration-only constraints defined.
virtual void	realizeAccelerationErrors (const State &, int ma, Real *aerr) const
	During realizeAcceleration(), calculate the `ma` errors arising from the specification of an acceleration-only constraint and write them to `aerr`.
virtual void	applyAccelerationConstraintForces (const State &, int ma, const Real *multipliers, Vector_< SpatialVec > &bodyForces, Vector &mobilityForces) const
	From the `ma` supplied Lagrange multipliers provided in `multipliers`, calculate the forces produced by this Constraint on its ConstrainedBodies and ConstrainedMobilizers due to its acceleration-only constraints.
Friends
class	Constraint::CustomImpl

Constructor & Destructor Documentation

virtual SimTK::Constraint::Custom::Implementation::~Implementation ( ) [inline, virtual]

Destructor is virtual so derived classes get a chance to clean up if necessary.

SimTK::Constraint::Custom::Implementation::Implementation	(	SimbodyMatterSubsystem &	,
		int	mp,
		int	mv,
		int	ma
	)

This Implementation base class constructor sets the topological defaults for the number of position level (holonomic), velocity level (nonholonomic), and acceleration-only constraint equations to be generated.

SimTK::Constraint::Custom::Implementation::Implementation ( SimbodyMatterSubsystem & ) [explicit]

The default constructor for the Implementation base class sets the number of generated equations to zero for this constraint, meaning the Constraint won't do anything by default.

The actual number can be changed using setNumConstraintEquationsInUse() prior to realizeModel().

Member Function Documentation

virtual Implementation* SimTK::Constraint::Custom::Implementation::clone ( ) const [pure virtual]

This method should produce a deep copy identical to the concrete derived Implementation object underlying this Implementation base class object.

Note that the result is new heap space; the caller must be sure to take ownership of the returned pointer and call delete on it when done.

const SimbodyMatterSubsystem& SimTK::Constraint::Custom::Implementation::getMatterSubsystem ( ) const

void SimTK::Constraint::Custom::Implementation::invalidateTopologyCache ( ) const

Call this if you want to make sure that the next realizeTopology() call does something.

This is done automatically when you modify the constraint in ways understood by Simbody, such as adding a ConstrainedBody. But if you are just changing some of your own topology and want to make sure you get a chance to recompute something in realizeTopology(), make this call at the time of modification.

Implementation& SimTK::Constraint::Custom::Implementation::setDefaultNumConstraintEquations	(	int	mp,
		int	mv,
		int	ma
	)

This is an alternate way to set the default number of equations to be generated if you didn't specify them in the base class constructor.

A reference to this Implementation is returned so that this can be used in a sequence like an assignment operator.

Implementation& SimTK::Constraint::Custom::Implementation::setDisabledByDefault ( bool shouldBeDisabled )

Normally Constraints are enabled when defined and can be disabled later.

If you want to define this constraint but have it be off by default, use this method. A reference to this Implementation is returned so that this can be used in a sequence like an assignment operator.

ConstrainedBodyIndex SimTK::Constraint::Custom::Implementation::addConstrainedBody ( const MobilizedBody & )

Call this during construction phase to add a body to the topological structure of this Constraint.

This body's mobilizer's mobilities are not part of the constraint; mobilizers must be added separately. Numbering starts from 0 for each Constraint. The supplied MobilizedBody must be in the Matter Subsystem of which this Constraint is a part.

ConstrainedMobilizerIndex SimTK::Constraint::Custom::Implementation::addConstrainedMobilizer ( const MobilizedBody & )

Call this during construction phase to add a mobilizer to the topological structure of this Constraint.

All the coordinates q and mobilities u for this mobilizer are added also, but we don't know how many of those there will be until Stage::Model. Numbering starts from 0 for each Constraint. The supplied MobilizedBody must be in the Matter Subsystem of which this Constraint is a part.

MobilizedBodyIndex SimTK::Constraint::Custom::Implementation::getMobilizedBodyIndexOfConstrainedBody ( ConstrainedBodyIndex ) const

MobilizedBodyIndex SimTK::Constraint::Custom::Implementation::getMobilizedBodyIndexOfConstrainedMobilizer ( ConstrainedMobilizerIndex ) const

Real SimTK::Constraint::Custom::Implementation::getOneQ	(	const State &	,
		ConstrainedMobilizerIndex	,
		MobilizerQIndex
	)		const

Extract from the State the value of a single generalized coordinate q from one of this Constraint's ConstrainedMobilizers.

The State must have been realized to Model stage.

Real SimTK::Constraint::Custom::Implementation::getOneU	(	const State &	,
		ConstrainedMobilizerIndex	,
		MobilizerUIndex
	)		const

Extract from the State the value of a single generalized speed (mobility) u from one of this Constraint's ConstrainedMobilizers.

The State needs to be realized only as high as Model stage, but don't use this value in a position-level method like realizePositionErrors() or in any of the applyConstraintForces methods! Those must be limited to dependencies on time and configuration only.

Real SimTK::Constraint::Custom::Implementation::getOneQDot	(	const State &	,
		ConstrainedMobilizerIndex	,
		MobilizerQIndex	,
		bool	realizingVelocity = `false`
	)		const

Extract from the State cache the value of a single generalized coordinate time derivative qdot.

State must already be realized to the Velocity stage, or if you are currently realizing that stage set realizingVelocity true in which case the State need only have been realized to the previous (Position) stage.

Real SimTK::Constraint::Custom::Implementation::getOneQDotDot	(	const State &	,
		ConstrainedMobilizerIndex	,
		MobilizerQIndex	,
		bool	realizingAcceleration = `false`
	)		const

Extract from the State cache the value of a single generalized coordinate second time derivative qdotdot.

State must already be realized to the Acceleration stage, or if you are currently realizing that stage set realizingAcceleration true in which case the State need only have been realized to the previous (Dynamics) stage.

Real SimTK::Constraint::Custom::Implementation::getOneUDot	(	const State &	,
		ConstrainedMobilizerIndex	,
		MobilizerUIndex	,
		bool	realizingAcceleration = `false`
	)		const

Extract from the State cache the value of a single generalized speed time derivative udot.

State must already be realized to the Acceleration stage, or if you are currently realizing that stage set realizingAcceleration true in which case the State need only have been realized to the previous (Dynamics) stage.

void SimTK::Constraint::Custom::Implementation::addInOneMobilityForce	(	const State &	,
		ConstrainedMobilizerIndex	,
		MobilizerUIndex	whichU,
		Real	force,
		Vector &	mobilityForces
	)		const

Apply a scalar generalized (mobility) force force to a particular mobility of one of this Constraint's Constrained Mobilizers, adding it in to the appropriate slot of the mobilityForces vector, which is of length getNumConstrainedU() for this Constraint.

State need only have been realized to Model stage, but this is intended for use in applyConstraintForce methods at Position stage.

const Transform& SimTK::Constraint::Custom::Implementation::getBodyTransform	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const

Extract from the State cache the spatial transform X_AB giving the location and orientation of a Constrained Body B's body frame in this Constraint's Ancestor frame A.

const SpatialVec& SimTK::Constraint::Custom::Implementation::getBodyVelocity	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const

Extract from the State cache the spatial velocity V_AB giving the linear and angular velocity of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.

const SpatialVec& SimTK::Constraint::Custom::Implementation::getBodyAcceleration	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const

Extract from the State cache the spatial acceleration A_AB giving the linear and angular acceleration of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.

const Rotation& SimTK::Constraint::Custom::Implementation::getBodyRotation	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const `[inline]`

Extract from the State cache the rotation matrix R_AB giving the orientation of a Constrained Body B's body frame in this Constraint's Ancestor frame A.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyAngularVelocity	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const `[inline]`

Extract from the State cache the angular velocity w_AB giving the angular velocity of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyAngularAcceleration	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const `[inline]`

Extract from the State cache the angular acceleration b_AB giving the angular acceleration of a Constrained Body B's body frame measured and expressed in this Constraint's Ancestor frame A.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginLocation	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const `[inline]`

Extract from the State cache the position vector p_AB (or more explicitly, p_OA_OB) giving the location of a Constrained Body B's body frame origin OB relative to this Constraint's Ancestor (A) frame origin OA.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginVelocity	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const `[inline]`

Extract from the State cache the linear velocity v_AB (or more explicitly, v_A_OB) giving the linear velocity of a Constrained Body B's body frame origin OB measured and expressed in this Constraint's Ancestor frame A.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginAcceleration	(	const State &	s,
		ConstrainedBodyIndex	B
	)		const `[inline]`

Extract from the State cache the linear acceleration a_AB (or more explicitly, a_A_OB) giving the linear acceleration of a Constrained Body B's body frame origin OB, measured and expressed in this Constraint's Ancestor frame A.

Vec3 SimTK::Constraint::Custom::Implementation::findStationLocation	(	const State &	s,
		ConstrainedBodyIndex	B,
		const Vec3 &	p_BS
	)		const `[inline]`

Calculate the location p_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame.

The return value is a position vector from the Ancestor frame's origin OA to the location of the point S, expressed in the Ancestor frame.

Vec3 SimTK::Constraint::Custom::Implementation::findStationVelocity	(	const State &	s,
		ConstrainedBodyIndex	B,
		const Vec3 &	p_BS
	)		const `[inline]`

Calculate the velocity v_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame.

The return value is a vector expressed in the Ancestor frame.

Vec3 SimTK::Constraint::Custom::Implementation::findStationAcceleration	(	const State &	s,
		ConstrainedBodyIndex	B,
		const Vec3 &	p_BS
	)		const `[inline]`

Calculate the acceleration a_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame.

The return value is a vector expressed in the Ancestor frame.e.

void SimTK::Constraint::Custom::Implementation::addInStationForce	(	const State &	s,
		ConstrainedBodyIndex	B,
		const Vec3 &	p_BS,
		const Vec3 &	forceInA,
		Vector_< SpatialVec > &	bodyForcesInA
	)		const

Apply an Ancestor-frame force to a B-frame station S given by the position vector p_BS (or more explicitly, p_OB_S) from the B frame origin to the point S, expressed in the B frame, adding to the appropriate bodyForcesInA entry for this ConstrainedBody B.

void SimTK::Constraint::Custom::Implementation::addInBodyTorque	(	const State &	s,
		ConstrainedBodyIndex	B,
		const Vec3 &	torqueInA,
		Vector_< SpatialVec > &	bodyForcesInA
	)		const

Apply an Ancestor-frame torque to body B, adding to the appropriate bodyForcesInA entry for this ConstrainedBody B.

virtual void SimTK::Constraint::Custom::Implementation::realizeTopology ( State & ) const [inline, protected, virtual]

The Matter Subsystem's realizeTopology() method will call this method after all MobilizedBody topology has been processed.