SQPOptimizerS2.java
/*
* Licensed to the Hipparchus project under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* The Hipparchus project licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.hipparchus.optim.nonlinear.vector.constrained;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.optim.SimpleBounds;
import org.hipparchus.optim.nonlinear.scalar.ObjectiveFunction;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.Pair;
import org.hipparchus.util.Precision;
/**
* Sequential Quadratic Programming Optimizer (extended version).
*
* <p>
* Minimizes a nonlinear objective function subject to equality, inequality, and
* box constraints using a Sequential Quadratic Programming method. This
* implementation is inspired by the algorithm described in: "On the convergence
* of a sequential quadratic programming method" by Klaus Schittkowski (1982).
* </p>
*
* <p>
* Supports: equality constraints, inequality constraints,bounds constraints, penalty function
* updates, line search strategies, BFGS Hessian update, and augmented QP
* formulation using a relaxation variable.
* </p>
*
* @since 3.1
*/
public class SQPOptimizerS2 extends AbstractSQPOptimizer2 {
private enum QPMode {
/**
* Solving the standard QP subproblem (no slack variable).
*/
QP_STARDARD,
/**
* Solving the augmented QP subproblem (with slack variable).
*/
QP_AUGMENTED
}
/** Lower-triangular Cholesky factor. */
private RealMatrix L;
/**
* Logger.
*/
private final SQPLogger formatter = SQPLogger.defaultLogger();
/**
* Value of the equality constraints (residual v = f(x) - lower).
*/
private RealVector eqEval;
/**
* Value of the inequality constraints (residual v = f(x) - lower).
*/
private RealVector ineqEval;
/**
* Value of the bounds (residual v = Ax - lower).
*/
private RealVector bEval;
/**
* Gradient of the objective function.
*/
private RealVector J;
/**
* Hessian approximation.
*/
private RealMatrix H;
/**
* Jacobian of the inequality constraints.
*/
private RealMatrix JI;
/**
* Jacobian of the equality constraints.
*/
private RealMatrix JE;
/**
* Jacobian of the bounds.
*/
private RealMatrix JB;
/**
* Evaluation of the objective function.
*/
private double functionEval;
/**
* Current point.
*/
private RealVector x;
/**
* Bounds in form of inequality constraints.
*/
private LinearInequalityConstraint BIQ;
/**
* Lower bound.
*/
private ArrayRealVector LB;
/**
* Upper bound.
*/
private ArrayRealVector UB;
/** Simple constructor.
*/
public SQPOptimizerS2() {
// nothing to do
}
/**
* {@inheritDoc}
*/
@Override
public LagrangeSolution doOptimize() {
formatter.setEps(getSettings().getEps());
formatter.logHeader();
int me = 0;
int mi = 0;
// Equality constraints configuration
if (this.getEqConstraint() != null) {
me = getEqConstraint().dimY();
}
// Inequality constraints configuration
if (this.getIqConstraint() != null) {
mi = getIqConstraint().dimY();
}
final int mb = buildBoundsAsInequalities();
final int m = me + mi + mb;
// Get initial value of slack variable from SQPOption
double rho = getSettings().getRhoCons();
x = (this.getStartPoint() != null) ? new ArrayRealVector(this.getStartPoint()) : initGuess();
RealVector y = new ArrayRealVector(me + mi + mb, 0.0);
// All the function and constraint evaluations will be performed inside the penalty function
MeritFunctionL2 penalty = new MeritFunctionL2(this.getObj(), this.getEqConstraint(), this.getIqConstraint(), this.BIQ, x);
LineSearch lineSearch = new LineSearch(getSettings().getAlphaMin(), getSettings().getHistory(), getSettings().getMu(), getSettings().getB(),
getSettings().getMaxLineSearchIteration(), 0);
// Initial values evaluation (Penalty function stores residuals v = eval - lower)
functionEval = penalty.getObjEval();
if (this.getEqConstraint() != null) {
eqEval = penalty.getEqEval();
}
if (this.getIqConstraint() != null) {
ineqEval = penalty.getIqEval();
}
if (this.BIQ != null) {
bEval = penalty.getBEval();
}
double EPS = this.getSettings().getEps();
double EPS2 = EPS * EPS;
double sqrtEPS = FastMath.sqrt(EPS);
computeGradients();
double gamma = 1.0;
H = MatrixUtils.createRealIdentityMatrix(x.getDimension()).scalarMultiply(FastMath.sqrt(gamma));
BFGSUpdater bfgs = new BFGSUpdater(H, EPS, true, getMatrixDecompositionTolerance().getEpsMatrixDecomposition());
L = bfgs.getL();
RealVector dx = new ArrayRealVector(x.getDimension());
RealVector u = new ArrayRealVector(y.getDimension());
penalty.update(J, JE, JI, x, y, dx, u);
QPMode QPMODE = QPMode.QP_STARDARD;
boolean crit0 = false;
boolean crit1 = false;
boolean crit2 = false;
boolean crit3 = false;
boolean crit4 = false;
boolean crit5 = false;
double sigma = 0.0;
double alpha = 0.0;
RealVector lagOld;
RealVector lagNew;
LagrangeSolution qpSolution = null;
boolean GRADFAIL = false;
boolean FALLBACK = false;
double VIOLATION = constraintViolation();
double COMPLEMENTARY = Double.POSITIVE_INFINITY;
double KKT = Double.POSITIVE_INFINITY;
double DHD = 0.0;
double XNORM = 0.0;
double FUNDIFF = 0.0;
int BFGSUPDATE;
int FUNEVALCOUNT=0;
SQPCustomCriterion customCriteria = getSettings().getConvergenceFunction();
for (int i = 0; i < getSettings().getMaxIteration(); i++) {
sigma = getSettings().getSigmaMax() * 10.0;
while ((sigma > getSettings().getSigmaMax() || sigma < 0.0) && rho < 1.0e9 && !FALLBACK) {
qpSolution = (QPMODE == QPMode.QP_AUGMENTED) ? solveAugmentedQP(y, rho) : solveQP();
sigma = (qpSolution == null || qpSolution.getX().getDimension() == 0 || Double.isInfinite(qpSolution.getX().getMaxValue()) || Double.isInfinite(qpSolution.getX().getMinValue())) ? getSettings().getSigmaMax() * 10.0 : qpSolution.getValue();
if ((sigma > getSettings().getSigmaMax() || sigma < 0.0)) {
if (QPMODE == QPMode.QP_AUGMENTED) {
rho = rhoUp(rho);
}
QPMODE = QPMode.QP_AUGMENTED;
}
}
// If sigma > sigma threshold after several attempts, assign direction from penalty gradient
if (rho >= 1.0e9 || FALLBACK) {
qpSolution = solveQPFallBack(penalty.gradX());
if (qpSolution == null || qpSolution.getX().getDimension() == 0 ) {
bfgs.resetHessian();
H=bfgs.getHessian();
L=bfgs.getL();
qpSolution = solveQPFallBack(penalty.gradX());
if (qpSolution == null || qpSolution.getX().getDimension() == 0 ) {
penalty.resetRj();
if (m > 0) {
u.set(0.0);
y.set(0.0);
}
penalty.update(J, JE, JI, x, y, new ArrayRealVector(x.getDimension()), u);
qpSolution = solveQPFallBack(penalty.gradX());
if (qpSolution == null || qpSolution.getX().getDimension() == 0 ) {
break;
}
} // Infeasible
// Estimation of multiplier from penalty grad y
}
if (m > 0 ) {
u = y.subtract(penalty.gradY());
}
dx = qpSolution.getX();
projectDirectionInPlace(x, dx);
QPMODE = QPMode.QP_AUGMENTED;
sigma = 0.0;
rho = getSettings().getRhoCons();
} else {
dx = qpSolution.getX();
u = qpSolution.getLambda();
sigma = qpSolution.getValue();
projectDirectionInPlace(x, dx);
penalty.updateRj(H, y, dx, u, sigma, iterations.getCount());
// Switch to normal QP if additional variable is small enough
if (QPMODE == QPMode.QP_AUGMENTED && FastMath.abs(sigma) < getSettings().getEps() && !GRADFAIL) {
QPMODE = QPMode.QP_STARDARD;
rho = getSettings().getRhoCons();
}
}
penalty.update(J, JE, JI, x, y, dx, u);
lagOld= lagrangianGradX(J, JE, JI, x, u);
double dlan = lagOld.getNorm();
// Convergence check before line search
KKT = dlan * dlan;
DHD = dx.dotProduct(H.operate(dx));
COMPLEMENTARY = complementarySlackness(u, me);
XNORM = dx.getNorm();
crit0 = KKT <= EPS;
crit1 = DHD <= EPS2;
crit2 = XNORM <= EPS * (1.0 + x.getNorm());
crit3 = VIOLATION <= sqrtEPS;
crit5 = COMPLEMENTARY <= EPS;
if ((crit0 || crit1) && crit5 && crit3) {
break;
}
if (penalty.getGradient() >= 0) {
if (QPMODE == QPMode.QP_AUGMENTED) {
rho = rhoUp(rho);
}
QPMODE = QPMode.QP_AUGMENTED;
GRADFAIL = true;
}
// If penalty gradient is < 0 proceed with line search
else {
iterations.increment();
GRADFAIL = false;
FALLBACK = false;
if (QPMODE == QPMode.QP_AUGMENTED) {
rho = updateRho(dx, u, sigma);
}
// Line Search
alpha = lineSearch.search(penalty);
FUNEVALCOUNT+=lineSearch.getIteration();
//UPDATE MULTIPLIER
if (m > 0) {
// y = y.add(u.subtract(y).mapMultiply(alpha));
//y.combineToSelf(1.0 - alpha, alpha, u);//this is worse
double yk;
for (int k = 0; k < y.getDimension(); ++k) {
yk = y.getEntry(k);
y.setEntry(k, yk + (u.getEntry(k) - yk) * alpha);
}
}
//UPDATE SOLUTION
//x = x.add(dx.mapMultiply(alpha));
x.combineToSelf(1.0, alpha, dx);
// UPDATE FUNCTION AND CONSTRAINT EVAL (Residuals v = eval - lower are stored)
// Old objective function evaluation.
final double functionEvalOld = functionEval;
functionEval = penalty.getObjEval();
eqEval = penalty.getEqEval();
ineqEval = penalty.getIqEval();
bEval = penalty.getBEval();
// Convergence check after line search
COMPLEMENTARY = complementarySlackness(u, me);
VIOLATION = constraintViolation();
lagNew= lagrangianGradX(J, JE, JI, x, u);
dlan = lagNew.getNorm();
KKT = dlan * dlan;
// Step length
if (alpha > 0) {
DHD = DHD * alpha * alpha;
XNORM = alpha * dx.getNorm();
}
FUNDIFF = FastMath.abs(functionEval - functionEvalOld);
crit0 = KKT <= EPS;
crit1 = DHD <= EPS2;
crit2 = XNORM <= EPS * (1.0 + x.getNorm());
crit3 = VIOLATION <= sqrtEPS;
crit4 = FUNDIFF < EPS * (1.0 + FastMath.abs(functionEval));
crit5 = COMPLEMENTARY <= EPS;
// if (!lineSearch.isBadStepDetected() && (crit0 || (crit5 && m > 0) || crit4 || crit2 ) && crit1 && crit3) {
// break;
// }
// if (manualBreak) {
// System.out.println("\n[Solver] Manual break triggered by the user.");
// break; // Exits the loop exactly at this checkpoint
// }
if (customCriteria!=null &&
customCriteria.converged(alpha, XNORM, DHD, COMPLEMENTARY, KKT, VIOLATION,
FUNDIFF, x.getNorm(), FastMath.abs(functionEval))) {
break;
}
if (!lineSearch.isBadStepDetected() && ((crit0 || crit1) && crit5 && crit3)) {
break;
}
//
if (lineSearch.isBadStepDetected() && (crit3 && (crit4 || crit2))) {
break;
}
computeGradients();
RealVector lagnew = lagrangianGradX(J, JE, JI, x, u);
// Hessian update with the logic of line search and internal damping logic
if (lineSearch.isBadStepFailed()) {
FALLBACK = false;
bfgs.resetHessian();
H = bfgs.getHessian();
L = bfgs.getL();
lineSearch.resetBadStepCount();
BFGSUPDATE = -2;
} else if (lineSearch.isBadStepDetected()) {
BFGSUPDATE = -1;
L = bfgs.getL();
H = bfgs.getHessian();
} else {
BFGSUPDATE = bfgs.update(dx.mapMultiply(alpha), lagnew.subtract(lagOld));
H = bfgs.getHessian();
L = bfgs.getL();
}
formatter.logRow(iterations.getCount(),
alpha, lineSearch.getIteration(),
XNORM, DHD, COMPLEMENTARY, KKT, VIOLATION, sigma,
penalty.getPenaltyEval(), functionEval, FUNDIFF, BFGSUPDATE);
}
}
formatter.logRow(iterations.getCount(),
alpha, lineSearch.getIteration(),
XNORM, DHD, COMPLEMENTARY, KKT, VIOLATION, sigma,
penalty.getPenaltyEval(), functionEval, FUNDIFF, -99);
FUNEVALCOUNT+=iterations.getCount();
formatter.logRow(FUNEVALCOUNT,crit2, crit1, crit5, crit0, crit3, crit4);
return new LagrangeSolution(x, y, functionEval);
}
/**
* Compute gradients.
*/
private void computeGradients() {
switch (getSettings().getGradientMode()) {
case EXTERNAL:
externalGradient();
break;
case FORWARD:
forwardGradient();
break;
default:
centralGradient();
break;
}
}
/**
* Compute constraints violations using residuals directly.
*
* @return constraints violations
*/
private double constraintViolation() {
double crit = 0.0;
if (this.getEqConstraint() != null) {
for (int k = 0; k < eqEval.getDimension(); k++) {
crit += FastMath.abs(eqEval.getEntry(k));
}
}
if (this.getIqConstraint() != null) {
for (int k = 0; k < ineqEval.getDimension(); k++) {
if (ineqEval.getEntry(k)< 0.0) {
crit -= ineqEval.getEntry(k);
}
}
}
if (this.BIQ != null) {
for (int k = 0; k < bEval.getDimension(); k++) {
if (bEval.getEntry(k) < 0.0) {
crit -= bEval.getEntry(k);
}
}
}
return crit;
}
/**
* Compute complementary slackness using residuals directly.
*
* @param u the multiplier vector
* @param me the number of equality constraints
* @return complementary slackness
*/
private double complementarySlackness(RealVector u, int me) {
if (u == null) {
return 0.0;
}
double crit = 0.0;
int shift = me;
if (this.getIqConstraint() != null) {
for (int i = 0; i < ineqEval.getDimension(); i++) {
crit += FastMath.abs(ineqEval.getEntry(i) * u.getEntry(shift + i));
}
shift += ineqEval.getDimension();
}
if (this.BIQ != null) {
for (int i = 0; i < bEval.getDimension(); i++) {
crit += FastMath.abs(bEval.getEntry(i) * u.getEntry(shift + i));
}
}
return crit;
}
/**
* Increase the rho penalty parameter.
*
* @param rho current rho
* @return increased rho
*/
private double rhoUp(double rho) {
return rho * 10.0;
}
/**
* Dynamically update rho based on the current step and multiplier difference.
*
* @param dx current step
* @param dy multiplier step
* @param additionalVariable slack/augmented variable
* @return new rho value
*/
private double updateRho(final RealVector dx, final RealVector dy, final double additionalVariable) {
int me = JE != null ? JE.getRowDimension() : 0;
int mi = JI != null ? JI.getRowDimension() : 0;
int mb = JB != null ? JB.getRowDimension() : 0;
if (me + mi + mb > 0) {
RealVector jacPreDy = new ArrayRealVector(x.getDimension());
int offset = 0;
if (me > 0 && JE != null) {
RealVector dyE = dy.getSubVector(offset, me);
jacPreDy.combineToSelf(1.0, 1.0, JE.preMultiply(dyE));
offset += me;
}
if (mi > 0 && JI != null) {
RealVector dyI = dy.getSubVector(offset, mi);
jacPreDy.combineToSelf(1.0, 1.0, JI.preMultiply(dyI));
offset += mi;
}
if (mb > 0 && JB != null) {
RealVector dyB = dy.getSubVector(offset, mb);
jacPreDy.combineToSelf(1.0, 1.0, JB.preMultiply(dyB));
}
double num = 10.0 * FastMath.pow(dx.dotProduct(jacPreDy), 2);
double den = (1.0 - additionalVariable) * (1.0 - additionalVariable) * dx.dotProduct(H.operate(dx));
if (den < Precision.SAFE_MIN) {
den = Precision.SAFE_MIN;
}
return FastMath.max(10.0, num / den);
}
return 1.0;
}
/**
* Solve augmented problem using residuals.
* <p>
* Mathematically equivalent to the original version, but optimized to avoid
* double[][] allocations and implementation-specific casts.
* </p>
* @param y Lagrange multipliers
* @param rho rho penalty parameter
* @return problem solution
*/
private LagrangeSolution solveAugmentedQP(final RealVector y, final double rho) {
final int n = x.getDimension();
final double eps = getSettings().getEps();
final int me = (getEqConstraint() != null) ? getEqConstraint().dimY() : 0;
final int mi = (getIqConstraint() != null) ? getIqConstraint().dimY() : 0;
final int mb = (BIQ != null) ? BIQ.dimY() : 0;
final int mc = mi + mb;
boolean violated = false;
if (mi > 0) {
violated = ineqEval.getMinValue() <= eps || y.getSubVector(me, mi).getMaxValue() > 0;
}
final int add = (me > 0 || violated) ? 1 : 0;
// --- OBJECTIVE ---
final RealVector g1 = new ArrayRealVector(n + add);
g1.setSubVector(0, J);
// L1: [L, 0; 0, sqrt(rho)]
final RealMatrix L1 = MatrixUtils.createRealMatrix(n + add, n + add);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
L1.setEntry(i, j, L.getEntry(i, j));
}
}
if (add == 1) {
L1.setEntry(n, n, FastMath.sqrt(rho));
}
// --- EQUALITIES ---
LinearEqualityConstraint eqc = null;
if (me > 0) {
final RealMatrix Ae = MatrixUtils.createRealMatrix(me, n + add);
final RealVector eqRhs = new ArrayRealVector(me);
for (int i = 0; i < me; i++) {
final double vi = eqEval.getEntry(i);
eqRhs.setEntry(i, -vi);
if (add == 1) {
Ae.setEntry(i, n, -vi);
}
for (int j = 0; j < n; j++) {
Ae.setEntry(i, j, JE.getEntry(i, j));
}
}
eqc = new LinearEqualityConstraint(Ae, eqRhs);
}
// --- INEQUALITIES & BOUNDS ---
LinearInequalityConstraint iqc = null;
if (mc > 0) {
final RealMatrix A = MatrixUtils.createRealMatrix(mc, n + add);
final RealVector B = new ArrayRealVector(mc);
// Part 1: Inequalities
if (mi > 0) {
for (int i = 0; i < mi; i++) {
final double vi = ineqEval.getEntry(i);
B.setEntry(i, -vi);
if (add == 1) {
if (vi <= eps || y.getEntry(me + i) > 0) {
A.setEntry(i, n, -vi);
}
}
for (int j = 0; j < n; j++) {
A.setEntry(i, j, JI.getEntry(i, j));
}
}
}
// Part 2: Bounds (Stacked below inequalities)
if (mb > 0) {
for (int i = 0; i < mb; i++) {
B.setEntry(mi + i, -bEval.getEntry(i));
for (int j = 0; j < n; j++) {
A.setEntry(mi + i, j, JB.getEntry(i, j));
}
}
}
iqc = new LinearInequalityConstraint(A, B);
}
// --- SLACK BOUNDS ---
LinearBoundedConstraint bc = null;
if (add == 1) {
final RealMatrix sigmaA = MatrixUtils.createRealMatrix(1, n + 1);
sigmaA.setEntry(0, n, 1.0);
bc = new LinearBoundedConstraint(sigmaA, new ArrayRealVector(1, 0.0), new ArrayRealVector(1, 1.0));
}
// Solve
QPDualActiveSolverOption optQP=new QPDualActiveSolverOption();
optQP.setEps(this.getSettings().getEpsQP());
optQP.setEpsRelaxation(this.getSettings().getEpsQP());
optQP.setMatrixMode(QPMatrixMode.CHOLESKY);
final LagrangeSolution sol = getQPSolver().optimize(optQP,
new ObjectiveFunction(new QuadraticFunction(L1, g1, 0)),
iqc, eqc, bc);
if (sol == null || sol.getX().getDimension() == 0) {
return sol;
}
final RealVector fullX = sol.getX();
final double sigma = (add == 1) ? fullX.getEntry(n) : 0.0;
return new LagrangeSolution(fullX.getSubVector(0, n),
(me + mc == 0) ? null : sol.getLambda().getSubVector(0, me + mc),
sigma);
}
/**
* Solves the standard Quadratic Programming (QP) subproblem using residuals.
* @return a LagrangeSolution containing primal and dual variables, or null if failed.
*/
private LagrangeSolution solveQP() {
final int n = x.getDimension();
final int me = (getEqConstraint() != null) ? getEqConstraint().dimY() : 0;
final int mi = (getIqConstraint() != null) ? getIqConstraint().dimY() : 0;
final int mb = (BIQ != null) ? BIQ.dimY() : 0;
final int mc = mi + mb;
// --- EQUALITY CONSTRAINTS SETUP ---
LinearEqualityConstraint eqc = null;
if (me > 0) {
// Formulation: JE * d = -eqEval
eqc = new LinearEqualityConstraint(JE, eqEval.mapMultiply(-1.0));
}
// --- INEQUALITY CONSTRAINTS SETUP ---
LinearInequalityConstraint iqc = null;
if (mc > 0) {
// Formulation: [JI; JB] * d >= -[ineqEval; bEval]
// Pre-allocate global matrix to avoid BlockRealMatrix fragmentation
final RealMatrix combinedAI = MatrixUtils.createRealMatrix(mc, n);
final RealVector combinedBI = new ArrayRealVector(mc);
// Copy Inequality Jacobian (JI) directly into the combined matrix
if (mi > 0) {
for (int i = 0; i < mi; i++) {
combinedBI.setEntry(i, -ineqEval.getEntry(i));
for (int j = 0; j < n; j++) {
combinedAI.setEntry(i, j, JI.getEntry(i, j));
}
}
}
// Copy Bounds Jacobian (JB) directly into the combined matrix (offset by mi)
if (mb > 0) {
for (int i = 0; i < mb; i++) {
combinedBI.setEntry(mi + i, -bEval.getEntry(i));
for (int j = 0; j < n; j++) {
combinedAI.setEntry(mi + i, j, JB.getEntry(i, j));
}
}
}
iqc = new LinearInequalityConstraint(combinedAI, combinedBI);
}
QPDualActiveSolverOption optQP=new QPDualActiveSolverOption();
optQP.setEps(this.getSettings().getEpsQP());
optQP.setEpsRelaxation(this.getSettings().getEpsQP());
optQP.setMatrixMode(QPMatrixMode.CHOLESKY);
// --- QUADRATIC OBJECTIVE ---
// Objective: 0.5 * d^T * L * d + J^T * d
final QuadraticFunction q = new QuadraticFunction(L, J, 0.0);
// --- SOLVE QP ---
final LagrangeSolution sol = getQPSolver().optimize(optQP,new ObjectiveFunction(q), iqc, eqc);
if (sol == null || sol.getX().getDimension() == 0) {
return null;
}
// IMPORTANT: Sigma must be 0.0 in standard QP to avoid triggering slack logic in SQP loop
return new LagrangeSolution(sol.getX(),
(me + mc > 0) ? sol.getLambda() : new ArrayRealVector(0),
0.0);
}
/**
* Solves the Quadratic Programming (QP) subproblem in the current SQP
* iteration if penalty gradient is >= 0
*
* @param penaltyGradx gradient of the penalty function
* @return a {@link LagrangeSolution} representing the QP solution, or
* {@code null} if the QP failed
*/
private LagrangeSolution solveQPFallBack(RealVector penaltyGradx) {
final QuadraticFunction q = new QuadraticFunction(this.H ,penaltyGradx, 0);
int mb = 0;
LinearInequalityConstraint iqc = null;
if (BIQ != null) {
mb = BIQ.dimY();
RealMatrix A = JB;
RealVector B = bEval.mapMultiply(-1.0);
iqc = new LinearInequalityConstraint(A, B);
}
QPDualActiveSolverOption optQP=new QPDualActiveSolverOption();
optQP.setEps(this.getSettings().getEpsQP());
optQP.setEpsRelaxation(this.getSettings().getEpsQP());
optQP.setMatrixMode(QPMatrixMode.FULL);
LagrangeSolution sol = getQPSolver().optimize(optQP,new ObjectiveFunction(q), iqc);
if (sol == null || sol.getX().getDimension() == 0) {
return sol;
}
RealVector solutionX = sol.getX();
RealVector solutionLambda = (mb > 0) ? sol.getLambda() : new ArrayRealVector(0, 0);
return new LagrangeSolution(solutionX, solutionLambda, 0.0);
}
/**
* Computes the gradient of the Lagrangian function with respect to the
* primal variable {@code x}.
* <p>
* The Lagrangian is defined as:
* </p>
* <pre>
* L(x, y) = f(x) - yₑᵗ·cₑ(x) - yᵢᵗ·cᵢ(x)
* </pre>
* <p>
* where:
* </p>
* <ul>
* <li>{@code f(x)} is the objective function</li>
* <li>{@code cₑ(x)} are the equality constraints</li>
* <li>{@code cᵢ(x)} are the inequality constraints</li>
* <li>{@code y = [yₑ; yᵢ]} is the vector of Lagrange multipliers</li>
* </ul>
* <p>
* The gradient with respect to {@code x} is given by:
* </p>
* <pre>
* ∇ₓ L(x, y) = ∇f(x) - JEᵗ·yₑ - JIᵗ·yᵢ
* </pre>
*
* @param otherJ the gradient of the objective function {@code ∇f(x)},
* length {@code n}
* @param otherJE the Jacobian of the equality constraints, shape
* {@code [me x n]} (nullable)
* @param otherJI the Jacobian of the inequality constraints, shape
* {@code [mi x n]} (nullable)
* @param ignoredX the current point in the primal space (not used directly,
* included for API symmetry)
* @param y the stacked Lagrange multipliers {@code [yₑ; yᵢ]}, length
* {@code me + mi}
* @return the gradient of the Lagrangian with respect to {@code x}, length
* {@code n}
*/
public RealVector lagrangianGradX(final RealVector otherJ, final RealMatrix otherJE, final RealMatrix otherJI,
final RealVector ignoredX, final RealVector y) {
RealVector gradL = new ArrayRealVector(otherJ);
int offset = 0;
if (otherJE != null) {
int me = otherJE.getRowDimension();
RealVector yEq = y.getSubVector(0, me);
gradL.combineToSelf(1.0, -1.0, otherJE.preMultiply(yEq));
offset += me;
}
if (otherJI != null) {
int mi = otherJI.getRowDimension();
RealVector yIq = y.getSubVector(offset, mi);
gradL.combineToSelf(1.0, -1.0, otherJI.preMultiply(yIq));
offset += mi;
}
if (JB != null) {
int mb = JB.getRowDimension();
RealVector yB = y.getSubVector(offset, mb);
gradL.combineToSelf(1.0, -1.0, JB.preMultiply(yB));
}
return gradL;
}
/**
* Computes the optimal finite difference step size and direction for a given variable,
* strictly respecting its lower and upper bounds.
* <p>
* To prevent catastrophic cancellation and numerical noise, this method strictly
* avoids arbitrary step shrinking. If the remaining room up to the active bound
* is strictly less than the required numerical step, the variable is
* considered numerically frozen and a zero step is returned.
* </p>
* <p>
* For central differences, if a symmetric step is not feasible, the method attempts
* to allocate enough room for an asymmetric 3-point formula step (2 * hBase)
* to preserve the <i>O(h^2)</i> truncation error.
* </p>
*
* @param i the index of the variable
* @param xi the current value of the variable
* @param hBase the theoretical base step size (e.g., square root or cube root of machine epsilon)
* @param central if {@code true}, step selection prioritizes central finite differences
* @return a {@link Pair} where the first element is the signed step size {@code h},
* and the second element is the direction indicator:
* <ul>
* <li>{@code 0} for true central difference</li>
* <li>{@code +1} for forward one-sided difference</li>
* <li>{@code -1} for backward one-sided difference</li>
* </ul>
* Returns {@code (0.0, 0)} if the variable is strictly bounded and cannot be perturbed safely.
*/
private Pair<Double, Integer> chooseStep(final int i,
final double xi,
final double hBase,
final boolean central) {
final double lb = LB.getEntry(i);
final double ub = UB.getEntry(i);
final double roomMinus = FastMath.max(0.0,
Double.isFinite(lb) ? (xi - lb) : Double.POSITIVE_INFINITY);
final double roomPlus = FastMath.max(0.0,
Double.isFinite(ub) ? (ub - xi) : Double.POSITIVE_INFINITY);
if (!central) {
if (roomPlus >= hBase) {
return new Pair<>(hBase, +1);
}
if (roomMinus >= hBase) {
return new Pair<>(-hBase, -1);
}
return new Pair<>(0.0, 0);
}
if (roomPlus >= hBase && roomMinus >= hBase) {
return new Pair<>(hBase, 0);
}
if (roomPlus >= 2.0 * hBase) {
return new Pair<>(hBase, +1);
}
if (roomMinus >= 2.0 * hBase) {
return new Pair<>(-hBase, -1);
}
return new Pair<>(0.0, 0);
}
/**
* Compute gradient directly.
*/
private void externalGradient() {
J = this.getObj().gradient(x);
if (this.getEqConstraint() != null) {
JE = this.getEqConstraint().jacobian(x);
}
if (this.getIqConstraint() != null) {
JI = this.getIqConstraint().jacobian(x);
}
}
/**
* Computes the gradient of the objective function and the Jacobians of the
* constraints using forward finite differences (first-order accurate).
* <p>
* Each variable is perturbed independently by a small step size proportional
* to the square root of machine precision. Variables tightly bounded are
* gracefully ignored (gradient component set to zero) to prevent out-of-bounds
* evaluations and Hessian corruption.
* </p>
* <p>
* Deep copies of the working vectors are instantiated within the loop to
* strictly prevent side effects and aliasing issues with external black-box
* evaluators that may cache object references.
* </p>
*/
private void forwardGradient() {
int n = x.getDimension();
double sqrtEps = FastMath.sqrt(getSettings().getGradientEps());
double xi;
double h1;
double fRef = this.functionEval;
RealVector eqRef = this.eqEval;
RealVector iqRef = this.ineqEval;
RealVector gradF = new ArrayRealVector(n);
RealMatrix gradEq = (getEqConstraint() != null) ? MatrixUtils.createRealMatrix(eqRef.getDimension(), n) : null;
RealMatrix gradIq = (getIqConstraint() != null) ? MatrixUtils.createRealMatrix(iqRef.getDimension(), n) : null;
for (int i = 0; i < n; i++) {
xi = x.getEntry(i);
h1 = sqrtEps * FastMath.max(1.0, FastMath.abs(xi));
Pair<Double, Integer> stepInfo = chooseStep(i, xi, h1, false);
double h = stepInfo.getFirst();
if (h == 0.0) {
gradF.setEntry(i, 0.0);
if (gradEq != null) {
gradEq.setColumnVector(i, new ArrayRealVector(eqEval.getDimension(), 0.0));
}
if (gradIq != null) {
gradIq.setColumnVector(i, new ArrayRealVector(ineqEval.getDimension(), 0.0));
}
} else {
RealVector xPerturbed = x.copy();
xPerturbed.setEntry(i, xi + h);
double fPerturbed = getObj().value(xPerturbed);
gradF.setEntry(i, (fPerturbed - fRef) / h);
if (gradEq != null) {
// Logica residuo: (valPert - lower) - (valRef - lower) = valPert - valRef
RealVector eqPerturbed = getEqConstraint().value(xPerturbed).combineToSelf(1.0,-1.0,getEqConstraint().getLowerBound());
gradEq.setColumnVector(i, eqPerturbed.combineToSelf(1.0,-1.0,eqRef).mapDivideToSelf(h));
}
if (gradIq != null) {
RealVector iqPerturbed = getIqConstraint().value(xPerturbed).combineToSelf(1.0,-1.0,getIqConstraint().getLowerBound());
gradIq.setColumnVector(i, iqPerturbed.combineToSelf(1.0,-1.0,iqRef).mapDivideToSelf(h));
}
}
}
this.J = gradF;
this.JE = gradEq;
this.JI = gradIq;
}
/**
* Computes the gradient of the objective function and the Jacobians of the
* constraints using centered finite differences (second-order accurate).
* <p>
* The base step size is proportional to the cube root of machine precision.
* If a symmetric centered difference is not possible due to variable bounds,
* the algorithm falls back to an asymmetric 3-point formula. This industrial-grade
* fallback guarantees that the O(h^2) truncation error is preserved, preventing
* catastrophic shocks to the BFGS Hessian approximation near active bounds.
* </p>
* <p>
* Vectors are explicitly instantiated to prevent aliasing with stateful objective
* functions. Furthermore, evaluations are strictly grouped by the perturbed point
* (e.g., all functions evaluated at x+h, then all at x-h) to maximize cache-hit
* ratios in complex black-box simulators.
* </p>
*/
private void centralGradient() {
int n = x.getDimension();
double hBase = FastMath.cbrt(getSettings().getGradientEps());
RealVector gradF = new ArrayRealVector(n);
RealMatrix gradEq = (getEqConstraint() != null) ? MatrixUtils.createRealMatrix(eqEval.getDimension(), n) : null;
RealMatrix gradIq = (getIqConstraint() != null) ? MatrixUtils.createRealMatrix(ineqEval.getDimension(), n) : null;
for (int i = 0; i < n; i++) {
double xi = x.getEntry(i);
double h1 = hBase * FastMath.max(1.0, FastMath.abs(xi));
Pair<Double, Integer> stepInfo = chooseStep(i, xi, h1, true);
double h = stepInfo.getFirst();
int dir = stepInfo.getSecond();
if (h == 0.0) {
// Variable is completely trapped, numerical derivative is unfeasible
gradF.setEntry(i, 0.0);
if (gradEq != null) {
gradEq.setColumnVector(i, new ArrayRealVector(eqEval.getDimension(), 0.0));
}
if (gradIq != null) {
gradIq.setColumnVector(i, new ArrayRealVector(ineqEval.getDimension(), 0.0));
}
} else if (dir == 0) {
// -------------------------------------------------
// CLASSICAL CENTRAL DIFFERENCE
// -------------------------------------------------
RealVector xPlus = new ArrayRealVector(x);
RealVector xMinus = new ArrayRealVector(x);
xPlus.addToEntry(i, h);
xMinus.addToEntry(i, -h);
// --- CACHE-FRIENDLY EVALUATION: Everything at xPlus ---
double fPlus = getObj().value(xPlus);
// Calcoliamo i residui v = f(x) - lower per la differenza finita
RealVector eqPlus = (gradEq != null) ? getEqConstraint().value(xPlus).combineToSelf(1.0,-1.0,getEqConstraint().getLowerBound()) : null;
RealVector iqPlus = (gradIq != null) ? getIqConstraint().value(xPlus).combineToSelf(1.0,-1.0,getIqConstraint().getLowerBound()) : null;
// --- CACHE-FRIENDLY EVALUATION: Everything at xMinus ---
double fMinus = getObj().value(xMinus);
RealVector eqMinus = (gradEq != null) ? getEqConstraint().value(xMinus).combineToSelf(1.0,-1.0,getEqConstraint().getLowerBound()) : null;
RealVector iqMinus = (gradIq != null) ? getIqConstraint().value(xMinus).combineToSelf(1.0,-1.0,getIqConstraint().getLowerBound()) : null;
// Derivatives Calculation (La differenza tra residui è uguale alla differenza tra valori raw)
gradF.setEntry(i, (fPlus - fMinus) / (2.0 * h));
if (gradEq != null) {
// Sfrutto eqPlus gia' allocato per la combinazione: (eqPlus - eqMinus) * inv2h
gradEq.setColumnVector(i, eqPlus.combineToSelf(1.0, -1.0, eqMinus).mapDivideToSelf(2.0*h));
}
if (gradIq != null) {
gradIq.setColumnVector(i, iqPlus.combineToSelf(1.0, -1.0, iqMinus).mapDivideToSelf(2.0*h));
}
} else {
// -------------------------------------------------
// ASYMMETRIC 3-POINT FORMULA (O(h^2) preservation)
// -------------------------------------------------
RealVector x1 = new ArrayRealVector(x);
RealVector x2 = new ArrayRealVector(x);
x1.addToEntry(i, h);
x2.addToEntry(i, 2.0 * h);
double f0 = functionEval; // Already evaluated at current x (residual if applicable)
// --- CACHE-FRIENDLY EVALUATION: Everything at x1 ---
double f1 = getObj().value(x1);
RealVector eq1 = (gradEq != null) ? getEqConstraint().value(x1).combineToSelf(1.0,-1.0,getEqConstraint().getLowerBound()) : null;
RealVector iq1 = (gradIq != null) ? getIqConstraint().value(x1).combineToSelf(1.0,-1.0,getIqConstraint().getLowerBound()) : null;
// --- CACHE-FRIENDLY EVALUATION: Everything at x2 ---
double f2 = getObj().value(x2);
RealVector eq2 = (gradEq != null) ? getEqConstraint().value(x2).combineToSelf(1.0,-1.0,getEqConstraint().getLowerBound()) : null;
RealVector iq2 = (gradIq != null) ? getIqConstraint().value(x2).combineToSelf(1.0,-1.0,getIqConstraint().getLowerBound()) : null;
// Derivatives Calculation
gradF.setEntry(i, (-3.0 * f0 + 4.0 * f1 - f2) / (2.0 * h));
if (gradEq != null) {
// eqEval è già un residuo (f(x) - L) calcolato in precedenza
RealVector dEq = eqEval.copy();
dEq.combineToSelf(-3.0, 4.0, eq1).combineToSelf(1.0, -1, eq2).mapDivideToSelf(2*h);
gradEq.setColumnVector(i, dEq);
}
if (gradIq != null) {
// ineqEval è già un residuo (f(x) - L) calcolato in precedenza
RealVector dIq = ineqEval.copy();
dIq.combineToSelf(-3.0, 4.0, iq1).combineToSelf(1.0, -1, iq2).mapDivideToSelf(2*h);
gradIq.setColumnVector(i, dIq);
}
}
}
this.J = gradF;
this.JE = gradEq;
this.JI = gradIq;
}
/**
* Set debug printer.
* @param printer debug printer
*/
public void setDebugPrinter(final DebugPrinter printer) {
formatter.setDebugPrinter(printer);
}
/**
* Build bounds constraint in the form of inequality.
* @return number of bounds
*/
private int buildBoundsAsInequalities() {
if (getSimpleBounds() == null) {
int n = getObj().dim();
this.BIQ = null;
LB = new ArrayRealVector(n, Double.NEGATIVE_INFINITY);
UB = new ArrayRealVector(n, Double.POSITIVE_INFINITY);
this.JB = null;
return 0;
}
int n = getObj().dim();
LB = (getSimpleBounds().getLower() != null) ?
new ArrayRealVector(getSimpleBounds().getLower()) :
new ArrayRealVector(n, Double.NEGATIVE_INFINITY);
UB = (getSimpleBounds().getUpper() != null) ?
new ArrayRealVector(getSimpleBounds().getUpper()) :
new ArrayRealVector(n, Double.POSITIVE_INFINITY);
java.util.ArrayList<double[]> rows = new java.util.ArrayList<>();
java.util.ArrayList<Double> rhs = new java.util.ArrayList<>();
for (int i = 0; i < n; i++) {
if (Double.isFinite(LB.getEntry(i))) {
double[] row = new double[n];
row[i] = 1.0;
rows.add(row);
rhs.add(LB.getEntry(i));
}
if (Double.isFinite(UB.getEntry(i))) {
double[] row = new double[n];
row[i] = -1.0;
rows.add(row);
rhs.add(-UB.getEntry(i));
}
}
if (rows.isEmpty()) {
this.BIQ = null;
this.JB = null;
return 0;
}
double[][] amat = rows.toArray(new double[rows.size()][]);
double[] bvec = new double[rhs.size()];
for (int k = 0; k < rhs.size(); k++) {
bvec[k] = rhs.get(k);
}
this.BIQ = new LinearInequalityConstraint(new Array2DRowRealMatrix(amat, false), new ArrayRealVector(bvec, false));
this.JB = this.BIQ.jacobian(null);
return JB.getRowDimension();
}
/**
* Build initial guess taking account the bounds.
* @return initial guess
*/
private RealVector initGuess() {
int n = this.getObj().dim();
SimpleBounds sb = this.getSimpleBounds();
double[] lbArr = (sb != null) ? sb.getLower() : null;
double[] ubArr = (sb != null) ? sb.getUpper() : null;
double[] xArr = getStartPoint();
if (xArr == null) {
xArr = new double[n];
}
for (int i = 0; i < n; i++) {
double li = (lbArr != null && i < lbArr.length && Double.isFinite(lbArr[i])) ? lbArr[i] : Double.NEGATIVE_INFINITY;
double ui = (ubArr != null && i < ubArr.length && Double.isFinite(ubArr[i])) ? ubArr[i] : Double.POSITIVE_INFINITY;
if (Double.isFinite(li) && Double.isFinite(ui)) {
xArr[i] = 0.5 * (li + ui);
} else if (Double.isFinite(ui)) {
xArr[i] = ui - 1.0;
} else if (Double.isFinite(li)) {
xArr[i] = li + 1.0;
} else {
xArr[i] = 0.0;
}
}
return new ArrayRealVector(xArr, false);
}
/**
* Project direction adjust QP solution direction to be sure respect the bounds.
* @param xVec current point
* @param dVec direction
*/
private void projectDirectionInPlace(RealVector xVec, RealVector dVec) {
final int n = xVec.getDimension();
for (int i = 0; i < n; i++) {
final double xi = xVec.getEntry(i);
final double minStep = LB.getEntry(i) - xi;
final double maxStep = UB.getEntry(i) - xi;
final double di = FastMath.min(FastMath.max(dVec.getEntry(i), minStep), maxStep);
dVec.setEntry(i, di);
}
}
}