SQPOptimizerGM.java

  1. /*
  2.  * Licensed to the Hipparchus project under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *      https://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.hipparchus.optim.nonlinear.vector.constrained;

  19. import org.hipparchus.linear.Array2DRowRealMatrix;
  20. import org.hipparchus.linear.ArrayRealVector;
  21. import org.hipparchus.linear.EigenDecompositionSymmetric;
  22. import org.hipparchus.linear.MatrixUtils;
  23. import org.hipparchus.linear.RealMatrix;
  24. import org.hipparchus.linear.RealVector;
  25. import org.hipparchus.optim.nonlinear.scalar.ObjectiveFunction;
  26. import org.hipparchus.util.FastMath;

  28. /**
  29.  * Sequential Quadratic Programming Optimizer.
  30.  * <br/>
  31.  * min f(x)
  32.  * <br/>
  33.  * q(x)=b1
  34.  * <br/>
  35.  * h(x)>=b2
  36.  * <br/>
  37.  * Algorithm based on paper:"Some Theoretical properties of an augmented lagrangian merit function
  38.  * (Gill,Murray,Sauders,Wriht,April 1986)"
  39.  * @since 3.1
  40.  */
  41. public class SQPOptimizerGM extends AbstractSQPOptimizer {

  43.     /** Jacobian constraint. */
  44.     private RealMatrix constraintJacob;

  46.     /** Value of the equality constraint. */
  47.     private RealVector equalityEval;

  49.     /** Value of the inequality constraint. */
  50.     private RealVector inequalityEval;

  52.     /** Gradient of the objective function. */
  53.     private RealVector functionGradient;

  55.     /** Hessian of the objective function. */
  56.     private RealMatrix hessian;

  58.     /** {@inheritDoc} */
  59.     @Override
  60.     public LagrangeSolution doOptimize() {
  61.         int me = 0;
  62.         int mi = 0;

  64.         //EQUALITY CONSTRAINT
  65.         if (getEqConstraint() != null) {
  66.             me = getEqConstraint().dimY();
  67.         }
  68.         //INEQUALITY CONSTRAINT
  69.         if (getIqConstraint() != null) {
  70.             mi = getIqConstraint().dimY();
  71.         }

  73.         double alpha;
  74.         double rho;
  75.         RealVector x;
  76.         if (getStartPoint() != null) {
  77.             x = new ArrayRealVector(getStartPoint());
  78.         } else {
  79.             x = new ArrayRealVector(getObj().dim());
  80.         }

  82.         RealVector y = new ArrayRealVector(me + mi, 0.0);
  83.          //INITIAL VALUES
  84.         double functionEval = getObj().value(x);
  85.         functionGradient = getObj().gradient(x);
  86.         double maxGrad = functionGradient.getLInfNorm();

  88.         if (getEqConstraint() != null) {
  89.           equalityEval = getEqConstraint().value(x);
  90.         }
  91.         if (getIqConstraint() != null) {
  92.             inequalityEval = getIqConstraint().value(x);
  93.         }
  94.         constraintJacob = computeJacobianConstraint(x);

  96.         if (getEqConstraint() != null) {
  97.             maxGrad = FastMath.max(maxGrad, equalityEval.getLInfNorm());
  98.         }
  99.         if (getIqConstraint() != null) {
  100.             maxGrad = FastMath.max(maxGrad, inequalityEval.getLInfNorm());
  101.         }

  103.         if (!getSettings().useFunHessian()) {
  104.             hessian= MatrixUtils.createRealIdentityMatrix(x.getDimension()).scalarMultiply(maxGrad);
  105.         } else {
  106.             hessian = getObj().hessian(x);
  107.         }


  110.         rho = 0;

  112.         for (int i = 0; i < getMaxIterations(); i++) {

  114.             iterations.increment();

  116.             alpha = 1.0;

  118.             LagrangeSolution sol1;
  119.             //SOLVE QP
  120.             sol1 = solveQP(x);
  121.             RealVector p = sol1.getX();
  122.             RealVector e = sol1.getLambda().subtract(y);

  124.             RealVector s = calculateSvector(y,rho);
  125.             RealVector se = null;
  126.             RealMatrix jacobi; // NOPMD - PMD detext a false positive here
  127.             RealVector q = null;
  128.             RealVector qe = null;

  130.             //TEST CON SI SOLO PER INEQUALITY AND Q FOR ALL
  131.             if (getEqConstraint() != null) {
  132.                 se = new ArrayRealVector(me);
  133.                 jacobi = constraintJacob.getSubMatrix(0, me - 1, 0, x.getDimension() - 1); // NOPMD - PMD detect a false positive here
  134.                 qe = new ArrayRealVector(me);
  135.             }
  136.             if (getIqConstraint() != null) {
  137.                 jacobi = constraintJacob.getSubMatrix(me, me + mi - 1, 0, x.getDimension() - 1);
  138.                 q = jacobi.operate(p).add(inequalityEval.subtract(getIqConstraint().getLowerBound())).subtract(s);
  139.             }



  143.             //CALCULATE PENALTY GRADIENT
  144.             //
  145.             double penaltyGradient = penaltyFunctionGrad(p, y, s, e, qe, q, rho);
  146.             ArrayRealVector g = new ArrayRealVector(me + mi);
  147.             if (me > 0) {
  148.                 g.setSubVector(0,equalityEval.subtract(getEqConstraint().getLowerBound()));
  149.             }
  150.             if (mi > 0) {
  151.                 g.setSubVector(me, inequalityEval.subtract(getIqConstraint().getLowerBound()).subtract(s));
  152.             }

  154.             double rhoSegnato = 2.0 * e.getNorm() / g.getNorm();

  156.            // rho = rhoSegnato;
  157.             //if (!(penaltyGradient<=-0.5 * p.dotProduct(hessian.operate(p))))
  158.            while (penaltyGradient > -0.5 * p.dotProduct(hessian.operate(p))) {

  160.                  rho = FastMath.max(rhoSegnato,2.0 * rho);

  162.                 penaltyGradient = penaltyFunctionGrad(p, y, s, e, qe, q, rho);
  163.             }
  164.             //LINE SEARCH
  165.             double alfaEval = getObj().value(x.add(p.mapMultiply(alpha)));

  167.             double alphaPenalty;
  168.             RealVector sineq = null;
  169.             RealVector seq = null;
  170.             if (se != null) {
  171.                 seq = se.add(qe.mapMultiply(alpha));
  172.             }
  173.             if (s != null) {
  174.                 sineq = s.add(q.mapMultiply(alpha));
  175.             }

  177.             double currentPenalty = penaltyFunction(functionEval, x, y, se, s, rho);
  178.             alphaPenalty = penaltyFunction(alfaEval,x.add(p.mapMultiply(alpha)),
  179.                                            y.add(e.mapMultiply(alpha)),
  180.                                            seq, sineq, rho);



  184.             int search = 0;
  185.             while ((alphaPenalty -currentPenalty) >= getSettings().getMu() * alpha *  penaltyGradient &&
  186.                    search < getSettings().getMaxLineSearchIteration()) {

  188.                 double alfaStar = -0.5 * alpha * alpha *  penaltyGradient/ (-alpha *  penaltyGradient + alphaPenalty - currentPenalty);

  190.                 alpha =  FastMath.max(getSettings().getB() * alpha, FastMath.min(1.0,alfaStar));
  191.                 alfaEval = getObj().value(x.add(p.mapMultiply(alpha)));
  192.                 if (se != null) {
  193.                     seq = se.add(qe.mapMultiply(alpha));
  194.                 }
  195.                 if (s != null) {
  196.                     sineq = s.add(q.mapMultiply(alpha));
  197.                 }
  198.                 alphaPenalty = penaltyFunction(alfaEval,x.add(p.mapMultiply(alpha)),y.add(e.mapMultiply(alpha)),seq,sineq,rho);

  200.                 search = search + 1;

  202.             }


  205.             if (getSettings().getConvCriteria() == 0) {
  206.                 if (p.mapMultiply(alpha).dotProduct(hessian.operate(p.mapMultiply(alpha))) < getSettings().getEps() * getSettings().getEps()) {
  207.                     break;
  208.                 }
  209.             } else {
  210.                 if (alpha * p.getNorm() <FastMath.sqrt(getSettings().getEps()) * (1 + x.getNorm())) {
  211.                     break;
  212.                 }

  214.             }

  216.             //UPDATE ALL FUNCTION
  217.             RealVector oldGradient = functionGradient;
  218.             RealMatrix oldJacob = constraintJacob;
  219.             RealVector old1 = lagrangianGradX(oldGradient,oldJacob,x,y.add(e.mapMultiply(alpha)));
  220.             functionEval = alfaEval;
  221.             functionGradient = getObj().gradient(x.add(p.mapMultiply(alpha)));
  222.             constraintJacob = computeJacobianConstraint(x.add(p.mapMultiply(alpha)));
  223.             RealVector new1 = lagrangianGradX(functionGradient,constraintJacob,x.add(p.mapMultiply(alpha)),y.add(e.mapMultiply(alpha)));
  224.             hessian = BFGSFormula(hessian, p, alpha, new1, old1);

  226.             if (getEqConstraint() != null) {
  227.                 equalityEval = getEqConstraint().value(x.add(p.mapMultiply(alpha)));
  228.             }
  229.             if (getIqConstraint() != null) {
  230.                 inequalityEval = getIqConstraint().value(x.add(p.mapMultiply(alpha)));
  231.             }

  233.             x = x.add(p.mapMultiply(alpha));
  234.             y = y.add(e.mapMultiply(alpha));
  235.         }

  237.         functionEval = getObj().value(x);
  238.         functionGradient = getObj().gradient(x);

  240.         constraintJacob = computeJacobianConstraint(x);
  241.         return new LagrangeSolution(x, y, functionEval);
  242.     }

  244.     private RealVector calculateSvector(RealVector y, double rho) {
  245.         int me = 0;
  246.         int mi;
  247.         RealVector si = null;
  248.         if (getEqConstraint() != null) {
  249.             me = getEqConstraint().dimY();
  250.         }
  251.         if (getIqConstraint() != null) {
  252.             mi = getIqConstraint().dimY();
  253.             si = new ArrayRealVector(mi);
  254.             RealVector yi = y.getSubVector(me, mi);
  255.             for (int i = 0; i < inequalityEval.getDimension(); i++) {
  256.                 if (rho == 0) {
  257.                     si.setEntry(i, FastMath.max(0, inequalityEval.getEntry(i)));
  258.                 } else {
  259.                     si.setEntry(i, FastMath.max(0, inequalityEval.getEntry(i) - yi.getEntry(i) / rho));
  260.                 }
  261.             }
  262.         }
  263.         return si;
  264.     }

  266.     private double penaltyFunction(double currentF, RealVector x, RealVector y, RealVector se, RealVector s, double rho) {
  267.         // the set of constraints is the same as the previous one but they must be evaluated with the increment

  269.         int me = 0;
  270.         int mi;
  271.         double partial = currentF;

  273.         if (getEqConstraint() != null) {
  274.             me = getEqConstraint().dimY();

  276.             RealVector ye = y.getSubVector(0, me);
  277.             RealVector g = getEqConstraint().value(x).subtract(getEqConstraint().getLowerBound());
  278.             partial += -ye.dotProduct(g.subtract(se)) + 0.5 * rho * (g.subtract(se)).dotProduct(g.subtract(se));
  279.         }

  281.         if (getIqConstraint() != null) {
  282.             mi = getIqConstraint().dimY();

  284.             RealVector yi = y.getSubVector(me, mi);

  286.             RealVector g = getIqConstraint().value(x).subtract(getIqConstraint().getLowerBound());





  292.             partial+= -yi.dotProduct(g.subtract(s)) +0.5 * rho*(g.subtract(s)).dotProduct(g.subtract(s));

  294.         }

  296.         return partial;
  297.     }

  299.     private double penaltyFunctionGrad(RealVector p, RealVector y,RealVector s,RealVector e,RealVector qe, RealVector q,double rho) {

  301.         int me = 0;
  302.         int mi;
  303.         double partial = functionGradient.dotProduct(p);
  304.         if (getEqConstraint() != null) {
  305.             me = getEqConstraint().dimY();

  307.             RealVector ye = y.getSubVector(0, me);
  308.             RealVector ee = e.getSubVector(0, me);
  309.           //  RealVector qe = q.getSubVector(0, me);


  312.             RealVector ge = equalityEval.subtract(getEqConstraint().getLowerBound());
  313.             RealMatrix jacob = constraintJacob.getSubMatrix(0, me - 1, 0, p.getDimension() - 1);
  314.             double term2 = p.dotProduct(jacob.transpose().operate(ye));
  315.             double term3 = p.dotProduct(jacob.transpose().operate(ge))*rho;
  316.             double term4 = ge.dotProduct(ee);
  317.             double term5 = ye.dotProduct(qe);
  318.             double term6 = qe.dotProduct(ge)*rho;
  319.             partial +=-term2 + term3-term4 + term5-term6;
  320.         }

  322.         if (getIqConstraint() != null) {
  323.             mi = getIqConstraint().dimY();

  325.             RealVector yi = y.getSubVector(me, mi);
  326.             RealVector ei = e.getSubVector(me, mi);

  328.             RealVector gi = inequalityEval.subtract(getIqConstraint().getLowerBound());
  329.             RealMatrix jacob = constraintJacob.getSubMatrix(me, me + mi - 1, 0, p.getDimension() - 1);
  330.             double term2 = p.dotProduct(jacob.transpose().operate(yi));
  331.             double term3 = p.dotProduct(jacob.transpose().operate(gi.subtract(s)))*rho;
  332.             double term4 = (gi.subtract(s)).dotProduct(ei);
  333.             double term5 = yi.dotProduct(q);
  334.             double term6 = q.dotProduct(gi.subtract(s))*rho;

  336.             partial +=-term2 + term3-term4 + term5-term6;
  337.         }

  339.         return partial;
  340.     }

  342.     private LagrangeSolution solveQP(RealVector x) {

  344.         RealMatrix H = hessian;
  345.         RealVector g = functionGradient;

  347.         int me = 0;
  348.         int mi = 0;
  349.         if (getEqConstraint() != null) {
  350.             me = getEqConstraint().dimY();
  351.         }
  352.         if (getIqConstraint() != null) {

  354.             mi = getIqConstraint().dimY();

  356.         }

  358.         RealMatrix H1 = new Array2DRowRealMatrix(H.getRowDimension() , H.getRowDimension() );
  359.         H1.setSubMatrix(H.getData(), 0, 0);
  360.         RealVector g1 = new ArrayRealVector(g.getDimension() );
  361.         g1.setSubVector(0, g);

  363.         LinearEqualityConstraint eqc = null;
  364.         if (getEqConstraint() != null) {
  365.             RealMatrix eqJacob = constraintJacob.getSubMatrix(0,me-1,0,x.getDimension()-1);

  367.             RealMatrix Ae = new Array2DRowRealMatrix(me, x.getDimension() );
  368.             RealVector be = new ArrayRealVector(me);
  369.             Ae.setSubMatrix(eqJacob.getData(), 0, 0);



  373.             be.setSubVector(0, getEqConstraint().getLowerBound().subtract(equalityEval));
  374.             eqc = new LinearEqualityConstraint(Ae, be);

  376.         }
  377.         LinearInequalityConstraint iqc = null;

  379.         if (getIqConstraint() != null) {

  381.             RealMatrix iqJacob = constraintJacob.getSubMatrix(me,me + mi-1,0,x.getDimension()-1);

  383.             RealMatrix Ai = new Array2DRowRealMatrix(mi, x.getDimension());
  384.             RealVector bi = new ArrayRealVector(mi);
  385.             Ai.setSubMatrix(iqJacob.getData(), 0, 0);

  387.             bi.setSubVector(0, getIqConstraint().getLowerBound().subtract(inequalityEval));

  389.             iqc = new LinearInequalityConstraint(Ai, bi);

  391.         }

  393.         QuadraticFunction q = new QuadraticFunction(H1, g1, 0);
  394.         LagrangeSolution sol;

  396.         ADMMQPOptimizer solver = new ADMMQPOptimizer();
  397.         sol = solver.optimize(new ObjectiveFunction(q), iqc, eqc);

  399.         return new LagrangeSolution(sol.getX(), sol.getLambda(),0.0);

  401.     }

  403.     private RealMatrix computeJacobianConstraint(RealVector x) {
  404.         int me = 0;
  405.         int mi = 0;
  406.         RealMatrix je = null;
  407.         RealMatrix ji = null;
  408.         if (getEqConstraint() != null) {
  409.             me = getEqConstraint().dimY();
  410.             je = getEqConstraint().jacobian(x);
  411.         }

  413.         if (getIqConstraint() != null) {
  414.             mi = getIqConstraint().dimY();
  415.             ji = getIqConstraint().jacobian(x);
  416.         }

  418.         RealMatrix jacobian = new Array2DRowRealMatrix(me + mi, x.getDimension());
  419.         if (me > 0) {
  420.             jacobian.setSubMatrix(je.getData(), 0, 0);
  421.         }
  422.         if (mi > 0) {
  423.             jacobian.setSubMatrix(ji.getData(), me, 0);
  424.         }

  426.         return jacobian;
  427.     }
  428.     /*
  429.      *DAMPED BFGS FORMULA
  430.      */

  432.     private RealMatrix BFGSFormula(RealMatrix oldH, RealVector dx, double alfa, RealVector newGrad, RealVector oldGrad) {

  434.         RealMatrix oldH1 = oldH;
  435.         RealVector y1 = newGrad.subtract(oldGrad);
  436.         RealVector s = dx.mapMultiply(alfa);

  438.         double theta = 1.0;
  439.         if (s.dotProduct(y1) <= 0.2 * s.dotProduct(oldH.operate(s))) {
  440.             theta = 0.8 * s.dotProduct(oldH.operate(s)) / (s.dotProduct(oldH.operate(s)) - s.dotProduct(y1));
  441.         }

  443.         RealVector y = y1.mapMultiply(theta).add(oldH.operate(s).mapMultiply(1.0 - theta));

  445.         RealMatrix firstTerm = y.outerProduct(y).scalarMultiply(1.0 / s.dotProduct(y));
  446.         RealMatrix secondTerm = oldH1.multiply(s.outerProduct(s)).multiply(oldH);
  447.         double thirtTerm = s.dotProduct(oldH1.operate(s));
  448.         RealMatrix Hnew = oldH1.add(firstTerm).subtract(secondTerm.scalarMultiply(1.0 / thirtTerm));
  449.         //RESET HESSIAN IF NOT POSITIVE DEFINITE
  450.         EigenDecompositionSymmetric dsX = new EigenDecompositionSymmetric(Hnew);
  451.         double min = new ArrayRealVector(dsX.getEigenvalues()).getMinValue();
  452.         if (new ArrayRealVector(dsX.getEigenvalues()).getMinValue() < 0) {

  454.             RealMatrix diag = dsX.getD().
  455.                               add(MatrixUtils.createRealIdentityMatrix(dx.getDimension()).
  456.                                   scalarMultiply(getSettings().getEps() - min));
  457.             Hnew = dsX.getV().multiply(diag.multiply(dsX.getVT()));

  459.         }
  460.         return Hnew;

  462.     }

  464. }
Created with JaCoCo 0.8.12.202403310830