Skip to content

Commit

Permalink
new horizon length
Browse files Browse the repository at this point in the history
  • Loading branch information
@fetty31
fetty31 committed Dec 7, 2022
1 parent 7ee86e6 commit 58c9959
Show file tree
Hide file tree
Showing 34 changed files with 258 additions and 243 deletions.
1 change: 0 additions & 1 deletion include/utils/params.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ struct Params{
double rk4_t; // runge kutta integration time [s]
int Nthreads; // number of threads
int nPlanning; // number of points we want from the planner
bool TroProfile; // set to true to follow TRO velocity profile
struct Topics{
string commands; // Car Commands topic
string state; // Car State topic
Expand Down
26 changes: 13 additions & 13 deletions params/dyn_autocross.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,10 @@ dictitems:
Df: 2785.4
Dr: 3152.3
Vy_max: 5.0
YawRate_max: 50.0
YawRate_max: 80.0
dMtv: 1.0
dRd: 0.2
diff_delta: 100.0
diff_delta: 1000.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
Bf: 10.8529
Expand All @@ -22,36 +22,36 @@ dictitems:
Df: 2785.4
Dr: 3152.3
Vy_max: 5.0
YawRate_max: 50.0
YawRate_max: 80.0
dMtv: 1.0
dRd: 0.2
diff_delta: 100.0
diff_delta: 1000.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
latency: 5
latency: 10
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
q_mu: 0.1
q_mu: 0.0
q_n: 15.0
q_nN: 15.0
q_s: 30.0
q_sN: 30.0
q_s: 50.0
q_sN: 50.0
q_slack_track: 100.0
q_slip: 2.0
state: true
type: ''
u_r: 0.45
state: []
latency: 5
q_mu: 0.1
latency: 10
q_mu: 0.0
q_n: 15.0
q_nN: 15.0
q_s: 30.0
q_sN: 30.0
q_s: 50.0
q_sN: 50.0
q_slack_track: 100.0
q_slip: 2.0
u_r: 0.45
state: []
state: []
28 changes: 14 additions & 14 deletions params/dyn_trackdrive.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@ dictitems:
Df: 2785.4
Dr: 3152.3
Vy_max: 5.0
YawRate_max: 40.0
YawRate_max: 80.0
dMtv: 1.0
dRd: 0.1
dRd: 0.2
diff_delta: 1000.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
Expand All @@ -22,35 +22,35 @@ dictitems:
Df: 2785.4
Dr: 3152.3
Vy_max: 5.0
YawRate_max: 40.0
YawRate_max: 80.0
dMtv: 1.0
dRd: 0.1
dRd: 0.2
diff_delta: 1000.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
latency: 15
latency: 10
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
q_mu: 0.1
q_n: 10.0
q_mu: 0.0
q_n: 15.0
q_nN: 15.0
q_s: 70.0
q_sN: 70.0
q_s: 50.0
q_sN: 50.0
q_slack_track: 100.0
q_slip: 2.0
state: true
type: ''
u_r: 0.45
state: []
latency: 15
q_mu: 0.1
q_n: 10.0
latency: 10
q_mu: 0.0
q_n: 15.0
q_nN: 15.0
q_s: 70.0
q_sN: 70.0
q_s: 50.0
q_sN: 50.0
q_slack_track: 100.0
q_slip: 2.0
u_r: 0.45
Expand Down
3 changes: 1 addition & 2 deletions params/tailored_mpc.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,12 @@ Topics:
# TO DO: fill with other topics

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

MPC:
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

nPlanning: 1900 # n points saved from the planner
2 changes: 1 addition & 1 deletion solver/FORCES_problem.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
addpath(solverDir);

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

Expand Down
14 changes: 7 additions & 7 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 160
% PARAMS.ub - column vector of length 140
% PARAMS.hu - column vector of length 40
% PARAMS.lb - column vector of length 320
% PARAMS.ub - column vector of length 280
% PARAMS.hu - column vector of length 80
% PARAMS.xinit - column vector of length 7
% PARAMS.x0 - column vector of length 160
% PARAMS.all_parameters - column vector of length 500
% PARAMS.x0 - column vector of length 320
% PARAMS.all_parameters - column vector of length 1000
% PARAMS.num_of_threads - scalar
%
% OUTPUT returns the values of the last iteration of the solver where
% OUTPUT.U - column vector of size 80
% OUTPUT.X - column vector of size 80
% OUTPUT.U - column vector of size 160
% OUTPUT.X - column vector of size 160
%
% [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.
14 changes: 7 additions & 7 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 160
% PARAMS.ub - column vector of length 140
% PARAMS.hu - column vector of length 40
% PARAMS.lb - column vector of length 320
% PARAMS.ub - column vector of length 280
% PARAMS.hu - column vector of length 80
% PARAMS.xinit - column vector of length 7
% PARAMS.x0 - column vector of length 160
% PARAMS.all_parameters - column vector of length 500
% PARAMS.x0 - column vector of length 320
% PARAMS.all_parameters - column vector of length 1000
% PARAMS.num_of_threads - scalar
%
% OUTPUT returns the values of the last iteration of the solver where
% OUTPUT.U - column vector of size 80
% OUTPUT.X - column vector of size 80
% OUTPUT.U - column vector of size 160
% OUTPUT.X - column vector of size 160
%
% [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
% the integer EXITFLAG indicating the state of the solution with
Expand Down
34 changes: 17 additions & 17 deletions solver/codeGen/TailoredSolver/include/TailoredSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ jurisdiction in case of any dispute.
*/

/* Generated by FORCESPRO v6.0.0 on Sunday, November 27, 2022 at 12:38:01 PM */
/* Generated by FORCESPRO v6.0.0 on Monday, December 5, 2022 at 8:27:11 PM */
#ifndef TailoredSolver_H
#define TailoredSolver_H

Expand Down Expand Up @@ -184,23 +184,23 @@ extern size_t TailoredSolver_get_const_size( void );
/* fill this with data before calling the solver! */
typedef struct
{
/* vector of size 160 */
TailoredSolver_float lb[160];
/* vector of size 320 */
TailoredSolver_float lb[320];

/* vector of size 140 */
TailoredSolver_float ub[140];
/* vector of size 280 */
TailoredSolver_float ub[280];

/* vector of size 40 */
TailoredSolver_float hu[40];
/* vector of size 80 */
TailoredSolver_float hu[80];

/* vector of size 7 */
TailoredSolver_float xinit[7];

/* vector of size 160 */
TailoredSolver_float x0[160];
/* vector of size 320 */
TailoredSolver_float x0[320];

/* vector of size 500 */
TailoredSolver_float all_parameters[500];
/* vector of size 1000 */
TailoredSolver_float all_parameters[1000];

/* scalar */
solver_int32_unsigned num_of_threads;
Expand All @@ -213,11 +213,11 @@ typedef struct
/* the desired variables are put here by the solver */
typedef struct
{
/* vector of size 80 */
TailoredSolver_float U[80];
/* vector of size 160 */
TailoredSolver_float U[160];

/* vector of size 80 */
TailoredSolver_float X[80];
/* vector of size 160 */
TailoredSolver_float X[160];


} TailoredSolver_output;
Expand Down Expand Up @@ -299,10 +299,10 @@ typedef struct


/* SOLVER FUNCTION DEFINITION -------------------------------------------*/
/* Time of Solver Generation: (UTC) Sunday, November 27, 2022 12:38:04 PM */
/* Time of Solver Generation: (UTC) Monday, December 5, 2022 8:27:16 PM */
/* User License expires on: (UTC) Monday, September 25, 2023 10:00:00 PM (approx.) (at the time of code generation) */
/* Solver Static License expires on: (UTC) Monday, September 25, 2023 10:00:00 PM (approx.) */
/* Solver Id: 5a4d13a6-bec1-4a6a-b016-8b174252534a */
/* Solver Id: 484892cb-eeff-4361-94ed-f59372485c0b */
/* examine exitflag before using the result! */
#ifdef __cplusplus
extern "C" {
Expand Down
2 changes: 1 addition & 1 deletion solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ jurisdiction in case of any dispute.
*/

/* Generated by FORCESPRO v6.0.0 on Sunday, November 27, 2022 at 12:38:01 PM */
/* Generated by FORCESPRO v6.0.0 on Monday, December 5, 2022 at 8:27:11 PM */
#ifndef TailoredSolver_MEMORY_H
#define TailoredSolver_MEMORY_H

Expand Down
14 changes: 7 additions & 7 deletions solver/codeGen/TailoredSolver/interface/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 160
% PARAMS.ub - column vector of length 140
% PARAMS.hu - column vector of length 40
% PARAMS.lb - column vector of length 320
% PARAMS.ub - column vector of length 280
% PARAMS.hu - column vector of length 80
% PARAMS.xinit - column vector of length 7
% PARAMS.x0 - column vector of length 160
% PARAMS.all_parameters - column vector of length 500
% PARAMS.x0 - column vector of length 320
% PARAMS.all_parameters - column vector of length 1000
% PARAMS.num_of_threads - scalar
%
% OUTPUT returns the values of the last iteration of the solver where
% OUTPUT.U - column vector of size 80
% OUTPUT.X - column vector of size 80
% OUTPUT.U - column vector of size 160
% OUTPUT.X - column vector of size 160
%
% [OUTPUT, EXITFLAG] = TailoredSolver(PARAMS) returns additionally
% the integer EXITFLAG indicating the state of the solution with
Expand Down
18 changes: 9 additions & 9 deletions solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ jurisdiction in case of any dispute.

typedef TailoredSolver_float solver_float;
typedef solver_int32_default solver_int;
#define NSTAGES ( 20 )
#define NSTAGES ( 40 )
#define MAX(X, Y) ((X) < (Y) ? (Y) : (X))

/* For compatibility with Microsoft Visual Studio 2015 */
Expand Down Expand Up @@ -84,14 +84,14 @@ TailoredSolver_extfunc pt2function_TailoredSolver = &TailoredSolver_adtool2force
static void getDims(const solver_int stage, solver_int* nvar, solver_int* neq, solver_int* dimh,
solver_int* dimp, solver_int* diml, solver_int* dimu, solver_int* dimhl, solver_int* dimhu)
{
const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};

*nvar = nvarArr[stage];
*neq = neqArr[stage];
Expand Down
18 changes: 9 additions & 9 deletions solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ jurisdiction in case of any dispute.

typedef TailoredSolver_float solver_float;
typedef solver_int32_default solver_int;
#define NSTAGES ( 20 )
#define NSTAGES ( 40 )
#define MAX(X, Y) ((X) < (Y) ? (Y) : (X))

/* For compatibility with Microsoft Visual Studio 2015 */
Expand Down Expand Up @@ -84,14 +84,14 @@ TailoredSolver_extfunc pt2function_TailoredSolver = &TailoredSolver_adtool2force
static void getDims(const solver_int stage, solver_int* nvar, solver_int* neq, solver_int* dimh,
solver_int* dimp, solver_int* diml, solver_int* dimu, solver_int* dimhl, solver_int* dimhu)
{
const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
const solver_int nvarArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int neqArr[NSTAGES] = {7, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5};
const solver_int dimhArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};
const solver_int dimpArr[NSTAGES] = {25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25};
const solver_int dimlArr[NSTAGES] = {8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8};
const solver_int dimuArr[NSTAGES] = {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
const solver_int dimhlArr[NSTAGES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
const solver_int dimhuArr[NSTAGES] = {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};

*nvar = nvarArr[stage];
*neq = neqArr[stage];
Expand Down
Loading

0 comments on commit 58c9959

Please sign in to comment.