Skip to content

Commit

Permalink
First lateral mpc version
Browse files Browse the repository at this point in the history
  • Loading branch information
@fetty31
fetty31 committed Nov 27, 2022
1 parent cda59bb commit 8e997cc
Show file tree
Hide file tree
Showing 40 changed files with 2,106 additions and 4,120 deletions.
14 changes: 3 additions & 11 deletions cfg/dynamic.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,9 @@ from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("diff_delta", double_t, 0, "Differential delta bounds", 100.0, 0.0, 10000.0)
gen.add("diff_Fm", double_t, 0, "Differential Fm bounds", 1000.0, 0.0, 10000.0)
gen.add("diff_delta", double_t, 0, "Differential delta bounds [º]", 100.0, 0.0, 10000.0)
gen.add("dRd", double_t, 0, "Steering change weight param", 8.0, 0.0, 1000.0)
gen.add("dRa", double_t, 0, "Throttle change weight param", 2.0, 0.0, 1000.0)
gen.add("dMtv", double_t, 0, "Mtv weight param", 1.0, 0.0, 1000.0)
gen.add("Vmax", double_t, 0, "Maximum longitudinal velocity", 25.0, 0.0, 35.0)
gen.add("Vmin", double_t, 0, "Minimum longitudinal velocity", 2.0, 0.0, 10.0)
gen.add("Dr", double_t, 0, "Pacekja constant", 3152.3, 0.0, 10000.0)
gen.add("Df", double_t, 0, "Pacekja constant", 2785.4, 0.0, 10000.0)
gen.add("Br", double_t, 0, "Pacekja constant", 10.1507, 0.0, 10000.0)
Expand All @@ -21,19 +17,15 @@ gen.add("Cf", double_t, 0, "Pacekja constant",
gen.add("u_r", double_t, 0, "u_r", 0.45, 0.0, 1.0)
gen.add("Cd", double_t, 0, "Drag constant", 0.8727, 0.0, 2.0)
gen.add("q_slip", double_t, 0, "Slip difference weight", 2.0, 0.0, 1000.0)
gen.add("p_long", double_t, 0, "p_long", 0.5, 0.0, 1000.0)
gen.add("q_n", double_t, 0, "Normal distance weight", 5, 0.0, 1000.0)
gen.add("q_nN", double_t, 0, "Normal distance weight for N stage", 5, 0.0, 1000.0)
gen.add("q_mu", double_t, 0, "Heading (track reference) weight", 0.0, 0.0, 1000.0)
gen.add("lambda", double_t, 0, "Ellipse of forces param", 3.0, 0.0, 1000.0)
gen.add("q_s", double_t, 0, "Progress rate weight", 30.0, 0.0, 1000.0)
gen.add("q_sN", double_t, 0, "Progress rate weight for N stage", 10.0, 0.0, 1000.0)
gen.add("q_slack_vx", double_t, 0, "Vx slack weight", 0.0, 0.0, 1000.0)
gen.add("q_slack_track", double_t, 0, "Track slack weight", 5.0, 0.0, 1000.0)
gen.add("latency", int_t, 0, "Latency of MPC", 5, 0, 1000)
gen.add("Cm", double_t, 0, "Maximum total Fx", 2000.0, 500.0, 7000.0)
gen.add("Ax_max", double_t, 0, "Maximum longitudinal acc", 7.0, 0.0, 15.0)
gen.add("Ay_max", double_t, 0, "Maximum lateral acc", 10.0, 0.0, 15.0)
gen.add("Vy_max", double_t, 0, "Maximum Vy [m/s]", 3.0, 0.0, 1000.0)
gen.add("YawRate_max", double_t, 0, "Maximum Yaw Rate [º/s]", 50.0, 0.0, 1000.0)


exit(gen.generate(PACKAGE, PACKAGE, "dynamic"))
26 changes: 9 additions & 17 deletions include/mpc.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,14 +34,14 @@ struct Boundaries{
// VARIABLES BOUNDARIES:

// Bounds and initial guess for the control
vector<double> u_min = { 0.0, 0.0, -3*M_PI/180, -5.0, -300}; // delta, Fm max,min bounds will be overwriten by dynamic reconfigure callback
vector<double> u_max = { 3*M_PI/180, 0.25, 300};
vector<double> u_min = { 0.0, -3*M_PI/180, -300}; // delta, Fm max,min bounds will be overwriten by dynamic reconfigure callback
vector<double> u_max = { 3*M_PI/180, 300};
vector<double> u0 = { 0.0, 0.0 };

// Bounds and initial guess for the state
vector<double> x_min = { -23.0*M_PI/180, -1, -2.5, -50.0*M_PI/180, 2.0, -2.0, -50.0*M_PI/180 };
vector<double> x_max = { 23.0*M_PI/180, 1, 2.5, 50.0*M_PI/180, 25.0, 2.0, 50.0*M_PI/180 };
vector<double> x0 = { 0.0, -1.25, 0.0, 0.0, 15.0, 0.0, 0.0 };
vector<double> x_min = { -23.0*M_PI/180, -2.0, -50.0*M_PI/180, -5.0, -50.0*M_PI/180 };
vector<double> x_max = { 23.0*M_PI/180, 2.0, 50.0*M_PI/180, 5.0, 50.0*M_PI/180 };
vector<double> x0 = { 0.0, 0.0, 0.0, 0.0, 0.0 };

};

Expand Down Expand Up @@ -74,7 +74,6 @@ class MPC{
// DYNAMIC PARAMETERS:
// see "dynamic.cfg" for explanation
double dRd = 8;
double dRa = 2;
double Dr = 3152.3;
double Df = 2785.4;
double Cr = 1.6;
Expand All @@ -84,20 +83,15 @@ class MPC{
double u_r = 0.45;
double Cd = 0.8727;
double q_slip = 2;
double p_long = 0.5;
double q_n = 5;
double q_nN = 5;
double q_mu = 0.1;
double lambda = 1;
double q_s = 30;
// int latency = 4;
double Cm = 4000;
// int latency = 4; made public for debugging
double dMtv = 1;
double ax_max = 7;
double ay_max = 10;
double q_sN = 10;

double q_slack_vx = 0;
double q_slack_track = 0;

// STATIC PARAMETERS:
Expand Down Expand Up @@ -132,8 +126,6 @@ class MPC{
void printVec(vector<double> &input, int firstElements=0);
Eigen::MatrixXd vector2eigen(vector<double> vect);
Eigen::MatrixXd output2eigen(double* array, int size);
double throttle_to_torque(double throttle);
double ax_to_throttle(double ax);
double continuous(double psi, double psi_last); // Garanty that both angles are on the same range
const string currentDateTime(); // get current date/time, format is YYYY-MM-DD.HH:mm:ss

Expand Down Expand Up @@ -175,11 +167,11 @@ class MPC{

// Previous state
Eigen::MatrixXd lastState; // [x, y, theta, vx, vy, w]
Eigen::MatrixXd lastCommands; // [diff_delta, diff_Fm, Mtv, delta, Fm]
Eigen::MatrixXd lastCommands; // [diff_delta, Mtv, delta]

// Previous solution
Eigen::MatrixXd solStates; // [n, mu, vx, vy, w]
Eigen::MatrixXd solCommands; // [slack_vx, slack_track, diff_delta, diff_acc, Mtv, delta, Fm]
Eigen::MatrixXd solStates; // [n, mu, vy, w]
Eigen::MatrixXd solCommands; // [slack_track, diff_delta, Mtv, delta]

};

Expand Down
54 changes: 20 additions & 34 deletions params/dyn_autocross.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,71 +1,57 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
Ax_max: 7.0
Ay_max: 10.0
Bf: 10.8529
Br: 10.1507
Cd: 0.8727
Cf: -1.1705
Cm: 2000.0
Cr: -1.1705
Df: 2785.4
Dr: 3152.3
Vmax: 15.0
Vmin: 2.0
Vy_max: 5.0
YawRate_max: 50.0
dMtv: 1.0
dRa: 1.0
dRd: 8.0
diff_Fm: 1000.0
dRd: 0.2
diff_delta: 100.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
Ax_max: 7.0
Ay_max: 10.0
Bf: 10.8529
Br: 10.1507
Cd: 0.8727
Cf: -1.1705
Cm: 2000.0
Cr: -1.1705
Df: 2785.4
Dr: 3152.3
Vmax: 15.0
Vmin: 2.0
Vy_max: 5.0
YawRate_max: 50.0
dMtv: 1.0
dRa: 1.0
dRd: 8.0
diff_Fm: 1000.0
dRd: 0.2
diff_delta: 100.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
lambda: 3.0
latency: 5.0
latency: 5
name: Default
p_long: 0.5
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
q_mu: 0.0
q_n: 5.0
q_nN: 5.0
q_s: 20.0
q_sN: 20.0
q_slack_vx: 1.0
q_mu: 0.1
q_n: 15.0
q_nN: 15.0
q_s: 30.0
q_sN: 30.0
q_slack_track: 100.0
q_slip: 2.0
state: true
type: ''
u_r: 0.45
state: []
lambda: 3.0
latency: 5.0
p_long: 0.5
q_mu: 0.0
q_n: 5.0
q_nN: 5.0
q_s: 20.0
q_sN: 20.0
q_slack_vx: 1.0
latency: 5
q_mu: 0.1
q_n: 15.0
q_nN: 15.0
q_s: 30.0
q_sN: 30.0
q_slack_track: 100.0
q_slip: 2.0
u_r: 0.45
state: []
60 changes: 22 additions & 38 deletions params/dyn_trackdrive.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,73 +1,57 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
Ax_max: 7.0
Ay_max: 10.0
Bf: 10.8529
Br: 10.1507
Cd: 0.8727
Cf: -1.1705
Cm: 2000.0
Cr: -1.1705
Df: 2785.4
Dr: 3152.3
Vmax: 10.0
Vmin: 2.0
Vy_max: 5.0
YawRate_max: 50.0
dMtv: 1.0
dRa: 1.0
dRd: 3.0
diff_Fm: 10.0
diff_delta: 0.2
dRd: 0.2
diff_delta: 100.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
Ax_max: 7.0
Ay_max: 10.0
Bf: 10.8529
Br: 10.1507
Cd: 0.8727
Cf: -1.1705
Cm: 2000.0
Cr: -1.1705
Df: 2785.4
Dr: 3152.3
Vmax: 10.0
Vmin: 2.0
Vy_max: 5.0
YawRate_max: 50.0
dMtv: 1.0
dRa: 1.0
dRd: 3.0
diff_Fm: 10.0
diff_delta: 0.2
dRd: 0.2
diff_delta: 100.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
lambda: 5.0
latency: 8
latency: 5
name: Default
p_long: 1.5
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
q_mu: 0.1
q_n: 7.0
q_nN: 7.0
q_s: 15.0
q_sN: 10.0
q_slack_track: 10.0
q_slack_vx: 100.0
q_slip: 1.0
q_n: 10.0
q_nN: 15.0
q_s: 30.0
q_sN: 30.0
q_slack_track: 100.0
q_slip: 2.0
state: true
type: ''
u_r: 0.45
state: []
lambda: 5.0
latency: 8
p_long: 1.5
latency: 5
q_mu: 0.1
q_n: 7.0
q_nN: 7.0
q_s: 15.0
q_sN: 10.0
q_slack_track: 10.0
q_slack_vx: 100.0
q_slip: 1.0
q_n: 10.0
q_nN: 15.0
q_s: 30.0
q_sN: 30.0
q_slack_track: 100.0
q_slip: 2.0
u_r: 0.45
state: []
8 changes: 4 additions & 4 deletions params/tailored_mpc.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@ Topics:
# TO DO: fill with other topics

NLOP:
N: 40 # Prediction horizon
Nslacks: 2 # Number of slacks variables
N: 20 # Prediction horizon
Nslacks: 1 # Number of slacks variables

Hz: 40 #[s] MPC frequency
rk4_t: 0.025 #[s] Runge Kutta integration time
Hz: 20 #[s] MPC frequency
rk4_t: 0.020 #[s] Runge Kutta integration time
Nthreads: 10 # Number of threads
TroProfile: false # Set to true to follow TRO velocity profile

Expand Down
2 changes: 1 addition & 1 deletion solver/FORCES_problem.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

%% Problem info
N = 20; % Horizon Length
n_states = 6;
n_states = 5;
n_controls = 3;

% Call function that generates the solver
Expand Down
Binary file added solver/LookUpTableDV.mat
View file
Binary file not shown.
16 changes: 8 additions & 8 deletions solver/codeGen/TailoredSolver.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,17 +2,17 @@
%
% OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
% subject to the parameters supplied in the following struct:
% PARAMS.lb - column vector of length 480
% PARAMS.ub - column vector of length 400
% PARAMS.hu - column vector of length 200
% PARAMS.xinit - column vector of length 10
% PARAMS.x0 - column vector of length 480
% PARAMS.all_parameters - column vector of length 1240
% PARAMS.lb - column vector of length 160
% PARAMS.ub - column vector of length 140
% PARAMS.hu - column vector of length 40
% PARAMS.xinit - column vector of length 7
% PARAMS.x0 - column vector of length 160
% PARAMS.all_parameters - column vector of length 500
% PARAMS.num_of_threads - scalar
%
% OUTPUT returns the values of the last iteration of the solver where
% OUTPUT.U - column vector of size 280
% OUTPUT.X - column vector of size 200
% OUTPUT.U - column vector of size 80
% OUTPUT.X - column vector of size 80
%
% [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
% the integer EXITFLAG indicating the state of the solution with
Expand Down
Binary file modified solver/codeGen/TailoredSolver.mexa64
View file
Binary file not shown.
16 changes: 8 additions & 8 deletions solver/codeGen/TailoredSolver/TailoredSolver.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,17 +2,17 @@
%
% OUTPUT = TailoredSolver(PARAMS) solves a multistage problem
% subject to the parameters supplied in the following struct:
% PARAMS.lb - column vector of length 480
% PARAMS.ub - column vector of length 400
% PARAMS.hu - column vector of length 200
% PARAMS.xinit - column vector of length 10
% PARAMS.x0 - column vector of length 480
% PARAMS.all_parameters - column vector of length 1240
% PARAMS.lb - column vector of length 160
% PARAMS.ub - column vector of length 140
% PARAMS.hu - column vector of length 40
% PARAMS.xinit - column vector of length 7
% PARAMS.x0 - column vector of length 160
% PARAMS.all_parameters - column vector of length 500
% PARAMS.num_of_threads - scalar
%
% OUTPUT returns the values of the last iteration of the solver where
% OUTPUT.U - column vector of size 280
% OUTPUT.X - column vector of size 200
% OUTPUT.U - column vector of size 80
% OUTPUT.X - column vector of size 80
%
% [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
% the integer EXITFLAG indicating the state of the solution with
Expand Down
Loading

0 comments on commit 8e997cc

Please sign in to comment.