From 8ec19fd8377139bd7dc0c8751edfe032d0e2a673 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Jun 2025 09:25:23 +0200 Subject: [PATCH] replace fmax & fmin in acados with MAX, MIN macros --- acados/ocp_nlp/ocp_nlp_common.c | 9 +++++---- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 7 ++++--- .../ocp_nlp_globalization_merit_backtracking.c | 3 ++- acados/ocp_nlp/ocp_nlp_qpscaling.c | 14 +++++++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 17 +++++++++-------- acados/ocp_qp/ocp_qp_osqp.c | 5 +++-- acados/utils/math.c | 2 +- 7 files changed, 31 insertions(+), 26 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 4030ad389d..93db7834b1 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -46,6 +46,7 @@ // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" +#include "acados/utils/math.h" #include "acados/utils/strsep.h" // openmp #if defined(ACADOS_WITH_OPENMP) @@ -2586,7 +2587,7 @@ double ocp_nlp_compute_dual_pi_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out { for (j=0; jpi+i, j))); + norm_pi = MAX(norm_pi, fabs(BLASFEO_DVECEL(nlp_out->pi+i, j))); } } return norm_pi; @@ -2603,7 +2604,7 @@ double ocp_nlp_compute_dual_lam_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_ou { for (j=0; j<2*dims->ni[i]; j++) { - norm_lam = fmax(norm_lam, fabs(BLASFEO_DVECEL(nlp_out->lam+i, j))); + norm_lam = MAX(norm_lam, fabs(BLASFEO_DVECEL(nlp_out->lam+i, j))); } } return norm_lam; @@ -2850,13 +2851,13 @@ static void adaptive_levenberg_marquardt_update_mu(double iter, double step_size if (step_size == 1.0) { double mu_tmp = mem->adaptive_levenberg_marquardt_mu; - mem->adaptive_levenberg_marquardt_mu = fmax(opts->adaptive_levenberg_marquardt_mu_min, + mem->adaptive_levenberg_marquardt_mu = MAX(opts->adaptive_levenberg_marquardt_mu_min, mem->adaptive_levenberg_marquardt_mu_bar/(opts->adaptive_levenberg_marquardt_lam)); mem->adaptive_levenberg_marquardt_mu_bar = mu_tmp; } else { - mem->adaptive_levenberg_marquardt_mu = fmin(opts->adaptive_levenberg_marquardt_lam * mem->adaptive_levenberg_marquardt_mu, 1.0); + mem->adaptive_levenberg_marquardt_mu = MIN(opts->adaptive_levenberg_marquardt_lam * mem->adaptive_levenberg_marquardt_mu, 1.0); } } } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index ef81c83623..151fb134fd 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -49,6 +49,7 @@ // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" +#include "acados/utils/math.h" /************************************************ @@ -226,7 +227,7 @@ void *ocp_nlp_globalization_funnel_memory_assign(void *config_, void *dims_, voi void initialize_funnel_width(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double initial_infeasibility) { - mem->funnel_width = fmax(opts->initialization_upper_bound, + mem->funnel_width = MAX(opts->initialization_upper_bound, opts->initialization_increase_factor*initial_infeasibility); } @@ -251,7 +252,7 @@ void update_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem, } if (mem->penalty_parameter * pred_optimality + pred_infeasibility < opts->penalty_eta * pred_infeasibility) { - mem->penalty_parameter = fmax(0.0, //objective multiplier should always be >= 0! + mem->penalty_parameter = MAX(0.0, //objective multiplier should always be >= 0! fmin(opts->penalty_contraction * mem->penalty_parameter, ((1-opts->penalty_eta) * pred_infeasibility) / (-pred_optimality + 1e-9)) ); @@ -308,7 +309,7 @@ bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalizat double pred, double alpha) { - if (negative_ared <= fmin(globalization_opts->eps_sufficient_descent * alpha * fmax(pred, 0) + 1e-18, 0)) + if (negative_ared <= fmin(globalization_opts->eps_sufficient_descent * alpha * MAX(pred, 0) + 1e-18, 0)) { return true; } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index e4c1848e8c..9f2a3275b7 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -44,6 +44,7 @@ #include "acados/ocp_nlp/ocp_nlp_globalization_common.h" #include "acados/ocp_nlp/ocp_nlp_common.h" #include "acados/utils/mem.h" +#include "acados/utils/math.h" // blasfeo #include "blasfeo_d_aux.h" @@ -945,7 +946,7 @@ static int ocp_nlp_ddp_backtracking_line_search(ocp_nlp_config *config, ocp_nlp_ negative_ared = trial_cost - nlp_mem->cost_value; // Check Armijo sufficient decrease condition - if (negative_ared <= fmin(-globalization_opts->eps_sufficient_descent*alpha* fmax(pred, 0) + 1e-18, 0)) + if (negative_ared <= fmin(-globalization_opts->eps_sufficient_descent*alpha* MAX(pred, 0) + 1e-18, 0)) { // IF step accepted: update x // reset evaluation point to SQP iterate diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.c b/acados/ocp_nlp/ocp_nlp_qpscaling.c index 903735a443..b033af4920 100644 --- a/acados/ocp_nlp/ocp_nlp_qpscaling.c +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.c @@ -295,7 +295,7 @@ static double norm_inf_matrix_col(int col_idx, int col_length, struct blasfeo_d for (int j = 0; j < col_length; ++j) { double tmp = BLASFEO_DMATEL(At, j, col_idx); - norm = fmax(norm, fabs(tmp)); + norm = MAX(norm, fabs(tmp)); } return norm; } @@ -475,14 +475,14 @@ void ocp_nlp_qpscaling_compute_obj_scaling_factor(ocp_nlp_qpscaling_dims *dims, for (int stage = 0; stage <= dim->N; stage++) { compute_gershgorin_max_abs_eig_estimate(nx[stage]+nu[stage], RSQrq+stage, &tmp); - max_abs_eig = fmax(max_abs_eig, tmp); + max_abs_eig = MAX(max_abs_eig, tmp); // take Z into account blasfeo_dvecnrm_inf(2*ns[stage], qp_in->Z+stage, 0, &tmp); - max_abs_eig = fmax(max_abs_eig, tmp); + max_abs_eig = MAX(max_abs_eig, tmp); // norm gradient blasfeo_dvecnrm_inf(nx[stage]+nu[stage]+2*ns[stage], qp_in->rqz+stage, 0, &tmp); - nrm_inf_grad_obj = fmax(nrm_inf_grad_obj, fabs(tmp)); + nrm_inf_grad_obj = MAX(nrm_inf_grad_obj, fabs(tmp)); } if (max_abs_eig < opts->ub_max_abs_eig) @@ -507,7 +507,7 @@ void ocp_nlp_qpscaling_compute_obj_scaling_factor(ocp_nlp_qpscaling_dims *dims, } lb_grad_norm_factor = opts->lb_norm_inf_grad_obj / nrm_inf_grad_obj; tmp = fmin(max_upscale_factor, lb_grad_norm_factor); - mem->obj_factor = fmax(mem->obj_factor, tmp); + mem->obj_factor = MAX(mem->obj_factor, tmp); mem->status = ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED; } if (opts->print_level > 0) @@ -552,10 +552,10 @@ void ocp_nlp_qpscaling_scale_constraints(ocp_nlp_qpscaling_dims *dims, void *opt mask_value_upper = BLASFEO_DVECEL(qp_in->d_mask+i, 2*nb[i]+ng[i]+j); // calculate scaling factor from row norm - double bound_max = fmax(fabs(mask_value_lower * BLASFEO_DVECEL(qp_in->d+i, nb[i]+j)), + double bound_max = MAX(fabs(mask_value_lower * BLASFEO_DVECEL(qp_in->d+i, nb[i]+j)), fabs(mask_value_upper * BLASFEO_DVECEL(qp_in->d+i, 2*nb[i]+ng[i]+j))); // only scale down. - scaling_factor = 1.0 / fmax(1.0, fmax(bound_max, coeff_norm)); + scaling_factor = 1.0 / MAX(1.0, MAX(bound_max, coeff_norm)); // store scaling factor BLASFEO_DVECEL(mem->constraints_scaling_vec+i, j) = scaling_factor; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index f5483c2f20..b0b13428e7 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -56,6 +56,7 @@ #include "acados/utils/print.h" #include "acados/utils/timing.h" #include "acados/utils/types.h" +#include "acados/utils/math.h" #include "acados_c/ocp_qp_interface.h" /************************************************ @@ -656,7 +657,7 @@ static double calculate_pred_l1_inf(ocp_nlp_sqp_wfqp_opts* opts, ocp_nlp_sqp_wfq } else { - if (mem->l1_infeasibility < fmin(opts->nlp_opts->tol_ineq, opts->nlp_opts->tol_eq)) + if (mem->l1_infeasibility < MIN(opts->nlp_opts->tol_ineq, opts->nlp_opts->tol_eq)) { return 0.0; } @@ -689,11 +690,11 @@ static double calculate_qp_l1_infeasibility_from_slacks(ocp_nlp_dims *dims, ocp_ { // Add lower slack tmp1 = BLASFEO_DVECEL(qp_out->ux + i, nx[i]+nu[i]+ns[i] + j); - l1_inf += fmax(0.0, tmp1); + l1_inf += MAX(0.0, tmp1); // Add upper slack tmp2 = BLASFEO_DVECEL(qp_out->ux + i, nx[i]+nu[i]+2*ns[i]+nns[i] + j); - l1_inf += fmax(0.0, tmp2); + l1_inf += MAX(0.0, tmp2); } } #if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) @@ -775,15 +776,15 @@ static double calculate_qp_l1_infeasibility_manually(ocp_nlp_dims *dims, ocp_nlp if (j < nb[i] + ng[i]) { // maximum(0, lower_bound - value) - l1_inf += fmax(0.0, tmp_bound-tmp); - // printf("lower bounds: bound: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, fmax(0.0, tmp_bound-tmp)); + l1_inf += MAX(0.0, tmp_bound-tmp); + // printf("lower bounds: bound: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, MAX(0.0, tmp_bound-tmp)); } else { // upper bounds have the wrong sign! // it is lower_bounds <= value <= -upper_bounds, therefore plus below - // printf("upper bounds: value: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, fmax(0.0, tmp_bound+tmp)); - l1_inf += fmax(0.0, tmp_bound+tmp); + // printf("upper bounds: value: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, MAX(0.0, tmp_bound+tmp)); + l1_inf += MAX(0.0, tmp_bound+tmp); } } } @@ -1475,7 +1476,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); } - if (l1_inf_QP_feasibility/(fmax(1.0, (double) mem->absolute_nns)) < nlp_opts->tol_ineq) + if (l1_inf_QP_feasibility/(MAX(1.0, (double) mem->absolute_nns)) < nlp_opts->tol_ineq) { mem->watchdog_zero_slacks_counter += 1; } diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index 7df6f585af..e16b55712b 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -38,6 +38,7 @@ #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/ocp_qp/ocp_qp_osqp.h" #include "acados/utils/mem.h" +#include "acados/utils/math.h" #include "acados/utils/print.h" #include "acados/utils/timing.h" #include "acados/utils/types.h" @@ -1026,8 +1027,8 @@ void ocp_qp_osqp_opts_set(void *config_, void *opts_, const char *field, void *v // opts->osqp_opts->eps_rel = *tol; // opts->osqp_opts->eps_dual_inf = *tol; - opts->osqp_opts->eps_rel = fmax(*tol, 1e-5); - opts->osqp_opts->eps_dual_inf = fmax(*tol, 1e-5); + opts->osqp_opts->eps_rel = MAX(*tol, 1e-5); + opts->osqp_opts->eps_dual_inf = MAX(*tol, 1e-5); if (*tol <= 1e-3) { diff --git a/acados/utils/math.c b/acados/utils/math.c index a18d2ae6e5..7b2ec88ed0 100644 --- a/acados/utils/math.c +++ b/acados/utils/math.c @@ -1149,7 +1149,7 @@ void compute_gershgorin_min_eig_estimate(int n, struct blasfeo_dmat *A, double * } a = BLASFEO_DMATEL(A, ii, ii); lam_i = a - r_i; - lam = fmin(lam, lam_i); + lam = MIN(lam, lam_i); } *out = lam; }