diff --git a/include/types.h b/include/types.h index 902e1d1..10dd89c 100644 --- a/include/types.h +++ b/include/types.h @@ -100,6 +100,7 @@ typedef struct{ typedef struct{ int is_symmetric; + int retry_rho_needed; c_float* Hsym; c_float* Hs_rho; diff --git a/include/utils.h b/include/utils.h index 1ede787..ce868a8 100644 --- a/include/utils.h +++ b/include/utils.h @@ -20,6 +20,7 @@ int daqp_normalize_M(DAQPWorkspace *work); int daqp_check_unconstrained(DAQPWorkspace* work, const int mask); int daqp_update_avi(DAQPAVI *avi, DAQPProblem *problem, c_float zero_tol); +int daqp_retry_avi_with_reduced_rho(DAQPWorkspace *work); int daqp_lu(c_float* A, int* P, int n); void daqp_lu_solve(c_float* LU, int* P, c_float* b, c_float* x, int n); diff --git a/interfaces/daqp-python/test/example_test.py b/interfaces/daqp-python/test/example_test.py index 308918c..28fba01 100644 --- a/interfaces/daqp-python/test/example_test.py +++ b/interfaces/daqp-python/test/example_test.py @@ -66,6 +66,30 @@ def test_avi_model_dispatches_on_symmetry(self): self.assertEqual(flag_ref, 1) np.testing.assert_allclose(x_model, x_ref, atol=1.0e-8) + def test_ill_conditioned_asymmetric_avi(self): + """An asymmetric AVI remains accurate with a hidden weak direction.""" + Q = np.array([[1.0, 1.0], [-1.0, 1.0]], dtype=c_double) / np.sqrt(2.0) + H = Q @ np.diag([1.0e-6, 1.0]) @ Q.T + H += np.array([[0.0, 1.0e-7], [-1.0e-7, 0.0]], dtype=c_double) + f = np.array([0.75, -0.25], dtype=c_double) + A = np.eye(2, dtype=c_double) + bupper = np.ones(2, dtype=c_double) + blower = -np.ones(2, dtype=c_double) + sense = np.zeros(2, dtype=c_int) + + x, _, flag, _ = daqp.solve( + H, f, A, bupper, blower, sense, is_avi=True) + + self.assertEqual(flag, 1) + residual = H @ x + f + for xi, ri in zip(x, residual): + if xi <= -1.0 + 1.0e-7: + self.assertGreaterEqual(ri, -1.0e-7) + elif xi >= 1.0 - 1.0e-7: + self.assertLessEqual(ri, 1.0e-7) + else: + self.assertAlmostEqual(ri, 0.0, delta=1.0e-7) + def test_warm_start_dual(self): """Dual warm start produces the same optimal solution as a cold start.""" H = np.array([[1.0, 0.0], [0.0, 1.0]], dtype=c_double) diff --git a/src/api.c b/src/api.c index 05c30af..2c31e49 100644 --- a/src/api.c +++ b/src/api.c @@ -370,6 +370,9 @@ void allocate_daqp_ldp(DAQPWorkspace *work, int n, int m, int ms, int alloc_R, i } void allocate_daqp_avi(DAQPAVI* avi, const int n){ avi->is_symmetric = 0; + avi->retry_rho_needed = 0; + avi->rho = 0.0; + // Allocate matrices avi->Hsym = malloc(n*n*sizeof(c_float)); diff --git a/src/avi.c b/src/avi.c index 5f69ecc..2033317 100644 --- a/src/avi.c +++ b/src/avi.c @@ -12,6 +12,7 @@ int daqp_solve_avi(DAQPWorkspace *work) { int tot_iter = 0; int counter = 0; int terminate_limit = 5; + int retry_requested = 0; c_float minimum_newton_residual = DAQP_INF; // Initial avi iterate @@ -51,6 +52,11 @@ int daqp_solve_avi(DAQPWorkspace *work) { // If no decrease since last Newton iterate -> revert Newton step if(sum > minimum_newton_residual){ for(i = 0; i < n; i++) work->avi->x[i] = work->xold[i]; + if(terminate_limit == 30 && avi->retry_rho_needed && + !DAQP_IS_REDUCED(work)){ + retry_requested = 1; + break; + } terminate_limit += 5; // Increase terminate limit to give DR more time to converge if(terminate_limit > 30) terminate_limit = 30; } @@ -95,6 +101,20 @@ int daqp_solve_avi(DAQPWorkspace *work) { } daqp_lu_solve(avi->H_rho, avi->P_H2, avi->xtemp, avi->x, n); } + if(retry_requested){ + int original_limit = work->settings->iter_limit; + int retry_flag = daqp_retry_avi_with_reduced_rho(work); + if(retry_flag < 0) return retry_flag; + if(retry_flag > 0 && k+1 < original_limit){ + work->settings->iter_limit = original_limit-(k+1); + retry_flag = daqp_solve_avi(work); + work->settings->iter_limit = original_limit; + work->iterations += tot_iter; + return retry_flag; + } + work->iterations = tot_iter; + return DAQP_EXIT_ITERLIMIT; + } if(k==work->settings->iter_limit) exitflag = -4; work->iterations = tot_iter; return exitflag; diff --git a/src/utils.c b/src/utils.c index 0cbb76d..4ef315c 100644 --- a/src/utils.c +++ b/src/utils.c @@ -3,6 +3,13 @@ #include #include +#ifndef DAQP_AVI_PIVOT_TRIGGER +#define DAQP_AVI_PIVOT_TRIGGER ((c_float)0.05) +#endif +#ifndef DAQP_AVI_RETRY_RHO_REDUCTION +#define DAQP_AVI_RETRY_RHO_REDUCTION ((c_float)16.0) +#endif + static c_float proximal_regularization_scaled( const DAQPWorkspace *work, c_float hessian_scale){ c_float eps = work->settings->eps_prox; @@ -11,6 +18,42 @@ static c_float proximal_regularization_scaled( return eps; } +static void daqp_install_avi_rho( + DAQPAVI* avi, const DAQPProblem* p, c_float rho){ + int i,j,disp; + const int n = p->n; + avi->rho = rho; + for(i = 0, disp = 0; i < n; i++){ + for(j = 0; j < n; j++, disp++){ + avi->Hs_rho[disp] = avi->Hsym[disp]; + avi->H_rho[disp] = p->H[disp]; + } + avi->Hs_rho[i*n+i] += rho; + avi->H_rho[i*n+i] += rho; + } +} + +int daqp_retry_avi_with_reduced_rho(DAQPWorkspace* work){ + DAQPAVI* avi = work->avi; + c_float retry_rho; + int error_flag; + + if(avi == NULL || !avi->retry_rho_needed) return 0; + if(DAQP_IS_REDUCED(work)) return 0; + avi->retry_rho_needed = 0; // At most one retry per setup + retry_rho = avi->rho/DAQP_AVI_RETRY_RHO_REDUCTION; + daqp_install_avi_rho(avi,work->qp,retry_rho); + daqp_lu(avi->H_rho,avi->P_H2,work->n); + error_flag = daqp_update_Rinv(work,avi->Hs_rho,0); + if(error_flag < 0) return error_flag; + daqp_update_v(work->qp->f,work,DAQP_UPDATE_Rinv); + error_flag = daqp_update_M(work,work->qp->A,DAQP_UPDATE_Rinv); + if(error_flag < 0) return error_flag; + daqp_normalize_Rinv(work); + daqp_update_d(work,work->qp->bupper,work->qp->blower); + return 1; +} + int daqp_update_ldp(const int mask, DAQPWorkspace *work, DAQPProblem* qp){ // TODO: copy dimensions from work->qp? int error_flag, i; @@ -648,6 +691,7 @@ int daqp_update_avi(DAQPAVI* avi, DAQPProblem* p, c_float zero_tol){ c_float fro_norm_sq = 0.0; c_float max_asymmetry = 0.0; avi->rho = 0.0; + avi->retry_rho_needed = 0; for (i = 0, disp=0; i < n; i++) { c_float row_sum = 0.0; for (j = 0; j < n; j++, disp++) { @@ -671,19 +715,28 @@ int daqp_update_avi(DAQPAVI* avi, DAQPProblem* p, c_float zero_tol){ avi->is_symmetric = max_asymmetry <= zero_tol * hessian_scale; if(avi->is_symmetric) return 1; - // Regularization + // Detect possible problematic rho from LU pivots + int lu_status = daqp_lu(avi->LU_H, avi->P_H, n); + if(lu_status == 0 && min_diag > 0.0){ + c_float min_lu_pivot = DAQP_INF; + for(i = 0; i < n; i++){ + c_float pivot = fabs(avi->LU_H[i*n+i]); + if(pivot < min_lu_pivot) min_lu_pivot = pivot; + } + if(min_lu_pivot < DAQP_AVI_PIVOT_TRIGGER * min_diag) + avi->retry_rho_needed = 1; + } + + // Start with default step length heuristic if(min_diag > 0.0 && max_row_sum > 0.0) avi->rho = sqrt(min_diag * max_row_sum); else avi->rho = sqrt(fro_norm_sq)/2; - for(i=0,disp=0; iHs_rho[disp] += avi->rho; avi->H_rho[disp] += avi->rho; - disp += n+1; } - // Factorize H and H_rho - daqp_lu(avi->LU_H, avi->P_H, n); // H_rho factorization deferred until needed (skipped if unconstrained optimal) return 1; }