//==============================================================================
// 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
}