diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py index c87c3bd4493555d9b160d3c17afc59b6f0b75a55..071432cc12f9a850e7492f55a97f35d48d45fd06 100644 --- a/scripts/MPC_Wrapper.py +++ b/scripts/MPC_Wrapper.py @@ -43,6 +43,7 @@ class MPC_Wrapper: self.dt = dt self.n_steps = n_steps self.T_gait = T_gait + self.gait_memory = np.zeros(4) self.mpc_type = mpc_type self.multiprocessing = multiprocessing @@ -81,6 +82,16 @@ class MPC_Wrapper: else: # Run in the same process than main loop self.run_MPC_synchronous(k, fstep_planner) + if k > 2 and not np.array_equal(self.gait_memory, fstep_planner.gait[0, 1:]): + self.gait_memory = (fstep_planner.gait[0, 1:]).copy() + mass = 2.5 + nb_ctc = np.sum(fstep_planner.gait[0, 1:]) + F = 9.81 * mass / nb_ctc + self.last_available_result[12:] = np.zeros(12) + for i in range(4): + if (fstep_planner.gait[0, 1+i] == 1): + self.last_available_result[12+3*i+2] = F + return 0 def get_latest_result(self):