mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-17 00:22:45 +00:00
Add Gradients for Cost Function & Nonlinear Constraints
- Set gradient options to true for `fmincon` optimizer - Play with optimizer limits a bit - Add gradient computation for cost function - Add gradient computation for track & obstacle constraints - Compute gradients offline in `miscellaneous_stuff.m` - Update `point_to_line` to output signed distance
This commit is contained in:
Sravan Balaji
@@ -99,14 +99,15 @@ classdef MPC_Class
|
||||
options = ...
|
||||
optimoptions( ...
|
||||
'fmincon', ...
|
||||
'MaxFunctionEvaluations', 5000, ...
|
||||
'SpecifyConstraintGradient', false, ...
|
||||
'SpecifyObjectiveGradient', false ...
|
||||
'MaxFunctionEvaluations', 3000, ...
|
||||
'MaxIterations', 3000, ...
|
||||
'SpecifyConstraintGradient', true, ...
|
||||
'SpecifyObjectiveGradient', true ...
|
||||
);
|
||||
|
||||
% Constraint Parameters
|
||||
track_dist_fact = 1.00; % Buffer factor on distance from track boundary
|
||||
obs_rad_fact = 0.90; % Buffer factor on obstacle radius
|
||||
obs_rad_fact = 1.00; % Buffer factor on obstacle radius
|
||||
end
|
||||
|
||||
%% Public Functions
|
||||
@@ -201,7 +202,7 @@ classdef MPC_Class
|
||||
|
||||
%% Private Cost Function
|
||||
methods (Access = private)
|
||||
function J = cost_fun(obj, Z_err)
|
||||
function [J, dJ] = cost_fun(obj, Z_err)
|
||||
%cost_fun Compute cost of current Z_err based
|
||||
% on specified state cost (Q) and input cost (R).
|
||||
|
||||
@@ -223,6 +224,7 @@ classdef MPC_Class
|
||||
|
||||
% Evaluate cost function (quadratic function)
|
||||
J = 0.5 * Z_err.' * H * Z_err;
|
||||
dJ = H * Z_err;
|
||||
end
|
||||
end
|
||||
|
||||
@@ -309,7 +311,7 @@ classdef MPC_Class
|
||||
end
|
||||
end
|
||||
|
||||
function [c, ceq] = nonlcon(obj, Z_err)
|
||||
function [c, ceq, gradc, gradceq] = nonlcon(obj, Z_err)
|
||||
%nonlcon Construct nonlinear constraints for
|
||||
% vehicle to stay within track bounds and
|
||||
% outside of obstacles using reference
|
||||
@@ -318,6 +320,7 @@ classdef MPC_Class
|
||||
|
||||
% No equality constraints, so leave `ceq` empty
|
||||
ceq = [];
|
||||
gradceq = [];
|
||||
|
||||
% Calculate number of constraints
|
||||
ntrack_cons = 2; % left and right track boundary
|
||||
@@ -328,6 +331,7 @@ classdef MPC_Class
|
||||
% Nonlinear inequality constraints from track boundary
|
||||
% and obstacles applied to states
|
||||
c = NaN(1, ntotal_cons);
|
||||
gradc = zeros(obj.ndec, ntotal_cons);
|
||||
cons_idx = 1;
|
||||
|
||||
% NOTE: Error = Actual - Reference
|
||||
@@ -339,41 +343,59 @@ classdef MPC_Class
|
||||
for i = 0:obj.npredstates-1
|
||||
% Get index of current state in decision variable
|
||||
idx = obj.get_state_start_idx(i);
|
||||
Y = Z(idx+1:idx+obj.nstates);
|
||||
|
||||
|
||||
% Get xy position from state vector
|
||||
p = [Y(1); Y(3)];
|
||||
p = [Z(idx+1); Z(idx+3)];
|
||||
|
||||
% Get reference and error state information
|
||||
x_ref = Z_ref(idx+1);
|
||||
y_ref = Z_ref(idx+3);
|
||||
x_err = Z_err(idx+1);
|
||||
y_err = Z_err(idx+3);
|
||||
|
||||
% Get indexes of closest two track border points
|
||||
[~,left_idxs] = mink(vecnorm(p - obj.TestTrack.bl), 2);
|
||||
[~,right_idxs] = mink(vecnorm(p - obj.TestTrack.br), 2);
|
||||
|
||||
% Compute distance and direction from left and right boundaries
|
||||
[left_dist, left_direction] = ...
|
||||
obj.point_to_line( ...
|
||||
p, ...
|
||||
obj.TestTrack.bl(:,min(left_idxs)), ...
|
||||
obj.TestTrack.bl(:,max(left_idxs)) ...
|
||||
);
|
||||
[right_dist, right_direction] = ...
|
||||
obj.point_to_line( ...
|
||||
p, ...
|
||||
obj.TestTrack.br(:,min(right_idxs)), ...
|
||||
obj.TestTrack.br(:,max(right_idxs)) ...
|
||||
);
|
||||
v1_left = obj.TestTrack.bl(:,min(left_idxs));
|
||||
v2_left = obj.TestTrack.bl(:,max(left_idxs));
|
||||
v1_right = obj.TestTrack.br(:,min(right_idxs));
|
||||
v2_right = obj.TestTrack.br(:,max(right_idxs));
|
||||
|
||||
% Construct Track Boundary Constraints
|
||||
|
||||
% Direction from left boundary should be negative,
|
||||
% so <= 0 constraint is satisfied when left_direction
|
||||
% so <= 0 constraint is satisfied when left_signed_dist
|
||||
% is negative
|
||||
c(cons_idx) = obj.track_dist_fact * left_dist * left_direction;
|
||||
left_signed_dist = obj.point_to_line(p, v1_left, v2_left);
|
||||
c(cons_idx) = obj.track_dist_fact * left_signed_dist;
|
||||
gradc(idx+1, cons_idx) = ...
|
||||
obj.point_to_line_grad_x( ...
|
||||
v1_left(1), v1_left(2), v2_left(1), v2_left(2), ...
|
||||
x_err, x_ref, y_err, y_ref ...
|
||||
);
|
||||
gradc(idx+3, cons_idx) = ...
|
||||
obj.point_to_line_grad_y( ...
|
||||
v1_left(1), v1_left(2), v2_left(1), v2_left(2), ...
|
||||
x_err, x_ref, y_err, y_ref ...
|
||||
);
|
||||
cons_idx = cons_idx + 1;
|
||||
|
||||
% Direction from right boundary should be positive,
|
||||
% so <= 0 constraint is satisfied when right_direction
|
||||
% so <= 0 constraint is satisfied when right_signed_dist
|
||||
% is positive
|
||||
c(cons_idx) = obj.track_dist_fact * right_dist * -right_direction;
|
||||
right_signed_dist = obj.point_to_line(p, v1_right, v2_right);
|
||||
c(cons_idx) = obj.track_dist_fact * -right_signed_dist;
|
||||
gradc(idx+1, cons_idx) = ...
|
||||
obj.point_to_line_grad_x( ...
|
||||
v1_right(1), v1_right(2), v2_right(1), v2_right(2), ...
|
||||
x_err, x_ref, y_err, y_ref ...
|
||||
);
|
||||
gradc(idx+3, cons_idx) = ...
|
||||
obj.point_to_line_grad_y( ...
|
||||
v1_right(1), v1_right(2), v2_right(1), v2_right(2), ...
|
||||
x_err, x_ref, y_err, y_ref ...
|
||||
);
|
||||
cons_idx = cons_idx + 1;
|
||||
|
||||
% Construct Obstacle Constraints
|
||||
@@ -387,10 +409,9 @@ classdef MPC_Class
|
||||
* obj.obs_rad_fact;
|
||||
|
||||
% Model obstacle as a circle
|
||||
c(cons_idx) = ...
|
||||
(rad_obstacle)^2 ...
|
||||
- (p(1) - cen_obstacle(1))^2 ...
|
||||
- (p(2) - cen_obstacle(2))^2;
|
||||
c(cons_idx) = obj.obstacle_cons(p, rad_obstacle, cen_obstacle);
|
||||
gradc(idx+1, cons_idx) = obj.obstacle_grad_x(cen_obstacle(1), x_err, x_ref);
|
||||
gradc(idx+3, cons_idx) = obj.obstacle_grad_y(cen_obstacle(2), y_err, y_ref);
|
||||
cons_idx = cons_idx + 1;
|
||||
end
|
||||
end
|
||||
@@ -584,27 +605,114 @@ classdef MPC_Class
|
||||
end
|
||||
end
|
||||
|
||||
function [dist, direction] = point_to_line(pt, v1, v2)
|
||||
function signed_dist = point_to_line(pt, v1, v2)
|
||||
%point_to_line Computes shortest distance from
|
||||
% a point pt to a line defined by v1 & v2
|
||||
% and direction (+/- 1) based on axis orthogonal to xy
|
||||
% plane being pointed upwards such that CCW rotation
|
||||
% is positive
|
||||
% with direction indicated by the sign where
|
||||
% CCW rotation is positive
|
||||
|
||||
% Convert 2D column vectors to 3D column vectors
|
||||
pt = [pt; 0];
|
||||
v1 = [v1; 0];
|
||||
v2 = [v2; 0];
|
||||
|
||||
% Compute distance
|
||||
a = v2 - v1; % vector from v1 to v2
|
||||
b = pt - v1; % vector from v1 to pt
|
||||
dist = norm(cross(a,b)) / norm(a);
|
||||
|
||||
% Compute direction as sign of cross product
|
||||
% Compute vectors
|
||||
a = v2 - v1; % vector from v1 to v2
|
||||
b = pt - v1; % vector from v1 to pt
|
||||
cross_p = cross(a,b);
|
||||
direction = sign(cross_p(3));
|
||||
|
||||
% Compute distance & direction
|
||||
dist = norm(cross_p) / norm(a);
|
||||
direction = sign(cross_p(3));
|
||||
signed_dist = dist * direction;
|
||||
end
|
||||
|
||||
function grad = point_to_line_grad_x(v1_x, v1_y, v2_x, v2_y, x_err, x_ref, y_err, y_ref)
|
||||
%point_to_line_grad_x Computes the `x_err` gradient of
|
||||
% point_to_line for track boundary constraint
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||
% 13-Dec-2021 13:46:47
|
||||
|
||||
t2 = -v1_x;
|
||||
t3 = -v1_y;
|
||||
t4 = -v2_x;
|
||||
t5 = -v2_y;
|
||||
t6 = t4+v1_x;
|
||||
t7 = t5+v1_y;
|
||||
t10 = t2+x_err+x_ref;
|
||||
t11 = t3+y_err+y_ref;
|
||||
t8 = abs(t6);
|
||||
t9 = abs(t7);
|
||||
t14 = t7.*t10;
|
||||
t15 = t6.*t11;
|
||||
t12 = t8.^2;
|
||||
t13 = t9.^2;
|
||||
t16 = -t15;
|
||||
t17 = t12+t13;
|
||||
t19 = t14+t16;
|
||||
t18 = 1.0./sqrt(t17);
|
||||
t20 = abs(t19);
|
||||
t21 = t20.^2;
|
||||
grad = t7.*t18.*sqrt(t21).*dirac(t19).*2.0+t7.*t18.*t20.*1.0./sqrt(t21).*sign(t19).^2;
|
||||
end
|
||||
|
||||
function grad = point_to_line_grad_y(v1_x, v1_y, v2_x, v2_y, x_err, x_ref, y_err, y_ref)
|
||||
%point_to_line_grad_y Computes the `y_err` gradient of
|
||||
% point_to_line for track boundary constraint
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||
% 13-Dec-2021 13:46:48
|
||||
|
||||
t2 = -v1_x;
|
||||
t3 = -v1_y;
|
||||
t4 = -v2_x;
|
||||
t5 = -v2_y;
|
||||
t6 = t4+v1_x;
|
||||
t7 = t5+v1_y;
|
||||
t10 = t2+x_err+x_ref;
|
||||
t11 = t3+y_err+y_ref;
|
||||
t8 = abs(t6);
|
||||
t9 = abs(t7);
|
||||
t14 = t7.*t10;
|
||||
t15 = t6.*t11;
|
||||
t12 = t8.^2;
|
||||
t13 = t9.^2;
|
||||
t16 = -t15;
|
||||
t17 = t12+t13;
|
||||
t19 = t14+t16;
|
||||
t18 = 1.0./sqrt(t17);
|
||||
t20 = abs(t19);
|
||||
t21 = t20.^2;
|
||||
grad = t6.*t18.*sqrt(t21).*dirac(t19).*-2.0-t6.*t18.*t20.*1.0./sqrt(t21).*sign(t19).^2;
|
||||
end
|
||||
|
||||
function val = obstacle_cons(p, radius, centroid)
|
||||
%obstacle_cons Computes the value of obstacle constraint
|
||||
% value where obstacle is modeled as a circle with
|
||||
% specified radius and centroid
|
||||
|
||||
val = radius^2 - (p(1) - centroid(1))^2 - (p(2) - centroid(2))^2;
|
||||
end
|
||||
|
||||
function grad = obstacle_grad_x(cen_obstacle_x,x_err,x_ref)
|
||||
%obstacle_grad_x Computes the `x_err` gradient of
|
||||
% obstacle_cons for obstacle constraint
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||
% 13-Dec-2021 14:05:24
|
||||
|
||||
grad = cen_obstacle_x.*2.0-x_err.*2.0-x_ref.*2.0;
|
||||
end
|
||||
|
||||
function grad = obstacle_grad_y(cen_obstacle_y,y_err,y_ref)
|
||||
%obstacle_grad_y Computes the `y_err` gradient of
|
||||
% obstacle_cons for obstacle constraint
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||
% 13-Dec-2021 14:05:25
|
||||
|
||||
grad = cen_obstacle_y.*2.0-y_err.*2.0-y_ref.*2.0;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
@@ -12,7 +12,7 @@ plot(Y_test(:,1), Y_test(:,3), 'k-')
|
||||
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.')
|
||||
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.')
|
||||
|
||||
%% Part 2 Symbolic Jacobians
|
||||
%% Part 2 Symbolic Jacobians for Linearized System
|
||||
close all;
|
||||
clear all;
|
||||
clc;
|
||||
@@ -65,4 +65,47 @@ B_c_symb = [ ...
|
||||
];
|
||||
|
||||
A_c = matlabFunction(A_c_symb, 'File', 'A_c_func');
|
||||
B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');
|
||||
B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');
|
||||
|
||||
%% Part 2 Symbolic Jacobians for Track Constraint
|
||||
close all;
|
||||
clear all;
|
||||
clc;
|
||||
|
||||
syms x_err y_err x_ref y_ref v1_x v1_y v2_x v2_y
|
||||
x = x_err + x_ref;
|
||||
y = y_err + y_ref;
|
||||
|
||||
pt = [x; y; 0];
|
||||
v1 = [v1_x; v1_y; 0];
|
||||
v2 = [v2_x; v2_y; 0];
|
||||
|
||||
% Compute vectors
|
||||
a = v2 - v1; % vector from v1 to v2
|
||||
b = pt - v1; % vector from v1 to pt
|
||||
cross_p = cross(a,b);
|
||||
|
||||
% Compute distance & direction
|
||||
dist = norm(cross_p) / norm(a);
|
||||
direction = sign(cross_p(3));
|
||||
signed_dist = dist * direction;
|
||||
|
||||
temp1 = matlabFunction(jacobian(signed_dist, x_err), 'File', 'signed_dist_x_err');
|
||||
temp2 = matlabFunction(jacobian(signed_dist, y_err), 'File', 'signed_dist_y_err');
|
||||
|
||||
%% Part 2 Symbolic Jacobians for Obstacle Constraint
|
||||
close all;
|
||||
clear all;
|
||||
clc;
|
||||
|
||||
syms x_err y_err x_ref y_ref rad_obstacle cen_obstacle_x cen_obstacle_y
|
||||
x = x_err + x_ref;
|
||||
y = y_err + y_ref;
|
||||
|
||||
obstacle_func = ...
|
||||
(rad_obstacle)^2 ...
|
||||
- (x - cen_obstacle_x)^2 ...
|
||||
- (y - cen_obstacle_y)^2;
|
||||
|
||||
temp1 = matlabFunction(jacobian(obstacle_func, x_err), 'File', 'obstacle_x_err');
|
||||
temp2 = matlabFunction(jacobian(obstacle_func, y_err), 'File', 'obstacle_y_err');
|
||||
Reference in New Issue
Block a user