Added saving to file in periodic step, jump on flat ground, and walki…

…ng on ramp
ami-iit · Jun 20, 2024 · b432ecb · b432ecb
1 parent 344e088
commit b432ecb
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 0 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py
@@ -1,6 +1,7 @@
 import logging
 
 import casadi as cs
+import hdf5storage
 import idyntree.bindings as idyntree
 import liecasadi
 import numpy as np
@@ -496,3 +497,17 @@ def get_references(
         save=True,
         file_name_stem="humanoid_walking_periodic",
     )
+
+    print("Saving data to humanoid_walking_periodic.mat")
+
+    humanoid_walking_periodic = {
+        "output": output.to_dict(),
+        "guess": planner_guess.to_dict(
+            flatten=False, output_conversion=hippopt.OptimizationObject.DMConversion
+        ),
+    }
+    hdf5storage.savemat(
+        file_name="humanoid_walking_periodic.mat",
+        mdict=humanoid_walking_periodic,
+        truncate_existing=True,
+    )
diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_jump_on_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_jump_on_flat_ground.py
@@ -3,6 +3,7 @@
 import math
 
 import casadi as cs
+import hdf5storage
 import idyntree.bindings as idyntree
 import liecasadi
 import numpy as np
@@ -603,6 +604,20 @@ def get_references(
         file_name_stem="humanoid_single_jump_flat",
     )
 
+    print("Saving data to humanoid_single_jump_flat.mat")
+
+    humanoid_single_jump_flat = {
+        "output": output.to_dict(),
+        "guess": planner_guess.to_dict(
+            flatten=False, output_conversion=hippopt.OptimizationObject.DMConversion
+        ),
+    }
+    hdf5storage.savemat(
+        file_name="humanoid_single_jump_flat.mat",
+        mdict=humanoid_single_jump_flat,
+        truncate_existing=True,
+    )
+
     plotter_settings = hp_rp.FootContactStatePlotterSettings()
     plotter_settings.terrain = planner_settings.terrain
     left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings)

diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_ramp.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_ramp.py
@@ -1,6 +1,7 @@
 import logging
 
 import casadi as cs
+import hdf5storage
 import idyntree.bindings as idyntree
 import liecasadi
 import numpy as np
@@ -592,6 +593,20 @@ def get_references(
         file_name_stem="humanoid_walking_ramp",
     )
 
+    print("Saving data to humanoid_walking_ramp.mat")
+
+    humanoid_walking_ramp = {
+        "output": output.to_dict(),
+        "guess": planner_guess.to_dict(
+            flatten=False, output_conversion=hippopt.OptimizationObject.DMConversion
+        ),
+    }
+    hdf5storage.savemat(
+        file_name="humanoid_walking_ramp.mat",
+        mdict=humanoid_walking_ramp,
+        truncate_existing=True,
+    )
+
     plotter_settings = hp_rp.FootContactStatePlotterSettings()
     plotter_settings.terrain = planner_settings.terrain
     left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings)