Nonlinear Track & Obstacle Constraints

- Add `nonlcon` function that checks if position is on track
  and not on or inside an obstacle
- Add `options` for `fmincon` to not specify constraint gradient,
  but specify objective gradient
- Add parameter for number of track boundary points to use
  around the closest point to current position when checking
  if point is inside track polygon

Experimentation/MPC_Class.m

@@ -78,7 +78,7 @@ classdef MPC_Class
         ref_idx;        % Index of reference trajectory closest to `Y_curr`
         FLAG_terminate; % Binary flag indicating simulation termination
 
-        % MPC Tunable Parameters
+        % MPC & Optimization Parameters
         Q = [ ...  % State Error Costs
             1; ... % x_err   [m]
             1; ... % u_err   [m/s]
@@ -91,6 +91,18 @@ classdef MPC_Class
             0.1; ...  % delta_f_err [rad]
             0.01; ... % F_x_err     [N]
         ];
+        options = ...
+            optimoptions( ...
+                'fmincon', ...
+                'SpecifyConstraintGradient', false, ...
+                'SpecifyObjectiveGradient',  true ...
+            );
+
+        % 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
@@ -212,6 +224,99 @@ classdef MPC_Class
                     = B(obj.ref_idx+i-1);
             end
         end
+
+        function [c, ceq] = nonlcon(obj, Z_err)
+            %nonlcon Construct nonlinear constraints for
+            %   vehicle to stay within track bounds and
+            %   outside of obstacles using reference
+            %   trajectory at the index closest to
+            %   `Y_curr`
+
+            % No equality constraints, so leave `ceq` empty
+            ceq = [];
+
+            % Nonlinear inequality constraints from track boundary
+            % and obstacles applied to states
+            c = NaN(1, obj.npredstates);
+
+            % Construct reference decision variable from reference
+            % states and inputs
+            Z_ref = zeros(obj.ndec, 1);
+
+            for i = 0:obj.npredstates-1
+                start_idx = get_state_start_idx(i);
+                Z_ref(start_idx+1:start_idx+obj.nstates) ...
+                    = obj.Y_ref(obj.ref_idx+i, :);
+            end
+
+            for i = 0:obj.npredinputs-1
+                start_idx = get_input_start_idx(i);
+                Z_ref(start_idx+1:start_idx+obj.ninputs) ...
+                    = obj.U_ref(obj.ref_idx+i, :);
+            end
+
+            % NOTE: Error = Actual - Reference
+            %    => Actual = Error + Reference
+            Z = Z_err + Z_ref;
+
+            % Construct constraint for each state
+            for i = 0:obj.npredstates-1
+                % Get index of current state in decision variable
+                idx = get_start_idx(i);
+                Y = Z(idx+1:idx+obj.nstates);
+
+                % 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 = clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
+                bl_idx_end   = clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
+                br_idx_start = clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
+                br_idx_end   = clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
+
+                boundary_pts = [ ...
+                    obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...
+                    obj.TestTrack.br(:,br_idx_end:-1:br_idx_start) ...
+                ];
+                xv_track = boundary_pts(1,:);
+                yv_track = boundary_pts(2,:);
+
+                % Check if p is within track polygon
+                in_track = inpolygon(p(1), p(2), xv_track, yv_track);
+
+                if ~in_track
+                    % Position not inside track
+                    c(i) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
+
+                    % Skip to next constraint
+                    continue;
+                end
+
+                for j = 1:size(obj.Xobs_seen,2)
+                    % Check if position is in or on each obstacle
+                    xv_obstacle = obj.Xobs_seen{i}(:,1);
+                    yv_obstacle = obj.Xobs_seen{i}(:,2);
+                    [in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle);
+
+                    if in_obstacle || on_obstacle
+                        % Point in or on obstacle
+                        c(i) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
+
+                        % Skip remaining obstacle checking
+                        break;
+                    end
+                end
+
+                if isnan(c(i))
+                    % If value not set, no constraints violated
+                    c(i) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied
+                end
+            end
+        end
     end
 
     %% Private Kinematic Models