TP4: Correct normal problem in collision detection.

tp4/compatibility.py

@@ -18,6 +18,8 @@
 P3X = [int(n) for n in pin.__version__.split(".")] >= [2, 99]
 HPPFCL3X = [int(n) for n in hppfcl.__version__.split(".")] >= [2, 99]
 
+assert(P3X and HPPFCL3X)
+
 # -------------------------------------------------------------------------------
 if not P3X:
     pin.ZAxis = np.array([0, 0, 1.0])

tp4/example_simu_frictionless.py

@@ -6,7 +6,6 @@
 from display_collision_patches import preallocateVisualObjects, updateVisualObjects
 from scenes import buildSceneCubes
 from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer
-from tp4.compatibility import P3X
 
 import proxsuite
 
@@ -19,7 +18,7 @@
 T = int(DURATION / DT)  # number of time steps
 WITH_CUBE_CORNERS = True  # Use contacts only at cube corners
 PRIMAL_FORMULATION = True
-DUAL_FORMULATION = False
+DUAL_FORMULATION = True
 assert PRIMAL_FORMULATION or DUAL_FORMULATION
 # Random seed for simulation initialization
 SEED = int(time.time() % 1 * 1000)
@@ -34,9 +33,7 @@
 # %jupyter_snippet init
 # ### SCENE
 # Create scene with multiple objects
-model, geom_model = buildSceneCubes(
-    3, with_floor=True, with_corner_collisions=True, with_cube_collisions=True
-)
+model, geom_model = buildSceneCubes(3)
 
 # Create the corresponding data to the models
 data = model.createData()
@@ -45,6 +42,7 @@
 for req in geom_data.collisionRequests:
     req.security_margin = 1e-3
     req.num_max_contacts = 20
+    req.enable_contact = True
 
 # ### VIZUALIZATION
 visual_model = geom_model.copy()
@@ -139,9 +137,7 @@
             # ### DUAL FORMULATION
             # Solve the dual QP (search the forces)
             # min_f .5 f D f st f>=0, D f + J vf >= 0
-            # Compatibility issue: the 3rd argument is to enforce compatibility between
-            # pinocchio 2x and 3x. Remove it if with P3X, or replace it by q if with P2X.
-            Minv = pin.computeMinverse(model, data, None if P3X else q)
+            Minv = pin.computeMinverse(model, data)
             delasus = J @ Minv @ J.T
 
             qp2 = QP(nc, 0, nc, box_constraints=True)

tp4/scenes.py

@@ -266,10 +266,10 @@ def buildSceneCubes(
     number_of_cubes,
     sizes=0.2,
     masses=1.0,
-    with_corner_collisions=True,
-    with_cube_collisions=False,
-    with_floor=False,
-    corner_insider_the_cube=True,
+    with_corner_collisions=False,
+    with_cube_collisions=True,
+    with_floor=True,
+    corner_inside_the_cube=True,
 ):
     """Creates the pinocchio pin.Model and pin.GeometryModel
     of a scene containing cubes. The number of cubes is defines by the length of
@@ -295,7 +295,7 @@ def buildSceneCubes(
 
         with_cube_collisions: add collision pairs between cubes. False by defaults (why?).
 
-        corner_insider_the_cube: added for compatibility with previous code. True by default.
+        corner_inside_the_cube: added for compatibility with previous code. True by default.
 
     Returns:
         model, geom_model
@@ -361,7 +361,7 @@ def buildSceneCubes(
         # Could be reset to a more classical order to avoid confusion later.
         corners = [
             np.array([x, y, z])
-            * (size / 2 - (ballSize if corner_insider_the_cube else 0))
+            * (size / 2 - (ballSize if corner_inside_the_cube else 0))
             for x in [1, -1]
             for z in [-1, 1]
             for y in [-1, 1]