Fix incorrect LCP construction in JointConstraint for multi-DOFs join…

…ts (#1596) (#1597)
dartsim · Aug 23, 2021 · 763eddd · 763eddd
1 parent a105c58
commit 763eddd
Show file tree

Hide file tree

Showing 8 changed files with 349 additions and 13 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,5 +1,11 @@
 ## DART 6
 
+### [DART 6.11.1 (TBD)](https://github.com/dartsim/dart/milestone/67?closed=1)
+
+* Dynamics
+
+  * Fixed incorrect LCP construction in JointConstraint for multi-DOFs joints : [#1597](https://github.com/dartsim/dart/pull/1597)
+
 ### [DART 6.11.0 (2021-07-15)](https://github.com/dartsim/dart/milestone/64?closed=1)
 
 * Math

diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp
@@ -400,14 +400,33 @@ void JointConstraint::getInformation(ConstraintInfo* lcp)
   const int dof = static_cast<int>(mJoint->getNumDofs());
   for (int i = 0; i < dof; ++i)
   {
-    assert(lcp->w[index] == 0.0);
+    if (!mActive[i])
+      continue;
+
+#ifndef NDEBUG // debug
+    if (std::abs(lcp->w[index]) > 1e-6)
+    {
+      dterr << "Invalid " << index
+            << "-th slack variable. Expected: 0.0. Actual: "
+            << lcp->w[index] << ".\n";
+      assert(false);
+    }
+#endif
 
     lcp->b[index] = mDesiredVelocityChange[i];
 
     lcp->lo[index] = mImpulseLowerBound[i];
     lcp->hi[index] = mImpulseUpperBound[i];
 
-    assert(lcp->findex[index] == -1);
+#ifndef NDEBUG // debug
+    if (lcp->findex[index] != -1)
+    {
+      dterr << "Invalid " << index
+            << "-th friction index. Expected: -1. Actual: "
+            << lcp->findex[index] << ".\n";
+      assert(false);
+    }
+#endif
 
     if (mLifeTime[i])
       lcp->x[index] = mOldX[i];

diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp
@@ -626,7 +626,7 @@ std::pair<dynamics::Joint*, dynamics::BodyNode*> createJointAndNodePair(
         parent,
         static_cast<const dynamics::ScrewJoint::Properties&>(*joint.properties),
         static_cast<const typename NodeType::Properties&>(*node.properties));
-  else if (std::string("revolute2") == type)
+  else if (std::string("revolute2") == type || std::string("universal") == type)
     return skeleton->createJointAndBodyNodePair<dynamics::UniversalJoint>(
         parent,
         static_cast<const dynamics::UniversalJoint::Properties&>(
@@ -1028,10 +1028,18 @@ void readVisualizationShapeNode(
     const common::Uri& baseUri,
     const common::ResourceRetrieverPtr& retriever)
 {
+  std::string visualName = "visual shape";
+  if (hasAttribute(vizShapeNodeEle, "name")) {
+    visualName = getAttributeString(vizShapeNodeEle, "name");
+  } else {
+    dtwarn << "Missing required attribute [name] in <visual> element of "
+           << "<link name = " << bodyNode->getName() << ">.\n";
+  }
+
   dynamics::ShapeNode* newShapeNode = readShapeNode(
       bodyNode,
       vizShapeNodeEle,
-      bodyNode->getName() + " - visual shape",
+      bodyNode->getName() + " - " + visualName,
       baseUri,
       retriever);
 
@@ -1052,10 +1060,18 @@ void readCollisionShapeNode(
     const common::Uri& baseUri,
     const common::ResourceRetrieverPtr& retriever)
 {
+  std::string collName = "collision shape";
+  if (hasAttribute(collShapeNodeEle, "name")) {
+    collName = getAttributeString(collShapeNodeEle, "name");
+  } else {
+    dtwarn << "Missing required attribute [name] in <collision> element of "
+           << "<link name = " << bodyNode->getName() << ">.\n";
+  }
+
   dynamics::ShapeNode* newShapeNode = readShapeNode(
       bodyNode,
       collShapeNodeEle,
-      bodyNode->getName() + " - collision shape",
+      bodyNode->getName() + " - " + collName,
       baseUri,
       retriever);
 
@@ -1224,23 +1240,42 @@ SDFJoint readJoint(
       = (childWorld * childToJoint).inverse() * _skeletonFrame;
 
   if (type == std::string("fixed"))
+  {
     newJoint.properties = dynamics::WeldJoint::Properties::createShared(
         readWeldJoint(_jointElement, parentModelFrame, name));
-  if (type == std::string("prismatic"))
+  }
+  else if (type == std::string("prismatic"))
+  {
     newJoint.properties = dynamics::PrismaticJoint::Properties::createShared(
         readPrismaticJoint(_jointElement, parentModelFrame, name));
-  if (type == std::string("revolute"))
+  }
+  else if (type == std::string("revolute"))
+  {
     newJoint.properties = dynamics::RevoluteJoint::Properties::createShared(
         readRevoluteJoint(_jointElement, parentModelFrame, name));
-  if (type == std::string("screw"))
+  }
+  else if (type == std::string("screw"))
+  {
     newJoint.properties = dynamics::ScrewJoint::Properties::createShared(
         readScrewJoint(_jointElement, parentModelFrame, name));
-  if (type == std::string("revolute2"))
+  }
+  else if (type == std::string("revolute2") || type == std::string("universal"))
+  {
     newJoint.properties = dynamics::UniversalJoint::Properties::createShared(
         readUniversalJoint(_jointElement, parentModelFrame, name));
-  if (type == std::string("ball"))
+  }
+  else if (type == std::string("ball"))
+  {
     newJoint.properties = dynamics::BallJoint::Properties::createShared(
         readBallJoint(_jointElement, parentModelFrame, name));
+  }
+  else
+  {
+    dterr << "Unsupported joint type [" << type
+          << "]. Using [fixed] joint type instead.\n";
+    newJoint.properties = dynamics::WeldJoint::Properties::createShared(
+        readWeldJoint(_jointElement, parentModelFrame, name));
+  }
 
   newJoint.type = type;
 

diff --git a/data/sdf/test/test_issue1596.model b/data/sdf/test/test_issue1596.model
@@ -0,0 +1,182 @@
+<sdf version="1.4">
+  <model name="model_1">
+    <link name="link_00">
+      <gravity>true</gravity>
+      <pose>0 0 2 0 0 0</pose>
+      <inertial>
+        <mass>0.1</mass>
+        <pose>0 0 -0.5 0 0 0</pose>
+        <inertia>
+          <ixx>0.01</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.01</iyy>
+          <iyz>0</iyz>
+          <izz>0.01</izz>
+        </inertia>
+      </inertial>
+      <collision name="col">
+        <pose>0 0 -0.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.10 0.20 1.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="vis">
+        <pose>0 0 -0.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.10 0.20 1.0</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Green</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="vis2">
+        <pose>0 0.125 -0.5 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.05</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Green</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+    <link name="link_01">
+      <gravity>true</gravity>
+      <pose>0 0 1.0 0 0 0</pose>
+      <inertial>
+        <mass>0.1</mass>
+        <pose>0 0 -0.5 0 0 0</pose>
+        <inertia>
+          <ixx>0.01</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.01</iyy>
+          <iyz>0</iyz>
+          <izz>0.01</izz>
+        </inertia>
+      </inertial>
+      <collision name="col">
+        <pose>0 0 -0.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.10 0.20 1.0</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.005</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>1.0</mu2>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+      <visual name="vis">
+        <pose>0 0 -0.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.10 0.20 1.0</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Red</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="vis2">
+        <pose>0 0.125 -0.5 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.05</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Green</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+
+    <joint name="joint_00" type="universal">
+      <parent>world</parent>
+      <child>link_00</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
+          <stiffness>1e6</stiffness>
+        </limit>
+        <dynamics>
+          <damping>0.0001</damping>
+        </dynamics>
+      </axis>
+      <axis2>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
+          <stiffness>1e6</stiffness>
+        </limit>
+        <dynamics>
+          <damping>0.0001</damping>
+        </dynamics>
+      </axis2>
+      <physics>
+        <ode>
+          <cfm_damping>1</cfm_damping>
+        </ode>
+      </physics>
+    </joint>
+    <joint name="joint_01" type="universal">
+      <child>link_01</child>
+      <parent>link_00</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+      <axis2>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis2>
+      <physics>
+        <ode>
+          <cfm_damping>1</cfm_damping>
+        </ode>
+      </physics>
+    </joint>
+  </model>
+</sdf>
diff --git a/package.xml b/package.xml
@@ -4,7 +4,7 @@
        a Catkin workspace. Catkin is not required to build DART. For more
        information, see: http://ros.org/reps/rep-0136.html -->
   <name>dartsim</name>
-  <version>6.11.0</version>
+  <version>6.11.1</version>
   <description>
     DART (Dynamic Animation and Robotics Toolkit) is a collaborative,
     cross-platform, open source library created by the Georgia Tech Graphics

diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt
@@ -5,6 +5,9 @@ dart_add_test("regression" test_Issue1243 test_Issue1243.cpp)
 if(TARGET dart-utils)
   dart_add_test("regression" test_Issue1583)
   target_link_libraries(test_Issue1583 dart-utils)
+
+  dart_add_test("regression" test_Issue1596)
+  target_link_libraries(test_Issue1596 dart-utils)
 endif()
 
 if(TARGET dart-utils-urdf)