diff --git a/SimPkg_F21(student_ver)/part1_generate_trajectory.m b/SimPkg_F21(student_ver)/part1_generate_trajectory.m index e330ecd..bf26f74 100644 --- a/SimPkg_F21(student_ver)/part1_generate_trajectory.m +++ b/SimPkg_F21(student_ver)/part1_generate_trajectory.m @@ -184,6 +184,27 @@ info = getTrajectoryInfo(Y,U) [Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0); info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input) +index_cl_cp = 1; +index_cl_nextcp = 2; +%xenia test +for i = 1:size(Y,1) + + curr_xy = [Y(i,1); Y(i,3)]; + dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp)); + dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp)); + + if (dist2cp2 < dist2cp1) + i + dist2cp1 + dist2cp2 + index_cl_cp = index_cl_cp + 1; + index_cl_nextcp = index_cl_nextcp + 1; + if (index_cl_nextcp > size(TestTrack.cline, 2)) + break + end + end +end + %% Figures % Plot segmented trajectory for debugging purposes figure(1) diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.asv b/SimPkg_F21(student_ver)/xenia_nonlinearopt.asv deleted file mode 100644 index e2af018..0000000 --- a/SimPkg_F21(student_ver)/xenia_nonlinearopt.asv +++ /dev/null @@ -1,281 +0,0 @@ -%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 - - -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= Fz_r*B*C*D; -Ca_f= Fz_f*B*C*D; - -A = @(t)[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)) - u(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/(x2)) + 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