//============================================================================== // CALC BODY ACCELERATIONS FROM UDOT //============================================================================== // This method calculates: // A_GB = J*udot + Jdot*u // in O(N) time, where udot is supplied as an argument and Jdot*u is the // coriolis acceleration which we get from the velocity cache. This also serves // as pass 1 for inverse dynamics. // // This must be called base to tip. The cost is 12*dof + 18 flops. template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>:: calcBodyAccelerationsFromUdotOutward( const SBTreePositionCache& pc, const SBTreeVelocityCache& vc, const Real* allUDot, SpatialVec* allA_GB) const { const Vec<dof>& udot = fromU(allUDot); SpatialVec& A_GB = allA_GB[nodeNum]; // Shift parent's A_GB outward. (Ground A_GB is zero.) 12 flops. const SpatialVec A_GP = ~getPhi(pc) * allA_GB[parent->getNodeNum()]; // 12*dof+6 flops. A_GB = A_GP + getH(pc)*udot + getMobilizerCoriolisAcceleration(vc); }
// // Calculate acceleration in internal coordinates, based on the last set // of forces that were reduced into epsilon (e.g., see above). // Base to tip: temp allA_GB does not need to be initialized before // beginning the iteration. // template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass2Outward( const SBInstanceCache& ic, const SBTreePositionCache& pc, const SBArticulatedBodyInertiaCache& abc, const SBTreeVelocityCache& vc, const SBDynamicsCache& dc, const Real* allEpsilon, SpatialVec* allA_GB, Real* allUDot, Real* allTau) const { const Vec<dof>& eps = fromU(allEpsilon); SpatialVec& A_GB = allA_GB[nodeNum]; Vec<dof>& udot = toU(allUDot); // pull out this node's udot const bool isPrescribed = isUDotKnown(ic); const HType& H = getH(pc); const PhiMatrix& phi = getPhi(pc); const ArticulatedInertia& P = getP(abc); const SpatialVec& a = getMobilizerCoriolisAcceleration(vc); const Mat<dof,dof>& DI = getDI(abc); const HType& G = getG(abc); // Shift parent's acceleration outward (Ground==0). 12 flops const SpatialVec& A_GP = allA_GB[parent->getNodeNum()]; const SpatialVec APlus = ~phi * A_GP; if (isPrescribed) { Vec<dof>& tau = updTau(ic,allTau); // pull out this node's tau // This is f - ~H(P*APlus + z); compare Jain 16.17b. Note sign // change since our tau is on the LHS while his is on the RHS. tau = eps - ~H*(P*APlus); // 66 + 12*dof flops } else { udot = DI*eps - ~G*APlus; // 2*dof^2 + 11*dof } A_GB = APlus + H*udot + a; }
void calcUDotPass2Outward( const SBInstanceCache&, const SBTreePositionCache& pc, const SBArticulatedBodyInertiaCache&, const SBTreeVelocityCache& vc, const SBDynamicsCache& dc, const Real* allEpsilon, SpatialVec* allA_GB, Real* allUDot, Real* allTau) const { SpatialVec& A_GB = allA_GB[nodeNum]; const PhiMatrix& phi = getPhi(pc); const SpatialVec& a = getMobilizerCoriolisAcceleration(vc); // Shift parent's acceleration outward (Ground==0). 12 flops const SpatialVec& A_GP = allA_GB[parent->getNodeNum()]; const SpatialVec APlus = ~phi * A_GP; A_GB = APlus + a; // no udot for weld }