Constraint::LineOnLineContact::LineOnLineContact
(MobilizedBody& mobod_F,
const Transform& defaultEdgeFrameF,
Real defaultHalfLengthF,
MobilizedBody& mobod_B,
const Transform& defaultEdgeFrameB,
Real defaultHalfLengthB,
bool enforceRolling)
: Constraint(new LineOnLineContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(),
"Constraint::LineOnLineContact","LineOnLineContact",
"Both mobilized bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B),
"Constraint::LineOnLineContact","LineOnLineContact",
"The two mobilized bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
mobod_F.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F);
updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B);
updImpl().m_def_X_FEf = defaultEdgeFrameF;
updImpl().m_def_hf = defaultHalfLengthF;
updImpl().m_def_X_BEb = defaultEdgeFrameB;
updImpl().m_def_hb = defaultHalfLengthB;
}
Constraint::SphereOnPlaneContact::SphereOnPlaneContact
(MobilizedBody& planeBody,
const Transform& defaultPlaneFrame,
MobilizedBody& sphereBody,
const Vec3& defaultSphereCenter,
Real defaultSphereRadius,
bool enforceRolling)
: Constraint(new SphereOnPlaneContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS( planeBody.isInSubsystem()
&& sphereBody.isInSubsystem(),
"Constraint::SphereOnPlaneContact","SphereOnPlaneContact",
"Both bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(planeBody.isInSameSubsystem(sphereBody),
"Constraint::SphereOnPlaneContact","SphereOnPlaneContact",
"The two bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
SimTK_APIARGCHECK1_ALWAYS(defaultSphereRadius > 0,
"Constraint::SphereOnPlaneContact","SphereOnPlaneContact",
"The sphere radius must be greater than zero but was %g.",
defaultSphereRadius);
planeBody.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_planeBody_F = updImpl().addConstrainedBody(planeBody);
updImpl().m_ballBody_B = updImpl().addConstrainedBody(sphereBody);
updImpl().m_def_X_FP = defaultPlaneFrame;
updImpl().m_def_p_BO = defaultSphereCenter;
updImpl().m_def_radius = defaultSphereRadius;
}
Constraint::SphereOnSphereContact::SphereOnSphereContact
(MobilizedBody& mobod_F,
const Vec3& defaultCenter_F,
Real defaultRadius_F,
MobilizedBody& mobod_B,
const Vec3& defaultCenter_B,
Real defaultRadius_B,
bool enforceRolling)
: Constraint(new SphereOnSphereContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(),
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"Both mobilized bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B),
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"The two mobilized bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
SimTK_APIARGCHECK2_ALWAYS(defaultRadius_F > 0 && defaultRadius_B > 0,
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"The sphere radii must be greater than zero; they were %g and %g.",
defaultRadius_F, defaultRadius_B);
mobod_F.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F);
updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B);
updImpl().m_def_p_FSf = defaultCenter_F;
updImpl().m_def_radius_F = defaultRadius_F;
updImpl().m_def_p_BSb = defaultCenter_B;
updImpl().m_def_radius_B = defaultRadius_B;
}
Force::MobilityLinearDamperImpl::
MobilityLinearDamperImpl(const MobilizedBody& mobod,
MobilizerUIndex whichU,
Real defaultDamping)
: m_matter(mobod.getMatterSubsystem()),
m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU),
m_defaultDamping(defaultDamping)
{
}
Force::MobilityConstantForceImpl::MobilityConstantForceImpl
(const MobilizedBody& mobod,
MobilizerUIndex whichU,
Real defaultForce)
: m_matter(mobod.getMatterSubsystem()),
m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU),
m_defaultForce(defaultForce)
{
}
Force::MobilityLinearSpringImpl::
MobilityLinearSpringImpl(const MobilizedBody& mobod,
MobilizerQIndex whichQ,
Real defaultStiffness,
Real defaultQZero)
: m_matter(mobod.getMatterSubsystem()),
m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichQ(whichQ),
m_defaultStiffness(defaultStiffness), m_defaultQZero(defaultQZero)
{
}
//==============================================================================
// GET REACTION PAIR
//==============================================================================
static ReactionPair getReactionPair(const State& state,
const MobilizedBody& mobod)
{
SpatialVec p = mobod.findMobilizerReactionOnParentAtFInGround(state);
SpatialVec c = mobod.findMobilizerReactionOnBodyAtMInGround(state);
const Rotation& R_GC = mobod.getBodyRotation(state);
const Rotation& R_GP = mobod.getParentMobilizedBody().getBodyRotation(state);
ReactionPair pair;
pair.reactionOnChildInChild = ~R_GC*c; // from Ground to Child
pair.reactionOnParentInParent = ~R_GP*p; // from Ground to Parent
return pair;
}
Force::MobilityLinearStopImpl::MobilityLinearStopImpl
(const MobilizedBody& mobod,
MobilizerQIndex whichQ,
Real defaultStiffness,
Real defaultDissipation,
Real defaultQLow,
Real defaultQHigh)
: m_matter(mobod.getMatterSubsystem()),
m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichQ(whichQ),
m_defStiffness(defaultStiffness), m_defDissipation(defaultDissipation),
m_defQLow(defaultQLow), m_defQHigh(defaultQHigh)
{
}
SpatialVec Force::DiscreteForces::
getOneBodyForce(const State& state, const MobilizedBody& mobod) const {
const Vector_<SpatialVec>& bodyForces = getImpl().getAllBodyForces(state);
if (bodyForces.size() == 0) {return SpatialVec(Vec3(0),Vec3(0));}
return bodyForces[mobod.getMobilizedBodyIndex()];
}
MyConstraintImplementation(MobilizedBody& mobilizer, Real speed)
: Implementation(mobilizer.updMatterSubsystem(), 0,1,0),
theMobilizer(), whichMobility(), prescribedSpeed(NaN)
{
theMobilizer = addConstrainedMobilizer(mobilizer);
whichMobility = MobilizerUIndex(0);
prescribedSpeed = speed;
}
Real Force::DiscreteForces::
getOneMobilityForce(const State& state, const MobilizedBody& mobod,
MobilizerUIndex whichU) const {
const Vector& mobForces = getImpl().getAllMobilityForces(state);
if (mobForces.size() == 0) {return 0;}
return mobod.getOneFromUPartition(state, whichU, mobForces);
}
void Force::DiscreteForces::
setOneBodyForce(State& state, const MobilizedBody& mobod,
const SpatialVec& spatialForceInG) const {
Vector_<SpatialVec>& bodyForces = getImpl().updAllBodyForces(state);
if (bodyForces.size() == 0) {
bodyForces.resize(getImpl().m_matter.getNumBodies());
bodyForces.setToZero();
}
bodyForces[mobod.getMobilizedBodyIndex()] = spatialForceInG;
}
void Force::DiscreteForces::
addForceToBodyPoint(State& state, const MobilizedBody& mobod,
const Vec3& pointInB, const Vec3& forceInG) const {
Vector_<SpatialVec>& bodyForces = getImpl().updAllBodyForces(state);
if (bodyForces.size() == 0) {
bodyForces.resize(getImpl().m_matter.getNumBodies());
bodyForces.setToZero();
}
mobod.applyForceToBodyPoint(state, pointInB, forceInG, bodyForces);
}
void Force::DiscreteForces::
setOneMobilityForce(State& state, const MobilizedBody& mobod,
MobilizerUIndex whichU, Real f) const {
Vector& mobForces = getImpl().updAllMobilityForces(state);
if (mobForces.size() == 0) {
mobForces.resize(state.getNU());
mobForces.setToZero();
}
// Don't use "apply" here because that would add in the force.
mobod.updOneFromUPartition(state, whichU, mobForces) = f;
}
void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy,
const Array_<MobilizedBodyIndex>& originalBodyIxs,
Array_<MobilizedBodyIndex>& copyBodyIxs,
bool& hasArtificialBaseBody)
{
const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
SimbodyMatterSubsystem copyMatter(copy);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
hasArtificialBaseBody = false;
for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
MobilizedBody* copyBody;
if (i == 0) {
if (originalBody.isGround())
copyBody = ©Matter.Ground();
else {
hasArtificialBaseBody = true; // not using the original joint here
MobilizedBody::Free free(copyMatter.Ground(), body);
copyBody = ©Matter.updMobilizedBody(free.getMobilizedBodyIndex());
}
}
else {
MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
copyBody = &originalBody.cloneForNewParent(parent);
}
copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
}
copy.realizeTopology();
State& s = copy.updDefaultState();
copyMatter.setUseEulerAngles(s, true);
copy.realizeModel(s);
}
Force::TwoPointConstantForceImpl::TwoPointConstantForceImpl(const MobilizedBody& body1, const Vec3& station1,
const MobilizedBody& body2, const Vec3& station2, Real force) : matter(body1.getMatterSubsystem()),
body1(body1.getMobilizedBodyIndex()), station1(station1),
body2(body2.getMobilizedBodyIndex()), station2(station2), force(force) {
}
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override {
const Vec3 frcColors[] = {Red,Orange,Cyan,Green,Blue,Yellow};
m_system.realize(state, Stage::Velocity);
const int ncont = m_compliant.getNumContactForces(state);
for (int i=0; i < ncont; ++i) {
const ContactForce& force = m_compliant.getContactForce(state,i);
const ContactId id = force.getContactId();
// define if it's femur_lat -> femur_med contact pair: id=8
//if (id == 6 || id == 8 || id==4 || id==2 || id==10 )
// continue;
ContactSnapshot cs = m_compliant.getContactTrackerSubsystem().getActiveContacts(state);
//cout << "contact: " << id << endl;
//ContactId idFemur = cs.getContactIdForSurfacePair( ContactSurfaceIndex(2), ContactSurfaceIndex(3));
//ContactId idMeniscFemur1 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(2));
//ContactId idMeniscTibia1 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(4));
//ContactId idMeniscFemur2 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(3));
//ContactId idMeniscTibia2 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(4));
ContactId idLatFemurTibia = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(2));
ContactId idMedFemurTibia = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(2));
ContactId idFemur = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(1));
if (id == idFemur)
continue;
const SimbodyMatterSubsystem& matter = m_compliant.getMultibodySystem().getMatterSubsystem();
// get tibia's mobilized body
MobilizedBody tibiaMobod = matter.getMobilizedBody(MobilizedBodyIndex(22));
MobilizedBody femurLatmobod = matter.getMobilizedBody(MobilizedBodyIndex(19));
MobilizedBody femurMedmobod = matter.getMobilizedBody(MobilizedBodyIndex(20));
//for (int numContacts=0; numContacts<cs.getNumContacts(); numContacts++)
//{
//ContactSurfaceIndex csi1 = cs.getContact(numContacts).getSurface1();
//ContactSurfaceIndex csi2 = cs.getContact(numContacts).getSurface2();
//cout << "surf1: " << csi1 << " surf2: " << csi2 << endl;
//}
if (cs.getNumContacts()>0)
{
DecorativeGeometry decTibiaContactGeometry = tibiaMobod.getBody().updDecoration(0);
decTibiaContactGeometry.setOpacity( 0.9);
decTibiaContactGeometry.setColor( Gray);
DecorativeGeometry decFemurLatContactGeometry = femurLatmobod.getBody().updDecoration(0);
decFemurLatContactGeometry.setOpacity(1);
decFemurLatContactGeometry.setColor(Gray);
DecorativeGeometry decFemurMedContactGeometry = femurMedmobod.getBody().updDecoration(0);
decFemurMedContactGeometry.setOpacity(1);
decFemurMedContactGeometry.setColor(Gray);
// Tibia transformation
const Transform& X_BM = tibiaMobod.getOutboardFrame(state); // M frame in B
const Transform& X_GB = tibiaMobod.getBodyTransform(state); // B in Ground
const Transform& X_PF = tibiaMobod.getInboardFrame(state);
Transform X_GM = X_GB*X_BM; // M frame in Ground
// rotate
//Rotation& newRot = X_GM.updR();
//Rotation parentRot = X_PF.R();
//newRot.operator*=( parentRot.invert());
// translate in front of the whole body
X_GM.setP( X_GM.operator+=( Vec3(0.3,0,0)).p());
// set new transform
decTibiaContactGeometry.setTransform( X_GM);
// Medial Femur Transformation
const Transform& X_BM_medFemur = femurMedmobod.getOutboardFrame(state); // M frame in B
const Transform& X_GB_medFemur = femurMedmobod.getBodyTransform(state); // B in Ground
const Transform& X_PF_medFemur = femurMedmobod.getInboardFrame(state);
Transform X_GM_medFemur = X_GB_medFemur*X_BM_medFemur; // M frame in Ground
// rotate
//Rotation& newRot_medFemur = X_GM_medFemur.updR();
//Rotation parentRot_medFemur = X_PF_medFemur.R();
//newRot_medFemur.operator*=(parentRot_medFemur).operator*=(Rotation(1.57079, CoordinateAxis::ZCoordinateAxis()));
//newRot_medFemur.setRotationFromAngleAboutZ(1.570796326794897);
// translate in front of the whole body
X_GM_medFemur.setP(X_GM_medFemur.operator+=(Vec3(0.3, 0.1, 0)).p());
// set new transform
decFemurMedContactGeometry.setTransform(X_GM_medFemur);
// Lateral Femur Transformation
const Transform& X_BM_latFemur = femurLatmobod.getOutboardFrame(state); // M frame in B
const Transform& X_GB_latFemur = femurLatmobod.getBodyTransform(state); // B in Ground
const Transform& X_PF_latFemur = femurLatmobod.getInboardFrame(state);
Transform X_GM_latFemur = X_GB_latFemur*X_BM_latFemur; // M frame in Ground
// rotate
//Rotation& newRot_latFemur = X_GM_latFemur.updR();
//Rotation parentRot_latFemur = X_PF_latFemur.R();
//newRot_latFemur.operator*=(parentRot_latFemur).operator*=(Rotation(1.57079, CoordinateAxis::ZCoordinateAxis()));
//newRot_latFemur.setRotationFromAngleAboutZ(1.570796326794897);
// translate in front of the whole body
X_GM_latFemur.setP(X_GM_latFemur.operator+=(Vec3(0.3, 0.1, 0)).p());
// set new transform
decFemurLatContactGeometry.setTransform(X_GM_latFemur);
ContactPatch patch;
const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
//cout << "patch for id" << id << " found=" << found << endl;
//cout << "resultant=" << patch.getContactForce() << endl;
//cout << "num details=" << patch.getNumDetails() << endl;
for (int k=0; k < patch.getNumDetails(); ++k) {
const ContactDetail& detail = patch.getContactDetail(k);
//const Real peakPressure = detail.getPeakPressure();
const Vec3 cp = detail.getContactPoint();
Vec3 newcp = Vec3(cp);
// transform point to attach tibia surface
Transform cpToTibiaTransf = Transform(newcp);
cpToTibiaTransf.setP(cpToTibiaTransf.operator+=( Vec3(0.3,0,0)).p());
DecorativeSphere decCpToTibia;
decCpToTibia.setScale(0.0005);
decCpToTibia.setTransform(cpToTibiaTransf);
decCpToTibia.setColor(Red);
// transform point to attach femur surface
Transform cpToFemurTransf = Transform(newcp);
//Rotation& newRotCpToFemur = cpToFemurTransf.updR();
//newRotCpToFemur.setRotationFromAngleAboutZ(1.570796326794897);
//cpToFemurTransf.set(X_GM_latFemur.R(), X_GM_latFemur.p());
//newRotCpToFemur.operator*=(parentRot_latFemur);
//cpToFemurTransf.setP( X_PF_latFemur.p());
//newRotCpToFemur.setRotationFromAngleAboutZ(1.570796326794897);
cpToFemurTransf.setP(cpToFemurTransf.operator+=(Vec3(0.3, 0.1, 0)).p());
DecorativeSphere decCpToFemur;
decCpToFemur.setScale(0.0005);
decCpToFemur.setTransform(cpToFemurTransf);
decCpToFemur.setColor(Red);
geometry.push_back(decCpToTibia);
geometry.push_back(decCpToFemur);
if (k == 0)
{
geometry.push_back(decTibiaContactGeometry);
geometry.push_back(decFemurLatContactGeometry);
geometry.push_back(decFemurMedContactGeometry);
}
}
}
}
}
MyTorqueLimitedMotor
(const MobilizedBody& mobod, MobilizerUIndex whichU,
Real gain, Real torqueLimit)
: m_matter(mobod.getMatterSubsystem()), m_mobod(mobod), m_whichU(whichU),
m_torqueGain(gain), m_torqueLimit(torqueLimit)
{ assert(gain >= 0 && torqueLimit >= 0); }
// Constructor takes two bodies and the desired separation distance
// between their body frame origins. Tell the base class that this
// constraint generates 1 holonomic (position level), 0 nonholonomic
// (velocity level), and 0 acceleration-only constraint equations.
ExampleConstraint(MobilizedBody& b1, MobilizedBody& b2, Real distance)
: Implementation(b1.updMatterSubsystem(), 1, 0, 0), distance(distance) {
body1 = addConstrainedBody(b1);
body2 = addConstrainedBody(b2);
}
Force::ConstantTorqueImpl::ConstantTorqueImpl(const MobilizedBody& body, const Vec3& torque) :
matter(body.getMatterSubsystem()), body(body.getMobilizedBodyIndex()), torque(torque) {
}
Force::TwoPointLinearSpringImpl::TwoPointLinearSpringImpl(const MobilizedBody& body1, const Vec3& station1,
const MobilizedBody& body2, const Vec3& station2, Real k, Real x0) : matter(body1.getMatterSubsystem()),
body1(body1.getMobilizedBodyIndex()), station1(station1),
body2(body2.getMobilizedBodyIndex()), station2(station2), k(k), x0(x0) {
}
Force::ConstantForceImpl::ConstantForceImpl(const MobilizedBody& body, const Vec3& station, const Vec3& force) :
matter(body.getMatterSubsystem()), body(body.getMobilizedBodyIndex()), station(station), force(force) {
}
Force::TwoPointLinearDamperImpl::TwoPointLinearDamperImpl(const MobilizedBody& body1, const Vec3& station1,
const MobilizedBody& body2, const Vec3& station2, Real damping) : matter(body1.getMatterSubsystem()),
body1(body1.getMobilizedBodyIndex()), station1(station1),
body2(body2.getMobilizedBodyIndex()), station2(station2), damping(damping) {
}
/* 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;
}
