//==============================================================================
// CALC INVERSE DYNAMICS
//==============================================================================
// This algorithm calculates
// f = M*udot + C(u) - f_applied
// in O(N) time, where C(u) are the velocity-dependent coriolis, centrifugal and
// gyroscopic forces, f_applied are the applied body and joint forces, and udot
// is given.
//
// Pass 1 is base to tip and is just a calculation of body accelerations arising
// from udot and the coriolis accelerations. It is embodied in the reusable
// calcBodyAccelerationsFromUdotOutward() method above.
//
// Pass 2 is tip to base. It takes body accelerations from pass 1 (including coriolis
// accelerations as well as hinge accelerations), and the applied forces,
// and calculates the additional hinge forces that would be necessary to produce
// the observed accelerations.
//
// Costs for inverse dynamics include both passes:
// pass1: 12*dof + 18 flops
// pass2: 12*dof + 75 flops
// -----------------
// total: 24*dof + 93 flops
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
calcInverseDynamicsPass2Inward(
const SBTreePositionCache& pc,
const SBTreeVelocityCache& vc,
const SpatialVec* allA_GB,
const Real* jointForces,
const SpatialVec* bodyForces,
SpatialVec* allF, // temp
Real* allTau) const
{
const Vec<dof>& myJointForce = fromU(jointForces);
const SpatialVec& myBodyForce = bodyForces[nodeNum];
const SpatialVec& A_GB = allA_GB[nodeNum];
SpatialVec& F = allF[nodeNum];
Vec<dof>& tau = toU(allTau);
// Start with rigid body force from desired body acceleration and
// gyroscopic forces due to angular velocity, minus external forces
// applied directly to this body.
F = getMk_G(pc)*A_GB + getGyroscopicForce(vc) - myBodyForce; // 57 flops
// Add in forces on children, shifted to this body.
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& FChild = allF[children[i]->getNodeNum()];
F += phiChild * FChild; // 18 flops
}
// Project body forces into hinge space and subtract any hinge forces already
// being applied to get the remaining hinge forces needed.
tau = ~getH(pc)*F - myJointForce; // 12*dof flops
}
void calcInverseDynamicsPass2Inward(
const SBTreePositionCache& pc,
const SBTreeVelocityCache& vc,
const SpatialVec* allA_GB,
const Real* jointForces,
const SpatialVec* bodyForces,
SpatialVec* allF,
Real* allTau) const
{
const SpatialVec& myBodyForce = bodyForces[nodeNum];
const SpatialVec& A_GB = allA_GB[nodeNum];
SpatialVec& F = allF[nodeNum];
// Start with rigid body force from desired body acceleration and
// gyroscopic forces due to angular velocity, minus external forces
// applied directly to this body.
F = getMk_G(pc)*A_GB + getGyroscopicForce(vc) - myBodyForce;
// Add in forces on children, shifted to this body.
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& FChild = allF[children[i]->getNodeNum()];
F += phiChild * FChild;
}
// no taus.
}
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;
}
void multiplyByMPass2Inward(
const SBTreePositionCache& pc,
const SpatialVec* allA_GB,
SpatialVec* allF, // temp
Real* allTau) const
{
const SpatialVec& A_GB = allA_GB[nodeNum];
SpatialVec& F = allF[nodeNum];
F = getMk_G(pc)*A_GB;
for (int i=0 ; i<(int)children.size() ; i++) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& FChild = allF[children[i]->getNodeNum()];
F += phiChild * FChild;
}
}
// Call tip to base after calling multiplyByMPass1Outward() for each
// rigid body.
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMPass2Inward(
const SBTreePositionCache& pc,
const SpatialVec* allA_GB,
SpatialVec* allF, // temp
Real* allTau) const
{
const SpatialVec& A_GB = allA_GB[nodeNum];
SpatialVec& F = allF[nodeNum];
Vec<dof>& tau = toU(allTau);
// 45 flops because Mk has a nice structure
F = getMk_G(pc)*A_GB;
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& FChild = allF[children[i]->getNodeNum()];
F += phiChild * FChild; // 18 flops
}
tau = ~getH(pc)*F; // 11*dof flops
}
//==============================================================================
// 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
}
