fix results export issue

config/walk_parameters.yaml

@@ -38,7 +38,7 @@ robot:
     # Parameters of Gait
     N_periods: 1
     gait: [20, 1, 0, 0, 1,
-           20, 0, 1, 1, 0]  # Initial gait matrix
+           20, 1, 1, 1, 1]  # Initial gait matrix
 
     # Parameters of Joystick
     gp_alpha_vel: 0.003  # Coefficient of the low pass filter applied to gamepad velocity

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -211,7 +211,7 @@ class OCP:
         self.results.a = acs_sol
         self.results.u = us_sol
         self.results.fs = fsol_to_ws
-        return self.results
+        return self.results.x, self.results.a, self.results.u, self.results.fs, 0
 
     def get_feet_position(self, xs_sol):
         """ Get the feet positions computed durnig one ocp

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -92,14 +92,14 @@ class ProblemData(problemDataAbstract):
         # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.base_velocity_tracking_w = 1e2
-        self.ground_collision_w = 1e3
+        self.base_velocity_tracking_w = 1e2*0
+        self.ground_collision_w = 1e3*0
 
         self.friction_cone_w = 1e4
         self.impact_altitude_w= 1e3
         self.impact_velocity_w = 1e3
 
-        self.control_bound_w = 1e3 * 0
+        self.control_bound_w = 1e3*0
         self.control_reg_w = 1e0
         self.state_reg_w = np.array([0] * 3
                                     + [1e1] * 3

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -16,7 +16,7 @@ class Result:
         self.xs = list(np.zeros((pd.T + 1, pd.nx)))
         self.us = list(np.zeros((pd.T, pd.nu)))
         self.acs = list(np.zeros((pd.T, pd.nv)))
-        self.fs = list(np.zeros((pd.T, pd.nu)))
+        self.fs = list(np.zeros((pd.T, 12)))
         self.K = list(np.zeros([pd.T, pd.nu, pd.ndx]))
         self.solving_duration = 0.0
         self.new_result = False
@@ -98,12 +98,11 @@ class MPC_Wrapper:
         Run the MPC (synchronous version)
         """
         self.ocp.solve(x0, tasks, gait, guess)
-        (
-            self.last_available_result.xs,
-            self.last_available_result.us,
-            self.last_available_result.K,
-            self.last_available_result.solving_duration,
-        ) = self.ocp.get_results()
+        self.last_available_result.xs,\
+        self.last_available_result.acs,\
+        self.last_available_result.us,\
+        self.last_available_result.fs,\
+        self.last_available_result.solving_duration  = self.ocp.get_results()
         self.new_result.value = True
 
     def run_MPC_asynchronous(self, k, x0, tasks, gait, xs, us):