//==============================================================================
// CALC REVERSE MOBILIZER HDOT_FM
//==============================================================================
// This is the default implementation for turning HDot_MF into HDot_FM.
// A mobilizer can override this to do it faster.
// We depend on H_FM having already been calculated.
//
// From the Simbody theory manual,
// HDot_FM_w = -R_FM * HDot_MF_w + w_FM_x H_FM_w
//
// HDot_FM_v = -R_FM * HDot_MF_v + w_FM_x H_FM_v
// - (p_FM_x HDot_FM_w
// + (v_FM_x - w_FM_x p_FM_x)H_FM_w)
//
// where "a_x" indicates the cross product matrix of vector a.
//
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcReverseMobilizerHDot_FM(
const SBStateDigest& sbs,
HType& HDot_FM) const
{
const SBTreePositionCache& pc = sbs.getTreePositionCache();
// Must use "upd" here rather than "get" because this is
// called during realize(Velocity).
const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
HType HDot_MF;
calcAcrossJointVelocityJacobianDot(sbs, HDot_MF);
const Rotation& R_FM = getX_FM(pc).R();
const Vec3& p_FM = getX_FM(pc).p();
const HType& H_FM = getH_FM(pc);
const Vec3& w_FM = getV_FM(vc)[0];
const Vec3& v_FM = getV_FM(vc)[1];
// Make cross product matrices.
const Mat33 p_FM_x = crossMat(p_FM); // 3 flops
const Mat33 w_FM_x = crossMat(w_FM); // 3 flops
const Mat33 v_FM_x = crossMat(v_FM); // 3 flops
const Mat33 vwp = v_FM_x - w_FM_x*p_FM_x; // 54 flops
// Initialize both rows with the first two terms above.
HDot_FM = w_FM_x*H_FM - (noR_FM ? HDot_MF : R_FM*HDot_MF); // 66*dof flops
// Add in the additional terms in the second row.
HDot_FM[1] -= p_FM_x * HDot_FM[0] + vwp * H_FM[0]; // 36*dof flops
}
void realizeInstance(const SBStateDigest& sbs) const {
// Initialize cache entries that will never be changed at later stages.
SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
updV_FM(vc) = 0;
updV_PB_G(vc) = 0;
updVD_PB_G(vc) = 0;
}
void realizeInstance(const SBStateDigest& sbs) const {
// Initialize cache entries that will never be changed at later stages.
SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
SBDynamicsCache& dc = sbs.updDynamicsCache();
SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
updY(dc) = SpatialMat(Mat33(0));
updA_GB(ac) = 0;
}
void realizeDynamics(const SBArticulatedBodyInertiaCache& abc,
const SBStateDigest& sbs) const {
// Mobilizer-specific.
const SBTreePositionCache& pc = sbs.getTreePositionCache();
const SBTreeVelocityCache& vc = sbs.getTreeVelocityCache();
SBDynamicsCache& dc = sbs.updDynamicsCache();
// Mobilizer independent.
calcJointIndependentDynamicsVel(pc,abc,vc,dc);
}
//==============================================================================
// 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);
}
void realizeVelocity(const SBStateDigest& sbs) const {
const SBTreePositionCache& pc = sbs.getTreePositionCache();
SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
calcJointIndependentKinematicsVel(pc,vc);
}
