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