//============================================================================== // 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]); } }