/* Calculate the equivalent spatial force, FB_G, acting on a mobilized body specified
by index acting at its mobilizer frame B, expressed in ground. */
SimTK::SpatialVec Joint::calcEquivalentSpatialForceForMobilizedBody(const SimTK::State &s,
const SimTK::MobilizedBodyIndex mbx, const SimTK::Vector &mobilityForces) const
{
// Get the mobilized body
const SimTK::MobilizedBody mbd = getModel().getMatterSubsystem().getMobilizedBody(mbx);
const SimTK::UIndex ustart = mbd.getFirstUIndex(s);
const int nu = mbd.getNumU(s);
const SimTK::MobilizedBody ground = getModel().getMatterSubsystem().getGround();
if (nu == 0) // No mobility forces (weld joint?).
return SimTK::SpatialVec(SimTK::Vec3(0), SimTK::Vec3(0));
// Construct the H (joint jacobian, joint transition) matrrix for this mobilizer
SimTK::Matrix transposeH_PB_w(nu, 3);
SimTK::Matrix transposeH_PB_v(nu, 3);
// from individual columns
SimTK::SpatialVec Hcol;
// To obtain the joint Jacobian, H_PB (H_FM in Simbody) need to be realized to at least position
_model->getMultibodySystem().realize(s, SimTK::Stage::Position);
SimTK::Vector f(nu, 0.0);
for(int i =0; i<nu; ++i){
f[i] = mobilityForces[ustart + i];
// Get the H matrix for this Joint by constructing it from the operator H*u
Hcol = mbd.getH_FMCol(s, SimTK::MobilizerUIndex(i));
const SimTK::Vector hcolw(Hcol[0]);
const SimTK::Vector hcolv(Hcol[1]);
transposeH_PB_w[i] = ~hcolw;
transposeH_PB_v[i] = ~hcolv;
}
// Spatial force and torque vectors
SimTK::Vector Fv(3, 0.0), Fw(3, 0.0);
// Solve the pseudoinverse problem of Fv = pinv(~H_PB_G_v)*f;
SimTK::FactorQTZ pinvForce(transposeH_PB_v);
//if rank = 0, body force cannot contribute to the mobility force
if(pinvForce.getRank() > 0)
pinvForce.solve(f, Fv);
// Now solve the pseudoinverse for torque for any unaccounted f: Fw = pinv(~H_PB_G_w)*(f - ~H_PB_G_v*Fv);
SimTK::FactorQTZ pinvTorq(transposeH_PB_w);
//if rank = 0, body torque cannot contribute to the mobility force
if(pinvTorq.getRank() > 0)
pinvTorq.solve(f, Fw);
// Now we have two solution with either the body force Fv or body torque accounting for some or all of f
SimTK::Vector fv = transposeH_PB_v*Fv;
SimTK::Vector fw = transposeH_PB_w*Fw;
// which to choose? Choose the more effective as fx.norm/Fx.norm
if(fv.norm() > SimTK::SignificantReal){ // if body force can contributes at all
// if body torque can contribute too and it is more effective
if(fw.norm() > SimTK::SignificantReal){
if (fw.norm()/Fw.norm() > fv.norm()/Fv.norm() ){
// account for f using torque, Fw, so compute Fv with remainder
pinvForce.solve(f-fw, Fv);
}else{
// account for f using force, Fv, first and Fw from remainder
pinvTorq.solve(f-fv, Fw);
}
}
// else no torque contribution and Fw should be zero
}
// no force contribution but have a torque
else if(fw.norm() > SimTK::SignificantReal){
// just Fw
}
else{
// should be the case where gen force is zero.
assert(f.norm() < SimTK::SignificantReal);
}
// The spatial forces above are expresseded in the joint frame of the parent
// Transform from parent joint frame, P into the parent body frame, Po
const SimTK::Rotation R_PPo = (mbd.getInboardFrame(s).R());
// Re-express forces in ground, first by describing force in the parent, Po,
// frame instead of joint frame
SimTK::Vec3 vecFw = R_PPo*SimTK::Vec3::getAs(&Fw[0]);
SimTK::Vec3 vecFv = R_PPo*SimTK::Vec3::getAs(&Fv[0]);
//Force Acting on joint frame, B, in child body expressed in Parent body, Po
SimTK::SpatialVec FB_Po(vecFw, vecFv);
const MobilizedBody parent = mbd.getParentMobilizedBody();
// to apply spatial forces on bodies they must be expressed in ground
vecFw = parent.expressVectorInAnotherBodyFrame(s, FB_Po[0], ground);
vecFv = parent.expressVectorInAnotherBodyFrame(s, FB_Po[1], ground);
// Package resulting torque and force as a spatial vec
SimTK::SpatialVec FB_G(vecFw, vecFv);
return FB_G;
}
