Simbody
3.6

This is the abstract base class for the implementation of custom constraints. See Constraint::Custom for more information. More...
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 accelerationonly 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 accelerationlevel 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 (mobilityspace) 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 "qspace" rather than "uspace". 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 higherlevel 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 Ancestorframe force to a Bframe 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 Ancestorframe 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 precalculate some expensive quantities and store them in the State cache for your future use. Note that the Position, Velocity, and Accelerationstage 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 Modelstage processing has been done. More...  
virtual void  realizeInstance (const State &) const 
The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instancestage processing has been done. More...  
virtual void  realizeTime (const State &) const 
The Matter Subsystem's realizeTime() method will call this method after any MobilizedBody Timestage processing has been done. More...  
virtual void  realizePosition (const State &) const 
The Matter Subsystem's realizePosition() method will call this method after any MobilizedBody Positionstage 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 Velocitystage 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 Dynamicsstage processing has been done. More...  
virtual void  realizeAcceleration (const State &) const 
The Matter Subsystem's realizeAcceleration() method will call this method after any MobilizedBody Accelerationstage 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 Reportstage 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 positionconstraint errors due to the positionlevel 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 positionlevel 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 positionlevel 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 velocityconstraint errors due to the velocitylevel 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 velocitylevel 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 velocitylevel (nonholonomic) constraints. More...  
AccelerationOnly Constraint Virtuals  
These must be defined if there are any accelerationonly 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 accelerationconstraint errors due to the specification of an accelerationonly 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 accelerationonly 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 nonvirtual. 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, false >  HandleBase 
typedef HandleBase  ParentHandle 
This is the abstract base class for the implementation of custom constraints. See Constraint::Custom for more information.

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 accelerationonly constraint equations to be generated.

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().

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.
[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. 
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.
[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. 
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 accelerationlevel 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.
[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. 
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.
[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. 
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.
[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. 
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 (mobilityspace) 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 Velocitystage calls to addInXXXConstraintForce() methods for nonholonomic (velocity) or accelerationonly constraint equations.
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 "qspace" rather than "uspace".
A scalar qspace 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 Positionstage 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 blockdiagonal kinematic coupling matrix that appears in the equation qdot = N*u.
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.

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

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.

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

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.

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

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.

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

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 accelerationlevel methods will be passed body accelerations explicitly.

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

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

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.

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.

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.

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.

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 accelerationlevel 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 Ancestorframe force to a Bframe 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 Ancestorframe 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.

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

inlineprotectedvirtual 
The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Modelstage processing has been done.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instancestage processing has been done.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizeTime() method will call this method after any MobilizedBody Timestage processing has been done.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizePosition() method will call this method after any MobilizedBody Positionstage processing has been done, and after the call has been made to your calcPositionErrors() operator.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizeVelocity() method will call this method after any MobilizedBody Velocitystage processing has been done, and after your calcVelocityErrors() and calcPositionDotErrors() operators have been called.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizeDynamics() method will call this method after any MobilizedBody Dynamicsstage processing has been done.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizeAcceleration() method will call this method after any MobilizedBody Accelerationstage processing has been done, and after your calcAccelerationErrors(), calcVelocityDotErrors(), and calcPositionDotDotErrors() operators have been called.
This gives the Constraint a chance to

inlineprotectedvirtual 
The Matter Subsystem's realizeReport() method will call this method after any MobilizedBody Reportstage processing has been done.
This gives the Constraint a chance to

protectedvirtual 
Calculate the mp positionconstraint errors due to the positionlevel 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 passedin arguments X_AB and constrainedQ, not from state.

protectedvirtual 
Calculate the mp velocity errors arising from the first time derivative of the positionlevel 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 passedin 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().

protectedvirtual 
Calculate the mp errors arising from the second time derivative of the positionlevel 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 accelerationlevel information used in your implementation must be taken from the passedin 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().

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 positionstage 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 forcereturn arrays have been allocated to the right size and properly initialized; you need update only those to which you are applying forces.

protectedvirtual 
Calculate the mv velocityconstraint errors due to the velocitylevel 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 velocitylevel information used in your implementation must be taken from the passedin arguments V_AB and constrainedU, not from state. However, you may obtain time or any positionrelated 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 velocitylevel information.

protectedvirtual 
Calculate the mv errors arising from the first time derivative of the velocitylevel 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 accelerationlevel information used in your implementation must be taken from the passedin 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().

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 velocitylevel (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 velocitystage 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 forcereturn arrays have been allocated to the right size and initialized properly; you need only update the nonzero ones.

protectedvirtual 
Calculate the ma accelerationconstraint errors due to the specification of an accelerationonly 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 accelerationlevel information used in your implementation must be taken from the passedin arguments A_AB and constrainedUDot, not from state. However, an accelerationonly 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 accelerationlevel information.

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 accelerationonly 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 velocitystage 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 forcereturn arrays have been allocated to the right size and initialized properly; you need only update the nonzero ones.

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.

friend 