From 28fc4d8b4734f5b2f81f0cbf7f48246a6e1e102e Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Fri, 10 Dec 2021 10:20:53 -0500
Subject: [PATCH] Boundary Constraints & Index Helper Functions

- Add boundary constraints for staying on the "map"
  and staying within operating limits of vehicle inputs
- Add helper functions to get indices of things
---
 Experimentation/MPC_Class.m | 58 +++++++++++++++++++++++++++++++++++--
 1 file changed, 56 insertions(+), 2 deletions(-)

diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m
index fb45e1d..6494d2a 100644
--- a/Experimentation/MPC_Class.m
+++ b/Experimentation/MPC_Class.m
@@ -32,7 +32,7 @@ classdef MPC_Class
 
         % Input Limits (Table 1)
         delta_lims = [-0.5, 0.5];
-        Fx_lims    = [-5e3, 5e3];
+        F_x_lims   = [-5e3, 5e3];
 
         % Position Limits (Min/Max based on track)
         x_lims = [ 200, 1600];
@@ -93,8 +93,62 @@ classdef MPC_Class
         end
     end
 
+    % TODO: Constraint Functions
     methods (Access = private)
-        % TODO: Add MPC related private functions
+        function [Lb, Ub] = bound_cons(obj, ref_idx)
+            % ref_idx is the index along reference trajectory that initial condition is at
+            Lb = -Inf(1, obj.ndec);
+            Ub =  Inf(1, obj.ndec);
+
+            for i = 0:obj.npred
+                start_idx = get_state_start_idx(i);
+
+                % x
+                Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(ref_idx+i, 1);
+                Ub(start_idx+1) = obj.x_lims(2) - obj.Y_ref(ref_idx+i, 1);
+
+                % y
+                Lb(start_idx+3) = obj.y_lims(1) - obj.Y_ref(ref_idx+i, 3);
+                Ub(start_idx+3) = obj.y_lims(2) - obj.Y_ref(ref_idx+i, 3);
+            end
+
+            for i = 0:obj.npred-1
+                start_idx = get_input_start_idx(i);
+
+                % delta_f
+                Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(ref_idx+i, 1);
+                Ub(start_idx+1) = obj.delta_lims(2) - obj.U_ref(ref_idx+i, 1);
+
+                % F_x
+                Lb(start_idx+2) = obj.F_x_lims(1) - obj.U_ref(ref_idx+i, 2);
+                Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(ref_idx+i, 2);
+            end
+        end
+    end
+
+    % TODO: Helper Functions
+    methods (Access = private)
+        function idx = get_state_start_idx(obj, i)
+            idx = obj.nstates*i;
+        end
+
+        function idx = get_input_start_idx(obj, i)
+            idx = (obj.npred+1)*obj.nstates + obj.ninputs*i;
+        end
+
+        function idx = get_ref_index(obj, curr_state)
+            % Get position (x,y) from current state
+            pos = [curr_state(1), curr_state(3)];
+            % Get positions (x,y) from reference trajectory
+            pos_ref = obj.Y_ref(:,[1,3]);
+
+            % Calculate Euclidean distance between current position and
+            % reference trajectory positions
+            dist = vecnorm(pos - pos_ref, 2, 2);
+
+            % Get index of reference position closest to current position
+            [~, idx] = min(dist);
+        end
     end
 end