us.ihmc.convexOptimization.linearProgram.LinearProgramSolver Maven / Gradle / Ivy
package us.ihmc.convexOptimization.linearProgram;
import gnu.trove.list.array.TIntArrayList;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import us.ihmc.matrixlib.MatrixTools;
import java.util.Arrays;
import static us.ihmc.convexOptimization.linearProgram.DictionaryFormLinearProgramSolver.maxVariables;
public class LinearProgramSolver
{
/* Fields for performing dictionary-form Linear Program optimization */
private final DictionaryFormLinearProgramSolver dictionaryFormSolver = new DictionaryFormLinearProgramSolver();
private final DMatrixRMaj startingDictionary = new DMatrixRMaj(maxVariables, maxVariables);
private final DMatrixRMaj augmentedInequalityMatrix = new DMatrixRMaj(maxVariables, maxVariables);
private final DMatrixRMaj augmentedInequalityVector = new DMatrixRMaj(maxVariables, maxVariables);
/* Fields for performing fixed-basis Linear Program optimization */
private final DMatrixRMaj basisMatrix = new DMatrixRMaj(0);
private final DMatrixRMaj basisMatrixInv = new DMatrixRMaj(0);
private final DMatrixRMaj dictionaryLeftColumn = new DMatrixRMaj(0);
/* Field for computing sensitivity wrt constraint matrix */
private final DMatrixRMaj tempSensitivityMatrix = new DMatrixRMaj(0);
/**
* Solves a standard form linear program
*
*
* max cTx, Ax <= b, x >= 0.
*
*
* Returns true if an optimal solution is computed or false otherwise.
*/
public boolean solve(DMatrixRMaj costVectorC, DMatrixRMaj inequalityConstraintMatrixA, DMatrixRMaj inequalityConstraintVectorB, DMatrixRMaj solutionToPack)
{
return solve(costVectorC, inequalityConstraintMatrixA, inequalityConstraintVectorB, solutionToPack, SolverMethod.SIMPLEX);
}
/**
* Solves the following linear program
*
*
* max cTx, Ax <= b, Cx == d, x >= 0.
*
*
* Returns true if an optimal solution is computed or false otherwise.
*/
public boolean solve(DMatrixRMaj costVectorC,
DMatrixRMaj inequalityConstraintMatrixA,
DMatrixRMaj inequalityConstraintVectorB,
DMatrixRMaj equalityConstraintMatrixC,
DMatrixRMaj equalityConstraintVectorD,
DMatrixRMaj solutionToPack,
SolverMethod solverMethod)
{
if (costVectorC.getNumCols() != 1 || inequalityConstraintVectorB.getNumCols() != 1 || equalityConstraintVectorD.getNumCols() != 1)
throw new IllegalArgumentException("Invalid matrix dimensions.");
if (inequalityConstraintMatrixA.getNumCols() != costVectorC.getNumRows())
throw new IllegalArgumentException("Invalid matrix dimensions.");
if (inequalityConstraintMatrixA.getNumRows() != inequalityConstraintVectorB.getNumRows())
throw new IllegalArgumentException("Invalid matrix dimensions.");
if (equalityConstraintMatrixC.getNumRows() != equalityConstraintVectorD.getNumRows())
throw new IllegalArgumentException("Invalid matrix dimensions.");
if (inequalityConstraintMatrixA.getNumCols() != equalityConstraintMatrixC.getNumCols())
throw new IllegalArgumentException("Invalid matrix dimensions.");
/* Pack augmented inequality matrices */
int constraints = inequalityConstraintMatrixA.getNumRows() + (2 * equalityConstraintMatrixC.getNumRows());
int dimensionality = inequalityConstraintMatrixA.getNumCols();
augmentedInequalityMatrix.reshape(constraints, dimensionality);
MatrixTools.setMatrixBlock(augmentedInequalityMatrix, 0, 0, inequalityConstraintMatrixA, 0, 0, inequalityConstraintMatrixA.getNumRows(), inequalityConstraintMatrixA.getNumCols(), 1.0);
MatrixTools.setMatrixBlock(augmentedInequalityMatrix, inequalityConstraintMatrixA.getNumRows(), 0, equalityConstraintMatrixC, 0, 0, equalityConstraintMatrixC.getNumRows(), equalityConstraintMatrixC.getNumCols(), 1.0);
MatrixTools.setMatrixBlock(augmentedInequalityMatrix, inequalityConstraintMatrixA.getNumRows() + equalityConstraintMatrixC.getNumRows(), 0, equalityConstraintMatrixC, 0, 0, equalityConstraintMatrixC.getNumRows(), equalityConstraintMatrixC.getNumCols(), -1.0);
augmentedInequalityVector.reshape(constraints, 1);
MatrixTools.setMatrixBlock(augmentedInequalityVector, 0, 0, inequalityConstraintVectorB, 0, 0, inequalityConstraintVectorB.getNumRows(), inequalityConstraintVectorB.getNumCols(), 1.0);
MatrixTools.setMatrixBlock(augmentedInequalityVector, inequalityConstraintVectorB.getNumRows(), 0, equalityConstraintVectorD, 0, 0, equalityConstraintVectorD.getNumRows(), equalityConstraintVectorD.getNumCols(), 1.0);
MatrixTools.setMatrixBlock(augmentedInequalityVector, inequalityConstraintVectorB.getNumRows() + equalityConstraintVectorD.getNumRows(), 0, equalityConstraintVectorD, 0, 0, equalityConstraintVectorD.getNumRows(), equalityConstraintVectorD.getNumCols(), -1.0);
return solve(costVectorC, augmentedInequalityMatrix, augmentedInequalityVector, solutionToPack, solverMethod);
}
/**
* Solves a standard form linear program using the selected SolverMethod (Simplex or Criss-Cross). If A contains implicit equality constraints,
* then the number of equality constraints can be passed in to notify the solver.
*
*
* max cTx, Ax <= b, x >= 0.
*
*
* Returns true if an optimal solution is computed or false otherwise.
*/
public boolean solve(DMatrixRMaj costVectorC, DMatrixRMaj inequalityConstraintMatrixA, DMatrixRMaj inequalityConstraintVectorB, DMatrixRMaj solutionToPack, SolverMethod solverMethod)
{
if (costVectorC.getNumCols() != 1 || inequalityConstraintVectorB.getNumCols() != 1)
throw new IllegalArgumentException("Invalid matrix dimensions.");
if (inequalityConstraintMatrixA.getNumCols() != costVectorC.getNumRows())
throw new IllegalArgumentException("Invalid matrix dimensions.");
if (inequalityConstraintMatrixA.getNumRows() != inequalityConstraintVectorB.getNumRows())
throw new IllegalArgumentException("Invalid matrix dimensions.");
startingDictionary.reshape(1 + inequalityConstraintMatrixA.getNumRows(), 1 + inequalityConstraintMatrixA.getNumCols());
Arrays.fill(startingDictionary.getData(), 0.0);
MatrixTools.setMatrixBlock(startingDictionary, 1, 0, inequalityConstraintVectorB, 0, 0, inequalityConstraintVectorB.getNumRows(), 1, 1.0);
MatrixTools.setMatrixBlock(startingDictionary, 1, 1, inequalityConstraintMatrixA, 0, 0, inequalityConstraintMatrixA.getNumRows(), inequalityConstraintMatrixA.getNumCols(), -1.0);
for (int i = 0; i < costVectorC.getNumRows(); i++)
{
startingDictionary.set(0, 1 + i, costVectorC.get(i));
}
if (solverMethod == SolverMethod.CRISS_CROSS)
{
dictionaryFormSolver.solveCrissCross(startingDictionary);
if (!dictionaryFormSolver.getCrissCrossStatistics().foundSolution())
{
return false;
}
}
else
{
dictionaryFormSolver.solveSimplex(startingDictionary);
if (!dictionaryFormSolver.getSimplexStatistics().foundSolution())
{
return false;
}
}
solutionToPack.set(dictionaryFormSolver.getPrimalSolution());
return true;
}
/**
* Given a Linear Program problem, this can directly compute the solution given a basis index set, assuming it is the optimal basis.
* Useful for computing how the output may change as the constraint matrix or cost vector changes, where small deviations maintain the same optimal basis.
* See doi.org/10.3929/ethz-b-000426221 CH 5 for details.
*/
public void solveForFixedBasis(DMatrixRMaj inequalityConstraintMatrixA, DMatrixRMaj inequalityConstraintVectorB, TIntArrayList basisIndices, DMatrixRMaj solutionToPack)
{
int numberOfNominalDecisionVariables = inequalityConstraintMatrixA.getNumCols();
int numberOfConstraints = inequalityConstraintMatrixA.getNumRows();
basisMatrix.reshape(numberOfConstraints, numberOfConstraints);
basisMatrix.zero();
for (int i = 1; i < basisIndices.size(); i++)
{
int basisMatrixColumn = i - 1;
int lexicalBasisIndex = basisIndices.get(i);
if (isNonNegativeConstraint(lexicalBasisIndex, numberOfNominalDecisionVariables))
{
int variableIndex = toVariableIndex(lexicalBasisIndex);
for (int j = 0; j < numberOfConstraints; j++)
{
basisMatrix.set(j, basisMatrixColumn, inequalityConstraintMatrixA.get(j, variableIndex));
}
}
else
{
int constraintIndex = toConstraintIndex(lexicalBasisIndex, numberOfNominalDecisionVariables);
basisMatrix.set(constraintIndex, basisMatrixColumn, 1.0);
}
}
CommonOps_DDRM.invert(basisMatrix, basisMatrixInv);
CommonOps_DDRM.mult(basisMatrixInv, inequalityConstraintVectorB, dictionaryLeftColumn);
solutionToPack.reshape(numberOfNominalDecisionVariables, 1);
solutionToPack.zero();
for (int i = 1; i < basisIndices.size(); i++)
{
int basisMatrixColumn = i - 1;
int lexicalBasisIndex = basisIndices.get(i);
if (isNonNegativeConstraint(lexicalBasisIndex, numberOfNominalDecisionVariables))
{
int variableIndex = toVariableIndex(lexicalBasisIndex);
solutionToPack.set(variableIndex, 0, dictionaryLeftColumn.get(basisMatrixColumn, 0));
}
}
}
public double computeSensitivity(DMatrixRMaj constraintMatrixVariation)
{
return computeSensitivity(constraintMatrixVariation,
dictionaryFormSolver.getPrimalSolution(),
dictionaryFormSolver.getDualSolution(),
tempSensitivityMatrix);
}
/**
* Computes the sensitivity of the objective cost with respect to variation of the constraint matrix.
* Given the nominal LP {max z = c^T x, Ax <= b, x <= 0}, this variation of A is A <-- (A + G * theta).
* For a nominal primal and dual solution x_p, x_d and small theta, variation in the objective is: dz / dtheta = - x_d^T G x_p.
* See doi.org/10.1007/BFb0121039.
*/
public static double computeSensitivity(DMatrixRMaj constraintMatrixVariation,
DMatrixRMaj primalSolution,
DMatrixRMaj dualSolution,
DMatrixRMaj tempSensitivityMatrix)
{
CommonOps_DDRM.mult(constraintMatrixVariation, primalSolution, tempSensitivityMatrix);
return - CommonOps_DDRM.dot(dualSolution, tempSensitivityMatrix);
}
public DMatrixRMaj getDualSolution()
{
return dictionaryFormSolver.getDualSolution();
}
public SolverStatistics getSimplexStatistics()
{
return dictionaryFormSolver.getSimplexStatistics();
}
public SolverStatistics getCrissCrossStatistics()
{
return dictionaryFormSolver.getCrissCrossStatistics();
}
public DMatrixRMaj getAugmentedInequalityMatrix()
{
return augmentedInequalityMatrix;
}
public DMatrixRMaj getAugmentedInequalityVector()
{
return augmentedInequalityVector;
}
/**
* Basis indices of the solution dictionary. See {@link LinearProgramDictionary} for details.
* The first entry always represents the objective xf variable.
*/
public TIntArrayList getBasisIndices()
{
return dictionaryFormSolver.getBasisIndices();
}
/**
* Non-basis indices of the solution dictionary. See {@link LinearProgramDictionary} for details.
* The first entry always represents the RHS xg variable.
*/
public TIntArrayList getNonBasisIndices()
{
return dictionaryFormSolver.getNonBasisIndices();
}
/**
* Returns true if the given lexical index corresponds to a non-negative constraint (x>0) or
* false if corresponds to a canonical form inequality constraint Ax<=b.
*/
public boolean isNonNegativeConstraint(int lexicalIndex)
{
return isNonNegativeConstraint(lexicalIndex, startingDictionary.getNumCols() - 1);
}
/**
* Returns the zero-indexed constraint index corresponding to the given lexical index
*/
public int toConstraintIndex(int lexicalIndex)
{
return toConstraintIndex(lexicalIndex, startingDictionary.getNumCols() - 1);
}
/**
* Returns true if the given lexical index corresponds to a non-negative constraint (x>0) or
* false if corresponds to a canonical form inequality constraint Ax<=b.
*/
public static boolean isNonNegativeConstraint(int lexicalIndex, int numCanonicalFormVariables)
{
if (lexicalIndex <= 0)
{
throw new RuntimeException("Invalid lexical index " + lexicalIndex + ", should be a constraint index");
}
return lexicalIndex <= numCanonicalFormVariables;
}
/**
* Returns the zero-indexed variable index corresponding to the given lexical index
*/
public static int toVariableIndex(int lexicalIndex)
{
if (lexicalIndex <= 0)
{
throw new RuntimeException("Invalid lexical index " + lexicalIndex + ", should be a constraint index");
}
return lexicalIndex - 1;
}
/**
* Returns the zero-indexed constraint index corresponding to the given lexical index
*/
public static int toConstraintIndex(int lexicalIndex, int numCanonicalFormVariables)
{
if (lexicalIndex <= 0)
{
throw new RuntimeException("Invalid lexical index " + lexicalIndex + ", should be a constraint index");
}
return lexicalIndex - numCanonicalFormVariables - 1;
}
}