/**
* Convert an OpenSim Joint into the equivalent SIMM joint, only subset of joint types are supported
*
*/
void SimbodySimmModel::convertJoint(const Joint& joint)
{
string parentName;
string childName;
bool parentJointAdded = addExtraJoints(joint, parentName, childName);
SimbodySimmJoint* ssj = new SimbodySimmJoint(joint.getName(), parentName, childName);
// If parentJointAdded is false, that means the position and orientation in the
// parent can be merged with the primary joint. So begin making the joint by
// adding the non-zero components to the SimbodySimmJoint.
if (parentJointAdded == false) {
int rotationsSoFar = 0;
SimTK::Vec3 location(0);
SimTK::Vec3 orientation(0);
const PhysicalOffsetFrame* offset =
dynamic_cast<const PhysicalOffsetFrame*>(&joint.getParentFrame());
if (offset) {
location = offset->get_translation();
orientation = offset->get_orientation();
}
if (NOT_EQUAL_WITHIN_ERROR(location[0], 0.0))
ssj->addConstantDof("tx", NULL, location[0]);
if (NOT_EQUAL_WITHIN_ERROR(location[1], 0.0))
ssj->addConstantDof("ty", NULL, location[1]);
if (NOT_EQUAL_WITHIN_ERROR(location[2], 0.0))
ssj->addConstantDof("tz", NULL, location[2]);
if (NOT_EQUAL_WITHIN_ERROR(orientation[0], 0.0))
ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[0], orientation[0] * 180.0 / SimTK::Pi);
if (NOT_EQUAL_WITHIN_ERROR(orientation[1], 0.0))
ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[1], orientation[1] * 180.0 / SimTK::Pi);
if (NOT_EQUAL_WITHIN_ERROR(orientation[2], 0.0))
ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[2], orientation[2] * 180.0 / SimTK::Pi);
}
if (joint.getConcreteClassName()==("WeldJoint")) {
// Nothing to do.
} else if (joint.getConcreteClassName()==("PinJoint")) {
int index = 0;
string coordName = joint.get_coordinates(index).getName();
SimTK::Vec3 axis(0.0, 0.0, 1.0); // Pin joints always rotate about the Z axis.
ssj->addFunctionDof(axis, coordName, 0, Coordinate::Rotational);
} else if (joint.getConcreteClassName()==("SliderJoint")) {
int index = 0;
string coordName = joint.get_coordinates(index).getName();
SimTK::Vec3 axis(1.0, 0.0, 0.0); // Slider joints always translate along the X axis.
ssj->addFunctionDof(axis, coordName, 0, Coordinate::Translational);
} else if (joint.getConcreteClassName()==("EllipsoidJoint")) {
// NOTE: Ellipsoid joints cannot be converted into SIMM joints.
} else if (joint.getConcreteClassName()==("FreeJoint")) {
SimTK::Vec3 xaxis(1.0, 0.0, 0.0);
SimTK::Vec3 yaxis(0.0, 1.0, 0.0);
SimTK::Vec3 zaxis(0.0, 0.0, 1.0);
int index = 0;
ssj->addFunctionDof(xaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Translational);
ssj->addFunctionDof(yaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Translational);
ssj->addFunctionDof(zaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Translational);
ssj->addFunctionDof(xaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Rotational);
ssj->addFunctionDof(yaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Rotational);
ssj->addFunctionDof(zaxis, joint.get_coordinates(index).getName(), 0, Coordinate::Rotational);
} else if (joint.getConcreteClassName()==("CustomJoint")) {
const CustomJoint* cj = (CustomJoint*)(&joint);
// Add the joint's transform axes to the SimbodySimmJoint.
const SpatialTransform& dofs = cj->getSpatialTransform();
// Custom joints have the rotational DOFs specified first, but the translations are applied first
// so you want to process them first here.
static int order[] = {3, 4, 5, 0, 1, 2};
for (int i=0; i<6; i++) {
const TransformAxis* ta = &dofs[order[i]];
if (ta->getCoordinateNames().size() > 0) { // transform axis is unused if it has no coordinate names
const Coordinate* coord = NULL;
const Coordinate* independentCoord = NULL;
const Function* constraintFunc = NULL;
int ix = -1;
Coordinate::MotionType motionType = (order[i]<3) ? Coordinate::Rotational : Coordinate::Translational;
if (ta->getCoordinateNames().size() > 0)
ix = cj->getProperty_coordinates().findIndexForName(ta->getCoordinateNames()[0]);
if (ix >= 0) {
coord = &cj->get_coordinates(ix);
constraintFunc = isDependent(coord, &independentCoord);
}
if (constraintFunc != NULL) { // dof is constrained to a coordinate in another joint
ssj->addFunctionDof(ta->getAxis(), independentCoord->getName(), addJointFunction(constraintFunc, independentCoord->getMotionType(), motionType), motionType);
} else {
if (ta->hasFunction())
constraintFunc = &ta->getFunction();
if (constraintFunc) // dof is constrained to a coordinate in this joint
ssj->addFunctionDof(ta->getAxis(), coord->getName(), addJointFunction(constraintFunc, coord->getMotionType(), motionType), motionType);
else // dof is unconstrained
ssj->addFunctionDof(ta->getAxis(), coord->getName(), 0, motionType);
}
}
}
}
ssj->finalize();
addSimmJoint(ssj);
}
