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