//==============================================================================
// CALC EQUIVALENT JOINT FORCES
//==============================================================================
// To be called from tip to base.
// Temps do not need to be initialized.
// (sherm 060727) In spatial operators, this calculates ~H*Phi*(F-(MA+b))
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcEquivalentJointForces(
const SBTreePositionCache& pc,
const SBDynamicsCache& dc,
const SpatialVec* bodyForces,
SpatialVec* allZ,
Real* jointForces) const
{
const SpatialVec& myBodyForce = bodyForces[nodeNum];
SpatialVec& z = allZ[nodeNum];
Vec<dof>& eps = toU(jointForces);
// Centrifugal forces are MA+b where M is body spatial inertia,
// A is total coriolis acceleration, and b is gyroscopic force.
z = myBodyForce - getTotalCentrifugalForces(dc);
for (unsigned i=0; i<children.size(); ++i) {
const SpatialVec& zChild = allZ[children[i]->getNodeNum()];
const PhiMatrix& phiChild = children[i]->getPhi(pc);
z += phiChild * zChild;
}
eps = ~getH(pc) * z;
}
void calcEquivalentJointForces(
const SBTreePositionCache& pc,
const SBDynamicsCache& dc,
const SpatialVec* bodyForces,
SpatialVec* allZ,
Real* jointForces) const
{
const SpatialVec& myBodyForce = bodyForces[nodeNum];
SpatialVec& z = allZ[nodeNum];
// Centrifugal forces are PA+b where P is articulated body inertia,
// A is total coriolis acceleration, and b is gyroscopic force.
z = myBodyForce - getTotalCentrifugalForces(dc);
for (int i=0 ; i<(int)children.size() ; i++) {
const SpatialVec& zChild = allZ[children[i]->getNodeNum()];
const PhiMatrix& phiChild = children[i]->getPhi(pc);
z += phiChild * zChild;
}
}
