Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
/* 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;
}
Ejemplo n.º 3
0
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;
}