diff --git a/Experimentation/xenia_sravan_nonlinearopt.m b/Experimentation/xenia_sravan_nonlinearopt.m index c49bb05..46989c0 100644 --- a/Experimentation/xenia_sravan_nonlinearopt.m +++ b/Experimentation/xenia_sravan_nonlinearopt.m @@ -287,184 +287,94 @@ function dzdt=kinematic_bike(t,x,U,T) (F_yf*a*cos(delta_f)-F_yr*b)/Iz]; end -%% nonlinear opt functions -function [lb, ub] = bounds(start_idx, end_idx) - load('TestTrack.mat'); - load('reftrack_info.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 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 = []; - - %hijacking this for loop to only consider one index for bounds - for i=start_idx:end_idx+1 - Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)]; - sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2; - dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5; - [minDist, indMin] = min(dist_to_cl); - - prev_idx = max(indMin-1, 1); - next_idx = min(indMin, 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; -Inf]; - ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; - - - lb=[lb;lb_x]; - ub=[ub;ub_x]; - - end - - for i=start_idx:end_idx - ub=[ub;ub_u]; - lb=[lb;lb_u]; - end -end - -function [J, dJ] = costfun(z) - global nsteps - global target_vec - load('TestTrack.mat'); - - targetPos = target_vec(1:2); - targetTheta = target_vec(3); - 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 - - 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 -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); - 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 - -%% Linearization Functions +%% Kinematic Bike Models function dXdt = nonlinear_bike_model(X,U) global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims + [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U); + + % Vehicle Dynamics (Equation 1) + dx = u*cos(psi) - v*sin(psi); + du = (1/m)*(-f*m*g + Nw*F_x - F_yf*sin(delta_f)) + v*r; + dy = u*sin(psi) + v*cos(psi); + dv = (1/m)*(F_yf*cos(delta_f) + F_yr) - u*r; + dpsi = r; + dr = (1/Iz)*(a*F_yf*cos(delta_f) - b*F_yr); + dXdt = [dx; du; dy; dv; dpsi; dr]; +end + +function [A, B] = linearized_bike_model(X_ref,U_ref) + global Nw f Iz a b By Cy Dy Ey Shy Svy m g + + syms x_var u_var y_var v_var psi_var r_var ... % states + delta_f_var F_x_var ... % inputs + F_yf_var F_yr_var % lateral forces + + % Vehicle Dynamics (Equation 1) + dx = u_var*cos(psi_var) - v_var*sin(psi_var); + du = (1/m)*(-f*m*g + Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var; + dy = u_var*sin(psi_var) + v_var*cos(psi_var); + dv = (1/m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var; + dpsi = r_var; + dr = (1/Iz)*(a*F_yf_var*cos(delta_f_var) - b*F_yr_var); + + % Jacobians of continuous-time linearized system + % TODO: Change A_c & B_c computation to regular functions (e.g., + % using `matlabFunction`) + A_c_symb = [ ... + diff(dx, x_var), diff(dx, u_var), diff(dx, y_var), diff(dx, v_var), diff(dx, psi_var), diff(dx, r_var); + diff(du, x_var), diff(du, u_var), diff(du, y_var), diff(du, v_var), diff(du, psi_var), diff(du, r_var); + diff(dy, x_var), diff(dy, u_var), diff(dy, y_var), diff(dy, v_var), diff(dy, psi_var), diff(dy, r_var); + diff(dv, x_var), diff(dv, u_var), diff(dv, y_var), diff(dv, v_var), diff(dv, psi_var), diff(dv, r_var); + diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var); + diff(dr, x_var), diff(dr, u_var), diff(dr, y_var), diff(dr, v_var), diff(dr, psi_var), diff(dr, r_var); + ]; + + B_c_symb = [ ... + diff(dx, delta_f_var), diff(dx, F_x_var); + diff(du, delta_f_var), diff(du, F_x_var); + diff(dy, delta_f_var), diff(dy, F_x_var); + diff(dv, delta_f_var), diff(dv, F_x_var); + diff(dpsi,delta_f_var), diff(dpsi,F_x_var); + diff(dr, delta_f_var), diff(dr, F_x_var); + ]; + + % Substitute values from reference trajectory into symbolic Jacobians + % TODO: Fix reference trajectory indexing with "i" + A_c = @(i) double( ... + subs( ... + A_c_symb, ... + [x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ... + bike_model_helper(X_ref(i), U_ref(i)) ... + ) ... + ); + B_c = @(i) double( ... + subs( ... + B_c_symb, ... + [x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ... + bike_model_helper(X_ref(i), U_ref(i)) ... + ) ... + ); + + % Discrete-time LTV system + A = @(i) eye(6) + dt*A_c(i); + B = @(i) dt * B_c(i); +end + +%% Helper Functions +function clamped_val = clamp(val, min, max) + clamped_val = val; + + if clamped_val < min + clamped_val = min; + elseif clamped_val > max + clamped_val = max; + end +end + +function [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U) + global Nw f Iz a b By Cy Dy Ey Shy Svy m g + % Get state & input variables x = X(1); u = X(2); @@ -474,7 +384,7 @@ function dXdt = nonlinear_bike_model(X,U) r = X(6); delta_f = U(1); - F_x = U(2); + F_x = U(2); % Front and rear lateral slip angles in radians (Equations 8 & 9) alpha_f_rad = delta_f - atan2(v + a*r, u); @@ -507,74 +417,4 @@ function dXdt = nonlinear_bike_model(X,U) % Apply input limits (Table 1) delta_f = clamp(delta_f, delta_lims(1), delta_lims(2)); F_x = clamp(F_x, Fx_lims(1), Fx_lims(2)); - - % Vehicle Dynamics (Equation 1) - dx = u*cos(psi) - v*sin(psi); - du = (1/m)*(-f*m*g + Nw*F_x - F_yf*sin(delta_f)) + v*r; - dy = u*sin(psi) + v*cos(psi); - dv = (1/m)*(F_yf*cos(delta_f) + F_yr) - u*r; - dpsi = r; - dr = (1/Iz)*(a*F_yf*cos(delta_f) - b*F_yr); - dXdt = [dx; du; dy; dv; dpsi; dr]; end - -function [A, B] =bikeLinearize(x,U) - 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 - - %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 - -%% Helper Functions -function clamped_val = clamp(val, min, max) - clamped_val = val; - - if clamped_val < min - clamped_val = min; - elseif clamped_val > max - clamped_val = max; - end -end \ No newline at end of file