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