Nonlinear Constraints for Road & Obstacles

- Use `inpolygon` to check if position is within road
  and not in or on an obstacle
- Add helper function `get_start_idx` to get
  starting index of Z(i) [states(i), inputs(i)]
  in Z vector (combined states & inputs for prediction
  horizon)

Experimentation/part2_MPC_controller.m

@@ -40,9 +40,12 @@ state_init = [ ...
 ];
 
 % Simulation Parameters
-global T_s T_p
+global T_s T_p num_preds num_states num_inputs
 T_s = 0.01; % Step Size [s]
 T_p = 0.5; % Prediction Horizon [s]
+num_preds = T_p / T_s;
+num_states = 6;
+num_inputs = 2;
 
 %% Setup
 curr_pos = [state_init(1);state_init(3)];
@@ -205,7 +208,7 @@ function x0vec = initvec(x0, u0)
 end
 
 %% mpc functions
-function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
+function [Aeq,beq] = eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
     %build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
     %in the form Aeq*z=beq
     %initial_idx specifies the time index of initial condition from the reference trajectory 
@@ -238,64 +241,65 @@ function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
     end
 end
 
-function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
-    %time_idx is the index along uref the initial condition is at
-    xsize=(npred+1)*nstates;
-    usize=npred*ninputs;
+function [c, ceq] = nonlincon(Z, TestTrack, Xobs)
+    global num_preds num_states
 
-    Lb=[];
-    Ub=[];
-    for j=1:ninputs
-    Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
-    Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
+    ceq = [];
+    c = NaN(1,num_preds);
+    
+    for i = 1:num_preds
+        idx = get_start_idx(i);
+        X = Z(idx+1:idx+num_states);
+
+        p = [X(1); X(3)];
+        [~,bl_idx] = min(vecnorm(TestTrack.bl - p));
+        [~,br_idx] = min(vecnorm(TestTrack.br - p));
+
+        bl_idx_start = clamp(bl_idx-1, 1, size(TestTrack.bl,2));
+        bl_idx_end   = clamp(bl_idx+1, 1, size(TestTrack.bl,2));
+        br_idx_start = clamp(br_idx-1, 1, size(TestTrack.br,2));
+        br_idx_end   = clamp(br_idx+1, 1, size(TestTrack.br,2));
+
+        boundary_pts = [ ...
+            TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...
+            TestTrack.br(:,br_idx_end:-1:br_idx_start) ...
+        ];
+        xv_road = boundary_pts(1,:);
+        yv_road = boundary_pts(2,:);
+
+        in_road = inpolygon(p(1), p(2), xv_road, yv_road);
+
+        if ~in_road
+            % Point not inside road
+            c(i) = 1; % c(x) > 0, nonlinear inequality constraint violated
+        end
+
+        if ~isnan(c(i))
+            % If value set, go to next "i"
+            continue
+        end
+
+        for j = 1:size(Xobs,2)
+            xv_obstacle = Xobs{i}(:,1);
+            yv_obstacle = Xobs{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(x) > 0, nonlinear inequality constraint violated
+            end
+
+            if ~isnan(c(i))
+                % If value set, skip checking remaining obstacles
+                break
+            end
+        end
+
+        if isnan(c(i))
+            % If value not set, no constraints violated
+            c(i) = -1; % c(x) <= 0, nonlinear inequality constraint satisfied
+        end
     end
-
-    Lb=reshape(Lb,[usize,1]);
-    Ub=reshape(Ub,[usize,1]);
-
-    Lb=[-Inf(xsize,1);Lb];
-    Ub=[Inf(xsize,1);Ub];
-
-end
-
-function dzdt=kinematic_bike(t,x,U,T)
-    global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims
-    
-    %generate input functions
-    delta_f=U(1);
-    F_x=U(2);
-    
-    %slip angle functions in degrees
-    a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
-    a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
-    
-    %Nonlinear Tire Dynamics
-    phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
-    phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
-    
-    F_zf=b/(a+b)*m*g;
-    F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
-    
-    F_zr=a/(a+b)*m*g;
-    F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
-    
-    F_total=sqrt((Nw*F_x)^2+(F_yr^2));
-    F_max=0.7*m*g;
-    
-    if F_total>F_max
-        
-        F_x=F_max/F_total*F_x;
-      
-        F_yr=F_max/F_total*F_yr;
-    end
-    
-    %vehicle dynamics
-    dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
-              (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
-              x(2)*sin(x(5))+x(4)*cos(x(5));...
-              (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
-              x(6);...
-              (F_yf*a*cos(delta_f)-F_yr*b)/Iz];
 end
 
 %% Kinematic Bike Models
@@ -428,3 +432,8 @@ function [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U)
     delta_f = clamp(delta_f, delta_lims(1), delta_lims(2));
     F_x = clamp(F_x, Fx_lims(1), Fx_lims(2));
 end
+
+function idx = get_start_idx(i)
+    global num_states num_inputs
+    idx = (i-1)*(num_states+num_inputs);
+end