コード例 #1
0
ファイル: Joint.cpp プロジェクト: jryong/opensim-core
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;
}
コード例 #2
0
ファイル: Joint.cpp プロジェクト: antoinefalisse/opensim-core
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;
}