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

nonlinear trajectory based on sravan segments

Browse Source
This commit is contained in:
xdemenchuk
2021-11-29 18:00:10 -05:00
parent 21434cb4de
commit 3a954497dc
3 changed files with 132 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
SimPkg_F21(student_ver)/reftrack_info.mat Normal file
View File

Binary file not shown.

BIN
SimPkg_F21(student_ver)/segments_info.mat Normal file
View File

Binary file not shown.

200
SimPkg_F21(student_ver)/xenia_nonlinearopt.m
View File

@@ -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 %generate z and make it bigger if didn't reach close enough to the goal
%(modify nsteps) %(modify nsteps)
Nobs = 25; Nobs = randi([10 25], 1,1);
global Xobs global Xobs
Xobs = generateRandomObstacles(Nobs); Xobs = generateRandomObstacles(Nobs);
global index_cl_cp %index of current checkpoint in centerline % global index_cl_cp %index of current checkpoint in centerline
index_cl_cp = 1; % index_cl_cp = 1;
global index_cl_nextcp %index of next checkpoint in centerline % global index_cl_nextcp %index of next checkpoint in centerline
index_cl_nextcp = 2; % index_cl_nextcp = 2;
%
% global nsteps
% nsteps = 100;
global nsteps % x0 = init;
nsteps = 100; % 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; % dist12atcp = [];
x0vec = []; % iter = 0;
%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 % factor = 0.1; %next checkpoint if significantly closer to next checkpoint
%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
U_final = []; U_final = [];
Y_final = []; Y_final = [];
options = optimoptions('fmincon','SpecifyConstraintGradient',true,... options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
'SpecifyObjectiveGradient',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)';
[Y, T] = forwardIntegrateControlInput(U, x0vec(1:6)); load('ROB535_ControlProject_part1_Team3.mat');
%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
load('reftrack_info.mat');
load('segments_info.mat');
curr_xy = [Y(end,1); Y(end,3)]; for i = 1:length(num_pts)
dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp)); [start_idx, end_idx] = get_indices(i, num_pts);
dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
if (dist2cp2 < (dist2cp1 - dist2cp1*factor)) delta = delta_vals(i);
dist12atcp = [dist12atcp; dist2cp1, dist2cp2]; F_x = F_x_vals(i);
%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 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 end
function x0vec = initvec(x0, u0) function x0vec = initvec(x0, u0)
@@ -130,8 +184,9 @@ function x0vec = initvec(x0, u0)
end end
end end
function [lb, ub] = bounds(StepsPerPoint, index) function [lb, ub] = bounds(start_idx, end_idx)
load('TestTrack.mat'); load('TestTrack.mat');
load('reftrack_info.mat');
%[m,nPts] = size(TestTrack.cline); %[m,nPts] = size(TestTrack.cline);
% numState = 6; % numState = 6;
@@ -153,26 +208,31 @@ function [lb, ub] = bounds(StepsPerPoint, index)
lb = []; ub = []; lb = []; ub = [];
%hijacking this for loop to only consider one index for bounds %hijacking this for loop to only consider one index for bounds
for i=index:index for i=start_idx:end_idx+1
prev_idx = max(i-1, 1); Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)];
next_idx = min(i+1, 246); 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), ... bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
TestTrack.br(1,prev_idx), TestTrack.br(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), ... bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
TestTrack.br(2,prev_idx), TestTrack.br(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]; lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
ub_x = [max(bound_X); Inf; max(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]; lb=[lb;lb_x];
ub=[ub;ub_x]; ub=[ub;ub_x];
end
end end
for i=1:StepsPerPoint-1 for i=1:start_idx:end_idx
ub=[ub;ub_u]; ub=[ub;ub_u];
lb=[lb;lb_u]; lb=[lb;lb_u];
end end
@@ -181,11 +241,11 @@ end
function [J, dJ] = costfun(z) function [J, dJ] = costfun(z)
global nsteps global nsteps
global index_cl_nextcp global target_vec
load('TestTrack.mat'); load('TestTrack.mat');
targetPos = TestTrack.cline(:, index_cl_nextcp); targetPos = target_vec(1:2);
targetTheta = TestTrack.theta(index_cl_nextcp); targetTheta = target_vec(3);
sumPos = []; sumPos = [];
sumInput = []; sumInput = [];
for i = 1:nsteps for i = 1:nsteps