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

Try adjusting track boundary constraints (still being violated, but not detected)

Browse Source 
- Use all track boundary points when checking if position is inside
- Still fails to stop simulation when car goes off track
This commit is contained in:
Sravan Balaji
2021-12-13 07:45:23 -05:00
parent 91fc8ff4c3
commit d591bb68c9
1 changed files with 9 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
Deliverables/MPC_Class.m
View File

@@ -96,19 +96,13 @@ classdef MPC_Class
1e6; ... % within track bounds
1e6; ... % outside obstacles
];
options = ...
optimoptions( ...
'fmincon', ...
'MaxFunctionEvaluations', 4000, ...
'SpecifyConstraintGradient', false, ...
'SpecifyObjectiveGradient', false ...
);
% Constraint Parameters
ntrack_boundary_pts = 1; % Number of track boundary points
% around closest boundary point
% to use when checking if position
% is within track polygon
end
%% Public Functions
@@ -171,13 +165,13 @@ classdef MPC_Class
obj.options ... % options: Optimization options
);
% Check `fmincon` exitflag
% Stop sim if `fmincon` failed to find feasible point
if exitflag == -2
disp('No feasible point was found')
obj.FLAG_terminate = 1;
end
% Check if nonlinear constraint was satisfied
% Stop sim if nonlinear constraint was violated
[c, ~] = obj.nonlcon(Z_err);
if any(c > 0)
disp('Found point violates nonlinear inequality constraint')
@@ -339,16 +333,12 @@ classdef MPC_Class
% Get xy position from state vector
p = [Y(1); Y(3)];
% Find closest boundary points to position
[~,bl_idx] = min(vecnorm(obj.TestTrack.bl - p));
[~,br_idx] = min(vecnorm(obj.TestTrack.br - p));
% Use closest boundary points +/- ntrack_boundary_pts
% to construct track polygon
bl_idx_start = obj.clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
bl_idx_end = obj.clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
br_idx_start = obj.clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
br_idx_end = obj.clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
% Construct track polygon from all boundary points
bl_idx_start = 1;
bl_idx_end = size(obj.TestTrack.bl,2);
br_idx_start = 1;
br_idx_end = size(obj.TestTrack.br,2);
boundary_pts = [ ...
obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...