Merge pull request #2502 from jorisv/topic/fix_coal

Fix coal integration

.github/workflows/update_pixi_lockfile.yml

@@ -48,5 +48,6 @@ jobs:
           labels: |
             pixi
             no changelog
+            build_all
           delete-branch: true
           add-paths: pixi.lock

CHANGELOG.md

@@ -14,6 +14,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Fix undefined behavior when using the site attribute in mjcf ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477))
 - Fix the type of image paths when loading textures in the meshcat visualizer ([#2478](https://github.com/stack-of-tasks/pinocchio/pull/2478))
 - Fix meshcat examples ([#2503])[https://github.com/stack-of-tasks/pinocchio/pull/2503]
+- Fix `pinocchio-test-cpp-parallel-geometry` when built with Coal ([#2502](https://github.com/stack-of-tasks/pinocchio/pull/2502))
 
 ### Changed
 - On GNU/Linux and macOS, hide all symbols by default ([#2469](https://github.com/stack-of-tasks/pinocchio/pull/2469))

examples/casadi/quadrotor-ocp.py

@@ -192,9 +192,8 @@ def solve(self, approx_hessian=True):
         except:  # noqa: E722
             self.sol = self.opti.debug
 
-        if self.sol.stats()["return_status"] == "Solve_Succeeded":
-            self._retract_trajectory()
-            self._compute_gaps()
+        self._retract_trajectory()
+        self._compute_gaps()
 
     def _retract_trajectory(self):
         self.xs = []

unittest/parallel-geometry.cpp

@@ -244,9 +244,43 @@ BOOST_AUTO_TEST_CASE(test_talos)
     const CollisionPair & cp = geometry_model.collisionPairs[k];
     BOOST_CHECK(geometry_data_ref.oMg[cp.first] == geometry_data.oMg[cp.first]);
     BOOST_CHECK(geometry_data_ref.oMg[cp.second] == geometry_data.oMg[cp.second]);
-    BOOST_CHECK(
-      geometry_data_ref.collisionResults[k].getContacts()
-      == geometry_data.collisionResults[k].getContacts());
+    BOOST_REQUIRE_EQUAL(
+      geometry_data_ref.collisionResults[k].getContacts().size(),
+      geometry_data.collisionResults[k].getContacts().size());
+    // This code is a workaround for https://github.com/coal-library/coal/issues/636
+    for (size_t l = 0; l < geometry_data.collisionResults[k].getContacts().size(); ++l)
+    {
+      const auto & contact = geometry_data.collisionResults[k].getContacts()[l];
+      const auto & contact_ref = geometry_data_ref.collisionResults[k].getContacts()[l];
+
+      // If contact is not filled with NaN do the standard comparison
+      if (contact.normal == contact.normal)
+      {
+        BOOST_CHECK(contact == contact_ref);
+      }
+      else
+      {
+        // Only run this part in coal, this issue doesn't happen in hpp-fcl
+#if HPP_FCL_VERSION_AT_LEAST(3, 0, 0)
+        // Compare standard values
+        BOOST_CHECK_EQUAL(contact.o1, contact_ref.o1);
+        BOOST_CHECK_EQUAL(contact.o2, contact_ref.o2);
+        BOOST_CHECK_EQUAL(contact.b1, contact_ref.b1);
+        BOOST_CHECK_EQUAL(contact.b2, contact_ref.b2);
+        BOOST_CHECK_EQUAL(contact.penetration_depth, contact_ref.penetration_depth);
+
+        // Check all is set to NaN
+        BOOST_CHECK(contact.normal != contact.normal);
+        BOOST_CHECK(contact.pos != contact.pos);
+        BOOST_CHECK(contact.nearest_points[0] != contact.nearest_points[0]);
+        BOOST_CHECK(contact.nearest_points[1] != contact.nearest_points[1]);
+        BOOST_CHECK(contact_ref.normal != contact_ref.normal);
+        BOOST_CHECK(contact_ref.pos != contact_ref.pos);
+        BOOST_CHECK(contact_ref.nearest_points[0] != contact_ref.nearest_points[0]);
+        BOOST_CHECK(contact_ref.nearest_points[1] != contact_ref.nearest_points[1]);
+#endif // hpp-fcl >= 3.0.0
+      }
+    }
   }
 }