diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m new file mode 100644 index 0000000..a5e8c2e --- /dev/null +++ b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m @@ -0,0 +1,290 @@ +%Vehicle Parameterrs +Nw=2; +f=0.01; +Iz=2667; +a=1.35; +b=1.45; +By=0.27; +Cy=1.2; +Dy=0.7; +Ey=-1.6; +Shy=0; +Svy=0; +m=1400; +g=9.806; + +%note constraints on input +%delta [-0.5, 0.5] +%Fx [-5000, 5000] + +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 + +[g,h,dg,dh]=nonlcon(z, Xobs); + +function [lb, ub] = bounds(StepsPerPoint) + load('TestTrack.mat'); + + [m,nPts] = size(TestTrack.cline); +% numState = 6; +% numInput = 2; + nsteps = StepsPerPoint * nPts; + + lb_u = [-0.5;-5000]; + ub_u = [0.5;5000]; + + bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ... + TestTrack.br(1,1), TestTrack.br(1,2)]; + bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ... + 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/2); -Inf]; + ub = [max(bound_X); Inf; max(bound_Y); Inf; +(pi/2); Inf]; + + + for i=1:nPts + prev_idx = max(i-1, 1); + next_idx = min(i+1, 246); + + bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ... + TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)]; + bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ... + TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)]; + phi_init = TestTrack.theta(i); + + lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -(pi/2); -Inf]; + ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +(pi/2); Inf]; + + for num = 1:StepsPerPoint + lb=[lb;lb_x]; + ub=[ub;ub_x]; + end + end + + for i=1:nsteps + ub=[ub;ub_u]; + lb=[lb;lb_u]; + end +end + + +function [g, h, dg, dh] = nonlcon(z, Xobs) + + nsteps = (size(z,2)/8); + curr_pos = [z(1); z(3)]; + + Xobs_seen = senseObstacles(curr_pos, Xobs); + centroids = []; + for i = 1:size(Xobs_seen,2) + centroids = [centroids; mean(Xobs_seen{1, i})]; + end + + dt = 0.01; + g = []; dg = []; + + %radius size + r = 3; + + for i = 1:nsteps + zInd_x = 6*(i-1) + 1; + zInd_y = 6*(i-1) + 3; + curr_xy = [z(zInd_x), z(zInd_y)]; + + %g based on if there's obstacles around + %initialize to zero + g_curr = 0; + zeroRow = zeros(1,size(z,2)); + dg(i,:) = zeroRow; + if (~isempty(centroids)) + dist2Obst = []; + for j = 1:size(Xobs_seen,2) + dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2)); + dist2Obst = [dist2Obst; dist]; + end + + %closest obstacle used for constraint + [minDist, indMin] = min(dist2Obst); + + %g + g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2; + + %dg + dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2; + dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2; + end + g = [g; g_curr]; + end + dg = transpose(dg); + + h = z(1:6); + for i = 2:nsteps + zInd_x = 6*(i-1) + 1; + zInd_x_prev = 6*(i-2) + 1; + + stateCurr = z(zInd_x:zInd_x+5); + statePrev = z(zInd_x_prev:zInd_x_prev+5); + + %get previous input + uInd_prev = 2*(i-1) + nsteps*6 - 1; + uPrev = [z(uInd_prev), z(uInd_prev + 1)]; + + derPrev= dt*bike(statePrev, uPrev); + + currH = stateCurr - statePrev - derPrev; + h(6*(i-1)+1) = currH(1); + h(6*(i-1)+2) = currH(2); + h(6*(i-1)+3) = currH(3); + h(6*(i-1)+4) = currH(4); + h(6*(i-1)+5) = currH(5); + h(6*(i-1)+6) = currH(6); + end + + dh = zeros(size(z,2), nsteps*6); + dh(1:6, 1:6) = eye(6); + for i = 1:nsteps-1 + uInd = 2*(i-1) + nsteps*6 + 1; + uCurr = [z(uInd), z(uInd + 1)]; + zInd_x = 6*(i-1) + 1; + stateCurr = z(zInd_x:zInd_x+5); + [A, B] = bikeLinearize(stateCurr, uCurr); + + dh(6*i+1:6*i+6, 6*i+1:6*i+6) = eye(6); + dh(6*i-5:6*i, 6*i+1:6*i+6) = -eye(6) - dt*A; + dh(nsteps*6+2*i-1:nsteps*6+2*i, 6*i+1:6*i+6) = -dt*transpose(B); + end + +end + +function dzdt=bike(x,U) +%constants +Nw=2; +f=0.01; +Iz=2667; +a=1.35; +b=1.45; +By=0.27; +Cy=1.2; +Dy=0.7; +Ey=-1.6; +Shy=0; +Svy=0; +m=1400; +g=9.806; + + +%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 + +function [A, B] =bikeLinearize(x,U) +%constants +Nw=2; +f=0.01; +Iz=2667; +a=1.35; +b=1.45; +By=0.27; +Cy=1.2; +Dy=0.7; +Ey=-1.6; +Shy=0; +Svy=0; +m=1400; +g=9.806; + + +%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 + + + +%we are just going to use cornering stiffness to make linear so this derivative +%easier, the vehicle parameter's are close enough to problem 1 hw 2 +B=10; +C=1.3; +D=1; +Ca_r= F_zr*B*C*D; +Ca_f= F_zf*B*C*D; + +A = [0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0; + 0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1; + 0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0; + 0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - x(2); + 0, 0, 0, 0, 0, 1 + 0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/x(2)) + a*Ca_f/Iz*(-a/x(2))]; + +B = [0, -Ca_f/(x(2)*m), 0, Ca_f/m, 0, a*Ca_f/Iz; + 0, Nw/m, 0, 0, 0, 0]'; +end \ No newline at end of file