From bf90a57adc260af70a543bbe47c6b0f7871e527b Mon Sep 17 00:00:00 2001
From: matssteinweg <mats.steinweg@me.com>
Date: Sun, 1 Dec 2019 16:40:04 +0100
Subject: [PATCH] Update MPC controller to use previously predicted control
 signal in case of infeasibility. If all predicted control signal exhausted
 and problem infeasible, stop execution

---
 MPC.py | 43 +++++++++++++++++++++++++++++++++++++++----
 1 file changed, 39 insertions(+), 4 deletions(-)

diff --git a/MPC.py b/MPC.py
index d14f891..c818bf0 100644
--- a/MPC.py
+++ b/MPC.py
@@ -178,6 +178,12 @@ class MPC_OSQP:
         # Current control and prediction
         self.current_prediction = None
 
+        # Counter for old control signals in case of infeasible problem
+        self.infeasibility_counter = 0
+
+        # Current control signals
+        self.current_control = None
+
         # Initialize Optimization Problem
         self.optimizer = osqp.OSQP()
 
@@ -282,10 +288,39 @@ class MPC_OSQP:
 
         # Solve optimization problem
         dec = self.optimizer.solve()
-        x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
-        delta = np.arctan(dec.x[-self.N] * self.model.l)
-        self.current_prediction = self.update_prediction(delta, x)
-        u = np.array([v, delta])
+
+        try:
+            # Get control signals
+            deltas = np.arctan(dec.x[-self.N:] * self.model.l)
+            delta = deltas[0]
+
+            # Update control signals
+            self.current_control = deltas
+
+            # Get predicted spatial states
+            x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
+            # Update predicted temporal states
+            self.current_prediction = self.update_prediction(delta, x)
+
+            # Get current control signal
+            u = np.array([v, delta])
+
+            # if problem solved, reset infeasibility counter
+            self.infeasibility_counter = 0
+
+        except:
+
+            print('Infeasible problem. Previously predicted'
+                  ' control signal used!')
+            u = np.array([v, self.current_control
+            [self.infeasibility_counter+1]])
+
+            # increase infeasibility counter
+            self.infeasibility_counter += 1
+
+        if self.infeasibility_counter == (self.N - 1):
+            print('No control signal computed!')
+            exit(1)
 
         return u