void realizeInstance(const SBStateDigest& sbs) const {
// Initialize cache entries that will never be changed at later stages.
SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
SBDynamicsCache& dc = sbs.updDynamicsCache();
SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
updY(dc) = SpatialMat(Mat33(0));
updA_GB(ac) = 0;
}
void realizeYOutward(
const SBInstanceCache&,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
SBDynamicsCache& dc) const
{
// This psi actually has the wrong sign, but it doesn't matter since we multiply
// by it twice.
SpatialMat psi = getPhi(pc).toSpatialMat();
updY(dc) = ~psi * parent->getY(dc) * psi;
}
//==============================================================================
// REALIZE Y
//==============================================================================
// To be called base to tip.
// This is calculating what Abhi Jain calls the operational space compliance
// kernel in his 2011 book. This is the inverse of the operational space inertia
// for each body at its body frame. Also, see Equation 20 in Rodriguez,Jain,
// & Kreutz-Delgado: A spatial operator algebra
// for manipulator modeling and control. Intl. J. Robotics Research
// 10(4):371-381 (1991).
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::realizeYOutward
(const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
SBDynamicsCache& dc) const
{
if (isUDotKnown(ic)) {
//TODO: (sherm 090810) is this right?
assert(false);
//updY(dc) = (~getPhi(pc) * parent->getY(dc)) * getPhi(pc); // rigid shift
return;
}
// Compute psi. Jain has TauBar=I-G*~H but we're negating that to G*~H-I
// because we can save 30 flops by just subtracting 1 from the diagonals
// rather than having to negate all the off-diagonals. Then Psi ends up
// with the wrong sign here also, which doesn't matter because we multiply
// by it twice.
SpatialMat tauBar = getG(abc)*~getH(pc);// 11*dof^2 flops
tauBar(0,0) -= 1; // subtract identity matrix (only touches diags: 3 flops)
tauBar(1,1) -= 1; // " (3 flops)
SpatialMat psi = getPhi(pc)*tauBar; // ~100 flops
// TODO: this is very expensive (~1000 flops?) Could cut be at least half
// by exploiting symmetry. Also, does Psi have special structure?
// And does this need to be computed for every body or only those
// which are loop "base" bodies or some such?
// Psi here has the opposite sign from Jain's, but we're multiplying twice
// by it here so it doesn't matter.
updY(dc) = (getH(pc) * getDI(abc) * ~getH(pc))
+ (~psi * parent->getY(dc) * psi);
}
