// Ground's "articulated" body inertia is still the infinite mass and
// inertia it started with; no need to look at the children.
void realizeArticulatedBodyInertiasInward(
const SBInstanceCache&,
const SBTreePositionCache&,
SBArticulatedBodyInertiaCache& abc) const
{ ArticulatedInertia& P = updP(abc);
P = ArticulatedInertia(SymMat33(Infinity), Mat33(Infinity),
SymMat33(0));
updPPlus(abc) = P;
}
void realizeArticulatedBodyInertiasInward
(const SBInstanceCache& ic,
const SBTreePositionCache& pc,
SBArticulatedBodyInertiaCache& abc) const
{
ArticulatedInertia& P = updP(abc);
P = ArticulatedInertia(getMk_G(pc));
for (unsigned i=0 ; i<children.size() ; i++) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const ArticulatedInertia& PPlusChild = children[i]->getPPlus(abc);
P += PPlusChild.shift(phiChild.l());
}
updPPlus(abc) = P;
}
//==============================================================================
// REALIZE ARTICULATED BODY INERTIAS
//==============================================================================
// Compute articulated body inertia and related quantities for this body B.
// This must be called tip-to-base (inward).
//
// Given only position-related quantities from the State
// Mk (this body's spatial inertia matrix)
// Phi (composite body child-to-parent shift matrix)
// H (joint transition matrix; sense is transposed from Jain & Schwieters)
// we calculate dynamic quantities
// P (articulated body inertia)
// PPlus (articulated body inertia as seen through the mobilizer)
// For a prescribed mobilizer, we have PPlus==P. Otherwise we also compute
// D (factored mass matrix LDL' diagonal part D=~H*P*H)
// DI (inverse of D)
// G (P * H * DI)
// PPlus (P - P * H * DI * ~H * P = P - G * ~H * P)
// and put them in the state cache.
//
// This is Algorithm 6.1 on page 106 of Jain's 2011 book modified to accommodate
// prescribed motion as described on page 323. Note that although P+ is "as
// felt" on the inboard side of the mobilizer it is still calculated about
// Bo just as is P.
//
// Cost is 93 flops per child plus
// n^3 + 23*n^2 + 115*n + 12
// e.g. pin=143, ball=591 (197/dof), free=1746 (291/dof)
// Note that per-child cost is paid just once for each non-base body in
// the whole tree; that is, each body is touched just once.
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
realizeArticulatedBodyInertiasInward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
SBArticulatedBodyInertiaCache& abc) const
{
ArticulatedInertia& P = updP(abc);
// Start with the spatial inertia of the current body (in Ground frame).
P = ArticulatedInertia(getMk_G(pc)); // 12 flops
// For each child, we previously took its articulated body inertia P and
// removed the portion of that inertia that can't be felt from the parent
// because of the joint mobilities. That is, we calculated
// P+ = P - P H DI ~H P, where the second term is the projection of P into
// the mobility space of the child's inboard mobilizer. Note that if the
// child's mobilizer is prescribed, then the entire inertia will be felt
// by the parent so P+ = P in that case. Now we're going to shift P+
// from child to parent: Pparent += Phi*P+*~Phi.
// TODO: can this be optimized for the common case where the
// child is a terminal body and hence its P is an ordinary
// spatial inertia? (Spatial inertia shift is 37 flops vs. 72 here; not
// really much help.)
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const ArticulatedInertia& PPlusChild = children[i]->getPPlus(abc);
// Apply the articulated body shift.
// This takes 93 flops (72 for the shift and 21 to add it in).
// (Note that PPlusChild==PChild if child's mobilizer is prescribed.)
P += PPlusChild.shift(phiChild.l());
}
// Now compute PPlus. P+ = P for a prescribed mobilizer. Otherwise
// it is P+ = P - P H DI ~H P = P - G*~PH. In the prescribed case
// we leave G, D, DI untouched -- they should have been set to NaN at
// Instance stage.
ArticulatedInertia& PPlus = updPPlus(abc);
if (isUDotKnown(ic)) {
PPlus = P; // prescribed
return;
}
// This is a non-prescribed mobilizer. Compute D, DI, G then P+.
const HType& H = getH(pc);
HType& G = updG(abc);
Mat<dof,dof>& D = updD(abc);
Mat<dof,dof>& DI = updDI(abc);
const HType PH = P*H; // 66*dof flops
D = ~H * PH; // 11*dof^2 flops (symmetric result)
// this will throw an exception if the matrix is ill conditioned
DI = D.invert(); // ~dof^3 flops (symmetric)
G = PH * DI; // 12*dof^2-6*dof flops
// Want P+ = P - G*~PH. We can do this in about 55*dof flops.
// These require 9 dot products of length dof. The symmetric ones could
// be done with 6 dot products instead for a small savings but this gives
// us a chance to symmetrize and hopefully clean up some numerical errors.
// The full price for all three is 54*dof-27 flops.
Mat33 massMoment = G.row(0)*~PH.row(1); // (full) 9*(2*dof-1) flops
Mat33 mass = G.row(1)*~PH.row(1); // symmetric result 9*(2*dof-1) flops
Mat33 inertia = G.row(0)*~PH.row(0); // symmetric result 9*(2*dof-1) flops
// These must be symmetrized due to numerical errors for 12 more flops.
SymMat33 symMass( mass(0,0),
(mass(1,0)+mass(0,1))/2, mass(1,1),
(mass(2,0)+mass(0,2))/2, (mass(2,1)+mass(1,2))/2, mass(2,2));
SymMat33 symInertia(
inertia(0,0),
(inertia(1,0)+inertia(0,1))/2, inertia(1,1),
(inertia(2,0)+inertia(0,2))/2, (inertia(2,1)+inertia(1,2))/2, inertia(2,2));
PPlus = P - ArticulatedInertia(symMass, massMoment, symInertia); // 21 flops
}
