Merge pull request loco-3d#1215 from cmastalli/topic/display-thrust

Extended displayers to show thrust + cleaned their code
PepMS · Jan 27, 2024 · dd5318f · dd5318f
2 parents 38c9eb3 + b02781b
commit dd5318f
Show file tree

Hide file tree

Showing 13 changed files with 399 additions and 352 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+* Enabled display of thrust and simplied displayers code in https://github.com/loco-3d/crocoddyl/pull/1215
 * Introduced floating base thruster actuation model in https://github.com/loco-3d/crocoddyl/pull/1213
 * Fixed quadruped and biped examples in https://github.com/loco-3d/crocoddyl/pull/1208
 * Fixed terminal computation in Python models in https://github.com/loco-3d/crocoddyl/pull/1204

diff --git a/bindings/python/crocoddyl/__init__.py b/bindings/python/crocoddyl/__init__.py
diff --git a/examples/bipedal_walk_fwddyn.py b/examples/bipedal_walk_fwddyn.py
@@ -111,11 +111,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [3.0, 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]
-        )
+        display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(talos_legs, frameNames=[rightFoot, leftFoot])
+        display = crocoddyl.MeshcatDisplay(talos_legs)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/bipedal_walk_invdyn.py b/examples/bipedal_walk_invdyn.py
@@ -111,11 +111,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [3.0, 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]
-        )
+        display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(talos_legs, frameNames=[rightFoot, leftFoot])
+        display = crocoddyl.MeshcatDisplay(talos_legs)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/bipedal_walk_ubound.py b/examples/bipedal_walk_ubound.py
@@ -114,11 +114,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [3.0, 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]
-        )
+        display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(talos_legs, frameNames=[rightFoot, leftFoot])
+        display = crocoddyl.MeshcatDisplay(talos_legs)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/boxfddp_vs_boxddp.py b/examples/boxfddp_vs_boxddp.py
@@ -100,13 +100,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [2.0, 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(
-            anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.MeshcatDisplay(anymal)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/humanoid_manipulation.py b/examples/humanoid_manipulation.py
@@ -45,24 +45,6 @@
 comRef = (rfPos0 + lfPos0) / 2
 comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item()
 
-# Initialize viewer
-display = None
-if WITHDISPLAY:
-    if display is None:
-        try:
-            import gepetto
-
-            gepetto.corbaserver.Client()
-            display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
-            display.robot.viewer.gui.addSphere(
-                "world/point", 0.05, [1.0, 0.0, 0.0, 1.0]
-            )  # radius = .1, RGBA=1001
-            display.robot.viewer.gui.applyConfiguration(
-                "world/point", [*target.tolist(), 0.0, 0.0, 0.0, 1.0]
-            )  # xyz+quaternion
-        except Exception:
-            display = crocoddyl.MeshcatDisplay(robot, frameNames=[rightFoot, leftFoot])
-
 # Add contact to the model
 contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu)
 supportContactModelLeft = crocoddyl.ContactModel6D(
@@ -180,17 +162,22 @@
 solver.solve(xs, us, 500, False, 0.1)
 
 # Visualizing the solution in gepetto-viewer
+display = None
 if WITHDISPLAY:
-    try:
-        import gepetto
-
-        gepetto.corbaserver.Client()
-        cameraTF = [1.4, 0.0, 0.2, 0.5, 0.5, 0.5, 0.5]
-        display = crocoddyl.GepettoDisplay(
-            robot, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]
-        )
-    except Exception:
-        display = crocoddyl.MeshcatDisplay(robot, frameNames=[rightFoot, leftFoot])
+    if display is None:
+        try:
+            import gepetto
+
+            gepetto.corbaserver.Client()
+            display = crocoddyl.GepettoDisplay(robot)
+            display.robot.viewer.gui.addSphere(
+                "world/point", 0.05, [1.0, 0.0, 0.0, 1.0]
+            )  # radius = .1, RGBA=1001
+            display.robot.viewer.gui.applyConfiguration(
+                "world/point", [*target.tolist(), 0.0, 0.0, 0.0, 1.0]
+            )  # xyz+quaternion
+        except Exception:
+            display = crocoddyl.MeshcatDisplay(robot)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/humanoid_manipulation_ubound.py b/examples/humanoid_manipulation_ubound.py
@@ -48,23 +48,6 @@
 comRef = (rfPos0 + lfPos0) / 2
 comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item()
 
-# Initialize viewer
-display = None
-if WITHDISPLAY:
-    if display is None:
-        try:
-            import gepetto
-
-            gepetto.corbaserver.Client()
-            display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
-            display.robot.viewer.gui.addSphere(
-                "world/point", 0.05, [1.0, 0.0, 0.0, 1.0]
-            )  # radius = .1, RGBA=1001
-            display.robot.viewer.gui.applyConfiguration(
-                "world/point", [*target.tolist(), 0.0, 0.0, 0.0, 1.0]
-            )  # xyz+quaternion
-        except Exception:
-            display = crocoddyl.MeshcatDisplay(robot, frameNames=[rightFoot, leftFoot])
 # Add contact to the model
 contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu)
 supportContactModelLeft = crocoddyl.ContactModel6D(
@@ -182,17 +165,22 @@
 solver.solve(xs, us, 500, False, 1e-9)
 
 # Visualizing the solution in gepetto-viewer
+display = None
 if WITHDISPLAY:
-    try:
-        import gepetto
-
-        gepetto.corbaserver.Client()
-        cameraTF = [1.4, 0.0, 0.2, 0.5, 0.5, 0.5, 0.5]
-        display = crocoddyl.GepettoDisplay(
-            robot, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]
-        )
-    except Exception:
-        display = crocoddyl.MeshcatDisplay(robot, frameNames=[rightFoot, leftFoot])
+    if display is None:
+        try:
+            import gepetto
+
+            gepetto.corbaserver.Client()
+            display = crocoddyl.GepettoDisplay(robot)
+            display.robot.viewer.gui.addSphere(
+                "world/point", 0.05, [1.0, 0.0, 0.0, 1.0]
+            )  # radius = .1, RGBA=1001
+            display.robot.viewer.gui.applyConfiguration(
+                "world/point", [*target.tolist(), 0.0, 0.0, 0.0, 1.0]
+            )  # xyz+quaternion
+        except Exception:
+            display = crocoddyl.MeshcatDisplay(robot)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/humanoid_taichi.py b/examples/humanoid_taichi.py
@@ -48,24 +48,6 @@
 comRef = (rfPos0 + lfPos0) / 2
 comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item()
 
