sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-16 16:12:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Move Files out of "SimPkg_F21(student_ver)" folder

Browse Source
This commit is contained in:
Sravan Balaji
2021-11-30 10:46:06 -05:00
parent 2de1b54ba9
commit b0ad7defd3
18 changed files with 220 additions and 1599 deletions
Download Patch File Download Diff File Expand all files Collapse all files

440
Main_Peter.m
View File

@@ -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

220
SimPkg_F21(student_ver)/Main_Peter.m
View File

@@ -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

BIN
SimPkg_F21(student_ver)/ROB535_ControlProject_part1_Team3.mat
View File

Binary file not shown.

BIN
SimPkg_F21(student_ver)/ROB535_Control_Project.pdf
View File

Binary file not shown.

BIN
SimPkg_F21(student_ver)/TestTrack.mat
View File

Binary file not shown.

116
SimPkg_F21(student_ver)/forwardIntegrate.m
View File

@@ -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<your team number>
% 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<your team number>(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)<dt/0.01+1 && FLAG_terminate == 0
fprintf('When FLAG_terminate = 0, Utemp cannot contain control inputs for less than %f second. \n',dt);
fprintf('Solving process is terminated.\n');
t_total = toc(TIMER);
break
end
if FLAG_terminate == 0
% if FLAG_terminate == 0, simulate forward vehicle dynamics for
% dt second.
U( (iteration-1)*dt/0.01+1:iteration*dt/0.01 , : ) = Utemp(1:dt/0.01,:);
Ytemp = forwardIntegrateControlInput( Utemp(1:dt/0.01+1,:) , curr_state );
Y( (iteration-1)*dt/0.01+2:iteration*dt/0.01+1 , : ) = Ytemp(2:end,:);
% update the counter
iteration = iteration + 1;
else
% if FLAG_terminate == 1, simulate forward vehicle dynamics for
% no more than dt second, and stop the solving process.
simulate_length = min(dt/0.01+1, size(Utemp,1));
U((iteration-1)*dt/0.01+1:(iteration-1)*dt/0.01+simulate_length-1, :) = Utemp(1:simulate_length-1,:);
Ytemp = forwardIntegrateControlInput( Utemp(1:simulate_length,:) , curr_state );
Y((iteration-1)*dt/0.01+2:(iteration-1)*dt/0.01+simulate_length, : ) = Ytemp(2:end,:);
end
% update t_total
t_total = toc(TIMER);
% stop the computation if FLAG_terminate == 1
if FLAG_terminate == 1
break
end
end
% if reach the finish line before TOTAL_TIME, ignore any parts of the
% trajectory after crossing the finish line.
idx_temp = find(sum(abs(Y),2)==0,1);
Y(idx_temp:end,:) = [];
U(idx_temp:end,:) = [];
t_update(iteration:end) = [];
end

114
SimPkg_F21(student_ver)/forwardIntegrateControlInput.m
View File

@@ -1,114 +0,0 @@
function [Y,T]=forwardIntegrateControlInput(U,x0)
% function [Y] = forwardIntegrateControlInput(U,x0)
%
% Given a set of inputs and an initial condition, returns the vehicles
% trajectory. If no initial condition is specified the default for the track
% is used.
%
% INPUTS:
% 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.
%
% x0 a 1-by-6 vector of the initial state of the vehicle.
% If not specified, the default is used
%
% OUTPUTS:
% Y an N-by-6 vector where each column is the trajectory of the
% state of the vehicle
%
% T a 1-by-N vector of time stamps for each of the rows of Y.
%
% Written by: Sean Vaskov
% Created: 31 Oct 2018
% Modified: 6 Nov 2018
%
% Revision notes:
% - Sid Dey (6 Dec 2019)
% if initial condition not given use default
if nargin < 2
x0 = [287,5,-176,0,2,0] ;
end
%generate time vector
T=0:0.01:(size(U,1)-1)*0.01;
if (length(T) == 1)
% in case U is a 1x2 vector, meaning T would be a 1x1 scalar,
% we return the initial condition.
Y = x0;
else
%Solve for trajectory
options = odeset('MaxStep',0.01);
[~,Y]=ode45(@(t,x)bike(t,x,T,U),T,x0,options);
% in case U is a 2x2 vector, meaning T would be a 1x2 vector [t0 tf],
% ode45 would provide the solution at its own integration timesteps
% (in between t0 and tf). To avoid this, we simply take the first
% and last value of Y (which should correspond with t0 and tf).
if ( size(U,1) ~= size(Y,1) )
% Take first and last value of Y.
Y = [Y(1, :); Y(end, :)];
end
end
end
function dzdt=bike(t,x,T,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=interp1(T,U(:,1),t,'previous','extrap');
F_x=interp1(T,U(:,2),t,'previous','extrap');
%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

101
SimPkg_F21(student_ver)/generateRandomObstacles.m
View File

@@ -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

95
SimPkg_F21(student_ver)/getTrajectoryInfo.m
View File

@@ -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)

BIN
SimPkg_F21(student_ver)/getTrajectoryInfo.p
View File

Binary file not shown.

221
SimPkg_F21(student_ver)/part1_generate_trajectory.m
View File

@@ -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

27
SimPkg_F21(student_ver)/senseObstacles.m
View File

@@ -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

204
SimPkg_F21(student_ver)/xenia_final_project.asv
View File

@@ -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

281
SimPkg_F21(student_ver)/xenia_nonlinearopt.asv
View File

@@ -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

0
SimPkg_F21(student_ver)/reftrack_info.mat → reftrack_info.mat
View File

0
SimPkg_F21(student_ver)/segments_info.mat → segments_info.mat
View File

0
SimPkg_F21(student_ver)/xenia_final_project.m → xenia_final_project.m
View File

0
SimPkg_F21(student_ver)/xenia_nonlinearopt.m → xenia_nonlinearopt.m
View File