mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 09:12:45 +00:00
nonlinear trajectory based on sravan segments
This commit is contained in:
xdemenchuk
BIN
SimPkg_F21(student_ver)/reftrack_info.mat
Normal file
BIN
SimPkg_F21(student_ver)/reftrack_info.mat
Normal file
Binary file not shown.
BIN
SimPkg_F21(student_ver)/segments_info.mat
Normal file
BIN
SimPkg_F21(student_ver)/segments_info.mat
Normal file
Binary file not shown.
@@ -38,78 +38,132 @@ curr_pos = [init(1);init(3)];
|
||||
|
||||
%generate z and make it bigger if didn't reach close enough to the goal
|
||||
%(modify nsteps)
|
||||
Nobs = 25;
|
||||
Nobs = randi([10 25], 1,1);
|
||||
global Xobs
|
||||
Xobs = generateRandomObstacles(Nobs);
|
||||
|
||||
global index_cl_cp %index of current checkpoint in centerline
|
||||
index_cl_cp = 1;
|
||||
global index_cl_nextcp %index of next checkpoint in centerline
|
||||
index_cl_nextcp = 2;
|
||||
% global index_cl_cp %index of current checkpoint in centerline
|
||||
% index_cl_cp = 1;
|
||||
% global index_cl_nextcp %index of next checkpoint in centerline
|
||||
% index_cl_nextcp = 2;
|
||||
%
|
||||
% global nsteps
|
||||
% nsteps = 100;
|
||||
|
||||
global nsteps
|
||||
nsteps = 100;
|
||||
% x0 = init;
|
||||
% x0vec = [];
|
||||
% %initialize x0vec to be initial condition for n steps and Sravan's ref inputs
|
||||
% %note: function 'initvec' changes inputs, but i'm not sure how
|
||||
% %initialization should look with fmincon
|
||||
% for i = 1:nsteps
|
||||
% x0vec = [x0vec, init];
|
||||
% end
|
||||
% for i = 1:nsteps-1
|
||||
% x0vec = [x0vec, -0.004, 3900];
|
||||
% end
|
||||
|
||||
x0 = init;
|
||||
x0vec = [];
|
||||
%initialize x0vec to be initial condition for n steps and Sravan's ref inputs
|
||||
%note: function 'initvec' changes inputs, but i'm not sure how
|
||||
%initialization should look with fmincon
|
||||
for i = 1:nsteps
|
||||
x0vec = [x0vec, init];
|
||||
end
|
||||
for i = 1:nsteps-1
|
||||
x0vec = [x0vec, -0.004, 3900];
|
||||
end
|
||||
|
||||
dist12atcp = [];
|
||||
iter = 0;
|
||||
|
||||
factor = 0.1; %next checkpoint if significantly closer to next checkpoint
|
||||
% dist12atcp = [];
|
||||
% iter = 0;
|
||||
%
|
||||
% factor = 0.1; %next checkpoint if significantly closer to next checkpoint
|
||||
U_final = [];
|
||||
Y_final = [];
|
||||
options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
|
||||
'SpecifyObjectiveGradient',true) ;
|
||||
while (index_cl_cp < size(TestTrack.cline,2))
|
||||
%because of the way cf/nc need to be)
|
||||
index_cl_cp
|
||||
iter = iter + 1;
|
||||
[lb, ub] = bounds(nsteps, index_cl_cp);
|
||||
cf=@costfun
|
||||
nc=@nonlcon
|
||||
z=fmincon(cf,x0vec,[],[],[],[],lb',ub',nc,options);
|
||||
Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
|
||||
load('ROB535_ControlProject_part1_Team3.mat');
|
||||
%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
|
||||
load('reftrack_info.mat');
|
||||
load('segments_info.mat');
|
||||
|
||||
for i = 1:length(num_pts)
|
||||
[start_idx, end_idx] = get_indices(i, num_pts);
|
||||
|
||||
[Y, T] = forwardIntegrateControlInput(U, x0vec(1:6));
|
||||
delta = delta_vals(i);
|
||||
F_x = F_x_vals(i);
|
||||
|
||||
curr_xy = [Y(end,1); Y(end,3)];
|
||||
dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
|
||||
dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
|
||||
|
||||
if (dist2cp2 < (dist2cp1 - dist2cp1*factor))
|
||||
dist12atcp = [dist12atcp; dist2cp1, dist2cp2];
|
||||
%add to final solution
|
||||
U_final = [U_final; U];
|
||||
Y_final = [Y_final; Y];
|
||||
|
||||
%reinstantiate
|
||||
%nsteps = 100;
|
||||
x0vec = initvec(Y_final(end,:), U_final(end,:));
|
||||
|
||||
%update checkpoint
|
||||
index_cl_cp = index_cl_cp + 1
|
||||
index_cl_nextcp = index_cl_nextcp + 1;
|
||||
if (index_cl_nextcp > size(TestTrack.cline, 2))
|
||||
index_cl_nextcp = size(TestTrack.cline, 2);
|
||||
end
|
||||
else
|
||||
%resize and try again
|
||||
%nsteps = nsteps + 20;
|
||||
x0vec = initvec(x0vec(1:6), U(1,:));
|
||||
|
||||
if (end_idx >= size(Y_submission,1))
|
||||
start_idx = start_idx - 1;
|
||||
end_idx = end_idx - 1;
|
||||
end
|
||||
|
||||
x0 = [];
|
||||
for j = start_idx:end_idx+1 %+1 end idx to keep z size consistent to hw
|
||||
x0 = [x0, Y_submission(j,:)];
|
||||
end
|
||||
|
||||
for j = start_idx:end_idx
|
||||
x0 = [x0, delta, F_x];
|
||||
end
|
||||
|
||||
%start and end index used to maintain size of vectors
|
||||
[lb, ub] = bounds(start_idx, end_idx);
|
||||
|
||||
%define for cost function, goal is to reach end of segment
|
||||
global target_vec
|
||||
target_vec = [Y_submission(end_idx,1), Y_submission(end_idx,3), Y_submission(end_idx,5)];
|
||||
|
||||
global nsteps
|
||||
nsteps = num_pts(i)+1;
|
||||
|
||||
cf=@costfun
|
||||
nc=@nonlcon
|
||||
z=fmincon(cf,x0,[],[],[],[],lb',ub',nc,options);
|
||||
Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
info = getTrajectoryInfo(Y0,U)
|
||||
U_final = [U_final; U];
|
||||
end
|
||||
% while (index_cl_cp < size(TestTrack.cline,2))
|
||||
% %because of the way cf/nc need to be)
|
||||
% index_cl_cp
|
||||
% iter = iter + 1;
|
||||
% [lb, ub] = bounds(nsteps, index_cl_cp);
|
||||
% cf=@costfun
|
||||
% nc=@nonlcon
|
||||
% z=fmincon(cf,x0vec,[],[],[],[],lb',ub',nc,options);
|
||||
% Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
% U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
%
|
||||
% [Y, T] = forwardIntegrateControlInput(U, x0vec(1:6));
|
||||
%
|
||||
% curr_xy = [Y(end,1); Y(end,3)];
|
||||
% dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
|
||||
% dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
|
||||
%
|
||||
% if (dist2cp2 < (dist2cp1 - dist2cp1*factor))
|
||||
% dist12atcp = [dist12atcp; dist2cp1, dist2cp2];
|
||||
% %add to final solution
|
||||
% U_final = [U_final; U];
|
||||
% Y_final = [Y_final; Y];
|
||||
%
|
||||
% %reinstantiate
|
||||
% %nsteps = 100;
|
||||
% x0vec = initvec(Y_final(end,:), U_final(end,:));
|
||||
%
|
||||
% %update checkpoint
|
||||
% index_cl_cp = index_cl_cp + 1
|
||||
% index_cl_nextcp = index_cl_nextcp + 1;
|
||||
% if (index_cl_nextcp > size(TestTrack.cline, 2))
|
||||
% index_cl_nextcp = size(TestTrack.cline, 2);
|
||||
% end
|
||||
% else
|
||||
% %resize and try again
|
||||
% %nsteps = nsteps + 20;
|
||||
% x0vec = initvec(x0vec(1:6), U(1,:));
|
||||
%
|
||||
% end
|
||||
%
|
||||
% end
|
||||
|
||||
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
|
||||
|
||||
function x0vec = initvec(x0, u0)
|
||||
@@ -130,8 +184,9 @@ function x0vec = initvec(x0, u0)
|
||||
end
|
||||
end
|
||||
|
||||
function [lb, ub] = bounds(StepsPerPoint, index)
|
||||
function [lb, ub] = bounds(start_idx, end_idx)
|
||||
load('TestTrack.mat');
|
||||
load('reftrack_info.mat');
|
||||
|
||||
%[m,nPts] = size(TestTrack.cline);
|
||||
% numState = 6;
|
||||
@@ -153,26 +208,31 @@ function [lb, ub] = bounds(StepsPerPoint, index)
|
||||
lb = []; ub = [];
|
||||
|
||||
%hijacking this for loop to only consider one index for bounds
|
||||
for i=index:index
|
||||
prev_idx = max(i-1, 1);
|
||||
next_idx = min(i+1, 246);
|
||||
for i=start_idx:end_idx+1
|
||||
Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)];
|
||||
sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
|
||||
dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
|
||||
[minDist, indMin] = min(dist_to_cl);
|
||||
|
||||
prev_idx = max(indMin-1, 1);
|
||||
next_idx = min(indMin, 246);
|
||||
|
||||
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
|
||||
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
|
||||
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
|
||||
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
|
||||
phi_init = TestTrack.theta(i);
|
||||
%phi_init = TestTrack.theta(i);
|
||||
|
||||
lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
|
||||
ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
|
||||
|
||||
for num = 1:StepsPerPoint
|
||||
lb=[lb;lb_x];
|
||||
ub=[ub;ub_x];
|
||||
end
|
||||
|
||||
lb=[lb;lb_x];
|
||||
ub=[ub;ub_x];
|
||||
|
||||
end
|
||||
|
||||
for i=1:StepsPerPoint-1
|
||||
for i=1:start_idx:end_idx
|
||||
ub=[ub;ub_u];
|
||||
lb=[lb;lb_u];
|
||||
end
|
||||
@@ -181,11 +241,11 @@ end
|
||||
function [J, dJ] = costfun(z)
|
||||
|
||||
global nsteps
|
||||
global index_cl_nextcp
|
||||
global target_vec
|
||||
load('TestTrack.mat');
|
||||
|
||||
targetPos = TestTrack.cline(:, index_cl_nextcp);
|
||||
targetTheta = TestTrack.theta(index_cl_nextcp);
|
||||
targetPos = target_vec(1:2);
|
||||
targetTheta = target_vec(3);
|
||||
sumPos = [];
|
||||
sumInput = [];
|
||||
for i = 1:nsteps
|
||||
|
||||
Reference in New Issue
Block a user