//==============================================================================
// CALC REVERSE MOBILIZER H_FM
//==============================================================================
// This is the default implementation for turning H_MF into H_FM.
// A mobilizer can override this to do it faster.
// From the Simbody theory manual,
// H_FM_w = - R_FM*H_MF_w
// H_FM_v = -(R_FM*H_MF_v + p_FM x H_FM_w)
//
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcReverseMobilizerH_FM(
const SBStateDigest& sbs,
HType& H_FM) const
{
// Must use "upd" here rather than "get" because this is
// called during realize(Position).
const SBTreePositionCache& pc = sbs.updTreePositionCache();
HType H_MF;
calcAcrossJointVelocityJacobian(sbs, H_MF);
const Rotation& R_FM = getX_FM(pc).R();
const Vec3& p_FM = getX_FM(pc).p();
// Make cross product matrix (3 flops). Saves a few flops (3*dof) to
// transpose this and get the negative of the cross product matrix.
const Mat33 np_FM_x = ~crossMat(p_FM);
if (noR_FM) {
H_FM[0] = -H_MF[0];
H_FM[1] = np_FM_x * H_FM[0] - H_MF[1]; // 15*dof flops
}
else {
H_FM[0] = -(R_FM * H_MF[0]); // 18*dof flops
H_FM[1] = np_FM_x * H_FM[0] - R_FM*H_MF[1]; // 33*dof flops
}
}
void realizePosition(const SBStateDigest& sbs) const {
SBTreePositionCache& pc = sbs.updTreePositionCache();
const Transform& X_MB = getX_MB(); // fixed
const Transform& X_PF = getX_PF(); // fixed
const Transform& X_GP = getX_GP(pc); // already calculated
updX_FM(pc).setToZero();
updX_PB(pc) = X_PF * X_MB;
updX_GB(pc) = X_GP * getX_PB(pc);
const Vec3 p_PB_G = getX_GP(pc).R() * getX_PB(pc).p();
// The Phi matrix conveniently performs child-to-parent (inward) shifting
// on spatial quantities (forces); its transpose does parent-to-child
// (outward) shifting for velocities.
updPhi(pc) = PhiMatrix(p_PB_G);
// Calculate spatial mass properties. That means we need to transform
// the local mass moments into the Ground frame and reconstruct the
// spatial inertia matrix Mk.
const Rotation& R_GB = getX_GB(pc).R();
const Vec3& p_GB = getX_GB(pc).p();
// reexpress inertia in ground (57 flops)
const UnitInertia G_Bo_G = getUnitInertia_OB_B().reexpress(~R_GB);
const Vec3 p_BBc_G = R_GB*getCOM_B(); // 15 flops
updCOM_G(pc) = p_GB + p_BBc_G; // 3 flops
// Calc Mk: the spatial inertia matrix about the body origin.
// Note: we need to calculate this now so that we'll be able to calculate
// kinetic energy without going past the Velocity stage.
updMk_G(pc) = SpatialInertia(getMass(), p_BBc_G, G_Bo_G);
}