void calcUDotPass1Inward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache&,
const SBDynamicsCache& dc,
const Real* jointForces,
const SpatialVec* bodyForces,
const Real* allUDot,
SpatialVec* allZ,
SpatialVec* allZPlus,
Real* allEpsilon) const
{
const SpatialVec& myBodyForce = bodyForces[nodeNum];
SpatialVec& z = allZ[nodeNum];
SpatialVec& zPlus = allZPlus[nodeNum];
z = getMobilizerCentrifugalForces(dc) - myBodyForce;
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
z += phiChild * zPlusChild; // 18 flops
}
zPlus = z;
}
//==============================================================================
// CALC UDOT
//==============================================================================
// To be called from tip to base.
// Temps do not need to be initialized.
//
// First pass
// ----------
// Given previously-calculated quantities from the State
// P this body's articulated body inertia
// Phi composite body child-to-parent shift matrix
// H joint transition matrix; sense is transposed from Jain
// G P * H * DI
// and supplied arguments
// F body force applied to B
// f generalize forces applied to B's mobilities
// udot_p known udots (if mobilizer is prescribed)
// calculate
// z articulated body residual force on B
// zPlus AB residual force as felt on inboard side of mobilizer
// eps f - ~H*z gen force plus gen equiv of body forces
// For a prescribed mobilizer we have zPlus=z. Otherwise, zPlus = z + G*eps.
//
// This is the first (inward) loop of Algorithm 16.2 on page 323 of Jain's
// 2011 book, modified to include the applied body force and not including
// the auxiliary quantity "nu=DI*eps".
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass1Inward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
const SBDynamicsCache& dc,
const Real* jointForces,
const SpatialVec* bodyForces,
const Real* allUDot,
SpatialVec* allZ,
SpatialVec* allZPlus,
Real* allEpsilon) const
{
const Vec<dof>& f = fromU(jointForces);
const SpatialVec& F = bodyForces[nodeNum];
SpatialVec& z = allZ[nodeNum];
SpatialVec& zPlus = allZPlus[nodeNum];
Vec<dof>& eps = toU(allEpsilon);
const bool isPrescribed = isUDotKnown(ic);
const HType& H = getH(pc);
const ArticulatedInertia& P = getP(abc);
const HType& G = getG(abc);
// z = Pa+b - F
z = getMobilizerCentrifugalForces(dc) - F; // 6 flops
// z += P H udot_p if prescribed
if (isPrescribed && !isUDotKnownToBeZero(ic)) {
const Vec<dof>& udot = fromU(allUDot);
z += P*(H*udot); // 66+12*dof flops
}
// z += sum(Phi(child) * zPlus(child)) for all children
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
z += phiChild * zPlusChild; // 18 flops
}
eps = f - ~H*z; // 12*dof flops
zPlus = z;
if (!isPrescribed)
zPlus += G*eps; // 12*dof flops
}
