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

28
Deliverables/MPC_Class.m
View File

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