diff --git a/cfg/dynamic.cfg b/cfg/dynamic.cfg index ad1c74e..28e839b 100755 --- a/cfg/dynamic.cfg +++ b/cfg/dynamic.cfg @@ -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) @@ -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")) \ No newline at end of file diff --git a/include/mpc.hh b/include/mpc.hh index c4a7c6b..176f60f 100644 --- a/include/mpc.hh +++ b/include/mpc.hh @@ -34,14 +34,14 @@ struct Boundaries{ // VARIABLES BOUNDARIES: // Bounds and initial guess for the control - vector 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 u_max = { 3*M_PI/180, 0.25, 300}; + vector u_min = { 0.0, -3*M_PI/180, -300}; // delta, Fm max,min bounds will be overwriten by dynamic reconfigure callback + vector u_max = { 3*M_PI/180, 300}; vector u0 = { 0.0, 0.0 }; // Bounds and initial guess for the state - vector 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 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 x0 = { 0.0, -1.25, 0.0, 0.0, 15.0, 0.0, 0.0 }; + vector x_min = { -23.0*M_PI/180, -2.0, -50.0*M_PI/180, -5.0, -50.0*M_PI/180 }; + vector x_max = { 23.0*M_PI/180, 2.0, 50.0*M_PI/180, 5.0, 50.0*M_PI/180 }; + vector x0 = { 0.0, 0.0, 0.0, 0.0, 0.0 }; }; @@ -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; @@ -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: @@ -132,8 +126,6 @@ class MPC{ void printVec(vector &input, int firstElements=0); Eigen::MatrixXd vector2eigen(vector 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 @@ -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] }; diff --git a/params/dyn_autocross.yaml b/params/dyn_autocross.yaml index 0dd9a3a..bddf0f4 100644 --- a/params/dyn_autocross.yaml +++ b/params/dyn_autocross.yaml @@ -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: [] diff --git a/params/dyn_trackdrive.yaml b/params/dyn_trackdrive.yaml index 29b0d99..489cbff 100644 --- a/params/dyn_trackdrive.yaml +++ b/params/dyn_trackdrive.yaml @@ -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: [] diff --git a/params/tailored_mpc.yaml b/params/tailored_mpc.yaml index 6bd5e64..40c9f85 100644 --- a/params/tailored_mpc.yaml +++ b/params/tailored_mpc.yaml @@ -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 diff --git a/solver/FORCES_problem.m b/solver/FORCES_problem.m index f15db66..811c9dc 100644 --- a/solver/FORCES_problem.m +++ b/solver/FORCES_problem.m @@ -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 diff --git a/solver/LookUpTableDV.mat b/solver/LookUpTableDV.mat new file mode 100644 index 0000000..f0bedcd Binary files /dev/null and b/solver/LookUpTableDV.mat differ diff --git a/solver/codeGen/TailoredSolver.m b/solver/codeGen/TailoredSolver.m index 53249ed..4e751e1 100644 --- a/solver/codeGen/TailoredSolver.m +++ b/solver/codeGen/TailoredSolver.m @@ -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 diff --git a/solver/codeGen/TailoredSolver.mexa64 b/solver/codeGen/TailoredSolver.mexa64 index a346264..e68b5d0 100755 Binary files a/solver/codeGen/TailoredSolver.mexa64 and b/solver/codeGen/TailoredSolver.mexa64 differ diff --git a/solver/codeGen/TailoredSolver/TailoredSolver.m b/solver/codeGen/TailoredSolver/TailoredSolver.m index 53249ed..4e751e1 100644 --- a/solver/codeGen/TailoredSolver/TailoredSolver.m +++ b/solver/codeGen/TailoredSolver/TailoredSolver.m @@ -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 diff --git a/solver/codeGen/TailoredSolver/include/TailoredSolver.h b/solver/codeGen/TailoredSolver/include/TailoredSolver.h index 8d96345..d1d74fe 100644 --- a/solver/codeGen/TailoredSolver/include/TailoredSolver.h +++ b/solver/codeGen/TailoredSolver/include/TailoredSolver.h @@ -19,7 +19,7 @@ jurisdiction in case of any dispute. */ -/* Generated by FORCESPRO v6.0.0 on Thursday, November 24, 2022 at 4:08:46 PM */ +/* Generated by FORCESPRO v6.0.0 on Sunday, November 27, 2022 at 12:38:01 PM */ #ifndef TailoredSolver_H #define TailoredSolver_H @@ -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 480 */ - TailoredSolver_float lb[480]; + /* vector of size 160 */ + TailoredSolver_float lb[160]; - /* vector of size 400 */ - TailoredSolver_float ub[400]; + /* vector of size 140 */ + TailoredSolver_float ub[140]; - /* vector of size 200 */ - TailoredSolver_float hu[200]; + /* vector of size 40 */ + TailoredSolver_float hu[40]; - /* vector of size 10 */ - TailoredSolver_float xinit[10]; + /* vector of size 7 */ + TailoredSolver_float xinit[7]; - /* vector of size 480 */ - TailoredSolver_float x0[480]; + /* vector of size 160 */ + TailoredSolver_float x0[160]; - /* vector of size 1240 */ - TailoredSolver_float all_parameters[1240]; + /* vector of size 500 */ + TailoredSolver_float all_parameters[500]; /* scalar */ solver_int32_unsigned num_of_threads; @@ -213,11 +213,11 @@ typedef struct /* the desired variables are put here by the solver */ typedef struct { - /* vector of size 280 */ - TailoredSolver_float U[280]; + /* vector of size 80 */ + TailoredSolver_float U[80]; - /* vector of size 200 */ - TailoredSolver_float X[200]; + /* vector of size 80 */ + TailoredSolver_float X[80]; } TailoredSolver_output; @@ -299,10 +299,10 @@ typedef struct /* SOLVER FUNCTION DEFINITION -------------------------------------------*/ -/* Time of Solver Generation: (UTC) Thursday, November 24, 2022 4:08:52 PM */ +/* Time of Solver Generation: (UTC) Sunday, November 27, 2022 12:38:04 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: fc8c44ee-d2e9-40f3-b0df-030c2036e216 */ +/* Solver Id: 5a4d13a6-bec1-4a6a-b016-8b174252534a */ /* examine exitflag before using the result! */ #ifdef __cplusplus extern "C" { diff --git a/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h b/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h index 0e2ff89..008d397 100644 --- a/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h +++ b/solver/codeGen/TailoredSolver/include/TailoredSolver_memory.h @@ -19,7 +19,7 @@ jurisdiction in case of any dispute. */ -/* Generated by FORCESPRO v6.0.0 on Thursday, November 24, 2022 at 4:08:46 PM */ +/* Generated by FORCESPRO v6.0.0 on Sunday, November 27, 2022 at 12:38:01 PM */ #ifndef TailoredSolver_MEMORY_H #define TailoredSolver_MEMORY_H diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver.m b/solver/codeGen/TailoredSolver/interface/TailoredSolver.m index 53249ed..4e751e1 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver.m +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver.m @@ -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 diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c index bb68791..f17354d 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_dynamics_mex.c @@ -30,7 +30,7 @@ jurisdiction in case of any dispute. typedef TailoredSolver_float solver_float; typedef solver_int32_default solver_int; -#define NSTAGES ( 40 ) +#define NSTAGES ( 20 ) #define MAX(X, Y) ((X) < (Y) ? (Y) : (X)) /* For compatibility with Microsoft Visual Studio 2015 */ @@ -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] = {12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}; - const solver_int neqArr[NSTAGES] = {10, 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 dimhArr[NSTAGES] = {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, 5}; - const solver_int dimpArr[NSTAGES] = {31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31}; - const solver_int dimlArr[NSTAGES] = {12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}; - const solver_int dimuArr[NSTAGES] = {10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10}; - 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] = {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, 5}; + 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}; *nvar = nvarArr[stage]; *neq = neqArr[stage]; diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c index 2a79f6f..9ee99ff 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_inequalities_mex.c @@ -30,7 +30,7 @@ jurisdiction in case of any dispute. typedef TailoredSolver_float solver_float; typedef solver_int32_default solver_int; -#define NSTAGES ( 40 ) +#define NSTAGES ( 20 ) #define MAX(X, Y) ((X) < (Y) ? (Y) : (X)) /* For compatibility with Microsoft Visual Studio 2015 */ @@ -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] = {12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}; - const solver_int neqArr[NSTAGES] = {10, 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 dimhArr[NSTAGES] = {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, 5}; - const solver_int dimpArr[NSTAGES] = {31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31}; - const solver_int dimlArr[NSTAGES] = {12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}; - const solver_int dimuArr[NSTAGES] = {10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10}; - 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] = {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, 5}; + 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}; *nvar = nvarArr[stage]; *neq = neqArr[stage]; diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl b/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl index d423647..8a82604 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_lib.mdl @@ -60,13 +60,13 @@ Model { } } } - Created "11/24/2022 4:08:53 PM" + Created "11/27/2022 12:38:04 PM" Creator "embotech AG" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" LastModifiedBy "embotech AG" ModifiedDateFormat "%" - LastModifiedDate "11/24/2022 4:08:53 PM" + LastModifiedDate "11/27/2022 12:38:04 PM" RTWModifiedTimeStamp 315310195 ModelVersionFormat "1.%" ConfigurationManager "None" @@ -801,17 +801,17 @@ Model { Help "TailoredSolver_simulinkBlock provides an easy Simulink interface for simulating your customized solver. \n" "\nOUTPUTS = TailoredSolver(INPUTS) solves an optimization problem where:\n\n" "INPUTS:\n" - " - lb - column vector of length 480\n " -" - ub - column vector of length 400\n " -" - hu - column vector of length 200\n " -" - xinit - column vector of length 10\n " -" - x0 - column vector of length 480\n " -" - all_parameters - column vector of length 1240\n " + " - lb - column vector of length 160\n " +" - ub - column vector of length 140\n " +" - hu - column vector of length 40\n " +" - xinit - column vector of length 7\n " +" - x0 - column vector of length 160\n " +" - all_parameters - column vector of length 500\n " " - num_of_threads - column vector of length 1\n " "\nOUTPUTS:\n" - " - U - column vector of length 280\n " -" - X - column vector of length 200\n " + " - U - column vector of length 80\n " +" - X - column vector of length 80\n " "\n For more information, see https://forces.embotech.com/Documentation/graphical_interface/index.html \n " diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c index 5077a3c..29fbe7a 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_mex.c @@ -168,14 +168,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau { mexErrMsgTxt("PARAMS.lb must be a double."); } - if( mxGetM(par) != 480 || mxGetN(par) != 1 ) + if( mxGetM(par) != 160 || mxGetN(par) != 1 ) { - mexErrMsgTxt("PARAMS.lb must be of size [480 x 1]"); + mexErrMsgTxt("PARAMS.lb must be of size [160 x 1]"); } #endif if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) ) { - copyMArrayToC_double(mxGetPr(par), params.lb,480); + copyMArrayToC_double(mxGetPr(par), params.lb,160); } par = mxGetField(PARAMS, 0, "ub"); @@ -188,14 +188,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau { mexErrMsgTxt("PARAMS.ub must be a double."); } - if( mxGetM(par) != 400 || mxGetN(par) != 1 ) + if( mxGetM(par) != 140 || mxGetN(par) != 1 ) { - mexErrMsgTxt("PARAMS.ub must be of size [400 x 1]"); + mexErrMsgTxt("PARAMS.ub must be of size [140 x 1]"); } #endif if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) ) { - copyMArrayToC_double(mxGetPr(par), params.ub,400); + copyMArrayToC_double(mxGetPr(par), params.ub,140); } par = mxGetField(PARAMS, 0, "hu"); @@ -208,14 +208,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau { mexErrMsgTxt("PARAMS.hu must be a double."); } - if( mxGetM(par) != 200 || mxGetN(par) != 1 ) + if( mxGetM(par) != 40 || mxGetN(par) != 1 ) { - mexErrMsgTxt("PARAMS.hu must be of size [200 x 1]"); + mexErrMsgTxt("PARAMS.hu must be of size [40 x 1]"); } #endif if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) ) { - copyMArrayToC_double(mxGetPr(par), params.hu,200); + copyMArrayToC_double(mxGetPr(par), params.hu,40); } par = mxGetField(PARAMS, 0, "xinit"); @@ -228,14 +228,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau { mexErrMsgTxt("PARAMS.xinit must be a double."); } - if( mxGetM(par) != 10 || mxGetN(par) != 1 ) + if( mxGetM(par) != 7 || mxGetN(par) != 1 ) { - mexErrMsgTxt("PARAMS.xinit must be of size [10 x 1]"); + mexErrMsgTxt("PARAMS.xinit must be of size [7 x 1]"); } #endif if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) ) { - copyMArrayToC_double(mxGetPr(par), params.xinit,10); + copyMArrayToC_double(mxGetPr(par), params.xinit,7); } par = mxGetField(PARAMS, 0, "x0"); @@ -248,14 +248,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau { mexErrMsgTxt("PARAMS.x0 must be a double."); } - if( mxGetM(par) != 480 || mxGetN(par) != 1 ) + if( mxGetM(par) != 160 || mxGetN(par) != 1 ) { - mexErrMsgTxt("PARAMS.x0 must be of size [480 x 1]"); + mexErrMsgTxt("PARAMS.x0 must be of size [160 x 1]"); } #endif if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) ) { - copyMArrayToC_double(mxGetPr(par), params.x0,480); + copyMArrayToC_double(mxGetPr(par), params.x0,160); } par = mxGetField(PARAMS, 0, "all_parameters"); @@ -268,14 +268,14 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau { mexErrMsgTxt("PARAMS.all_parameters must be a double."); } - if( mxGetM(par) != 1240 || mxGetN(par) != 1 ) + if( mxGetM(par) != 500 || mxGetN(par) != 1 ) { - mexErrMsgTxt("PARAMS.all_parameters must be of size [1240 x 1]"); + mexErrMsgTxt("PARAMS.all_parameters must be of size [500 x 1]"); } #endif if ( (mxGetN(par) != 0) && (mxGetM(par) != 0) ) { - copyMArrayToC_double(mxGetPr(par), params.all_parameters,1240); + copyMArrayToC_double(mxGetPr(par), params.all_parameters,500); } par = mxGetField(PARAMS, 0, "num_of_threads"); @@ -314,12 +314,12 @@ void mexFunction( solver_int32_default nlhs, mxArray *plhs[], solver_int32_defau /* copy output to matlab arrays */ plhs[0] = mxCreateStructMatrix(1, 1, 2, outputnames); - outvar = mxCreateDoubleMatrix(280, 1, mxREAL); - copyCArrayToM_double( output.U, mxGetPr(outvar), 280); + outvar = mxCreateDoubleMatrix(80, 1, mxREAL); + copyCArrayToM_double( output.U, mxGetPr(outvar), 80); mxSetField(plhs[0], 0, "U", outvar); - outvar = mxCreateDoubleMatrix(200, 1, mxREAL); - copyCArrayToM_double( output.X, mxGetPr(outvar), 200); + outvar = mxCreateDoubleMatrix(80, 1, mxREAL); + copyCArrayToM_double( output.X, mxGetPr(outvar), 80); mxSetField(plhs[0], 0, "X", outvar); diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c index 2905b6d..06aea3f 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_objective_mex.c @@ -30,7 +30,7 @@ jurisdiction in case of any dispute. typedef TailoredSolver_float solver_float; typedef solver_int32_default solver_int; -#define NSTAGES ( 40 ) +#define NSTAGES ( 20 ) #define MAX(X, Y) ((X) < (Y) ? (Y) : (X)) /* For compatibility with Microsoft Visual Studio 2015 */ @@ -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] = {12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}; - const solver_int neqArr[NSTAGES] = {10, 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 dimhArr[NSTAGES] = {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, 5}; - const solver_int dimpArr[NSTAGES] = {31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31}; - const solver_int dimlArr[NSTAGES] = {12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}; - const solver_int dimuArr[NSTAGES] = {10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10}; - 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] = {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, 5}; + 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}; *nvar = nvarArr[stage]; *neq = neqArr[stage]; diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py b/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py index e095232..0b657ec 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_py.py @@ -22,17 +22,17 @@ OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem subject to the parameters supplied in the following dictionary: - 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_py.TailoredSolver_solve(PARAMS) returns additionally the integer EXITFLAG indicating the state of the solution with @@ -88,12 +88,12 @@ class TailoredSolver_params_ctypes(ctypes.Structure): # @classmethod # def from_param(self): # return self - _fields_ = [('lb', ctypes.c_double * 480), -('ub', ctypes.c_double * 400), -('hu', ctypes.c_double * 200), -('xinit', ctypes.c_double * 10), -('x0', ctypes.c_double * 480), -('all_parameters', ctypes.c_double * 1240), + _fields_ = [('lb', ctypes.c_double * 160), +('ub', ctypes.c_double * 140), +('hu', ctypes.c_double * 40), +('xinit', ctypes.c_double * 7), +('x0', ctypes.c_double * 160), +('all_parameters', ctypes.c_double * 500), ('num_of_threads', ctypes.c_uint), ] @@ -126,8 +126,8 @@ class TailoredSolver_outputs_ctypes(ctypes.Structure): # @classmethod # def from_param(self): # return self - _fields_ = [('U', ctypes.c_double * 280), -('X', ctypes.c_double * 200), + _fields_ = [('U', ctypes.c_double * 80), +('X', ctypes.c_double * 80), ] TailoredSolver_outputs = {'U' : np.array([]), @@ -185,17 +185,17 @@ def TailoredSolver_solve(params_arg): OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem subject to the parameters supplied in the following dictionary: - 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_py.TailoredSolver_solve(PARAMS) returns additionally the integer EXITFLAG indicating the state of the solution with diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c index 2c3db48..47fdddd 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlock.c @@ -75,42 +75,42 @@ static void mdlInitializeSizes(SimStruct *S) if (!ssSetNumInputPorts(S, 7)) return; /* Input Port 0 */ - ssSetInputPortMatrixDimensions(S, 0, 480, 1); + ssSetInputPortMatrixDimensions(S, 0, 160, 1); ssSetInputPortDataType(S, 0, SS_DOUBLE); ssSetInputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 0, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 0, 1); /*direct input signal access*/ /* Input Port 1 */ - ssSetInputPortMatrixDimensions(S, 1, 400, 1); + ssSetInputPortMatrixDimensions(S, 1, 140, 1); ssSetInputPortDataType(S, 1, SS_DOUBLE); ssSetInputPortComplexSignal(S, 1, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 1, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 1, 1); /*direct input signal access*/ /* Input Port 2 */ - ssSetInputPortMatrixDimensions(S, 2, 200, 1); + ssSetInputPortMatrixDimensions(S, 2, 40, 1); ssSetInputPortDataType(S, 2, SS_DOUBLE); ssSetInputPortComplexSignal(S, 2, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 2, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 2, 1); /*direct input signal access*/ /* Input Port 3 */ - ssSetInputPortMatrixDimensions(S, 3, 10, 1); + ssSetInputPortMatrixDimensions(S, 3, 7, 1); ssSetInputPortDataType(S, 3, SS_DOUBLE); ssSetInputPortComplexSignal(S, 3, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 3, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 3, 1); /*direct input signal access*/ /* Input Port 4 */ - ssSetInputPortMatrixDimensions(S, 4, 480, 1); + ssSetInputPortMatrixDimensions(S, 4, 160, 1); ssSetInputPortDataType(S, 4, SS_DOUBLE); ssSetInputPortComplexSignal(S, 4, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 4, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 4, 1); /*direct input signal access*/ /* Input Port 5 */ - ssSetInputPortMatrixDimensions(S, 5, 1240, 1); + ssSetInputPortMatrixDimensions(S, 5, 500, 1); ssSetInputPortDataType(S, 5, SS_DOUBLE); ssSetInputPortComplexSignal(S, 5, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 5, 1); /* Feedthrough enabled */ @@ -129,12 +129,12 @@ static void mdlInitializeSizes(SimStruct *S) if (!ssSetNumOutputPorts(S, 2)) return; /* Output Port 0 */ - ssSetOutputPortMatrixDimensions(S, 0, 280, 1); + ssSetOutputPortMatrixDimensions(S, 0, 80, 1); ssSetOutputPortDataType(S, 0, SS_DOUBLE); ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */ /* Output Port 1 */ - ssSetOutputPortMatrixDimensions(S, 1, 200, 1); + ssSetOutputPortMatrixDimensions(S, 1, 80, 1); ssSetOutputPortDataType(S, 1, SS_DOUBLE); ssSetOutputPortComplexSignal(S, 1, COMPLEX_NO); /* no complex signals suppported */ @@ -252,32 +252,32 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* Copy inputs */ - for( i=0; i<480; i++) + for( i=0; i<160; i++) { params.lb[i] = (double) lb[i]; } - for( i=0; i<400; i++) + for( i=0; i<140; i++) { params.ub[i] = (double) ub[i]; } - for( i=0; i<200; i++) + for( i=0; i<40; i++) { params.hu[i] = (double) hu[i]; } - for( i=0; i<10; i++) + for( i=0; i<7; i++) { params.xinit[i] = (double) xinit[i]; } - for( i=0; i<480; i++) + for( i=0; i<160; i++) { params.x0[i] = (double) x0[i]; } - for( i=0; i<1240; i++) + for( i=0; i<500; i++) { params.all_parameters[i] = (double) all_parameters[i]; } @@ -319,12 +319,12 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* Copy outputs */ - for( i=0; i<280; i++) + for( i=0; i<80; i++) { U[i] = (real_T) output.U[i]; } - for( i=0; i<200; i++) + for( i=0; i<80; i++) { X[i] = (real_T) output.X[i]; } diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c index 0470d76..509e560 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolver_simulinkBlockcompact.c @@ -75,42 +75,42 @@ static void mdlInitializeSizes(SimStruct *S) if (!ssSetNumInputPorts(S, 7)) return; /* Input Port 0 */ - ssSetInputPortMatrixDimensions(S, 0, 480, 1); + ssSetInputPortMatrixDimensions(S, 0, 160, 1); ssSetInputPortDataType(S, 0, SS_DOUBLE); ssSetInputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 0, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 0, 1); /*direct input signal access*/ /* Input Port 1 */ - ssSetInputPortMatrixDimensions(S, 1, 400, 1); + ssSetInputPortMatrixDimensions(S, 1, 140, 1); ssSetInputPortDataType(S, 1, SS_DOUBLE); ssSetInputPortComplexSignal(S, 1, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 1, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 1, 1); /*direct input signal access*/ /* Input Port 2 */ - ssSetInputPortMatrixDimensions(S, 2, 200, 1); + ssSetInputPortMatrixDimensions(S, 2, 40, 1); ssSetInputPortDataType(S, 2, SS_DOUBLE); ssSetInputPortComplexSignal(S, 2, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 2, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 2, 1); /*direct input signal access*/ /* Input Port 3 */ - ssSetInputPortMatrixDimensions(S, 3, 10, 1); + ssSetInputPortMatrixDimensions(S, 3, 7, 1); ssSetInputPortDataType(S, 3, SS_DOUBLE); ssSetInputPortComplexSignal(S, 3, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 3, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 3, 1); /*direct input signal access*/ /* Input Port 4 */ - ssSetInputPortMatrixDimensions(S, 4, 480, 1); + ssSetInputPortMatrixDimensions(S, 4, 160, 1); ssSetInputPortDataType(S, 4, SS_DOUBLE); ssSetInputPortComplexSignal(S, 4, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 4, 1); /* Feedthrough enabled */ ssSetInputPortRequiredContiguous(S, 4, 1); /*direct input signal access*/ /* Input Port 5 */ - ssSetInputPortMatrixDimensions(S, 5, 1240, 1); + ssSetInputPortMatrixDimensions(S, 5, 500, 1); ssSetInputPortDataType(S, 5, SS_DOUBLE); ssSetInputPortComplexSignal(S, 5, COMPLEX_NO); /* no complex signals suppported */ ssSetInputPortDirectFeedThrough(S, 5, 1); /* Feedthrough enabled */ @@ -129,7 +129,7 @@ static void mdlInitializeSizes(SimStruct *S) if (!ssSetNumOutputPorts(S, 1)) return; /* Output Port 0 */ - ssSetOutputPortMatrixDimensions(S, 0, 480, 1); + ssSetOutputPortMatrixDimensions(S, 0, 160, 1); ssSetOutputPortDataType(S, 0, SS_DOUBLE); ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO); /* no complex signals suppported */ @@ -246,32 +246,32 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* Copy inputs */ - for( i=0; i<480; i++) + for( i=0; i<160; i++) { params.lb[i] = (double) lb[i]; } - for( i=0; i<400; i++) + for( i=0; i<140; i++) { params.ub[i] = (double) ub[i]; } - for( i=0; i<200; i++) + for( i=0; i<40; i++) { params.hu[i] = (double) hu[i]; } - for( i=0; i<10; i++) + for( i=0; i<7; i++) { params.xinit[i] = (double) xinit[i]; } - for( i=0; i<480; i++) + for( i=0; i<160; i++) { params.x0[i] = (double) x0[i]; } - for( i=0; i<1240; i++) + for( i=0; i<500; i++) { params.all_parameters[i] = (double) all_parameters[i]; } @@ -313,13 +313,13 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* Copy outputs */ - for( i=0; i<280; i++) + for( i=0; i<80; i++) { outputs[i] = (real_T) output.U[i]; } - k=280; - for( i=0; i<200; i++) + k=80; + for( i=0; i<80; i++) { outputs[k++] = (real_T) output.X[i]; } diff --git a/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl b/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl index a71e5c9..4a54132 100644 --- a/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl +++ b/solver/codeGen/TailoredSolver/interface/TailoredSolvercompact_lib.mdl @@ -60,13 +60,13 @@ Model { } } } - Created "11/24/2022 4:08:53 PM" + Created "11/27/2022 12:38:04 PM" Creator "embotech AG" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" LastModifiedBy "embotech AG" ModifiedDateFormat "%" - LastModifiedDate "11/24/2022 4:08:53 PM" + LastModifiedDate "11/27/2022 12:38:04 PM" RTWModifiedTimeStamp 315310195 ModelVersionFormat "1.%" ConfigurationManager "None" @@ -801,16 +801,16 @@ Model { Help "TailoredSolver_simulinkBlockcompact provides an easy Simulink interface for simulating your customized solver. \n" "\nOUTPUTS = TailoredSolver(INPUTS) solves an optimization problem where:\n\n" "INPUTS:\n" - " - lb - column vector of length 480\n " -" - ub - column vector of length 400\n " -" - hu - column vector of length 200\n " -" - xinit - column vector of length 10\n " -" - x0 - column vector of length 480\n " -" - all_parameters - column vector of length 1240\n " + " - lb - column vector of length 160\n " +" - ub - column vector of length 140\n " +" - hu - column vector of length 40\n " +" - xinit - column vector of length 7\n " +" - x0 - column vector of length 160\n " +" - all_parameters - column vector of length 500\n " " - num_of_threads - column vector of length 1\n " "\nOUTPUTS:\n" - " - outputs - column vector of length 480\n " + " - outputs - column vector of length 160\n " "\n For more information, see https://forces.embotech.com/Documentation/graphical_interface/index.html \n " diff --git a/solver/codeGen/TailoredSolver/interface/definitions.py b/solver/codeGen/TailoredSolver/interface/definitions.py index 4f90415..24fca8e 100644 --- a/solver/codeGen/TailoredSolver/interface/definitions.py +++ b/solver/codeGen/TailoredSolver/interface/definitions.py @@ -6,22 +6,22 @@ lib = "lib/libTailoredSolver.so" lib_static = "lib/libTailoredSolver.a" c_header = "include/TailoredSolver.h" -nstages = 40 +nstages = 20 # Parameter | Type | Scalar type | Ctypes type | Numpy type | Shape | Len params = \ -[("lb" , "dense" , "" , ctypes.c_double, numpy.float64, (480, 1), 480), - ("ub" , "dense" , "" , ctypes.c_double, numpy.float64, (400, 1), 400), - ("hu" , "dense" , "" , ctypes.c_double, numpy.float64, (200, 1), 200), - ("xinit" , "dense" , "" , ctypes.c_double, numpy.float64, ( 10, 1), 10), - ("x0" , "dense" , "" , ctypes.c_double, numpy.float64, (480, 1), 480), - ("all_parameters" , "dense" , "" , ctypes.c_double, numpy.float64, (1240, 1), 1240), +[("lb" , "dense" , "" , ctypes.c_double, numpy.float64, (160, 1), 160), + ("ub" , "dense" , "" , ctypes.c_double, numpy.float64, (140, 1), 140), + ("hu" , "dense" , "" , ctypes.c_double, numpy.float64, ( 40, 1), 40), + ("xinit" , "dense" , "" , ctypes.c_double, numpy.float64, ( 7, 1), 7), + ("x0" , "dense" , "" , ctypes.c_double, numpy.float64, (160, 1), 160), + ("all_parameters" , "dense" , "" , ctypes.c_double, numpy.float64, (500, 1), 500), ("num_of_threads" , "" , "solver_int32_unsigned", ctypes.c_uint , numpy.uint32 , ( 0, 1), 1)] # Output | Type | Scalar type | Ctypes type | Numpy type | Shape | Len outputs = \ -[("U" , "" , "" , ctypes.c_double, numpy.float64, ( 7,), 280), - ("X" , "" , "" , ctypes.c_double, numpy.float64, ( 5,), 200)] +[("U" , "" , "" , ctypes.c_double, numpy.float64, ( 4,), 80), + ("X" , "" , "" , ctypes.c_double, numpy.float64, ( 4,), 80)] # Info Struct Fields info = \ @@ -50,44 +50,24 @@ # Dynamics dimensions # nvar | neq | dimh | dimp | diml | dimu | dimhl | dimhu dynamics_dims = [ - (12, 10, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5), - (12, 7, 5, 31, 12, 10, 0, 5) + (8, 7, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2), + (8, 5, 2, 25, 8, 7, 0, 2) ] \ No newline at end of file diff --git a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a index 7991669..8b74f9e 100644 Binary files a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a and b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.a differ diff --git a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so index 7e003c5..a30881f 100644 Binary files a/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so and b/solver/codeGen/TailoredSolver/lib/libTailoredSolver.so differ diff --git a/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so b/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so index c5aa17c..4d29615 100755 Binary files a/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so and b/solver/codeGen/TailoredSolver/lib/libTailoredSolver_withModel.so differ diff --git a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a index 0fe6eb9..271d4ed 100644 Binary files a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a and b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.a differ diff --git a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so index 7a38b73..7c0cf98 100755 Binary files a/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so and b/solver/codeGen/TailoredSolver/lib_target/libTailoredSolver.so differ diff --git a/solver/codeGen/TailoredSolver_adtool2forces.c b/solver/codeGen/TailoredSolver_adtool2forces.c index a22d5ea..a2037cb 100644 --- a/solver/codeGen/TailoredSolver_adtool2forces.c +++ b/solver/codeGen/TailoredSolver_adtool2forces.c @@ -60,15 +60,15 @@ extern solver_int32_default TailoredSolver_adtool2forces(TailoredSolver_float *x /* Allocate working arrays for AD tool */ - TailoredSolver_callback_float w[184]; + TailoredSolver_callback_float w[109]; /* temporary storage for AD tool sparse output */ TailoredSolver_callback_float this_f; - TailoredSolver_callback_float nabla_f_sparse[10]; - TailoredSolver_callback_float h_sparse[5]; - TailoredSolver_callback_float nabla_h_sparse[18]; - TailoredSolver_callback_float c_sparse[7]; - TailoredSolver_callback_float nabla_c_sparse[48]; + TailoredSolver_callback_float nabla_f_sparse[7]; + TailoredSolver_callback_float h_sparse[2]; + TailoredSolver_callback_float nabla_h_sparse[6]; + TailoredSolver_callback_float c_sparse[5]; + TailoredSolver_callback_float nabla_c_sparse[26]; /* pointers to row and column info for @@ -82,7 +82,7 @@ extern solver_int32_default TailoredSolver_adtool2forces(TailoredSolver_float *x in[2] = l; in[3] = y; - if ((stage >= 0) && (stage < 39)) + if ((stage >= 0) && (stage < 19)) { out[0] = &this_f; out[1] = nabla_f_sparse; @@ -140,38 +140,38 @@ extern solver_int32_default TailoredSolver_adtool2forces(TailoredSolver_float *x } - if ((stage >= 39) && (stage < 40)) + if ((stage >= 19) && (stage < 20)) { out[0] = &this_f; out[1] = nabla_f_sparse; - TailoredSolver_objective_40(in, out, NULL, w, 0); + TailoredSolver_objective_20(in, out, NULL, w, 0); if (nabla_f != NULL) { - nrow = TailoredSolver_objective_40_sparsity_out(1)[0]; - ncol = TailoredSolver_objective_40_sparsity_out(1)[1]; - colind = TailoredSolver_objective_40_sparsity_out(1) + 2; - row = TailoredSolver_objective_40_sparsity_out(1) + 2 + (ncol + 1); + nrow = TailoredSolver_objective_20_sparsity_out(1)[0]; + ncol = TailoredSolver_objective_20_sparsity_out(1)[1]; + colind = TailoredSolver_objective_20_sparsity_out(1) + 2; + row = TailoredSolver_objective_20_sparsity_out(1) + 2 + (ncol + 1); TailoredSolver_sparse2fullcopy(nrow, ncol, colind, row, nabla_f_sparse, nabla_f); } out[0] = h_sparse; out[1] = nabla_h_sparse; - TailoredSolver_inequalities_40(in, out, NULL, w, 0); + TailoredSolver_inequalities_20(in, out, NULL, w, 0); if (h != NULL) { - nrow = TailoredSolver_inequalities_40_sparsity_out(0)[0]; - ncol = TailoredSolver_inequalities_40_sparsity_out(0)[1]; - colind = TailoredSolver_inequalities_40_sparsity_out(0) + 2; - row = TailoredSolver_inequalities_40_sparsity_out(0) + 2 + (ncol + 1); + nrow = TailoredSolver_inequalities_20_sparsity_out(0)[0]; + ncol = TailoredSolver_inequalities_20_sparsity_out(0)[1]; + colind = TailoredSolver_inequalities_20_sparsity_out(0) + 2; + row = TailoredSolver_inequalities_20_sparsity_out(0) + 2 + (ncol + 1); TailoredSolver_sparse2fullcopy(nrow, ncol, colind, row, h_sparse, h); } if (nabla_h != NULL) { - nrow = TailoredSolver_inequalities_40_sparsity_out(1)[0]; - ncol = TailoredSolver_inequalities_40_sparsity_out(1)[1]; - colind = TailoredSolver_inequalities_40_sparsity_out(1) + 2; - row = TailoredSolver_inequalities_40_sparsity_out(1) + 2 + (ncol + 1); + nrow = TailoredSolver_inequalities_20_sparsity_out(1)[0]; + ncol = TailoredSolver_inequalities_20_sparsity_out(1)[1]; + colind = TailoredSolver_inequalities_20_sparsity_out(1) + 2; + row = TailoredSolver_inequalities_20_sparsity_out(1) + 2 + (ncol + 1); TailoredSolver_sparse2fullcopy(nrow, ncol, colind, row, nabla_h_sparse, nabla_h); } diff --git a/solver/codeGen/TailoredSolver_adtool2forces.o b/solver/codeGen/TailoredSolver_adtool2forces.o index 0867f38..4ab8b46 100644 Binary files a/solver/codeGen/TailoredSolver_adtool2forces.o and b/solver/codeGen/TailoredSolver_adtool2forces.o differ diff --git a/solver/codeGen/TailoredSolver_casadi.c b/solver/codeGen/TailoredSolver_casadi.c index 3b977d4..f16708d 100644 --- a/solver/codeGen/TailoredSolver_casadi.c +++ b/solver/codeGen/TailoredSolver_casadi.c @@ -60,136 +60,117 @@ casadi_real casadi_sq(casadi_real x) { return x*x;} casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} -static const casadi_int casadi_s0[16] = {12, 1, 0, 12, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; -static const casadi_int casadi_s1[35] = {31, 1, 0, 31, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30}; +static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; +static const casadi_int casadi_s1[29] = {25, 1, 0, 25, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24}; static const casadi_int casadi_s2[3] = {0, 0, 0}; static const casadi_int casadi_s3[5] = {1, 1, 0, 1, 0}; -static const casadi_int casadi_s4[25] = {1, 12, 0, 1, 2, 3, 4, 5, 6, 6, 7, 8, 9, 10, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; -static const casadi_int casadi_s5[11] = {7, 1, 0, 7, 0, 1, 2, 3, 4, 5, 6}; -static const casadi_int casadi_s6[63] = {7, 12, 0, 0, 0, 6, 12, 17, 23, 29, 31, 33, 38, 43, 48, 0, 2, 3, 4, 5, 6, 1, 2, 3, 4, 5, 6, 2, 3, 4, 5, 6, 0, 2, 3, 4, 5, 6, 1, 2, 3, 4, 5, 6, 2, 3, 2, 3, 2, 3, 4, 5, 6, 2, 3, 4, 5, 6, 2, 3, 4, 5, 6}; -static const casadi_int casadi_s7[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4}; -static const casadi_int casadi_s8[33] = {5, 12, 0, 1, 3, 3, 3, 3, 5, 7, 9, 11, 14, 16, 18, 4, 0, 1, 2, 3, 2, 3, 0, 1, 0, 1, 2, 3, 4, 2, 3, 2, 3}; +static const casadi_int casadi_s4[18] = {1, 8, 0, 1, 2, 3, 4, 5, 6, 7, 7, 0, 0, 0, 0, 0, 0, 0}; +static const casadi_int casadi_s5[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4}; +static const casadi_int casadi_s6[37] = {5, 8, 0, 0, 5, 9, 14, 16, 18, 22, 26, 0, 1, 2, 3, 4, 1, 2, 3, 4, 0, 1, 2, 3, 4, 1, 2, 1, 2, 1, 2, 3, 4, 1, 2, 3, 4}; +static const casadi_int casadi_s7[6] = {2, 1, 0, 2, 0, 1}; +static const casadi_int casadi_s8[17] = {2, 8, 0, 2, 2, 2, 2, 4, 6, 6, 6, 0, 1, 0, 1, 0, 1}; -/* TailoredSolver_objective_1:(i0[12],i1[31],i2[],i3[])->(o0,o1[1x12,10nz]) */ +/* TailoredSolver_objective_1:(i0[8],i1[25],i2[],i3[])->(o0,o1[1x8,7nz]) */ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a4, a5, a6, a7, a8, a9; + casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a3, a4, a5, a6, a7, a8, a9; a0=arg[1]? arg[1][0] : 0; - a1=arg[0]? arg[0][2] : 0; + a1=arg[0]? arg[0][1] : 0; a2=casadi_sq(a1); a2=(a0*a2); - a3=arg[1]? arg[1][22] : 0; - a4=arg[0]? arg[0][9] : 0; - a5=arg[0]? arg[0][8] : 0; - a6=cos(a5); - a7=(a4*a6); - a8=arg[0]? arg[0][10] : 0; - a9=sin(a5); + a3=arg[1]? arg[1][19] : 0; + a4=arg[1]? arg[1][21] : 0; + a5=arg[1]? arg[1][23] : 0; + a6=arg[0]? arg[0][5] : 0; + a7=cos(a6); + a7=(a5*a7); + a8=arg[0]? arg[0][6] : 0; + a9=sin(a6); a10=(a8*a9); a7=(a7-a10); + a7=(a4*a7); a10=1.; - a11=arg[0]? arg[0][7] : 0; - a12=arg[1]? arg[1][30] : 0; + a11=arg[0]? arg[0][4] : 0; + a12=arg[1]? arg[1][24] : 0; a13=(a11*a12); a13=(a10-a13); a7=(a7/a13); - a14=arg[1]? arg[1][27] : 0; - a15=(a7*a14); - a15=(a3*a15); - a2=(a2-a15); - a15=arg[1]? arg[1][1] : 0; - a16=arg[0]? arg[0][3] : 0; - a17=casadi_sq(a16); - a17=(a15*a17); - a2=(a2+a17); - a17=arg[1]? arg[1][26] : 0; - a18=arg[0]? arg[0][4] : 0; - a19=casadi_sq(a18); - a19=(a17*a19); - a2=(a2+a19); - a19=arg[1]? arg[1][17] : 0; - a20=(a8/a4); - a21=atan(a20); - a22=arg[0]? arg[0][5] : 0; - a23=arg[1]? arg[1][5] : 0; - a22=(a22*a23); - a24=arg[1]? arg[1][4] : 0; - a24=(a23+a24); - a22=(a22/a24); - a25=atan(a22); - a21=(a21-a25); - a25=casadi_sq(a21); - a25=(a19*a25); + a14=(a3*a7); + a2=(a2-a14); + a14=arg[1]? arg[1][20] : 0; + a15=arg[0]? arg[0][2] : 0; + a16=casadi_sq(a15); + a16=(a14*a16); + a2=(a2+a16); + a16=arg[1]? arg[1][16] : 0; + a17=(a8/a5); + a18=atan(a17); + a19=arg[0]? arg[0][3] : 0; + a20=arg[1]? arg[1][4] : 0; + a19=(a19*a20); + a21=arg[1]? arg[1][3] : 0; + a21=(a20+a21); + a19=(a19/a21); + a22=atan(a19); + a18=(a18-a22); + a22=casadi_sq(a18); + a22=(a16*a22); + a2=(a2+a22); + a22=arg[1]? arg[1][18] : 0; + a23=casadi_sq(a6); + a23=(a22*a23); + a2=(a2+a23); + a23=arg[1]? arg[1][17] : 0; + a24=casadi_sq(a11); + a24=(a23*a24); + a2=(a2+a24); + a24=arg[1]? arg[1][22] : 0; + a25=arg[0]? arg[0][0] : 0; + a25=(a24*a25); a2=(a2+a25); - a25=arg[1]? arg[1][20] : 0; - a26=casadi_sq(a5); - a26=(a25*a26); - a2=(a2+a26); - a26=arg[1]? arg[1][19] : 0; - a27=casadi_sq(a11); - a27=(a26*a27); - a2=(a2+a27); - a27=arg[1]? arg[1][28] : 0; - a28=arg[0]? arg[0][0] : 0; - a28=(a27*a28); - a2=(a2+a28); - a28=arg[1]? arg[1][29] : 0; - a29=arg[0]? arg[0][1] : 0; - a29=(a28*a29); - a2=(a2+a29); if (res[0]!=0) res[0][0]=a2; - if (res[1]!=0) res[1][0]=a27; - if (res[1]!=0) res[1][1]=a28; + if (res[1]!=0) res[1][0]=a24; a1=(a1+a1); a1=(a1*a0); - if (res[1]!=0) res[1][2]=a1; - a16=(a16+a16); - a16=(a16*a15); - if (res[1]!=0) res[1][3]=a16; + if (res[1]!=0) res[1][1]=a1; + a15=(a15+a15); + a15=(a15*a14); + if (res[1]!=0) res[1][2]=a15; a18=(a18+a18); - a18=(a18*a17); - if (res[1]!=0) res[1][4]=a18; - a21=(a21+a21); - a21=(a21*a19); - a22=casadi_sq(a22); - a22=(a10+a22); - a22=(a21/a22); - a22=(a22/a24); - a23=(a23*a22); - a23=(-a23); - if (res[1]!=0) res[1][5]=a23; + a18=(a18*a16); + a19=casadi_sq(a19); + a19=(a10+a19); + a19=(a18/a19); + a19=(a19/a21); + a20=(a20*a19); + a20=(-a20); + if (res[1]!=0) res[1][3]=a20; a11=(a11+a11); - a11=(a11*a26); + a11=(a11*a23); a7=(a7/a13); - a14=(a14*a3); - a7=(a7*a14); + a7=(a7*a3); a12=(a12*a7); a11=(a11-a12); - if (res[1]!=0) res[1][6]=a11; - a11=(a5+a5); - a11=(a11*a25); - a25=cos(a5); - a14=(a14/a13); - a8=(a8*a14); - a25=(a25*a8); - a11=(a11+a25); - a5=sin(a5); - a25=(a4*a14); - a5=(a5*a25); - a11=(a11+a5); - if (res[1]!=0) res[1][7]=a11; - a11=(a20/a4); - a20=casadi_sq(a20); - a10=(a10+a20); - a21=(a21/a10); - a11=(a11*a21); - a6=(a6*a14); + if (res[1]!=0) res[1][4]=a11; + a11=(a6+a6); + a11=(a11*a22); + a22=cos(a6); + a3=(a3/a13); + a4=(a4*a3); + a8=(a8*a4); + a22=(a22*a8); + a11=(a11+a22); + a6=sin(a6); + a22=(a5*a4); + a6=(a6*a22); a11=(a11+a6); - a11=(-a11); - if (res[1]!=0) res[1][8]=a11; - a21=(a21/a4); - a9=(a9*a14); - a21=(a21+a9); - if (res[1]!=0) res[1][9]=a21; + if (res[1]!=0) res[1][5]=a11; + a17=casadi_sq(a17); + a10=(a10+a17); + a18=(a18/a10); + a18=(a18/a5); + a9=(a9*a4); + a18=(a18+a9); + if (res[1]!=0) res[1][6]=a18; return 0; } @@ -205,14 +186,14 @@ const casadi_int* TailoredSolver_objective_1_sparsity_out(casadi_int i) { } } -/* TailoredSolver_dynamics_1:(i0[12],i1[31],i2[],i3[])->(o0[7],o1[7x12,48nz]) */ +/* TailoredSolver_dynamics_1:(i0[8],i1[25],i2[],i3[])->(o0[5],o1[5x8,26nz]) */ static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a109, a11, a110, a111, a112, a113, a114, a115, a116, a117, a118, a119, a12, a120, a121, a122, a123, a124, a125, a126, a127, a128, a129, a13, a130, a131, a132, a133, a134, a135, a136, a137, a138, a139, a14, a140, a141, a142, a143, a144, a145, a146, a147, a148, a149, a15, a150, a151, a152, a153, a154, a155, a156, a157, a158, a159, a16, a160, a161, a162, a163, a164, a165, a166, a167, a168, a169, a17, a170, a171, a172, a173, a174, a175, a176, a177, a178, a179, a18, a180, a181, a182, a183, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a32, a33, a34, a35, a36, a37, a38, a39, a4, a40, a41, a42, a43, a44, a45, a46, a47, a48, a49, a5, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59, a6, a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a7, a70, a71, a72, a73, a74, a75, a76, a77, a78, a79, a8, a80, a81, a82, a83, a84, a85, a86, a87, a88, a89, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; - a0=arg[0]? arg[0][5] : 0; - a1=arg[1]? arg[1][27] : 0; + casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a32, a33, a34, a35, a36, a37, a38, a39, a4, a40, a41, a42, a43, a44, a45, a46, a47, a48, a49, a5, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59, a6, a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a7, a70, a71, a72, a73, a74, a75, a76, a77, a78, a79, a8, a80, a81, a82, a83, a84, a85, a86, a87, a88, a89, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; + a0=arg[0]? arg[0][3] : 0; + a1=arg[1]? arg[1][21] : 0; a2=6.; a2=(a1/a2); - a3=arg[0]? arg[0][2] : 0; + a3=arg[0]? arg[0][1] : 0; a4=2.; a5=(a4*a3); a5=(a3+a5); @@ -222,2715 +203,1398 @@ static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, a5=(a2*a5); a5=(a0+a5); if (res[0]!=0) res[0][0]=a5; - a5=arg[0]? arg[0][6] : 0; - a6=arg[0]? arg[0][3] : 0; - a7=(a4*a6); - a7=(a6+a7); - a8=(a4*a6); - a7=(a7+a8); - a7=(a7+a6); - a7=(a2*a7); - a7=(a5+a7); - if (res[0]!=0) res[0][1]=a7; - a7=arg[0]? arg[0][7] : 0; - a8=arg[0]? arg[0][9] : 0; - a9=arg[0]? arg[0][8] : 0; - a10=sin(a9); - a11=(a8*a10); - a12=arg[0]? arg[0][10] : 0; - a13=cos(a9); - a14=(a12*a13); - a11=(a11+a14); - a14=(a1/a4); - a15=arg[1]? arg[1][25] : 0; - a16=(a15*a5); - a17=1.; - a18=cos(a0); - a18=(a17+a18); - a19=(a16*a18); - a20=arg[1]? arg[1][2] : 0; - a21=arg[1]? arg[1][12] : 0; - a22=(a20*a21); - a23=arg[1]? arg[1][13] : 0; - a22=(a22*a23); - a19=(a19-a22); - a22=5.0000000000000000e-01; - a24=arg[1]? arg[1][14] : 0; - a25=(a22*a24); - a26=arg[1]? arg[1][15] : 0; - a25=(a25*a26); - a27=arg[1]? arg[1][16] : 0; - a25=(a25*a27); - a28=casadi_sq(a8); - a28=(a25*a28); - a19=(a19-a28); - a28=arg[1]? arg[1][7] : 0; - a29=arg[1]? arg[1][9] : 0; - a30=arg[1]? arg[1][11] : 0; - a31=arg[1]? arg[1][4] : 0; - a32=arg[0]? arg[0][11] : 0; - a33=(a31*a32); - a33=(a12+a33); - a33=(a33/a8); + a5=arg[0]? arg[0][4] : 0; + a6=arg[1]? arg[1][23] : 0; + a7=arg[0]? arg[0][5] : 0; + a8=sin(a7); + a8=(a6*a8); + a9=arg[0]? arg[0][6] : 0; + a10=cos(a7); + a11=(a9*a10); + a8=(a8+a11); + a11=(a1/a4); + a12=arg[0]? arg[0][7] : 0; + a13=arg[1]? arg[1][24] : 0; + a14=cos(a7); + a14=(a6*a14); + a15=sin(a7); + a16=(a9*a15); + a14=(a14-a16); + a16=1.; + a17=(a5*a13); + a17=(a16-a17); + a14=(a14/a17); + a18=(a13*a14); + a18=(a12-a18); + a19=(a11*a18); + a19=(a7+a19); + a20=sin(a19); + a20=(a6*a20); + a21=arg[1]? arg[1][5] : 0; + a22=arg[1]? arg[1][7] : 0; + a23=arg[1]? arg[1][9] : 0; + a24=arg[1]? arg[1][4] : 0; + a25=(a24*a12); + a25=(a9-a25); + a25=(a25/a6); + a26=atan(a25); + a26=(a23*a26); + a27=atan(a26); + a27=(a22*a27); + a28=sin(a27); + a28=(a21*a28); + a29=arg[1]? arg[1][6] : 0; + a30=arg[1]? arg[1][8] : 0; + a31=arg[1]? arg[1][10] : 0; + a32=arg[1]? arg[1][3] : 0; + a33=(a32*a12); + a33=(a9+a33); + a33=(a33/a6); a34=atan(a33); a34=(a34-a0); - a34=(a30*a34); + a34=(a31*a34); a35=atan(a34); - a35=(a29*a35); + a35=(a30*a35); a36=sin(a35); - a36=(a28*a36); - a37=sin(a0); + a36=(a29*a36); + a37=cos(a0); a38=(a36*a37); - a19=(a19-a38); - a38=(a20*a12); - a39=(a38*a32); - a19=(a19+a39); - a19=(a19/a20); - a39=(a14*a19); - a39=(a8+a39); - a40=arg[1]? arg[1][30] : 0; - a41=cos(a9); - a42=(a8*a41); - a43=sin(a9); - a44=(a12*a43); - a42=(a42-a44); - a44=(a7*a40); - a44=(a17-a44); - a42=(a42/a44); - a45=(a40*a42); - a45=(a32-a45); - a46=(a14*a45); - a46=(a9+a46); - a47=sin(a46); - a48=(a39*a47); - a49=arg[1]? arg[1][6] : 0; - a50=arg[1]? arg[1][8] : 0; - a51=arg[1]? arg[1][10] : 0; - a52=arg[1]? arg[1][5] : 0; - a53=(a52*a32); - a53=(a12-a53); - a53=(a53/a8); - a54=atan(a53); - a54=(a51*a54); + a38=(a28+a38); + a39=arg[1]? arg[1][1] : 0; + a40=(a39*a6); + a41=(a40*a12); + a38=(a38-a41); + a38=(a38/a39); + a41=(a11*a38); + a41=(a9+a41); + a42=cos(a19); + a43=(a41*a42); + a20=(a20+a43); + a43=(a4*a20); + a43=(a8+a43); + a44=(a1/a4); + a45=cos(a0); + a46=(a36*a45); + a46=(a46*a32); + a28=(a28*a24); + a46=(a46-a28); + a28=arg[0]? arg[0][2] : 0; + a46=(a46+a28); + a47=arg[1]? arg[1][2] : 0; + a46=(a46/a47); + a48=(a11*a46); + a48=(a12+a48); + a49=cos(a19); + a49=(a6*a49); + a50=sin(a19); + a51=(a41*a50); + a49=(a49-a51); + a8=(a11*a8); + a8=(a5+a8); + a8=(a8*a13); + a8=(a16-a8); + a49=(a49/a8); + a51=(a13*a49); + a51=(a48-a51); + a52=(a44*a51); + a52=(a7+a52); + a53=sin(a52); + a53=(a6*a53); + a54=(a24*a48); + a54=(a41-a54); + a54=(a54/a6); a55=atan(a54); - a55=(a50*a55); - a56=sin(a55); - a56=(a49*a56); - a57=(a15*a5); - a58=sin(a0); - a59=(a57*a58); - a59=(a56+a59); - a60=cos(a0); - a61=(a36*a60); - a59=(a59+a61); - a61=(a20*a8); - a62=(a61*a32); - a59=(a59-a62); - a59=(a59/a20); - a62=(a14*a59); - a62=(a12+a62); - a63=cos(a46); + a55=(a23*a55); + a56=atan(a55); + a56=(a22*a56); + a57=sin(a56); + a57=(a21*a57); + a58=(a32*a48); + a58=(a41+a58); + a58=(a58/a6); + a59=atan(a58); + a60=(a11*a3); + a60=(a0+a60); + a59=(a59-a60); + a59=(a31*a59); + a61=atan(a59); + a61=(a30*a61); + a62=sin(a61); + a62=(a29*a62); + a63=cos(a60); a64=(a62*a63); - a48=(a48+a64); - a64=(a4*a48); - a64=(a11+a64); - a65=(a1/a4); - a66=(a14*a6); - a66=(a5+a66); - a67=(a15*a66); - a68=(a14*a3); - a68=(a0+a68); - a69=cos(a68); - a69=(a17+a69); - a70=(a67*a69); - a71=(a20*a21); - a71=(a71*a23); - a70=(a70-a71); - a71=(a22*a24); - a71=(a71*a26); - a71=(a71*a27); - a72=casadi_sq(a39); - a72=(a71*a72); - a70=(a70-a72); - a72=cos(a0); - a73=(a36*a72); - a74=(a15*a5); - a75=sin(a0); - a76=(a74*a75); - a73=(a73+a76); - a73=(a73*a31); - a56=(a56*a52); - a73=(a73-a56); - a56=arg[0]? arg[0][4] : 0; - a73=(a73+a56); - a76=arg[1]? arg[1][3] : 0; - a73=(a73/a76); - a77=(a14*a73); - a77=(a32+a77); - a78=(a31*a77); - a78=(a62+a78); - a78=(a78/a39); - a79=atan(a78); - a79=(a79-a68); - a79=(a30*a79); - a80=atan(a79); - a80=(a29*a80); - a81=sin(a80); - a81=(a28*a81); - a82=sin(a68); - a83=(a81*a82); - a70=(a70-a83); - a83=(a20*a62); - a84=(a83*a77); - a70=(a70+a84); - a70=(a70/a20); - a84=(a65*a70); - a84=(a8+a84); - a85=cos(a46); - a86=(a39*a85); - a87=sin(a46); - a88=(a62*a87); - a86=(a86-a88); - a11=(a14*a11); - a11=(a7+a11); - a11=(a11*a40); - a11=(a17-a11); - a86=(a86/a11); - a88=(a40*a86); - a88=(a77-a88); - a89=(a65*a88); - a89=(a9+a89); - a90=sin(a89); - a91=(a84*a90); - a92=(a52*a77); - a92=(a62-a92); - a92=(a92/a39); - a93=atan(a92); - a93=(a51*a93); - a94=atan(a93); - a94=(a50*a94); - a95=sin(a94); - a95=(a49*a95); - a96=(a15*a66); - a97=sin(a68); - a98=(a96*a97); - a98=(a95+a98); - a99=cos(a68); - a100=(a81*a99); - a98=(a98+a100); - a100=(a20*a39); - a101=(a100*a77); - a98=(a98-a101); - a98=(a98/a20); - a101=(a65*a98); - a101=(a12+a101); - a102=cos(a89); - a103=(a101*a102); - a91=(a91+a103); - a103=(a4*a91); - a64=(a64+a103); - a103=(a65*a6); - a103=(a5+a103); - a104=(a15*a103); - a105=(a65*a3); - a105=(a0+a105); - a106=cos(a105); - a106=(a17+a106); - a107=(a104*a106); - a108=(a20*a21); - a108=(a108*a23); - a107=(a107-a108); - a108=(a22*a24); - a108=(a108*a26); - a108=(a108*a27); - a109=casadi_sq(a84); - a109=(a108*a109); - a107=(a107-a109); - a109=cos(a68); - a110=(a81*a109); - a66=(a15*a66); - a111=sin(a68); - a112=(a66*a111); - a110=(a110+a112); - a110=(a110*a31); - a95=(a95*a52); - a110=(a110-a95); - a110=(a110+a56); - a110=(a110/a76); - a95=(a65*a110); - a95=(a32+a95); - a112=(a31*a95); - a112=(a101+a112); - a112=(a112/a84); - a113=atan(a112); - a113=(a113-a105); - a113=(a30*a113); - a114=atan(a113); - a114=(a29*a114); - a115=sin(a114); - a115=(a28*a115); - a116=sin(a105); - a117=(a115*a116); - a107=(a107-a117); - a117=(a20*a101); - a118=(a117*a95); - a107=(a107+a118); - a107=(a107/a20); - a118=(a1*a107); - a118=(a8+a118); - a119=cos(a89); - a120=(a84*a119); - a121=sin(a89); - a122=(a101*a121); - a120=(a120-a122); + a64=(a57+a64); + a65=(a39*a6); a48=(a65*a48); - a48=(a7+a48); - a48=(a48*a40); - a48=(a17-a48); - a120=(a120/a48); - a122=(a40*a120); - a122=(a95-a122); - a123=(a1*a122); - a123=(a9+a123); - a124=sin(a123); - a125=(a118*a124); - a126=(a52*a95); - a126=(a101-a126); - a126=(a126/a84); - a127=atan(a126); - a127=(a51*a127); - a128=atan(a127); - a128=(a50*a128); - a129=sin(a128); - a129=(a49*a129); - a130=(a15*a103); - a131=sin(a105); - a132=(a130*a131); - a132=(a129+a132); - a133=cos(a105); - a134=(a115*a133); - a132=(a132+a134); - a134=(a20*a84); - a135=(a134*a95); - a132=(a132-a135); - a132=(a132/a20); - a135=(a1*a132); - a135=(a12+a135); - a136=cos(a123); - a137=(a135*a136); - a125=(a125+a137); - a64=(a64+a125); - a64=(a2*a64); - a64=(a7+a64); - if (res[0]!=0) res[0][2]=a64; - a88=(a4*a88); - a45=(a45+a88); - a122=(a4*a122); - a45=(a45+a122); - a122=cos(a105); - a88=(a115*a122); - a103=(a15*a103); - a64=sin(a105); - a125=(a103*a64); - a88=(a88+a125); - a88=(a88*a31); - a129=(a129*a52); - a88=(a88-a129); - a88=(a88+a56); - a88=(a88/a76); - a129=(a1*a88); - a129=(a32+a129); - a125=cos(a123); - a137=(a118*a125); - a138=sin(a123); - a139=(a135*a138); - a137=(a137-a139); - a91=(a1*a91); - a7=(a7+a91); - a7=(a7*a40); - a7=(a17-a7); - a137=(a137/a7); - a91=(a40*a137); - a91=(a129-a91); - a45=(a45+a91); - a45=(a2*a45); - a45=(a9+a45); - if (res[0]!=0) res[0][3]=a45; - a70=(a4*a70); - a19=(a19+a70); - a107=(a4*a107); - a19=(a19+a107); - a6=(a1*a6); - a5=(a5+a6); - a6=(a15*a5); + a64=(a64-a48); + a64=(a64/a39); + a48=(a44*a64); + a48=(a9+a48); + a66=cos(a52); + a67=(a48*a66); + a53=(a53+a67); + a67=(a4*a53); + a43=(a43+a67); + a67=cos(a60); + a68=(a62*a67); + a68=(a68*a32); + a57=(a57*a24); + a68=(a68-a57); + a68=(a68+a28); + a68=(a68/a47); + a57=(a44*a68); + a57=(a12+a57); + a69=cos(a52); + a69=(a6*a69); + a70=sin(a52); + a71=(a48*a70); + a69=(a69-a71); + a20=(a44*a20); + a20=(a5+a20); + a20=(a20*a13); + a20=(a16-a20); + a69=(a69/a20); + a71=(a13*a69); + a71=(a57-a71); + a72=(a1*a71); + a72=(a7+a72); + a73=sin(a72); + a73=(a6*a73); + a74=(a24*a57); + a74=(a48-a74); + a74=(a74/a6); + a75=atan(a74); + a75=(a23*a75); + a76=atan(a75); + a76=(a22*a76); + a77=sin(a76); + a77=(a21*a77); + a78=(a32*a57); + a78=(a48+a78); + a78=(a78/a6); + a79=atan(a78); + a80=(a44*a3); + a80=(a0+a80); + a79=(a79-a80); + a79=(a31*a79); + a81=atan(a79); + a81=(a30*a81); + a82=sin(a81); + a82=(a29*a82); + a83=cos(a80); + a84=(a82*a83); + a84=(a77+a84); + a85=(a39*a6); + a57=(a85*a57); + a84=(a84-a57); + a84=(a84/a39); + a57=(a1*a84); + a57=(a9+a57); + a86=cos(a72); + a87=(a57*a86); + a73=(a73+a87); + a43=(a43+a73); + a43=(a2*a43); + a43=(a5+a43); + if (res[0]!=0) res[0][1]=a43; + a51=(a4*a51); + a18=(a18+a51); + a71=(a4*a71); + a18=(a18+a71); + a71=cos(a80); + a51=(a82*a71); + a51=(a51*a32); + a77=(a77*a24); + a51=(a51-a77); + a51=(a51+a28); + a51=(a51/a47); + a77=(a1*a51); + a77=(a12+a77); + a43=cos(a72); + a43=(a6*a43); + a73=sin(a72); + a87=(a57*a73); + a43=(a43-a87); + a53=(a1*a53); + a5=(a5+a53); + a5=(a5*a13); + a5=(a16-a5); + a43=(a43/a5); + a53=(a13*a43); + a53=(a77-a53); + a18=(a18+a53); + a18=(a2*a18); + a18=(a7+a18); + if (res[0]!=0) res[0][2]=a18; + a64=(a4*a64); + a38=(a38+a64); + a84=(a4*a84); + a38=(a38+a84); + a84=(a24*a77); + a84=(a57-a84); + a84=(a84/a6); + a64=atan(a84); + a64=(a23*a64); + a18=atan(a64); + a18=(a22*a18); + a53=sin(a18); + a53=(a21*a53); + a87=(a32*a77); + a87=(a57+a87); + a87=(a87/a6); + a88=atan(a87); a3=(a1*a3); a3=(a0+a3); - a107=cos(a3); - a107=(a17+a107); - a70=(a6*a107); - a21=(a20*a21); - a21=(a21*a23); - a70=(a70-a21); - a22=(a22*a24); - a22=(a22*a26); - a22=(a22*a27); - a27=casadi_sq(a118); - a27=(a22*a27); - a70=(a70-a27); - a27=(a31*a129); - a27=(a135+a27); - a27=(a27/a118); - a26=atan(a27); - a26=(a26-a3); - a26=(a30*a26); - a24=atan(a26); - a24=(a29*a24); - a21=sin(a24); - a21=(a28*a21); - a23=sin(a3); - a45=(a21*a23); - a70=(a70-a45); - a45=(a20*a135); - a91=(a45*a129); - a70=(a70+a91); - a70=(a70/a20); - a19=(a19+a70); - a19=(a2*a19); - a19=(a8+a19); - if (res[0]!=0) res[0][4]=a19; - a98=(a4*a98); - a59=(a59+a98); - a132=(a4*a132); - a59=(a59+a132); - a132=(a52*a129); - a132=(a135-a132); - a132=(a132/a118); - a98=atan(a132); - a98=(a51*a98); - a19=atan(a98); - a19=(a50*a19); - a70=sin(a19); - a70=(a49*a70); - a91=(a15*a5); - a139=sin(a3); - a140=(a91*a139); - a140=(a70+a140); - a141=cos(a3); - a142=(a21*a141); - a140=(a140+a142); - a142=(a20*a118); - a143=(a142*a129); - a140=(a140-a143); - a140=(a140/a20); - a59=(a59+a140); - a59=(a2*a59); - a59=(a12+a59); - if (res[0]!=0) res[0][5]=a59; - a110=(a4*a110); - a73=(a73+a110); - a88=(a4*a88); - a73=(a73+a88); - a88=cos(a3); - a110=(a21*a88); - a5=(a15*a5); - a59=sin(a3); - a140=(a5*a59); - a110=(a110+a140); - a110=(a110*a31); - a70=(a70*a52); - a110=(a110-a70); - a110=(a110+a56); - a110=(a110/a76); - a73=(a73+a110); - a73=(a2*a73); - a73=(a32+a73); - if (res[0]!=0) res[0][6]=a73; + a88=(a88-a3); + a88=(a31*a88); + a89=atan(a88); + a89=(a30*a89); + a90=sin(a89); + a90=(a29*a90); + a91=cos(a3); + a92=(a90*a91); + a92=(a53+a92); + a93=(a39*a6); + a77=(a93*a77); + a92=(a92-a77); + a92=(a92/a39); + a38=(a38+a92); + a38=(a2*a38); + a38=(a9+a38); + if (res[0]!=0) res[0][3]=a38; + a68=(a4*a68); + a46=(a46+a68); + a51=(a4*a51); + a46=(a46+a51); + a51=cos(a3); + a68=(a90*a51); + a68=(a68*a32); + a53=(a53*a24); + a68=(a68-a53); + a68=(a68+a28); + a68=(a68/a47); + a46=(a46+a68); + a46=(a2*a46); + a12=(a12+a46); + if (res[0]!=0) res[0][4]=a12; if (res[1]!=0) res[1][0]=a1; - a73=cos(a68); - a110=(a73*a14); - a110=(a96*a110); - a80=cos(a80); - a56=(a30*a14); + a76=cos(a76); + a61=cos(a61); + a12=(a31*a11); + a59=casadi_sq(a59); + a59=(a16+a59); + a12=(a12/a59); + a12=(a30*a12); + a12=(a61*a12); + a12=(a29*a12); + a46=(a67*a12); + a68=sin(a60); + a28=(a68*a11); + a28=(a62*a28); + a46=(a46+a28); + a46=(a32*a46); + a46=(a46/a47); + a28=(a44*a46); + a53=(a24*a28); + a12=(a63*a12); + a60=sin(a60); + a38=(a60*a11); + a38=(a62*a38); + a12=(a12+a38); + a12=(a12/a39); + a38=(a44*a12); + a53=(a53-a38); + a53=(a53/a6); + a74=casadi_sq(a74); + a74=(a16+a74); + a53=(a53/a74); + a53=(a23*a53); + a75=casadi_sq(a75); + a75=(a16+a75); + a53=(a53/a75); + a53=(a22*a53); + a53=(a76*a53); + a53=(a21*a53); + a81=cos(a81); + a92=(a32*a28); + a92=(a38+a92); + a92=(a92/a6); + a78=casadi_sq(a78); + a78=(a16+a78); + a92=(a92/a78); + a92=(a92+a44); + a92=(a31*a92); a79=casadi_sq(a79); - a79=(a17+a79); - a56=(a56/a79); - a56=(a29*a56); - a56=(a80*a56); - a56=(a28*a56); - a70=(a99*a56); - a140=sin(a68); - a143=(a140*a14); - a143=(a81*a143); - a70=(a70+a143); - a110=(a110-a70); - a110=(a110/a20); - a70=(a65*a110); - a143=(a102*a70); - a144=sin(a68); - a145=(a144*a14); - a145=(a67*a145); - a146=cos(a68); - a147=(a146*a14); - a147=(a81*a147); - a148=(a82*a56); - a147=(a147-a148); - a145=(a145+a147); - a145=(a145/a20); - a147=(a65*a145); - a148=(a90*a147); - a143=(a143-a148); - a148=(a4*a143); - a149=(a84+a84); - a150=(a149*a147); - a150=(a108*a150); - a151=sin(a105); - a152=(a151*a65); - a152=(a104*a152); - a150=(a150-a152); - a114=cos(a114); - a152=cos(a68); - a153=(a152*a14); - a153=(a66*a153); - a56=(a109*a56); - a68=sin(a68); - a154=(a68*a14); - a154=(a81*a154); - a56=(a56+a154); - a153=(a153-a56); - a153=(a31*a153); - a153=(a153/a76); - a56=(a65*a153); - a154=(a31*a56); - a154=(a70+a154); - a154=(a154/a84); - a155=(a112/a84); - a156=(a155*a147); - a154=(a154+a156); - a112=casadi_sq(a112); - a112=(a17+a112); - a154=(a154/a112); - a154=(a154-a65); - a154=(a30*a154); - a113=casadi_sq(a113); - a113=(a17+a113); - a154=(a154/a113); - a154=(a29*a154); - a154=(a114*a154); - a154=(a28*a154); - a156=(a116*a154); - a157=cos(a105); - a158=(a157*a65); - a158=(a115*a158); - a156=(a156+a158); - a150=(a150-a156); - a156=(a20*a70); - a156=(a95*a156); - a158=(a117*a56); - a156=(a156+a158); - a150=(a150+a156); - a150=(a150/a20); - a156=(a1*a150); - a158=(a124*a156); - a159=cos(a123); - a160=(a119*a147); - a161=(a121*a70); - a160=(a160+a161); - a160=(a160/a48); - a160=(a40*a160); - a160=(a56+a160); - a161=(a1*a160); - a162=(a159*a161); - a162=(a118*a162); - a158=(a158+a162); - a128=cos(a128); - a162=(a52*a56); - a70=(a70-a162); - a70=(a70/a84); - a162=(a126/a84); - a163=(a162*a147); - a70=(a70+a163); - a126=casadi_sq(a126); - a126=(a17+a126); - a70=(a70/a126); - a70=(a51*a70); - a127=casadi_sq(a127); - a127=(a17+a127); - a70=(a70/a127); - a70=(a50*a70); - a70=(a128*a70); - a70=(a49*a70); - a163=cos(a105); - a164=(a163*a65); - a164=(a130*a164); - a164=(a70+a164); - a165=(a133*a154); - a166=sin(a105); - a167=(a166*a65); - a167=(a115*a167); - a165=(a165-a167); - a164=(a164+a165); - a56=(a134*a56); - a147=(a20*a147); - a147=(a95*a147); - a56=(a56-a147); - a164=(a164-a56); - a164=(a164/a20); - a56=(a1*a164); - a147=(a136*a56); - a165=sin(a123); - a167=(a165*a161); - a167=(a135*a167); - a147=(a147-a167); - a158=(a158+a147); - a148=(a148+a158); - a148=(a2*a148); - if (res[1]!=0) res[1][1]=a148; - a160=(a4*a160); - a154=(a122*a154); - a148=sin(a105); - a158=(a148*a65); - a158=(a115*a158); - a154=(a154-a158); - a105=cos(a105); - a158=(a105*a65); - a158=(a103*a158); - a154=(a154+a158); - a154=(a31*a154); - a70=(a52*a70); - a154=(a154-a70); - a154=(a154/a76); - a70=(a1*a154); - a158=(a125*a156); - a147=sin(a123); - a167=(a147*a161); - a167=(a118*a167); - a158=(a158-a167); - a167=(a138*a56); - a123=cos(a123); - a161=(a123*a161); - a161=(a135*a161); - a167=(a167+a161); - a158=(a158-a167); - a158=(a158/a7); - a137=(a137/a7); - a143=(a1*a143); - a143=(a40*a143); - a143=(a137*a143); - a158=(a158+a143); - a158=(a40*a158); - a158=(a70-a158); - a160=(a160+a158); - a160=(a2*a160); - if (res[1]!=0) res[1][2]=a160; - a150=(a4*a150); - a145=(a4*a145); - a150=(a150-a145); - a145=(a20*a56); - a145=(a129*a145); - a160=(a45*a70); - a145=(a145+a160); - a160=sin(a3); - a158=(a160*a1); - a158=(a6*a158); - a143=(a118+a118); - a167=(a143*a156); - a167=(a22*a167); - a158=(a158+a167); - a24=cos(a24); - a167=(a31*a70); - a167=(a56+a167); - a167=(a167/a118); - a161=(a27/a118); - a168=(a161*a156); - a167=(a167-a168); - a27=casadi_sq(a27); - a27=(a17+a27); - a167=(a167/a27); - a167=(a167-a1); - a167=(a30*a167); - a26=casadi_sq(a26); - a26=(a17+a26); - a167=(a167/a26); - a167=(a29*a167); - a167=(a24*a167); - a167=(a28*a167); - a168=(a23*a167); - a169=cos(a3); - a170=(a169*a1); - a170=(a21*a170); - a168=(a168+a170); - a158=(a158+a168); - a145=(a145-a158); - a145=(a145/a20); - a150=(a150+a145); - a150=(a2*a150); - if (res[1]!=0) res[1][3]=a150; - a110=(a4*a110); - a164=(a4*a164); - a110=(a110+a164); - a19=cos(a19); - a164=(a52*a70); - a56=(a56-a164); - a56=(a56/a118); - a164=(a132/a118); - a150=(a164*a156); - a56=(a56-a150); - a132=casadi_sq(a132); - a132=(a17+a132); - a56=(a56/a132); - a56=(a51*a56); - a98=casadi_sq(a98); - a98=(a17+a98); - a56=(a56/a98); - a56=(a50*a56); - a56=(a19*a56); - a56=(a49*a56); - a150=cos(a3); - a145=(a150*a1); - a145=(a91*a145); - a145=(a56+a145); - a158=(a141*a167); - a168=sin(a3); - a170=(a168*a1); - a170=(a21*a170); - a158=(a158-a170); - a145=(a145+a158); - a156=(a20*a156); - a156=(a129*a156); - a70=(a142*a70); - a156=(a156+a70); - a145=(a145-a156); - a145=(a145/a20); - a110=(a110+a145); - a110=(a2*a110); - if (res[1]!=0) res[1][4]=a110; - a153=(a4*a153); - a154=(a4*a154); - a153=(a153+a154); - a167=(a88*a167); - a154=sin(a3); - a110=(a154*a1); - a110=(a21*a110); - a167=(a167-a110); - a3=cos(a3); - a110=(a3*a1); - a110=(a5*a110); - a167=(a167+a110); - a167=(a31*a167); - a56=(a52*a56); - a167=(a167-a56); - a167=(a167/a76); - a153=(a153+a167); - a153=(a2*a153); - if (res[1]!=0) res[1][5]=a153; - if (res[1]!=0) res[1][6]=a1; - a153=(a15*a14); - a153=(a69*a153); - a153=(a153/a20); - a167=(a65*a153); - a56=(a90*a167); - a110=(a15*a14); - a110=(a97*a110); - a110=(a110/a20); - a145=(a65*a110); - a156=(a102*a145); - a56=(a56+a156); - a156=(a4*a56); - a70=(a15*a65); - a70=(a106*a70); - a158=(a149*a167); - a158=(a108*a158); - a70=(a70-a158); - a158=(a15*a14); - a158=(a111*a158); - a158=(a31*a158); - a158=(a158/a76); - a170=(a65*a158); - a171=(a31*a170); - a171=(a145+a171); - a171=(a171/a84); - a172=(a155*a167); - a171=(a171-a172); - a171=(a171/a112); - a171=(a30*a171); - a171=(a171/a113); - a171=(a29*a171); - a171=(a114*a171); - a171=(a28*a171); - a172=(a116*a171); - a70=(a70-a172); - a172=(a20*a145); - a172=(a95*a172); - a173=(a117*a170); - a172=(a172+a173); - a70=(a70+a172); - a70=(a70/a20); - a172=(a1*a70); - a173=(a124*a172); - a174=(a119*a167); - a175=(a121*a145); - a174=(a174-a175); - a174=(a174/a48); - a174=(a40*a174); - a174=(a170-a174); - a175=(a1*a174); - a176=(a159*a175); - a176=(a118*a176); - a173=(a173+a176); - a176=(a52*a170); - a145=(a145-a176); - a145=(a145/a84); - a176=(a162*a167); - a145=(a145-a176); - a145=(a145/a126); - a145=(a51*a145); - a145=(a145/a127); - a145=(a50*a145); - a145=(a128*a145); - a145=(a49*a145); - a176=(a15*a65); - a176=(a131*a176); - a176=(a145+a176); - a177=(a133*a171); - a176=(a176+a177); - a167=(a20*a167); - a167=(a95*a167); - a170=(a134*a170); - a167=(a167+a170); - a176=(a176-a167); - a176=(a176/a20); - a167=(a1*a176); - a170=(a136*a167); - a177=(a165*a175); - a177=(a135*a177); - a170=(a170-a177); - a173=(a173+a170); - a156=(a156+a173); - a156=(a2*a156); - if (res[1]!=0) res[1][7]=a156; - a174=(a4*a174); - a171=(a122*a171); - a156=(a15*a65); - a156=(a64*a156); - a171=(a171+a156); - a171=(a31*a171); - a145=(a52*a145); - a171=(a171-a145); - a171=(a171/a76); - a145=(a1*a171); - a156=(a125*a172); - a173=(a147*a175); - a173=(a118*a173); - a156=(a156-a173); - a173=(a138*a167); - a175=(a123*a175); - a175=(a135*a175); - a173=(a173+a175); - a156=(a156-a173); - a156=(a156/a7); - a56=(a1*a56); - a56=(a40*a56); - a56=(a137*a56); - a156=(a156+a56); - a156=(a40*a156); - a156=(a145-a156); - a174=(a174+a156); - a174=(a2*a174); - if (res[1]!=0) res[1][8]=a174; - a153=(a4*a153); - a70=(a4*a70); - a153=(a153+a70); - a70=(a15*a1); - a70=(a107*a70); - a174=(a143*a172); - a174=(a22*a174); - a70=(a70-a174); - a174=(a31*a145); - a174=(a167+a174); - a174=(a174/a118); - a156=(a161*a172); - a174=(a174-a156); - a174=(a174/a27); - a174=(a30*a174); - a174=(a174/a26); - a174=(a29*a174); - a174=(a24*a174); - a174=(a28*a174); - a156=(a23*a174); - a70=(a70-a156); - a156=(a20*a167); - a156=(a129*a156); - a56=(a45*a145); - a156=(a156+a56); - a70=(a70+a156); - a70=(a70/a20); - a153=(a153+a70); - a153=(a2*a153); - if (res[1]!=0) res[1][9]=a153; - a110=(a4*a110); - a176=(a4*a176); - a110=(a110+a176); - a176=(a52*a145); - a167=(a167-a176); - a167=(a167/a118); - a176=(a164*a172); - a167=(a167-a176); - a167=(a167/a132); - a167=(a51*a167); - a167=(a167/a98); - a167=(a50*a167); - a167=(a19*a167); - a167=(a49*a167); - a176=(a15*a1); - a176=(a139*a176); - a176=(a167+a176); - a153=(a141*a174); - a176=(a176+a153); - a172=(a20*a172); - a172=(a129*a172); - a145=(a142*a145); - a172=(a172+a145); - a176=(a176-a172); - a176=(a176/a20); - a110=(a110+a176); - a110=(a2*a110); - if (res[1]!=0) res[1][10]=a110; - a158=(a4*a158); - a171=(a4*a171); - a158=(a158+a171); - a174=(a88*a174); - a171=(a15*a1); - a171=(a59*a171); - a174=(a174+a171); - a174=(a31*a174); - a167=(a52*a167); - a174=(a174-a167); - a174=(a174/a76); - a158=(a158+a174); - a158=(a2*a158); - if (res[1]!=0) res[1][11]=a158; - a158=(a14/a76); - a174=(a83*a158); - a167=(a31*a158); - a167=(a167/a39); - a171=casadi_sq(a78); - a171=(a17+a171); - a167=(a167/a171); - a167=(a30*a167); - a167=(a167/a79); - a167=(a29*a167); - a167=(a80*a167); - a167=(a28*a167); - a110=(a82*a167); - a174=(a174-a110); - a174=(a174/a20); - a110=(a65*a174); - a176=(a90*a110); - a172=cos(a89); - a145=(a65*a158); - a153=(a172*a145); - a153=(a84*a153); - a176=(a176+a153); - a153=(a99*a167); - a94=cos(a94); - a70=(a52*a158); - a70=(a70/a39); - a156=casadi_sq(a92); - a156=(a17+a156); - a70=(a70/a156); - a70=(a51*a70); - a93=casadi_sq(a93); - a93=(a17+a93); - a70=(a70/a93); - a70=(a50*a70); - a70=(a94*a70); - a70=(a49*a70); - a153=(a153-a70); - a56=(a100*a158); - a153=(a153-a56); - a153=(a153/a20); - a56=(a65*a153); - a173=(a102*a56); - a175=sin(a89); - a170=(a175*a145); - a170=(a101*a170); - a173=(a173-a170); - a176=(a176+a173); - a173=(a4*a176); - a170=(a20*a56); - a170=(a95*a170); - a167=(a109*a167); - a167=(a31*a167); - a70=(a52*a70); - a167=(a167+a70); - a167=(a167+a17); - a167=(a167/a76); - a70=(a65*a167); - a177=(a117*a70); - a170=(a170+a177); - a177=(a149*a110); - a177=(a108*a177); - a178=(a31*a70); - a178=(a56+a178); - a178=(a178/a84); - a179=(a155*a110); - a178=(a178-a179); - a178=(a178/a112); - a178=(a30*a178); - a178=(a178/a113); - a178=(a29*a178); - a178=(a114*a178); - a178=(a28*a178); - a179=(a116*a178); - a177=(a177+a179); - a170=(a170-a177); - a170=(a170/a20); - a177=(a1*a170); - a179=(a124*a177); - a180=(a119*a110); - a181=sin(a89); - a182=(a181*a145); - a182=(a84*a182); - a180=(a180-a182); - a182=(a121*a56); + a79=(a16+a79); + a92=(a92/a79); + a92=(a30*a92); + a92=(a81*a92); + a92=(a29*a92); + a77=(a83*a92); + a94=sin(a80); + a95=(a94*a44); + a95=(a82*a95); + a77=(a77+a95); + a77=(a53-a77); + a95=(a85*a28); + a77=(a77+a95); + a77=(a77/a39); + a95=(a1*a77); + a96=(a86*a95); + a97=sin(a72); + a98=(a70*a38); + a98=(a98/a20); + a98=(a13*a98); + a28=(a28+a98); + a98=(a1*a28); + a99=(a97*a98); + a99=(a57*a99); + a96=(a96+a99); + a99=cos(a72); + a100=(a99*a98); + a100=(a6*a100); + a96=(a96-a100); + a38=(a66*a38); + a100=(a4*a38); + a96=(a96-a100); + a96=(a2*a96); + if (res[1]!=0) res[1][1]=a96; + a28=(a4*a28); + a92=(a71*a92); + a80=sin(a80); + a96=(a80*a44); + a96=(a82*a96); + a92=(a92+a96); + a92=(a32*a92); + a53=(a24*a53); + a92=(a92+a53); + a92=(a92/a47); + a53=(a1*a92); + a96=sin(a72); + a100=(a96*a98); + a100=(a6*a100); + a101=(a73*a95); + a72=cos(a72); + a98=(a72*a98); + a98=(a57*a98); + a101=(a101-a98); + a100=(a100-a101); + a100=(a100/a5); + a43=(a43/a5); + a38=(a1*a38); + a38=(a13*a38); + a38=(a43*a38); + a100=(a100-a38); + a100=(a13*a100); + a100=(a53+a100); + a28=(a28+a100); + a28=(a2*a28); + a28=(-a28); + if (res[1]!=0) res[1][2]=a28; + a77=(a4*a77); + a12=(a4*a12); + a77=(a77-a12); + a18=cos(a18); + a12=(a24*a53); + a12=(a95+a12); + a12=(a12/a6); + a84=casadi_sq(a84); + a84=(a16+a84); + a12=(a12/a84); + a12=(a23*a12); + a64=casadi_sq(a64); + a64=(a16+a64); + a12=(a12/a64); + a12=(a22*a12); + a12=(a18*a12); + a12=(a21*a12); a89=cos(a89); - a145=(a89*a145); - a145=(a101*a145); - a182=(a182+a145); - a180=(a180-a182); - a180=(a180/a48); - a180=(a40*a180); - a180=(a70-a180); - a182=(a1*a180); - a145=(a159*a182); - a145=(a118*a145); - a179=(a179+a145); - a145=(a52*a70); - a56=(a56-a145); - a56=(a56/a84); - a145=(a162*a110); - a56=(a56-a145); - a56=(a56/a126); - a56=(a51*a56); - a56=(a56/a127); - a56=(a50*a56); - a56=(a128*a56); - a56=(a49*a56); - a145=(a133*a178); - a145=(a56+a145); - a110=(a20*a110); - a110=(a95*a110); - a70=(a134*a70); - a110=(a110+a70); - a145=(a145-a110); - a145=(a145/a20); - a110=(a1*a145); - a70=(a136*a110); - a183=(a165*a182); - a183=(a135*a183); - a70=(a70-a183); - a179=(a179+a70); - a173=(a173+a179); - a173=(a2*a173); - if (res[1]!=0) res[1][12]=a173; - a158=(a4*a158); - a180=(a4*a180); - a158=(a158+a180); - a178=(a122*a178); - a178=(a31*a178); - a56=(a52*a56); - a178=(a178-a56); - a178=(a178+a17); - a178=(a178/a76); - a56=(a1*a178); - a180=(a125*a177); - a173=(a147*a182); - a173=(a118*a173); - a180=(a180-a173); - a173=(a138*a110); - a182=(a123*a182); - a182=(a135*a182); - a173=(a173+a182); - a180=(a180-a173); - a180=(a180/a7); - a176=(a1*a176); - a176=(a40*a176); - a176=(a137*a176); - a180=(a180+a176); - a180=(a40*a180); - a180=(a56-a180); - a158=(a158+a180); - a158=(a2*a158); - if (res[1]!=0) res[1][13]=a158; - a174=(a4*a174); - a170=(a4*a170); - a174=(a174+a170); - a170=(a20*a110); - a170=(a129*a170); - a158=(a45*a56); - a170=(a170+a158); - a158=(a143*a177); - a158=(a22*a158); - a180=(a31*a56); - a180=(a110+a180); - a180=(a180/a118); - a176=(a161*a177); - a180=(a180-a176); - a180=(a180/a27); - a180=(a30*a180); - a180=(a180/a26); - a180=(a29*a180); - a180=(a24*a180); - a180=(a28*a180); - a176=(a23*a180); - a158=(a158+a176); - a170=(a170-a158); - a170=(a170/a20); - a174=(a174+a170); - a174=(a2*a174); - if (res[1]!=0) res[1][14]=a174; - a153=(a4*a153); - a145=(a4*a145); - a153=(a153+a145); - a145=(a52*a56); - a110=(a110-a145); - a110=(a110/a118); - a145=(a164*a177); - a110=(a110-a145); - a110=(a110/a132); - a110=(a51*a110); - a110=(a110/a98); - a110=(a50*a110); - a110=(a19*a110); - a110=(a49*a110); - a145=(a141*a180); - a145=(a110+a145); - a177=(a20*a177); - a177=(a129*a177); - a56=(a142*a56); - a177=(a177+a56); - a145=(a145-a177); - a145=(a145/a20); - a153=(a153+a145); - a153=(a2*a153); - if (res[1]!=0) res[1][15]=a153; - a153=(1./a76); - a167=(a4*a167); - a153=(a153+a167); - a178=(a4*a178); - a153=(a153+a178); - a180=(a88*a180); - a180=(a31*a180); - a110=(a52*a110); - a180=(a180-a110); - a180=(a180+a17); - a180=(a180/a76); - a153=(a153+a180); - a153=(a2*a153); - if (res[1]!=0) res[1][16]=a153; - if (res[1]!=0) res[1][17]=a17; - a153=cos(a0); - a57=(a57*a153); + a28=(a32*a53); + a95=(a95-a28); + a95=(a95/a6); + a87=casadi_sq(a87); + a87=(a16+a87); + a95=(a95/a87); + a95=(a95-a1); + a95=(a31*a95); + a88=casadi_sq(a88); + a88=(a16+a88); + a95=(a95/a88); + a95=(a30*a95); + a95=(a89*a95); + a95=(a29*a95); + a28=(a91*a95); + a100=sin(a3); + a38=(a100*a1); + a38=(a90*a38); + a28=(a28-a38); + a28=(a12+a28); + a53=(a93*a53); + a28=(a28+a53); + a28=(a28/a39); + a77=(a77+a28); + a77=(a2*a77); + if (res[1]!=0) res[1][3]=a77; + a95=(a51*a95); + a3=sin(a3); + a77=(a3*a1); + a77=(a90*a77); + a95=(a95-a77); + a95=(a32*a95); + a12=(a24*a12); + a95=(a95-a12); + a95=(a95/a47); + a46=(a4*a46); + a92=(a4*a92); + a46=(a46+a92); + a95=(a95-a46); + a95=(a2*a95); + if (res[1]!=0) res[1][4]=a95; + a95=cos(a52); + a46=(a11/a47); + a92=(a44*a46); + a12=(a95*a92); + a12=(a6*a12); + a77=(a32*a46); + a77=(a77/a6); + a58=casadi_sq(a58); + a58=(a16+a58); + a77=(a77/a58); + a77=(a31*a77); + a77=(a77/a59); + a77=(a30*a77); + a77=(a61*a77); + a77=(a29*a77); + a28=(a63*a77); + a56=cos(a56); + a53=(a24*a46); + a53=(a53/a6); + a54=casadi_sq(a54); + a54=(a16+a54); + a53=(a53/a54); + a53=(a23*a53); + a55=casadi_sq(a55); + a55=(a16+a55); + a53=(a53/a55); + a53=(a22*a53); + a53=(a56*a53); + a53=(a21*a53); + a28=(a28-a53); + a38=(a65*a46); + a28=(a28-a38); + a28=(a28/a39); + a38=(a44*a28); + a101=(a66*a38); + a98=sin(a52); + a102=(a98*a92); + a102=(a48*a102); + a101=(a101-a102); + a12=(a12+a101); + a101=(a4*a12); + a77=(a67*a77); + a77=(a32*a77); + a53=(a24*a53); + a77=(a77+a53); + a77=(a77+a16); + a77=(a77/a47); + a53=(a44*a77); + a102=sin(a52); + a103=(a102*a92); + a103=(a6*a103); + a104=(a70*a38); + a52=cos(a52); + a92=(a52*a92); + a92=(a48*a92); + a104=(a104+a92); + a103=(a103+a104); + a103=(a103/a20); + a103=(a13*a103); + a103=(a53+a103); + a104=(a1*a103); + a92=(a99*a104); + a92=(a6*a92); + a105=(a24*a53); + a105=(a38-a105); + a105=(a105/a6); + a105=(a105/a74); + a105=(a23*a105); + a105=(a105/a75); + a105=(a22*a105); + a105=(a76*a105); + a105=(a21*a105); + a106=(a32*a53); + a38=(a38+a106); + a38=(a38/a6); + a38=(a38/a78); + a38=(a31*a38); + a38=(a38/a79); + a38=(a30*a38); + a38=(a81*a38); + a38=(a29*a38); + a106=(a83*a38); + a106=(a105+a106); + a53=(a85*a53); + a106=(a106-a53); + a106=(a106/a39); + a53=(a1*a106); + a107=(a86*a53); + a108=(a97*a104); + a108=(a57*a108); + a107=(a107-a108); + a92=(a92+a107); + a101=(a101+a92); + a101=(a2*a101); + if (res[1]!=0) res[1][5]=a101; + a46=(a4*a46); + a103=(a4*a103); + a46=(a46+a103); + a38=(a71*a38); + a38=(a32*a38); + a105=(a24*a105); + a38=(a38-a105); + a38=(a38+a16); + a38=(a38/a47); + a105=(a1*a38); + a12=(a1*a12); + a12=(a13*a12); + a12=(a43*a12); + a103=(a96*a104); + a103=(a6*a103); + a101=(a73*a53); + a104=(a72*a104); + a104=(a57*a104); + a101=(a101+a104); + a103=(a103+a101); + a103=(a103/a5); + a12=(a12-a103); + a12=(a13*a12); + a12=(a105-a12); + a46=(a46+a12); + a46=(a2*a46); + if (res[1]!=0) res[1][6]=a46; + a28=(a4*a28); + a106=(a4*a106); + a28=(a28+a106); + a106=(a24*a105); + a106=(a53-a106); + a106=(a106/a6); + a106=(a106/a84); + a106=(a23*a106); + a106=(a106/a64); + a106=(a22*a106); + a106=(a18*a106); + a106=(a21*a106); + a46=(a32*a105); + a53=(a53+a46); + a53=(a53/a6); + a53=(a53/a87); + a53=(a31*a53); + a53=(a53/a88); + a53=(a30*a53); + a53=(a89*a53); + a53=(a29*a53); + a46=(a91*a53); + a46=(a106+a46); + a105=(a93*a105); + a46=(a46-a105); + a46=(a46/a39); + a28=(a28+a46); + a28=(a2*a28); + if (res[1]!=0) res[1][7]=a28; + a28=(1./a47); + a77=(a4*a77); + a28=(a28+a77); + a38=(a4*a38); + a28=(a28+a38); + a53=(a51*a53); + a53=(a32*a53); + a106=(a24*a106); + a53=(a53-a106); + a53=(a53+a16); + a53=(a53/a47); + a28=(a28+a53); + a28=(a2*a28); + if (res[1]!=0) res[1][8]=a28; + if (res[1]!=0) res[1][9]=a16; a35=cos(a35); a34=casadi_sq(a34); - a34=(a17+a34); - a153=(a30/a34); - a153=(a29*a153); - a153=(a35*a153); - a153=(a28*a153); - a180=(a60*a153); - a110=sin(a0); - a110=(a36*a110); - a180=(a180+a110); - a57=(a57-a180); - a57=(a57/a20); - a180=(a14*a57); - a110=(a63*a180); - a178=sin(a0); - a16=(a16*a178); - a178=cos(a0); - a178=(a36*a178); - a167=(a37*a153); - a178=(a178-a167); - a16=(a16+a178); - a16=(a16/a20); - a178=(a14*a16); - a167=(a47*a178); - a110=(a110-a167); - a167=(a4*a110); - a145=(a39+a39); - a177=(a145*a178); - a177=(a71*a177); - a67=(a67*a144); - a177=(a177-a67); - a67=cos(a0); - a74=(a74*a67); - a153=(a72*a153); + a34=(a16+a34); + a28=(a31/a34); + a28=(a30*a28); + a28=(a35*a28); + a28=(a29*a28); + a53=(a45*a28); + a106=sin(a0); + a106=(a36*a106); + a53=(a53+a106); + a53=(a32*a53); + a53=(a53/a47); + a106=(a11*a53); + a38=(a24*a106); + a28=(a37*a28); a0=sin(a0); a36=(a36*a0); - a153=(a153+a36); - a74=(a74-a153); - a74=(a31*a74); - a74=(a74/a76); - a153=(a14*a74); - a36=(a31*a153); - a36=(a180+a36); - a36=(a36/a39); - a78=(a78/a39); - a0=(a78*a178); - a36=(a36+a0); - a36=(a36/a171); + a28=(a28+a36); + a28=(a28/a39); + a36=(a11*a28); + a38=(a38-a36); + a38=(a38/a6); + a38=(a38/a54); + a38=(a23*a38); + a38=(a38/a55); + a38=(a22*a38); + a38=(a56*a38); + a38=(a21*a38); a0=-1.; - a36=(a36+a0); - a36=(a30*a36); - a36=(a36/a79); - a36=(a29*a36); - a36=(a80*a36); - a36=(a28*a36); - a67=(a82*a36); - a146=(a81*a146); - a67=(a67+a146); - a177=(a177-a67); - a67=(a20*a180); - a67=(a77*a67); - a146=(a83*a153); - a67=(a67+a146); - a177=(a177+a67); - a177=(a177/a20); - a67=(a65*a177); - a146=(a90*a67); - a144=(a85*a178); - a56=(a87*a180); - a144=(a144+a56); - a144=(a144/a11); - a144=(a40*a144); - a144=(a153+a144); - a56=(a65*a144); - a174=(a172*a56); - a174=(a84*a174); - a146=(a146+a174); - a174=(a52*a153); - a180=(a180-a174); - a180=(a180/a39); - a92=(a92/a39); - a174=(a92*a178); - a180=(a180+a174); - a180=(a180/a156); - a180=(a51*a180); - a180=(a180/a93); - a180=(a50*a180); - a180=(a94*a180); - a180=(a49*a180); - a96=(a96*a73); - a96=(a180+a96); - a73=(a99*a36); - a140=(a81*a140); - a73=(a73-a140); - a96=(a96+a73); - a153=(a100*a153); - a178=(a20*a178); - a178=(a77*a178); - a153=(a153-a178); - a96=(a96-a153); - a96=(a96/a20); - a153=(a65*a96); - a178=(a102*a153); - a73=(a175*a56); - a73=(a101*a73); - a178=(a178-a73); - a146=(a146+a178); - a178=(a4*a146); - a167=(a167+a178); - a178=(a20*a153); - a178=(a95*a178); - a36=(a109*a36); - a81=(a81*a68); - a36=(a36-a81); - a66=(a66*a152); - a36=(a36+a66); - a36=(a31*a36); - a180=(a52*a180); - a36=(a36-a180); - a36=(a36/a76); - a180=(a65*a36); - a66=(a117*a180); - a178=(a178+a66); - a104=(a104*a151); - a151=(a149*a67); - a151=(a108*a151); - a104=(a104+a151); - a151=(a31*a180); - a151=(a153+a151); - a151=(a151/a84); - a66=(a155*a67); - a151=(a151-a66); - a151=(a151/a112); - a151=(a151+a0); - a151=(a30*a151); - a151=(a151/a113); - a151=(a29*a151); - a151=(a114*a151); - a151=(a28*a151); - a66=(a116*a151); - a157=(a115*a157); - a66=(a66+a157); - a104=(a104+a66); - a178=(a178-a104); - a178=(a178/a20); - a104=(a1*a178); - a66=(a124*a104); - a157=(a119*a67); - a152=(a181*a56); - a152=(a84*a152); - a157=(a157-a152); - a152=(a121*a153); - a56=(a89*a56); - a56=(a101*a56); - a152=(a152+a56); - a157=(a157-a152); - a157=(a157/a48); - a120=(a120/a48); - a110=(a65*a110); - a110=(a40*a110); - a110=(a120*a110); - a157=(a157+a110); - a157=(a40*a157); - a157=(a180-a157); - a110=(a1*a157); - a152=(a159*a110); - a152=(a118*a152); - a66=(a66+a152); - a152=(a52*a180); - a153=(a153-a152); - a153=(a153/a84); - a152=(a162*a67); - a153=(a153-a152); - a153=(a153/a126); - a153=(a51*a153); - a153=(a153/a127); - a153=(a50*a153); - a153=(a128*a153); - a153=(a49*a153); - a130=(a130*a163); - a130=(a153+a130); - a163=(a133*a151); - a166=(a115*a166); - a163=(a163-a166); - a130=(a130+a163); - a67=(a20*a67); - a67=(a95*a67); - a180=(a134*a180); - a67=(a67+a180); - a130=(a130-a67); - a130=(a130/a20); - a67=(a1*a130); - a180=(a136*a67); - a163=(a165*a110); - a163=(a135*a163); - a180=(a180-a163); - a66=(a66+a180); - a167=(a167+a66); - a167=(a2*a167); - if (res[1]!=0) res[1][18]=a167; - a144=(a4*a144); - a157=(a4*a157); - a144=(a144+a157); - a151=(a122*a151); - a115=(a115*a148); - a151=(a151-a115); - a103=(a103*a105); - a151=(a151+a103); - a151=(a31*a151); - a153=(a52*a153); - a151=(a151-a153); - a151=(a151/a76); - a153=(a1*a151); - a103=(a125*a104); - a105=(a147*a110); - a105=(a118*a105); - a103=(a103-a105); - a105=(a138*a67); - a110=(a123*a110); - a110=(a135*a110); - a105=(a105+a110); - a103=(a103-a105); - a103=(a103/a7); - a146=(a1*a146); - a146=(a40*a146); - a146=(a137*a146); - a103=(a103+a146); - a103=(a40*a103); - a103=(a153-a103); - a144=(a144+a103); - a144=(a2*a144); - if (res[1]!=0) res[1][19]=a144; - a177=(a4*a177); - a177=(a177-a16); - a178=(a4*a178); - a177=(a177+a178); - a178=(a20*a67); - a178=(a129*a178); - a16=(a45*a153); - a178=(a178+a16); - a6=(a6*a160); - a160=(a143*a104); - a160=(a22*a160); - a6=(a6+a160); - a160=(a31*a153); - a160=(a67+a160); - a160=(a160/a118); - a16=(a161*a104); - a160=(a160-a16); - a160=(a160/a27); - a160=(a160+a0); - a160=(a30*a160); - a160=(a160/a26); - a160=(a29*a160); - a160=(a24*a160); - a160=(a28*a160); - a0=(a23*a160); - a169=(a21*a169); - a0=(a0+a169); - a6=(a6+a0); - a178=(a178-a6); - a178=(a178/a20); - a177=(a177+a178); - a177=(a2*a177); - if (res[1]!=0) res[1][20]=a177; - a96=(a4*a96); - a57=(a57+a96); - a130=(a4*a130); - a57=(a57+a130); - a130=(a52*a153); - a67=(a67-a130); - a67=(a67/a118); - a130=(a164*a104); - a67=(a67-a130); - a67=(a67/a132); - a67=(a51*a67); - a67=(a67/a98); - a67=(a50*a67); - a67=(a19*a67); - a67=(a49*a67); - a91=(a91*a150); - a91=(a67+a91); - a150=(a141*a160); - a168=(a21*a168); - a150=(a150-a168); - a91=(a91+a150); - a104=(a20*a104); - a104=(a129*a104); - a153=(a142*a153); - a104=(a104+a153); - a91=(a91-a104); - a91=(a91/a20); - a57=(a57+a91); - a57=(a2*a57); - if (res[1]!=0) res[1][21]=a57; - a36=(a4*a36); - a74=(a74+a36); - a151=(a4*a151); - a74=(a74+a151); - a160=(a88*a160); - a21=(a21*a154); - a160=(a160-a21); - a5=(a5*a3); - a160=(a160+a5); - a160=(a31*a160); - a67=(a52*a67); - a160=(a160-a67); - a160=(a160/a76); - a74=(a74+a160); - a74=(a2*a74); - if (res[1]!=0) res[1][22]=a74; - if (res[1]!=0) res[1][23]=a17; - a18=(a18*a15); - a18=(a18/a20); - a74=(a14*a18); - a160=(a47*a74); - a58=(a58*a15); - a58=(a58/a20); - a67=(a14*a58); - a5=(a63*a67); - a160=(a160+a5); - a5=(a4*a160); - a69=(a69*a15); - a3=(a145*a74); - a3=(a71*a3); - a69=(a69-a3); - a75=(a75*a15); - a75=(a31*a75); - a75=(a75/a76); - a3=(a14*a75); - a21=(a31*a3); - a21=(a67+a21); - a21=(a21/a39); - a154=(a78*a74); - a21=(a21-a154); - a21=(a21/a171); - a21=(a30*a21); - a21=(a21/a79); - a21=(a29*a21); - a21=(a80*a21); - a21=(a28*a21); - a154=(a82*a21); - a69=(a69-a154); - a154=(a20*a67); - a154=(a77*a154); - a151=(a83*a3); - a154=(a154+a151); - a69=(a69+a154); + a77=(a32*a106); + a77=(a36+a77); + a77=(a77/a6); + a77=(a77/a58); + a77=(a0-a77); + a77=(a31*a77); + a77=(a77/a59); + a77=(a30*a77); + a77=(a61*a77); + a77=(a29*a77); + a46=(a63*a77); + a60=(a62*a60); + a46=(a46-a60); + a46=(a38+a46); + a60=(a65*a106); + a46=(a46+a60); + a46=(a46/a39); + a60=(a44*a46); + a105=(a66*a60); + a12=(a50*a36); + a12=(a12/a8); + a12=(a13*a12); + a106=(a106+a12); + a12=(a44*a106); + a103=(a98*a12); + a103=(a48*a103); + a105=(a105+a103); + a103=(a95*a12); + a103=(a6*a103); + a105=(a105-a103); + a103=(a4*a105); + a36=(a42*a36); + a101=(a4*a36); + a103=(a103-a101); + a77=(a67*a77); + a62=(a62*a68); + a77=(a77-a62); + a77=(a32*a77); + a38=(a24*a38); + a77=(a77-a38); + a77=(a77/a47); + a38=(a44*a77); + a62=(a102*a12); + a62=(a6*a62); + a68=(a70*a60); + a12=(a52*a12); + a12=(a48*a12); + a68=(a68-a12); + a62=(a62-a68); + a62=(a62/a20); a69=(a69/a20); - a154=(a65*a69); - a151=(a90*a154); - a36=(a85*a74); - a57=(a87*a67); - a36=(a36-a57); - a36=(a36/a11); - a36=(a40*a36); - a36=(a3-a36); - a57=(a65*a36); - a91=(a172*a57); - a91=(a84*a91); - a151=(a151+a91); - a91=(a52*a3); - a67=(a67-a91); - a67=(a67/a39); - a91=(a92*a74); - a67=(a67-a91); - a67=(a67/a156); - a67=(a51*a67); - a67=(a67/a93); - a67=(a50*a67); - a67=(a94*a67); - a67=(a49*a67); - a97=(a97*a15); - a97=(a67+a97); - a91=(a99*a21); - a97=(a97+a91); - a74=(a20*a74); - a74=(a77*a74); - a3=(a100*a3); - a74=(a74+a3); - a97=(a97-a74); - a97=(a97/a20); - a74=(a65*a97); - a3=(a102*a74); - a91=(a175*a57); - a91=(a101*a91); - a3=(a3-a91); - a151=(a151+a3); - a3=(a4*a151); - a5=(a5+a3); - a106=(a106*a15); - a3=(a149*a154); - a3=(a108*a3); - a106=(a106-a3); - a21=(a109*a21); - a111=(a111*a15); - a21=(a21+a111); - a21=(a31*a21); - a67=(a52*a67); - a21=(a21-a67); - a21=(a21/a76); - a67=(a65*a21); - a111=(a31*a67); - a111=(a74+a111); - a111=(a111/a84); - a3=(a155*a154); - a111=(a111-a3); - a111=(a111/a112); - a111=(a30*a111); - a111=(a111/a113); - a111=(a29*a111); - a111=(a114*a111); - a111=(a28*a111); - a3=(a116*a111); - a106=(a106-a3); - a3=(a20*a74); - a3=(a95*a3); - a91=(a117*a67); - a3=(a3+a91); - a106=(a106+a3); - a106=(a106/a20); - a3=(a1*a106); - a91=(a124*a3); - a104=(a119*a154); - a153=(a181*a57); - a153=(a84*a153); - a104=(a104-a153); - a153=(a121*a74); - a57=(a89*a57); - a57=(a101*a57); - a153=(a153+a57); - a104=(a104-a153); - a104=(a104/a48); - a160=(a65*a160); - a160=(a40*a160); - a160=(a120*a160); - a104=(a104+a160); - a104=(a40*a104); - a104=(a67-a104); - a160=(a1*a104); - a153=(a159*a160); - a153=(a118*a153); - a91=(a91+a153); - a153=(a52*a67); - a74=(a74-a153); - a74=(a74/a84); - a153=(a162*a154); - a74=(a74-a153); - a74=(a74/a126); - a74=(a51*a74); - a74=(a74/a127); - a74=(a50*a74); - a74=(a128*a74); - a74=(a49*a74); - a131=(a131*a15); - a131=(a74+a131); - a153=(a133*a111); - a131=(a131+a153); - a154=(a20*a154); - a154=(a95*a154); - a67=(a134*a67); - a154=(a154+a67); - a131=(a131-a154); - a131=(a131/a20); - a154=(a1*a131); - a67=(a136*a154); - a153=(a165*a160); - a153=(a135*a153); - a67=(a67-a153); - a91=(a91+a67); - a5=(a5+a91); - a5=(a2*a5); - if (res[1]!=0) res[1][24]=a5; - a36=(a4*a36); - a104=(a4*a104); - a36=(a36+a104); - a111=(a122*a111); - a64=(a64*a15); - a111=(a111+a64); - a111=(a31*a111); - a74=(a52*a74); - a111=(a111-a74); - a111=(a111/a76); - a74=(a1*a111); - a64=(a125*a3); - a104=(a147*a160); - a104=(a118*a104); - a64=(a64-a104); - a104=(a138*a154); - a160=(a123*a160); - a160=(a135*a160); - a104=(a104+a160); - a64=(a64-a104); - a64=(a64/a7); - a151=(a1*a151); - a151=(a40*a151); - a151=(a137*a151); - a64=(a64+a151); - a64=(a40*a64); - a64=(a74-a64); - a36=(a36+a64); - a36=(a2*a36); - if (res[1]!=0) res[1][25]=a36; - a69=(a4*a69); - a18=(a18+a69); + a36=(a44*a36); + a36=(a13*a36); + a36=(a69*a36); + a62=(a62-a36); + a62=(a13*a62); + a62=(a38-a62); + a36=(a1*a62); + a68=(a99*a36); + a68=(a6*a68); + a12=(a24*a38); + a12=(a60-a12); + a12=(a12/a6); + a12=(a12/a74); + a12=(a23*a12); + a12=(a12/a75); + a12=(a22*a12); + a12=(a76*a12); + a12=(a21*a12); + a101=(a32*a38); + a60=(a60+a101); + a60=(a60/a6); + a60=(a60/a78); + a60=(a60+a0); + a60=(a31*a60); + a60=(a60/a79); + a60=(a30*a60); + a60=(a81*a60); + a60=(a29*a60); + a101=(a83*a60); + a94=(a82*a94); + a101=(a101-a94); + a101=(a12+a101); + a38=(a85*a38); + a101=(a101-a38); + a101=(a101/a39); + a38=(a1*a101); + a94=(a86*a38); + a104=(a97*a36); + a104=(a57*a104); + a94=(a94-a104); + a68=(a68+a94); + a103=(a103+a68); + a103=(a2*a103); + if (res[1]!=0) res[1][10]=a103; + a62=(a4*a62); a106=(a4*a106); - a18=(a18+a106); - a107=(a107*a15); - a106=(a143*a3); - a106=(a22*a106); - a107=(a107-a106); - a106=(a31*a74); - a106=(a154+a106); - a106=(a106/a118); - a69=(a161*a3); - a106=(a106-a69); - a106=(a106/a27); - a106=(a30*a106); - a106=(a106/a26); - a106=(a29*a106); - a106=(a24*a106); - a106=(a28*a106); - a69=(a23*a106); - a107=(a107-a69); - a69=(a20*a154); - a69=(a129*a69); - a36=(a45*a74); - a69=(a69+a36); - a107=(a107+a69); - a107=(a107/a20); - a18=(a18+a107); - a18=(a2*a18); - if (res[1]!=0) res[1][26]=a18; - a97=(a4*a97); - a58=(a58+a97); - a131=(a4*a131); - a58=(a58+a131); - a131=(a52*a74); - a154=(a154-a131); - a154=(a154/a118); - a131=(a164*a3); - a154=(a154-a131); - a154=(a154/a132); - a154=(a51*a154); - a154=(a154/a98); - a154=(a50*a154); - a154=(a19*a154); - a154=(a49*a154); - a139=(a139*a15); - a139=(a154+a139); - a131=(a141*a106); - a139=(a139+a131); - a3=(a20*a3); - a3=(a129*a3); - a74=(a142*a74); - a3=(a3+a74); - a139=(a139-a3); - a139=(a139/a20); - a58=(a58+a139); - a58=(a2*a58); - if (res[1]!=0) res[1][27]=a58; - a21=(a4*a21); - a75=(a75+a21); - a111=(a4*a111); - a75=(a75+a111); - a106=(a88*a106); - a59=(a59*a15); - a106=(a106+a59); - a106=(a31*a106); - a154=(a52*a154); - a106=(a106-a154); - a106=(a106/a76); - a75=(a75+a106); - a75=(a2*a75); - if (res[1]!=0) res[1][28]=a75; - a75=sin(a46); - a42=(a42/a44); - a42=(a42*a40); - a42=(a40*a42); - a106=(a14*a42); - a154=(a75*a106); - a154=(a62*a154); - a59=cos(a46); - a15=(a59*a106); - a15=(a39*a15); - a154=(a154-a15); - a15=(a4*a154); - a111=sin(a46); - a21=(a111*a106); - a21=(a39*a21); - a46=cos(a46); - a106=(a46*a106); - a106=(a62*a106); - a21=(a21+a106); - a21=(a21/a11); - a86=(a86/a11); - a106=(a86*a40); - a21=(a21+a106); - a21=(a40*a21); - a106=(a65*a21); - a58=(a175*a106); - a58=(a101*a58); - a139=(a172*a106); - a139=(a84*a139); - a58=(a58-a139); - a139=(a4*a58); - a15=(a15+a139); - a139=(a181*a106); - a139=(a84*a139); - a106=(a89*a106); - a106=(a101*a106); - a139=(a139+a106); - a139=(a139/a48); - a154=(a65*a154); - a154=(a17+a154); - a154=(a40*a154); - a154=(a120*a154); - a139=(a139+a154); - a139=(a40*a139); - a154=(a1*a139); - a106=(a165*a154); - a106=(a135*a106); - a3=(a159*a154); - a3=(a118*a3); - a106=(a106-a3); - a15=(a15+a106); - a15=(a2*a15); - a15=(a17+a15); - if (res[1]!=0) res[1][29]=a15; - a21=(a4*a21); - a42=(a42+a21); - a139=(a4*a139); - a42=(a42+a139); - a139=(a147*a154); - a139=(a118*a139); - a154=(a123*a154); - a154=(a135*a154); - a139=(a139+a154); - a139=(a139/a7); - a58=(a1*a58); - a58=(a17+a58); - a58=(a40*a58); - a58=(a137*a58); - a139=(a139+a58); - a139=(a40*a139); - a42=(a42+a139); - a42=(a2*a42); - a42=(-a42); - if (res[1]!=0) res[1][30]=a42; - a42=cos(a9); - a42=(a8*a42); - a139=sin(a9); - a139=(a12*a139); - a42=(a42-a139); - a139=sin(a9); - a139=(a8*a139); - a9=cos(a9); - a12=(a12*a9); - a139=(a139+a12); - a139=(a139/a44); - a139=(a40*a139); - a12=(a14*a139); - a12=(a17+a12); - a9=(a59*a12); - a9=(a39*a9); - a58=(a75*a12); - a58=(a62*a58); - a9=(a9-a58); - a58=(a4*a9); - a58=(a42+a58); - a42=(a14*a42); - a42=(a40*a42); - a42=(a86*a42); - a154=(a111*a12); - a154=(a39*a154); - a12=(a46*a12); - a12=(a62*a12); - a154=(a154+a12); - a154=(a154/a11); - a42=(a42-a154); - a42=(a40*a42); - a154=(a65*a42); - a154=(a17-a154); - a12=(a172*a154); - a12=(a84*a12); - a21=(a175*a154); - a21=(a101*a21); - a12=(a12-a21); - a21=(a4*a12); - a58=(a58+a21); - a9=(a65*a9); - a9=(a40*a9); - a9=(a120*a9); - a21=(a181*a154); - a21=(a84*a21); - a154=(a89*a154); - a154=(a101*a154); - a21=(a21+a154); - a21=(a21/a48); - a9=(a9-a21); - a9=(a40*a9); - a21=(a1*a9); - a21=(a17-a21); - a154=(a159*a21); - a154=(a118*a154); - a15=(a165*a21); - a15=(a135*a15); - a154=(a154-a15); - a58=(a58+a154); - a58=(a2*a58); - if (res[1]!=0) res[1][31]=a58; - a42=(a4*a42); - a139=(a139-a42); - a9=(a4*a9); - a139=(a139-a9); - a12=(a1*a12); - a12=(a40*a12); - a12=(a137*a12); - a9=(a147*a21); - a9=(a118*a9); - a21=(a123*a21); - a21=(a135*a21); - a9=(a9+a21); - a9=(a9/a7); - a12=(a12-a9); - a12=(a40*a12); - a139=(a139-a12); - a139=(a2*a139); - a139=(a17+a139); - if (res[1]!=0) res[1][32]=a139; - a139=(a33/a8); + a62=(a62-a106); + a60=(a71*a60); + a82=(a82*a80); + a60=(a60-a82); + a60=(a32*a60); + a12=(a24*a12); + a60=(a60-a12); + a60=(a60/a47); + a12=(a1*a60); + a105=(a1*a105); + a105=(a13*a105); + a105=(a43*a105); + a82=(a96*a36); + a82=(a6*a82); + a80=(a73*a38); + a36=(a72*a36); + a36=(a57*a36); + a80=(a80+a36); + a82=(a82+a80); + a82=(a82/a5); + a105=(a105-a82); + a105=(a13*a105); + a105=(a12-a105); + a62=(a62+a105); + a62=(a2*a62); + if (res[1]!=0) res[1][11]=a62; + a46=(a4*a46); + a46=(a46-a28); + a101=(a4*a101); + a46=(a46+a101); + a101=(a24*a12); + a101=(a38-a101); + a101=(a101/a6); + a101=(a101/a84); + a101=(a23*a101); + a101=(a101/a64); + a101=(a22*a101); + a101=(a18*a101); + a101=(a21*a101); + a28=(a32*a12); + a38=(a38+a28); + a38=(a38/a6); + a38=(a38/a87); + a38=(a38+a0); + a38=(a31*a38); + a38=(a38/a88); + a38=(a30*a38); + a38=(a89*a38); + a38=(a29*a38); + a0=(a91*a38); + a100=(a90*a100); + a0=(a0-a100); + a0=(a101+a0); + a12=(a93*a12); + a0=(a0-a12); + a0=(a0/a39); + a46=(a46+a0); + a46=(a2*a46); + if (res[1]!=0) res[1][12]=a46; + a77=(a4*a77); + a77=(a77-a53); + a60=(a4*a60); + a77=(a77+a60); + a38=(a51*a38); + a90=(a90*a3); + a38=(a38-a90); + a38=(a32*a38); + a101=(a24*a101); + a38=(a38-a101); + a38=(a38/a47); + a77=(a77+a38); + a77=(a2*a77); + if (res[1]!=0) res[1][13]=a77; + a77=sin(a19); + a14=(a14/a17); + a14=(a14*a13); + a14=(a13*a14); + a38=(a11*a14); + a101=(a77*a38); + a101=(a41*a101); + a90=cos(a19); + a3=(a90*a38); + a3=(a6*a3); + a101=(a101-a3); + a3=(a4*a101); + a60=sin(a19); + a53=(a60*a38); + a53=(a6*a53); + a19=cos(a19); + a38=(a19*a38); + a38=(a41*a38); + a53=(a53+a38); + a53=(a53/a8); + a49=(a49/a8); + a38=(a49*a13); + a53=(a53+a38); + a53=(a13*a53); + a38=(a44*a53); + a46=(a98*a38); + a46=(a48*a46); + a0=(a95*a38); + a0=(a6*a0); + a46=(a46-a0); + a0=(a4*a46); + a3=(a3+a0); + a0=(a102*a38); + a0=(a6*a0); + a38=(a52*a38); + a38=(a48*a38); + a0=(a0+a38); + a0=(a0/a20); + a101=(a44*a101); + a101=(a16+a101); + a101=(a13*a101); + a101=(a69*a101); + a0=(a0+a101); + a0=(a13*a0); + a101=(a1*a0); + a38=(a97*a101); + a38=(a57*a38); + a12=(a99*a101); + a12=(a6*a12); + a38=(a38-a12); + a3=(a3+a38); + a3=(a2*a3); + a3=(a16+a3); + if (res[1]!=0) res[1][14]=a3; + a53=(a4*a53); + a14=(a14+a53); + a0=(a4*a0); + a14=(a14+a0); + a0=(a96*a101); + a0=(a6*a0); + a101=(a72*a101); + a101=(a57*a101); + a0=(a0+a101); + a0=(a0/a5); + a46=(a1*a46); + a46=(a16+a46); + a46=(a13*a46); + a46=(a43*a46); + a0=(a0+a46); + a0=(a13*a0); + a14=(a14+a0); + a14=(a2*a14); + a14=(-a14); + if (res[1]!=0) res[1][15]=a14; + a14=cos(a7); + a14=(a6*a14); + a0=sin(a7); + a0=(a9*a0); + a14=(a14-a0); + a0=sin(a7); + a0=(a6*a0); + a7=cos(a7); + a9=(a9*a7); + a0=(a0+a9); + a0=(a0/a17); + a0=(a13*a0); + a9=(a11*a0); + a9=(a16+a9); + a7=(a90*a9); + a7=(a6*a7); + a46=(a77*a9); + a46=(a41*a46); + a7=(a7-a46); + a46=(a4*a7); + a46=(a14+a46); + a14=(a11*a14); + a14=(a13*a14); + a14=(a49*a14); + a101=(a60*a9); + a101=(a6*a101); + a9=(a19*a9); + a9=(a41*a9); + a101=(a101+a9); + a101=(a101/a8); + a14=(a14-a101); + a14=(a13*a14); + a101=(a44*a14); + a101=(a16-a101); + a9=(a95*a101); + a9=(a6*a9); + a53=(a98*a101); + a53=(a48*a53); + a9=(a9-a53); + a53=(a4*a9); + a46=(a46+a53); + a7=(a44*a7); + a7=(a13*a7); + a7=(a69*a7); + a53=(a102*a101); + a53=(a6*a53); + a101=(a52*a101); + a101=(a48*a101); + a53=(a53+a101); + a53=(a53/a20); + a7=(a7-a53); + a7=(a13*a7); + a53=(a1*a7); + a53=(a16-a53); + a101=(a99*a53); + a101=(a6*a101); + a3=(a97*a53); + a3=(a57*a3); + a101=(a101-a3); + a46=(a46+a101); + a46=(a2*a46); + if (res[1]!=0) res[1][16]=a46; + a14=(a4*a14); + a0=(a0-a14); + a7=(a4*a7); + a0=(a0-a7); + a9=(a1*a9); + a9=(a13*a9); + a9=(a43*a9); + a7=(a96*a53); + a7=(a6*a7); + a53=(a72*a53); + a53=(a57*a53); + a7=(a7+a53); + a7=(a7/a5); + a9=(a9-a7); + a9=(a13*a9); + a0=(a0-a9); + a0=(a2*a0); + a0=(a16+a0); + if (res[1]!=0) res[1][17]=a0; + a15=(a15/a17); + a15=(a13*a15); + a17=(a11*a15); + a0=(a90*a17); + a0=(a6*a0); + a27=cos(a27); + a25=casadi_sq(a25); + a25=(a16+a25); + a9=(1./a25); + a9=(a9/a6); + a9=(a23*a9); + a26=casadi_sq(a26); + a26=(a16+a26); + a9=(a9/a26); + a9=(a22*a9); + a9=(a27*a9); + a9=(a21*a9); a33=casadi_sq(a33); - a33=(a17+a33); - a139=(a139/a33); - a139=(a30*a139); - a139=(a139/a34); - a139=(a29*a139); - a139=(a35*a139); - a139=(a28*a139); - a12=(a37*a139); - a9=(a8+a8); - a25=(a25*a9); - a12=(a12-a25); - a12=(a12/a20); - a25=(a14*a12); - a25=(a17+a25); - a9=(a47*a25); - a41=(a41/a44); - a41=(a40*a41); - a21=(a14*a41); - a42=(a59*a21); - a42=(a39*a42); - a9=(a9-a42); - a42=(a75*a21); - a42=(a62*a42); - a55=cos(a55); - a58=(a53/a8); - a53=casadi_sq(a53); - a53=(a17+a53); - a58=(a58/a53); - a58=(a51*a58); - a54=casadi_sq(a54); - a54=(a17+a54); - a58=(a58/a54); - a58=(a50*a58); - a58=(a55*a58); - a58=(a49*a58); - a154=(a60*a139); - a154=(a58+a154); - a15=(a32*a20); - a154=(a154+a15); - a154=(a154/a20); - a15=(a14*a154); - a106=(a63*a15); - a42=(a42-a106); - a9=(a9+a42); - a42=(a4*a9); - a42=(a10+a42); - a58=(a52*a58); - a139=(a72*a139); - a139=(a31*a139); - a58=(a58-a139); - a58=(a58/a76); - a139=(a14*a58); - a106=(a83*a139); - a3=(a20*a15); - a3=(a77*a3); - a106=(a106-a3); - a3=(a145*a25); - a3=(a71*a3); - a74=(a31*a139); - a74=(a74-a15); - a74=(a74/a39); - a131=(a78*a25); - a74=(a74-a131); - a74=(a74/a171); - a74=(a30*a74); - a74=(a74/a79); - a74=(a29*a74); - a74=(a80*a74); - a74=(a28*a74); - a131=(a82*a74); - a3=(a3+a131); - a106=(a106-a3); - a106=(a106/a20); - a3=(a65*a106); + a33=(a16+a33); + a7=(1./a33); + a7=(a7/a6); + a7=(a31*a7); + a7=(a7/a34); + a7=(a30*a7); + a7=(a35*a7); + a7=(a29*a7); + a53=(a37*a7); + a53=(a9+a53); + a53=(a53/a39); + a14=(a11*a53); + a14=(a16+a14); + a46=(a42*a14); + a101=(a77*a17); + a101=(a41*a101); + a46=(a46-a101); + a0=(a0+a46); + a46=(a4*a0); + a46=(a10+a46); + a7=(a45*a7); + a7=(a32*a7); + a9=(a24*a9); + a7=(a7-a9); + a7=(a7/a47); + a9=(a11*a7); + a10=(a11*a10); + a10=(a13*a10); + a49=(a49*a10); + a10=(a60*a17); + a10=(a6*a10); + a101=(a50*a14); + a17=(a19*a17); + a17=(a41*a17); + a101=(a101+a17); + a10=(a10+a101); + a10=(a10/a8); + a49=(a49-a10); + a49=(a13*a49); + a49=(a9-a49); + a10=(a44*a49); + a101=(a95*a10); + a101=(a6*a101); + a17=(a24*a9); + a17=(a14-a17); + a17=(a17/a6); + a17=(a17/a54); + a17=(a23*a17); + a17=(a17/a55); + a17=(a22*a17); + a17=(a56*a17); + a17=(a21*a17); + a3=(a32*a9); + a14=(a14+a3); + a14=(a14/a6); + a14=(a14/a58); + a14=(a31*a14); + a14=(a14/a59); + a14=(a30*a14); + a14=(a61*a14); + a14=(a29*a14); + a3=(a63*a14); a3=(a17+a3); - a131=(a90*a3); - a97=(a85*a25); - a18=(a111*a21); - a18=(a39*a18); - a97=(a97+a18); - a18=(a87*a15); - a21=(a46*a21); - a21=(a62*a21); - a18=(a18+a21); - a97=(a97+a18); - a97=(a97/a11); - a10=(a14*a10); - a10=(a40*a10); - a10=(a86*a10); - a97=(a97+a10); - a97=(a40*a97); - a97=(a139-a97); - a10=(a65*a97); - a18=(a172*a10); - a18=(a84*a18); - a131=(a131+a18); - a18=(a99*a74); - a21=(a52*a139); - a15=(a15+a21); - a15=(a15/a39); - a21=(a92*a25); - a15=(a15+a21); - a15=(a15/a156); - a15=(a51*a15); - a15=(a15/a93); - a15=(a50*a15); - a15=(a94*a15); - a15=(a49*a15); - a18=(a18-a15); - a25=(a20*a25); - a25=(a77*a25); - a139=(a100*a139); - a25=(a25+a139); - a18=(a18-a25); - a18=(a18/a20); - a25=(a65*a18); - a139=(a102*a25); - a21=(a175*a10); - a21=(a101*a21); - a139=(a139-a21); - a131=(a131+a139); - a139=(a4*a131); - a42=(a42+a139); - a139=(a20*a25); - a139=(a95*a139); - a74=(a109*a74); - a74=(a31*a74); - a15=(a52*a15); - a74=(a74+a15); - a74=(a74/a76); - a15=(a65*a74); - a21=(a117*a15); - a139=(a139+a21); - a21=(a149*a3); - a21=(a108*a21); - a107=(a31*a15); - a107=(a25+a107); - a107=(a107/a84); - a69=(a155*a3); - a107=(a107-a69); - a107=(a107/a112); - a107=(a30*a107); - a107=(a107/a113); - a107=(a29*a107); - a107=(a114*a107); - a107=(a28*a107); - a69=(a116*a107); - a21=(a21+a69); - a139=(a139-a21); - a139=(a139/a20); - a21=(a1*a139); - a21=(a17+a21); - a69=(a124*a21); - a36=(a119*a3); - a64=(a181*a10); - a64=(a84*a64); - a36=(a36-a64); - a64=(a121*a25); - a10=(a89*a10); - a10=(a101*a10); - a64=(a64+a10); - a36=(a36-a64); - a36=(a36/a48); a9=(a65*a9); - a9=(a40*a9); - a9=(a120*a9); - a36=(a36+a9); - a36=(a40*a36); - a36=(a15-a36); - a9=(a1*a36); - a64=(a159*a9); - a64=(a118*a64); - a69=(a69+a64); - a64=(a52*a15); - a25=(a25-a64); - a25=(a25/a84); - a64=(a162*a3); - a25=(a25-a64); - a25=(a25/a126); - a25=(a51*a25); - a25=(a25/a127); - a25=(a50*a25); - a25=(a128*a25); - a25=(a49*a25); - a64=(a133*a107); - a64=(a25+a64); - a3=(a20*a3); - a3=(a95*a3); - a15=(a134*a15); - a3=(a3+a15); - a64=(a64-a3); - a64=(a64/a20); - a3=(a1*a64); - a15=(a136*a3); - a10=(a165*a9); - a10=(a135*a10); - a15=(a15-a10); - a69=(a69+a15); - a42=(a42+a69); - a42=(a2*a42); - if (res[1]!=0) res[1][33]=a42; - a97=(a4*a97); - a97=(a97-a41); - a36=(a4*a36); - a97=(a97+a36); - a107=(a122*a107); - a107=(a31*a107); - a25=(a52*a25); - a107=(a107-a25); - a107=(a107/a76); - a25=(a1*a107); - a36=(a125*a21); - a41=(a147*a9); - a41=(a118*a41); - a36=(a36-a41); - a41=(a138*a3); - a9=(a123*a9); - a9=(a135*a9); - a41=(a41+a9); - a36=(a36-a41); - a36=(a36/a7); - a131=(a1*a131); - a131=(a40*a131); - a131=(a137*a131); - a36=(a36+a131); - a36=(a40*a36); - a36=(a25-a36); - a97=(a97+a36); - a97=(a2*a97); - if (res[1]!=0) res[1][34]=a97; - a106=(a4*a106); - a12=(a12+a106); - a139=(a4*a139); - a12=(a12+a139); - a139=(a20*a3); - a139=(a129*a139); - a106=(a45*a25); - a139=(a139+a106); - a106=(a143*a21); - a106=(a22*a106); - a97=(a31*a25); - a97=(a3+a97); - a97=(a97/a118); - a36=(a161*a21); - a97=(a97-a36); - a97=(a97/a27); - a97=(a30*a97); - a97=(a97/a26); - a97=(a29*a97); - a97=(a24*a97); - a97=(a28*a97); - a36=(a23*a97); - a106=(a106+a36); - a139=(a139-a106); - a139=(a139/a20); - a12=(a12+a139); - a12=(a2*a12); - a12=(a17+a12); - if (res[1]!=0) res[1][35]=a12; - a18=(a4*a18); - a18=(a18-a154); - a64=(a4*a64); - a18=(a18+a64); - a64=(a52*a25); - a3=(a3-a64); - a3=(a3/a118); - a64=(a164*a21); - a3=(a3-a64); - a3=(a3/a132); - a3=(a51*a3); - a3=(a3/a98); - a3=(a50*a3); - a3=(a19*a3); - a3=(a49*a3); - a64=(a141*a97); - a64=(a3+a64); - a21=(a20*a21); - a21=(a129*a21); - a25=(a142*a25); - a21=(a21+a25); - a64=(a64-a21); - a64=(a64/a20); - a18=(a18+a64); - a18=(a2*a18); - if (res[1]!=0) res[1][36]=a18; - a74=(a4*a74); - a58=(a58+a74); - a107=(a4*a107); - a58=(a58+a107); - a97=(a88*a97); - a97=(a31*a97); - a3=(a52*a3); - a97=(a97-a3); - a97=(a97/a76); - a58=(a58+a97); - a58=(a2*a58); - if (res[1]!=0) res[1][37]=a58; - a32=(a32*a20); - a58=(1./a33); - a58=(a58/a8); - a58=(a30*a58); - a58=(a58/a34); - a58=(a29*a58); - a58=(a35*a58); - a58=(a28*a58); - a97=(a37*a58); - a32=(a32-a97); - a32=(a32/a20); - a97=(a14*a32); - a3=(a47*a97); - a43=(a43/a44); - a43=(a40*a43); - a44=(a14*a43); - a107=(a59*a44); - a107=(a39*a107); - a3=(a3+a107); - a107=(1./a53); - a107=(a107/a8); - a107=(a51*a107); - a107=(a107/a54); - a107=(a50*a107); - a107=(a55*a107); - a107=(a49*a107); - a74=(a60*a58); - a74=(a107+a74); - a74=(a74/a20); - a18=(a14*a74); - a18=(a17+a18); - a64=(a63*a18); - a21=(a75*a44); - a21=(a62*a21); - a64=(a64-a21); - a3=(a3+a64); - a64=(a4*a3); - a64=(a13+a64); - a21=(a20*a18); - a21=(a77*a21); - a58=(a72*a58); - a58=(a31*a58); - a107=(a52*a107); - a58=(a58-a107); - a58=(a58/a76); - a107=(a14*a58); - a25=(a83*a107); - a21=(a21+a25); - a25=(a145*a97); - a25=(a71*a25); - a154=(a31*a107); - a154=(a18+a154); - a154=(a154/a39); - a12=(a78*a97); - a154=(a154-a12); - a154=(a154/a171); - a154=(a30*a154); - a154=(a154/a79); - a154=(a29*a154); - a154=(a80*a154); - a154=(a28*a154); - a12=(a82*a154); - a25=(a25+a12); - a21=(a21-a25); - a21=(a21/a20); - a25=(a65*a21); - a12=(a90*a25); - a139=(a85*a97); - a106=(a111*a44); - a106=(a39*a106); - a139=(a139-a106); - a106=(a87*a18); - a44=(a46*a44); - a44=(a62*a44); - a106=(a106+a44); - a139=(a139-a106); - a139=(a139/a11); - a13=(a14*a13); - a13=(a40*a13); - a86=(a86*a13); - a139=(a139+a86); - a139=(a40*a139); - a139=(a107-a139); - a86=(a65*a139); - a13=(a172*a86); - a13=(a84*a13); - a12=(a12+a13); - a13=(a52*a107); - a18=(a18-a13); - a18=(a18/a39); - a13=(a92*a97); - a18=(a18-a13); - a18=(a18/a156); - a18=(a51*a18); - a18=(a18/a93); - a18=(a50*a18); - a18=(a94*a18); - a18=(a49*a18); - a13=(a99*a154); - a13=(a18+a13); - a97=(a20*a97); - a97=(a77*a97); - a107=(a100*a107); - a97=(a97+a107); - a13=(a13-a97); - a13=(a13/a20); - a97=(a65*a13); - a97=(a17+a97); - a107=(a102*a97); - a106=(a175*a86); - a106=(a101*a106); - a107=(a107-a106); - a12=(a12+a107); - a107=(a4*a12); - a64=(a64+a107); - a107=(a20*a97); - a107=(a95*a107); - a154=(a109*a154); - a154=(a31*a154); - a18=(a52*a18); - a154=(a154-a18); - a154=(a154/a76); - a18=(a65*a154); - a106=(a117*a18); - a107=(a107+a106); - a106=(a149*a25); - a106=(a108*a106); - a44=(a31*a18); - a44=(a97+a44); - a44=(a44/a84); - a36=(a155*a25); - a44=(a44-a36); - a44=(a44/a112); - a44=(a30*a44); - a44=(a44/a113); - a44=(a29*a44); - a44=(a114*a44); - a44=(a28*a44); - a36=(a116*a44); - a106=(a106+a36); - a107=(a107-a106); - a107=(a107/a20); - a106=(a1*a107); - a36=(a124*a106); - a131=(a119*a25); - a41=(a181*a86); - a41=(a84*a41); - a131=(a131-a41); - a41=(a121*a97); - a86=(a89*a86); - a86=(a101*a86); - a41=(a41+a86); - a131=(a131-a41); - a131=(a131/a48); - a3=(a65*a3); - a3=(a40*a3); - a3=(a120*a3); - a131=(a131+a3); - a131=(a40*a131); - a131=(a18-a131); - a3=(a1*a131); - a41=(a159*a3); - a41=(a118*a41); - a36=(a36+a41); - a41=(a52*a18); - a97=(a97-a41); - a97=(a97/a84); - a41=(a162*a25); - a97=(a97-a41); - a97=(a97/a126); - a97=(a51*a97); - a97=(a97/a127); - a97=(a50*a97); - a97=(a128*a97); - a97=(a49*a97); - a41=(a133*a44); - a41=(a97+a41); - a25=(a20*a25); - a25=(a95*a25); - a18=(a134*a18); - a25=(a25+a18); - a41=(a41-a25); - a41=(a41/a20); - a25=(a1*a41); - a25=(a17+a25); - a18=(a136*a25); - a86=(a165*a3); - a86=(a135*a86); - a18=(a18-a86); - a36=(a36+a18); - a64=(a64+a36); - a64=(a2*a64); - if (res[1]!=0) res[1][38]=a64; - a139=(a4*a139); - a43=(a43+a139); - a131=(a4*a131); - a43=(a43+a131); - a44=(a122*a44); - a44=(a31*a44); - a97=(a52*a97); - a44=(a44-a97); - a44=(a44/a76); - a97=(a1*a44); - a131=(a125*a106); - a139=(a147*a3); - a139=(a118*a139); - a131=(a131-a139); - a139=(a138*a25); - a3=(a123*a3); - a3=(a135*a3); - a139=(a139+a3); - a131=(a131-a139); - a131=(a131/a7); - a12=(a1*a12); - a12=(a40*a12); - a12=(a137*a12); - a131=(a131+a12); - a131=(a40*a131); - a131=(a97-a131); - a43=(a43+a131); - a43=(a2*a43); - if (res[1]!=0) res[1][39]=a43; - a21=(a4*a21); - a32=(a32+a21); - a107=(a4*a107); - a32=(a32+a107); - a107=(a20*a25); - a107=(a129*a107); - a21=(a45*a97); - a107=(a107+a21); - a21=(a143*a106); - a21=(a22*a21); - a43=(a31*a97); - a43=(a25+a43); - a43=(a43/a118); - a131=(a161*a106); - a43=(a43-a131); - a43=(a43/a27); - a43=(a30*a43); - a43=(a43/a26); - a43=(a29*a43); - a43=(a24*a43); - a43=(a28*a43); - a131=(a23*a43); - a21=(a21+a131); - a107=(a107-a21); - a107=(a107/a20); - a32=(a32+a107); - a32=(a2*a32); - if (res[1]!=0) res[1][40]=a32; - a13=(a4*a13); - a74=(a74+a13); - a41=(a4*a41); - a74=(a74+a41); - a41=(a52*a97); - a25=(a25-a41); - a25=(a25/a118); - a41=(a164*a106); - a25=(a25-a41); - a25=(a25/a132); - a25=(a51*a25); - a25=(a25/a98); - a25=(a50*a25); - a25=(a19*a25); - a25=(a49*a25); - a41=(a141*a43); - a41=(a25+a41); - a106=(a20*a106); - a106=(a129*a106); - a97=(a142*a97); - a106=(a106+a97); - a41=(a41-a106); - a41=(a41/a20); - a74=(a74+a41); - a74=(a2*a74); - a74=(a17+a74); - if (res[1]!=0) res[1][41]=a74; - a154=(a4*a154); - a58=(a58+a154); - a44=(a4*a44); - a58=(a58+a44); - a43=(a88*a43); - a43=(a31*a43); - a25=(a52*a25); - a43=(a43-a25); - a43=(a43/a76); - a58=(a58+a43); - a58=(a2*a58); - if (res[1]!=0) res[1][42]=a58; - a58=(a31/a8); - a58=(a58/a33); - a58=(a30*a58); - a58=(a58/a34); - a58=(a29*a58); - a35=(a35*a58); - a35=(a28*a35); - a37=(a37*a35); - a38=(a38-a37); + a3=(a3-a9); + a3=(a3/a39); + a9=(a44*a3); + a9=(a16+a9); + a38=(a66*a9); + a12=(a98*a10); + a12=(a48*a12); + a38=(a38-a12); + a101=(a101+a38); + a38=(a4*a101); + a46=(a46+a38); + a14=(a67*a14); + a14=(a32*a14); + a17=(a24*a17); + a14=(a14-a17); + a14=(a14/a47); + a17=(a44*a14); + a0=(a44*a0); + a0=(a13*a0); + a0=(a69*a0); + a38=(a102*a10); + a38=(a6*a38); + a12=(a70*a9); + a10=(a52*a10); + a10=(a48*a10); + a12=(a12+a10); + a38=(a38+a12); a38=(a38/a20); - a37=(a14*a38); - a47=(a47*a37); - a59=(a59*a14); - a59=(a39*a59); - a47=(a47+a59); - a60=(a60*a35); - a8=(a52/a8); - a8=(a8/a53); - a8=(a51*a8); - a8=(a8/a54); - a8=(a50*a8); - a55=(a55*a8); - a55=(a49*a55); - a60=(a60-a55); - a60=(a60-a61); - a60=(a60/a20); - a61=(a14*a60); + a0=(a0-a38); + a0=(a13*a0); + a0=(a17-a0); + a38=(a1*a0); + a12=(a99*a38); + a12=(a6*a12); + a10=(a24*a17); + a10=(a9-a10); + a10=(a10/a6); + a10=(a10/a74); + a10=(a23*a10); + a10=(a10/a75); + a10=(a22*a10); + a10=(a76*a10); + a10=(a21*a10); + a100=(a32*a17); + a9=(a9+a100); + a9=(a9/a6); + a9=(a9/a78); + a9=(a31*a9); + a9=(a9/a79); + a9=(a30*a9); + a9=(a81*a9); + a9=(a29*a9); + a100=(a83*a9); + a100=(a10+a100); + a17=(a85*a17); + a100=(a100-a17); + a100=(a100/a39); + a17=(a1*a100); + a17=(a16+a17); + a28=(a86*a17); + a62=(a97*a38); + a62=(a57*a62); + a28=(a28-a62); + a12=(a12+a28); + a46=(a46+a12); + a46=(a2*a46); + if (res[1]!=0) res[1][18]=a46; + a49=(a4*a49); + a15=(a15+a49); + a0=(a4*a0); + a15=(a15+a0); + a9=(a71*a9); + a9=(a32*a9); + a10=(a24*a10); + a9=(a9-a10); + a9=(a9/a47); + a10=(a1*a9); + a101=(a1*a101); + a101=(a13*a101); + a101=(a43*a101); + a0=(a96*a38); + a0=(a6*a0); + a49=(a73*a17); + a38=(a72*a38); + a38=(a57*a38); + a49=(a49+a38); + a0=(a0+a49); + a0=(a0/a5); + a101=(a101-a0); + a101=(a13*a101); + a101=(a10-a101); + a15=(a15+a101); + a15=(a2*a15); + if (res[1]!=0) res[1][19]=a15; + a3=(a4*a3); + a53=(a53+a3); + a100=(a4*a100); + a53=(a53+a100); + a100=(a24*a10); + a100=(a17-a100); + a100=(a100/a6); + a100=(a100/a84); + a100=(a23*a100); + a100=(a100/a64); + a100=(a22*a100); + a100=(a18*a100); + a100=(a21*a100); + a3=(a32*a10); + a17=(a17+a3); + a17=(a17/a6); + a17=(a17/a87); + a17=(a31*a17); + a17=(a17/a88); + a17=(a30*a17); + a17=(a89*a17); + a17=(a29*a17); + a3=(a91*a17); + a3=(a100+a3); + a10=(a93*a10); + a3=(a3-a10); + a3=(a3/a39); + a53=(a53+a3); + a53=(a2*a53); + a53=(a16+a53); + if (res[1]!=0) res[1][20]=a53; + a14=(a4*a14); + a7=(a7+a14); + a9=(a4*a9); + a7=(a7+a9); + a17=(a51*a17); + a17=(a32*a17); + a100=(a24*a100); + a17=(a17-a100); + a17=(a17/a47); + a7=(a7+a17); + a7=(a2*a7); + if (res[1]!=0) res[1][21]=a7; + a90=(a90*a11); + a90=(a6*a90); + a7=(a32/a6); + a7=(a7/a33); + a7=(a31*a7); + a7=(a7/a34); + a7=(a30*a7); + a35=(a35*a7); + a35=(a29*a35); + a37=(a37*a35); + a7=(a24/a6); + a7=(a7/a25); + a7=(a23*a7); + a7=(a7/a26); + a7=(a22*a7); + a27=(a27*a7); + a27=(a21*a27); + a37=(a37-a27); + a37=(a37-a40); + a37=(a37/a39); + a40=(a11*a37); + a42=(a42*a40); + a77=(a77*a11); + a77=(a41*a77); + a42=(a42-a77); + a90=(a90+a42); + a42=(a4*a90); + a45=(a45*a35); + a45=(a32*a45); + a27=(a24*a27); + a45=(a45+a27); + a45=(a45/a47); + a27=(a11*a45); + a27=(a16+a27); + a60=(a60*a11); + a60=(a6*a60); + a50=(a50*a40); + a19=(a19*a11); + a41=(a41*a19); + a50=(a50+a41); + a60=(a60+a50); + a60=(a60/a8); + a60=(a13*a60); + a60=(a27+a60); + a8=(a44*a60); + a95=(a95*a8); + a95=(a6*a95); + a50=(a24*a27); + a50=(a40-a50); + a50=(a50/a6); + a50=(a50/a54); + a50=(a23*a50); + a50=(a50/a55); + a50=(a22*a50); + a56=(a56*a50); + a56=(a21*a56); + a50=(a32*a27); + a40=(a40+a50); + a40=(a40/a6); + a40=(a40/a58); + a40=(a31*a40); + a40=(a40/a59); + a40=(a30*a40); + a61=(a61*a40); + a61=(a29*a61); a63=(a63*a61); - a75=(a75*a14); - a75=(a62*a75); - a63=(a63-a75); - a47=(a47+a63); - a63=(a4*a47); - a75=(a20*a61); - a75=(a77*a75); - a72=(a72*a35); - a72=(a31*a72); - a55=(a52*a55); - a72=(a72+a55); - a72=(a72/a76); - a55=(a14*a72); - a55=(a17+a55); - a83=(a83*a55); - a75=(a75+a83); - a145=(a145*a37); - a71=(a71*a145); - a145=(a31*a55); - a145=(a61+a145); - a145=(a145/a39); - a78=(a78*a37); - a145=(a145-a78); - a145=(a145/a171); - a145=(a30*a145); - a145=(a145/a79); - a145=(a29*a145); - a80=(a80*a145); - a80=(a28*a80); - a82=(a82*a80); - a71=(a71+a82); - a75=(a75-a71); - a75=(a75/a20); - a71=(a65*a75); - a90=(a90*a71); - a85=(a85*a37); - a111=(a111*a14); - a111=(a39*a111); - a85=(a85-a111); - a87=(a87*a61); - a46=(a46*a14); - a62=(a62*a46); - a87=(a87+a62); - a85=(a85-a87); - a85=(a85/a11); - a85=(a40*a85); - a85=(a55-a85); - a11=(a65*a85); - a172=(a172*a11); - a172=(a84*a172); - a90=(a90+a172); - a172=(a52*a55); - a61=(a61-a172); - a61=(a61/a39); - a92=(a92*a37); - a61=(a61-a92); - a61=(a61/a156); - a61=(a51*a61); - a61=(a61/a93); - a61=(a50*a61); - a94=(a94*a61); - a94=(a49*a94); - a99=(a99*a80); - a99=(a94+a99); - a37=(a20*a37); - a77=(a77*a37); - a100=(a100*a55); - a77=(a77+a100); - a99=(a99-a77); - a99=(a99/a20); - a77=(a65*a99); - a102=(a102*a77); - a175=(a175*a11); - a175=(a101*a175); - a102=(a102-a175); - a90=(a90+a102); - a102=(a4*a90); - a63=(a63+a102); - a102=(a20*a77); - a102=(a95*a102); - a109=(a109*a80); - a109=(a31*a109); - a94=(a52*a94); - a109=(a109-a94); - a109=(a109/a76); - a94=(a65*a109); - a94=(a17+a94); - a117=(a117*a94); - a102=(a102+a117); - a149=(a149*a71); - a108=(a108*a149); - a149=(a31*a94); - a149=(a77+a149); - a149=(a149/a84); - a155=(a155*a71); - a149=(a149-a155); - a149=(a149/a112); - a149=(a30*a149); - a149=(a149/a113); - a149=(a29*a149); - a114=(a114*a149); - a114=(a28*a114); - a116=(a116*a114); - a108=(a108+a116); - a102=(a102-a108); + a63=(a56+a63); + a65=(a65*a27); + a63=(a63-a65); + a63=(a63/a39); + a65=(a44*a63); + a66=(a66*a65); + a98=(a98*a8); + a98=(a48*a98); + a66=(a66-a98); + a95=(a95+a66); + a66=(a4*a95); + a42=(a42+a66); + a67=(a67*a61); + a67=(a32*a67); + a56=(a24*a56); + a67=(a67-a56); + a67=(a67/a47); + a56=(a44*a67); + a56=(a16+a56); + a44=(a44*a90); + a44=(a13*a44); + a69=(a69*a44); + a102=(a102*a8); + a102=(a6*a102); + a70=(a70*a65); + a52=(a52*a8); + a48=(a48*a52); + a70=(a70+a48); + a102=(a102+a70); a102=(a102/a20); - a108=(a1*a102); - a124=(a124*a108); - a119=(a119*a71); - a181=(a181*a11); - a181=(a84*a181); - a119=(a119-a181); - a121=(a121*a77); - a89=(a89*a11); - a101=(a101*a89); - a121=(a121+a101); - a119=(a119-a121); - a119=(a119/a48); - a65=(a65*a47); - a65=(a40*a65); - a120=(a120*a65); - a119=(a119+a120); - a119=(a40*a119); - a119=(a94-a119); - a120=(a1*a119); - a159=(a159*a120); - a159=(a118*a159); - a124=(a124+a159); - a159=(a52*a94); - a77=(a77-a159); - a77=(a77/a84); - a162=(a162*a71); - a77=(a77-a162); - a77=(a77/a126); - a77=(a51*a77); - a77=(a77/a127); - a77=(a50*a77); - a128=(a128*a77); - a128=(a49*a128); - a133=(a133*a114); - a133=(a128+a133); - a71=(a20*a71); - a95=(a95*a71); - a134=(a134*a94); - a95=(a95+a134); - a133=(a133-a95); - a133=(a133/a20); - a95=(a1*a133); - a136=(a136*a95); - a165=(a165*a120); - a165=(a135*a165); - a136=(a136-a165); - a124=(a124+a136); - a63=(a63+a124); - a63=(a2*a63); - if (res[1]!=0) res[1][43]=a63; - a85=(a4*a85); - a85=(a17+a85); - a119=(a4*a119); - a85=(a85+a119); - a122=(a122*a114); - a122=(a31*a122); - a128=(a52*a128); - a122=(a122-a128); - a122=(a122/a76); - a128=(a1*a122); - a128=(a17+a128); - a125=(a125*a108); - a147=(a147*a120); - a147=(a118*a147); - a125=(a125-a147); - a138=(a138*a95); - a123=(a123*a120); - a135=(a135*a123); - a138=(a138+a135); - a125=(a125-a138); - a125=(a125/a7); - a1=(a1*a90); - a1=(a40*a1); - a137=(a137*a1); - a125=(a125+a137); - a40=(a40*a125); - a40=(a128-a40); - a85=(a85+a40); - a85=(a2*a85); - if (res[1]!=0) res[1][44]=a85; - a75=(a4*a75); - a38=(a38+a75); - a102=(a4*a102); - a38=(a38+a102); - a102=(a20*a95); - a102=(a129*a102); - a45=(a45*a128); - a102=(a102+a45); - a143=(a143*a108); - a22=(a22*a143); - a143=(a31*a128); - a143=(a95+a143); - a143=(a143/a118); - a161=(a161*a108); - a143=(a143-a161); - a143=(a143/a27); - a30=(a30*a143); - a30=(a30/a26); - a29=(a29*a30); - a24=(a24*a29); - a28=(a28*a24); - a23=(a23*a28); - a22=(a22+a23); - a102=(a102-a22); - a102=(a102/a20); - a38=(a38+a102); - a38=(a2*a38); - if (res[1]!=0) res[1][45]=a38; - a99=(a4*a99); - a60=(a60+a99); - a133=(a4*a133); - a60=(a60+a133); - a133=(a52*a128); - a95=(a95-a133); - a95=(a95/a118); - a164=(a164*a108); - a95=(a95-a164); - a95=(a95/a132); - a51=(a51*a95); - a51=(a51/a98); - a50=(a50*a51); - a19=(a19*a50); - a49=(a49*a19); - a141=(a141*a28); - a141=(a49+a141); - a108=(a20*a108); - a129=(a129*a108); - a142=(a142*a128); - a129=(a129+a142); - a141=(a141-a129); - a141=(a141/a20); - a60=(a60+a141); + a69=(a69-a102); + a69=(a13*a69); + a69=(a56-a69); + a102=(a1*a69); + a99=(a99*a102); + a99=(a6*a99); + a20=(a24*a56); + a20=(a65-a20); + a20=(a20/a6); + a20=(a20/a74); + a20=(a23*a20); + a20=(a20/a75); + a20=(a22*a20); + a76=(a76*a20); + a76=(a21*a76); + a20=(a32*a56); + a65=(a65+a20); + a65=(a65/a6); + a65=(a65/a78); + a65=(a31*a65); + a65=(a65/a79); + a65=(a30*a65); + a81=(a81*a65); + a81=(a29*a81); + a83=(a83*a81); + a83=(a76+a83); + a85=(a85*a56); + a83=(a83-a85); + a83=(a83/a39); + a85=(a1*a83); + a86=(a86*a85); + a97=(a97*a102); + a97=(a57*a97); + a86=(a86-a97); + a99=(a99+a86); + a42=(a42+a99); + a42=(a2*a42); + if (res[1]!=0) res[1][22]=a42; + a60=(a4*a60); + a60=(a16+a60); + a69=(a4*a69); + a60=(a60+a69); + a71=(a71*a81); + a71=(a32*a71); + a76=(a24*a76); + a71=(a71-a76); + a71=(a71/a47); + a76=(a1*a71); + a76=(a16+a76); + a1=(a1*a95); + a1=(a13*a1); + a43=(a43*a1); + a96=(a96*a102); + a96=(a6*a96); + a73=(a73*a85); + a72=(a72*a102); + a57=(a57*a72); + a73=(a73+a57); + a96=(a96+a73); + a96=(a96/a5); + a43=(a43-a96); + a13=(a13*a43); + a13=(a76-a13); + a60=(a60+a13); a60=(a2*a60); - if (res[1]!=0) res[1][46]=a60; - a109=(a4*a109); - a72=(a72+a109); - a4=(a4*a122); - a72=(a72+a4); - a88=(a88*a28); - a31=(a31*a88); - a52=(a52*a49); - a31=(a31-a52); - a31=(a31/a76); - a72=(a72+a31); - a2=(a2*a72); - a17=(a17+a2); - if (res[1]!=0) res[1][47]=a17; + if (res[1]!=0) res[1][23]=a60; + a63=(a4*a63); + a37=(a37+a63); + a83=(a4*a83); + a37=(a37+a83); + a83=(a24*a76); + a83=(a85-a83); + a83=(a83/a6); + a83=(a83/a84); + a23=(a23*a83); + a23=(a23/a64); + a22=(a22*a23); + a18=(a18*a22); + a21=(a21*a18); + a18=(a32*a76); + a85=(a85+a18); + a85=(a85/a6); + a85=(a85/a87); + a31=(a31*a85); + a31=(a31/a88); + a30=(a30*a31); + a89=(a89*a30); + a29=(a29*a89); + a91=(a91*a29); + a91=(a21+a91); + a93=(a93*a76); + a91=(a91-a93); + a91=(a91/a39); + a37=(a37+a91); + a37=(a2*a37); + if (res[1]!=0) res[1][24]=a37; + a67=(a4*a67); + a45=(a45+a67); + a4=(a4*a71); + a45=(a45+a4); + a51=(a51*a29); + a32=(a32*a51); + a24=(a24*a21); + a32=(a32-a24); + a32=(a32/a47); + a45=(a45+a32); + a2=(a2*a45); + a16=(a16+a2); + if (res[1]!=0) res[1][25]=a16; return 0; } @@ -2946,12 +1610,12 @@ const casadi_int* TailoredSolver_dynamics_1_sparsity_out(casadi_int i) { } } -/* TailoredSolver_inequalities_1:(i0[12],i1[31],i2[],i3[])->(o0[5],o1[5x12,18nz]) */ +/* TailoredSolver_inequalities_1:(i0[8],i1[25],i2[],i3[])->(o0[2],o1[2x8,6nz]) */ static int casadi_f2(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a4, a5, a6, a7, a8, a9; - a0=arg[0]? arg[0][7] : 0; + casadi_real a0, a1, a2, a3, a4, a5, a6, a7; + a0=arg[0]? arg[0][4] : 0; a1=1.3600000000000001e+00; - a2=arg[0]? arg[0][8] : 0; + a2=arg[0]? arg[0][5] : 0; a3=fabs(a2); a4=sin(a3); a4=(a1*a4); @@ -2960,7 +1624,7 @@ static int casadi_f2(const casadi_real** arg, casadi_real** res, casadi_int* iw, a6=cos(a2); a6=(a5*a6); a4=(a4+a6); - a6=arg[0]? arg[0][1] : 0; + a6=arg[0]? arg[0][0] : 0; a4=(a4-a6); if (res[0]!=0) res[0][0]=a4; a4=fabs(a2); @@ -2972,105 +1636,21 @@ static int casadi_f2(const casadi_real** arg, casadi_real** res, casadi_int* iw, a7=(a7+a0); a7=(a7-a6); if (res[0]!=0) res[0][1]=a7; - a7=arg[1]? arg[1][18] : 0; - a6=arg[1]? arg[1][25] : 0; - a0=arg[0]? arg[0][6] : 0; - a0=(a6*a0); - a8=1.; - a9=arg[0]? arg[0][5] : 0; - a10=cos(a9); - a10=(a8+a10); - a11=(a0*a10); - a12=(a7*a11); - a13=arg[1]? arg[1][6] : 0; - a12=(a12/a13); - a14=casadi_sq(a12); - a15=arg[1]? arg[1][8] : 0; - a16=arg[1]? arg[1][10] : 0; - a17=arg[0]? arg[0][10] : 0; - a18=arg[1]? arg[1][5] : 0; - a19=arg[0]? arg[0][11] : 0; - a20=(a18*a19); - a20=(a17-a20); - a21=arg[0]? arg[0][9] : 0; - a20=(a20/a21); - a22=atan(a20); - a22=(a16*a22); - a23=atan(a22); - a23=(a15*a23); - a24=sin(a23); - a25=casadi_sq(a24); - a14=(a14+a25); - if (res[0]!=0) res[0][2]=a14; - a11=(a7*a11); - a14=arg[1]? arg[1][7] : 0; - a11=(a11/a14); - a25=casadi_sq(a11); - a26=arg[1]? arg[1][9] : 0; - a27=arg[1]? arg[1][11] : 0; - a28=arg[1]? arg[1][4] : 0; - a19=(a28*a19); - a17=(a17+a19); - a17=(a17/a21); - a19=atan(a17); - a19=(a19-a9); - a19=(a27*a19); - a29=atan(a19); - a29=(a26*a29); - a30=sin(a29); - a31=casadi_sq(a30); - a25=(a25+a31); - if (res[0]!=0) res[0][3]=a25; - a25=arg[0]? arg[0][0] : 0; - a25=(a21-a25); - if (res[0]!=0) res[0][4]=a25; - a25=-1.; - if (res[1]!=0) res[1][0]=a25; - if (res[1]!=0) res[1][1]=a25; - if (res[1]!=0) res[1][2]=a25; - a12=(a12+a12); - a9=sin(a9); - a0=(a0*a9); - a9=(a7*a0); - a9=(a9/a13); - a9=(a12*a9); - a9=(-a9); - if (res[1]!=0) res[1][3]=a9; - a11=(a11+a11); - a0=(a7*a0); - a0=(a0/a14); - a0=(a11*a0); - a30=(a30+a30); - a29=cos(a29); - a19=casadi_sq(a19); - a19=(a8+a19); - a9=(a27/a19); - a9=(a26*a9); - a9=(a29*a9); - a9=(a30*a9); - a0=(a0+a9); - a0=(-a0); - if (res[1]!=0) res[1][4]=a0; - a10=(a10*a6); - a6=(a7*a10); - a6=(a6/a13); - a12=(a12*a6); - if (res[1]!=0) res[1][5]=a12; - a7=(a7*a10); - a7=(a7/a14); - a11=(a11*a7); - if (res[1]!=0) res[1][6]=a11; - if (res[1]!=0) res[1][7]=a8; - if (res[1]!=0) res[1][8]=a25; + a7=-1.; + if (res[1]!=0) res[1][0]=a7; + if (res[1]!=0) res[1][1]=a7; + a6=1.; + if (res[1]!=0) res[1][2]=a6; + if (res[1]!=0) res[1][3]=a7; a3=cos(a3); - a25=casadi_sign(a2); - a3=(a3*a25); + a7=casadi_sign(a2); + a3=(a3*a7); a3=(a1*a3); - a25=sin(a2); - a25=(a5*a25); - a3=(a3+a25); + a7=sin(a2); + a7=(a5*a7); + a3=(a3+a7); a3=(-a3); - if (res[1]!=0) res[1][9]=a3; + if (res[1]!=0) res[1][4]=a3; a4=cos(a4); a3=casadi_sign(a2); a4=(a4*a3); @@ -3078,67 +1658,7 @@ static int casadi_f2(const casadi_real** arg, casadi_real** res, casadi_int* iw, a2=sin(a2); a5=(a5*a2); a1=(a1-a5); - if (res[1]!=0) res[1][10]=a1; - a24=(a24+a24); - a23=cos(a23); - a1=(a20/a21); - a20=casadi_sq(a20); - a20=(a8+a20); - a1=(a1/a20); - a1=(a16*a1); - a22=casadi_sq(a22); - a22=(a8+a22); - a1=(a1/a22); - a1=(a15*a1); - a1=(a23*a1); - a1=(a24*a1); - a1=(-a1); - if (res[1]!=0) res[1][11]=a1; - a1=(a17/a21); - a17=casadi_sq(a17); - a17=(a8+a17); - a1=(a1/a17); - a1=(a27*a1); - a1=(a1/a19); - a1=(a26*a1); - a1=(a29*a1); - a1=(a30*a1); - a1=(-a1); - if (res[1]!=0) res[1][12]=a1; - if (res[1]!=0) res[1][13]=a8; - a8=(1./a20); - a8=(a8/a21); - a8=(a16*a8); - a8=(a8/a22); - a8=(a15*a8); - a8=(a23*a8); - a8=(a24*a8); - if (res[1]!=0) res[1][14]=a8; - a8=(1./a17); - a8=(a8/a21); - a8=(a27*a8); - a8=(a8/a19); - a8=(a26*a8); - a8=(a29*a8); - a8=(a30*a8); - if (res[1]!=0) res[1][15]=a8; - a18=(a18/a21); - a18=(a18/a20); - a16=(a16*a18); - a16=(a16/a22); - a15=(a15*a16); - a23=(a23*a15); - a24=(a24*a23); - a24=(-a24); - if (res[1]!=0) res[1][16]=a24; - a28=(a28/a21); - a28=(a28/a17); - a27=(a27*a28); - a27=(a27/a19); - a26=(a26*a27); - a29=(a29*a26); - a30=(a30*a29); - if (res[1]!=0) res[1][17]=a30; + if (res[1]!=0) res[1][5]=a1; return 0; } @@ -3154,134 +1674,115 @@ const casadi_int* TailoredSolver_inequalities_1_sparsity_out(casadi_int i) { } } -/* TailoredSolver_objective_40:(i0[12],i1[31],i2[],i3[])->(o0,o1[1x12,10nz]) */ +/* TailoredSolver_objective_20:(i0[8],i1[25],i2[],i3[])->(o0,o1[1x8,7nz]) */ static int casadi_f3(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a4, a5, a6, a7, a8, a9; + casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a3, a4, a5, a6, a7, a8, a9; a0=arg[1]? arg[1][0] : 0; - a1=arg[0]? arg[0][2] : 0; + a1=arg[0]? arg[0][1] : 0; a2=casadi_sq(a1); a2=(a0*a2); - a3=arg[1]? arg[1][22] : 0; - a4=arg[0]? arg[0][9] : 0; - a5=arg[0]? arg[0][8] : 0; - a6=cos(a5); - a7=(a4*a6); - a8=arg[0]? arg[0][10] : 0; - a9=sin(a5); + a3=arg[1]? arg[1][19] : 0; + a4=arg[1]? arg[1][21] : 0; + a5=arg[1]? arg[1][23] : 0; + a6=arg[0]? arg[0][5] : 0; + a7=cos(a6); + a7=(a5*a7); + a8=arg[0]? arg[0][6] : 0; + a9=sin(a6); a10=(a8*a9); a7=(a7-a10); + a7=(a4*a7); a10=1.; - a11=arg[0]? arg[0][7] : 0; - a12=arg[1]? arg[1][30] : 0; + a11=arg[0]? arg[0][4] : 0; + a12=arg[1]? arg[1][24] : 0; a13=(a11*a12); a13=(a10-a13); a7=(a7/a13); - a14=arg[1]? arg[1][27] : 0; - a15=(a7*a14); - a15=(a3*a15); - a2=(a2-a15); - a15=arg[1]? arg[1][1] : 0; - a16=arg[0]? arg[0][3] : 0; - a17=casadi_sq(a16); - a17=(a15*a17); - a2=(a2+a17); - a17=arg[1]? arg[1][26] : 0; - a18=arg[0]? arg[0][4] : 0; - a19=casadi_sq(a18); - a19=(a17*a19); - a2=(a2+a19); - a19=arg[1]? arg[1][17] : 0; - a20=(a8/a4); - a21=atan(a20); - a22=arg[0]? arg[0][5] : 0; - a23=arg[1]? arg[1][5] : 0; - a22=(a22*a23); - a24=arg[1]? arg[1][4] : 0; - a24=(a23+a24); - a22=(a22/a24); - a25=atan(a22); - a21=(a21-a25); - a25=casadi_sq(a21); - a25=(a19*a25); + a14=(a3*a7); + a2=(a2-a14); + a14=arg[1]? arg[1][20] : 0; + a15=arg[0]? arg[0][2] : 0; + a16=casadi_sq(a15); + a16=(a14*a16); + a2=(a2+a16); + a16=arg[1]? arg[1][16] : 0; + a17=(a8/a5); + a18=atan(a17); + a19=arg[0]? arg[0][3] : 0; + a20=arg[1]? arg[1][4] : 0; + a19=(a19*a20); + a21=arg[1]? arg[1][3] : 0; + a21=(a20+a21); + a19=(a19/a21); + a22=atan(a19); + a18=(a18-a22); + a22=casadi_sq(a18); + a22=(a16*a22); + a2=(a2+a22); + a22=arg[1]? arg[1][18] : 0; + a23=casadi_sq(a6); + a23=(a22*a23); + a2=(a2+a23); + a23=arg[1]? arg[1][17] : 0; + a24=casadi_sq(a11); + a24=(a23*a24); + a2=(a2+a24); + a24=arg[1]? arg[1][22] : 0; + a25=arg[0]? arg[0][0] : 0; + a25=(a24*a25); a2=(a2+a25); - a25=arg[1]? arg[1][20] : 0; - a26=casadi_sq(a5); - a26=(a25*a26); - a2=(a2+a26); - a26=arg[1]? arg[1][19] : 0; - a27=casadi_sq(a11); - a27=(a26*a27); - a2=(a2+a27); - a27=arg[1]? arg[1][28] : 0; - a28=arg[0]? arg[0][0] : 0; - a28=(a27*a28); - a2=(a2+a28); - a28=arg[1]? arg[1][29] : 0; - a29=arg[0]? arg[0][1] : 0; - a29=(a28*a29); - a2=(a2+a29); if (res[0]!=0) res[0][0]=a2; - if (res[1]!=0) res[1][0]=a27; - if (res[1]!=0) res[1][1]=a28; + if (res[1]!=0) res[1][0]=a24; a1=(a1+a1); a1=(a1*a0); - if (res[1]!=0) res[1][2]=a1; - a16=(a16+a16); - a16=(a16*a15); - if (res[1]!=0) res[1][3]=a16; + if (res[1]!=0) res[1][1]=a1; + a15=(a15+a15); + a15=(a15*a14); + if (res[1]!=0) res[1][2]=a15; a18=(a18+a18); - a18=(a18*a17); - if (res[1]!=0) res[1][4]=a18; - a21=(a21+a21); - a21=(a21*a19); - a22=casadi_sq(a22); - a22=(a10+a22); - a22=(a21/a22); - a22=(a22/a24); - a23=(a23*a22); - a23=(-a23); - if (res[1]!=0) res[1][5]=a23; + a18=(a18*a16); + a19=casadi_sq(a19); + a19=(a10+a19); + a19=(a18/a19); + a19=(a19/a21); + a20=(a20*a19); + a20=(-a20); + if (res[1]!=0) res[1][3]=a20; a11=(a11+a11); - a11=(a11*a26); + a11=(a11*a23); a7=(a7/a13); - a14=(a14*a3); - a7=(a7*a14); + a7=(a7*a3); a12=(a12*a7); a11=(a11-a12); - if (res[1]!=0) res[1][6]=a11; - a11=(a5+a5); - a11=(a11*a25); - a25=cos(a5); - a14=(a14/a13); - a8=(a8*a14); - a25=(a25*a8); - a11=(a11+a25); - a5=sin(a5); - a25=(a4*a14); - a5=(a5*a25); - a11=(a11+a5); - if (res[1]!=0) res[1][7]=a11; - a11=(a20/a4); - a20=casadi_sq(a20); - a10=(a10+a20); - a21=(a21/a10); - a11=(a11*a21); - a6=(a6*a14); + if (res[1]!=0) res[1][4]=a11; + a11=(a6+a6); + a11=(a11*a22); + a22=cos(a6); + a3=(a3/a13); + a4=(a4*a3); + a8=(a8*a4); + a22=(a22*a8); + a11=(a11+a22); + a6=sin(a6); + a22=(a5*a4); + a6=(a6*a22); a11=(a11+a6); - a11=(-a11); - if (res[1]!=0) res[1][8]=a11; - a21=(a21/a4); - a9=(a9*a14); - a21=(a21+a9); - if (res[1]!=0) res[1][9]=a21; + if (res[1]!=0) res[1][5]=a11; + a17=casadi_sq(a17); + a10=(a10+a17); + a18=(a18/a10); + a18=(a18/a5); + a9=(a9*a4); + a18=(a18+a9); + if (res[1]!=0) res[1][6]=a18; return 0; } -int TailoredSolver_objective_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ +int TailoredSolver_objective_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ return casadi_f3(arg, res, iw, w, mem); } -const casadi_int* TailoredSolver_objective_40_sparsity_out(casadi_int i) { +const casadi_int* TailoredSolver_objective_20_sparsity_out(casadi_int i) { switch (i) { case 0: return casadi_s3; case 1: return casadi_s4; @@ -3289,12 +1790,12 @@ const casadi_int* TailoredSolver_objective_40_sparsity_out(casadi_int i) { } } -/* TailoredSolver_inequalities_40:(i0[12],i1[31],i2[],i3[])->(o0[5],o1[5x12,18nz]) */ +/* TailoredSolver_inequalities_20:(i0[8],i1[25],i2[],i3[])->(o0[2],o1[2x8,6nz]) */ static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a4, a5, a6, a7, a8, a9; - a0=arg[0]? arg[0][7] : 0; + casadi_real a0, a1, a2, a3, a4, a5, a6, a7; + a0=arg[0]? arg[0][4] : 0; a1=1.3600000000000001e+00; - a2=arg[0]? arg[0][8] : 0; + a2=arg[0]? arg[0][5] : 0; a3=fabs(a2); a4=sin(a3); a4=(a1*a4); @@ -3303,7 +1804,7 @@ static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw, a6=cos(a2); a6=(a5*a6); a4=(a4+a6); - a6=arg[0]? arg[0][1] : 0; + a6=arg[0]? arg[0][0] : 0; a4=(a4-a6); if (res[0]!=0) res[0][0]=a4; a4=fabs(a2); @@ -3315,105 +1816,21 @@ static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw, a7=(a7+a0); a7=(a7-a6); if (res[0]!=0) res[0][1]=a7; - a7=arg[1]? arg[1][18] : 0; - a6=arg[1]? arg[1][25] : 0; - a0=arg[0]? arg[0][6] : 0; - a0=(a6*a0); - a8=1.; - a9=arg[0]? arg[0][5] : 0; - a10=cos(a9); - a10=(a8+a10); - a11=(a0*a10); - a12=(a7*a11); - a13=arg[1]? arg[1][6] : 0; - a12=(a12/a13); - a14=casadi_sq(a12); - a15=arg[1]? arg[1][8] : 0; - a16=arg[1]? arg[1][10] : 0; - a17=arg[0]? arg[0][10] : 0; - a18=arg[1]? arg[1][5] : 0; - a19=arg[0]? arg[0][11] : 0; - a20=(a18*a19); - a20=(a17-a20); - a21=arg[0]? arg[0][9] : 0; - a20=(a20/a21); - a22=atan(a20); - a22=(a16*a22); - a23=atan(a22); - a23=(a15*a23); - a24=sin(a23); - a25=casadi_sq(a24); - a14=(a14+a25); - if (res[0]!=0) res[0][2]=a14; - a11=(a7*a11); - a14=arg[1]? arg[1][7] : 0; - a11=(a11/a14); - a25=casadi_sq(a11); - a26=arg[1]? arg[1][9] : 0; - a27=arg[1]? arg[1][11] : 0; - a28=arg[1]? arg[1][4] : 0; - a19=(a28*a19); - a17=(a17+a19); - a17=(a17/a21); - a19=atan(a17); - a19=(a19-a9); - a19=(a27*a19); - a29=atan(a19); - a29=(a26*a29); - a30=sin(a29); - a31=casadi_sq(a30); - a25=(a25+a31); - if (res[0]!=0) res[0][3]=a25; - a25=arg[0]? arg[0][0] : 0; - a25=(a21-a25); - if (res[0]!=0) res[0][4]=a25; - a25=-1.; - if (res[1]!=0) res[1][0]=a25; - if (res[1]!=0) res[1][1]=a25; - if (res[1]!=0) res[1][2]=a25; - a12=(a12+a12); - a9=sin(a9); - a0=(a0*a9); - a9=(a7*a0); - a9=(a9/a13); - a9=(a12*a9); - a9=(-a9); - if (res[1]!=0) res[1][3]=a9; - a11=(a11+a11); - a0=(a7*a0); - a0=(a0/a14); - a0=(a11*a0); - a30=(a30+a30); - a29=cos(a29); - a19=casadi_sq(a19); - a19=(a8+a19); - a9=(a27/a19); - a9=(a26*a9); - a9=(a29*a9); - a9=(a30*a9); - a0=(a0+a9); - a0=(-a0); - if (res[1]!=0) res[1][4]=a0; - a10=(a10*a6); - a6=(a7*a10); - a6=(a6/a13); - a12=(a12*a6); - if (res[1]!=0) res[1][5]=a12; - a7=(a7*a10); - a7=(a7/a14); - a11=(a11*a7); - if (res[1]!=0) res[1][6]=a11; - if (res[1]!=0) res[1][7]=a8; - if (res[1]!=0) res[1][8]=a25; + a7=-1.; + if (res[1]!=0) res[1][0]=a7; + if (res[1]!=0) res[1][1]=a7; + a6=1.; + if (res[1]!=0) res[1][2]=a6; + if (res[1]!=0) res[1][3]=a7; a3=cos(a3); - a25=casadi_sign(a2); - a3=(a3*a25); + a7=casadi_sign(a2); + a3=(a3*a7); a3=(a1*a3); - a25=sin(a2); - a25=(a5*a25); - a3=(a3+a25); + a7=sin(a2); + a7=(a5*a7); + a3=(a3+a7); a3=(-a3); - if (res[1]!=0) res[1][9]=a3; + if (res[1]!=0) res[1][4]=a3; a4=cos(a4); a3=casadi_sign(a2); a4=(a4*a3); @@ -3421,75 +1838,15 @@ static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw, a2=sin(a2); a5=(a5*a2); a1=(a1-a5); - if (res[1]!=0) res[1][10]=a1; - a24=(a24+a24); - a23=cos(a23); - a1=(a20/a21); - a20=casadi_sq(a20); - a20=(a8+a20); - a1=(a1/a20); - a1=(a16*a1); - a22=casadi_sq(a22); - a22=(a8+a22); - a1=(a1/a22); - a1=(a15*a1); - a1=(a23*a1); - a1=(a24*a1); - a1=(-a1); - if (res[1]!=0) res[1][11]=a1; - a1=(a17/a21); - a17=casadi_sq(a17); - a17=(a8+a17); - a1=(a1/a17); - a1=(a27*a1); - a1=(a1/a19); - a1=(a26*a1); - a1=(a29*a1); - a1=(a30*a1); - a1=(-a1); - if (res[1]!=0) res[1][12]=a1; - if (res[1]!=0) res[1][13]=a8; - a8=(1./a20); - a8=(a8/a21); - a8=(a16*a8); - a8=(a8/a22); - a8=(a15*a8); - a8=(a23*a8); - a8=(a24*a8); - if (res[1]!=0) res[1][14]=a8; - a8=(1./a17); - a8=(a8/a21); - a8=(a27*a8); - a8=(a8/a19); - a8=(a26*a8); - a8=(a29*a8); - a8=(a30*a8); - if (res[1]!=0) res[1][15]=a8; - a18=(a18/a21); - a18=(a18/a20); - a16=(a16*a18); - a16=(a16/a22); - a15=(a15*a16); - a23=(a23*a15); - a24=(a24*a23); - a24=(-a24); - if (res[1]!=0) res[1][16]=a24; - a28=(a28/a21); - a28=(a28/a17); - a27=(a27*a28); - a27=(a27/a19); - a26=(a26*a27); - a29=(a29*a26); - a30=(a30*a29); - if (res[1]!=0) res[1][17]=a30; + if (res[1]!=0) res[1][5]=a1; return 0; } -int TailoredSolver_inequalities_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ +int TailoredSolver_inequalities_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ return casadi_f4(arg, res, iw, w, mem); } -const casadi_int* TailoredSolver_inequalities_40_sparsity_out(casadi_int i) { +const casadi_int* TailoredSolver_inequalities_20_sparsity_out(casadi_int i) { switch (i) { case 0: return casadi_s7; case 1: return casadi_s8; diff --git a/solver/codeGen/TailoredSolver_casadi.h b/solver/codeGen/TailoredSolver_casadi.h index a3cd582..525773a 100644 --- a/solver/codeGen/TailoredSolver_casadi.h +++ b/solver/codeGen/TailoredSolver_casadi.h @@ -34,17 +34,17 @@ void TailoredSolver_inequalities_1_incref(void); const casadi_int* TailoredSolver_inequalities_1_sparsity_out(casadi_int i); -int TailoredSolver_objective_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +int TailoredSolver_objective_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); -void TailoredSolver_objective_40_incref(void); +void TailoredSolver_objective_20_incref(void); -const casadi_int* TailoredSolver_objective_40_sparsity_out(casadi_int i); +const casadi_int* TailoredSolver_objective_20_sparsity_out(casadi_int i); -int TailoredSolver_inequalities_40(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +int TailoredSolver_inequalities_20(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); -void TailoredSolver_inequalities_40_incref(void); +void TailoredSolver_inequalities_20_incref(void); -const casadi_int* TailoredSolver_inequalities_40_sparsity_out(casadi_int i); +const casadi_int* TailoredSolver_inequalities_20_sparsity_out(casadi_int i); #ifdef __cplusplus } /* extern "C" */ diff --git a/solver/codeGen/TailoredSolver_casadi.o b/solver/codeGen/TailoredSolver_casadi.o index 632bdc2..bf30052 100644 Binary files a/solver/codeGen/TailoredSolver_casadi.o and b/solver/codeGen/TailoredSolver_casadi.o differ diff --git a/solver/codeGen/TailoredSolver_py.py b/solver/codeGen/TailoredSolver_py.py index e095232..0b657ec 100644 --- a/solver/codeGen/TailoredSolver_py.py +++ b/solver/codeGen/TailoredSolver_py.py @@ -22,17 +22,17 @@ OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem subject to the parameters supplied in the following dictionary: - 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_py.TailoredSolver_solve(PARAMS) returns additionally the integer EXITFLAG indicating the state of the solution with @@ -88,12 +88,12 @@ class TailoredSolver_params_ctypes(ctypes.Structure): # @classmethod # def from_param(self): # return self - _fields_ = [('lb', ctypes.c_double * 480), -('ub', ctypes.c_double * 400), -('hu', ctypes.c_double * 200), -('xinit', ctypes.c_double * 10), -('x0', ctypes.c_double * 480), -('all_parameters', ctypes.c_double * 1240), + _fields_ = [('lb', ctypes.c_double * 160), +('ub', ctypes.c_double * 140), +('hu', ctypes.c_double * 40), +('xinit', ctypes.c_double * 7), +('x0', ctypes.c_double * 160), +('all_parameters', ctypes.c_double * 500), ('num_of_threads', ctypes.c_uint), ] @@ -126,8 +126,8 @@ class TailoredSolver_outputs_ctypes(ctypes.Structure): # @classmethod # def from_param(self): # return self - _fields_ = [('U', ctypes.c_double * 280), -('X', ctypes.c_double * 200), + _fields_ = [('U', ctypes.c_double * 80), +('X', ctypes.c_double * 80), ] TailoredSolver_outputs = {'U' : np.array([]), @@ -185,17 +185,17 @@ def TailoredSolver_solve(params_arg): OUTPUT = TailoredSolver_py.TailoredSolver_solve(PARAMS) solves a multistage problem subject to the parameters supplied in the following dictionary: - 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_py.TailoredSolver_solve(PARAMS) returns additionally the integer EXITFLAG indicating the state of the solution with diff --git a/solver/generate_solver.asv b/solver/generate_solver.asv deleted file mode 100644 index 285d5fd..0000000 --- a/solver/generate_solver.asv +++ /dev/null @@ -1,244 +0,0 @@ -function [model, codeoptions] = generate_solver(solverDir, horizonLength, n_states, n_controls, useFastSolver, floattype) - %% Check function args - if (nargin < 3) - error('The function ''generatePathTrackingSolver'' is not meant to run standalone! Please run ''FORCES_problem'' instead.'); - end - if nargin < 6 - useFastSolver = false; - end - if nargin < 7 - floattype = 'double'; - end - - %% Problem dimensions - N = horizonLength; - Npar = 26; - model = {}; - model.N = N; % horizon length - model.nvar = n_states+n_controls; % number of variables - model.neq = n_states; % number of equality constraints - model.nh = 2; % number of inequality constraint functions - model.npar = Npar; % number of runtime parameters - - %% Objective function - model.objective = @objective; - - %% Dynamics, i.e. equality constraints - model.eq = @integrated_dynamics; -% model.continuous_dynamics = @my_continuous_dynamics; - - % Nonlinear inequalities - model.ineq = @nonlin_const; - model.huidx = (1:model.nh)'; - model.hlidx = []; - model.hu = []; - model.hl = []; - - % Indices on LHS of dynamical constraint - for efficiency reasons, make - % sure the matrix E has structure [0 I] where I is the identity matrix. - model.E = [zeros(n_states,n_controls), eye(n_states)]; - - %% Inequality constraints - % upper/lower variable bounds lb <= z <= ub - % inputs | states - % z = [slack_track, diff_delta, Mtv, delta, n, mu, vx, vy, w] - model.lbidx = [1, 2, 3, 4, 5, 6, 7, 8, 9]'; - model.ubidx = [2, 3, 4, 5, 6, 7, 8, 9]'; - model.lb = []; - model.ub = []; - - %% Initial conditions - % Initial conditions on all states - model.xinitidx = 2:9; % use this to specify on which variables initial conditions are imposed - - %% Linear subsystem -% model.linInIdx = [1, 2]'; - - %% Define solver options -% if useFastSolver -% codeoptions = ForcesGetDefaultOptions('TailoredSolver','SQP_NLP_fast',floattype); -% else -% codeoptions = ForcesGetDefaultOptions('TailoredSolver','SQP_NLP',floattype); -% end - - codeoptions = getOptions('TailoredSolver'); - - % Define integrator -% codeoptions.nlp.integrator.type = 'ERK4'; -% codeoptions.nlp.integrator.Ts = rkTime; -% codeoptions.nlp.integrator.nodes = 1; -% codeoptions.nlp.integrator.differentiation_method = 'chainrule'; - - codeoptions.maxit = 150; % Maximum number of iterations - codeoptions.optlevel = 2; % 0: no optimization, 1: optimize for size, 2: optimize for speed, 3: optimize for size & speed - codeoptions.platform = 'Gnu-x86_64'; % Specify the platform - codeoptions.printlevel = 0; % Optional, on some platforms printing is not supported - codeoptions.cleanup = 0; % To keep necessary files for target compile - - % Necessary to set bounds dynamically - codeoptions.nlp.stack_parambounds = 1; - codeoptions.dump_formulation = 1; - - % Speed up the solver - codeoptions.nlp.checkFunctions = 0; - codeoptions.nlp.linear_solver = 'symm_indefinite_fast'; % 'symm_indefinite' - - codeoptions.nlp.BarrStrat = 'monotone'; % 'loqo' (default). Strategy for updating the barrier parameter -% codeoptions.nlp.hessian_approximation = 'bfgs'; % hessian approximation ('gauss-newton' also supported) - - codeoptions.overwrite = 1; % Overwrite existing solver - codeoptions.BuildSimulinkBlock = 0; - codeoptions.nohash = 1; % Enforce solver regeneration - -% codeoptions.nlp.integrator.attempt_subsystem_exploitation = 1; % exploit possible linear subsystems - - codeoptions.init = 0; % Solver initialization method (0: cold start; 1: centered start; 2: primal warm start; see https://forces.embotech.com/Documentation/solver_options/index.html#compiler-optimization-level) - - codeoptions.parallel = 1; % Internal Parallelization -% codeoptions.nlp.max_num_threads = 5; % When using code generated integrators (RK4) we must specify the maximum number of threads available - - % Embotech solution for server error -% codeoptions.legacy_integrators = 1; - - %% Generate FORCESPRO solver - cd(solverDir); - -end - - %% Eval and dynamics functions - -function f = objective(z, p) - - dRd = p(1); - Lf = p(4); - Lr = p(5); - q_slip = p(17); - q_n = p(18); - q_s = p(21); - q_mu = p(19); - q_Mtv = p(22); - Ts = p(23); - k = p(26); - q_slack_track = p(24); - - % Progress rate - sdot = ( z(7)*cos(z(6)) - z(8)*sin(z(6)) )/(1 - z(5)*k) * Ts; % == (vx*cos(mu) - vy*sin(mu))/(1 - n*k) - - % Slip difference - beta_dyn = atan(z(8)/z(7)); - beta_kin = atan(z(4)*Lr/(Lr+Lf)); - diff_beta = beta_dyn - beta_kin; - - %Objective function - f = -q_s*sdot + dRd*(z(2))^2 + q_Mtv*(z(3))^2 + q_slip*(diff_beta)^2 + q_mu*(z(6))^2 + q_n*(z(5))^2 + q_slack_track*z(1); - -end - - -function xnext = integrated_dynamics(z, p) - - u = z(1:3); - x = z(4:9); - Ts = p(24); - - xnext = RK4(x, u, @my_continuous_dynamics, Ts, p); - -end - -function xdot = my_continuous_dynamics(x, u, p) - - delta = x(1); - n = x(2); - mu = x(3); - vx = x(4); - vy = x(5); - w = x(6); - - diff_delta = u(2); - Mtv = u(3); - - m = p(2); - I = p(3); - Lf = p(4); - Lr = p(5); - Dr = p(6); - Df = p(7); - Cr = p(8); - Cf = p(9); - Br = p(10); - Bf = p(11); - u_r = p(12); - g = p(13); - Cd = p(14); - rho = p(15); - Ar = p(16); - k = p(26); - - % Slip angles - alpha_R = atan((vy-Lr*w)/(vx)); - alpha_F = atan((vy+Lf*w)/(vx)) - delta; - - % Simplified Pacejka magic formula - Fr = Dr*sin(Cr*atan(Br*alpha_R)); - Ff = Df*sin(Cf*atan(Bf*alpha_F)); - Fx = Cm*Fm*(1+cos(delta)) - m*u_r*g - 0.5*Cd*rho*Ar*vx^2; - - - %Progress rate change - sdot = (vx*cos(mu) - vy*sin(mu))/(1 - n*k); - - % Differential equations (time dependent) - xdot = [diff_delta; - vx*sin(mu) + vy*cos(mu); - w - k*sdot; - (1/m)*(Fx - Ff*sin(delta) + m*vy*w); - (1/m)*(Fr + Cm*Fm*sin(delta) + Ff*cos(delta) - m*vx*w); - (1/I)*((Ff*cos(delta) + Cm*Fm*sin(delta))*Lf - Fr*Lr + Mtv)]; - -end - -function h = nonlin_const(z, p) - - m = p(2); - Lf = p(4); - Lr = p(5); - Dr = p(6); - Df = p(7); - Cr = p(8); - Cf = p(9); - Br = p(10); - Bf = p(11); - - s1 = z(1); - - delta = z(6); - Fm = z(7); - n = z(8); - mu = z(9); - vx = z(10); - vy = z(11); - w = z(12); - - % Length and width of the car - long = 2.72; - width = 1.2 + 0.4; - - % Maximum longitudinal & lateral acceleration - Ax_max = p(24); - Ay_max = p(25); - - alpha_R = atan((vy-Lr*w)/(vx)); - alpha_F = atan((vy+Lf*w)/(vx)) - delta; - - Fr = Dr*sin(Cr*atan(Br*alpha_R)); - Ff = Df*sin(Cf*atan(Bf*alpha_F)); - - Fx = Cm*Fm*(1+cos(delta)); - - h = [ n - long/2*sin(abs(mu)) + width/2*cos(mu) - s2; % <= L(s) - -n + long/2*sin(abs(mu)) + width/2*cos(mu) - s2; % <= R(s) - (p_long*Fx/Dr)^2 + (Fr/Dr)^2; % <= lambda - (p_long*Fx/Df)^2 + (Ff/Df)^2; % <= lambda - vx - s1]; - -end \ No newline at end of file diff --git a/solver/generate_solver.m b/solver/generate_solver.m index 4e6a70c..80df685 100644 --- a/solver/generate_solver.m +++ b/solver/generate_solver.m @@ -12,7 +12,7 @@ %% Problem dimensions N = horizonLength; - Npar = 26; + Npar = 25; model = {}; model.N = N; % horizon length model.nvar = n_states+n_controls; % number of variables @@ -41,31 +41,33 @@ %% Inequality constraints % upper/lower variable bounds lb <= z <= ub % inputs | states - % z = [slack_track, diff_delta, Mtv, delta, n, mu, vx, vy, w] - model.lbidx = [1, 2, 3, 4, 5, 6, 7, 8, 9]'; - model.ubidx = [2, 3, 4, 5, 6, 7, 8, 9]'; + % z = [slack_track, diff_delta, Mtv, delta, n, mu, vy, w] + model.lbidx = [1, 2, 3, 4, 5, 6, 7, 8]'; + model.ubidx = [2, 3, 4, 5, 6, 7, 8]'; model.lb = []; model.ub = []; %% Initial conditions % Initial conditions on all states - model.xinitidx = 2:9; % use this to specify on which variables initial conditions are imposed + model.xinitidx = 2:8; % use this to specify on which variables initial conditions are imposed %% Linear subsystem % model.linInIdx = [1, 2]'; %% Define solver options - if useFastSolver - codeoptions = ForcesGetDefaultOptions('TailoredSolver','SQP_NLP_fast',floattype); - else - codeoptions = getOptions('TailoredSolver',floattype); - end +% if useFastSolver +% codeoptions = ForcesGetDefaultOptions('TailoredSolver','SQP_NLP_fast',floattype); +% else +% codeoptions = getOptions('TailoredSolver',floattype); +% end % Define integrator % codeoptions.nlp.integrator.type = 'ERK4'; % codeoptions.nlp.integrator.Ts = rkTime; % codeoptions.nlp.integrator.nodes = 1; % codeoptions.nlp.integrator.differentiation_method = 'chainrule'; + + codeoptions = getOptions('TailoredSolver'); codeoptions.maxit = 150; % Maximum number of iterations codeoptions.optlevel = 2; % 0: no optimization, 1: optimize for size, 2: optimize for speed, 3: optimize for size & speed @@ -112,23 +114,30 @@ Lr = p(5); q_slip = p(17); q_n = p(18); - q_s = p(21); + q_s = p(20); q_mu = p(19); - q_Mtv = p(22); - Ts = p(23); - k = p(26); - q_slack_track = p(24); + q_Mtv = p(21); + Ts = p(22); + k = p(25); + q_slack_track = p(23); + + vx = p(24); + delta = z(4); + n = z(5); + mu = z(6); + vy = z(7); + w = z(8); % Progress rate - sdot = ( z(7)*cos(z(6)) - z(8)*sin(z(6)) )/(1 - z(5)*k) * Ts; % == (vx*cos(mu) - vy*sin(mu))/(1 - n*k) + sdot = Ts * (vx*cos(mu) - vy*sin(mu))/(1 - n*k); % Slip difference - beta_dyn = atan(z(8)/z(7)); - beta_kin = atan(z(4)*Lr/(Lr+Lf)); + beta_dyn = atan(vy/vx); + beta_kin = atan(delta*Lr/(Lr+Lf)); diff_beta = beta_dyn - beta_kin; %Objective function - f = -q_s*sdot + dRd*(z(2))^2 + q_Mtv*(z(3))^2 + q_slip*(diff_beta)^2 + q_mu*(z(6))^2 + q_n*(z(5))^2 + q_slack_track*z(1); + f = -q_s*sdot + dRd*(z(2))^2 + q_Mtv*(z(3))^2 + q_slip*(diff_beta)^2 + q_mu*(mu)^2 + q_n*(n)^2 + q_slack_track*z(1); end @@ -136,8 +145,8 @@ function xnext = integrated_dynamics(z, p) u = z(1:3); - x = z(4:9); - Ts = p(24); + x = z(4:8); + Ts = p(22); xnext = RK4(x, u, @my_continuous_dynamics, Ts, p); @@ -148,9 +157,9 @@ delta = x(1); n = x(2); mu = x(3); - vx = x(4); - vy = x(5); - w = x(6); + vx = p(24); + vy = x(4); + w = x(5); diff_delta = u(2); Mtv = u(3); @@ -170,7 +179,7 @@ Cd = p(14); rho = p(15); Ar = p(16); - k = p(26); + k = p(25); % Slip angles alpha_R = atan((vy-Lr*w)/(vx)); @@ -179,9 +188,6 @@ % Simplified Pacejka magic formula Fr = Dr*sin(Cr*atan(Br*alpha_R)); Ff = Df*sin(Cf*atan(Bf*alpha_F)); - - Fm = lookuptable(); - Fx = Fm - m*u_r*g - 0.5*Cd*rho*Ar*vx^2; %Progress rate change sdot = (vx*cos(mu) - vy*sin(mu))/(1 - n*k); @@ -190,9 +196,8 @@ xdot = [diff_delta; vx*sin(mu) + vy*cos(mu); w - k*sdot; - (1/m)*(Fx - Ff*sin(delta) + m*vy*w); - (1/m)*(Fr + Fm*sin(delta) + Ff*cos(delta) - m*vx*w); - (1/I)*((Ff*cos(delta) + Fm*sin(delta))*Lf - Fr*Lr + Mtv)]; + (1/m)*(Fr + Ff*cos(delta) - m*vx*w); + (1/I)*(Ff*cos(delta)*Lf - Fr*Lr + Mtv)]; end function h = nonlin_const(z, p) @@ -208,8 +213,4 @@ h = [ n - long/2*sin(abs(mu)) + width/2*cos(mu) - s1; % <= L(s) -n + long/2*sin(abs(mu)) + width/2*cos(mu) - s1]; % <= R(s) -end - -function Fm = lookuptable() - end \ No newline at end of file diff --git a/solver/try_solver.m b/solver/try_solver.m index c823d97..0dccda4 100644 --- a/solver/try_solver.m +++ b/solver/try_solver.m @@ -4,12 +4,12 @@ track = readtable('gro2_0.0490_0_501.csv'); curvature = track.curvature; -N = 40; %Horizon length -Npar = 28; %Number of real time params -delta_t = 0.025; %Time discretization +N = 20; %Horizon length +Npar = 25; %Number of real time params +delta_t = 0.020; %Time discretization n = 1000; %Param to choose in which curvature we start sampleS = 1; %Points sample -its = 30; %Number of iterations +its = 1; %Number of iterations latency = 5; %Latency opt_control = []; @@ -25,8 +25,8 @@ param_data = []; for i=1:N k = curvature(i*sampleS+n+N*(e-1)); - %param = [dRd, dRa, m, I, Lf, Lr, Dr, Df, Cr, Cf, Br, Bf, u_r, g, Cd, rho, Ar, q_slip, p_long, q_n, q_mu, lambda, q_s, ax_max, ay_max, Cm, dMtv, k] - param_k = [1, 1, 240, 93, .708, .822, 3152.3, 2785.4, 1.6, 1.6, 10.1507, 10.8529, .045, 9.81, .8727, 1.255, 1, 1, 0.5, 10, 1, 3, 100, 10, 12, 4000, 1, k]'; + %param = [dRd, m, I, Lf, Lr, Dr, Df, Cr, Cf, Br, Bf, u_r, g, Cd, rho, Ar, q_slip, q_n, q_mu, q_s, dMtv, t_fact, q_slack_track, vx, k] + param_k = [1, 240, 93, .708, .822, 3152.3, 2785.4, 1.6, 1.6, 10.1507, 10.8529, .045, 9.81, .8727, 1.255, 1, 1, 10, 1, 100, 1, delta_t, 0.0, 10, k]'; param_data = [param_data,param_k]; end @@ -34,10 +34,11 @@ % z_lb = [-deg2rad(2), -0.05, -deg2rad(23), -1, -3, -deg2rad(150), 0, -3, -deg2rad(100)]'; % z_ub = [deg2rad(2), 0.05, deg2rad(23), 1, 3, deg2rad(150), 30, 3, deg2rad(100)]'; - z_lb = [-100, -1000, -100, -deg2rad(23), -1, -3, -deg2rad(50), 0, -3, -deg2rad(40)]'; - z_ub = [100, 1000, 100, deg2rad(23), 1, 3, deg2rad(50), 25, 3, deg2rad(40)]'; + z_lb = [0.0, -100, -300, -deg2rad(23), -3, -deg2rad(50), -3, -deg2rad(40)]'; + z_ub = [100, 300, deg2rad(23), 3, deg2rad(50), 3, deg2rad(40)]'; lb = repmat(z_lb, N, 1); + lb2 = repmat(z_lb(2:end), N, 1); ub = repmat(z_ub, N, 1); % problem.lb = [lb(1:2);lb(10:end)]; @@ -49,14 +50,14 @@ %inequality constraints problem.hu = repmat([1.5;1.5],N,1); - x0 = (ub+lb)/2; + x0 = (ub+lb2)/2; if e==1 - problem.x0 = x0; - problem.xinit = [0,0,0,deg2rad(5), 0.01, 0.01, 0.05, 10, .1, .01]'; + problem.x0 = [0.0;x0]; + problem.xinit = [0,0,deg2rad(5), 0.5, 0.05, .01, .01]'; else - problem.x0 = repmat([0,0,0,controls(5*(e-2)+4),controls(5*(e-2)+5),states(5*(e-2)+1),states(5*(e-2)+2),states(5*(e-2)+3),states(5*(e-2)+4),states(5*(e-2)+5)]',N,1); - problem.xinit = [0,0,0,controls(5*(e-2)+4),controls(5*(e-2)+5),states(5*(e-2)+1),states(5*(e-2)+2),states(5*(e-2)+3),states(5*(e-2)+4),states(5*(e-2)+5)]'; +% problem.x0 = repmat([0,0,controls(5*(e-2)+4),controls(5*(e-2)+5),states(5*(e-2)+1),states(5*(e-2)+2),states(5*(e-2)+3),states(5*(e-2)+4),states(5*(e-2)+5)]',N,1); +% problem.xinit = [0,0,controls(5*(e-2)+4),controls(5*(e-2)+5),states(5*(e-2)+1),states(5*(e-2)+2),states(5*(e-2)+3),states(5*(e-2)+4),states(5*(e-2)+5)]'; end problem.all_parameters = reshape(param_data(:,1:N),Npar*N,1); @@ -66,8 +67,8 @@ opt_control = [opt_control;output.U]; opt_states = [opt_states;output.X]; - controls = [controls;output.U(5*latency+1:5*latency+5)]; - states = [states;output.X(5*latency+1:5*latency+5)]; + controls = [controls;output.U(4*latency+1:4*latency+4)]; + states = [states;output.X(4*latency+1:4*latency+4)]; exitflags(e) = exitflag; end @@ -82,64 +83,64 @@ t = tcurv(2:end); %% State plots -figure() -subplot(3,2,1) -plot(t,states(3:5:end)); -title('Vx'); -xlabel('t [s]'); -ylabel('Vx [m/s]'); - -subplot(3,2,2) -plot(t,states(1:5:end)) -title('n'); -xlabel('t [s]'); -ylabel('n [mm]'); - -subplot(3,2,3) -plot(t,states(4:5:end)) -title('Vy'); -xlabel('t [s]'); -ylabel('Vy [m/s]'); - -subplot(3,2,4) -plot(t,states(5:5:end)) -title('w'); -xlabel('t [s]'); -ylabel('w [rad/s]'); -hold off - -subplot(3,2,5:6) -plot(t,states(2:5:end)) -title('mu') -xlabel('t [s]') -ylabel('mu [rad]') - +% figure() +% subplot(3,2,1) +% plot(t,states(3:5:end)); +% title('Vx'); +% xlabel('t [s]'); +% ylabel('Vx [m/s]'); +% +% subplot(3,2,2) +% plot(t,states(1:5:end)) +% title('n'); +% xlabel('t [s]'); +% ylabel('n [mm]'); +% +% subplot(3,2,3) +% plot(t,states(4:5:end)) +% title('Vy'); +% xlabel('t [s]'); +% ylabel('Vy [m/s]'); +% +% subplot(3,2,4) +% plot(t,states(5:5:end)) +% title('w'); +% xlabel('t [s]'); +% ylabel('w [rad/s]'); +% hold off +% +% subplot(3,2,5:6) +% plot(t,states(2:5:end)) +% title('mu') +% xlabel('t [s]') +% ylabel('mu [rad]') +% %% Controls plot -figure() -subplot(2,2,1) -plot(t,controls(5:5:end)) -title('F_m'); -xlabel('t [s]'); -ylabel('Fm'); - -subplot(2,2,2) -plot(t,controls(4:5:end)) -title('delta'); -xlabel('t [s]'); -ylabel('delta [rad]'); - -subplot(2,2,3) -plot(t,controls(2:5:end)) -title('diff F_m'); -xlabel('t [s]'); -ylabel('diff_Fm'); - -subplot(2,2,4) -plot(t,controls(1:5:end)) -title('diff delta'); -xlabel('t [s]'); -ylabel('diff delta'); -hold off +% figure() +% subplot(2,2,1) +% plot(t,controls(5:5:end)) +% title('F_m'); +% xlabel('t [s]'); +% ylabel('Fm'); +% +% subplot(2,2,2) +% plot(t,controls(4:5:end)) +% title('delta'); +% xlabel('t [s]'); +% ylabel('delta [rad]'); +% +% subplot(2,2,3) +% plot(t,controls(2:5:end)) +% title('diff F_m'); +% xlabel('t [s]'); +% ylabel('diff_Fm'); +% +% subplot(2,2,4) +% plot(t,controls(1:5:end)) +% title('diff delta'); +% xlabel('t [s]'); +% ylabel('diff delta'); +% hold off %% Curvature plot figure() diff --git a/src/main.cpp b/src/main.cpp index bd701ed..06d9af4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -77,7 +77,7 @@ int main(int argc, char **argv) { mpc.mission = 1; // mision hardcoded - ros::Duration(2).sleep(); + ros::Duration(2.0).sleep(); ROS_INFO_STREAM("MPC: publish frequency: " << mpc.Hz << "Hz"); ROS_WARN_STREAM("MPC: internal threads: " << mpc.Nthreads); diff --git a/src/mpc.cpp b/src/mpc.cpp index 09cc690..ebc738f 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -16,6 +16,7 @@ MPC::MPC(const Params* params){ this->Nthreads = params->mpc.Nthreads; this->troProfile = params->mpc.TroProfile; + cout << "Hz: " << Hz << endl; cout << "rk4_t: " << rk4_t << endl; cout << "troProfile: " << troProfile << endl; @@ -51,10 +52,10 @@ MPC::MPC(const Params* params){ progress = Eigen::VectorXd::Zero(N); lastState = Eigen::MatrixXd::Zero(N,6); // [x, y, theta, vx, vy, w] - lastCommands = Eigen::MatrixXd::Zero(N,n_controls-Nslacks); // [diff_delta, diff_Fm, Mtv, delta, acc] + lastCommands = Eigen::MatrixXd::Zero(N,n_controls-Nslacks); // [diff_delta, Mtv, delta] - solStates = Eigen::MatrixXd::Zero(N,n_states); // [n, mu, vx, vy, w] - solCommands = Eigen::MatrixXd::Zero(N,n_controls); // [slack_vx, slack_track, diff_delta, diff_acc, Mtv, delta, Fm] + solStates = Eigen::MatrixXd::Zero(N,n_states); // [n, mu, vy, w] + solCommands = Eigen::MatrixXd::Zero(N,n_controls); // [slack_track, diff_delta, Mtv, delta] this->paramFlag = true; } @@ -189,7 +190,7 @@ void MPC::initial_conditions(){ Eigen::Vector2d tangent; tangent << planner(1,0) - planner(0,0), planner(1,1) - planner(0,1); - // Calculate vector the first point of the trajectory to the car + // Calculate vector from the first point of the trajectory to the car Eigen::Vector2d s0_to_car; s0_to_car << carState(0) - planner(0,0), carState(1) - planner(0,1); @@ -210,20 +211,17 @@ void MPC::initial_conditions(){ ROS_WARN_STREAM("mu0: " << mu0); ROS_WARN_STREAM("n0: " << n0); - // xinit = [diff_delta, diff_Fm, Mtv, delta, Fm, n, mu, Vx, Vy, w] + // xinit = [diff_delta, Mtv, delta, n, mu, Vy, w] forces.params.xinit[0] = 0.0; - forces.params.xinit[1] = 0.0; - forces.params.xinit[2] = carState(8); - forces.params.xinit[3] = carState(6); - forces.params.xinit[4] = ax_to_throttle(carState(7)); - forces.params.xinit[5] = n0; - forces.params.xinit[6] = mu0; - forces.params.xinit[7] = carState(3); - forces.params.xinit[8] = carState(4); - forces.params.xinit[9] = carState(5); + forces.params.xinit[1] = carState(8); + forces.params.xinit[2] = carState(6); + forces.params.xinit[3] = n0; + forces.params.xinit[4] = mu0; + forces.params.xinit[5] = carState(4); + forces.params.xinit[6] = carState(5); cout << "Xinit:\n"; - for(int i;i<10;i++){ + for(int i=0;i<7;i++){ cout << forces.params.xinit[i] << endl; } @@ -266,36 +264,30 @@ void MPC::set_params_bounds(){ for(int k = 0; k < this->N; k++){ this->forces.params.all_parameters[ 0 + k*this->Npar] = this->dRd; - this->forces.params.all_parameters[ 1 + k*this->Npar] = this->dRa; - this->forces.params.all_parameters[ 2 + k*this->Npar] = this->m; - this->forces.params.all_parameters[ 3 + k*this->Npar] = this->I; - this->forces.params.all_parameters[ 4 + k*this->Npar] = this->Lf; - this->forces.params.all_parameters[ 5 + k*this->Npar] = this->Lr; - this->forces.params.all_parameters[ 6 + k*this->Npar] = this->Dr; - this->forces.params.all_parameters[ 7 + k*this->Npar] = this->Df; - this->forces.params.all_parameters[ 8 + k*this->Npar] = this->Cr; - this->forces.params.all_parameters[ 9 + k*this->Npar] = this->Cf; - this->forces.params.all_parameters[10 + k*this->Npar] = this->Br; - this->forces.params.all_parameters[11 + k*this->Npar] = this->Bf; - this->forces.params.all_parameters[12 + k*this->Npar] = this->u_r; - this->forces.params.all_parameters[13 + k*this->Npar] = this->gravity; - this->forces.params.all_parameters[14 + k*this->Npar] = this->Cd; - this->forces.params.all_parameters[15 + k*this->Npar] = this->rho; - this->forces.params.all_parameters[16 + k*this->Npar] = this->Ar; - this->forces.params.all_parameters[17 + k*this->Npar] = this->q_slip; - this->forces.params.all_parameters[18 + k*this->Npar] = this->p_long; - this->forces.params.all_parameters[19 + k*this->Npar] = (k == N-1) ? this->q_nN : this->q_n; - this->forces.params.all_parameters[20 + k*this->Npar] = this->q_mu; - this->forces.params.all_parameters[21 + k*this->Npar] = this->lambda; - this->forces.params.all_parameters[22 + k*this->Npar] = (k == N-1) ? this->q_sN : this->q_s; - this->forces.params.all_parameters[23 + k*this->Npar] = this->ax_max; - this->forces.params.all_parameters[24 + k*this->Npar] = this->ay_max; - this->forces.params.all_parameters[25 + k*this->Npar] = this->Cm; - this->forces.params.all_parameters[26 + k*this->Npar] = this->dMtv; - - this->forces.params.all_parameters[27 + k*this->Npar] = this->rk4_t; // It's actually rk4_t * t_fact (0.025s * scaling_factor) - this->forces.params.all_parameters[28 + k*this->Npar] = this->q_slack_vx; - this->forces.params.all_parameters[29 + k*this->Npar] = this->q_slack_track; + this->forces.params.all_parameters[ 1 + k*this->Npar] = this->m; + this->forces.params.all_parameters[ 2 + k*this->Npar] = this->I; + this->forces.params.all_parameters[ 3 + k*this->Npar] = this->Lf; + this->forces.params.all_parameters[ 4 + k*this->Npar] = this->Lr; + this->forces.params.all_parameters[ 5 + k*this->Npar] = this->Dr; + this->forces.params.all_parameters[ 6 + k*this->Npar] = this->Df; + this->forces.params.all_parameters[ 7 + k*this->Npar] = this->Cr; + this->forces.params.all_parameters[ 8 + k*this->Npar] = this->Cf; + this->forces.params.all_parameters[ 9 + k*this->Npar] = this->Br; + this->forces.params.all_parameters[10 + k*this->Npar] = this->Bf; + this->forces.params.all_parameters[11 + k*this->Npar] = this->u_r; + this->forces.params.all_parameters[12 + k*this->Npar] = this->gravity; + this->forces.params.all_parameters[13 + k*this->Npar] = this->Cd; + this->forces.params.all_parameters[14 + k*this->Npar] = this->rho; + this->forces.params.all_parameters[15 + k*this->Npar] = this->Ar; + this->forces.params.all_parameters[16 + k*this->Npar] = this->q_slip; + this->forces.params.all_parameters[17 + k*this->Npar] = (k == N-1) ? this->q_nN : this->q_n; + this->forces.params.all_parameters[18 + k*this->Npar] = this->q_mu; + this->forces.params.all_parameters[19 + k*this->Npar] = (k == N-1) ? this->q_sN : this->q_s; + this->forces.params.all_parameters[20 + k*this->Npar] = this->dMtv; + + this->forces.params.all_parameters[21 + k*this->Npar] = this->rk4_t; + this->forces.params.all_parameters[22 + k*this->Npar] = this->q_slack_track; + this->forces.params.all_parameters[23 + k*this->Npar] = carState(3); // CHANGE!! --> vx prediction int plannerIdx = 0; if(firstIter){ @@ -336,130 +328,95 @@ void MPC::set_params_bounds(){ progress(k) = planner(plannerIdx, 2); - this->forces.params.all_parameters[30 + k*this->Npar] = planner(plannerIdx, 3); // curvature + this->forces.params.all_parameters[24 + k*this->Npar] = planner(plannerIdx, 3); // curvature cout << "Curvature: " << forces.params.all_parameters[30+k*Npar] << endl; // Inequality constraints bounds: this->forces.params.hu[k*nh] = fabs(planner(plannerIdx, 5)); // L(s) ortogonal left dist from the path to the track limits this->forces.params.hu[k*nh + 1] = fabs(planner(plannerIdx, 6)); // R(s) ortogonal right dist from the path to the track limits - this->forces.params.hu[k*nh + 2] = this->lambda; // forces ellipse contraints - this->forces.params.hu[k*nh + 3] = this->lambda; - this->forces.params.hu[k*nh + 4] = this->bounds.x_max[4]; // v_max // Variables bounds: this->forces.params.lb[k*Nvar] = this->bounds.u_min[0]; this->forces.params.lb[k*Nvar + 1] = this->bounds.u_min[1]; this->forces.params.lb[k*Nvar + 2] = this->bounds.u_min[2]; - this->forces.params.lb[k*Nvar + 3] = this->bounds.u_min[3]; - this->forces.params.lb[k*Nvar + 4] = this->bounds.u_min[4]; - this->forces.params.lb[k*Nvar + 5] = this->bounds.x_min[0]; - this->forces.params.lb[k*Nvar + 6] = this->bounds.x_min[1]; - this->forces.params.lb[k*Nvar + 7] = this->bounds.x_min[2]; - this->forces.params.lb[k*Nvar + 8] = this->bounds.x_min[3]; - this->forces.params.lb[k*Nvar + 9] = this->bounds.x_min[4]; - this->forces.params.lb[k*Nvar + 10]= this->bounds.x_min[5]; - this->forces.params.lb[k*Nvar + 11]= this->bounds.x_min[6]; + this->forces.params.lb[k*Nvar + 3] = this->bounds.x_min[0]; + this->forces.params.lb[k*Nvar + 4] = this->bounds.x_min[1]; + this->forces.params.lb[k*Nvar + 5] = this->bounds.x_min[2]; + this->forces.params.lb[k*Nvar + 6] = this->bounds.x_min[3]; + this->forces.params.lb[k*Nvar + 7] = this->bounds.x_min[4]; // Slack variables doesn't have any upper bounds, so Nvar-Nslacks is used here this->forces.params.ub[k*(Nvar-Nslacks)] = this->bounds.u_max[0]; this->forces.params.ub[k*(Nvar-Nslacks) + 1] = this->bounds.u_max[1]; - this->forces.params.ub[k*(Nvar-Nslacks) + 2] = this->bounds.u_max[2]; - this->forces.params.ub[k*(Nvar-Nslacks) + 3] = this->bounds.x_max[0]; - this->forces.params.ub[k*(Nvar-Nslacks) + 4] = this->bounds.x_max[1]; - this->forces.params.ub[k*(Nvar-Nslacks) + 5] = this->bounds.x_max[2]; - this->forces.params.ub[k*(Nvar-Nslacks) + 6] = this->bounds.x_max[3]; - this->forces.params.ub[k*(Nvar-Nslacks) + 7] = (troActive && troProfile) ? planner(plannerIdx,4) : this->bounds.x_max[4]; - this->forces.params.ub[k*(Nvar-Nslacks) + 8] = this->bounds.x_max[5]; - this->forces.params.ub[k*(Nvar-Nslacks) + 9] = this->bounds.x_max[6]; + this->forces.params.ub[k*(Nvar-Nslacks) + 2] = this->bounds.x_max[0]; + this->forces.params.ub[k*(Nvar-Nslacks) + 3] = this->bounds.x_max[1]; + this->forces.params.ub[k*(Nvar-Nslacks) + 4] = this->bounds.x_max[2]; + this->forces.params.ub[k*(Nvar-Nslacks) + 5] = this->bounds.x_max[3]; + this->forces.params.ub[k*(Nvar-Nslacks) + 6] = this->bounds.x_max[4]; if(!firstIter){ if((latency + k) < this->N - 1){ if(this->forces.exit_flag == 1){ - this->forces.params.x0[k*Nvar] = 0.0; - this->forces.params.x0[k*Nvar + 1] = 0.0; + this->forces.params.x0[k*Nvar] = 0.0; + this->forces.params.x0[k*Nvar + 1] = solCommands(latency+k, 1); this->forces.params.x0[k*Nvar + 2] = solCommands(latency+k, 2); this->forces.params.x0[k*Nvar + 3] = solCommands(latency+k, 3); - this->forces.params.x0[k*Nvar + 4] = solCommands(latency+k, 4); - this->forces.params.x0[k*Nvar + 5] = solCommands(latency+k, 5); - this->forces.params.x0[k*Nvar + 6] = solCommands(latency+k, 6); - this->forces.params.x0[k*Nvar + 7] = solStates(latency+k, 0); - this->forces.params.x0[k*Nvar + 8] = solStates(latency+k, 1); - this->forces.params.x0[k*Nvar + 9] = solStates(latency+k, 2); - this->forces.params.x0[k*Nvar + 10]= solStates(latency+k, 3); - this->forces.params.x0[k*Nvar + 11]= solStates(latency+k, 4); + this->forces.params.x0[k*Nvar + 4] = solStates(latency+k, 0); + this->forces.params.x0[k*Nvar + 5] = solStates(latency+k, 1); + this->forces.params.x0[k*Nvar + 6] = solStates(latency+k, 2); + this->forces.params.x0[k*Nvar + 7] = solStates(latency+k, 3); }else{ this->forces.params.x0[k*Nvar] = this->forces.params.lb[k*Nvar]; - this->forces.params.x0[k*Nvar + 1] = this->forces.params.lb[k*Nvar + 1]; - this->forces.params.x0[k*Nvar + 2] = (this->forces.params.lb[k*Nvar + 2] + this->forces.params.ub[k*(Nvar-Nslacks)])/2; - this->forces.params.x0[k*Nvar + 3] = (this->forces.params.lb[k*Nvar + 3] + this->forces.params.ub[k*(Nvar-Nslacks) + 1])/2; - this->forces.params.x0[k*Nvar + 4] = (this->forces.params.lb[k*Nvar + 4] + this->forces.params.ub[k*(Nvar-Nslacks) + 2])/2; - this->forces.params.x0[k*Nvar + 5] = (this->forces.params.lb[k*Nvar + 5] + this->forces.params.ub[k*(Nvar-Nslacks) + 3])/2; - this->forces.params.x0[k*Nvar + 6] = (this->forces.params.lb[k*Nvar + 6] + this->forces.params.ub[k*(Nvar-Nslacks) + 4])/2; - this->forces.params.x0[k*Nvar + 7] = (this->forces.params.lb[k*Nvar + 7] + this->forces.params.ub[k*(Nvar-Nslacks) + 5])/2; - this->forces.params.x0[k*Nvar + 8] = (this->forces.params.lb[k*Nvar + 8] + this->forces.params.ub[k*(Nvar-Nslacks) + 6])/2; - this->forces.params.x0[k*Nvar + 9] = (this->forces.params.lb[k*Nvar + 9] + this->forces.params.ub[k*(Nvar-Nslacks) + 7])/2; - this->forces.params.x0[k*Nvar + 10]= (this->forces.params.lb[k*Nvar + 10] + this->forces.params.ub[k*(Nvar-Nslacks) + 8])/2; - this->forces.params.x0[k*Nvar + 11]= (this->forces.params.lb[k*Nvar + 11] + this->forces.params.ub[k*(Nvar-Nslacks) + 9])/2; + this->forces.params.x0[k*Nvar + 1] = (this->forces.params.lb[k*Nvar + 1] + this->forces.params.ub[k*(Nvar-Nslacks)])/2; + this->forces.params.x0[k*Nvar + 2] = (this->forces.params.lb[k*Nvar + 2] + this->forces.params.ub[k*(Nvar-Nslacks) + 1])/2; + this->forces.params.x0[k*Nvar + 3] = (this->forces.params.lb[k*Nvar + 3] + this->forces.params.ub[k*(Nvar-Nslacks) + 2])/2; + this->forces.params.x0[k*Nvar + 4] = (this->forces.params.lb[k*Nvar + 4] + this->forces.params.ub[k*(Nvar-Nslacks) + 3])/2; + this->forces.params.x0[k*Nvar + 5] = (this->forces.params.lb[k*Nvar + 5] + this->forces.params.ub[k*(Nvar-Nslacks) + 4])/2; + this->forces.params.x0[k*Nvar + 6] = (this->forces.params.lb[k*Nvar + 6] + this->forces.params.ub[k*(Nvar-Nslacks) + 5])/2; + this->forces.params.x0[k*Nvar + 7] = (this->forces.params.lb[k*Nvar + 7] + this->forces.params.ub[k*(Nvar-Nslacks) + 6])/2; } }else{ if(this->forces.exit_flag == 1){ - this->forces.params.x0[k*Nvar] = 0.0; - this->forces.params.x0[k*Nvar + 1] = 0.0; + this->forces.params.x0[k*Nvar] = 0.0; + this->forces.params.x0[k*Nvar + 1] = solCommands(N-1, 1); this->forces.params.x0[k*Nvar + 2] = solCommands(N-1, 2); this->forces.params.x0[k*Nvar + 3] = solCommands(N-1, 3); - this->forces.params.x0[k*Nvar + 4] = solCommands(N-1, 4); - this->forces.params.x0[k*Nvar + 5] = solCommands(N-1, 5); - this->forces.params.x0[k*Nvar + 6] = solCommands(N-1, 6); - this->forces.params.x0[k*Nvar + 7] = solStates(N-1, 0); - this->forces.params.x0[k*Nvar + 8] = solStates(N-1, 1); - this->forces.params.x0[k*Nvar + 9] = solStates(N-1, 2); - this->forces.params.x0[k*Nvar + 10]= solStates(N-1, 3); - this->forces.params.x0[k*Nvar + 11]= solStates(N-1, 4); + this->forces.params.x0[k*Nvar + 4] = solStates(N-1, 0); + this->forces.params.x0[k*Nvar + 5] = solStates(N-1, 1); + this->forces.params.x0[k*Nvar + 6] = solStates(N-1, 2); + this->forces.params.x0[k*Nvar + 7] = solStates(N-1, 3); }else{ this->forces.params.x0[k*Nvar] = this->forces.params.lb[k*Nvar]; - this->forces.params.x0[k*Nvar + 1] = this->forces.params.lb[k*Nvar + 1]; - this->forces.params.x0[k*Nvar + 2] = (this->forces.params.lb[k*Nvar + 2] + this->forces.params.ub[k*(Nvar-Nslacks)])/2; - this->forces.params.x0[k*Nvar + 3] = (this->forces.params.lb[k*Nvar + 3] + this->forces.params.ub[k*(Nvar-Nslacks) + 1])/2; - this->forces.params.x0[k*Nvar + 4] = (this->forces.params.lb[k*Nvar + 4] + this->forces.params.ub[k*(Nvar-Nslacks) + 2])/2; - this->forces.params.x0[k*Nvar + 5] = (this->forces.params.lb[k*Nvar + 5] + this->forces.params.ub[k*(Nvar-Nslacks) + 3])/2; - this->forces.params.x0[k*Nvar + 6] = (this->forces.params.lb[k*Nvar + 6] + this->forces.params.ub[k*(Nvar-Nslacks) + 4])/2; - this->forces.params.x0[k*Nvar + 7] = (this->forces.params.lb[k*Nvar + 7] + this->forces.params.ub[k*(Nvar-Nslacks) + 5])/2; - this->forces.params.x0[k*Nvar + 8] = (this->forces.params.lb[k*Nvar + 8] + this->forces.params.ub[k*(Nvar-Nslacks) + 6])/2; - this->forces.params.x0[k*Nvar + 9] = (this->forces.params.lb[k*Nvar + 9] + this->forces.params.ub[k*(Nvar-Nslacks) + 7])/2; - this->forces.params.x0[k*Nvar + 10]= (this->forces.params.lb[k*Nvar + 10] + this->forces.params.ub[k*(Nvar-Nslacks) + 8])/2; - this->forces.params.x0[k*Nvar + 11]= (this->forces.params.lb[k*Nvar + 11] + this->forces.params.ub[k*(Nvar-Nslacks) + 9])/2; + this->forces.params.x0[k*Nvar + 1] = (this->forces.params.lb[k*Nvar + 1] + this->forces.params.ub[k*(Nvar-Nslacks)])/2; + this->forces.params.x0[k*Nvar + 2] = (this->forces.params.lb[k*Nvar + 2] + this->forces.params.ub[k*(Nvar-Nslacks) + 1])/2; + this->forces.params.x0[k*Nvar + 3] = (this->forces.params.lb[k*Nvar + 3] + this->forces.params.ub[k*(Nvar-Nslacks) + 2])/2; + this->forces.params.x0[k*Nvar + 4] = (this->forces.params.lb[k*Nvar + 4] + this->forces.params.ub[k*(Nvar-Nslacks) + 3])/2; + this->forces.params.x0[k*Nvar + 5] = (this->forces.params.lb[k*Nvar + 5] + this->forces.params.ub[k*(Nvar-Nslacks) + 4])/2; + this->forces.params.x0[k*Nvar + 6] = (this->forces.params.lb[k*Nvar + 6] + this->forces.params.ub[k*(Nvar-Nslacks) + 5])/2; + this->forces.params.x0[k*Nvar + 7] = (this->forces.params.lb[k*Nvar + 7] + this->forces.params.ub[k*(Nvar-Nslacks) + 6])/2; } } }else{ if(k==0){ - this->forces.params.x0[k*Nvar] = 0; - this->forces.params.x0[k*Nvar + 1] = 0; - this->forces.params.x0[k*Nvar + 2] = 0; - this->forces.params.x0[k*Nvar + 3] = 0; - this->forces.params.x0[k*Nvar + 4] = carState(8); - this->forces.params.x0[k*Nvar + 5] = carState(6); - this->forces.params.x0[k*Nvar + 6] = ax_to_throttle(carState(7)); - this->forces.params.x0[k*Nvar + 7] = forces.params.xinit[5]; - this->forces.params.x0[k*Nvar + 8] = forces.params.xinit[6]; - this->forces.params.x0[k*Nvar + 9] = carState(3); - this->forces.params.x0[k*Nvar + 10]= carState(4); - this->forces.params.x0[k*Nvar + 11]= carState(5); - + this->forces.params.x0[k*Nvar] = 0.0; + this->forces.params.x0[k*Nvar + 1] = 0.0; + this->forces.params.x0[k*Nvar + 2] = carState(8); + this->forces.params.x0[k*Nvar + 3] = carState(6); + this->forces.params.x0[k*Nvar + 4] = forces.params.xinit[3]; + this->forces.params.x0[k*Nvar + 5] = forces.params.xinit[4];; + this->forces.params.x0[k*Nvar + 6] = carState(4); + this->forces.params.x0[k*Nvar + 7] = carState(5); }else{ this->forces.params.x0[k*Nvar] = this->forces.params.lb[k*Nvar]; - this->forces.params.x0[k*Nvar + 1] = this->forces.params.lb[k*Nvar + 1]; - this->forces.params.x0[k*Nvar + 2] = (this->forces.params.lb[k*Nvar + 2] + this->forces.params.ub[k*(Nvar-Nslacks)])/2; - this->forces.params.x0[k*Nvar + 3] = (this->forces.params.lb[k*Nvar + 3] + this->forces.params.ub[k*(Nvar-Nslacks) + 1])/2; - this->forces.params.x0[k*Nvar + 4] = (this->forces.params.lb[k*Nvar + 4] + this->forces.params.ub[k*(Nvar-Nslacks) + 2])/2; - this->forces.params.x0[k*Nvar + 5] = (this->forces.params.lb[k*Nvar + 5] + this->forces.params.ub[k*(Nvar-Nslacks) + 3])/2; - this->forces.params.x0[k*Nvar + 6] = (this->forces.params.lb[k*Nvar + 6] + this->forces.params.ub[k*(Nvar-Nslacks) + 4])/2; - this->forces.params.x0[k*Nvar + 7] = (this->forces.params.lb[k*Nvar + 7] + this->forces.params.ub[k*(Nvar-Nslacks) + 5])/2; - this->forces.params.x0[k*Nvar + 8] = (this->forces.params.lb[k*Nvar + 8] + this->forces.params.ub[k*(Nvar-Nslacks) + 6])/2; - this->forces.params.x0[k*Nvar + 9] = (this->forces.params.lb[k*Nvar + 9] + this->forces.params.ub[k*(Nvar-Nslacks) + 7])/2; - this->forces.params.x0[k*Nvar + 10]= (this->forces.params.lb[k*Nvar + 10] + this->forces.params.ub[k*(Nvar-Nslacks) + 8])/2; - this->forces.params.x0[k*Nvar + 11]= (this->forces.params.lb[k*Nvar + 11] + this->forces.params.ub[k*(Nvar-Nslacks) + 9])/2; + this->forces.params.x0[k*Nvar + 1] = (this->forces.params.lb[k*Nvar + 1] + this->forces.params.ub[k*(Nvar-Nslacks)])/2; + this->forces.params.x0[k*Nvar + 2] = (this->forces.params.lb[k*Nvar + 2] + this->forces.params.ub[k*(Nvar-Nslacks) + 1])/2; + this->forces.params.x0[k*Nvar + 3] = (this->forces.params.lb[k*Nvar + 3] + this->forces.params.ub[k*(Nvar-Nslacks) + 2])/2; + this->forces.params.x0[k*Nvar + 4] = (this->forces.params.lb[k*Nvar + 4] + this->forces.params.ub[k*(Nvar-Nslacks) + 3])/2; + this->forces.params.x0[k*Nvar + 5] = (this->forces.params.lb[k*Nvar + 5] + this->forces.params.ub[k*(Nvar-Nslacks) + 4])/2; + this->forces.params.x0[k*Nvar + 6] = (this->forces.params.lb[k*Nvar + 6] + this->forces.params.ub[k*(Nvar-Nslacks) + 5])/2; + this->forces.params.x0[k*Nvar + 7] = (this->forces.params.lb[k*Nvar + 7] + this->forces.params.ub[k*(Nvar-Nslacks) + 6])/2; } } } @@ -478,7 +435,7 @@ void MPC::s_prediction(){ lastState(0,0) = carState(0); lastState(0,1) = carState(1); lastState(0,2) = carState(2); - lastState(0,3) = solStates(0,2); + lastState(0,3) = carState(3); // CHANGE!! --> vx prediction lastState(0,4) = solStates(0,3); lastState(0,5) = solStates(0,4); @@ -487,10 +444,10 @@ void MPC::s_prediction(){ double theta = lastState(i-1, 2); double n = solStates(i-1, 0); double mu = solStates(i-1, 1); - double vx = solStates(i-1, 2); - double vy = solStates(i-1, 3); - double w = solStates(i-1, 4); - double k = forces.params.all_parameters[30 + (i-1)*this->Npar]; + double vx = carState(3); // CHANGE!! --> vx prediction + double vy = solStates(i-1, 2); + double w = solStates(i-1, 3); + double k = forces.params.all_parameters[24 + (i-1)*this->Npar]; double sdot = (vx*cos(mu) - vy*sin(mu))/(1 - n*k); @@ -514,11 +471,9 @@ void MPC::s_prediction(){ lastState(i,4) = vy; lastState(i,5) = w; - lastCommands(i-1,0) = solCommands(i-1,2); // diff_delta - lastCommands(i-1,1) = solCommands(i-1,3); // diff_Fm - lastCommands(i-1,2) = solCommands(i-1,4); // Mtv - lastCommands(i-1,3) = solCommands(i-1,5); // Delta - lastCommands(i-1,4) = solCommands(i-1,6); // Fm + lastCommands(i-1,0) = solCommands(i-1,1); // diff_delta + lastCommands(i-1,1) = solCommands(i-1,2); // Mtv + lastCommands(i-1,2) = solCommands(i-1,3); // Delta } @@ -559,13 +514,12 @@ void MPC::get_solution(){ void MPC::msgCommands(as_msgs::CarCommands *msg){ msg->header.stamp = ros::Time::now(); - msg->motor = solCommands(this->latency, 6); - msg->steering = solCommands(this->latency, 5); - msg->Mtv = solCommands(this->latency, 4); + msg->motor = 0.2; + msg->steering = solCommands(this->latency, 3); + msg->Mtv = solCommands(this->latency, 2); - cout << "steering: " << solCommands(this->latency, 5) << endl; - cout << "motor: " << solCommands(this->latency, 6) << endl; - cout << "Mtv: " << solCommands(this->latency, 4) << endl; + cout << "steering: " << solCommands(this->latency, 3) << endl; + cout << "Mtv: " << solCommands(this->latency, 2) << endl; return; } @@ -611,16 +565,6 @@ Eigen::MatrixXd MPC::output2eigen(double arr[], int size){ return result; } -double MPC::throttle_to_torque(double throttle){ - - return throttle*this->Cm*this->Rwheel; -} - -double MPC::ax_to_throttle(double ax){ - if(ax >= 0) return min(ax/ax_max, 1.0); - else return max(ax/ax_max, -1.0); -} - double MPC::continuous(double psi, double psi_last){ double diff = psi - psi_last; @@ -699,7 +643,6 @@ void MPC::reconfigure(tailored_mpc::dynamicConfig& config){ try{ this->dRd = config.dRd; - this->dRa = config.dRa; this->Dr = config.Dr; this->Df = config.Df; this->Cr = config.Cr; @@ -709,29 +652,23 @@ void MPC::reconfigure(tailored_mpc::dynamicConfig& config){ this->u_r = config.u_r; this->Cd = config.Cd; this->q_slip = config.q_slip; - this->p_long = config.p_long; this->q_n = config.q_n; this->q_nN = config.q_nN; this->q_mu = config.q_mu; - this->lambda = config.lambda; this->q_s = config.q_s; this->latency = config.latency; - this->Cm = config.Cm; this->dMtv = config.dMtv; - this->ax_max = config.Ax_max; - this->ay_max = config.Ay_max; this->q_sN = config.q_sN; - this->q_slack_vx = config.q_slack_vx; this->q_slack_track = config.q_slack_track; - this->bounds.u_min[1] = -config.diff_delta; - this->bounds.u_min[2] = -config.diff_Fm; + this->bounds.u_min[1] = -config.diff_delta*M_PI/180.0; + this->bounds.u_max[0] = config.diff_delta*M_PI/180.0; - this->bounds.u_max[0] = config.diff_delta; - this->bounds.u_max[1] = config.diff_Fm; + this->bounds.x_min[3] = -config.Vy_max; + this->bounds.x_max[3] = config.Vy_max; - this->bounds.x_max[4] = config.Vmax; - this->bounds.x_min[4] = config.Vmin; + this->bounds.x_min[4] = -config.YawRate_max*M_PI/180.0; + this->bounds.x_max[4] = config.YawRate_max*M_PI/180.0; this->dynParamFlag = true; diff --git a/src/utils/vis_tools.cpp b/src/utils/vis_tools.cpp index 9ba4caf..048d997 100644 --- a/src/utils/vis_tools.cpp +++ b/src/utils/vis_tools.cpp @@ -89,7 +89,7 @@ visualization_msgs::MarkerArray VisualizationTools::getMarkersSteering(Eigen::Ma marker.ns= "steering"; marker.id = id++; - q.setRPY( 0, 0, state(i, 2) + commands(i, 3)*3.9); + q.setRPY( 0, 0, state(i, 2) + commands(i, 2)*3.9); marker.pose.position.x = state(i, 0); marker.pose.position.y = state(i, 1);