Fix incorrect LCP construction in JointConstraint for multi-DOFs joints (#1596)

[jslee02]

• Fix incorrect LCP construction in JointConstraint for multi-DOFs joints (fixes Gazebo crashes when universal joints reach limits since 6.11.0 #1596)
• Add universal joint type in SDFParser
• Encode visual/collision name attribute of SDF in ShapeNode name
• Bump version to 6.11.1

---

Before creating a pull request

• Document new methods and classes
• Format new code files using clang-format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[jslee02]

These two lines are the actual fix for #1596. Without these lines, this function attempted to access invalid indices of the arrays in lcp.

[scpeters]

👍

[scpeters]

Thanks for the incredibly quick fix and adding the test over. I would make one small adjustment to the test, since I tried reverting the two lines that fix the bug, and the test currently passes without the fix. I've widened the joint limits a bit in the following patch, which I think is an improvement because it makes the test fail on my machine without the fix, while it still passes with the fix:

diff --git a/data/sdf/test/test_issue1596.model b/data/sdf/test/test_issue1596.model
index 793b8467f7..ee7342057b 100644
--- a/data/sdf/test/test_issue1596.model
+++ b/data/sdf/test/test_issue1596.model
@@ -124,8 +124,8 @@
       <axis>
         <xyz>1 0 0</xyz>
         <limit>
-          <lower>-0.1</lower>
-          <upper>0.1</upper>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
           <stiffness>1e6</stiffness>
         </limit>
         <dynamics>
@@ -135,8 +135,8 @@
       <axis2>
         <xyz>0 1 0</xyz>
         <limit>
-          <lower>-0.1</lower>
-          <upper>0.1</upper>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
           <stiffness>1e6</stiffness>
         </limit>
         <dynamics>
@@ -155,8 +155,8 @@
       <axis>
         <xyz>1 0 0</xyz>
         <limit>
-          <lower>-0.1</lower>
-          <upper>0.1</upper>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
         </limit>
         <dynamics>
           <damping>0.1</damping>
@@ -165,8 +165,8 @@
       <axis2>
         <xyz>0 1 0</xyz>
         <limit>
-          <lower>-0.1</lower>
-          <upper>0.1</upper>
+          <lower>-1.2</lower>
+          <upper>1.2</upper>
         </limit>
         <dynamics>
           <damping>0.1</damping>

[jslee02]

Interesting, it was the opposite in my case. Narrowing the limits made the difference. I guess it's possible that the gravity I used (i.e., [1, 1, 0]) was different from the one in your test. Using [9.81, 9.81, 0] showed the same results as yours with the widen limits.

[scpeters]

I left comments about two asserts, but I leave judgment about them in your hands

aside from that, everything looks great to me, so I will mark this as approved

I can re-review if you decide to make further changes