From 2c0733f5391d3c61c1dbd5fcce28f553dc668ec1 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 27 Sep 2024 23:38:53 +0200 Subject: [PATCH 1/6] simplify sfun wrt multiple declarations --- .../matlab_templates/acados_solver_sfun.in.c | 82 +++++++++---------- 1 file changed, 40 insertions(+), 42 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 7212dd1aa2..d9bd71ce7c 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -609,7 +609,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // local buffer {%- set buffer_size = buffer_sizes | sort | last %} - real_t buffer[{{ buffer_size }}]; + double buffer[{{ buffer_size }}]; double tmp_double; int tmp_offset, tmp_int; @@ -1097,141 +1097,139 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- endif %} /* set outputs */ - // assign pointers to output signals - real_t *out_u0, *out_utraj, *out_xtraj, *out_ztraj, *out_pi_all, *out_status, *out_sqp_iter, *out_KKT_res, *out_KKT_residuals, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin, *out_cost_value, *out_parameter_traj, *out_slack_values; - + double *out_ptr; {%- set i_output = -1 -%}{# note here i_output is 0-based #} {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} {%- set i_output = i_output + 1 %} - out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }}); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_ptr); {%- endif %} {%- if simulink_opts.outputs.utraj == 1 %} {%- set i_output = i_output + 1 %} - out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); for (int ii = 0; ii < N; ii++) ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "u", (void *) (out_utraj + ii * {{ dims.nu }})); + "u", (void *) (out_ptr + ii * {{ dims.nu }})); {%- endif %} {% if simulink_opts.outputs.xtraj == 1 %} {%- set i_output = i_output + 1 %} - out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); for (int ii = 0; ii < {{ solver_options.N_horizon + 1 }}; ii++) ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "x", (void *) (out_xtraj + ii * {{ dims.nx }})); + "x", (void *) (out_ptr + ii * {{ dims.nx }})); {%- endif %} {% if simulink_opts.outputs.ztraj == 1 %} {%- set i_output = i_output + 1 %} - out_ztraj = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); for (int ii = 0; ii < N; ii++) ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "z", (void *) (out_ztraj + ii * {{ dims.nz }})); + "z", (void *) (out_ptr + ii * {{ dims.nz }})); {%- endif %} {% if simulink_opts.outputs.pi_all == 1 %} {%- set i_output = i_output + 1 %} - out_pi_all = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); for (int ii = 0; ii < N; ii++) ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "pi", (void *) (out_pi_all + ii * {{ dims.nx }})); + "pi", (void *) (out_ptr + ii * {{ dims.nx }})); {%- endif %} {% if simulink_opts.outputs.slack_values == 1 %} {%- set i_output = i_output + 1 %} - out_slack_values = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); tmp_offset = 0; for (int ii = 0; ii <= N; ii++) { tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, ii, "sl"); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "sl", (void *) (out_slack_values + tmp_offset)); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "sl", (void *) (out_ptr + tmp_offset)); tmp_offset += tmp_int; - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "su", (void *) (out_slack_values + tmp_offset)); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "su", (void *) (out_ptr + tmp_offset)); tmp_offset += tmp_int; } {%- endif %} {%- if simulink_opts.outputs.solver_status == 1 %} {%- set i_output = i_output + 1 %} - out_status = ssGetOutputPortRealSignal(S, {{ i_output }}); - *out_status = (real_t) acados_status; + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + *out_ptr = (double) acados_status; {%- endif %} {%- if simulink_opts.outputs.cost_value == 1 %} {%- set i_output = i_output + 1 %} - out_cost_value = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); ocp_nlp_eval_cost(capsule->nlp_solver, nlp_in, nlp_out); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "cost_value", (void *) out_cost_value); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "cost_value", (void *) out_ptr); {%- endif %} {%- if simulink_opts.outputs.KKT_residual == 1 %} {%- set i_output = i_output + 1 %} - out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }}); - *out_KKT_res = (real_t) nlp_out->inf_norm_res; + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + *out_ptr = (double) nlp_out->inf_norm_res; {%- endif %} {%- if simulink_opts.outputs.KKT_residuals == 1 %} {%- set i_output = i_output + 1 %} - out_KKT_residuals = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); {%- if solver_options.nlp_solver_type == "SQP_RTI" %} ocp_nlp_eval_residuals(capsule->nlp_solver, nlp_in, nlp_out); {%- endif %} - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_stat", (void *) &out_KKT_residuals[0]); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_eq", (void *) &out_KKT_residuals[1]); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_ineq", (void *) &out_KKT_residuals[2]); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_comp", (void *) &out_KKT_residuals[3]); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_stat", (void *) &out_ptr[0]); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_eq", (void *) &out_ptr[1]); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_ineq", (void *) &out_ptr[2]); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_comp", (void *) &out_ptr[3]); {%- endif %} {%- if solver_options.N_horizon > 0 and simulink_opts.outputs.x1 == 1 %} {%- set i_output = i_output + 1 %} - out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }}); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_ptr); {%- endif %} {%- if simulink_opts.outputs.CPU_time == 1 %} {%- set i_output = i_output + 1 %} - out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }}); - out_cpu_time[0] = tmp_double; + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr[0] = tmp_double; {%- endif -%} {%- if simulink_opts.outputs.CPU_time_sim == 1 %} {%- set i_output = i_output + 1 %} - out_cpu_time_sim = ssGetOutputPortRealSignal(S, {{ i_output }}); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_cpu_time_sim); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_ptr); {%- endif -%} {%- if simulink_opts.outputs.CPU_time_qp == 1 %} {%- set i_output = i_output + 1 %} - out_cpu_time_qp = ssGetOutputPortRealSignal(S, {{ i_output }}); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_cpu_time_qp); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_ptr); {%- endif -%} {%- if simulink_opts.outputs.CPU_time_lin == 1 %} {%- set i_output = i_output + 1 %} - out_cpu_time_lin = ssGetOutputPortRealSignal(S, {{ i_output }}); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_cpu_time_lin); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_ptr); {%- endif -%} {%- if simulink_opts.outputs.sqp_iter == 1 %} {%- set i_output = i_output + 1 %} - out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); // get sqp iter ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int); - *out_sqp_iter = (real_t) tmp_int; + *out_ptr = (double) tmp_int; {%- endif %} {% if simulink_opts.outputs.parameter_traj == 1 %} {%- set i_output = i_output + 1 %} - out_parameter_traj = ssGetOutputPortRealSignal(S, {{ i_output }}); + out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); for (int ii = 0; ii < N+1; ii++) ocp_nlp_in_get(nlp_config, nlp_dims, nlp_in, ii, - "p", (void *) (out_parameter_traj + ii * {{ dims.np }})); + "p", (void *) (out_ptr + ii * {{ dims.np }})); {%- endif %} } From 2359516c22e819c02c4cd027e76053239312ea9a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Sep 2024 13:59:59 +0200 Subject: [PATCH 2/6] parameter_traj: towards varying dimension --- .../matlab_templates/acados_solver_sfun.in.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index d9bd71ce7c..65f18e4538 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -638,13 +638,17 @@ static void mdlOutputs(SimStruct *S, int_T tid) // parameters - stage-variant !!! {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - // update value of parameters - for (int ii = 0; ii <= N; ii++) + tmp_offset = 0; + for (int stage = 0; stage <= N; stage++) { - for (int jj = 0; jj < {{ dims.np }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]); - {{ name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }}); + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "p"); + for (int jj = 0; jj < tmp_int; jj++) + { + buffer[jj] = (double)(*in_sign[tmp_offset+jj]); + } + {{ name }}_acados_update_params(capsule, stage, buffer, tmp_int); + tmp_offset += tmp_int; } {%- endif %} From 5eac1489c7cc629fe7feaebeae1dccc393452ea0 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Sep 2024 14:03:23 +0200 Subject: [PATCH 3/6] sfun: use stage as index name --- .../matlab_templates/acados_solver_sfun.in.c | 127 +++++++++--------- 1 file changed, 63 insertions(+), 64 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 65f18e4538..2a005bf5fa 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -668,11 +668,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < N; ii++) + for (int stage = 1; stage < N; stage++) { for (int jj = 0; jj < {{ dims.ny }}; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); + buffer[jj] = (double)(*in_sign[(stage-1)*{{ dims.ny }}+jj]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, stage, "yref", (void *) buffer); } {%- endif %} @@ -691,22 +691,22 @@ static void mdlOutputs(SimStruct *S, int_T tid) // lbx {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < N; ii++) + for (int stage = 1; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nbx }}; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer); + buffer[jj] = (double)(*in_sign[(stage-1)*{{ dims.nbx }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lbx", (void *) buffer); } {%- endif %} {%- if dims.nbx > 0 and solver_options.N_horizon > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} // ubx {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < N; ii++) + for (int stage = 1; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nbx }}; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer); + buffer[jj] = (double)(*in_sign[(stage-1)*{{ dims.nbx }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ubx", (void *) buffer); } {%- endif %} @@ -735,22 +735,22 @@ static void mdlOutputs(SimStruct *S, int_T tid) // lbu {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nbu }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer); + buffer[jj] = (double)(*in_sign[stage*{{ dims.nbu }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lbu", (void *) buffer); } {%- endif -%} {%- if dims.nbu > 0 and solver_options.N_horizon > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} // ubu {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nbu }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer); + buffer[jj] = (double)(*in_sign[stage*{{ dims.nbu }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ubu", (void *) buffer); } {%- endif -%} @@ -759,11 +759,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.ng }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", (void *) buffer); + buffer[jj] = (double)(*in_sign[stage*{{ dims.ng }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lg", (void *) buffer); } {%- endif -%} {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} @@ -771,11 +771,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.ng }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", (void *) buffer); + buffer[jj] = (double)(*in_sign[stage*{{ dims.ng }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ug", (void *) buffer); } {%- endif -%} @@ -784,11 +784,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nh }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", (void *) buffer); + buffer[jj] = (double)(*in_sign[stage*{{ dims.nh }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lh", (void *) buffer); } {%- endif -%} {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} @@ -796,11 +796,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nh }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", (void *) buffer); + buffer[jj] = (double)(*in_sign[stage*{{ dims.nh }}+jj]); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "uh", (void *) buffer); } {%- endif -%} @@ -855,8 +855,8 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.ny * dims.ny }}; i++) buffer[i] = (double)(*in_sign[i]); - for (int ii = 1; ii < N; ii++) - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer); + for (int stage = 1; stage < N; stage++) + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, stage, "W", buffer); {%- endif %} {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} @@ -953,11 +953,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) // x_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < {{ solver_options.N_horizon + 1 }}; ii++) + for (int stage = 0; stage < {{ solver_options.N_horizon + 1 }}; stage++) { for (int jj = 0; jj < {{ dims.nx }}; jj++) - buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer); + buffer[jj] = (double)(*in_sign[(stage)*{{ dims.nx }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "x", (void *) buffer); } {%- endif %} @@ -965,11 +965,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) // u_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nu }}; jj++) - buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer); + buffer[jj] = (double)(*in_sign[(stage)*{{ dims.nu }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "u", (void *) buffer); } {%- endif %} @@ -977,11 +977,11 @@ static void mdlOutputs(SimStruct *S, int_T tid) // pi_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int stage = 0; stage < N; stage++) { for (int jj = 0; jj < {{ dims.nx }}; jj++) - buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "pi", (void *) buffer); + buffer[jj] = (double)(*in_sign[(stage)*{{ dims.nx }}+jj]); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "pi", (void *) buffer); } {%- endif %} @@ -990,19 +990,19 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); tmp_offset = 0; - for (int ii = 0; ii <= N; ii++) + for (int stage = 0; stage <= N; stage++) { - tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, ii, "sl"); + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "sl"); // set sl for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); tmp_offset += tmp_int; - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "sl", (void *) buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "sl", (void *) buffer); // set su for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); tmp_offset += tmp_int; - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "su", (void *) buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "su", (void *) buffer); } {%- endif %} } @@ -1040,15 +1040,15 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- endfor %} // update for stages - for (int ii = {{ stage_idx_0 }}; ii < {{ stage_idx_e }}+1; ii++) + for (int stage = {{ stage_idx_0 }}; stage < {{ stage_idx_e }}+1; stage++) { - tmp_offset = 1 + (ii - {{ stage_idx_0 }}) * {{ param_length }}; + tmp_offset = 1 + (stage - {{ stage_idx_0 }}) * {{ param_length }}; // copy new parameter values to buffer for (int jj = 0; jj < {{ param_length }}; jj++) { buffer[jj] = (double)(*in_sign[jj + tmp_offset]); } - {{ name }}_acados_update_params_sparse(capsule, ii, idx, buffer, {{ param_length }}); + {{ name }}_acados_update_params_sparse(capsule, stage, idx, buffer, {{ param_length }}); } } {%- endif -%} @@ -1112,48 +1112,48 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- if simulink_opts.outputs.utraj == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "u", (void *) (out_ptr + ii * {{ dims.nu }})); + for (int stage = 0; stage < N; stage++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, + "u", (void *) (out_ptr + stage * {{ dims.nu }})); {%- endif %} {% if simulink_opts.outputs.xtraj == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < {{ solver_options.N_horizon + 1 }}; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "x", (void *) (out_ptr + ii * {{ dims.nx }})); + for (int stage = 0; stage < {{ solver_options.N_horizon + 1 }}; stage++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, + "x", (void *) (out_ptr + stage * {{ dims.nx }})); {%- endif %} {% if simulink_opts.outputs.ztraj == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "z", (void *) (out_ptr + ii * {{ dims.nz }})); + for (int stage = 0; stage < N; stage++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, + "z", (void *) (out_ptr + stage * {{ dims.nz }})); {%- endif %} {% if simulink_opts.outputs.pi_all == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, - "pi", (void *) (out_ptr + ii * {{ dims.nx }})); + for (int stage = 0; stage < N; stage++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, + "pi", (void *) (out_ptr + stage * {{ dims.nx }})); {%- endif %} {% if simulink_opts.outputs.slack_values == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); tmp_offset = 0; - for (int ii = 0; ii <= N; ii++) + for (int stage = 0; stage <= N; stage++) { - tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, ii, "sl"); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "sl", (void *) (out_ptr + tmp_offset)); + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "sl"); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, "sl", (void *) (out_ptr + tmp_offset)); tmp_offset += tmp_int; - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "su", (void *) (out_ptr + tmp_offset)); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, "su", (void *) (out_ptr + tmp_offset)); tmp_offset += tmp_int; } {%- endif %} @@ -1231,9 +1231,8 @@ static void mdlOutputs(SimStruct *S, int_T tid) {% if simulink_opts.outputs.parameter_traj == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < N+1; ii++) - ocp_nlp_in_get(nlp_config, nlp_dims, nlp_in, ii, - "p", (void *) (out_ptr + ii * {{ dims.np }})); + for (int stage = 0; stage < N+1; stage++) + ocp_nlp_in_get(nlp_config, nlp_dims, nlp_in, stage, "p", (void *) (out_ptr + stage * {{ dims.np }})); {%- endif %} } From 0189e3abe9ffd10624bb6d20d10951c14e3f2c13 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Sep 2024 14:41:14 +0200 Subject: [PATCH 4/6] Simulink: towards varying dimensions --- .../matlab_templates/acados_solver_sfun.in.c | 36 +++++++++++++------ 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 2a005bf5fa..69ce07c666 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -635,7 +635,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- endif %} {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} - // parameters - stage-variant !!! + // parameter_traj {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); // update value of parameters @@ -1119,29 +1119,38 @@ static void mdlOutputs(SimStruct *S, int_T tid) {% if simulink_opts.outputs.xtraj == 1 %} {%- set i_output = i_output + 1 %} - out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + tmp_offset = 0; for (int stage = 0; stage < {{ solver_options.N_horizon + 1 }}; stage++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, - "x", (void *) (out_ptr + stage * {{ dims.nx }})); + { + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "x"); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, "x", (void *) (out_ptr + tmp_offset)); + tmp_offset += tmp_int; + } {%- endif %} {% if simulink_opts.outputs.ztraj == 1 %} {%- set i_output = i_output + 1 %} - out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + tmp_offset = 0; for (int stage = 0; stage < N; stage++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, - "z", (void *) (out_ptr + stage * {{ dims.nz }})); + { + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "z"); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, "z", (void *) (out_ptr + tmp_offset)); + tmp_offset += tmp_int; + } {%- endif %} {% if simulink_opts.outputs.pi_all == 1 %} {%- set i_output = i_output + 1 %} - out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + tmp_offset = 0; for (int stage = 0; stage < N; stage++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, - "pi", (void *) (out_ptr + stage * {{ dims.nx }})); + { + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "pi"); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, "pi", (void *) (out_ptr + stage * {{ dims.nx }})); + tmp_offset += tmp_int; + } {%- endif %} {% if simulink_opts.outputs.slack_values == 1 %} @@ -1231,8 +1240,13 @@ static void mdlOutputs(SimStruct *S, int_T tid) {% if simulink_opts.outputs.parameter_traj == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + tmp_offset = 0; for (int stage = 0; stage < N+1; stage++) - ocp_nlp_in_get(nlp_config, nlp_dims, nlp_in, stage, "p", (void *) (out_ptr + stage * {{ dims.np }})); + { + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "p"); + ocp_nlp_in_get(nlp_config, nlp_dims, nlp_in, stage, "p", (void *) (out_ptr + tmp_offset)); + tmp_offset += tmp_int; + } {%- endif %} } From d2da1532ef02d5d1792da7338e046ac0bb69c549 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Sep 2024 14:46:44 +0200 Subject: [PATCH 5/6] Simulink: towards varying dimensions: inits --- .../matlab_templates/acados_solver_sfun.in.c | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 69ce07c666..69727d71aa 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -953,11 +953,14 @@ static void mdlOutputs(SimStruct *S, int_T tid) // x_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + tmp_offset = 0; for (int stage = 0; stage < {{ solver_options.N_horizon + 1 }}; stage++) { - for (int jj = 0; jj < {{ dims.nx }}; jj++) - buffer[jj] = (double)(*in_sign[(stage)*{{ dims.nx }}+jj]); + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "x"); + for (int jj = 0; jj < tmp_int; jj++) + buffer[jj] = (double)(*in_sign[tmp_offset+jj]); ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "x", (void *) buffer); + tmp_offset += tmp_int; } {%- endif %} @@ -965,11 +968,14 @@ static void mdlOutputs(SimStruct *S, int_T tid) // u_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + tmp_offset = 0; for (int stage = 0; stage < N; stage++) { + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "u"); for (int jj = 0; jj < {{ dims.nu }}; jj++) - buffer[jj] = (double)(*in_sign[(stage)*{{ dims.nu }}+jj]); + buffer[jj] = (double)(*in_sign[tmp_offset+jj]); ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "u", (void *) buffer); + tmp_offset += tmp_int; } {%- endif %} @@ -977,11 +983,14 @@ static void mdlOutputs(SimStruct *S, int_T tid) // pi_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + tmp_offset = 0; for (int stage = 0; stage < N; stage++) { - for (int jj = 0; jj < {{ dims.nx }}; jj++) - buffer[jj] = (double)(*in_sign[(stage)*{{ dims.nx }}+jj]); + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "pi"); + for (int jj = 0; jj < tmp_int; jj++) + buffer[jj] = (double)(*in_sign[tmp_offset+jj]); ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, stage, "pi", (void *) buffer); + tmp_offset += tmp_int; } {%- endif %} From a291f59241785f31cdde5f8d4b1adc2cd9dbb772 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Sep 2024 15:10:12 +0200 Subject: [PATCH 6/6] utraj varying dims --- .../matlab_templates/acados_solver_sfun.in.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 69727d71aa..6874d22853 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -1121,9 +1121,13 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- if simulink_opts.outputs.utraj == 1 %} {%- set i_output = i_output + 1 %} out_ptr = ssGetOutputPortRealSignal(S, {{ i_output }}); + tmp_offset = 0; for (int stage = 0; stage < N; stage++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, - "u", (void *) (out_ptr + stage * {{ dims.nu }})); + { + tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "u"); + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, stage, "u", (void *) (out_ptr + tmp_offset)); + tmp_offset += tmp_int; + } {%- endif %} {% if simulink_opts.outputs.xtraj == 1 %}