TP4: minor cleaning, arriving to the example simu not working with 3 …

…cubes.

tp4/example_proxqp.py

@@ -40,7 +40,7 @@
 NEQ = 5  # number of equalities
 NINEQ = 3  # Number of inequalities
 WITH_BOUNDS = True  # Additional bounds on x
-VERBOSE = False  # Do you want to see the result?
+VERBOSE = True  # Do you want to see the result?
 ACCURACY = 1e-6  # Threshold for solver stoping criteria and posterior checks
 # %end_jupyter_snippet
 

tp4/example_separate_objects.py

@@ -1,3 +1,14 @@
+'''
+Integrative example using the collision model.
+Load a complex scene (pills box), then iteratively applies an ad-hoc
+repulsive field to each pair of objects that are colliding. The script
+then converges to a collision free scene, with distance margin.
+The movement of the scene is displayed in meshcat in real-time.
+The convergence of the distances of each collision pair with negative 
+(colliding) distances is finally plot.
+'''
+
+
 import matplotlib.pyplot as plt
 import numpy as np
 import pinocchio as pin
@@ -118,6 +129,9 @@
     # Finally, modify the current config q with the push dq
     q = pin.integrate(model, q, dq)
 
+    print(f'Iteration #{i}/{NB_ITER}: {len(contact_models)} active collision '
+          +f'pairs (given margin {EPSILON})')
+
     # Display the current configuration
     if i % 10 == 0:
         # Meshcat is slow to display the patches, display once in a while

tp4/example_simu_frictionless.py

@@ -19,12 +19,12 @@
 T = int(DURATION / DT)  # number of time steps
 WITH_CUBE_CORNERS = True  # Use contacts only at cube corners
 PRIMAL_FORMULATION = True
-DUAL_FORMULATION = True
+DUAL_FORMULATION = False
 assert PRIMAL_FORMULATION or DUAL_FORMULATION
 # Random seed for simulation initialization
 SEED = int(time.time() % 1 * 1000)
-print("SEED = ", SEED)
 SEED = 1
+print("SEED = ", SEED)
 
 # ### RANDOM INIT
 # Init random generators
@@ -35,7 +35,7 @@
 # ### SCENE
 # Create scene with multiple objects
 model, geom_model = buildSceneCubes(
-    1, with_floor=True, with_corner_collisions=True, with_cube_collisions=False
+    3, with_floor=True, with_corner_collisions=True, with_cube_collisions=True
 )
 
 # Create the corresponding data to the models
@@ -48,7 +48,7 @@
 
 # ### VIZUALIZATION
 visual_model = geom_model.copy()
-preallocateVisualObjects(visual_model, 10)
+preallocateVisualObjects(visual_model, 16)
 viz = MeshcatVisualizer(
     model=model, collision_model=geom_model, visual_model=visual_model
 )
@@ -111,8 +111,8 @@
             # min_v  .5 vMv - vfMv st Jv>=0
             qp1 = QP(model.nv, 0, nc, False)
             qp1.init(
-                data.M, -data.M @ vf, [], [], J, l=np.zeros(nc)
-            )  # ,u=np.ones(nc)*1e20)
+                H=data.M, g=-data.M @ vf, A=None, b=None, C=J, l=np.zeros(nc)
+            )
             qp1.settings.eps_abs = 1e-12
             qp1.solve()
 

tp4/scenes.py

@@ -161,7 +161,6 @@ def addFloor(
     for ig, g in enumerate(geom_model.geometryObjects):
         if g.name != "floor":
             geom_model.addCollisionPair(pin.CollisionPair(ig, ifloor))
-            # geom_model.addCollisionPair(pin.CollisionPair(ifloor,ig))
 
 
 def addBox(