/* 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; }