This is the abstract base class for the implementation of custom constraints. See Constraint::Custom for more information. More...

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. More...

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. More...

	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. More...

	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. More...

const SimbodyMatterSubsystem &	getMatterSubsystem () const
	Return a reference to the matter subsystem containing this constraint. More...

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

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. More...

Implementation &	setDisabledByDefault (bool shouldBeDisabled)
	Normally Constraints are enabled when defined and can be disabled later. More...

ConstrainedBodyIndex	addConstrainedBody (const MobilizedBody &)
	Call this during construction phase to add a body to the topological structure of this Constraint. More...

ConstrainedMobilizerIndex	addConstrainedMobilizer (const MobilizedBody &)
	Call this during construction phase to add a mobilizer to the topological structure of this Constraint. More...

MobilizedBodyIndex	getMobilizedBodyIndexOfConstrainedBody (ConstrainedBodyIndex) const
	Map a constrained body for this constraint to the mobilized body to which it corresponds in the matter subsystem. More...

MobilizedBodyIndex	getMobilizedBodyIndexOfConstrainedMobilizer (ConstrainedMobilizerIndex) const
	Map a constrained mobilizer for this constraint to the mobilized body to which it corresponds in the matter subsystem. More...

Methods for use with ConstrainedMobilizers
When a constraint acts directly on generalized coordinates q or generalized speeds u (or their time derivatives), use methods in this section to access those values in your constraint error and force methods. The "from state" methods should only be used to pull information from the state that is at a higher level than the method being written. For example, if you are calculating velocity errors you can get positions from the state, but not velocities. Instead, the velocities will be passed as an argument.
Real	getOneQ (const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQ, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
	Use this method in your calcPositionErrors() implementation to extract the value of a particular generalized coordinate q selected by (mobilizer,whichQ), from the "constrained q" argument that is passed to the method from Simbody. More...

Real	getOneQFromState (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
	Same as the getOneQ() method but for use in methods to which no explicit "constrained q" argument is supplied. More...

Real	getOneQDot (const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQDot, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
	Use this method in your calcPositionDotErrors() implementation to extract the value of a particular generalized coordinate derivative qdot selected by (mobilizer,whichQ), from the "constrained qdot" argument that is passed to the method from Simbody. More...

Real	getOneQDotFromState (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
	Same as the getOneQDot() method above but for use in velocity- or acceleration-level methods to which no explicit "constrained qdot" argument is supplied. More...

Real	getOneQDotDot (const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQDotDot, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
	Use this method in your calcPositionDotDotErrors() implementation to extract the value of a particular generalized coordinate second derivative qdotdot selected by (mobilizer,whichQ), from the "constrained qdotdot" argument that is passed to the method from Simbody. More...

Real	getOneU (const State &state, const Array_< Real, ConstrainedUIndex > &constrainedU, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const
	Use this method in your calcVelocityErrors() implementation to extract the value of a particular generalized speed u selected by (mobilizer,whichU), from the "constrained u" argument that is passed to the method from Simbody. More...

Real	getOneUFromState (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const
	Same as the getOneU() method but for use in velocity- or acceleration- level methods to which no explicit "constrained u" argument is supplied. More...

Real	getOneUDot (const State &state, const Array_< Real, ConstrainedUIndex > &constrainedUDot, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const
	Use this method in your calcVelocityDotErrors() and calcAccelerationErrors() implementations to extract the value of a particular generalized speed derivative udot selected by (mobilizer,whichU), from the "constrained udot" argument that is passed to these two methods from Simbody. More...

void	addInOneMobilityForce (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU, Real fu, Array_< Real, ConstrainedUIndex > &mobilityForces) const
	Apply a scalar generalized (mobility-space) force fu 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. More...

void	addInOneQForce (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ, Real fq, Array_< Real, ConstrainedQIndex > &qForces) const
	For use with holonomic (position) constraints, this method allows generalized forces to be applied in "q-space" rather than "u-space". More...

Methods for use with Constrained Bodies
When a constraint is enforced (at least in part) by applying forces to bodies, use the methods in this section to access position, velocity, and acceleration information about those constrained bodies. Note that you can pull higher-level information from the state, but information at the current level for a method must be taken from the supplied arguments instead. For example, if you are writing an acceleration error routine, you can get time, position, and velocity information from the state but must get acceleration information from the body accelerations that are supplied by Simbody as arguments.
const Transform &	getBodyTransform (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const
	Extract from the allX_AB argument the spatial transform X_AB giving the pose (orientation and location) of a Constrained Body B's body frame B in this constraint's Ancestor frame A. More...

const Rotation &	getBodyRotation (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyTransform() that returns just the orientation as the Rotation matrix R_AB. More...

const Vec3 &	getBodyOriginLocation (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyTransform() that returns just the location part of B's pose in A, that is the vector p_AB from A's origin Ao to B's origin Bo, expressed in A. More...

const Transform &	getBodyTransformFromState (const State &state, ConstrainedBodyIndex B) const
	Extract from the State cache the spatial transform X_AB giving the pose (orientation and location) of a Constrained Body B's body frame B in this constraint's Ancestor frame A. More...

const Rotation &	getBodyRotationFromState (const State &state, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyTransformFromState() that returns just the orientation as the Rotation matrix R_AB. More...

const Vec3 &	getBodyOriginLocationFromState (const State &state, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyTransformFromState() that returns just the location part of B's pose in A, that is the vector p_AB from A's origin Ao to B's origin Bo, expressed in A. More...

const SpatialVec &	getBodyVelocity (const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const
	Extract from the allV_AB argument the spatial velocity V_AB giving the angular and linear velocity of a Constrained Body B's body frame B measured and expressed in this constraint's Ancestor frame A. More...

const Vec3 &	getBodyAngularVelocity (const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyVelocity() that returns just the angular velocity vector w_AB. More...

const Vec3 &	getBodyOriginVelocity (const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyVelocity() that returns just the linear velocity vector v_AB. More...

const SpatialVec &	getBodyVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const
	Extract from the State cache the spatial velocity V_AB giving the angular and linear velocity of a Constrained Body B's body frame B measured and expressed in this Constraint's Ancestor frame A. More...

const Vec3 &	getBodyAngularVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyVelocityFromState() that returns just the angular velocity vector w_AB. More...

const Vec3 &	getBodyOriginVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyVelocityFromState() that returns just the linear velocity vector v_AB. More...

const SpatialVec &	getBodyAcceleration (const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const
	Extract from the allA_AB argument the spatial acceleration A_AB giving the angular and linear acceleration of a Constrained Body B's body frame B measured and expressed in this constraint's Ancestor frame A. More...

const Vec3 &	getBodyAngularAcceleration (const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyAcceleration() that returns just the angular acceleration vector b_AB. More...

const Vec3 &	getBodyOriginAcceleration (const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const
	Convenient inline interface to getBodyAcceleration() that returns just the linear acceleration vector a_AB. More...

Vec3	findStationLocation (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
	Calculate the position 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_BoS) from the B frame origin Bo to the point S, expressed in the B frame. More...

Vec3	findStationLocationFromState (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
	Same as findStationLocation() but for when you have to get the position information from the state rather than from an explicit argument. More...

Vec3	findStationVelocity (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB, 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_BoS) from the B frame origin Bo to the point S, expressed in the B frame. More...

Vec3	findStationVelocityFromState (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
	Same as findStationVelocity() but for when you have to get the velocity information from the state rather than from an explicit argument. More...

Vec3	findStationAcceleration (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB, 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_BoS) from the B frame origin Bo to the point S, expressed in the B frame. More...

void	addInStationForce (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS, const Vec3 &forceInA, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA) const
	Apply an Ancestor-frame force to a B-frame station S given by the position vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo to the point S, expressed in the B frame, adding to the appropriate `bodyForcesInA` entry for this ConstrainedBody B. More...

void	addInBodyTorque (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &torqueInA, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA) const
	Apply an Ancestor-frame torque to body B, adding to the appropriate `bodyForcesInA` entry for this ConstrainedBody B. More...

Utility methods
These provide access to quantities associated with this constraint, suitable for use in the optional realize() virtual methods.
void	getMultipliers (const State &state, Array_< Real > &multipliers) const
	Given a state as passed to your realizeAcceleration() implementation, obtain the multipliers that Simbody just calculated for this Constraint. More...

Public Member Functions inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl >
bool	isEmptyHandle () const
	Returns true if this handle is empty, that is, does not refer to any implementation object. More...

bool	isOwnerHandle () const
	Returns true if this handle is the owner of the implementation object to which it refers. More...

bool	isSameHandle (const Implementation &other) const
	Determine whether the supplied handle is the same object as "this" PIMPLHandle. More...

void	disown (Implementation &newOwner)
	Give up ownership of the implementation to an empty handle. More...

PIMPLHandle &	referenceAssign (const Implementation &source)
	"Copy" assignment but with shallow (pointer) semantics. More...

PIMPLHandle &	copyAssign (const Implementation &source)
	This is real copy assignment, with ordinary C++ object ("value") semantics. More...

void	clearHandle ()
	Make this an empty handle, deleting the implementation object if this handle is the owner of it. More...

const ImplementationImpl &	getImpl () const
	Get a const reference to the implementation associated with this Handle. More...

ImplementationImpl &	updImpl ()
	Get a writable reference to the implementation associated with this Handle. More...

int	getImplHandleCount () const
	Return the number of handles the implementation believes are referencing it. More...

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. More...

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 after the constraint error calculating methods associated with this Constraint's constraint equations have been used by Simbody to perform any constraint calculations. That means, for example, you can access calculated multipliers from your realizeAcceleration() method.
virtual void	realizeTopology (State &) const
	The Matter Subsystem's realizeTopology() method will call this method after all MobilizedBody topology has been processed. More...

virtual void	realizeModel (State &) const
	The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Model-stage processing has been done. More...

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. More...

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. More...

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, and after the call has been made to your calcPositionErrors() operator. More...

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, and after your calcVelocityErrors() and calcPositionDotErrors() operators have been called. More...

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. More...

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, and after your calcAccelerationErrors(), calcVelocityDotErrors(), and calcPositionDotDotErrors() operators have been called. More...

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. More...

Position (Holonomic) Constraint Virtuals
These must be defined if there are any position (holonomic) constraint equations generated by this Constraint.
virtual void	calcPositionErrors (const State &state, const Array_< Transform, ConstrainedBodyIndex > &X_AB, const Array_< Real, ConstrainedQIndex > &constrainedQ, Array_< Real > &perr) const
	Calculate the mp position-constraint errors due to the position-level specification of a holonomic constraint and write them to perr, which will have been allocated to length mp; do not reallocate it. More...

virtual void	calcPositionDotErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &V_AB, const Array_< Real, ConstrainedQIndex > &constrainedQDot, Array_< Real > &pverr) const
	Calculate the mp velocity errors arising from the first time derivative of the position-level holonomic constraint function calcPositionErrors(), and write them to pverr, which will have been allocated to length mp; do not reallocate it. More...

virtual void	calcPositionDotDotErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedQIndex > &constrainedQDotDot, Array_< Real > &paerr) const
	Calculate the mp errors arising from the second time derivative of the position-level holonomic constraint function calcPositionErrors(), and write them to paerr, which will have been allocated to length mp; do not reallocate it. More...

virtual void	addInPositionConstraintForces (const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedQIndex > &qForces) const
	From the mp supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Qs. More...

Velocity (Nonholonomic) Constraint Virtuals
These must be defined if there are any velocity (nonholonomic) constraint equations generated by this Constraint.
virtual void	calcVelocityErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &V_AB, const Array_< Real, ConstrainedUIndex > &constrainedU, Array_< Real > &verr) const
	Calculate the mv velocity-constraint errors due to the velocity-level specification of a nonholonomic constraint and write them to verr, which will already have been allocated to length mv; do not reallocate it. More...

virtual void	calcVelocityDotErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedUIndex > &constrainedUDot, Array_< Real > &vaerr) const
	Calculate the mv errors arising from the first time derivative of the velocity-level specification of a nonholonomic constraint and write them to vaerr, which will already have been allocated to length mv; do not reallocate it. More...

virtual void	addInVelocityConstraintForces (const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedUIndex > &mobilityForces) const
	From the `mv` supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Mobilities due to its velocity-level (nonholonomic) constraints. More...

Acceleration-Only Constraint Virtuals
These must be defined if there are any acceleration-only constraint equations generated by this Constraint.
virtual void	calcAccelerationErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedUIndex > &constrainedUDot, Array_< Real > &aerr) const
	Calculate the ma acceleration-constraint errors due to the specification of an acceleration-only constraint and write them to aerr, which will already have been allocated to length ma; do not reallocate it. More...

virtual void	addInAccelerationConstraintForces (const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedUIndex > &mobilityForces) const
	From the ma supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Mobilities due to its acceleration-only constraints. More...

Protected Member Functions inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl >
	PIMPLHandle ()
	The default constructor makes this an empty handle. More...

	PIMPLHandle (ImplementationImpl *p)
	This provides consruction of a handle referencing an existing implementation object. More...

	PIMPLHandle (const PIMPLHandle &source)
	The copy constructor makes either a deep (value) or shallow (reference) copy of the supplied source PIMPL object, based on whether this is a "pointer semantics" (PTR=true) or "object (value) semantics" (PTR=false, default) class. More...

	~PIMPLHandle ()
	Note that the destructor is non-virtual. More...

PIMPLHandle &	operator= (const PIMPLHandle &source)
	Copy assignment makes the current handle either a deep (value) or shallow (reference) copy of the supplied source PIMPL object, based on whether this is a "pointer sematics" (PTR=true) or "object (value) semantics" (PTR=false, default) class. More...

void	setImpl (ImplementationImpl *p)
	Set the implementation for this empty handle. More...

bool	hasSameImplementation (const Implementation &other) const
	Determine whether the supplied handle is a reference to the same implementation object as is referenced by "this" PIMPLHandle. More...

Friends
class	Constraint::CustomImpl

Additional Inherited Members
Public Types inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl >
typedef PIMPLHandle < Implementation, ImplementationImpl, PTR >	HandleBase

typedef HandleBase	ParentHandle

Detailed Description

This is the abstract base class for the implementation of custom constraints. See Constraint::Custom for more information.

Constructor & Destructor Documentation

virtual SimTK::Constraint::Custom::Implementation::~Implementation ( )

inlinevirtual

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

Return a reference to the matter subsystem containing this constraint.

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

Map a constrained body for this constraint to the mobilized body to which it corresponds in the matter subsystem.

You should not use this to extract any information in the constraint error or forces methods; always work with the constrained bodies and constrained mobilities instead.

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

Map a constrained mobilizer for this constraint to the mobilized body to which it corresponds in the matter subsystem.

You should not use this to extract any information in the constraint error or forces methods; always work with the constrained bodies and constrained mobilities instead.

Real SimTK::Constraint::Custom::Implementation::getOneQ	(	const State &	state,
		const Array_< Real, ConstrainedQIndex > &	constrainedQ,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerQIndex	whichQ
	)		const

Use this method in your calcPositionErrors() implementation to extract the value of a particular generalized coordinate q selected by (mobilizer,whichQ), from the "constrained q" argument that is passed to the method from Simbody.

Parameters

[in]	state	Supplied state which is used only for modeling information; generalized coordinates q within state are ignored.
[in]	constrainedQ	This is the argument that is supplied to calcPositionErrors() from which we will extract the particular q value selected by the next two arguments.
[in]	mobilizer	The constrained mobilizer one of whose generalized coordinates is of interest.
[in]	whichQ	The particular generalized coordinate of mobilizer whose value we want. The actual value will be selected from constrainedQ.

Returns: The value of the generalized coordinate q of interest, extracted from the constrainedQ argument.

Real SimTK::Constraint::Custom::Implementation::getOneQFromState	(	const State &	state,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerQIndex	whichQ
	)		const

Same as the getOneQ() method but for use in methods to which no explicit "constrained q" argument is supplied.

The desired q value is obtained from state. You can call this from any constraint implementation method except calcPositionError().

Real SimTK::Constraint::Custom::Implementation::getOneQDot	(	const State &	state,
		const Array_< Real, ConstrainedQIndex > &	constrainedQDot,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerQIndex	whichQ
	)		const

Use this method in your calcPositionDotErrors() implementation to extract the value of a particular generalized coordinate derivative qdot selected by (mobilizer,whichQ), from the "constrained qdot" argument that is passed to the method from Simbody.

Parameters

[in]	state	Supplied state which is used only for modeling information; qdots within state are ignored.
[in]	constrainedQDot	This is the argument that is supplied to calcPositionDotErrors() from which we will extract the particular qdot value selected by the next two arguments.
[in]	mobilizer	The constrained mobilizer one of whose generalized coordinates is of interest.
[in]	whichQ	The particular generalized coordinate of mobilizer whose qdot value we want. The actual value will be selected from constrainedQDot.

Returns: The value of the generalized coordinate derivative qdot of interest, extracted from the constrainedQDot argument.

Real SimTK::Constraint::Custom::Implementation::getOneQDotFromState	(	const State &	state,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerQIndex	whichQ
	)		const

Same as the getOneQDot() method above but for use in velocity- or acceleration-level methods to which no explicit "constrained qdot" argument is supplied.

The desired qdot value is obtained from state. You can call this from calcPositionDotDotError(). State must already be realized to the Velocity stage.

Real SimTK::Constraint::Custom::Implementation::getOneQDotDot	(	const State &	state,
		const Array_< Real, ConstrainedQIndex > &	constrainedQDotDot,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerQIndex	whichQ
	)		const

Use this method in your calcPositionDotDotErrors() implementation to extract the value of a particular generalized coordinate second derivative qdotdot selected by (mobilizer,whichQ), from the "constrained qdotdot" argument that is passed to the method from Simbody.

Parameters

[in]	state	Supplied state which is used only for modeling information; qdotdots within state are ignored.
[in]	constrainedQDotDot	This is the argument that is supplied to calcPositionDotDotErrors() from which we will extract the particular qdotdot value selected by the next two arguments.
[in]	mobilizer	The constrained mobilizer one of whose generalized coordinates is of interest.
[in]	whichQ	The particular generalized coordinate of mobilizer whose qdotdot value we want. The actual value will be selected from constrainedQDotDot.

Returns: The value of the generalized coordinate second derivative qdotdot of interest, extracted from the constrainedQDotDot argument.

There is no getOneQDotDotFromState() method because all the acceleration- level methods are passed qdotdot or udot as an explicit argument.

Real SimTK::Constraint::Custom::Implementation::getOneU	(	const State &	state,
		const Array_< Real, ConstrainedUIndex > &	constrainedU,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerUIndex	whichU
	)		const

Use this method in your calcVelocityErrors() implementation to extract the value of a particular generalized speed u selected by (mobilizer,whichU), from the "constrained u" argument that is passed to the method from Simbody.

Parameters

[in]	state	Supplied state which is used only for modeling information; generalized speeds u within state are ignored.
[in]	constrainedU	This is the argument that is supplied to calcVelocityErrors() from which we will extract the particular u value selected by the next two arguments.
[in]	mobilizer	The constrained mobilizer one of whose generalized speeds is of interest.
[in]	whichU	The particular generalized speed of mobilizer whose value we want. The actual value will be selected from constrainedU.

Returns: The value of the generalized speed u of interest, extracted from the constrainedU argument.

Real SimTK::Constraint::Custom::Implementation::getOneUFromState	(	const State &	state,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerUIndex	whichU
	)		const

Same as the getOneU() method but for use in velocity- or acceleration- level methods to which no explicit "constrained u" argument is supplied.

The desired u value is obtained from state. You can call this only from calcVelocityDotError(), calcAccelerationError(), and any constraint force method. The State needs to be realized only as high as Model stage, but don't use this value in calcPositionError() or addInPositionConstraintForces(). Those must be limited to dependencies on time and configuration only.

Real SimTK::Constraint::Custom::Implementation::getOneUDot	(	const State &	state,
		const Array_< Real, ConstrainedUIndex > &	constrainedUDot,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerUIndex	whichU
	)		const

Use this method in your calcVelocityDotErrors() and calcAccelerationErrors() implementations to extract the value of a particular generalized speed derivative udot selected by (mobilizer,whichU), from the "constrained udot" argument that is passed to these two methods from Simbody.

Parameters

[in]	state	Supplied state which is used only for modeling information; udots within state are ignored.
[in]	constrainedUDot	This is the argument that is supplied to calcVelocityDotErrors() and calcAccelerationErrros() from which we will extract the particular udot value selected by the next two arguments.
[in]	mobilizer	The constrained mobilizer one of whose generalized speeds is of interest.
[in]	whichU	The particular generalized speed of mobilizer whose udot value we want. The actual value will be selected from constrainedUDot.

Returns: The value of the generalized speed derivative udot of interest, extracted from the constrainedUDot argument.

There is no getOneUDotFromState() method because all the acceleration- level methods are passed qdotdot or udot as an explicit argument.

void SimTK::Constraint::Custom::Implementation::addInOneMobilityForce	(	const State &	state,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerUIndex	whichU,
		Real	fu,
		Array_< Real, ConstrainedUIndex > &	mobilityForces
	)		const

Apply a scalar generalized (mobility-space) force fu 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 Velocity-stage calls to addInXXXConstraintForce() methods for nonholonomic (velocity) or acceleration-only constraint equations.

See also: addInOneQForce() for use in position (holonomic) constraints

void SimTK::Constraint::Custom::Implementation::addInOneQForce	(	const State &	state,
		ConstrainedMobilizerIndex	mobilizer,
		MobilizerQIndex	whichQ,
		Real	fq,
		Array_< Real, ConstrainedQIndex > &	qForces
	)		const

For use with holonomic (position) constraints, this method allows generalized forces to be applied in "q-space" rather than "u-space".

A scalar q-space generalized force fq is applied to a particular generalized coordinate (q) of one of this position (holonomic) Constraint's Constrained Mobilizers, adding it in to the appropriate slot of the qForces vector, which must be of length getNumConstrainedQ() for this Constraint. State need only have been realized to Model stage, but this is intended for Position-stage use in the addInPositionConstraintForce() method for position constraint equations.

Simbody will convert these automatically to mobility (u) space as needed via fu = ~N * fq, where N is block-diagonal kinematic coupling matrix that appears in the equation qdot = N*u.

See also: addInOneMobilityForces() for velocity and acceleration-only constraint equations

const Transform& SimTK::Constraint::Custom::Implementation::getBodyTransform	(	const Array_< Transform, ConstrainedBodyIndex > &	allX_AB,
		ConstrainedBodyIndex	bodyB
	)		const

Extract from the allX_AB argument the spatial transform X_AB giving the pose (orientation and location) of a Constrained Body B's body frame B in this constraint's Ancestor frame A.

const Rotation& SimTK::Constraint::Custom::Implementation::getBodyRotation	(	const Array_< Transform, ConstrainedBodyIndex > &	allX_AB,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyTransform() that returns just the orientation as the Rotation matrix R_AB.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginLocation	(	const Array_< Transform, ConstrainedBodyIndex > &	allX_AB,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyTransform() that returns just the location part of B's pose in A, that is the vector p_AB from A's origin Ao to B's origin Bo, expressed in A.

const Transform& SimTK::Constraint::Custom::Implementation::getBodyTransformFromState	(	const State &	state,
		ConstrainedBodyIndex	B
	)		const

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

Do not use this method in a routine that has an explicit argument providing the transforms X_AB; use the above getBodyTransform() method instead.

const Rotation& SimTK::Constraint::Custom::Implementation::getBodyRotationFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyTransformFromState() that returns just the orientation as the Rotation matrix R_AB.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginLocationFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyTransformFromState() that returns just the location part of B's pose in A, that is the vector p_AB from A's origin Ao to B's origin Bo, expressed in A.

const SpatialVec& SimTK::Constraint::Custom::Implementation::getBodyVelocity	(	const Array_< SpatialVec, ConstrainedBodyIndex > &	allV_AB,
		ConstrainedBodyIndex	bodyB
	)		const

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

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyAngularVelocity	(	const Array_< SpatialVec, ConstrainedBodyIndex > &	allV_AB,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyVelocity() that returns just the angular velocity vector w_AB.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginVelocity	(	const Array_< SpatialVec, ConstrainedBodyIndex > &	allV_AB,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyVelocity() that returns just the linear velocity vector v_AB.

const SpatialVec& SimTK::Constraint::Custom::Implementation::getBodyVelocityFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB
	)		const

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

Do not use this method in a routine that has an explicit argument providing the spatial velocities V_AB; use the above getBodyVelocity() method instead.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyAngularVelocityFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyVelocityFromState() that returns just the angular velocity vector w_AB.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginVelocityFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyVelocityFromState() that returns just the linear velocity vector v_AB.

const SpatialVec& SimTK::Constraint::Custom::Implementation::getBodyAcceleration	(	const Array_< SpatialVec, ConstrainedBodyIndex > &	allA_AB,
		ConstrainedBodyIndex	bodyB
	)		const

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

Note that there is no getBodyAccelerationFromState() method because all acceleration-level methods will be passed body accelerations explicitly.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyAngularAcceleration	(	const Array_< SpatialVec, ConstrainedBodyIndex > &	allA_AB,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyAcceleration() that returns just the angular acceleration vector b_AB.

const Vec3& SimTK::Constraint::Custom::Implementation::getBodyOriginAcceleration	(	const Array_< SpatialVec, ConstrainedBodyIndex > &	allA_AB,
		ConstrainedBodyIndex	bodyB
	)		const

inline

Convenient inline interface to getBodyAcceleration() that returns just the linear acceleration vector a_AB.

Vec3 SimTK::Constraint::Custom::Implementation::findStationLocation	(	const Array_< Transform, ConstrainedBodyIndex > &	allX_AB,
		ConstrainedBodyIndex	bodyB,
		const Vec3 &	p_BS
	)		const

inline

Calculate the position 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_BoS) from the B frame origin Bo to the point S, expressed in the B frame.

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

Vec3 SimTK::Constraint::Custom::Implementation::findStationLocationFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB,
		const Vec3 &	p_BS
	)		const

inline

Same as findStationLocation() but for when you have to get the position information from the state rather than from an explicit argument.

Cost is 18 flops.

Vec3 SimTK::Constraint::Custom::Implementation::findStationVelocity	(	const State &	state,
		const Array_< SpatialVec, ConstrainedBodyIndex > &	allV_AB,
		ConstrainedBodyIndex	bodyB,
		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_BoS) from the B frame origin Bo to the point S, expressed in the B frame.

The return value v_AS is a vector expressed in the Ancestor frame, and is the time derivative taken in A of the position vector p_AS. Cost is 27 flops.

Vec3 SimTK::Constraint::Custom::Implementation::findStationVelocityFromState	(	const State &	state,
		ConstrainedBodyIndex	bodyB,
		const Vec3 &	p_BS
	)		const

inline

Same as findStationVelocity() but for when you have to get the velocity information from the state rather than from an explicit argument.

Cost is 27 flops.

Vec3 SimTK::Constraint::Custom::Implementation::findStationAcceleration	(	const State &	state,
		const Array_< SpatialVec, ConstrainedBodyIndex > &	allA_AB,
		ConstrainedBodyIndex	bodyB,
		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_BoS) from the B frame origin Bo to the point S, expressed in the B frame.

The return value a_AS is a vector expressed in the Ancestor frame, and is the time derivative taken in A of the velocity vector v_AS and hence the second derivative taken in A of the position vectory p_AS. Note that there is no findStationAccelerationFromState() method because all acceleration-level routines here are provided acceleration information in explicit arguments. Cost is 48 flops.

void SimTK::Constraint::Custom::Implementation::addInStationForce	(	const State &	state,
		ConstrainedBodyIndex	bodyB,
		const Vec3 &	p_BS,
		const Vec3 &	forceInA,
		Array_< SpatialVec, ConstrainedBodyIndex > &	bodyForcesInA
	)		const

Apply an Ancestor-frame force to a B-frame station S given by the position vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo 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 &	state,
		ConstrainedBodyIndex	bodyB,
		const Vec3 &	torqueInA,
		Array_< SpatialVec, ConstrainedBodyIndex > &	bodyForcesInA
	)		const

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

void SimTK::Constraint::Custom::Implementation::getMultipliers	(	const State &	state,
		Array_< Real > &	multipliers
	)		const

Given a state as passed to your realizeAcceleration() implementation, obtain the multipliers that Simbody just calculated for this Constraint.

virtual void SimTK::Constraint::Custom::Implementation::realizeTopology ( State & ) const

inlineprotectedvirtual

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