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
|
||||
obj.npred = obj.T_p / obj.T_s;
|
||||
obj.npredstates = obj.npred + 1;
|
||||
obj.npredinputs = obj.npred;
|
||||
obj.npredinputs = obj.npred + 1;
|
||||
|
||||
obj.ndecstates = obj.npredstates * obj.nstates;
|
||||
obj.ndecinputs = obj.npredinputs * obj.ninputs;
|
||||
@@ -187,12 +187,12 @@ classdef MPC_Class
|
||||
H = NaN(obj.ndec, 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;
|
||||
end
|
||||
|
||||
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;
|
||||
end
|
||||
|
||||
@@ -222,7 +222,7 @@ classdef MPC_Class
|
||||
|
||||
% State Limits
|
||||
for i = 0:obj.npredstates-1
|
||||
start_idx = get_state_start_idx(i);
|
||||
start_idx = obj.get_state_start_idx(i);
|
||||
|
||||
% x position limits
|
||||
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
|
||||
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
|
||||
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
|
||||
for i = 0:obj.npredstates-1
|
||||
% 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);
|
||||
|
||||
% Get xy position from state vector
|
||||
@@ -337,7 +337,7 @@ classdef MPC_Class
|
||||
|
||||
if ~in_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
|
||||
continue;
|
||||
@@ -345,22 +345,22 @@ classdef MPC_Class
|
||||
|
||||
for j = 1:size(obj.Xobs_seen,2)
|
||||
% Check if position is in or on each obstacle
|
||||
xv_obstacle = obj.Xobs_seen{i}(:,1);
|
||||
yv_obstacle = obj.Xobs_seen{i}(:,2);
|
||||
xv_obstacle = obj.Xobs_seen{j}(:,1);
|
||||
yv_obstacle = obj.Xobs_seen{j}(:,2);
|
||||
[in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle);
|
||||
|
||||
if in_obstacle || 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
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
if isnan(c(i))
|
||||
if isnan(c(i+1))
|
||||
% 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
|
||||
@@ -368,79 +368,15 @@ classdef MPC_Class
|
||||
|
||||
%% Private Kinematic Models
|
||||
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)
|
||||
%linearized_bike_model Computes the discrete-time LTV
|
||||
% system matrices of the nonlinear bike model linearized
|
||||
% around the reference trajectory starting at the index
|
||||
% closest to `Y_curr`
|
||||
|
||||
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/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,:)) ...
|
||||
) ...
|
||||
);
|
||||
% Continuous-time system
|
||||
A_c = @(i) obj.A_c_func(i);
|
||||
B_c = @(i) obj.B_c_func(i);
|
||||
|
||||
% Discrete-time LTV system
|
||||
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));
|
||||
F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
|
||||
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
|
||||
|
||||
%% Private Helper Functions
|
||||
@@ -540,13 +526,13 @@ classdef MPC_Class
|
||||
Z_ref = zeros(obj.ndec, 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) ...
|
||||
= obj.Y_ref(obj.ref_idx+i, :);
|
||||
end
|
||||
|
||||
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) ...
|
||||
= obj.U_ref(obj.ref_idx+i, :);
|
||||
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