diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py index 5a141be1cb417241e19d017e5efc40e51ddcc7b4..6949537f0985bfe6939240d76f6eacb0fbc5c0be 100644 --- a/python/quadruped_reactive_walking/tools/LoggerControl.py +++ b/python/quadruped_reactive_walking/tools/LoggerControl.py @@ -157,13 +157,15 @@ class LoggerControl: plt.style.use("seaborn") legend = ["Hip", "Shoulder", "Knee"] + t_range = np.array([k * self.pd.dt_wbc for k in range(self.tstamps.shape[0])]) + plt.figure(figsize=(12, 6), dpi=90) i = 0 for i in range(4): plt.subplot(2, 2, i + 1) plt.title("Joint position of " + str(i)) [ - plt.plot(np.array(self.q_mes)[:, (3 * i + jj)] * 180 / np.pi) + plt.plot(t_range, np.array(self.q_mes)[:, (3 * i + jj)] * 180 / np.pi) for jj in range(3) ] plt.ylabel("Joint position [deg]") @@ -179,7 +181,7 @@ class LoggerControl: plt.subplot(2, 2, i + 1) plt.title("Joint velocity of " + str(i)) [ - plt.plot(np.array(self.v_mes)[:, (3 * i + jj)] * 180 / np.pi) + plt.plot(t_range, np.array(self.v_mes)[:, (3 * i + jj)] * 180 / np.pi) for jj in range(3) ] plt.ylabel("Joint velocity [deg/s]") @@ -194,13 +196,15 @@ class LoggerControl: plt.style.use("seaborn") legend = ["Hip", "Shoulder", "Knee"] + t_range = np.array([k * self.pd.dt_wbc for k in range(self.tstamps.shape[0])]) + plt.figure(figsize=(12, 6), dpi=90) i = 0 for i in range(4): plt.subplot(2, 2, i + 1) plt.title("Joint torques of " + str(i)) [ - plt.plot(np.array(self.torquesFromCurrentMeasurment)[:, (3 * i + jj)]) + plt.plot(t_range, np.array(self.torquesFromCurrentMeasurment)[:, (3 * i + jj)]) for jj in range(3) ] plt.ylabel("Torque [Nm]") @@ -252,30 +256,31 @@ class LoggerControl: # Target plot legend = ["x", "y", "z"] + t_range = np.array([k * self.pd.dt_wbc for k in range(self.tstamps.shape[0])]) fig, axs = plt.subplots(3, sharex=True) for p in range(3): axs[p].set_title("Free foot on " + legend[p]) - axs[p].plot(self.target[:, p], label="Target") - axs[p].plot(m_feet_p_log[self.pd.rfFootId][:, p], label="Measured") - axs[p].plot(feet_p_log[self.pd.rfFootId][:, p], label="Predicted") + axs[p].plot(t_range, self.target[:, p], label="Target") + axs[p].plot(t_range, m_feet_p_log[self.pd.rfFootId][:, p], label="Measured") + axs[p].plot(t_range, feet_p_log[self.pd.rfFootId][:, p], label="Predicted") axs[p].legend() 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]) - # plt.plot(t_scale, self.target[:, p], color="tomato") - # plt.plot(t_scale, m_feet_p_log[self.pd.rfFootId][:, p], color="lightgreen") - # 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])) + 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") def plot_riccati_gains(self, n, save=False, fileName="/tmp"): import matplotlib.pyplot as plt @@ -307,7 +312,7 @@ class LoggerControl: def plot_controller_times(self): import matplotlib.pyplot as plt - t_range = np.array([k * self.pd.dt for k in range(self.tstamps.shape[0])]) + t_range = np.array([k * self.pd.dt_wbc for k in range(self.tstamps.shape[0])]) plt.figure() plt.scatter(t_range, self.t_measures, marker = "+")