settings

config/walk_parameters.yaml

@@ -26,7 +26,7 @@ robot:
     type_MPC: 3  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
     save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
     movement: "step" # name of the movement to perform
-    interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
+    interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
     interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
     # Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     # Kd_main: [0., 0., 0.]  # Derivative gains for the PD+

python/quadruped_reactive_walking/Controller.py

@@ -208,7 +208,9 @@ class Controller:
         t_measures = time.time()
         self.t_measures = t_measures - t_start
 
-        self.target_footstep = self.target.compute(self.k + self.pd.T * self.pd.mpc_wbc_ratio)
+        self.target_footstep = self.target.compute(
+            self.k + self.pd.T * self.pd.mpc_wbc_ratio
+        )
 
         if self.k % self.pd.mpc_wbc_ratio == 0:
             if self.mpc_solved:
@@ -216,25 +218,32 @@ class Controller:
                 self.mpc_solved = False
 
             try:
-                # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
-                if self.initialized:
-                    self.mpc.solve(
-                        self.k,
-                        self.mpc_result.xs[1],
-                        self.target_footstep.copy(),
-                        self.gait,
-                        self.xs_init,
-                        self.us_init,
-                    )
-                else:
-                    self.mpc.solve(
-                        self.k,
-                        m["x_m"],
-                        self.target_footstep.copy(),
-                        self.gait,
-                        self.xs_init,
-                        self.us_init,
-                    )
+                self.mpc.solve(
+                    self.k,
+                    m["x_m"],
+                    self.target_footstep.copy(),
+                    self.gait,
+                    self.xs_init,
+                    self.us_init,
+                )
+                # if self.initialized:
+                #     self.mpc.solve(
+                #         self.k,
+                #         self.mpc_result.xs[1],
+                #         self.target_footstep.copy(),
+                #         self.gait,
+                #         self.xs_init,
+                #         self.us_init,
+                #     )
+                # else:
+                #     self.mpc.solve(
+                #         self.k,
+                #         m["x_m"],
+                #         self.target_footstep.copy(),
+                #         self.gait,
+                #         self.xs_init,
+                #         self.us_init,
+                #     )
             except ValueError:
                 self.error = True
                 print("MPC Problem")
@@ -287,8 +296,8 @@ class Controller:
         t_send = time.time()
         self.t_send = t_send - t_mpc
 
-        # self.clamp_result(device)
-        # self.security_check(m)
+        self.clamp_result(device)
+        self.security_check(m)
 
         if self.error:
             self.set_null_control()