diff --git a/Main_Peter.m b/Main_Peter.m index 7b66d3b..ddb63a2 100644 --- a/Main_Peter.m +++ b/Main_Peter.m @@ -1,220 +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 - - - - - - - - - - - - - - - - - - - +%% ====================================================================== +% 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)/Main_Peter.m b/SimPkg_F21(student_ver)/Main_Peter.m deleted file mode 100644 index ddb63a2..0000000 --- a/SimPkg_F21(student_ver)/Main_Peter.m +++ /dev/null @@ -1,220 +0,0 @@ -%% ====================================================================== -% 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 deleted file mode 100644 index ca3bb36..0000000 Binary files a/SimPkg_F21(student_ver)/ROB535_ControlProject_part1_Team3.mat and /dev/null differ diff --git a/SimPkg_F21(student_ver)/ROB535_Control_Project.pdf b/SimPkg_F21(student_ver)/ROB535_Control_Project.pdf deleted file mode 100644 index 718ae97..0000000 Binary files a/SimPkg_F21(student_ver)/ROB535_Control_Project.pdf and /dev/null differ diff --git a/SimPkg_F21(student_ver)/TestTrack.mat b/SimPkg_F21(student_ver)/TestTrack.mat deleted file mode 100644 index 7c4ba0f..0000000 Binary files a/SimPkg_F21(student_ver)/TestTrack.mat and /dev/null differ diff --git a/SimPkg_F21(student_ver)/forwardIntegrate.m b/SimPkg_F21(student_ver)/forwardIntegrate.m deleted file mode 100644 index d14e890..0000000 --- a/SimPkg_F21(student_ver)/forwardIntegrate.m +++ /dev/null @@ -1,116 +0,0 @@ -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 deleted file mode 100644 index cd41ae5..0000000 --- a/SimPkg_F21(student_ver)/generateRandomObstacles.m +++ /dev/null @@ -1,101 +0,0 @@ -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 deleted file mode 100644 index f81157e..0000000 --- a/SimPkg_F21(student_ver)/getTrajectoryInfo.m +++ /dev/null @@ -1,95 +0,0 @@ -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 deleted file mode 100644 index 9cd2f46..0000000 Binary files a/SimPkg_F21(student_ver)/getTrajectoryInfo.p and /dev/null differ diff --git a/SimPkg_F21(student_ver)/part1_generate_trajectory.m b/SimPkg_F21(student_ver)/part1_generate_trajectory.m deleted file mode 100644 index e330ecd..0000000 --- a/SimPkg_F21(student_ver)/part1_generate_trajectory.m +++ /dev/null @@ -1,221 +0,0 @@ -%% 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 deleted file mode 100644 index 8b3dfb7..0000000 --- a/SimPkg_F21(student_ver)/senseObstacles.m +++ /dev/null @@ -1,27 +0,0 @@ -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 deleted file mode 100644 index 2e44e66..0000000 --- a/SimPkg_F21(student_ver)/xenia_final_project.asv +++ /dev/null @@ -1,204 +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] - -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_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 diff --git a/SimPkg_F21(student_ver)/reftrack_info.mat b/reftrack_info.mat similarity index 100% rename from SimPkg_F21(student_ver)/reftrack_info.mat rename to reftrack_info.mat diff --git a/SimPkg_F21(student_ver)/segments_info.mat b/segments_info.mat similarity index 100% rename from SimPkg_F21(student_ver)/segments_info.mat rename to segments_info.mat diff --git a/SimPkg_F21(student_ver)/xenia_final_project.m b/xenia_final_project.m similarity index 100% rename from SimPkg_F21(student_ver)/xenia_final_project.m rename to xenia_final_project.m diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m b/xenia_nonlinearopt.m similarity index 100% rename from SimPkg_F21(student_ver)/xenia_nonlinearopt.m rename to xenia_nonlinearopt.m