//==============================================================================
// 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 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);
}
void realizeVelocity(const SBStateDigest& sbs) const {
const SBTreePositionCache& pc = sbs.getTreePositionCache();
SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
calcJointIndependentKinematicsVel(pc,vc);
}