-# Initialize viewer
-display = None
-if WITHDISPLAY:
-    if display is None:
-        try:
-            import gepetto
-
-            gepetto.corbaserver.Client()
-            display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
-            display.robot.viewer.gui.addSphere(
-                "world/point", 0.05, [1.0, 0.0, 0.0, 1.0]
-            )  # radius = .1, RGBA=1001
-            display.robot.viewer.gui.applyConfiguration(
-                "world/point", [*target.tolist(), 0.0, 0.0, 0.0, 1.0]
-            )  # xyz+quaternion
-        except Exception:
-            display = crocoddyl.MeshcatDisplay(robot, frameNames=[rightFoot, leftFoot])
-
 # Create two contact models used along the motion
 contactModel1Foot = crocoddyl.ContactModelMultiple(state, actuation.nu)
 contactModel2Feet = crocoddyl.ContactModelMultiple(state, actuation.nu)
@@ -230,17 +212,22 @@
 solver.solve(xs, us, 500, False, 1e-9)
 
 # Visualizing the solution in gepetto-viewer
+display = None
 if WITHDISPLAY:
-    try:
-        import gepetto
+    if display is None:
+        try:
+            import gepetto
 
-        gepetto.corbaserver.Client()
-        cameraTF = [1.4, 0.0, 0.2, 0.5, 0.5, 0.5, 0.5]
-        display = crocoddyl.GepettoDisplay(
-            robot, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot]
-        )
-    except Exception:
-        display = crocoddyl.MeshcatDisplay(robot, frameNames=[rightFoot, leftFoot])
+            gepetto.corbaserver.Client()
+            display = crocoddyl.GepettoDisplay(robot)
+            display.robot.viewer.gui.addSphere(
+                "world/point", 0.05, [1.0, 0.0, 0.0, 1.0]
+            )  # radius = .1, RGBA=1001
+            display.robot.viewer.gui.applyConfiguration(
+                "world/point", [*target.tolist(), 0.0, 0.0, 0.0, 1.0]
+            )  # xyz+quaternion
+        except Exception:
+            display = crocoddyl.MeshcatDisplay(robot)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/notebooks/bipedal_walking.ipynb b/examples/notebooks/bipedal_walking.ipynb
@@ -102,7 +102,7 @@
     "ddp = crocoddyl.SolverFDDP(problem)\n",
     "\n",
     "# Using the meshcat displayer, you could enable gepetto viewer for nicer view\n",
-    "# display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, frameNames=[rightFoot, leftFoot])\n",
+    "# display = crocoddyl.GepettoDisplay(talos_legs, 4, 4)\n",
     "display = crocoddyl.MeshcatDisplay(talos_legs, 4, 4, False)\n",
     "ddp.setCallbacks(\n",
     "    [\n",

diff --git a/examples/quadrupedal_gaits_fwddyn.py b/examples/quadrupedal_gaits_fwddyn.py
@@ -166,13 +166,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [2.0, 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(
-            anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.MeshcatDisplay(anymal)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/quadrupedal_gaits_invdyn.py b/examples/quadrupedal_gaits_invdyn.py
@@ -168,13 +168,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [2.0, 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(
-            anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.MeshcatDisplay(anymal)
     display.rate = -1
     display.freq = 1
     while True:

diff --git a/examples/quadrupedal_walk_ubound.py b/examples/quadrupedal_walk_ubound.py
@@ -93,13 +93,9 @@
 
         gepetto.corbaserver.Client()
         cameraTF = [2.0, 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
-        display = crocoddyl.GepettoDisplay(
-            anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF)
     except Exception:
-        display = crocoddyl.MeshcatDisplay(
-            anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot]
-        )
+        display = crocoddyl.MeshcatDisplay(anymal)
     display.rate = -1
     display.freq = 1
     while True: