mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 09:12:45 +00:00
Bunch of Fixes & Add Some Testing Scripts
- Fix missing references to `obj` - Fix `npredinputs` so it matches `npredstates` as specified by `forwardIntegrate.m` - Fix typo: `get_start_idx` -> `obj.get_state_start_idx` - Fix nonlinear constraint indexing issue - Fix nonlinear constraint obstacle indexing typo - Remove unused `nonlinear_bike_model` function - Compute continuous system matrices offline and convert to function with `matlabFunction`: `A_c_func` & `B_c_func` - Add `miscellaneous_stuff.m` which has some random testing and continuous system matrix generation code - Add `part2_test_controller.m` for testing part2 deliverable
This commit is contained in:
Sravan Balaji
@@ -120,7 +120,7 @@ classdef MPC_Class
|
|||||||
% Calculate decision variable related quantities
|
% Calculate decision variable related quantities
|
||||||
obj.npred = obj.T_p / obj.T_s;
|
obj.npred = obj.T_p / obj.T_s;
|
||||||
obj.npredstates = obj.npred + 1;
|
obj.npredstates = obj.npred + 1;
|
||||||
obj.npredinputs = obj.npred;
|
obj.npredinputs = obj.npred + 1;
|
||||||
|
|
||||||
obj.ndecstates = obj.npredstates * obj.nstates;
|
obj.ndecstates = obj.npredstates * obj.nstates;
|
||||||
obj.ndecinputs = obj.npredinputs * obj.ninputs;
|
obj.ndecinputs = obj.npredinputs * obj.ninputs;
|
||||||
@@ -187,12 +187,12 @@ classdef MPC_Class
|
|||||||
H = NaN(obj.ndec, 1);
|
H = NaN(obj.ndec, 1);
|
||||||
|
|
||||||
for i = 0:obj.npredstates-1
|
for i = 0:obj.npredstates-1
|
||||||
start_idx = get_state_start_idx(i);
|
start_idx = obj.get_state_start_idx(i);
|
||||||
H(start_idx+1:start_idx+obj.nstates) = obj.Q;
|
H(start_idx+1:start_idx+obj.nstates) = obj.Q;
|
||||||
end
|
end
|
||||||
|
|
||||||
for i = 0:obj.npredinputs-1
|
for i = 0:obj.npredinputs-1
|
||||||
start_idx = get_input_start_idx(i);
|
start_idx = obj.get_input_start_idx(i);
|
||||||
H(start_idx+1:start_idx+obj.ninputs) = obj.R;
|
H(start_idx+1:start_idx+obj.ninputs) = obj.R;
|
||||||
end
|
end
|
||||||
|
|
||||||
@@ -222,7 +222,7 @@ classdef MPC_Class
|
|||||||
|
|
||||||
% State Limits
|
% State Limits
|
||||||
for i = 0:obj.npredstates-1
|
for i = 0:obj.npredstates-1
|
||||||
start_idx = get_state_start_idx(i);
|
start_idx = obj.get_state_start_idx(i);
|
||||||
|
|
||||||
% x position limits
|
% x position limits
|
||||||
Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(obj.ref_idx+i, 1);
|
Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(obj.ref_idx+i, 1);
|
||||||
@@ -235,7 +235,7 @@ classdef MPC_Class
|
|||||||
|
|
||||||
% Input Limits
|
% Input Limits
|
||||||
for i = 0:obj.npredinputs-1
|
for i = 0:obj.npredinputs-1
|
||||||
start_idx = get_input_start_idx(i);
|
start_idx = obj.get_input_start_idx(i);
|
||||||
|
|
||||||
% delta_f input limits
|
% delta_f input limits
|
||||||
Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(obj.ref_idx+i, 1);
|
Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(obj.ref_idx+i, 1);
|
||||||
@@ -309,7 +309,7 @@ classdef MPC_Class
|
|||||||
% Construct constraint for each state
|
% Construct constraint for each state
|
||||||
for i = 0:obj.npredstates-1
|
for i = 0:obj.npredstates-1
|
||||||
% Get index of current state in decision variable
|
% Get index of current state in decision variable
|
||||||
idx = get_start_idx(i);
|
idx = obj.get_state_start_idx(i);
|
||||||
Y = Z(idx+1:idx+obj.nstates);
|
Y = Z(idx+1:idx+obj.nstates);
|
||||||
|
|
||||||
% Get xy position from state vector
|
% Get xy position from state vector
|
||||||
@@ -337,7 +337,7 @@ classdef MPC_Class
|
|||||||
|
|
||||||
if ~in_track
|
if ~in_track
|
||||||
% Position not inside track
|
% Position not inside track
|
||||||
c(i) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
|
c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
|
||||||
|
|
||||||
% Skip to next constraint
|
% Skip to next constraint
|
||||||
continue;
|
continue;
|
||||||
@@ -345,22 +345,22 @@ classdef MPC_Class
|
|||||||
|
|
||||||
for j = 1:size(obj.Xobs_seen,2)
|
for j = 1:size(obj.Xobs_seen,2)
|
||||||
% Check if position is in or on each obstacle
|
% Check if position is in or on each obstacle
|
||||||
xv_obstacle = obj.Xobs_seen{i}(:,1);
|
xv_obstacle = obj.Xobs_seen{j}(:,1);
|
||||||
yv_obstacle = obj.Xobs_seen{i}(:,2);
|
yv_obstacle = obj.Xobs_seen{j}(:,2);
|
||||||
[in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle);
|
[in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle);
|
||||||
|
|
||||||
if in_obstacle || on_obstacle
|
if in_obstacle || on_obstacle
|
||||||
% Point in or on obstacle
|
% Point in or on obstacle
|
||||||
c(i) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
|
c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated
|
||||||
|
|
||||||
% Skip remaining obstacle checking
|
% Skip remaining obstacle checking
|
||||||
break;
|
break;
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
if isnan(c(i))
|
if isnan(c(i+1))
|
||||||
% If value not set, no constraints violated
|
% If value not set, no constraints violated
|
||||||
c(i) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied
|
c(i+1) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
@@ -368,79 +368,15 @@ classdef MPC_Class
|
|||||||
|
|
||||||
%% Private Kinematic Models
|
%% Private Kinematic Models
|
||||||
methods (Access = private)
|
methods (Access = private)
|
||||||
function dYdt = nonlinear_bike_model(obj, Y, U)
|
|
||||||
%nonlinear_bike_model Computes the derivative of the states
|
|
||||||
% based on current states and inputs using the full
|
|
||||||
% nonlinear bike model.
|
|
||||||
|
|
||||||
% NOTE: I don't think this function is currently being used at all
|
|
||||||
|
|
||||||
[~,u,~,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(Y, U);
|
|
||||||
|
|
||||||
% Vehicle Dynamics (Equation 1)
|
|
||||||
dx = u*cos(psi) - v*sin(psi);
|
|
||||||
du = (1/obj.m)*(-obj.f*obj.m*obj.g + obj.Nw*F_x - F_yf*sin(delta_f)) + v*r;
|
|
||||||
dy = u*sin(psi) + v*cos(psi);
|
|
||||||
dv = (1/obj.m)*(F_yf*cos(delta_f) + F_yr) - u*r;
|
|
||||||
dpsi = r;
|
|
||||||
dr = (1/obj.Iz)*(obj.a*F_yf*cos(delta_f) - obj.b*F_yr);
|
|
||||||
dYdt = [dx; du; dy; dv; dpsi; dr];
|
|
||||||
end
|
|
||||||
|
|
||||||
function [A, B] = linearized_bike_model(obj)
|
function [A, B] = linearized_bike_model(obj)
|
||||||
%linearized_bike_model Computes the discrete-time LTV
|
%linearized_bike_model Computes the discrete-time LTV
|
||||||
% system matrices of the nonlinear bike model linearized
|
% system matrices of the nonlinear bike model linearized
|
||||||
% around the reference trajectory starting at the index
|
% around the reference trajectory starting at the index
|
||||||
% closest to `Y_curr`
|
% closest to `Y_curr`
|
||||||
|
|
||||||
syms x_var u_var y_var v_var psi_var r_var ... % states
|
% Continuous-time system
|
||||||
delta_f_var F_x_var ... % inputs
|
A_c = @(i) obj.A_c_func(i);
|
||||||
F_yf_var F_yr_var % lateral forces
|
B_c = @(i) obj.B_c_func(i);
|
||||||
|
|
||||||
% Vehicle Dynamics (Equation 1)
|
|
||||||
dx = u_var*cos(psi_var) - v_var*sin(psi_var);
|
|
||||||
du = (1/obj.m)*(-obj.f*obj.m*obj.g + obj.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/obj.m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var;
|
|
||||||
dpsi = r_var;
|
|
||||||
dr = (1/obj.Iz)*(obj.a*F_yf_var*cos(delta_f_var) - obj.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);
|
|
||||||
];
|
|
||||||
|
|
||||||
% Substitute values from reference trajectory into symbolic Jacobians
|
|
||||||
A_c = @(i) double( ...
|
|
||||||
subs( ...
|
|
||||||
A_c_symb, ...
|
|
||||||
[x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
|
|
||||||
obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ...
|
|
||||||
) ...
|
|
||||||
);
|
|
||||||
B_c = @(i) double( ...
|
|
||||||
subs( ...
|
|
||||||
B_c_symb, ...
|
|
||||||
[x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
|
|
||||||
obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ...
|
|
||||||
) ...
|
|
||||||
);
|
|
||||||
|
|
||||||
% Discrete-time LTV system
|
% Discrete-time LTV system
|
||||||
A = @(i) eye(obj.nstates) + obj.T_s*A_c(i);
|
A = @(i) eye(obj.nstates) + obj.T_s*A_c(i);
|
||||||
@@ -494,6 +430,56 @@ classdef MPC_Class
|
|||||||
delta_f = obj.clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2));
|
delta_f = obj.clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2));
|
||||||
F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
|
F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
|
||||||
end
|
end
|
||||||
|
|
||||||
|
function A_c = A_c_func(obj, i)
|
||||||
|
%A_c Computes the continuous time `A` matrix
|
||||||
|
|
||||||
|
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||||
|
% 12-Dec-2021 15:05:59
|
||||||
|
|
||||||
|
[~,u_var,~,v_var,psi_var,r_var,~,~,~,~] ...
|
||||||
|
= obj.bike_model_helper( ...
|
||||||
|
obj.Y_ref(obj.ref_idx+i,:), ...
|
||||||
|
obj.U_ref(obj.ref_idx+i,:) ...
|
||||||
|
);
|
||||||
|
|
||||||
|
t2 = cos(psi_var);
|
||||||
|
t3 = sin(psi_var);
|
||||||
|
A_c = reshape([ ...
|
||||||
|
0.0,0.0,0.0,0.0,0.0,0.0, ...
|
||||||
|
t2,0.0,t3,-r_var,0.0,0.0, ...
|
||||||
|
0.0,0.0,0.0,0.0,0.0,0.0, ...
|
||||||
|
-t3,r_var,t2,0.0,0.0,0.0, ...
|
||||||
|
-t3.*u_var-t2.*v_var,0.0,t2.*u_var-t3.*v_var,0.0,0.0,0.0, ...
|
||||||
|
0.0,v_var,0.0,-u_var,1.0,0.0 ...
|
||||||
|
], ...
|
||||||
|
[6,6] ...
|
||||||
|
);
|
||||||
|
end
|
||||||
|
|
||||||
|
function B_c = B_c_func(obj, i)
|
||||||
|
%B_c_func Computes the continuous time `B` matrix
|
||||||
|
|
||||||
|
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||||
|
% 12-Dec-2021 15:05:59
|
||||||
|
[~,~,~,~,~,~,delta_f_var,~,F_yf_var,~] ...
|
||||||
|
= obj.bike_model_helper( ...
|
||||||
|
obj.Y_ref(obj.ref_idx+i,:), ...
|
||||||
|
obj.U_ref(obj.ref_idx+i,:) ...
|
||||||
|
);
|
||||||
|
|
||||||
|
t2 = sin(delta_f_var);
|
||||||
|
B_c = reshape([ ...
|
||||||
|
0.0,F_yf_var.*cos(delta_f_var).*(-7.142857142857143e-4), ...
|
||||||
|
0.0,F_yf_var.*t2.*(-7.142857142857143e-4), ...
|
||||||
|
0.0,F_yf_var.*t2.*(-5.061867266591676e-4), ...
|
||||||
|
0.0,1.0./7.0e+2, ...
|
||||||
|
0.0,0.0, ...
|
||||||
|
0.0,0.0 ...
|
||||||
|
], ...
|
||||||
|
[6,2] ...
|
||||||
|
);
|
||||||
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
%% Private Helper Functions
|
%% Private Helper Functions
|
||||||
@@ -540,13 +526,13 @@ classdef MPC_Class
|
|||||||
Z_ref = zeros(obj.ndec, 1);
|
Z_ref = zeros(obj.ndec, 1);
|
||||||
|
|
||||||
for i = 0:obj.npredstates-1
|
for i = 0:obj.npredstates-1
|
||||||
start_idx = get_state_start_idx(i);
|
start_idx = obj.get_state_start_idx(i);
|
||||||
Z_ref(start_idx+1:start_idx+obj.nstates) ...
|
Z_ref(start_idx+1:start_idx+obj.nstates) ...
|
||||||
= obj.Y_ref(obj.ref_idx+i, :);
|
= obj.Y_ref(obj.ref_idx+i, :);
|
||||||
end
|
end
|
||||||
|
|
||||||
for i = 0:obj.npredinputs-1
|
for i = 0:obj.npredinputs-1
|
||||||
start_idx = get_input_start_idx(i);
|
start_idx = obj.get_input_start_idx(i);
|
||||||
Z_ref(start_idx+1:start_idx+obj.ninputs) ...
|
Z_ref(start_idx+1:start_idx+obj.ninputs) ...
|
||||||
= obj.U_ref(obj.ref_idx+i, :);
|
= obj.U_ref(obj.ref_idx+i, :);
|
||||||
end
|
end
|
||||||
|
|||||||
68
Experimentation/miscellaneous_stuff.m
Normal file
68
Experimentation/miscellaneous_stuff.m
Normal file
@@ -0,0 +1,68 @@
|
|||||||
|
%% 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
|
||||||
|
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');
|
||||||
21
Experimentation/part2_test_controller.m
Normal file
21
Experimentation/part2_test_controller.m
Normal file
@@ -0,0 +1,21 @@
|
|||||||
|
%% Close Figures, Clear Workspace, and Clear Terminal
|
||||||
|
close all;
|
||||||
|
clear;
|
||||||
|
clc;
|
||||||
|
|
||||||
|
%% Call simulation functions
|
||||||
|
[Y,U,t_total,t_update] = forwardIntegrate();
|
||||||
|
info = getTrajectoryInfo(Y,U)
|
||||||
|
|
||||||
|
%% Figures
|
||||||
|
% Plot segmented trajectory for debugging purposes
|
||||||
|
figure(1)
|
||||||
|
hold on;
|
||||||
|
grid on;
|
||||||
|
|
||||||
|
plot(Y(:,1), Y(:,3), '-');
|
||||||
|
|
||||||
|
load('TestTrack.mat')
|
||||||
|
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r');
|
||||||
|
plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r');
|
||||||
|
plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g');
|
||||||
Reference in New Issue
Block a user