From 593d451ebf84fdc2d754a04b1e929bc786ab46b7 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Mon, 13 Dec 2021 14:21:52 -0500 Subject: [PATCH] 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 --- Deliverables/MPC_Class.m | 192 ++++++++++++++++++++------ Experimentation/miscellaneous_stuff.m | 47 ++++++- 2 files changed, 195 insertions(+), 44 deletions(-) diff --git a/Deliverables/MPC_Class.m b/Deliverables/MPC_Class.m index a49c61e..feb76e1 100644 --- a/Deliverables/MPC_Class.m +++ b/Deliverables/MPC_Class.m @@ -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 - diff --git a/Experimentation/miscellaneous_stuff.m b/Experimentation/miscellaneous_stuff.m index f75bf6e..39cdd3b 100644 --- a/Experimentation/miscellaneous_stuff.m +++ b/Experimentation/miscellaneous_stuff.m @@ -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'); \ No newline at end of file +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'); \ No newline at end of file