Added visualization of forces in flat ground planner

ami-iit · Nov 2, 2023 · 0ccf11b · 0ccf11b
1 parent 456820f
commit 0ccf11b
Showing 1 changed file with 20 additions and 6 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py
@@ -402,21 +402,35 @@ def get_references(
 
     plotter_settings = hp_rp.FootContactStatePlotterSettings()
     plotter_settings.terrain = planner_settings.terrain
-    left_complementarity_plotter = hp_rp.FootContactStatePlotter(plotter_settings)
-    left_complementarity_plotter.plot_complementarity(
+    left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings)
+    right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings)
+
+    left_foot_plotter.plot_complementarity(
         states=left_contact_points,
         time_s=output.values.dt,
         title="Left Foot Complementarity",
         blocking=False,
     )
-    right_complementarity_plotter = hp_rp.FootContactStatePlotter(plotter_settings)
-    right_complementarity_plotter.plot_complementarity(
+    right_foot_plotter.plot_complementarity(
         states=right_contact_points,
         time_s=output.values.dt,
         title="Right Foot Complementarity",
         blocking=False,
     )
 
+    left_foot_plotter.plot_forces(
+        states=left_contact_points,
+        time_s=output.values.dt,
+        title="Left Foot Forces",
+        blocking=False,
+    )
+    right_foot_plotter.plot_forces(
+        states=right_contact_points,
+        time_s=output.values.dt,
+        title="Right Foot Forces",
+        blocking=False,
+    )
+
     visualizer.visualize(
         state=humanoid_states, timestep_s=output.values.dt, time_multiplier=2.0
     )
@@ -431,5 +445,5 @@ def get_references(
 
     print("Press [Enter] to exit.")
     input()
-    left_complementarity_plotter.close()
-    right_complementarity_plotter.close()
+    left_foot_plotter.close()
+    right_foot_plotter.close()