/**
* Add extra joints to the SIMM model to represent the joint location/orientation
* in the parent body and in the child body.
*
* @param aJoint The Simbody joint.
* @param rParentName The name of the parent body for the "real" joint.
* @param rChildName The name of the child body for the "real" joint.
* @return Whether or not a 'parent' joint was added.
*/
bool SimbodySimmModel::addExtraJoints(const OpenSim::Joint& aJoint, string& rParentName, string& rChildName)
{
SimTK::Vec3 location;
SimTK::Vec3 orientation;
bool parentJointAdded = false;
if (isParentJointNeeded(aJoint)) {
location = aJoint.getLocationInParent();
orientation = aJoint.getOrientationInParent();
string bodyName = aJoint.getChildBodyName() + "_pjc";
SimbodySimmBody* b = new SimbodySimmBody(NULL, bodyName);
_simmBody.append(b);
makeSimmJoint(aJoint.getName() + "_pjc", aJoint.getParentBodyName(), bodyName, location, orientation);
rParentName = bodyName;
parentJointAdded = true;
} else {
rParentName = aJoint.getParentBodyName();
parentJointAdded = false;
}
if (isChildJointNeeded(aJoint)) {
location = aJoint.getLocationInChild();
orientation = aJoint.getOrientationInChild();
string bodyName = aJoint.getChildBodyName() + "_jcc";
SimbodySimmBody* b = new SimbodySimmBody(NULL, bodyName);
_simmBody.append(b);
// This joint is specified in the reverse direction.
makeSimmJoint(aJoint.getName() + "_jcc", aJoint.getChildBodyName(), bodyName, location, orientation);
rChildName = bodyName;
} else {
rChildName = aJoint.getChildBodyName();
}
return parentJointAdded;
}
/**
* Check to see if the location (in parent) and orientation (in parent) of a
* Simbody joint can be merged with the joint's "real" DOFs and still be
* represented with a single SIMM joint.
*
* @param aJoint The Simbody joint to check.
* @return Whether or not a separate SIMM joint is needed to represent the location
* and orientation in parent.
*/
bool SimbodySimmModel::isParentJointNeeded(const OpenSim::Joint& aJoint)
{
// If the joint has no "real" DOFs (e.g., WeldJoints), then the parent joint is not needed.
if (aJoint.getConcreteClassName()=="WeldJoint")
return false;
SimTK::Vec3 location;
SimTK::Vec3 orientation;
location = aJoint.getLocationInParent();
orientation = aJoint.getOrientationInParent();
bool translationsUsed[] = {false, false, false}, translationsDone = false;
int numTranslations = 0, numRotations = 0;
// First see which components are needed for the location and orientation.
for (int i=0; i<3; i++) {
if (NOT_EQUAL_WITHIN_ERROR(location[i], 0.0)) {
translationsUsed[i] = true;
numTranslations++;
}
}
for (int i=0; i<3; i++) {
if (NOT_EQUAL_WITHIN_ERROR(orientation[i], 0.0)) {
numRotations++;
if (numTranslations > 0)
translationsDone = true;
}
}
if (aJoint.getConcreteClassName()==("PinJoint")) {
if (numRotations >= 3) // have already defined three rotations (can't add more)
return true;
} else if (aJoint.getConcreteClassName()==("SliderJoint")) {
if (translationsUsed[0]) // slider joint translations are always along the X axis
return true;
} else if (aJoint.getConcreteClassName()==("EllipsoidJoint")) {
return true; // NOTE: ellipsoid joints cannot be converted to SIMM joints
} else if (aJoint.getConcreteClassName()==("FreeJoint")) {
return true;
} else if (aJoint.getConcreteClassName()==("CustomJoint")) {
const CustomJoint* cj = (CustomJoint*)(&aJoint);
const SpatialTransform& dofs = cj->getSpatialTransform();
// Now see if the joint's "real" DOFs can be added.
for (int i=0; i<6; i++) {
const TransformAxis* ta = &dofs[i];
if (i >= 3) {
double axis[3];
ta->getAxis(axis);
for (int j=0; j<3; j++) {
if (EQUAL_WITHIN_ERROR(axis[j], 1.0)) {
if (ta->getCoordinateNames().size() > 0) { // transform axis is unused if it has no coordinate names
if (translationsUsed[i-3]) // this translation component already defined
return true;
if (translationsDone) // have already defined translations then rotation (can't add more translations)
return true;
translationsUsed[i-3] = true;
numTranslations++;
}
break;
}
}
} else {
if (ta->getCoordinateNames().size() > 0) { // transform axis is unused if it has no coordinate names
if (numRotations >= 3) // have already defined three rotations (can't add more)
return true;
numRotations++;
if (numTranslations > 0)
translationsDone = true;
}
}
}
}
return false;
}
