Simbody  3.5
SimTK::Constraint::Custom::Implementation Class Referenceabstract

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 Implementationclone () 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 SimbodyMatterSubsystemgetMatterSubsystem () 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...
 
ImplementationsetDefaultNumConstraintEquations (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...
 
ImplementationsetDisabledByDefault (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 TransformgetBodyTransform (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 RotationgetBodyRotation (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 Vec3getBodyOriginLocation (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 TransformgetBodyTransformFromState (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 RotationgetBodyRotationFromState (const State &state, ConstrainedBodyIndex bodyB) const
 Convenient inline interface to getBodyTransformFromState() that returns just the orientation as the Rotation matrix R_AB. More...
 
const Vec3getBodyOriginLocationFromState (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 SpatialVecgetBodyVelocity (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 Vec3getBodyAngularVelocity (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 Vec3getBodyOriginVelocity (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 SpatialVecgetBodyVelocityFromState (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 Vec3getBodyAngularVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const
 Convenient inline interface to getBodyVelocityFromState() that returns just the angular velocity vector w_AB. More...
 
const Vec3getBodyOriginVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const
 Convenient inline interface to getBodyVelocityFromState() that returns just the linear velocity vector v_AB. More...
 
const SpatialVecgetBodyAcceleration (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 Vec3getBodyAngularAcceleration (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 Vec3getBodyOriginAcceleration (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...
 
PIMPLHandlereferenceAssign (const Implementation &source)
 "Copy" assignment but with shallow (pointer) semantics. More...
 
PIMPLHandlecopyAssign (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...
 
PIMPLHandleoperator= (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, false > 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]stateSupplied state which is used only for modeling information; generalized coordinates q within state are ignored.
[in]constrainedQThis is the argument that is supplied to calcPositionErrors() from which we will extract the particular q value selected by the next two arguments.
[in]mobilizerThe constrained mobilizer one of whose generalized coordinates is of interest.
[in]whichQThe 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]stateSupplied state which is used only for modeling information; qdots within state are ignored.
[in]constrainedQDotThis is the argument that is supplied to calcPositionDotErrors() from which we will extract the particular qdot value selected by the next two arguments.
[in]mobilizerThe constrained mobilizer one of whose generalized coordinates is of interest.
[in]whichQThe 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]stateSupplied state which is used only for modeling information; qdotdots within state are ignored.
[in]constrainedQDotDotThis is the argument that is supplied to calcPositionDotDotErrors() from which we will extract the particular qdotdot value selected by the next two arguments.
[in]mobilizerThe constrained mobilizer one of whose generalized coordinates is of interest.
[in]whichQThe 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]stateSupplied state which is used only for modeling information; generalized speeds u within state are ignored.
[in]constrainedUThis is the argument that is supplied to calcVelocityErrors() from which we will extract the particular u value selected by the next two arguments.
[in]mobilizerThe constrained mobilizer one of whose generalized speeds is of interest.
[in]whichUThe 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]stateSupplied state which is used only for modeling information; udots within state are ignored.
[in]constrainedUDotThis 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]mobilizerThe constrained mobilizer one of whose generalized speeds is of interest.
[in]whichUThe 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.

This gives the Constraint a chance to

  • calculate Topology stage "cache" values (mutable values which are stored in the derived Implementation class directly), and
  • allocate Model-stage state variables for later use, and
  • allocate Model-stage cache entries in the State. The indices to the Model-stage state & cache entries must be stored locally as part of the Topology-stage cache.
virtual void SimTK::Constraint::Custom::Implementation::realizeModel ( State ) const
inlineprotectedvirtual

The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Model-stage processing has been done.

This gives the Constraint a chance to

  • calculate Model stage cache values according to the settings of the Model variables,
  • allocate any later-Stage variables that may be needed (typically these will be Instance stage variables containing geometric information or constraint parameters like lengths or velocities. The indices to any of the State entries allocated here must be stored in the State as part of the Model-stage cache.
virtual void SimTK::Constraint::Custom::Implementation::realizeInstance ( const State ) const
inlineprotectedvirtual

The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instance-stage processing has been done.

This gives the Constraint a chance to

  • calculate Instance stage cache values according to the settings of the Instance variables.
virtual void SimTK::Constraint::Custom::Implementation::realizeTime ( const State ) const
inlineprotectedvirtual

The Matter Subsystem's realizeTime() method will call this method after any MobilizedBody Time-stage processing has been done.

This gives the Constraint a chance to

  • calculate Time stage cache values according to the current value of time found in the State.
virtual void SimTK::Constraint::Custom::Implementation::realizePosition ( const State ) const
inlineprotectedvirtual

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.

This gives the Constraint a chance to

  • calculate Position stage cache values according to the current values of positions and position errors found in the State.
virtual void SimTK::Constraint::Custom::Implementation::realizeVelocity ( const State ) const
inlineprotectedvirtual

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.

This gives the Constraint a chance to

  • calculate Velocity stage cache values according to the current values of velocities and velocity errors found in the State.
virtual void SimTK::Constraint::Custom::Implementation::realizeDynamics ( const State ) const
inlineprotectedvirtual

The Matter Subsystem's realizeDynamics() method will call this method after any MobilizedBody Dynamics-stage processing has been done.

This gives the Constraint a chance to

  • calculate Dynamics stage cache values according to the current values found in the State.
virtual void SimTK::Constraint::Custom::Implementation::realizeAcceleration ( const State ) const
inlineprotectedvirtual

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.

This gives the Constraint a chance to

  • calculate Acceleration stage cache values according to the current values of body and mobility accelerations, acceleration errors, and multiplier values found in the state.
virtual void SimTK::Constraint::Custom::Implementation::realizeReport ( const State ) const
inlineprotectedvirtual

The Matter Subsystem's realizeReport() method will call this method after any MobilizedBody Report-stage processing has been done.

This gives the Constraint a chance to

  • calculate Report stage cache values according to the current values found in the State.
virtual void SimTK::Constraint::Custom::Implementation::calcPositionErrors ( const State state,
const Array_< Transform, ConstrainedBodyIndex > &  X_AB,
const Array_< Real, ConstrainedQIndex > &  constrainedQ,
Array_< Real > &  perr 
) const
protectedvirtual

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.

When this is called, state will already have been realized to Stage::Time; all position information used in your implementation must be taken from the passed-in arguments X_AB and constrainedQ, not from state.

virtual void SimTK::Constraint::Custom::Implementation::calcPositionDotErrors ( const State state,
const Array_< SpatialVec, ConstrainedBodyIndex > &  V_AB,
const Array_< Real, ConstrainedQIndex > &  constrainedQDot,
Array_< Real > &  pverr 
) const
protectedvirtual

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.

When this is called, state will have already been realized to Stage::Position; all velocity information used in your implementation must be taken from the passed-in arguments V_AB and constrainedQDot, not from state. However, you can obtain position information for the constrained bodies and constrained mobilizers from state using getOneQFromState(), getBodyTransformFromState(), and related methods. The implementation of this method must produce exactly the time derivative of the implementation of calcPositionErrors().

virtual void SimTK::Constraint::Custom::Implementation::calcPositionDotDotErrors ( const State state,
const Array_< SpatialVec, ConstrainedBodyIndex > &  A_AB,
const Array_< Real, ConstrainedQIndex > &  constrainedQDotDot,
Array_< Real > &  paerr 
) const
protectedvirtual

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.

When this is called, state will already have been realized to Stage::Velocity; all acceleration-level information used in your implementation must be taken from the passed-in arguments A_AB and constrainedQDotDot, not from state. However, you can obtain position and velocity information for the constrained bodies and constrained mobilizers from state using getOneQFromState(), getOneQDotFromState(), getBodyTransformFromState(), getBodyVelocityFromState(), and related methods. The implementation of this method must produce exactly the time derivative of the implementation of calcPositionDotErrors().

virtual void SimTK::Constraint::Custom::Implementation::addInPositionConstraintForces ( const State state,
const Array_< Real > &  multipliers,
Array_< SpatialVec, ConstrainedBodyIndex > &  bodyForcesInA,
Array_< Real, ConstrainedQIndex > &  qForces 
) const
protectedvirtual

From the mp supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Qs.

Body spatial forces are applied at the body origin and expressed in the Ancestor frame and written to an array bodyForcesInA of length getNumConstrainedBodies(). Q forces are written to an array qForces of length getNumConstrainedQ(), that is, the number of constrained generalized coordinates q, not the number of constrained mobilizers or constrained mobilities u. When this is called, state will already have been realized to Stage::Position and all position-stage cache information is available including any that may have been calculated during the prior call to this Constraint's calcPositionErrors() method and realizePosition() method. Simbody will already have ensured that the force-return arrays have been allocated to the right size and properly initialized; you need update only those to which you are applying forces.

Note
Don't forget that you must add in your force contributions; don't just write them or you'll wipe out all preceding constraints' contributions!
virtual void SimTK::Constraint::Custom::Implementation::calcVelocityErrors ( const State state,
const Array_< SpatialVec, ConstrainedBodyIndex > &  V_AB,
const Array_< Real, ConstrainedUIndex > &  constrainedU,
Array_< Real > &  verr 
) const
protectedvirtual

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.

When this is called, state will have been realized to Stage::Position; all velocity-level information used in your implementation must be taken from the passed-in arguments V_AB and constrainedU, not from state. However, you may obtain time or any position-related information from state. A nonholonomic constraint may depend on any position information; you do not have to limit that to constrained bodies and mobilizers as you do for velocity-level information.

virtual void SimTK::Constraint::Custom::Implementation::calcVelocityDotErrors ( const State state,
const Array_< SpatialVec, ConstrainedBodyIndex > &  A_AB,
const Array_< Real, ConstrainedUIndex > &  constrainedUDot,
Array_< Real > &  vaerr 
) const
protectedvirtual

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.

When this is called, state will have been realized to Stage::Velocity; all acceleration-level information used in your implementation must be taken from the passed-in arguments A_AB and constrainedUDot, not from state. However, you can obtain from state time, and any needed position and velocity information. The implementation of this method must produce exactly the time derivative of the implementation of calcVelocityErrors().

virtual void SimTK::Constraint::Custom::Implementation::addInVelocityConstraintForces ( const State state,
const Array_< Real > &  multipliers,
Array_< SpatialVec, ConstrainedBodyIndex > &  bodyForcesInA,
Array_< Real, ConstrainedUIndex > &  mobilityForces 
) const
protectedvirtual

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.

Body spatial forces are applied at the body origin and expressed in the Ancestor frame and written to an array bodyForcesInA of length getNumConstrainedBodies(). Mobility (generalized) forces are written to an array mobilityForces of length getNumConstrainedU(), that is, the number of constrained mobilities, not the number of constrained mobilizers. The supplied state will have been realized to Stage::Velocity and all position- and velocity-stage cache information is available including any that may have been calculated during the prior call to this constraint's realizePosition() and realizeVelocity() methods. Simbody will already have ensured that the force-return arrays have been allocated to the right size and initialized properly; you need only update the non-zero ones.

Note
Don't forget that you must add in your force contributions; don't just write them or you'll wipe out all preceding constraints' contributions!
virtual void SimTK::Constraint::Custom::Implementation::calcAccelerationErrors ( const State state,
const Array_< SpatialVec, ConstrainedBodyIndex > &  A_AB,
const Array_< Real, ConstrainedUIndex > &  constrainedUDot,
Array_< Real > &  aerr 
) const
protectedvirtual

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.

When this is called, state will have been realized to Stage::Velocity; all acceleration-level information used in your implementation must be taken from the passed-in arguments A_AB and constrainedUDot, not from state. However, an acceleration-only constraint may depend arbitrarily on time, position, and velocity information which you may obtain freely from state; you do not have to limit that to constrained bodies and mobilizers as you do for acceleration-level information.

Note
This method must be linear in the accelerations; Simbody has no way to enforce that so it is up to you to do this correctly.
virtual void SimTK::Constraint::Custom::Implementation::addInAccelerationConstraintForces ( const State state,
const Array_< Real > &  multipliers,
Array_< SpatialVec, ConstrainedBodyIndex > &  bodyForcesInA,
Array_< Real, ConstrainedUIndex > &  mobilityForces 
) const
protectedvirtual

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.

Body spatial forces are applied at the body origin and expressed in the Ancestor frame and written to an array bodyForcesInA of length getNumConstrainedBodies(). Mobility forces are written to an array mobilityForces of length getNumConstrainedU(), that is, the number of constrained mobilities, not the number of constrained mobilizers. The state will have been realized to Stage::Velocity and all position- and velocity-stage cache information is available including any that may have been calculated during the prior call to this constraint's realizePosition() and realizeVelocity() methods. Simbody will already have ensured that the force-return arrays have been allocated to the right size and initialized properly; you need only update the non-zero ones.

Note
Don't forget that you must add in your force contributions; don't just write them or you'll wipe out all preceding constraints' contributions!
virtual void SimTK::Constraint::Custom::Implementation::calcDecorativeGeometryAndAppend ( const State s,
Stage  stage,
Array_< DecorativeGeometry > &  geom 
) const
inlineprotectedvirtual

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.

For example, if your constraint connects two points, you might want to draw a line between those points. You can also generate text labels, and you can provide methods for controlling the presence or appearance of your generated geometry. If you don't implement this routine no geometry will be generated.

Friends And Related Function Documentation

friend class Constraint::CustomImpl
friend

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