int Joint::assignSystemIndicesToBodyAndCoordinates( const SimTK::MobilizedBody& mobod, const OpenSim::Body* mobilized, const int& numMobilities, const int& startingCoordinateIndex) const { // If not OpenSim body provided as the one being mobilized assume it is // and intermedidate body and ignore. if (mobilized){ // Index can only be assigned to a parent or child body connected by this // Joint SimTK_ASSERT3( ( (mobilized == &getParentBody()) || (mobilized == &getChildBody()) || (mobilized == _slaveBodyForParent) || (mobilized == _slaveBodyForChild) ), "%s::'%s' - Cannot assign underlying system index to a Body '%s', " "which is not a parent or child Body of this Joint.", getConcreteClassName().c_str(), getName().c_str(), mobilized->getName().c_str()); // ONLY the base Joint can do this assignment mobilized->_index = mobod.getMobilizedBodyIndex(); } int nc = numCoordinates(); SimTK_ASSERT3(numMobilities <= (nc - startingCoordinateIndex), "%s attempted to create an underlying SimTK::MobilizedBody that " "supplies %d mobilities but only %d required.", getConcreteClassName().c_str(), numMobilities, nc - startingCoordinateIndex); const CoordinateSet& coords = get_CoordinateSet(); int j = startingCoordinateIndex; for (int iq = 0; iq < numMobilities; ++iq){ if (j < nc){ // assign coords[j]._mobilizerQIndex = SimTK::MobilizerQIndex(iq); coords[j]._bodyIndex = mobod.getMobilizedBodyIndex(); j++; } else{ std::string msg = getConcreteClassName() + " creating MobilizedBody with more mobilities than declared Coordinates."; throw Exception(msg); } } return j; }
/* 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; }
int Joint::assignSystemIndicesToBodyAndCoordinates( const SimTK::MobilizedBody& mobod, const OpenSim::PhysicalFrame* mobilized, const int& numMobilities, const int& startingCoordinateIndex) const { // If not OpenSim body provided as the one being mobilized assume it is // an intermediate body and ignore. if (mobilized){ // Index can only be assigned to a parent or child body connected by this // Joint SimTK_ASSERT3( ( (mobilized == &getParentFrame()) || (mobilized == &getChildFrame()) || (mobilized == _slaveBodyForParent.get()) || (mobilized == _slaveBodyForChild.get()) ), "%s::'%s' - Cannot assign underlying system index to a PhysicalFrame '%s', " "which is not a parent or child Frame of this Joint.", getConcreteClassName().c_str(), getName().c_str(), mobilized->getName().c_str()); // ONLY the base Joint can do this assignment mobilized->setMobilizedBodyIndex(mobod.getMobilizedBodyIndex()); // Note that setting the mobilized body index of a PhysicalOffsetFrame // does not set it on the parent PhysicalFrame. // Do the check and set it here as well since only the Joint can set the index. const PhysicalOffsetFrame* physOff = dynamic_cast<const PhysicalOffsetFrame*>(mobilized); if (physOff) { physOff->getParentFrame().setMobilizedBodyIndex(mobod.getMobilizedBodyIndex()); } } const int nc = numCoordinates(); SimTK_ASSERT3(numMobilities <= (nc - startingCoordinateIndex), "%s attempted to create an underlying SimTK::MobilizedBody that " "supplies %d mobilities but only %d required.", getConcreteClassName().c_str(), numMobilities, nc - startingCoordinateIndex); // Need a writable reference to this Joint so indices can be set on its // Coordinates. Self& mutableSelf = const_cast<Self&>(*this); int j = startingCoordinateIndex; for (int iq = 0; iq < numMobilities; ++iq){ if (j < nc){ // assign mutableSelf.upd_coordinates(j)._mobilizerQIndex = SimTK::MobilizerQIndex(iq); mutableSelf.upd_coordinates(j)._bodyIndex = mobod.getMobilizedBodyIndex(); j++; } else{ std::string msg = getConcreteClassName() + " creating MobilizedBody with more mobilities than declared Coordinates."; throw Exception(msg); } } return j; }