From d591bb68c99dd1118d211d8e907577881e5343cc Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Mon, 13 Dec 2021 07:45:23 -0500
Subject: [PATCH] Try adjusting track boundary constraints (still being
 violated, but not detected)

- Use all track boundary points when checking if position is inside
- Still fails to stop simulation when car goes off track
---
 Deliverables/MPC_Class.m | 28 +++++++++-------------------
 1 file changed, 9 insertions(+), 19 deletions(-)

diff --git a/Deliverables/MPC_Class.m b/Deliverables/MPC_Class.m
index 9d3788d..8dc7c36 100644
--- a/Deliverables/MPC_Class.m
+++ b/Deliverables/MPC_Class.m
@@ -96,19 +96,13 @@ classdef MPC_Class
             1e6; ...   % within track bounds
             1e6; ...   % outside obstacles
         ];
-
         options = ...
             optimoptions( ...
                 'fmincon', ...
+                'MaxFunctionEvaluations',    4000, ...
                 'SpecifyConstraintGradient', false, ...
                 'SpecifyObjectiveGradient',  false ...
-            );
-
-        % Constraint Parameters
-        ntrack_boundary_pts = 1; % Number of track boundary points
-                                 % around closest boundary point
-                                 % to use when checking if position
-                                 % is within track polygon
+        );
     end
     
     %% Public Functions
@@ -171,13 +165,13 @@ classdef MPC_Class
                     obj.options ...    % options: Optimization options
             );
 
-            % Check `fmincon` exitflag
+            % Stop sim if `fmincon` failed to find feasible point
             if exitflag == -2
                 disp('No feasible point was found')
                 obj.FLAG_terminate = 1;
             end
 
-            % Check if nonlinear constraint was satisfied
+            % Stop sim if nonlinear constraint was violated
             [c, ~] = obj.nonlcon(Z_err);
             if any(c > 0)
                 disp('Found point violates nonlinear inequality constraint')
@@ -339,16 +333,12 @@ classdef MPC_Class
 
                 % Get xy position from state vector
                 p = [Y(1); Y(3)];
-                % Find closest boundary points to position
-                [~,bl_idx] = min(vecnorm(obj.TestTrack.bl - p));
-                [~,br_idx] = min(vecnorm(obj.TestTrack.br - p));
 
-                % Use closest boundary points +/- ntrack_boundary_pts
-                % to construct track polygon
-                bl_idx_start = obj.clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
-                bl_idx_end   = obj.clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
-                br_idx_start = obj.clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
-                br_idx_end   = obj.clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
+                % Construct track polygon from all boundary points
+                bl_idx_start = 1;
+                bl_idx_end   = size(obj.TestTrack.bl,2);
+                br_idx_start = 1;
+                br_idx_end   = size(obj.TestTrack.br,2);
 
                 boundary_pts = [ ...
                     obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...