Simbody
3.6

This is the implementation class for Custom mobilizers. 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 nu, int nq, int nAngles=0)  
This Implementation base class constructor sets the topological defaults for the number of mobilities (generalized speeds) u, the number of generalized coordinates q, and the number of those q's that are angles. More...  
Vector  getQ (const State &s) const 
Return a Vector containing all the generalized coordinates q currently in use by this mobilizer. More...  
Vector  getU (const State &s) const 
Return a Vector containing all the generalized speeds u currently in use by this mobilizer. More...  
Vector  getQDot (const State &s) const 
Return a Vector containing all the generalized coordinate derivatives qdot currently in use by this mobilizer. More...  
Vector  getUDot (const State &s) const 
Return a Vector containing all the generalized accelerations udot currently in use by this mobilizer. More...  
Vector  getQDotDot (const State &s) const 
Return a Vector containing all the generalized coordinate second derivatives qdotdot currently in use by this mobilizer. More...  
Transform  getMobilizerTransform (const State &s) const 
Get the crossmobilizer transform X_FM, the body's "moving" mobilizer frame M measured and expressed in the parent body's corresponding "fixed" frame F. More...  
SpatialVec  getMobilizerVelocity (const State &s) const 
Get the crossmobilizer velocity V_FM, the relative velocity of this body's "moving" mobilizer frame M in the parent body's corresponding "fixed" frame F, measured and expressed in F. More...  
bool  getUseEulerAngles (const State &s) const 
Get whether rotations are being represented as quaternions or Euler angles. More...  
void  invalidateTopologyCache () const 
Call this if you want to make sure that the next realizeTopology() call does something. More...  
MobilizedBody Virtuals  
These must be defined for any Custom MobilizedBody. Note that the numbers nu, nq, and nAngles are passed in to these routines for redundancy – you should make sure they have the values you are expecting!  
virtual Transform  calcMobilizerTransformFromQ (const State &s, int nq, const Real *q) const =0 
Given values for this mobilizer's nq generalized coordinates q, compute X_FM(q), that is, the crossmobilizer spatial Transform giving the configuration of the "moving" frame M fixed to the outboard (child) body B in the "fixed" frame F attached to the inboard (parent) body P. More...  
virtual SpatialVec  multiplyByHMatrix (const State &s, int nu, const Real *u) const =0 
Calculate V_FM(u) = H*u where H=H(q) is the joint transition matrix mapping the mobilities to the relative spatial velocity between the F frame on the parent to the M frame on the child. More...  
virtual void  multiplyByHTranspose (const State &s, const SpatialVec &F, int nu, Real *f) const =0 
Calculate f = ~H*F where F is a spatial force (torque+force) and f is its mapping onto the mobilities. More...  
virtual SpatialVec  multiplyByHDotMatrix (const State &s, int nu, const Real *u) const =0 
Calculate A0_FM = HDot*u where HDot=HDot(q,u) is the time derivative of H. More...  
virtual void  multiplyByHDotTranspose (const State &s, const SpatialVec &F, int nu, Real *f) const =0 
Calculate f = ~HDot*F where F is a spatial vector and f is its mapping onto the mobilities. More...  
virtual void  multiplyByN (const State &s, bool transposeMatrix, int nIn, const Real *in, int nOut, Real *out) const 
Calculate out_q = N(q)*in_u (e.g., qdot=N*u) or out_u = ~N*in_q. More...  
virtual void  multiplyByNInv (const State &s, bool transposeMatrix, int nIn, const Real *in, int nOut, Real *out) const 
Calculate out_u = NInv(q)*in_q (e.g., u=NInv*qdot) or out_q = ~NInv*in_u. More...  
virtual void  multiplyByNDot (const State &s, bool transposeMatrix, int nIn, const Real *in, int nOut, Real *out) const 
Calculate out_q = NDot(q)*in_u or out_u = ~NDot*in_q. More...  
virtual void  setQToFitTransform (const State &, const Transform &X_FM, int nq, Real *q) const 
Find a set of q's for this mobilizer that best approximate the supplied Transform which requests a particular relative orientation and translation between the "fixed" and "moving" frames connected by this mobilizer. More...  
virtual void  setUToFitVelocity (const State &, const SpatialVec &V_FM, int nu, Real *u) const 
Find a set of u's (generalized speeds) for this mobilizer that best approximate the supplied spatial velocity V_FM which requests the relative angular and linear velocity between the "fixed" and "moving" frames connected by this mobilizer. More...  
virtual void  calcDecorativeGeometryAndAppend (const State &s, Stage stage, Array_< DecorativeGeometry > &geom) const 
Implement this optional method if you would like your MobilizedBody to generate any suggestions for geometry that could be used as default visualization as an aid to understanding a system containing this MobilizedBody. 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 and Velocity realize methods will be called before calling the matrix operator methods for this MobilizedBody. That way if you want to precalculate the H or HDot matrix, for example, you can do so in realizePosition() or realizeVelocity() and then use it in multiplyByHMatrix(), etc.  
virtual void  realizeTopology (State &) const 
The Matter Subsystem's realizeTopology() method will call this method along with the builtin MobilizedBodies' realizeTopology() methods. More...  
virtual void  realizeModel (State &) const 
The Matter Subsystem's realizeModel() method will call this method along with the builtin MobilizedBodies' realizeModel() methods. More...  
virtual void  realizeInstance (const State &) const 
The Matter Subsystem's realizeInstance() method will call this method along with the builtin MobilizedBodies' realizeInstance() methods. More...  
virtual void  realizeTime (const State &) const 
The Matter Subsystem's realizeTime() method will call this method along with the builtin MobilizedBodies' realizeTime() methods. More...  
virtual void  realizePosition (const State &) const 
The Matter Subsystem's realizePosition() method will call this method along with the builtin MobilizedBodies' realizePosition() methods. More...  
virtual void  realizeVelocity (const State &) const 
The Matter Subsystem's realizeVelocity() method will call this method along with the builtin MobilizedBodies' realizeVelocity() methods. More...  
virtual void  realizeDynamics (const State &) const 
The Matter Subsystem's realizeDynamics() method will call this method along with the builtin MobilizedBodies' realizeDynamics() methods. More...  
virtual void  realizeAcceleration (const State &) const 
The Matter Subsystem's realizeAcceleration() method will call this method along with the builtin MobilizedBodies' realizeAcceleration() methods. More...  
virtual void  realizeReport (const State &) const 
The Matter Subsystem's realizeReport() method will call this method along with the builtin MobilizedBodies' realizeReport() methods. 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...  
Friends  
class  MobilizedBody::CustomImpl 
Additional Inherited Members  
Public Types inherited from SimTK::PIMPLHandle< Implementation, ImplementationImpl >  
typedef PIMPLHandle< Implementation, ImplementationImpl, false >  HandleBase 
typedef HandleBase  ParentHandle 
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...  
This is the implementation class for Custom mobilizers.

