From 21434cb4de2c00529dc646d4f9dc69f5374475df Mon Sep 17 00:00:00 2001 From: xdemenchuk Date: Mon, 29 Nov 2021 14:09:19 -0500 Subject: [PATCH] iteration attempt --- SimPkg_F21(student_ver)/xenia_nonlinearopt.m | 170 ++++++++++++++++--- 1 file changed, 151 insertions(+), 19 deletions(-) diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m index 3ba188c..667f1ce 100644 --- a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m +++ b/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] - lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf]; - ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; + %phi restricted to just [-pi pi] + %lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf]; + %ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; + lb = []; ub = []; - - for i=1:nPts + %hijacking this for loop to only consider one index for bounds + 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);