Simbody
3.7
|
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 acceleration-only constraint equations to be generated. More... | |
Implementation (SimbodyMatterSubsystem &) | |
The default constructor for the Implementation base class sets the number of generated equations to zero for this constraint, meaning the Constraint won't do anything by default. More... | |
const SimbodyMatterSubsystem & | getMatterSubsystem () const |
Return a reference to the matter subsystem containing this constraint. More... | |
void | invalidateTopologyCache () const |
Call this if you want to make sure that the next realizeTopology() call does something. More... | |
Implementation & | setDefaultNumConstraintEquations (int mp, int mv, int ma) |
This is an alternate way to set the default number of equations to be generated if you didn't specify them in the base class constructor. More... | |
Implementation & | setDisabledByDefault (bool shouldBeDisabled) |
Normally Constraints are enabled when defined and can be disabled later. More... | |
ConstrainedBodyIndex | addConstrainedBody (const MobilizedBody &) |
Call this during construction phase to add a body to the topological structure of this Constraint. More... | |
ConstrainedMobilizerIndex | addConstrainedMobilizer (const MobilizedBody &) |
Call this during construction phase to add a mobilizer to the topological structure of this Constraint. More... | |
MobilizedBodyIndex | getMobilizedBodyIndexOfConstrainedBody (ConstrainedBodyIndex) const |
Map a constrained body for this constraint to the mobilized body to which it corresponds in the matter subsystem. More... | |
MobilizedBodyIndex | getMobilizedBodyIndexOfConstrainedMobilizer (ConstrainedMobilizerIndex) const |
Map a constrained mobilizer for this constraint to the mobilized body to which it corresponds in the matter subsystem. More... | |
Methods for use with ConstrainedMobilizers | |
When a constraint acts directly on generalized coordinates q or generalized speeds u (or their time derivatives), use methods in this section to access those values in your constraint error and force methods. The "from state" methods should only be used to pull information from the state that is at a higher level than the method being written. For example, if you are calculating velocity errors you can get positions from the state, but not velocities. Instead, the velocities will be passed as an argument. | |
Real | getOneQ (const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQ, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const |
Use this method in your calcPositionErrors() implementation to extract the value of a particular generalized coordinate q selected by (mobilizer,whichQ), from the "constrained q" argument that is passed to the method from Simbody. More... | |
Real | getOneQFromState (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const |
Same as the getOneQ() method but for use in methods to which no explicit "constrained q" argument is supplied. More... | |
Real | getOneQDot (const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQDot, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const |
Use this method in your calcPositionDotErrors() implementation to extract the value of a particular generalized coordinate derivative qdot selected by (mobilizer,whichQ), from the "constrained qdot" argument that is passed to the method from Simbody. More... | |
Real | getOneQDotFromState (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const |
Same as the getOneQDot() method above but for use in velocity- or acceleration-level methods to which no explicit "constrained qdot" argument is supplied. More... | |
Real | getOneQDotDot (const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQDotDot, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const |
Use this method in your calcPositionDotDotErrors() implementation to extract the value of a particular generalized coordinate second derivative qdotdot selected by (mobilizer,whichQ), from the "constrained qdotdot" argument that is passed to the method from Simbody. More... | |
Real | getOneU (const State &state, const Array_< Real, ConstrainedUIndex > &constrainedU, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const |
Use this method in your calcVelocityErrors() implementation to extract the value of a particular generalized speed u selected by (mobilizer,whichU), from the "constrained u" argument that is passed to the method from Simbody. More... | |
Real | getOneUFromState (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const |
Same as the getOneU() method but for use in velocity- or acceleration- level methods to which no explicit "constrained u" argument is supplied. More... | |
Real | getOneUDot (const State &state, const Array_< Real, ConstrainedUIndex > &constrainedUDot, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const |
Use this method in your calcVelocityDotErrors() and calcAccelerationErrors() implementations to extract the value of a particular generalized speed derivative udot selected by (mobilizer,whichU), from the "constrained udot" argument that is passed to these two methods from Simbody. More... | |
void | addInOneMobilityForce (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU, Real fu, Array_< Real, ConstrainedUIndex > &mobilityForces) const |
Apply a scalar generalized (mobility-space) force fu to a particular mobility of one of this Constraint's Constrained Mobilizers, adding it in to the appropriate slot of the mobilityForces vector, which is of length getNumConstrainedU() for this Constraint. More... | |
void | addInOneQForce (const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ, Real fq, Array_< Real, ConstrainedQIndex > &qForces) const |
For use with holonomic (position) constraints, this method allows generalized forces to be applied in "q-space" rather than "u-space". More... | |
Methods for use with Constrained Bodies | |
When a constraint is enforced (at least in part) by applying forces to bodies, use the methods in this section to access position, velocity, and acceleration information about those constrained bodies. Note that you can pull higher-level information from the state, but information at the current level for a method must be taken from the supplied arguments instead. For example, if you are writing an acceleration error routine, you can get time, position, and velocity information from the state but must get acceleration information from the body accelerations that are supplied by Simbody as arguments. | |
const Transform & | getBodyTransform (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const |
Extract from the allX_AB argument the spatial transform X_AB giving the pose (orientation and location) of a Constrained Body B's body frame B in this constraint's Ancestor frame A. More... | |
const Rotation & | getBodyRotation (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyTransform() that returns just the orientation as the Rotation matrix R_AB. More... | |
const Vec3 & | getBodyOriginLocation (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyTransform() that returns just the location part of B's pose in A, that is the vector p_AB from A's origin Ao to B's origin Bo, expressed in A. More... | |
const Transform & | getBodyTransformFromState (const State &state, ConstrainedBodyIndex B) const |
Extract from the State cache the spatial transform X_AB giving the pose (orientation and location) of a Constrained Body B's body frame B in this constraint's Ancestor frame A. More... | |
const Rotation & | getBodyRotationFromState (const State &state, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyTransformFromState() that returns just the orientation as the Rotation matrix R_AB. More... | |
const Vec3 & | getBodyOriginLocationFromState (const State &state, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyTransformFromState() that returns just the location part of B's pose in A, that is the vector p_AB from A's origin Ao to B's origin Bo, expressed in A. More... | |
const SpatialVec & | getBodyVelocity (const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const |
Extract from the allV_AB argument the spatial velocity V_AB giving the angular and linear velocity of a Constrained Body B's body frame B measured and expressed in this constraint's Ancestor frame A. More... | |
const Vec3 & | getBodyAngularVelocity (const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyVelocity() that returns just the angular velocity vector w_AB. More... | |
const Vec3 & | getBodyOriginVelocity (const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyVelocity() that returns just the linear velocity vector v_AB. More... | |
const SpatialVec & | getBodyVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const |
Extract from the State cache the spatial velocity V_AB giving the angular and linear velocity of a Constrained Body B's body frame B measured and expressed in this Constraint's Ancestor frame A. More... | |
const Vec3 & | getBodyAngularVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyVelocityFromState() that returns just the angular velocity vector w_AB. More... | |
const Vec3 & | getBodyOriginVelocityFromState (const State &state, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyVelocityFromState() that returns just the linear velocity vector v_AB. More... | |
const SpatialVec & | getBodyAcceleration (const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const |
Extract from the allA_AB argument the spatial acceleration A_AB giving the angular and linear acceleration of a Constrained Body B's body frame B measured and expressed in this constraint's Ancestor frame A. More... | |
const Vec3 & | getBodyAngularAcceleration (const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyAcceleration() that returns just the angular acceleration vector b_AB. More... | |
const Vec3 & | getBodyOriginAcceleration (const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const |
Convenient inline interface to getBodyAcceleration() that returns just the linear acceleration vector a_AB. More... | |
Vec3 | findStationLocation (const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const |
Calculate the position p_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo to the point S, expressed in the B frame. More... | |
Vec3 | findStationLocationFromState (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const |
Same as findStationLocation() but for when you have to get the position information from the state rather than from an explicit argument. More... | |
Vec3 | findStationVelocity (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const |
Calculate the velocity v_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo to the point S, expressed in the B frame. More... | |
Vec3 | findStationVelocityFromState (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const |
Same as findStationVelocity() but for when you have to get the velocity information from the state rather than from an explicit argument. More... | |
Vec3 | findStationAcceleration (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const |
Calculate the acceleration a_AS in the Ancestor frame of a station S of a Constrained Body B, specified with the position vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo to the point S, expressed in the B frame. More... | |
void | addInStationForce (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS, const Vec3 &forceInA, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA) const |
Apply an Ancestor-frame force to a B-frame station S given by the position vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo to the point S, expressed in the B frame, adding to the appropriate bodyForcesInA entry for this ConstrainedBody B. More... | |
void | addInBodyTorque (const State &state, ConstrainedBodyIndex bodyB, const Vec3 &torqueInA, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA) const |
Apply an Ancestor-frame torque to body B, adding to the appropriate bodyForcesInA entry for this ConstrainedBody B. More... | |
Utility methods | |
These provide access to quantities associated with this constraint, suitable for use in the optional realize() virtual methods. | |
void | getMultipliers (const State &state, Array_< Real > &multipliers) const |
Given a state as passed to your realizeAcceleration() implementation, obtain the multipliers that Simbody just calculated for this Constraint. More... | |
Public Member Functions inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl > | |
bool | isEmptyHandle () const |
Returns true if this handle is empty, that is, does not refer to any implementation object. More... | |
bool | isOwnerHandle () const |
Returns true if this handle is the owner of the implementation object to which it refers. More... | |
bool | isSameHandle (const Implementation &other) const |
Determine whether the supplied handle is the same object as "this" PIMPLHandle. More... | |
void | disown (Implementation &newOwner) |
Give up ownership of the implementation to an empty handle. More... | |
PIMPLHandle & | referenceAssign (const Implementation &source) |
"Copy" assignment but with shallow (pointer) semantics. More... | |
PIMPLHandle & | copyAssign (const Implementation &source) |
This is real copy assignment, with ordinary C++ object ("value") semantics. More... | |
void | clearHandle () |
Make this an empty handle, deleting the implementation object if this handle is the owner of it. More... | |
const ImplementationImpl & | getImpl () const |
Get a const reference to the implementation associated with this Handle. More... | |
ImplementationImpl & | updImpl () |
Get a writable reference to the implementation associated with this Handle. More... | |
int | getImplHandleCount () const |
Return the number of handles the implementation believes are referencing it. More... | |
Protected Member Functions | |
virtual void | calcDecorativeGeometryAndAppend (const State &s, Stage stage, Array_< DecorativeGeometry > &geom) const |
Implement this optional method if you would like your constraint to generate any suggestions for geometry that could be used as default visualization as an aid to understanding a system containing this constraint. More... | |
Optional realize() virtual methods | |
Provide implementations of these methods if you want to allocate State variables (such as modeling options or parameters) or want to pre-calculate some expensive quantities and store them in the State cache for your future use. Note that the Position, Velocity, and Acceleration-stage realize methods will be called after the constraint error calculating methods associated with this Constraint's constraint equations have been used by Simbody to perform any constraint calculations. That means, for example, you can access calculated multipliers from your realizeAcceleration() method. | |
virtual void | realizeTopology (State &) const |
The Matter Subsystem's realizeTopology() method will call this method after all MobilizedBody topology has been processed. More... | |
virtual void | realizeModel (State &) const |
The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Model-stage processing has been done. More... | |
virtual void | realizeInstance (const State &) const |
The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instance-stage processing has been done. More... | |
virtual void | realizeTime (const State &) const |
The Matter Subsystem's realizeTime() method will call this method after any MobilizedBody Time-stage processing has been done. More... | |
virtual void | realizePosition (const State &) const |
The Matter Subsystem's realizePosition() method will call this method after any MobilizedBody Position-stage processing has been done, and after the call has been made to your calcPositionErrors() operator. More... | |
virtual void | realizeVelocity (const State &) const |
The Matter Subsystem's realizeVelocity() method will call this method after any MobilizedBody Velocity-stage processing has been done, and after your calcVelocityErrors() and calcPositionDotErrors() operators have been called. More... | |
virtual void | realizeDynamics (const State &) const |
The Matter Subsystem's realizeDynamics() method will call this method after any MobilizedBody Dynamics-stage processing has been done. More... | |
virtual void | realizeAcceleration (const State &) const |
The Matter Subsystem's realizeAcceleration() method will call this method after any MobilizedBody Acceleration-stage processing has been done, and after your calcAccelerationErrors(), calcVelocityDotErrors(), and calcPositionDotDotErrors() operators have been called. More... | |
virtual void | realizeReport (const State &) const |
The Matter Subsystem's realizeReport() method will call this method after any MobilizedBody Report-stage processing has been done. More... | |
Position (Holonomic) Constraint Virtuals | |
These must be defined if there are any position (holonomic) constraint equations generated by this Constraint. | |
virtual void | calcPositionErrors (const State &state, const Array_< Transform, ConstrainedBodyIndex > &X_AB, const Array_< Real, ConstrainedQIndex > &constrainedQ, Array_< Real > &perr) const |
Calculate the mp position-constraint errors due to the position-level specification of a holonomic constraint and write them to perr, which will have been allocated to length mp; do not reallocate it. More... | |
virtual void | calcPositionDotErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &V_AB, const Array_< Real, ConstrainedQIndex > &constrainedQDot, Array_< Real > &pverr) const |
Calculate the mp velocity errors arising from the first time derivative of the position-level holonomic constraint function calcPositionErrors(), and write them to pverr, which will have been allocated to length mp; do not reallocate it. More... | |
virtual void | calcPositionDotDotErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedQIndex > &constrainedQDotDot, Array_< Real > &paerr) const |
Calculate the mp errors arising from the second time derivative of the position-level holonomic constraint function calcPositionErrors(), and write them to paerr, which will have been allocated to length mp; do not reallocate it. More... | |
virtual void | addInPositionConstraintForces (const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedQIndex > &qForces) const |
From the mp supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Qs. More... | |
Velocity (Nonholonomic) Constraint Virtuals | |
These must be defined if there are any velocity (nonholonomic) constraint equations generated by this Constraint. | |
virtual void | calcVelocityErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &V_AB, const Array_< Real, ConstrainedUIndex > &constrainedU, Array_< Real > &verr) const |
Calculate the mv velocity-constraint errors due to the velocity-level specification of a nonholonomic constraint and write them to verr, which will already have been allocated to length mv; do not reallocate it. More... | |
virtual void | calcVelocityDotErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedUIndex > &constrainedUDot, Array_< Real > &vaerr) const |
Calculate the mv errors arising from the first time derivative of the velocity-level specification of a nonholonomic constraint and write them to vaerr, which will already have been allocated to length mv; do not reallocate it. More... | |
virtual void | addInVelocityConstraintForces (const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedUIndex > &mobilityForces) const |
From the mv supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Mobilities due to its velocity-level (nonholonomic) constraints. More... | |
Acceleration-Only Constraint Virtuals | |
These must be defined if there are any acceleration-only constraint equations generated by this Constraint. | |
virtual void | calcAccelerationErrors (const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedUIndex > &constrainedUDot, Array_< Real > &aerr) const |
Calculate the ma acceleration-constraint errors due to the specification of an acceleration-only constraint and write them to aerr, which will already have been allocated to length ma; do not reallocate it. More... | |
virtual void | addInAccelerationConstraintForces (const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedUIndex > &mobilityForces) const |
From the ma supplied Lagrange multipliers provided in multipliers, calculate the forces produced by this Constraint on its Constrained Bodies and Constrained Mobilities due to its acceleration-only constraints. More... | |
Protected Member Functions inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl > | |
PIMPLHandle () | |
The default constructor makes this an empty handle. More... | |
PIMPLHandle (ImplementationImpl *p) | |
This provides consruction of a handle referencing an existing implementation object. More... | |
PIMPLHandle (const PIMPLHandle &source) | |
The copy constructor makes either a deep (value) or shallow (reference) copy of the supplied source PIMPL object, based on whether this is a "pointer
semantics" (PTR=true) or "object (value) semantics" (PTR=false, default) class. More... | |
~PIMPLHandle () | |
Note that the destructor is non-virtual. More... | |
PIMPLHandle & | operator= (const PIMPLHandle &source) |
Copy assignment makes the current handle either a deep (value) or shallow (reference) copy of the supplied source PIMPL object, based on whether this is a "pointer sematics" (PTR=true) or "object (value) semantics" (PTR=false, default) class. More... | |
void | setImpl (ImplementationImpl *p) |
Set the implementation for this empty handle. More... | |
bool | hasSameImplementation (const Implementation &other) const |
Determine whether the supplied handle is a reference to the same implementation object as is referenced by "this" PIMPLHandle. More... | |
Friends | |
class | Constraint::CustomImpl |
Additional Inherited Members | |
Public Types inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl > | |
typedef PIMPLHandle< Implementation, ImplementationImpl, 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 acceleration-only 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 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.
[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. |
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.
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.
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 acceleration-level 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 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.
|
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 Model-stage 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 Instance-stage 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 Time-stage 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 Position-stage 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 Velocity-stage 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 Dynamics-stage 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 Acceleration-stage 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 Report-stage processing has been done.
This gives the Constraint a chance to
|
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.
|
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().
|
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().
|
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.
|
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.
|
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().
|
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.
|
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.
|
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.
|
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 |