Hi.
I have been trying to port the race_cars
example to MATLAB but am currently struggling during code generation which leads to the following stack violation:
Configuration:
Crash Decoding : Disabled - No sandbox or build area path
Crash Mode : continue (default)
Default Encoding : UTF-8
Deployed : false
Desktop Environment : ubuntu:GNOME
GNU C Library : 2.27 stable
Graphics Driver : Unknown hardware
Graphics card 1 : 0x10de ( 0x10de ) 0x1fb8 Version 440.33.1.0 (0-0-0)
Graphics card 2 : 0x8086 ( 0x8086 ) 0x3e9b Version 0.0.0.0 (0-0-0)
Java Version : Java 1.8.0_202-b08 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
MATLAB Architecture : glnxa64
MATLAB Entitlement ID : 6428867
MATLAB Root : /usr/local/MATLAB/R2019b
MATLAB Version : 9.7.0.1296695 (R2019b) Update 4
OpenGL : hardware
Operating System : Ubuntu 18.04.4 LTS
Process ID : 29842
Processor ID : x86 Family 6 Model 158 Stepping 13, GenuineIntel
Session Key : 96c58266-4b3a-4410-954f-77f2426cc916
Static TLS mitigation : Enabled: Full
Window System : The X.Org Foundation (12005000), display :0
Fault Count: 5
Abnormal termination:
Segmentation violation
Register State (from fault):
RAX = 0000000000000000 RBX = 00007fa10e727020
RCX = 0000000000000005 RDX = 0000000000000007
RSP = 00007fa38bff9d00 RBP = 00007fa38bffa110
RSI = 0000000000000000 RDI = 00007fa10e727020
R8 = 00007fa38bff9b90 R9 = 00007fa29df705ec
R10 = 00007fa29f746d08 R11 = 00007fa29f746dd4
R12 = 00007fa29f741c50 R13 = 00007fa29df56090
R14 = 00007fa2748cdde0 R15 = 0000000000000032
RIP = 00007fa10e720619 EFL = 0000000000010206
CS = 0033 FS = 0000 GS = 0000
Stack Trace (from fault):
[ 0] 0x00007fa10e720619 /home/thomas/build/ocp_create.mexa64+00009753 mexFunction+00005193
[ 1] 0x00007fa397eb6de3 /usr/local/MATLAB/R2019b/bin/glnxa64/libmex.so+00548323
[ 2] 0x00007fa397eb6ee5 /usr/local/MATLAB/R2019b/bin/glnxa64/libmex.so+00548581
[ 3] 0x00007fa397eb7317 /usr/local/MATLAB/R2019b/bin/glnxa64/libmex.so+00549655
[ 4] 0x00007fa397eb7f93 /usr/local/MATLAB/R2019b/bin/glnxa64/libmex.so+00552851
[ 5] 0x00007fa397ea37ac /usr/local/MATLAB/R2019b/bin/glnxa64/libmex.so+00468908
[ 6] 0x00007fa398cd815f /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_dispatcher.so+01073503 _ZN8Mfh_file20dispatch_file_commonEMS_FviPP11mxArray_tagiS2_EiS2_iS2_+00000207
[ 7] 0x00007fa398cd9c5e /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_dispatcher.so+01080414
[ 8] 0x00007fa398cda1a1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_dispatcher.so+01081761 _ZN8Mfh_file8dispatchEiPSt10unique_ptrI11mxArray_tagN6matrix6detail17mxDestroy_deleterEEiPPS1_+00000033
[ 9] 0x00007fa396373323 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14025507
[ 10] 0x00007fa3963780d6 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14045398
[ 11] 0x00007fa39647eebe /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+15122110
[ 12] 0x00007fa396472211 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+15069713
[ 13] 0x00007fa3963dee34 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14466612
[ 14] 0x00007fa39640544d /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14623821
[ 15] 0x00007fa395b8483b /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05707835
[ 16] 0x00007fa395b86a74 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05716596
[ 17] 0x00007fa395b8381d /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05703709
[ 18] 0x00007fa395b70cf1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05627121
[ 19] 0x00007fa395b70f29 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05627689
[ 20] 0x00007fa395b83026 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05701670
[ 21] 0x00007fa395b83126 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05701926
[ 22] 0x00007fa395cbcc49 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+06986825
[ 23] 0x00007fa395cc0383 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+07000963
[ 24] 0x00007fa39622f0f1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+12697841
[ 25] 0x00007fa3963608a1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+13949089
[ 26] 0x00007fa39636199d /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+13953437
[ 27] 0x00007fa398cd815f /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_dispatcher.so+01073503 _ZN8Mfh_file20dispatch_file_commonEMS_FviPP11mxArray_tagiS2_EiS2_iS2_+00000207
[ 28] 0x00007fa398cd9c5e /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_dispatcher.so+01080414
[ 29] 0x00007fa398cda1a1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_dispatcher.so+01081761 _ZN8Mfh_file8dispatchEiPSt10unique_ptrI11mxArray_tagN6matrix6detail17mxDestroy_deleterEEiPPS1_+00000033
[ 30] 0x00007fa396373323 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14025507
[ 31] 0x00007fa3963780d6 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14045398
[ 32] 0x00007fa39647eebe /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+15122110
[ 33] 0x00007fa39647236c /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+15070060
[ 34] 0x00007fa3963dee34 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14466612
[ 35] 0x00007fa39640547d /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+14623869
[ 36] 0x00007fa395b8483b /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05707835
[ 37] 0x00007fa395b86a74 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05716596
[ 38] 0x00007fa395b8381d /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05703709
[ 39] 0x00007fa395b70cf1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05627121
[ 40] 0x00007fa395b70f29 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05627689
[ 41] 0x00007fa395b83026 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05701670
[ 42] 0x00007fa395b83126 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+05701926
[ 43] 0x00007fa395cbcc49 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+06986825
[ 44] 0x00007fa395cc0383 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+07000963
[ 45] 0x00007fa39622f0f1 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+12697841
[ 46] 0x00007fa3961d9693 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+12347027
[ 47] 0x00007fa3961ddd2f /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+12365103
[ 48] 0x00007fa3961e10e2 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+12378338
[ 49] 0x00007fa39627df8f /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+13021071
[ 50] 0x00007fa39627e279 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwm_lxe.so+13021817
[ 51] 0x00007fa397f464c4 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwbridge.so+00341188 _Z8mnParserv+00000596
[ 52] 0x00007fa398e235b5 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmcr.so+01017269
[ 53] 0x00007fa3adb2242b /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmvm.so+03097643 _ZN14cmddistributor15PackagedTaskIIP10invokeFuncIN7mwboost8functionIFvvEEEEENS2_10shared_ptrINS2_13unique_futureIDTclfp_EEEEEERKT_+00000059
[ 54] 0x00007fa3adb22518 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmvm.so+03097880 _ZNSt17_Function_handlerIFN7mwboost3anyEvEZN14cmddistributor15PackagedTaskIIP10createFuncINS0_8functionIFvvEEEEESt8functionIS2_ET_EUlvE_E9_M_invokeERKSt9_Any_data+00000024
[ 55] 0x00007fa398f7389c /usr/local/MATLAB/R2019b/bin/glnxa64/libmwiqm.so+00751772 _ZN7mwboost6detail8function21function_obj_invoker0ISt8functionIFNS_3anyEvEES4_E6invokeERNS1_15function_bufferE+00000028
[ 56] 0x00007fa398f73557 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwiqm.so+00750935 _ZN3iqm18PackagedTaskPlugin7executeEP15inWorkSpace_tag+00000439
[ 57] 0x00007fa398e12015 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmcr.so+00946197
[ 58] 0x00007fa398f586a0 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwiqm.so+00640672
[ 59] 0x00007fa398f3ce01 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwiqm.so+00527873
[ 60] 0x00007fa398f3da7f /usr/local/MATLAB/R2019b/bin/glnxa64/libmwiqm.so+00531071
[ 61] 0x00007fa398df9575 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmcr.so+00845173
[ 62] 0x00007fa398df9b93 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmcr.so+00846739
[ 63] 0x00007fa398dfa404 /usr/local/MATLAB/R2019b/bin/glnxa64/libmwmcr.so+00848900
[ 64] 0x00007fa3abdf7bdd /usr/local/MATLAB/R2019b/bin/glnxa64/libmwboost_thread.so.1.65.1+00080861
[ 65] 0x00007fa3acdc46db /lib/x86_64-linux-gnu/libpthread.so.0+00030427
[ 66] 0x00007fa3ac54da3f /lib/x86_64-linux-gnu/libc.so.6+01186367 clone+00000063
[ 67] 0x0000000000000000 <unknown-module>+00000000
This error was detected while a MEX-file was running. If the MEX-file
is not an official MathWorks function, please examine its source code
for errors. Please consult the External Interfaces Guide for information
on debugging MEX-files.
The part of the MATLAB code that I use to define the Acados solver is included below:
clear all
import casadi.*
%% Solver parameters
nlp_solver = 'sqp'; % sqp, sqp_rti
qp_solver = 'partial_condensing_hpipm';
% full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases
qp_solver_cond_N = 5; % for partial condensing
% integrator type
sim_method = 'erk'; % erk, irk, irk_gnsf
%% horizon parameters
N = 50;
T = 1.0; % time horizon length
%% model dynamics
[model, constraint] = bicycle_model(track_file);
nx = length(model.x);
nu = length(model.u);
%% model to create the solver
ocp_model = acados_ocp_model();
%% acados ocp model
ocp_model.set('name', model.name);
ocp_model.set('T', T);
% symbolics
ocp_model.set('sym_x', model.x);
ocp_model.set('sym_u', model.u);
ocp_model.set('sym_xdot', model.xdot);
ocp_model.set('sym_z', model.z);
ocp_model.set('sym_p', model.p);
% dynamics
if (strcmp(sim_method, 'erk'))
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.f_expl_expr);
else % irk irk_gnsf
ocp_model.set('dyn_type', 'implicit');
ocp_model.set('dyn_expr_f', model.f_impl_expr);
end
% constraintsJbx = zeros(1,nx);
nbx = 1;
Jbx(nbx,2) = 1;
ocp_model.set('constr_Jbx', Jbx);
ocp_model.set('constr_lbx', [-12]);
ocp_model.set('constr_ubx', [12]);
nbu = 2;
Jbu = zeros(nbu,nu);
Jbu(1,1) = 1;
Jbu(2,2) = 1;
ocp_model.set('constr_Jbu', Jbu);
ocp_model.set('constr_lbu', [model.dthrottle_min, model.ddelta_min]);
ocp_model.set('constr_ubu', [model.dthrottle_max, model.ddelta_max]);
%ocp_model.set('constr_type', 'bgh');
ocp_model.set('constr_expr_h', constraint.expr);
ocp_model.set('constr_lh', [...
constraint.along_min,...
constraint.alat_min,...
model.n_min,...
model.throttle_min,...
model.delta_min,...
]);
ocp_model.set('constr_uh', [...
constraint.along_max,...
constraint.alat_max,...
model.n_max,...
model.throttle_max,...
model.delta_max,...
]);
%ocp_model.set('constr_expr_h_e', constraint.expr);
%ocp_model.set('constr_lh_e', 0);
%ocp_model.set('constr_uh_e', 0);
%ocp.constraints.idxsh = np.array([0, 2])
% Upper and lower bounds on slacks, are zero by default.
% ocp.constraints.lsh = np.zeros(ocp.dims.nsh)
% ocp.constraints.ush = np.zeros(ocp.dims.nsh)
% set intial condition
ocp_model.set('constr_x0', model.x0);
% cost = define linear cost on x and u
%ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost);
%ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e);
ocp_model.set('cost_type', 'linear_ls');
ocp_model.set('cost_type_e', 'linear_ls');
% number of outputs is the concatenation of x and u
ny = nx + nu;
ny_e = nx;
% The linear cost contributions is defined through Vx, Vu and Vz
Vx = zeros(ny, nx);
Vx_e = zeros(ny_e, nx);
Vu = zeros(ny, nu);
Vx(1:nx,:) = eye(nx);
Vx_e(1:nx,:) = eye(nx);
Vu(nx+1:end,:) = eye(nu);
ocp_model.set('cost_Vx', Vx);
ocp_model.set('cost_Vx_e', Vx_e);
ocp_model.set('cost_Vu', Vu);
% Define cost on states and input
Q = diag([ 1e-1, 1e-8, 1e-8, 1e-8, 1e-3, 5e-3 ]);
R = eye(nu);
R(1, 1) = 1e-3;
R(2, 2) = 5e-3;
Qe = diag([ 5e0, 1e1, 1e-8, 1e-8, 5e-3, 2e-3 ]);
unscale = N / T;
W = unscale * blkdiag(Q, R);
W_e = Qe / unscale;
ocp_model.set('cost_W', W);
ocp_model.set('cost_W_e', W_e);
% set intial references
y_ref = zeros(ny,1);
y_ref_e = zeros(ny_e,1);
y_ref(1) = 1; % set reference on 's' to 1 to push the car forward (progress)
ocp_model.set('cost_y_ref', y_ref);
ocp_model.set('cost_y_ref_e', y_ref_e);
% Set dimensions
ocp_model.set('dim_nx', nx);
ocp_model.set('dim_nu', nu);
ocp_model.set('dim_ny', ny);
ocp_model.set('dim_ny_e', ny_e);
ocp_model.set('dim_nbx', nbx);
ocp_model.set('dim_nbu', nbu);
% ... see ocp_model.model_struct to see what other fields can be set
%% acados ocp set opts
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('nlp_solver_exact_hessian', false); % false=gauss_newton, true=exact
ocp_opts.set('sim_method', sim_method);
ocp_opts.set('sim_method_num_stages', 4);
ocp_opts.set('sim_method_num_steps', 3);
ocp_opts.set('qp_solver', qp_solver);
%ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);
%ocp_opts.set('nlp_solver_tol_stat', 1e-4);
%ocp_opts.set('nlp_solver_tol_eq', 1e-4);
%ocp_opts.set('nlp_solver_tol_ineq', 1e-4);
%ocp_opts.set('nlp_solver_tol_comp', 1e-4);
% ... see ocp_opts.opts_struct to see what other fields can be set
%% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts);
In the model definition I am using CasADi MX objects? Would that matter?
I am also using GCC 7.4.0 but I am able to compile and run the other MATLAB examples without any problems.
Any clues?
Best regards
Thomas Jespersen