diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml index ca11e4612e3da58e8979f98b592381281200ed13..7fdbcff79a8bceb90c8bd08fb3b0b30247416fb5 100644 --- a/config/walk_parameters.yaml +++ b/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 diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py index c3df055dce66463118f770ce0aecdeb7781328f6..10de55563d80fc5ee12e1589b02f74b5ba29d3f3 100644 --- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py +++ b/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() diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py index 689e6bb482589f82aa7996079445ccb1a9ecb667..39fd08a3247ad3329c6dd3bc1c0eedb4009171d0 100644 --- a/python/quadruped_reactive_walking/tools/LoggerControl.py +++ b/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()