From 91fc8ff4c32d7ce8f599bc8dd23d9382cdc614db Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Mon, 13 Dec 2021 07:16:27 -0500
Subject: [PATCH] Playing with Nonlinear Constraint Costs

- Drastically increase nonlinear constraint violation cost (I think)
- Make state & input costs much smaller than constraint costs
- Add condition to stop simulation if nonlinear constraint
  is not satisfied (doesn't seem to work for going off track)
- Create separate nonlinear constraints for each state to check
  if it is on track and if it doesn't intersect each obstacle
- Change indexing of nonlinear constraints
---
 Deliverables/MPC_Class.m | 65 +++++++++++++++++++++++-----------------
 1 file changed, 37 insertions(+), 28 deletions(-)

diff --git a/Deliverables/MPC_Class.m b/Deliverables/MPC_Class.m
index 638e0b4..9d3788d 100644
--- a/Deliverables/MPC_Class.m
+++ b/Deliverables/MPC_Class.m
@@ -56,7 +56,7 @@ classdef MPC_Class
         % Simulation Parameters
         T_s = 0.01;           % Step Size [s]
         T_p = 0.5;            % Prediction Horizon [s]
-        sim_stop_idx = 1.5e3; % Index in reference trajectory to stop sim
+        sim_stop_idx = 2.0e3; % Index in reference trajectory to stop sim
 
         % Decision Variables
         nstates = 6; % Number of states per prediction
@@ -80,18 +80,23 @@ classdef MPC_Class
         FLAG_terminate; % Binary flag indicating simulation termination
 
         % MPC & Optimization Parameters
-        Q = [ ...  % State Error Costs
-            1; ... % x_err   [m]
-            1; ... % u_err   [m/s]
-            1; ... % y_err   [m]
-            1; ... % v_err   [m/s]
-            1; ... % psi_err [rad]
-            1; ... % r_err   [rad/s]
+        Q = [ ...      % State Error Costs
+            1e-2; ...  % x_err   [m]
+            1e-2; ...  % u_err   [m/s]
+            1e-2; ...  % y_err   [m]
+            1e-2; ...  % v_err   [m/s]
+            1e-2; ...  % psi_err [rad]
+            1e-2; ...  % r_err   [rad/s]
         ];
-        R = [ ...     % Input Error Costs
-            0.1; ...  % delta_f_err [rad]
-            0.01; ... % F_x_err     [N]
+        R = [ ...      % Input Error Costs
+            1e-2; ...  % delta_f_err [rad]
+            1e-2; ...  % F_x_err     [N]
         ];
+        NL = [ ...     % Nonlinear Constraint Cost
+            1e6; ...   % within track bounds
+            1e6; ...   % outside obstacles
+        ];
+
         options = ...
             optimoptions( ...
                 'fmincon', ...
@@ -149,9 +154,9 @@ classdef MPC_Class
 
             [ ...          % `fmincon` outputs
              Z_err, ...    % x:        Solution
-             J_val, ...    % fval:     Objective function value at solution
+             ~, ...        % fval:     Objective function value at solution
              exitflag, ... % exitflag: Reason `fmincon` stopped
-             output, ...   % output:   Information about the optimization process
+             ~, ...        % output:   Information about the optimization process
             ] = ...
                 fmincon( ...           % `fmincon` inputs
                     @obj.cost_fun, ... % fun:     Function to minimize
@@ -166,11 +171,19 @@ classdef MPC_Class
                     obj.options ...    % options: Optimization options
             );
 
+            % Check `fmincon` exitflag
             if exitflag == -2
                 disp('No feasible point was found')
                 obj.FLAG_terminate = 1;
             end
 
+            % Check if nonlinear constraint was satisfied
+            [c, ~] = obj.nonlcon(Z_err);
+            if any(c > 0)
+                disp('Found point violates nonlinear inequality constraint')
+                obj.FLAG_terminate = 1;
+            end
+
             % NOTE: Error = Actual - Reference
             %    => Actual = Error + Reference
             Z_ref = obj.get_ref_decision_variable();
@@ -310,7 +323,8 @@ classdef MPC_Class
 
             % Nonlinear inequality constraints from track boundary
             % and obstacles applied to states
-            c = NaN(1, obj.npredstates);
+            c = NaN(1, obj.npredstates*(1+length(obj.Xobs_seen)));
+            cons_idx = 1;
 
             % NOTE: Error = Actual - Reference
             %    => Actual = Error + Reference
@@ -348,13 +362,13 @@ classdef MPC_Class
 
                 if ~in_track
                     % Position not inside track
-                    c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
-
-                    % Skip to next constraint
-                    continue;
+                    c(cons_idx) = obj.NL(1); % c(Z_err) > 0, nonlinear inequality constraint violated
+                else
+                    c(cons_idx) = -obj.NL(1); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
                 end
+                cons_idx = cons_idx + 1;
 
-                for j = 1:size(obj.Xobs_seen,2)
+                for j = 1:length(obj.Xobs_seen)
                     % Check if position is in or on each obstacle
                     xv_obstacle = obj.Xobs_seen{j}(:,1);
                     yv_obstacle = obj.Xobs_seen{j}(:,2);
@@ -362,16 +376,11 @@ classdef MPC_Class
 
                     if in_obstacle || on_obstacle
                         % Point in or on obstacle
-                        c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
-
-                        % Skip remaining obstacle checking
-                        break;
+                        c(cons_idx) = obj.NL(2); % c(Z_err) > 0, nonlinear inequality constraint violated
+                    else
+                        c(cons_idx) = -obj.NL(2); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
                     end
-                end
-
-                if isnan(c(i+1))
-                    % If value not set, no constraints violated
-                    c(i+1) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied
+                    cons_idx = cons_idx + 1;
                 end
             end
         end