Added saving to file of walking on stairs output

src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py

@@ -1,6 +1,7 @@
 import logging
 
 import casadi as cs
+import hdf5storage
 import idyntree.bindings as idyntree
 import liecasadi
 import numpy as np
@@ -584,6 +585,21 @@ def get_references(
         file_name_stem="humanoid_walking_step",
     )
 
+    print("Saving data to humanoid_walking_step.mat")
+
+    humanoid_walking_step = {
+        "output": output.to_dict(),
+        "guess": planner_guess.to_dict(
+            flatten=False,
+            output_conversion=hippopt.OptimizationObject.DMConversion,
+        ),
+    }
+    hdf5storage.savemat(
+        file_name="humanoid_walking_step.mat",
+        mdict=humanoid_walking_step,
+        truncate_existing=True,
+    )
+
     plotter_settings = hp_rp.FootContactStatePlotterSettings()
     plotter_settings.terrain = planner_settings.terrain
     left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings)