Enforce SDF joint limits when finite (#2232)

Author: jslee02

dart/utils/sdf/SdfParser.cpp

@@ -1482,7 +1482,7 @@ static void reportMissingElement(
   DART_ASSERT(0);
 }
 
-static void readAxisElement(
+static bool readAxisElement(
     const ElementPtr& axisElement,
     const Eigen::Isometry3d& _parentModelFrame,
     Eigen::Vector3d& axis,
@@ -1494,6 +1494,8 @@ static void readAxisElement(
     double& friction,
     double& spring_stiffness)
 {
+  bool hasFinitePositionLimit = false;
+
   // use_parent_model_frame
   bool useParentModelFrame = false;
   if (hasElement(axisElement, "use_parent_model_frame"))
@@ -1538,11 +1540,13 @@ static void readAxisElement(
     // lower
     if (hasElement(limitElement, "lower")) {
       lower = getValueDouble(limitElement, "lower");
+      hasFinitePositionLimit = hasFinitePositionLimit || std::isfinite(lower);
     }
 
     // upper
     if (hasElement(limitElement, "upper")) {
       upper = getValueDouble(limitElement, "upper");
+      hasFinitePositionLimit = hasFinitePositionLimit || std::isfinite(upper);
     }
   }
 
@@ -1561,6 +1565,8 @@ static void readAxisElement(
     // Apply the same logic to the rest position.
     rest = initial;
   }
+
+  return hasFinitePositionLimit;
 }
 
 dart::dynamics::WeldJoint::Properties readWeldJoint(
@@ -1585,7 +1591,7 @@ dynamics::RevoluteJoint::Properties readRevoluteJoint(
   if (hasElement(_revoluteJointElement, "axis")) {
     const ElementPtr& axisElement = getElement(_revoluteJointElement, "axis");
 
-    readAxisElement(
+    const bool hasLimitedAxis = readAxisElement(
         axisElement,
         _parentModelFrame,
         newRevoluteJoint.mAxis,
@@ -1596,6 +1602,7 @@ dynamics::RevoluteJoint::Properties readRevoluteJoint(
         newRevoluteJoint.mDampingCoefficients[0],
         newRevoluteJoint.mFrictions[0],
         newRevoluteJoint.mSpringStiffnesses[0]);
+    newRevoluteJoint.mIsPositionLimitEnforced = hasLimitedAxis;
   } else {
     reportMissingElement("readRevoluteJoint", "axis", "joint", _name);
   }
@@ -1617,7 +1624,7 @@ dynamics::PrismaticJoint::Properties readPrismaticJoint(
   if (hasElement(_jointElement, "axis")) {
     const ElementPtr& axisElement = getElement(_jointElement, "axis");
 
-    readAxisElement(
+    const bool hasLimitedAxis = readAxisElement(
         axisElement,
         _parentModelFrame,
         newPrismaticJoint.mAxis,
@@ -1628,6 +1635,7 @@ dynamics::PrismaticJoint::Properties readPrismaticJoint(
         newPrismaticJoint.mDampingCoefficients[0],
         newPrismaticJoint.mFrictions[0],
         newPrismaticJoint.mSpringStiffnesses[0]);
+    newPrismaticJoint.mIsPositionLimitEnforced = hasLimitedAxis;
   } else {
     reportMissingElement("readPrismaticJoint", "axis", "joint", _name);
   }
@@ -1649,7 +1657,7 @@ dynamics::ScrewJoint::Properties readScrewJoint(
   if (hasElement(_jointElement, "axis")) {
     const ElementPtr& axisElement = getElement(_jointElement, "axis");
 
-    readAxisElement(
+    const bool hasLimitedAxis = readAxisElement(
         axisElement,
         _parentModelFrame,
         newScrewJoint.mAxis,
@@ -1660,6 +1668,7 @@ dynamics::ScrewJoint::Properties readScrewJoint(
         newScrewJoint.mDampingCoefficients[0],
         newScrewJoint.mFrictions[0],
         newScrewJoint.mSpringStiffnesses[0]);
+    newScrewJoint.mIsPositionLimitEnforced = hasLimitedAxis;
   } else {
     reportMissingElement("readScrewJoint", "axis", "joint", _name);
   }
@@ -1681,13 +1690,15 @@ dynamics::UniversalJoint::Properties readUniversalJoint(
   DART_ASSERT(_jointElement != nullptr);
 
   dynamics::UniversalJoint::Properties newUniversalJoint;
+  bool hasLimitedAxis1 = false;
+  bool hasLimitedAxis2 = false;
 
   //--------------------------------------------------------------------------
   // axis
   if (hasElement(_jointElement, "axis")) {
     const ElementPtr& axisElement = getElement(_jointElement, "axis");
 
-    readAxisElement(
+    hasLimitedAxis1 = readAxisElement(
         axisElement,
         _parentModelFrame,
         newUniversalJoint.mAxis[0],
@@ -1707,7 +1718,7 @@ dynamics::UniversalJoint::Properties readUniversalJoint(
   if (hasElement(_jointElement, "axis2")) {
     const ElementPtr& axis2Element = getElement(_jointElement, "axis2");
 
-    readAxisElement(
+    hasLimitedAxis2 = readAxisElement(
         axis2Element,
         _parentModelFrame,
         newUniversalJoint.mAxis[1],
@@ -1722,6 +1733,9 @@ dynamics::UniversalJoint::Properties readUniversalJoint(
     reportMissingElement("readUniversalJoint", "axis2", "joint", _name);
   }
 
+  newUniversalJoint.mIsPositionLimitEnforced
+      = hasLimitedAxis1 || hasLimitedAxis2;
+
   return newUniversalJoint;
 }
 

tests/integration/io/test_SdfParser.cpp

@@ -193,6 +193,7 @@ TEST(SdfParser, SDFJointProperties)
   const double epsilon = 1e-7;
 
   auto testProperties = [epsilon](const Joint* joint, const size_t idx) {
+    EXPECT_TRUE(joint->areLimitsEnforced()) << joint->getName();
     EXPECT_NEAR(joint->getPositionLowerLimit(idx), 0, epsilon);
     EXPECT_NEAR(joint->getPositionUpperLimit(idx), 3, epsilon);
     EXPECT_NEAR(joint->getDampingCoefficient(idx), 0, epsilon);
@@ -209,6 +210,8 @@ TEST(SdfParser, SDFJointProperties)
     } else if (joint->getType() == UniversalJoint::getStaticType()) {
       testProperties(joint, 0);
       testProperties(joint, 1);
+    } else if (joint->getType() == FreeJoint::getStaticType()) {
+      EXPECT_FALSE(joint->areLimitsEnforced());
     }
   });
 }