investigate tracking performance loss

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: true  # Enable/disable running the MPC in another process in parallel of the main loop
+    enable_multiprocessing: false  # 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
@@ -22,16 +22,16 @@ robot:
     # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
     q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
     dt_wbc: 0.001  # Time step of the whole body control
-    dt_mpc: 0.01  # Time step of the model predictive control
+    dt_mpc: 0.001  # Time step of the model predictive control
     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
     interpolate_mpc: true # 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+
+    # Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
+    # Kd_main: [0., 0., 0.]  # Derivative gains for the PD+
 #     Kff_main: 0.0  # Feedforward torques multiplier for the PD+
-    # Kp_main: [3, 3, 3]  # Proportional gains for the PD+
-    # Kd_main: [0.3, 0.3, 0.3]  # Derivative gains for the PD+
+    Kp_main: [3, 3, 3]  # Proportional gains for the PD+
+    Kd_main: [0.3, 0.3, 0.3]  # Derivative gains for the PD+
     Kff_main: 1.0  # Feedforward torques multiplier for the PD+
 
     # Parameters of Gait

python/quadruped_reactive_walking/Controller.py

@@ -250,16 +250,18 @@ class Controller:
             self.result.FF = self.params.Kff_main * np.ones(12)
             self.result.tau_ff[3:6] = self.compute_torque(m)[:]
 
-            if self.params.interpolate_mpc:
-                if self.mpc_result.new_result:
-                    if self.params.interpolation_type == 3:
-                        self.interpolator.update(xs[0], xs[1], xs[2])
-                    # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
-
-                t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
-                q, v = self.interpolator.interpolate(t)
-            else:
-                q, v = self.integrate_x(m)
+            # if self.params.interpolate_mpc:
+            #     if self.mpc_result.new_result:
+            #         if self.params.interpolation_type == 3:
+            #             self.interpolator.update(xs[0], xs[1], xs[2])
+            #         # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+
+            #     t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
+            #     q, v = self.interpolator.interpolate(t)
+            # else:
+            #     q, v = self.integrate_x(m)
+            q = xs[1][:3]
+            v = xs[1][3:]
 
             self.result.q_des[3:6] = q[:]
             self.result.v_des[3:6] = v[:]
@@ -422,8 +424,9 @@ class Controller:
         #         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)
+        # 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
 
     def integrate_x(self, m):

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -147,7 +147,6 @@ class LoggerControl:
         self.plot_controller_times()
         if not self.params.enable_multiprocessing:
             self.plot_OCP_times()
-            self.plot_OCP_update_times()
 
         plt.show()