From e360f166590320dafee9266e2ffbbe646c7237e6 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 8 Feb 2021 16:20:12 +0100
Subject: [PATCH] Properly update MPC warm start when contact status is
modified
---
scripts/MPC_Wrapper.py | 11 +++++++++++
1 file changed, 11 insertions(+)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index c87c3bd4..071432cc 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):
--
GitLab