|
Simbody
|
|
Simbody
|
#include <MobilizedBody.h>
Inheritance diagram for SimTK::MobilizedBody::Custom::Implementation:Public Member Functions | |
| virtual | ~Implementation () |
| Destructor is virtual so derived classes get a chance to clean up if necessary. | |
| 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. | |
| 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. | |
| Vector | getQ (const State &s) const |
| Return a Vector containing all the generalized coordinates q currently in use by this mobilizer. | |
| Vector | getU (const State &s) const |
| Return a Vector containing all the generalized speeds u currently in use by this mobilizer. | |
| Vector | getQDot (const State &s) const |
| Return a Vector containing all the generalized coordinate derivatives qdot currently in use by this mobilizer. | |
| Vector | getUDot (const State &s) const |
| Return a Vector containing all the generalized accelerations udot currently in use by this mobilizer. | |
| Vector | getQDotDot (const State &s) const |
| Return a Vector containing all the generalized coordinate second derivatives qdotdot currently in use by this mobilizer. | |
| Transform | getMobilizerTransform (const State &s) const |
| Get the cross-mobilizer transform X_FM, the body's "moving" mobilizer frame M measured and expressed in the parent body's corresponding "fixed" frame F. | |
| SpatialVec | getMobilizerVelocity (const State &s) const |
| Get the cross-mobilizer 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. | |
| bool | getUseEulerAngles (const State &s) const |
| Get whether rotations are being represented as quaternions or Euler angles. | |
| void | invalidateTopologyCache () const |
| Call this if you want to make sure that the next realizeTopology() call does something. | |
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 cross-mobilizer 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
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 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 built-in MobilizedBodies' realizeTopology() methods. | |
| virtual void | realizeModel (State &) const |
| The Matter Subsystem's realizeModel() method will call this method along with the built-in MobilizedBodies' realizeModel() methods. | |
| virtual void | realizeInstance (const State &) const |
| The Matter Subsystem's realizeInstance() method will call this method along with the built-in MobilizedBodies' realizeInstance() methods. | |
| virtual void | realizeTime (const State &) const |
| The Matter Subsystem's realizeTime() method will call this method along with the built-in MobilizedBodies' realizeTime() methods. | |
| virtual void | realizePosition (const State &) const |
| The Matter Subsystem's realizePosition() method will call this method along with the built-in MobilizedBodies' realizePosition() methods. | |
| virtual void | realizeVelocity (const State &) const |
| The Matter Subsystem's realizeVelocity() method will call this method along with the built-in MobilizedBodies' realizeVelocity() methods. | |
| virtual void | realizeDynamics (const State &) const |
| The Matter Subsystem's realizeDynamics() method will call this method along with the built-in MobilizedBodies' realizeDynamics() methods. | |
| virtual void | realizeAcceleration (const State &) const |
| The Matter Subsystem's realizeAcceleration() method will call this method along with the built-in MobilizedBodies' realizeAcceleration() methods. | |
| virtual void | realizeReport (const State &) const |
| The Matter Subsystem's realizeReport() method will call this method along with the built-in MobilizedBodies' realizeReport() methods. | |
Friends | |
| class | MobilizedBody::CustomImpl |
| virtual SimTK::MobilizedBody::Custom::Implementation::~Implementation | ( | ) | [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 nq-1 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 mange things yourself. However, there is no way to get quaternions normalized and converted if you don't tell Simbody about them.
| virtual Implementation* SimTK::MobilizedBody::Custom::Implementation::clone | ( | ) | const [pure virtual] |
This method should produce a deep copy identical to the concrete derived Implementation object underlying this Implementation base class object.
Note that the result is new heap space; the caller must be sure to take ownership of the returned pointer and call delete on it when done.
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 cross-mobilizer 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 cross-mobilizer 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.
| virtual Transform SimTK::MobilizedBody::Custom::Implementation::calcMobilizerTransformFromQ | ( | const State & | s, |
| int | nq, | ||
| const Real * | q | ||
| ) | const [pure virtual] |
Given values for this mobilizer's nq generalized coordinates q, compute X_FM(q), that is, the cross-mobilizer 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.
| virtual SpatialVec SimTK::MobilizedBody::Custom::Implementation::multiplyByHMatrix | ( | const State & | s, |
| int | nu, | ||
| const Real * | u | ||
| ) | const [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 mobilizer-writing user is going to be thinking about.
| virtual void SimTK::MobilizedBody::Custom::Implementation::multiplyByHTranspose | ( | const State & | s, |
| const SpatialVec & | F, | ||
| int | nu, | ||
| Real * | f | ||
| ) | const [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.
| virtual SpatialVec SimTK::MobilizedBody::Custom::Implementation::multiplyByHDotMatrix | ( | const State & | s, |
| int | nu, | ||
| const Real * | u | ||
| ) | const [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 cross-mobilizer 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.
| virtual void SimTK::MobilizedBody::Custom::Implementation::multiplyByHDotTranspose | ( | const State & | s, |
| const SpatialVec & | F, | ||
| int | nu, | ||
| Real * | f | ||
| ) | const [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 void SimTK::MobilizedBody::Custom::Implementation::multiplyByN | ( | const State & | s, |
| bool | transposeMatrix, | ||
| int | nIn, | ||
| const Real * | in, | ||
| int | nOut, | ||
| Real * | out | ||
| ) | const [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 "q-like" while the other is "u-like", 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 void SimTK::MobilizedBody::Custom::Implementation::multiplyByNInv | ( | const State & | s, |
| bool | transposeMatrix, | ||
| int | nIn, | ||
| const Real * | in, | ||
| int | nOut, | ||
| Real * | out | ||
| ) | const [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 "q-like" while the other is "u-like", 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 void SimTK::MobilizedBody::Custom::Implementation::multiplyByNDot | ( | const State & | s, |
| bool | transposeMatrix, | ||
| int | nIn, | ||
| const Real * | in, | ||
| int | nOut, | ||
| Real * | out | ||
| ) | const [virtual] |
Calculate out_q = NDot(q)*in_u or out_u = ~NDot*in_q.
Note that one of "in" and "out" is always "q-like" while the other is "u-like", 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 void SimTK::MobilizedBody::Custom::Implementation::setQToFitTransform | ( | const State & | , |
| const Transform & | X_FM, | ||
| int | nq, | ||
| Real * | q | ||
| ) | const [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 void SimTK::MobilizedBody::Custom::Implementation::setUToFitVelocity | ( | const State & | , |
| const SpatialVec & | V_FM, | ||
| int | nu, | ||
| Real * | u | ||
| ) | const [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.
| virtual void SimTK::MobilizedBody::Custom::Implementation::calcDecorativeGeometryAndAppend | ( | const State & | s, |
| Stage | stage, | ||
| Array_< DecorativeGeometry > & | geom | ||
| ) | const [inline, virtual] |
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.
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeTopology | ( | State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeTopology() method will call this method along with the built-in MobilizedBodies' realizeTopology() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeModel | ( | State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeModel() method will call this method along with the built-in MobilizedBodies' realizeModel() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeInstance | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeInstance() method will call this method along with the built-in MobilizedBodies' realizeInstance() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeTime | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeTime() method will call this method along with the built-in MobilizedBodies' realizeTime() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizePosition | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizePosition() method will call this method along with the built-in MobilizedBodies' realizePosition() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeVelocity | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeVelocity() method will call this method along with the built-in MobilizedBodies' realizeVelocity() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeDynamics | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeDynamics() method will call this method along with the built-in MobilizedBodies' realizeDynamics() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeAcceleration | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeAcceleration() method will call this method along with the built-in MobilizedBodies' realizeAcceleration() methods.
This gives the MobilizedBody a chance to
| virtual void SimTK::MobilizedBody::Custom::Implementation::realizeReport | ( | const State & | ) | const [inline, virtual] |
The Matter Subsystem's realizeReport() method will call this method along with the built-in MobilizedBodies' realizeReport() methods.
This gives the MobilizedBody a chance to
friend class MobilizedBody::CustomImpl [friend] |
1.7.3
