diff --git a/SimPkg_F21(student_ver)/Main_Peter.m b/SimPkg_F21(student_ver)/Main_Peter.m new file mode 100644 index 0000000..ddb63a2 --- /dev/null +++ b/SimPkg_F21(student_ver)/Main_Peter.m @@ -0,0 +1,220 @@ +%% ====================================================================== +% ROB 535 Control Project: Peerayos Pongsachai +% clear all;clc; close all; +load("TestTrack.mat"); + +% Objectives: +% - Come up with an inequality constraint function that keeps the +% center of mass inside the track limits for all timesteps [hard] +% - Compute gradient of road inequality constraints [medium] +X0= [287;5;-176;0;2;0]; + +% rng(0); +n = 25; +Pose = [randi([900,960],1,n);randi([440,520],1,n)]; + +xhat = 370; yhat = 140; Phat = [xhat;yhat]; +Pinit = [X0(1);X0(3)]; +Pend = [TestTrack.cline(1,end);TestTrack.cline(2,end)]; +[m,nPts] = size(TestTrack.cline); + +% Plot Track +figure;hold; +plot(TestTrack.bl(1,:),TestTrack.bl(2,:), 'k') +plot(TestTrack.cline(1,:),TestTrack.cline(2,:), '--k') +plot(TestTrack.br(1,:),TestTrack.br(2,:), 'k') +plot([TestTrack.bl(1,end),TestTrack.br(1,end)], ... + [TestTrack.bl(2,end),TestTrack.br(2,end)], 'r') +hold; + + +d = vecnorm(TestTrack.cline - Pinit); +eps = 1; +Idx = find(d <= eps); + +while(isempty(Idx)) + eps = eps + 1; + Idx = find(d <= eps); +end + +idx = Idx(1); +prev_idx = max(1-1, 1); +next_idx = idx+1; + +bl_vec = [TestTrack.bl(1,[prev_idx,next_idx]);TestTrack.bl(2,[prev_idx,next_idx])]; +br_vec = [TestTrack.br(1,[prev_idx,next_idx]);TestTrack.br(2,[prev_idx,next_idx])]; + +cp = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2)); + +% fprintf('Pose (%.2f, %.2f) - nearest point (%.2f, %.2f): %i \n', ... +% Pose(1,i), Pose(2,i),TestTrack.cline(1,idx),TestTrack.cline(2,idx),cp) + +hold; +plot(Pinit(1),Pinit(2),'og') +plot(Pend(1),Pend(2),'or') +plot(TestTrack.cline(1,idx),TestTrack.cline(2,idx), 'o') +plot(bl_vec(1,:),bl_vec(2,:), 'ob') +plot(br_vec(1,:),br_vec(2,:), 'ob') +hold; + + +[LB, UB] = bounds(TestTrack, 10); +size(LB) + +[g,h,dg,dh]=nonlcon(z, TestTrack); + +function cp = ontrack(pose, bl_prev, bl_next, br_prev, br_next) + + l_vec = bl_next - bl_prev; % l = l_i+1 - l_i-1 + r_vec = br_next - br_prev; % r = r_i+1 - r_i-1 + + pl = pose - bl_prev; % p_l = p - l_i-1 + pr = pose - br_prev; % p_r = p - r_i-1 + + c1 = l_vec(1)*pl(2) - l_vec(2)*pl(1); + c2 = r_vec(1)*pr(2) - r_vec(2)*pr(1); + + cp = c1*c2; +end + +function dg = diff_g(pose, bl_prev, bl_next, br_prev, br_next) +% syms Lx Ly Rx Ry ly lx rx ry X Y; +% g = (Lx*(Y - ly)-Ly*(X-lx))*(Rx*(Y - ry)-Ry*(X-rx)); +% diff(g,X); +% diff(g,Y); + + dg = [0,0]; + l_vec = bl_next - bl_prev; % l = l_i+1 - l_i-1 + r_vec = br_next - br_prev; % r = r_i+1 - r_i-1 + + pl = pose - bl_prev; % p_l = p - l_i-1 + pr = pose - br_prev; % p_r = p - r_i-1 + + dg(1) = r_vec(2)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) + ... + l_vec(2)*(r_vec(2)*pr(1) - r_vec(1)*pr(2)); + dg(2) = - r_vec(1)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) - ... + l_vec(1)*(r_vec(2)*pr(1) - r_vec(1)*pr(2)); +end + +function [lb, ub] = bounds(TestTrack, StepsPerPoint) + + [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); + + lb = [min(bound_X); -Inf; min(bound_Y); -Inf; phi_init-(pi/2); -Inf]; + ub = [max(bound_X); Inf; max(bound_Y); Inf; phi_init+(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; phi_init-(pi/2); -Inf]; + ub_x = [max(bound_X); Inf; max(bound_Y); Inf; phi_init+(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, TestTrack) + if size(z,2)>size(z,1) + z = z' ; + end + numState = 6; + numInput = 2; + numXandU = 8; + nsteps = (size(z,1)+2)/numXandU; + + dt = 0.01; + + zx=z(1:nsteps*numState); + zu=z(nsteps*numState + 1:end); + + g = zeros(nsteps,1) ; + dg = zeros(nsteps,numXandU*nsteps-2) ; + + h = zeros(numState*nsteps,1) ; + dh = zeros(numState*nsteps,numXandU*nsteps-2); + + for i=1:nsteps + % At given position (Xi, Yi) at Zi, find nearest centerline + % Use the index of nearest centerline to calculate g function + +% pos = [Xi Yi]; + car_pos = [z(numState*i-numState+1); z(numState*i-numState+3)]; + d = vecnorm(TestTrack.cline - car_pos); + eps = 1; + Idx = find(d <= eps); + while(isempty(Idx)) + eps = eps + 1; + Idx = find(d <= eps); + end + + idx = Idx(1); + prev_idx = max(idx-1, 1); + next_idx = min(idx+1, size(TestTrack.cline, 2)); + + bl_vec = [TestTrack.bl(1,[prev_idx,next_idx]);TestTrack.bl(2,[prev_idx,next_idx])]; + br_vec = [TestTrack.br(1,[prev_idx,next_idx]);TestTrack.br(2,[prev_idx,next_idx])]; + + g = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2)); + + dgi_dzj = diff_g(pose, bl_prev, bl_next, br_prev, br_next); + dg(i,numState*i-numState+1) = dgi_dzj(1); + dg(i,numState*i-numState+2) = dgi_dzj(2); + +% if i==1 +% h(1:3) = z(1:3,:) ; +% dh(1:3,1:3)=eye(3); +% else +% h(3*i-2:3*i) = zx(3*i-2:3*i)-zx(3*i-5:3*i-3)-... +% dt*odefun(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ; +% dh(3*i-2:3*i,3*i-5:3*i) = [-eye(3)-dt*statepart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)),eye(3)] ; +% dh(3*i-2:3*i,3*nsteps+2*i-3:3*nsteps+2*i-2) = -dt*inputpart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ; +% end + end +end + + + + + + + + + + + + + + + + + + + diff --git a/SimPkg_F21(student_ver)/ROB535_ControlProject_part1_Team3.mat b/SimPkg_F21(student_ver)/ROB535_ControlProject_part1_Team3.mat new file mode 100644 index 0000000..ca3bb36 Binary files /dev/null and b/SimPkg_F21(student_ver)/ROB535_ControlProject_part1_Team3.mat differ diff --git a/SimPkg_F21(student_ver)/ROB535_Control_Project.pdf b/SimPkg_F21(student_ver)/ROB535_Control_Project.pdf new file mode 100644 index 0000000..718ae97 Binary files /dev/null and b/SimPkg_F21(student_ver)/ROB535_Control_Project.pdf differ diff --git a/SimPkg_F21(student_ver)/TestTrack.mat b/SimPkg_F21(student_ver)/TestTrack.mat new file mode 100644 index 0000000..7c4ba0f Binary files /dev/null and b/SimPkg_F21(student_ver)/TestTrack.mat differ diff --git a/SimPkg_F21(student_ver)/forwardIntegrate.m b/SimPkg_F21(student_ver)/forwardIntegrate.m new file mode 100644 index 0000000..d14e890 --- /dev/null +++ b/SimPkg_F21(student_ver)/forwardIntegrate.m @@ -0,0 +1,116 @@ +function [Y,U,t_total,t_update] = forwardIntegrate() +% [Y,U,t_total,t_update] = forwardIntegrate +% +% This script returns the vehicle trajectory with control input being +% generated via the control input generation function: +% ROB535_ControlProject_part2_Team +% Obstacles are randomly generated along the test track. Notice that the +% vehicle can only sense (observe) the obstacles within 150m, therefore +% the control input generation function is called repeatedly. In this +% script, we assume the control input generation function is called every +% 'dt' second (see line 32). +% +% OUTPUTS: +% Y an N-by-6 vector where each column is the trajectory of the +% state of the vehicle +% +% U an N-by-2 vector of inputs, where the first column is the +% steering input in radians, and the second column is the +% longitudinal force in Newtons +% +% t_total a scalar that records the total computational time +% +% t_update a M-by-1 vector of time that records the time consumption +% when the control input generation function is called +% +% Written by: Jinsun Liu +% Created: 31 Oct 2021 + + + load('TestTrack.mat') % load test track + + dt = 0.5; + TOTAL_TIME = 20*60; % second + + % initialization + t_total = 0; + t_update = zeros(TOTAL_TIME/dt+1,1); + Y = zeros(TOTAL_TIME/0.01+1,6); + U = zeros(TOTAL_TIME/0.01,2); + Y(1,:) = [287,5,-176,0,2,0]; + + % generate obstacles along the track + Xobs = generateRandomObstacles(9 + randi(16),TestTrack); + + iteration = 1; % a counter that counts how many times the control input + % generation function is called. + + TIMER = tic; % start the timer + + % you only have TOTAL_TIME seconds to sense the obstacles, update + % control inputs, and simulate forward vehicle dynamcis. + while t_total < TOTAL_TIME + curr_pos = Y( (iteration-1)*dt/0.01+1 , [1,3] ); % record current vehicle position + Xobs_seen = senseObstacles(curr_pos, Xobs); % sense the obstacles within 150m + curr_state = Y( (iteration-1)*dt/0.01+1 , : ); % record current vehicle states + + + + % compute control inputs, and record the time consumption + t_temp = toc(TIMER); + %%%%%%%%%%%%%%%% THIS IS WHERE YOUR FUNCTION IS CALLED (replace in your team number). %%%%%%%%%%%%%%%%%%%%%%%%%%% + [Utemp, FLAG_terminate] = ROB535_ControlProject_part2_Team(TestTrack,Xobs_seen,curr_state); %% + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% FLAG_terminate = randi(2)-1 % GSIs: This line is just for us to debug. Feel free to play with it if you want +% Utemp = rand(dt/0.01+1 + FLAG_terminate* (randi(10)-5),2); % GSIs: This line is just for us to debug. Feel free to play with it if you want + t_update(iteration) = toc(TIMER)-t_temp; + + + + % Utemp must contain control inputs for at least dt second, + % otherwise stop the whole computation. + if size(Utemp,1)
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 + diff --git a/SimPkg_F21(student_ver)/generateRandomObstacles.m b/SimPkg_F21(student_ver)/generateRandomObstacles.m new file mode 100644 index 0000000..cd41ae5 --- /dev/null +++ b/SimPkg_F21(student_ver)/generateRandomObstacles.m @@ -0,0 +1,101 @@ +function Xobs = generateRandomObstacles(Nobs,TestTrack) +% Xobs = generateRandomObstacles(Nobs) +% +% Given a number of obstacles Nobs and a track, place obstacles at random +% orientations with one corner of each obstacle pinned to the center line +% of the track +% +% INPUTS: +% Nobs an integer defining the number of obstacles to output +% +% TestTrack a TestTrack object for which TestTrack.cline is the +% centerline +% +% OUTPUTS: +% Xobs a 1-by-Nobs cell array, where each cell contains a single +% rectangular obstacle defined as a 4-by-2 matrix where the +% first column is the x coordinates and the second column is +% the y coordinates +% +% Written by: Shreyas Kousik +% Created: 12 Nov 2017 +% Modified: 13 Nov 2017 + + if nargin < 2 + loaded_file = load('TestTrack.mat') ; + TestTrack = loaded_file.TestTrack ; + end + + if Nobs > 100 + warning(['Number of obstacles is greater than 100! This is likely to ',... + 'make the resulting course infeasible.']) + end + + % get the center line and boundaries, but exclude the parts of the + % track that are close to the beginning and end + c = TestTrack.cline(:,4:end-4) ; + h = TestTrack.theta(:,4:end-4) ; + + % get the cumulative and total distance along the centerline + dists_along_cline = cumsum([0, sqrt(diff(c(1,:)).^2 + diff(c(2,:)).^2)]) ; + total_dist_along_cline = dists_along_cline(end) ; + + % create a vector of random distances between obstacles + min_dist_btwn_obs = 10 ; % meters + max_dist_btwn_obs = total_dist_along_cline / Nobs ; % also meters + dists_btwn_obs = (max_dist_btwn_obs-min_dist_btwn_obs).*rand(1,Nobs) + min_dist_btwn_obs ; + obs_start_dists = cumsum(dists_btwn_obs) ; + + % scale up the distances between the obstacles to be along the whole length + % of the track (this means the min and max distanes between obstacles will + % increase, but this is a hack anyways, so hah) + end_pct = 0.1*rand(1) + 0.85 ; + obs_start_dists = obs_start_dists.*(end_pct*total_dist_along_cline./obs_start_dists(end)) ; + + % NOTE: The lines above are meant to encourage the track to be + % feasible, but there is never a 100% guarantee off feasibility + + % get the start point and orientation of each obstacle + obs_start_x = interp1(dists_along_cline,c(1,:),obs_start_dists) ; + obs_start_y = interp1(dists_along_cline,c(2,:),obs_start_dists) ; + obs_heading = interp1(dists_along_cline,h,obs_start_dists) ; + + % generate a random size and random side of the road for each obstacle; + % the parameters below work well for the COTA track segment that we are + % using in ROB 599 + obs_min_length = 1 ; + obs_max_length = 4 ; + obs_min_width = 3 ; + obs_max_width = 7 ; + obs_lengths = (obs_max_length - obs_min_length).*rand(1,Nobs) + obs_min_length ; + obs_widths = (obs_max_width - obs_min_width).*rand(1,Nobs) + obs_min_width ; + obs_sides = round(rand(1,Nobs)) ; % 0 is right, 1 is left + + % from each start point, create a CCW contour defining a box that is + % pinned to the centerline at one corner + Xobs = cell(1,Nobs) ; + for idx = 1:Nobs + % create box + lidx = obs_lengths(idx) ; + widx = obs_widths(idx) ; + obs_box = [0, 0, lidx, lidx ; + 0, -widx, -widx, 0] ; + + % if the box is on left side of track, shift it by +widx in its + % local y-direction + obs_box = obs_box + obs_sides(idx).*[zeros(1,4) ; widx.*ones(1,4)] ; + + % rotate box to track orientation + hidx = obs_heading(idx) ; + Ridx = [cos(hidx) -sin(hidx) ; sin(hidx) cos(hidx)] ; + obs_box = Ridx*obs_box ; + + % shift box to start point + xidx = obs_start_x(idx) ; + yidx = obs_start_y(idx) ; + obs_box = obs_box + repmat([xidx;yidx],1,4) ; + + % fill in Xobs + Xobs{idx} = obs_box' ; + end +end \ No newline at end of file diff --git a/SimPkg_F21(student_ver)/getTrajectoryInfo.m b/SimPkg_F21(student_ver)/getTrajectoryInfo.m new file mode 100644 index 0000000..f81157e --- /dev/null +++ b/SimPkg_F21(student_ver)/getTrajectoryInfo.m @@ -0,0 +1,95 @@ +function info = getTrajectoryInfo(Y,U,Xobs,t_update,TestTrack) +% info = getTrajectoryInfo(Y,U,Xobs) +% +% Given a trajectory, input, and a cell array of obstacles, return +% information about whether or not the trajectory left the track or crashed +% into any obstacles, and if the input limits were exceeded. +% +% NOTE: the trajectory is only for the vehicle's center of mass, so we +% are not checking if the "corners" of the vehicle leave the track or hit +% any obstacles. +% +% INPUTS: +% Y an N-by-2 trajectory in the x and y coordinates of the +% vehicle's states, where the first column is x and the +% second column is y OR an N-by-6 trajectory in the full +% state, where the first column is x and the third column is +% y +% +% U an N-by-2 vector of inputs, where the first column is the +% steering angle and the second column is the rear wheel +% driving force +% +% Xobs a 1-by-Nobs cell array where each cell contains a 4-by-2 +% obstacle definition, as would be generated by the +% generateRandomObstacles function (this is an optional +% argument, so leave it out if your trajectory doesn't avoid +% any obstacles) +% +% TestTrack a TestTrack object for which TestTrack.cline is the +% centerline (this is an optional argument; the function will +% by default try to load the provided TestTrack.mat file) +% +% t_update a M-by-1 vector of time that records the time consumption +% when the control input generation function is called +% +% OUTPUTS: +% info a struct containing the following catergories +% +% info.Y : The trajectory given as an input argument to the +% function. +% +% info.U : The inputs given as an input argument to the +% function. +% +% info.t_finished : Time in seconds when the finish line is +% crossed. An empty vector is returned if finish line is not +% crossed. +% +% info.t_end : Time in seconds at end of trajectory. +% +% info.left_track_position : 2-by-1 vector with x and y +% coordinates of location where vehicle first leaves the +% track. An empty vector is returned if vehicle never leaves +% the track. +% +% info.left_track_time : Time in seconds when vehicle first +% leaves the track. An empty vector is returned if vehicle +% never leaves the track. +% +% info.left_percent_of_track_completed : Percentage of the +% track completed before vehicle first leaves the track. An +% empty vector is returned if vehicle never leaves the track. +% +% info.crash_position : 2-by-1 vector with x and y +% coordinates of location where vehicle first hits an +% obstacle. An empty vector is returned if vehicle never hits +% an obstacle. +% +% info.crash_time : Time in seconds when vehicle first hits +% an obstacle. An empty vector is returned if vehicle never +% hits an obstacle. +% +% info.crash_percent_of_track_completed : Percentage of the +% track completed before vehicle first hits an obstacle. An +% empty vector is returned if vehicle never hits an obstacle. +% +% info.input_exceeded : 1-by-2 boolean with the first entry +% being true if constraints on input 1 are violated and entry +% 2 being true if constraints on input 2 are violated. +% +% info.percent_of_track_completed : Percentage of track +% complete prior to leaving the track, hitting an obstacle, +% or the control inputs end. +% +% info.t_score : Score of computational time as +% info.t_finished + M * max(num_exceed_time_limit,0), +% where M(=10) is some penalty value, num_exceed_time_limit +% is the number of elements in t_update that are longer than +% 0.5 second. An empty vector is returned if finish line is +% not crossed. +% +% Written by: Shreyas Kousik and Matthew Porter +% Created: 14 Dec 2017 +% Modified: 24 Oct 2018 +% Modified: 1 Nov 2021 (Jinsun Liu) diff --git a/SimPkg_F21(student_ver)/getTrajectoryInfo.p b/SimPkg_F21(student_ver)/getTrajectoryInfo.p new file mode 100644 index 0000000..9cd2f46 Binary files /dev/null and b/SimPkg_F21(student_ver)/getTrajectoryInfo.p differ diff --git a/SimPkg_F21(student_ver)/part1_generate_trajectory.m b/SimPkg_F21(student_ver)/part1_generate_trajectory.m new file mode 100644 index 0000000..e330ecd --- /dev/null +++ b/SimPkg_F21(student_ver)/part1_generate_trajectory.m @@ -0,0 +1,221 @@ +%% ROB 535 Team 3 Control Project +close all; +clear; +clc; + +%% Model Parameters +% Vehicle Parameters +delta_lim = [-0.5, 0.5]; +F_x_lim = [-5000, 5000]; +m = 1400; +N_w = 2; +f = 0.01; +I_z = 2667; +a = 1.35; +b = 1.45; +B_y = 0.27; +C_y = 1.2; +D_y = 0.7; +E_y = -1.6; +S_hy = 0; +S_vy = 0; +g = 9.806; +% Note: The Pacejka parameters are for the slip angle in degrees + +% Initial Conditions +x_0 = 287; % meters +u_0 = 5; % meters/second +y_0 = -176; % meters +v_0 = 0; % meters/second +psi_0 = 2; % radians +r_0 = 0; % radians/second +state_0 = [x_0, u_0, y_0, v_0, psi_0, r_0]; + +load('TestTrack.mat') + +%% Trajectory Synthesis +% Segment 1 +segment_num = 1; +num_pts(segment_num) = 6e2; +delta_vals(segment_num) = -0.004; +F_x_vals(segment_num) = 3900; + +% Segment 2 +segment_num = 2; +num_pts(segment_num) = 4e2; +delta_vals(segment_num) = -0.3; +F_x_vals(segment_num) = -2000; + +% Segment 3 +segment_num = 3; +num_pts(segment_num) = 1e2; +delta_vals(segment_num) = -0.05; +F_x_vals(segment_num) = 0; + +% Segment 4 +segment_num = 4; +num_pts(segment_num) = 7.5e2; +delta_vals(segment_num) = 0.0; +F_x_vals(segment_num) = 1000; + +% Segment 5 +segment_num = 5; +num_pts(segment_num) = 3e2; +delta_vals(segment_num) = 0.3; +F_x_vals(segment_num) = -500; + +% Segment 6 +segment_num = 6; +num_pts(segment_num) = 3.5e2; +delta_vals(segment_num) = -0.03; +F_x_vals(segment_num) = 1000; + +% Segment 7 +segment_num = 7; +num_pts(segment_num) = 1e2; +delta_vals(segment_num) = -0.005; +F_x_vals(segment_num) = -1000; + +% Segment 8 +segment_num = 8; +num_pts(segment_num) = 2e2; +delta_vals(segment_num) = 0.0275; +F_x_vals(segment_num) = -750; + +% Segment 9 +segment_num = 9; +num_pts(segment_num) = 2.4e2; +delta_vals(segment_num) = 0.5; +F_x_vals(segment_num) = -500; + +% Segment 10 +segment_num = 10; +num_pts(segment_num) = 5e2; +delta_vals(segment_num) = -0.02; +F_x_vals(segment_num) = 0; + +% Segment 11 +segment_num = 11; +num_pts(segment_num) = 2.5e2; +delta_vals(segment_num) = -0.05; +F_x_vals(segment_num) = 500; + +% Segment 12 +segment_num = 12; +num_pts(segment_num) = 2e2; +delta_vals(segment_num) = -0.01; +F_x_vals(segment_num) = 5000; + +% Segment 13 +segment_num = 13; +num_pts(segment_num) = 2e2; +delta_vals(segment_num) = -0.1; +F_x_vals(segment_num) = -2000; + +% Segment 14 +segment_num = 14; +num_pts(segment_num) = 3e2; +delta_vals(segment_num) = 0.175; +F_x_vals(segment_num) = -2000; + +% Segment 15 +segment_num = 15; +num_pts(segment_num) = 4.75e2; +delta_vals(segment_num) = 0.0025; +F_x_vals(segment_num) = 1000; + +% Segment 16 +segment_num = 16; +num_pts(segment_num) = 4.5e2; +delta_vals(segment_num) = 0.05; +F_x_vals(segment_num) = 0; + +% Segment 17 +segment_num = 17; +num_pts(segment_num) = 5e2; +delta_vals(segment_num) = 0.0; +F_x_vals(segment_num) = 500; + +% Segment 18 +segment_num = 18; +num_pts(segment_num) = 8e2; +delta_vals(segment_num) = -0.05; +F_x_vals(segment_num) = -500; + +% Segment 19 +segment_num = 19; +num_pts(segment_num) = 5.8e2; +delta_vals(segment_num) = 0.065; +F_x_vals(segment_num) = 0; + +% Segment 20 +segment_num = 20; +num_pts(segment_num) = 7.5e2; +delta_vals(segment_num) = 0; +F_x_vals(segment_num) = 2000; + +% Segment 21 +segment_num = 21; +num_pts(segment_num) = 2.25e2; +delta_vals(segment_num) = 0.5; +F_x_vals(segment_num) = -4400; + +% Segment 22 +segment_num = 22; +num_pts(segment_num) = 9e2; +delta_vals(segment_num) = 0.0; +F_x_vals(segment_num) = 5000; + +%% Load Inputs from File +load('ROB535_ControlProject_part1_Team3.mat'); + +%% Simulate Trajectory +for i = 1:length(num_pts) + [start_idx, end_idx] = get_indices(i, num_pts); + + delta = delta_vals(i); + F_x = F_x_vals(i); + U(start_idx:end_idx,:) = [delta * ones(num_pts(i),1), F_x * ones(num_pts(i),1)]; +end + +[Y, T] = forwardIntegrateControlInput(U, state_0); +info = getTrajectoryInfo(Y,U) + +[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0); +info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input) + +%% Figures +% Plot segmented trajectory for debugging purposes +figure(1) +hold on; +grid on; + +for i = 1:length(num_pts) + [start_idx, end_idx] = get_indices(i, num_pts); + plot(Y(start_idx:end_idx,1), Y(start_idx:end_idx,3), '-'); +end + +plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r'); +plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r'); +plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g'); + +% Plot final trajectory from submission inputs +figure(2) +hold on; +grid on; + +plot(Y_submission(:,1), Y_submission(:,3), '-b'); +plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '-r'); +plot(TestTrack.br(1,:), TestTrack.br(2,:), '-r'); +plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '--g'); + +%% Functions +function [start_idx, end_idx] = get_indices(segment_num, num_pts) + if segment_num == 1 + start_idx = 1; + end_idx = num_pts(segment_num); + else + start_idx = sum(num_pts(1:segment_num-1)) + 1; + end_idx = sum(num_pts(1:segment_num)); + end +end \ No newline at end of file diff --git a/SimPkg_F21(student_ver)/senseObstacles.m b/SimPkg_F21(student_ver)/senseObstacles.m new file mode 100644 index 0000000..8b3dfb7 --- /dev/null +++ b/SimPkg_F21(student_ver)/senseObstacles.m @@ -0,0 +1,27 @@ +function Xobs_seen = senseObstacles(curr_pos, Xobs) +% Xobs_seen = senseObstacles(curr_pos, Xobs) +% +% Given the current vehicle position, sense the obstacles within 150m. +% +% INPUTS: +% curr_pos a 2-by-1 vector where the 1st and 2nd elements represent the +% x and y coordinates of the current vehicle position +% +% Xobs a cell array generated by generateRandomObstacles.m +% +% OUTPUTS: +% Xobs_seen a cell array which contains all obstacles that are no +% greater than 150m from the vehicle. Each cell has the same +% structure as the cells in Xobs. +% +% Written by: Jinsun Liu +% Created: 31 Oct 2021 + + + + + Xobs_mat = cell2mat(Xobs'); + dist = (Xobs_mat(:,1) - curr_pos(1)).^2 + (Xobs_mat(:,2) - curr_pos(2)).^2; + idx = unique(ceil(find(dist<=150^2)/4)); + Xobs_seen = {Xobs{idx}}; +end \ No newline at end of file diff --git a/SimPkg_F21(student_ver)/xenia_final_project.asv b/SimPkg_F21(student_ver)/xenia_final_project.asv new file mode 100644 index 0000000..2e44e66 --- /dev/null +++ b/SimPkg_F21(student_ver)/xenia_final_project.asv @@ -0,0 +1,204 @@ +%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] + +init = [287, 5, -176, 0, 2, 0]; +curr_xy = [287, -176]; +U_try = [0.4, 5000]; + +for i = 1:12 + U_try = [U_try; U_try]; +end +U_final = []; +U_try = [0.4, 5000; 0.4, 5000]; +[Y, T] = forwardIntegrateControlInput(U_try); +traj_inf = getTrajectoryInfo(Y, U_try); +%traj_inf.t_finished + +init = [287, 5, -176, 0, 2, 0]; +U_try = [0.25, 2500; 0.25, 2500; 0.25, 2500]; +noise = [randn*0.01, randn*10]; +U_try = U_try + noise; +[Y, T] = forwardIntegrateControlInput(U_try); +traj_inf = getTrajectoryInfo(Y, U_try); +U_final = U_try; + +max_percent = 0; +num_iter = 0; +subt = 0; +randomize = 0; +percent_comp = 0; +avgcount = []; +while (size(traj_inf.t_finished) == 0) +%for i = 1:100 + noise = [randn*0.01, randn*10]; + U_try = U_try + noise; + [Y_temp, T] = forwardIntegrateControlInput(U_try, Y(end,:)); + traj_inf = getTrajectoryInfo([Y;Y_temp], [U_final; U_try]); + + + if mod(num_iter+1, 11) == 0 | traj_inf.percent_of_track_completed < (percent_comp - 0.0001) + randomize = 1; + end + %if crashes, redo, if not add on and keep going + %also will allow it to change inputs once in a while for randomization + + %next thing to onsider trying is the intersectLineSegment thing and + %following centerline + count = 1; + factor = 3; + while size(traj_inf.left_track_position) ~= 0 | randomize == 1 + if subt == 0 + U_og = U_try; + U_try = U_try + [0.011, 250]; + if (U_try(1,1) > 0.49 && U_try(1,2) > 4900) + subt = 1; + end + if U_try(1,1) > 0.45 + U_try = U_try - [0.05, 0]; + end + if U_try(1,2) > 4500 + U_try = U_try - [0, 660]; + end + noise = [randn*0.01, randn*10]; + U_try = U_try + noise; + end + + if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700) + subt = 1; + [Y_comp1, T] = forwardIntegrateControlInput(U_try, Y(end,:)); + U_try = U_og; + end + + + if subt == 1 + if (U_try(1,1) < -0.49 && U_try(1,2) < -4900) + subt = 0; + end + U_try = U_try - [0.011, 250]; + if U_try(1,1) < -0.45 + U_try = U_try + [0.05, 0]; + end + if U_try(1,2) < -4500 + U_try = U_try + [0, 660]; + end + noise = [randn*0.01, randn*10]; + U_try = U_try + noise; + end + + if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700) + [Y_comp2, T] = forwardIntegrateControlInput(U_try, Y(end,:)); + + + for i = 1:(246/3) + dist1 = norm([Y_comp1(:,1)'; Y_comp1(:,3)'] - TestTrack.cline(:,i:i+2)); + dist2 = norm([Y_comp2(:,1)'; Y_comp2(:,3)'] - TestTrack.cline(:,i:i+2)); + + diff1 = min(dist1); + diff2 = min(dist2); + + if diff1 < 20 | diff2 < 20 + break + end + end + + if diff1 > diff2 + subt = 0; + end + if diff2 > diff1 + subt = 1; + end + + U_try = U_og; + if subt == 0 + U_try = U_try + [0.011, 250]; +% if (U_try(1,1) > 0.49 && U_try(1,2) > 4900) +% subt = 1; +% end + if U_try(1,1) > 0.5 + U_try = U_try - [0.05, 0]; + end + if U_try(1,2) > 4000 + U_try = U_try - [0, 760]; + end + end + + + if subt == 1 +% if (U_try(1,1) < -0.49 && U_try(1,2) < -4900) +% subt = 0; +% end + U_try = U_try - [0.011, 250]; + if U_try(1,1) < -0.5 + U_try = U_try + [0.05, 0]; + end + if U_try(1,2) < -4000 + U_try = U_try + [0, 760]; + end + + end + end + + %instead we'll go back one step and so we'll send 6 commands and + %check + + if mod(count,14) == 0 + factor = factor + 3; + end + + if factor > size(Y,1) + Y = []; + U_try = [rand*0.5, rand*100; rand*0.5, rand*100; rand*0.5, rand*100; + + U_try(:,1) = min(max(U_try(:,1), -0.5), 0.5); + U_try(:,2) = min(max(U_try(:,2), -5000), 5000); + U_try2 = U_try; + + for j = 1:(factor/3) + U_try2 = [U_try2; U_try]; + end + + [Y_temp, T] = forwardIntegrateControlInput(U_try2, Y(end-factor,:)); + traj_inf = getTrajectoryInfo([Y(1:end-factor,:);Y_temp], [U_final(1:end-factor,:); U_try2]); + + if size(traj_inf.left_track_position) == 0 + U_final = U_final(1:end-factor,:); + Y = Y(1:end-factor,:); + end + count = count + 1; + randomize = 0; + end + + if count ~= 1 + avgcount = [avgcount ; count]; + end + + if (traj_inf.percent_of_track_completed > percent_comp) + percent_comp = traj_inf.percent_of_track_completed; + end + + U_final = [U_final; U_try]; + Y = [Y; Y_temp(1:3,:)]; + num_iter = num_iter + count +end + + + + + diff --git a/SimPkg_F21(student_ver)/xenia_final_project.m b/SimPkg_F21(student_ver)/xenia_final_project.m new file mode 100644 index 0000000..26799bf --- /dev/null +++ b/SimPkg_F21(student_ver)/xenia_final_project.m @@ -0,0 +1,206 @@ +%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] + +init = [287, 5, -176, 0, 2, 0]; +curr_xy = [287, -176]; +U_try = [0.4, 5000]; + +for i = 1:12 + U_try = [U_try; U_try]; +end +U_final = []; +U_try = [0.4, 5000; 0.4, 5000]; +[Y, T] = forwardIntegrateControlInput(U_try); +traj_inf = getTrajectoryInfo(Y, U_try); +%traj_inf.t_finished + +init = [287, 5, -176, 0, 2, 0]; +U_try = [0.25, 2500; 0.25, 2500; 0.25, 2500]; +noise = [randn*0.01, randn*10]; +U_try = U_try + noise; +[Y, T] = forwardIntegrateControlInput(U_try); +traj_inf = getTrajectoryInfo(Y, U_try); +U_final = U_try; + +max_percent = 0; +num_iter = 0; +subt = 0; +randomize = 0; +percent_comp = 0; +avgcount = []; +while (size(traj_inf.t_finished) == 0) +%for i = 1:100 + noise = [randn*0.01, randn*10]; + U_try = U_try + noise; + [Y_temp, T] = forwardIntegrateControlInput(U_try, Y(end,:)); + traj_inf = getTrajectoryInfo([Y;Y_temp], [U_final; U_try]); + + + if mod(num_iter+1, 11) == 0 | traj_inf.percent_of_track_completed < (percent_comp - 0.0001) + randomize = 1; + end + %if crashes, redo, if not add on and keep going + %also will allow it to change inputs once in a while for randomization + + %next thing to onsider trying is the intersectLineSegment thing and + %following centerline + count = 1; + factor = 3; + while size(traj_inf.left_track_position) ~= 0 | randomize == 1 + if subt == 0 + U_og = U_try; + U_try = U_try + [0.011, 250]; + if (U_try(1,1) > 0.49 && U_try(1,2) > 4900) + subt = 1; + end + if U_try(1,1) > 0.45 + U_try = U_try - [0.05, 0]; + end + if U_try(1,2) > 4500 + U_try = U_try - [0, 660]; + end + noise = [randn*0.01, randn*10]; + U_try = U_try + noise; + end + + if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700) + subt = 1; + [Y_comp1, T] = forwardIntegrateControlInput(U_try, Y(end,:)); + U_try = U_og; + end + + + if subt == 1 + if (U_try(1,1) < -0.49 && U_try(1,2) < -4900) + subt = 0; + end + U_try = U_try - [0.011, 250]; + if U_try(1,1) < -0.45 + U_try = U_try + [0.05, 0]; + end + if U_try(1,2) < -4500 + U_try = U_try + [0, 660]; + end + noise = [randn*0.01, randn*10]; + U_try = U_try + noise; + end + + if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700) + [Y_comp2, T] = forwardIntegrateControlInput(U_try, Y(end,:)); + + + for i = 1:(246/3) + dist1 = norm([Y_comp1(:,1)'; Y_comp1(:,3)'] - TestTrack.cline(:,i:i+2)); + dist2 = norm([Y_comp2(:,1)'; Y_comp2(:,3)'] - TestTrack.cline(:,i:i+2)); + + diff1 = min(dist1); + diff2 = min(dist2); + + if diff1 < 20 | diff2 < 20 + break + end + end + + if diff1 > diff2 + subt = 0; + end + if diff2 > diff1 + subt = 1; + end + + U_try = U_og; + if subt == 0 + U_try = U_try + [0.011, 250]; +% if (U_try(1,1) > 0.49 && U_try(1,2) > 4900) +% subt = 1; +% end + if U_try(1,1) > 0.5 + U_try = U_try - [0.05, 0]; + end + if U_try(1,2) > 4000 + U_try = U_try - [0, 760]; + end + end + + + if subt == 1 +% if (U_try(1,1) < -0.49 && U_try(1,2) < -4900) +% subt = 0; +% end + U_try = U_try - [0.011, 250]; + if U_try(1,1) < -0.5 + U_try = U_try + [0.05, 0]; + end + if U_try(1,2) < -4000 + U_try = U_try + [0, 760]; + end + + end + end + + %instead we'll go back one step and so we'll send 6 commands and + %check + + if mod(count,14) == 0 + factor = factor + 3; + end + + if factor > 5 + enteredifstatementon166 = 1 + break + break + end + + U_try(:,1) = min(max(U_try(:,1), -0.5), 0.5); + U_try(:,2) = min(max(U_try(:,2), -5000), 5000); + U_try2 = U_try; + + for j = 1:(factor/3) + U_try2 = [U_try2; U_try]; + end + + [Y_temp, T] = forwardIntegrateControlInput(U_try2, Y(end-factor,:)); + traj_inf = getTrajectoryInfo([Y(1:end-factor,:);Y_temp], [U_final(1:end-factor,:); U_try2]); + + if size(traj_inf.left_track_position) == 0 + U_final = U_final(1:end-factor,:); + Y = Y(1:end-factor,:); + end + count = count + 1; + randomize = 0; + end + + if count ~= 1 + avgcount = [avgcount ; count]; + end + + if (traj_inf.percent_of_track_completed > percent_comp) + percent_comp = traj_inf.percent_of_track_completed; + end + + U_final = [U_final; U_try]; + Y = [Y; Y_temp(1:3,:)]; + num_iter = num_iter + count +end + + + + + diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.asv b/SimPkg_F21(student_ver)/xenia_nonlinearopt.asv new file mode 100644 index 0000000..e2af018 --- /dev/null +++ b/SimPkg_F21(student_ver)/xenia_nonlinearopt.asv @@ -0,0 +1,281 @@ +%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 diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m index a5e8c2e..92e6b59 100644 --- a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m +++ b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m @@ -52,8 +52,8 @@ function [lb, ub] = bounds(StepsPerPoint) 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]; + lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi); -Inf]; + ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; for i=1:nPts @@ -66,8 +66,8 @@ function [lb, ub] = bounds(StepsPerPoint) 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]; + lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf]; + ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; for num = 1:StepsPerPoint lb=[lb;lb_x];