/** * 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; SimTK::Vec3 orientation; location = joint.getLocationInParent(); orientation = joint.getOrientationInParent(); 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.getCoordinateSet().get(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.getCoordinateSet().get(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.getCoordinateSet().get(index++).getName(), 0, Coordinate::Translational); ssj->addFunctionDof(yaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Translational); ssj->addFunctionDof(zaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Translational); ssj->addFunctionDof(xaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Rotational); ssj->addFunctionDof(yaxis, joint.getCoordinateSet().get(index++).getName(), 0, Coordinate::Rotational); ssj->addFunctionDof(zaxis, joint.getCoordinateSet().get(index).getName(), 0, Coordinate::Rotational); } else if (joint.getConcreteClassName()==("CustomJoint")) { const CustomJoint* cj = (CustomJoint*)(&joint); const CoordinateSet& coordinates = cj->getCoordinateSet(); // 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; Coordinate::MotionType motionType = (order[i]<3) ? Coordinate::Rotational : Coordinate::Translational; if (ta->getCoordinateNames().size() > 0) coord = &coordinates.get(ta->getCoordinateNames()[0]); if (coord) 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); }