mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-21 18:12:45 +00:00
593d451ebf
- 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
111 lines
3.5 KiB
Matlab
111 lines
3.5 KiB
Matlab
%% Part 1 Testing Xenia's Code
|
|
close all;
|
|
figure(1)
|
|
hold on
|
|
plot(Y_ref(1,:), Y_ref(3,:), 'g-')
|
|
plot(Y(1,:), Y(3,:), 'b-')
|
|
|
|
[Y_test, T_test] = forwardIntegrateControlInput(U', init);
|
|
traj_info = getTrajectoryInfo(Y_test(1:439,:), U(:,1:439)')
|
|
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 for Linearized System
|
|
close all;
|
|
clear all;
|
|
clc;
|
|
|
|
Nw = 2;
|
|
f = 0.01;
|
|
Iz = 2667;
|
|
a = 1.35;
|
|
b = 1.45;
|
|
By = 0.27;
|
|
Cy = 1.2;
|
|
Dy = 0.7;
|
|
Ey = -1.6;
|
|
Shy = 0;
|
|
Svy = 0;
|
|
m = 1400;
|
|
g = 9.806;
|
|
|
|
syms x_var u_var y_var v_var psi_var r_var ... % states
|
|
delta_f_var F_x_var ... % inputs
|
|
F_yf_var F_yr_var % lateral forces
|
|
|
|
% Vehicle Dynamics (Equation 1)
|
|
dx = u_var*cos(psi_var) - v_var*sin(psi_var);
|
|
du = (1/m)*(-f*m*g + Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var;
|
|
dy = u_var*sin(psi_var) + v_var*cos(psi_var);
|
|
dv = (1/m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var;
|
|
dpsi = r_var;
|
|
dr = (1/Iz)*(a*F_yf_var*cos(delta_f_var) - b*F_yr_var);
|
|
|
|
% Jacobians of continuous-time linearized system
|
|
% TODO: Change A_c & B_c computation to regular functions (e.g.,
|
|
% using `matlabFunction`) if speed is an issue
|
|
A_c_symb = [ ...
|
|
diff(dx, x_var), diff(dx, u_var), diff(dx, y_var), diff(dx, v_var), diff(dx, psi_var), diff(dx, r_var);
|
|
diff(du, x_var), diff(du, u_var), diff(du, y_var), diff(du, v_var), diff(du, psi_var), diff(du, r_var);
|
|
diff(dy, x_var), diff(dy, u_var), diff(dy, y_var), diff(dy, v_var), diff(dy, psi_var), diff(dy, r_var);
|
|
diff(dv, x_var), diff(dv, u_var), diff(dv, y_var), diff(dv, v_var), diff(dv, psi_var), diff(dv, r_var);
|
|
diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var);
|
|
diff(dr, x_var), diff(dr, u_var), diff(dr, y_var), diff(dr, v_var), diff(dr, psi_var), diff(dr, r_var);
|
|
];
|
|
|
|
B_c_symb = [ ...
|
|
diff(dx, delta_f_var), diff(dx, F_x_var);
|
|
diff(du, delta_f_var), diff(du, F_x_var);
|
|
diff(dy, delta_f_var), diff(dy, F_x_var);
|
|
diff(dv, delta_f_var), diff(dv, F_x_var);
|
|
diff(dpsi,delta_f_var), diff(dpsi,F_x_var);
|
|
diff(dr, delta_f_var), diff(dr, F_x_var);
|
|
];
|
|
|
|
A_c = matlabFunction(A_c_symb, 'File', 'A_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'); |