//==============================================================================
// CALC H_PB_G
//==============================================================================
// Same for all mobilizers.
// CAUTION: our H matrix definition is transposed from Jain and Schwieters.
// Cost: 60 + 45*dof flops
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcParentToChildVelocityJacobianInGround(
const SBModelVars& mv,
const SBTreePositionCache& pc,
HType& H_PB_G) const
{
const HType& H_FM = getH_FM(pc);
// We want R_GF so we can reexpress the cross-joint velocity V_FB (==V_PB)
// in the ground frame, to get V_PB_G.
const Rotation& R_PF = getX_PF().R(); // fixed config of F in P
// Calculated already since we're going base to tip.
const Rotation& R_GP = getX_GP(pc).R(); // parent orientation in ground
const Rotation R_GF = (noR_PF ? R_GP : R_GP * R_PF); // 45 flops
if (noX_MB || noR_FM)
H_PB_G = R_GF * H_FM; // 3*dof flops
else {
// want r_MB_F, that is, the vector from Mo to Bo, expressed in F
const Vec3& r_MB = getX_MB().p(); // fixed
const Rotation& R_FM = getX_FM(pc).R(); // just calculated
const Vec3 r_MB_F = (noR_FM ? r_MB : R_FM*r_MB); // 15 flops
HType H_MB_F;
H_MB_F[0] = Vec3(0); // fills top row with zero
H_MB_F[1] = -r_MB_F % H_FM[0]; // 9*dof (negation not actually done)
H_PB_G = R_GF * (H_FM + H_MB_F); // 36*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);
}
//==============================================================================
// CALC H_PB_G_DOT
//==============================================================================
// Same for all mobilizers.
// CAUTION: our H matrix definition is transposed from Jain and Schwieters.
// Cost is 69 + 65*dof flops
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
calcParentToChildVelocityJacobianInGroundDot(
const SBModelVars& mv,
const SBTreePositionCache& pc,
const SBTreeVelocityCache& vc,
HType& HDot_PB_G) const
{
const HType& H_FM = getH_FM(pc);
const HType& HDot_FM = getHDot_FM(vc);
// We want R_GF so we can reexpress the cross-joint velocity V_FB (==V_PB)
// in the ground frame, to get V_PB_G.
const Rotation& R_PF = getX_PF().R(); // fixed config of F in P
// Calculated already since we're going base to tip.
const Rotation& R_GP = getX_GP(pc).R(); // parent orientation in ground
const Rotation R_GF = (noR_PF ? R_GP : R_GP * R_PF); // 45 flops (TODO: again??)
const Vec3& w_GF = getV_GP(vc)[0]; // F and P have same angular velocity
// Note: time derivative of R_GF is crossMat(w_GF)*R_GF.
// H = H_PB_G = R_GF * (H_FM + H_MB_F) (see above method)
const HType& H_PB_G = getH(pc);
if (noX_MB || noR_FM)
HDot_PB_G = R_GF * HDot_FM // 48*dof
+ HType(w_GF % H_PB_G[0],
w_GF % H_PB_G[1]);
else {
// want r_MB_F, that is, the vector from OM to OB, expressed in F
const Vec3& r_MB = getX_MB().p(); // fixed
const Rotation& R_FM = getX_FM(pc).R(); // just calculated
const Vec3 r_MB_F = (noR_FM ? r_MB : R_FM*r_MB); // 15 flops
const Vec3& w_FM = getV_FM(vc)[0]; // local angular velocity
HType HDot_MB_F;
HDot_MB_F[0] = Vec3(0);
HDot_MB_F[1] = -r_MB_F % HDot_FM[0] // 21*dof + 9 flops
- (w_FM % r_MB_F) % H_FM[0];
HDot_PB_G = R_GF * (HDot_FM + HDot_MB_F) // 54*dof
+ HType(w_GF % H_PB_G[0],
w_GF % H_PB_G[1]);
}
}
