iteration attempt

SimPkg_F21(student_ver)/xenia_nonlinearopt.m

@@ -22,25 +22,121 @@ load("TestTrack.mat");
 init = [287, 5, -176, 0, 2, 0];
 curr_pos = [init(1);init(3)];
 
-%generate bounds, will need to index these appropriately for obstacle
-%avoidance
-[LB, UB] = bounds(10);
-
-Nobs = 25;
-Xobs = generateRandomObstacles(Nobs);
 
 %state_size = tbd;
-z = [init, init, -0.004, 3900, -0.004, 3900]; %for testing purposes
+% z = [init, init, -0.004, 3900]; %for testing purposes
+% nsteps = 2;
+% [LB, UB] = bounds(nsteps, 1);
+% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps);
+% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps);
 
-[g,h,dg,dh]=nonlcon(z, Xobs);
+%%okay, idea is to create an iterative process that uses the centerline as a
+%checkpoint
+%keep trajecting until closer to the next point on centerline, then repeat
+%bounds will be based on index of current&previous cp index  
+%cost function uses next checkpoint in centerline 
 
-function [lb, ub] = bounds(StepsPerPoint)
+%generate z and make it bigger if didn't reach close enough to the goal
+%(modify nsteps)
+Nobs = 25;
+global Xobs
+Xobs = generateRandomObstacles(Nobs);
+
+global index_cl_cp %index of current checkpoint in centerline
+index_cl_cp = 1;
+global index_cl_nextcp %index of next checkpoint in centerline
+index_cl_nextcp = 2;
+
+global nsteps
+nsteps = 100;
+
+x0 = init;
+x0vec = []; 
+%initialize x0vec to be initial condition for n steps and Sravan's ref inputs
+%note: function 'initvec' changes inputs, but i'm not sure how
+%initialization should look with fmincon 
+for i = 1:nsteps
+    x0vec = [x0vec, init];
+end
+for i = 1:nsteps-1
+    x0vec = [x0vec, -0.004, 3900];
+end
+
+dist12atcp = [];
+iter = 0;
+
+factor = 0.1; %next checkpoint if significantly closer to next checkpoint  
+U_final = [];
+Y_final = [];
+options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
+                        'SpecifyObjectiveGradient',true) ;
+while (index_cl_cp < size(TestTrack.cline,2))
+    %because of the way cf/nc need to be)
+    index_cl_cp
+    iter = iter + 1;
+    [lb, ub] = bounds(nsteps, index_cl_cp);
+    cf=@costfun
+    nc=@nonlcon
+    z=fmincon(cf,x0vec,[],[],[],[],lb',ub',nc,options);
+    Y0=reshape(z(1:6*nsteps),6,nsteps)';
+    U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
+    
+    [Y, T] = forwardIntegrateControlInput(U, x0vec(1:6));
+    
+    curr_xy = [Y(end,1); Y(end,3)];
+    dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
+    dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
+    
+    if (dist2cp2  < (dist2cp1 - dist2cp1*factor))
+        dist12atcp = [dist12atcp; dist2cp1, dist2cp2];
+        %add to final solution
+        U_final = [U_final; U];
+        Y_final = [Y_final; Y];
+        
+        %reinstantiate 
+        %nsteps = 100;
+        x0vec = initvec(Y_final(end,:), U_final(end,:));  
+        
+        %update checkpoint
+        index_cl_cp = index_cl_cp + 1
+        index_cl_nextcp = index_cl_nextcp + 1;
+        if (index_cl_nextcp > size(TestTrack.cline, 2))
+            index_cl_nextcp = size(TestTrack.cline, 2);
+        end
+    else 
+        %resize and try again
+        %nsteps = nsteps + 20; 
+        x0vec = initvec(x0vec(1:6), U(1,:));
+        
+    end
+    
+end
+
+function x0vec = initvec(x0, u0)
+    %function used because fmincon needs initial condition to be size of
+    %state vector
+    %x0 - last row of Y at checkpoint
+    %u0 - last row of U at checkpoint
+    global nsteps
+    x0vec = [];
+    for i = 1:nsteps
+        x0vec = [x0vec, x0];
+    end
+    
+    %not sure if inputs should be instantiated or not 
+    %will instantiate them to previous u 
+    for i = 1:nsteps-1
+        x0vec = [x0vec, u0]; 
+    end
+end
+
+function [lb, ub] = bounds(StepsPerPoint, index)
     load('TestTrack.mat');
     
-    [m,nPts] = size(TestTrack.cline);
+    %[m,nPts] = size(TestTrack.cline);
 %     numState = 6;
 %     numInput = 2;
-    nsteps = StepsPerPoint * nPts;
+   % nsteps = StepsPerPoint * nPts;
     
     lb_u = [-0.5;-5000];
     ub_u = [0.5;5000];
@@ -51,12 +147,13 @@ function [lb, ub] = bounds(StepsPerPoint)
                TestTrack.br(2,1), TestTrack.br(2,2)];
     phi_init = TestTrack.theta(1);
     
-    %phi restricted to just [-pi/2 pi/2]
+    %phi restricted to just [-pi pi]
-    lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
+    %lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
-    ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
+    %ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
+    lb = []; ub = [];
     
-
+    %hijacking this for loop to only consider one index for bounds    
-    for i=1:nPts
+    for i=index:index
         prev_idx = max(i-1, 1);
         next_idx = min(i+1, 246);
         
@@ -75,16 +172,51 @@ function [lb, ub] = bounds(StepsPerPoint)
         end
     end
 
-    for i=1:nsteps
+    for i=1:StepsPerPoint-1
         ub=[ub;ub_u];
         lb=[lb;lb_u];
     end
 end
 
+function [J, dJ] = costfun(z)
+    
+    global nsteps
+    global index_cl_nextcp
+    load('TestTrack.mat');
+    
+    targetPos = TestTrack.cline(:, index_cl_nextcp);
+    targetTheta = TestTrack.theta(index_cl_nextcp);
+    sumPos = [];
+    sumInput = [];
+    for i = 1:nsteps
+        zIndx = 6*(i-1) + 1;
+        sumPos(i) = (z(zIndx) - targetPos(1))^2 + (z(zIndx+2) - targetPos(2))^2 + (z(zIndx+4) - targetTheta)^2;
+       
+        if (i <= nsteps-1)
+            uInd = 2*(i-1) + nsteps*6 - 1;
+            sumInput(i) = z(uInd)^2 + z(uInd+1)^2;
+        end
+    end
 
-function [g, h, dg, dh] = nonlcon(z, Xobs)
+    J = sum(sumPos) + sum(sumInput);
+    dJ = transpose(z.*2);
+    
+    uStart = nsteps*6 - 1;
+    for i = 1:6:nsteps*6
+        zIndx = i;
+        dJ(i) = (z(zIndx) - targetPos(1))*2;
+        dJ(i+2) = (z(zIndx+2) - targetPos(2))*2; 
+        dJ(i+4) = (z(zIndx+4) - targetTheta)*2;
+    end
+        
 
-    nsteps = (size(z,2)/8);
+end
+
+function [g, h, dg, dh] = nonlcon(z)
+
+    %nsteps = (size(z,2)/8);
+    global nsteps
+    global Xobs
     curr_pos = [z(1); z(3)];
   
     Xobs_seen = senseObstacles(curr_pos, Xobs);