ocp plots trial

config/walk_parameters.yaml

@@ -11,9 +11,9 @@ robot:
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
     predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
-    N_SIMULATION: 5000  # Number of simulated wbc time steps
+    N_SIMULATION: 1000  # 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

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -58,7 +58,7 @@ class OCP:
         t_warm_start = time()
         self.t_warm_start = t_warm_start - t_update
 
-        # self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
+        self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
         self.ddp.solve(xs, us, self.max_iter, False)
 
         t_ddp = time()

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -147,8 +147,8 @@ class LoggerControl:
         self.plot_target(save, fileName)
         # self.plot_riccati_gains(0, save, fileName)
         self.plot_controller_times()
-        if not self.params.enable_multiprocessing:
-            self.plot_OCP_times()
+        # if not self.params.enable_multiprocessing:
+        #     self.plot_OCP_times()
 
         plt.show()
 
@@ -269,18 +269,21 @@ class LoggerControl:
         if save:
             plt.savefig(fileName + "/target")
 
-        # legend = ['x', 'y', 'z']
-        # plt.figure(figsize=(12, 18), dpi = 90)
-        # for p in range(3):
-        #     plt.subplot(3,1, p+1)
-        #     plt.title('Free foot on ' + legend[p])
-        #     for i in range(horizon-1):
-        #         t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
-        #         y = all_ocp_feet_p_log[self.pd.rfFootId][i][:,p]
-        #         for j in range(len(y) - 1):
-        #             plt.plot(t[j:j+2], y[j:j+2], color='royalblue', linewidth = 3, marker='o' ,alpha=max([1 - j/len(y), 0]))
-        #     plt.plot(t_scale, self.target[:, p], color="tomato")
-        #     plt.plot(t_scale, m_feet_p_log[self.pd.rfFootId][:, p], color="lightgreen")
+        legend = ['x', 'y', 'z']
+        plt.figure(figsize=(12, 18), dpi = 90)
+        for p in range(3):
+            plt.subplot(3,1, p+1)
+            plt.title('Free foot on ' + legend[p])
+            for i in range(horizon-1):
+                t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
+                y = all_ocp_feet_p_log[self.pd.rfFootId][i][:,p]
+                for j in range(len(y) - 1):
+                    plt.plot(t[j:j+2], y[j:j+2], color='royalblue', linewidth = 3, marker='o' ,alpha=max([1 - j/len(y), 0]))
+            plt.plot(t_scale, self.target[:, p], color="tomato")
+            plt.plot(t_scale, m_feet_p_log[self.pd.rfFootId][:, p], color="lightgreen")
+        
+        if save:
+            plt.savefig(fileName + "/ocp_predictions")
 
     def plot_riccati_gains(self, n, save=False, fileName="/tmp"):
         import matplotlib.pyplot as plt
@@ -453,6 +456,6 @@ if __name__ == "__main__":
     params = qrw.Params()
     pd = ProblemDataFull(params)
 
-    logger = LoggerControl(pd, params, file="/tmp/logs/2022_09_11_11_43/data.npz")
+    logger = LoggerControl(pd, params, file="/tmp/logs/2022_09_13_10_01/data.npz")
     logger.load()
     logger.plot()