virtual 
Destructor is virtual so derived classes get a chance to clean up if necessary.
SimTK::MobilizedBody::Custom::Implementation::Implementation  (  SimbodyMatterSubsystem &  , 
int  nu,  
int  nq,  
int  nAngles = 0 

) 
This Implementation base class constructor sets the topological defaults for the number of mobilities (generalized speeds) u, the number of generalized coordinates q, and the number of those q's that are angles.
There can be up to 3 angular coordinates (which must be measured in radians). You also can specify 4 as the number of angles, which is interpreted to mean the the mobilizer uses a quaternion to represent orientation. Because quaternions are not appropriate for some calculations, however, the user may globally disable them by calling setUseEulerAngles() on the SimbodyMatterSubsystem. Therefore, if you specify nAngles=4, the actual number of angular state variables may be either 3 (a set of Euler angles) or 4 (quaternion components), and the total number of state variables could be either nq1 or nq. Before interpreting the state variables, you must first call getUseEulerAngles() to determine which representation is in use.
In any case, if there are any angular coordinates they must be the first coordinates in the array of q's associated with this mobilizer. Translational or other q's will immediately follow the angular ones. This permits Simbody to handle quaternion normalization and conversion automatically, and to find angles which need to have their sines and cosines calculated.
NOTE: if you don't say there are any angles, you can manage things yourself. However, there is no way to get quaternions normalized and converted if you don't tell Simbody about them.

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.
Return a Vector containing all the generalized coordinates q currently in use by this mobilizer.
Note that if this mobilizer uses quaternions, the number of q's will depend on whether quaternions are currently enabled. Call getUseEulerAngles() to check this.
Return a Vector containing all the generalized speeds u currently in use by this mobilizer.
Return a Vector containing all the generalized coordinate derivatives qdot currently in use by this mobilizer.
Note that if this mobilizer uses quaternions, the number of q's will depend on whether quaternions are currently enabled. Call getUseEulerAngles() to check this.
Return a Vector containing all the generalized accelerations udot currently in use by this mobilizer.
Return a Vector containing all the generalized coordinate second derivatives qdotdot currently in use by this mobilizer.
Note that if this mobilizer uses quaternions, the number of q's will depend on whether quaternions are currently enabled. Call getUseEulerAngles() to check this.
Transform SimTK::MobilizedBody::Custom::Implementation::getMobilizerTransform  (  const State &  s  )  const 
Get the crossmobilizer transform X_FM, the body's "moving" mobilizer frame M measured and expressed in the parent body's corresponding "fixed" frame F.
The state must have been realized to at least Position stage. Note: this refers to F and M as defined, not as they are if the mobilizer has been reversed (that is, we're really returning X_F0M0 here).
SpatialVec SimTK::MobilizedBody::Custom::Implementation::getMobilizerVelocity  (  const State &  s  )  const 
Get the crossmobilizer velocity V_FM, the relative velocity of this body's "moving" mobilizer frame M in the parent body's corresponding "fixed" frame F, measured and expressed in F.
Note that this isn't the usual spatial velocity since it isn't expressed in G. The state must have been realized to at least Velocity stage. Note: this refers to F and M as defined, not as they are if the mobilizer has been reversed (that is, we're really returning V_F0M0 here).
bool SimTK::MobilizedBody::Custom::Implementation::getUseEulerAngles  (  const State &  s  )  const 
Get whether rotations are being represented as quaternions or Euler angles.
This method is only relevant if the constructor was invoked with nAngles==4. If this returns false, the first four q's should be interpreted as the components of a (possibly not normalized) quaternion. If it returns true, the first three q's should be interpreted as Euler angles.
Note that the total number of state variables is one less when using Euler angles than when using quaternions.
void SimTK::MobilizedBody::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 MobilizedBody in ways understood by Simbody. 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.

pure virtual 
Given values for this mobilizer's nq generalized coordinates q, compute X_FM(q), that is, the crossmobilizer spatial Transform giving the configuration of the "moving" frame M fixed to the outboard (child) body B in the "fixed" frame F attached to the inboard (parent) body P.
The state is guaranteed to have been realized to at least Instance stage. Caution: if your mobilizer has a quaternion, the four q's will not necessarily be normalized here but you must normalize them before converting them to a Rotation. Casting them to a Quaternion will do that automatically.

pure virtual 
Calculate V_FM(u) = H*u where H=H(q) is the joint transition matrix mapping the mobilities to the relative spatial velocity between the F frame on the parent to the M frame on the child.
The state is guaranteed to have been realized to at least Position stage.
IMPORTANT – H should depend only on X_FM(q), not directly on q, since different sets of q's can generate the same Transform (e.g. quaternions and Euler angles). You can call getMobilizerTransform(s) to get the already calculated Transform.
EVEN MORE IMPORTANT – H here must be the same as the H^T used in multiplyByHTranspose(), and the HDot methods must use the time derivative of H.
Note: the "H" we're using here is the transpose of what is used in Schwieter's IVM paper and in all of Abhi Jain's papers. That's because Jain used H^T as the joint kinematics Jacobian, with H being the force transmission matrix which no mobilizerwriting user is going to be thinking about.

pure virtual 
Calculate f = ~H*F where F is a spatial force (torque+force) and f is its mapping onto the mobilities.
IMPORTANT – H should depend only on X_FM(q), not directly on q, since different sets of q's can generate the same Transform (e.g. quaternions and Euler angles). You can call getMobilizerTransform(s) to get the already calculated Transform. H here must match H and HDot in the other methods for this mobilizer.

pure virtual 
Calculate A0_FM = HDot*u where HDot=HDot(q,u) is the time derivative of H.
This calculates the "bias acceleration" due to coriolis effects, such that the full crossmobilizer acceleration is A_FM=A0_FM + H*udot. The state is guaranteed to have been realized to at least Velocity stage.
IMPORTANT – HDot should depend only on X_FM(q) and V_FM(q,u), not directly on q or u, since different choices of coordinates can generate the same X and V, but all such choices must produce the same H and HDot. You can call getMobilizerTransform(s) to get the already calculated Transform, and getMobilizerVelocity(s) to get the already calculated velocity.

pure virtual 
Calculate f = ~HDot*F where F is a spatial vector and f is its mapping onto the mobilities.
The state is guaranteed to have been realized to at least Velocity stage.
IMPORTANT – HDot should depend only on X_FM(q) and V_FM(q,u), not directly on q or u, since different choices of coordinates can generate the same X and V, but all such choices must produce the same H and HDot. You can call getMobilizerTransform(s) to get the already calculated Transform, and getMobilizerVelocity(s) to get the already calculated velocity.

virtual 
Calculate out_q = N(q)*in_u (e.g., qdot=N*u) or out_u = ~N*in_q.
Note that one of "in" and "out" is always "qlike" while the other is "ulike", but which is which changes if the matrix is transposed. Note that the transposed operation here is the same as multiplying by N on the right, with the Vectors viewed as RowVectors instead. The default implementation assumes that N is an identity matrix, and will only work if nq=nu=nIn=nOut and nAngles < 4 (i.e., no quaternions). If this is true for your mobilizer, you do not need to implement this method.
The state is guaranteed to have been realized to at least Position stage.

virtual 
Calculate out_u = NInv(q)*in_q (e.g., u=NInv*qdot) or out_q = ~NInv*in_u.
Note that one of "in" and "out" is always "qlike" while the other is "ulike", but which is which changes if the matrix is transposed. Note that the transposed operation here is the same as multiplying by NInv on the right, with the Vectors viewed as RowVectors instead. The default implementation assumes that NInv is an identity matrix, and will only work if nq=nu=nIn=nOut and nAngles < 4 (i.e., no quaternions). If this is true for your mobilizer, you do not need to implement this method.
The state is guaranteed to have been realized to at least Position stage.

virtual 
Calculate out_q = NDot(q)*in_u or out_u = ~NDot*in_q.
Note that one of "in" and "out" is always "qlike" while the other is "ulike", but which is which changes if the matrix is transposed. Note that the transposed operation here is the same as multiplying by NDot on the right, with the Vectors viewed as RowVectors instead. The default implementation assumes that NDot is zero, and will only work if nq=nu=nIn=nOut and nAngles < 4 (i.e., no quaternions) If this is true for your mobilizer, you do not need to implement this method.
The state is guaranteed to have been realized to at least Position stage.

virtual 
Find a set of q's for this mobilizer that best approximate the supplied Transform which requests a particular relative orientation and translation between the "fixed" and "moving" frames connected by this mobilizer.
The state is guaranteed to have been realized to at least Instance stage.
The default implementation uses a nonlinear optimizer to search for the best fit. Whenever possible, subclasses should override this to provide a faster and more robust implementation.

virtual 
Find a set of u's (generalized speeds) for this mobilizer that best approximate the supplied spatial velocity V_FM
which requests the relative angular and linear velocity between the "fixed" and "moving" frames connected by this mobilizer.
Routines which affect generalized speeds u depend on the generalized coordinates q already having been set; they never change these coordinates. The state is guaranteed to have been realized to at least Position stage.
The default implementation uses a nonlinear optimizer to search for the best fit. Whenever possible, subclasses should override this to provide a faster and more robust implementation.

inlinevirtual 
Implement this optional method if you would like your MobilizedBody to generate any suggestions for geometry that could be used as default visualization as an aid to understanding a system containing this MobilizedBody.
For example, if your mobilizer 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 extra geometry will be generated here.

inlinevirtual 
The Matter Subsystem's realizeTopology() method will call this method along with the builtin MobilizedBodies' realizeTopology() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeModel() method will call this method along with the builtin MobilizedBodies' realizeModel() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeInstance() method will call this method along with the builtin MobilizedBodies' realizeInstance() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeTime() method will call this method along with the builtin MobilizedBodies' realizeTime() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizePosition() method will call this method along with the builtin MobilizedBodies' realizePosition() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeVelocity() method will call this method along with the builtin MobilizedBodies' realizeVelocity() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeDynamics() method will call this method along with the builtin MobilizedBodies' realizeDynamics() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeAcceleration() method will call this method along with the builtin MobilizedBodies' realizeAcceleration() methods.
This gives the MobilizedBody a chance to

inlinevirtual 
The Matter Subsystem's realizeReport() method will call this method along with the builtin MobilizedBodies' realizeReport() methods.
This gives the MobilizedBody a chance to

friend 