added riccatio gains

config/walk_parameters.yaml

@@ -13,7 +13,7 @@ robot:
     predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
     N_SIMULATION: 10000  # Number of simulated wbc time steps
     enable_corba_viewer: false  # Enable/disable Corba Viewer
-    enable_multiprocessing: false  # Enable/disable running the MPC in another process in parallel of the main loop
+    enable_multiprocessing: true  # Enable/disable running the MPC in another process in parallel of the main loop
     perfect_estimator: true  # Enable/disable perfect estimator by using data directly from PyBullet
 
     # General control parameters

python/quadruped_reactive_walking/Controller.py

@@ -414,17 +414,17 @@ class Controller:
         """
         Compute the feedforward torque using ricatti gains
         """
-        # x_diff = np.concatenate(
-        #     [
-        #         pin.difference(
-        #             self.pd.model,
-        #             m["x_m"][: self.pd.nq],
-        #             self.mpc_result.xs[0][: self.pd.nq],
-        #         ),
-        #         m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
-        #     ]
-        # )
-        x_diff = self.mpc_result.xs[0] - m["x_m"]
+        x_diff = np.concatenate(
+            [
+                pin.difference(
+                    self.pd.model,
+                    m["x_m"][: self.pd.nq],
+                    self.mpc_result.xs[0][: self.pd.nq],
+                ),
+                m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
+            ]
+        )
+        # x_diff = self.mpc_result.xs[0] - m["x_m"]
         tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
         # tau = self.mpc_result.us[0]
         return tau