예제 #1
0
/**
 * 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(0);
   SimTK::Vec3 orientation(0);
    bool parentJointAdded = false;

   if (isParentJointNeeded(aJoint)) {
       const PhysicalOffsetFrame* offset =
           dynamic_cast<const PhysicalOffsetFrame*>(&aJoint.getParentFrame());
       if (offset) {
           location = offset->get_translation();
           orientation = offset->get_orientation();
       }

      string bodyName = aJoint.getChildFrame().getName() + "_pjc";
      SimbodySimmBody* b = new SimbodySimmBody(NULL, bodyName);
      _simmBody.append(b);
      makeSimmJoint(aJoint.getName() + "_pjc", 
          aJoint.getParentFrame().getName(), bodyName, location, orientation);
      rParentName = bodyName;
        parentJointAdded = true;
   } else {
      rParentName = aJoint.getParentFrame().getName();
        parentJointAdded = false;
   }

   location = 0;
   orientation = 0;

   if (isChildJointNeeded(aJoint)) {
       const PhysicalOffsetFrame* offset =
           dynamic_cast<const PhysicalOffsetFrame*>(&aJoint.getChildFrame());
       if (offset) {
           location = offset->get_translation();
           orientation = offset->get_orientation();
       }
       string bodyName = aJoint.getChildFrame().getName() + "_jcc";
      SimbodySimmBody* b = new SimbodySimmBody(NULL, bodyName);
      _simmBody.append(b);
      // This joint is specified in the reverse direction.
      makeSimmJoint(aJoint.getName() + "_jcc",
          aJoint.getChildFrame().getName(), bodyName, location, orientation);
      rChildName = bodyName;
   } else {
       rChildName = aJoint.getChildFrame().getName();
   }

    return parentJointAdded;
}
예제 #2
0
/**
 * Check to see if a location (in child) and orientation (in child) are all zeros.
 * If they are not, then a separate SIMM joint is needed to hold this transform.
 *
 * @param aJoint The Simbody joint to check.
 * @return Whether or not a separate SIMM joint is needed to represent the location
 *         and orientation in child.
 */
bool SimbodySimmModel::isChildJointNeeded(const OpenSim::Joint& aJoint)
{
	SimTK::Vec3 location;
	SimTK::Vec3 orientation;

	location = aJoint.getLocationInChild();
	orientation= aJoint.getOrientationInChild();

	double sum = location.scalarNormSqr();
   if (NOT_EQUAL_WITHIN_ERROR(sum, 0.0))
      return true;

   sum = orientation.scalarNormSqr();
   if (NOT_EQUAL_WITHIN_ERROR(sum, 0.0))
      return true;

   return false;
}
예제 #3
0
/**
 * Check to see if a location (in child) and orientation (in child) are all zeros.
 * If they are not, then a separate SIMM joint is needed to hold this transform.
 *
 * @param aJoint The Simbody joint to check.
 * @return Whether or not a separate SIMM joint is needed to represent the location
 *         and orientation in child.
 */
bool SimbodySimmModel::isChildJointNeeded(const OpenSim::Joint& aJoint)
{
    SimTK::Vec3 location(0);
    SimTK::Vec3 orientation(0);
    const PhysicalOffsetFrame* offset =
        dynamic_cast<const PhysicalOffsetFrame*>(&aJoint.getChildFrame());
    if (offset) {
        location = offset->get_translation();
        orientation = offset->get_orientation();
    }
    else {
        return false;
    }

    double sum = location.scalarNormSqr();
   if (NOT_EQUAL_WITHIN_ERROR(sum, 0.0))
      return true;

   sum = orientation.scalarNormSqr();
   if (NOT_EQUAL_WITHIN_ERROR(sum, 0.0))
      return true;

   return false;
}
예제 #4
0
/**
 * 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(0);
    SimTK::Vec3 orientation(0);

    const PhysicalOffsetFrame* offset =
        dynamic_cast<const PhysicalOffsetFrame*>(&aJoint.getParentFrame());
    if (offset) {
        location = offset->get_translation();
        orientation = offset->get_orientation();
    }

    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) {
                const auto& axis = ta->getAxis();
                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;
}
예제 #5
0
/**
 * 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;
}