int main(int argc,char *argv[])
{
MSKrescodee r;
MSKidxt i,j;
double c[] = {3.0, 1.0, 5.0, 1.0};
/* Below is the sparse representation of the A
matrix stored by column. */
MSKlidxt aptrb[] = {0, 2, 5, 7};
MSKlidxt aptre[] = {2, 5, 7, 9};
MSKidxt asub[] = { 0, 1,
0, 1, 2,
0, 1,
1, 2};
double aval[] = { 3.0, 2.0,
1.0, 1.0, 2.0,
2.0, 3.0,
1.0, 3.0};
/* Bounds on constraints. */
MSKboundkeye bkc[] = {MSK_BK_FX, MSK_BK_LO, MSK_BK_UP };
double blc[] = {30.0, 15.0, -MSK_INFINITY};
double buc[] = {30.0, +MSK_INFINITY, 25.0 };
/* Bounds on variables. */
MSKboundkeye bkx[] = {MSK_BK_LO, MSK_BK_RA, MSK_BK_LO, MSK_BK_LO };
double blx[] = {0.0, 0.0, 0.0, 0.0 };
double bux[] = {+MSK_INFINITY, 10.0, +MSK_INFINITY, +MSK_INFINITY };
double xx[NUMVAR];
MSKenv_t env = NULL;
MSKtask_t task = NULL;
/* Create the mosek environment. */
r = MSK_makeenv(&env,NULL,NULL,NULL,NULL);
/* Directs the env log stream to the 'printstr' function. */
if ( r==MSK_RES_OK )
MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,printstr);
/* Initialize the environment. */
if ( r==MSK_RES_OK )
r = MSK_initenv(env);
if ( r==MSK_RES_OK )
{
/* Create the optimization task. */
r = MSK_maketask(env,NUMCON,NUMVAR,&task);
/* Directs the log task stream to the 'printstr' function. */
if ( r==MSK_RES_OK )
MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
/* Give MOSEK an estimate of the size of the input data.
This is done to increase the speed of inputting data.
However, it is optional. */
if (r == MSK_RES_OK)
r = MSK_putmaxnumvar(task,NUMVAR);
if (r == MSK_RES_OK)
r = MSK_putmaxnumcon(task,NUMCON);
if (r == MSK_RES_OK)
r = MSK_putmaxnumanz(task,NUMANZ);
/* Append 'NUMCON' empty constraints.
The constraints will initially have no bounds. */
if ( r == MSK_RES_OK )
r = MSK_append(task,MSK_ACC_CON,NUMCON);
/* Append 'NUMVAR' variables.
The variables will initially be fixed at zero (x=0). */
if ( r == MSK_RES_OK )
r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
/* Optionally add a constant term to the objective. */
if ( r ==MSK_RES_OK )
r = MSK_putcfix(task,0.0);
for(j=0; j<NUMVAR && r == MSK_RES_OK; ++j)
{
/* Set the linear term c_j in the objective.*/
if(r == MSK_RES_OK)
r = MSK_putcj(task,j,c[j]);
/* Set the bounds on variable j.
blx[j] <= x_j <= bux[j] */
if(r == MSK_RES_OK)
r = MSK_putbound(task,
MSK_ACC_VAR, /* Put bounds on variables.*/
j, /* Index of variable.*/
bkx[j], /* Bound key.*/
blx[j], /* Numerical value of lower bound.*/
bux[j]); /* Numerical value of upper bound.*/
/* Input column j of A */
if(r == MSK_RES_OK)
r = MSK_putavec(task,
MSK_ACC_VAR, /* Input columns of A.*/
j, /* Variable (column) index.*/
aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
asub+aptrb[j], /* Pointer to row indexes of column j.*/
aval+aptrb[j]); /* Pointer to Values of column j.*/
}
/* Set the bounds on constraints.
for i=1, ...,NUMCON : blc[i] <= constraint i <= buc[i] */
for(i=0; i<NUMCON && r==MSK_RES_OK; ++i)
r = MSK_putbound(task,
MSK_ACC_CON, /* Put bounds on constraints.*/
i, /* Index of constraint.*/
bkc[i], /* Bound key.*/
blc[i], /* Numerical value of lower bound.*/
buc[i]); /* Numerical value of upper bound.*/
/* Maximize objective function. */
if (r == MSK_RES_OK)
r = MSK_putobjsense(task,
MSK_OBJECTIVE_SENSE_MAXIMIZE);
if ( r==MSK_RES_OK )
{
MSKrescodee trmcode;
/* Run optimizer */
r = MSK_optimizetrm(task,&trmcode);
/* Print a summary containing information
about the solution for debugging purposes. */
MSK_solutionsummary (task,MSK_STREAM_LOG);
if ( r==MSK_RES_OK )
{
MSKsolstae solsta;
int j;
MSK_getsolutionstatus (task,
MSK_SOL_BAS,
NULL,
&solsta);
switch(solsta)
{
case MSK_SOL_STA_OPTIMAL:
case MSK_SOL_STA_NEAR_OPTIMAL:
MSK_getsolutionslice(task,
MSK_SOL_BAS, /* Request the basic solution. */
MSK_SOL_ITEM_XX,/* Which part of solution. */
0, /* Index of first variable. */
NUMVAR, /* Index of last variable+1. */
xx);
printf("Optimal primal solution\n");
for(j=0; j<NUMVAR; ++j)
printf("x[%d]: %e\n",j,xx[j]);
break;
case MSK_SOL_STA_DUAL_INFEAS_CER:
case MSK_SOL_STA_PRIM_INFEAS_CER:
case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
printf("Primal or dual infeasibility certificate found.\n");
break;
case MSK_SOL_STA_UNKNOWN:
printf("The status of the solution could not be determined.\n");
break;
default:
printf("Other solution status.");
break;
}
}
else
{
printf("Error while optimizing.\n");
}
}
if (r != MSK_RES_OK)
{
/* In case of an error print error code and description. */
char symname[MSK_MAX_STR_LEN];
char desc[MSK_MAX_STR_LEN];
printf("An error occurred while optimizing.\n");
MSK_getcodedesc (r,
symname,
desc);
printf("Error %s - '%s'\n",symname,desc);
}
MSK_deletetask(&task);
MSK_deleteenv(&env);
}
return r;
}
bool ConicSolver::Solve(VectorXd& sol)
{
bool ret = false;
#ifdef _WIN32
VectorXd solution;
convertMatrixVectorFormat();
MSKenv_t env;
MSKtask_t task;
MSKrescodee r;
r = MSK_makeenv(&env, NULL, NULL, NULL, NULL);
if (r == MSK_RES_OK)
{
r = MSK_linkfunctoenvstream(env, MSK_STREAM_LOG, NULL, printstr);
}
r = MSK_initenv(env);
if (r == MSK_RES_OK)
{
r = MSK_maketask(env, mNumCon, mNumVar, &task);
if (r == MSK_RES_OK)
{
r = MSK_linkfunctotaskstream(task, MSK_STREAM_LOG, NULL, printstr);
}
if (r == MSK_RES_OK)
r = MSK_putmaxnumvar(task, mNumVar);
if (r == MSK_RES_OK)
r = MSK_putmaxnumcon(task, mNumCon);
/* Append ¡¯NUMCON ¡¯ empty constraints .
The constraints will initially have no bounds . */
if (r == MSK_RES_OK)
r = MSK_append(task, MSK_ACC_CON, mNumCon);
/* Append ¡¯NUMVAR ¡¯ variables .
The variables will initially be fixed at zero (x =0). */
if (r == MSK_RES_OK)
r = MSK_append(task, MSK_ACC_VAR, mNumVar);
/* Optionally add a constant term to the objective . */
if (r == MSK_RES_OK)
r = MSK_putcfix(task, mConstant);
for (int j = 0; j < mNumVar && r == MSK_RES_OK; ++j)
{
/* Set the linear term c_j in the objective .*/
if (r == MSK_RES_OK)
r = MSK_putcj(task, j, mc[j]);
/* Set the bounds on variable j.*/
if (r == MSK_RES_OK)
{
if (mbLowerBounded[j] && mbUpperBounded[j])
{
if (mlb[j] == mub[j])
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_FX, mlb[j], mub[j]);
else
{
CHECK(mlb[j] < mub[j]);
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_RA, mlb[j], mub[j]);
}
}
else if (mbLowerBounded[j])
{
r = MSK_putbound(task, MSK_ACC_VAR, j , MSK_BK_LO, mlb[j], +MSK_INFINITY);
}
else if (mbUpperBounded[j])
{
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_UP, -MSK_INFINITY, mub[j]);
}
else
{
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_FR, -MSK_INFINITY, +MSK_INFINITY);
}
}
/* Input column j of A */
if (r == MSK_RES_OK && mNumCon)
{
int currentColumnIdx = mAColumnStartIdx[j];
int nextColumnIdx = mAColumnStartIdx[j + 1];
if (nextColumnIdx - currentColumnIdx > 0)
r = MSK_putavec(task, MSK_ACC_VAR, j, nextColumnIdx - currentColumnIdx, &(mARowIdx[currentColumnIdx]), &(mAValues[currentColumnIdx]));
}
}
/* Set the bounds on constraints .
for i=1, ... , NUMCON : blc [i] <= constraint i <= buc [i] */
for (int i = 0; i < mNumCon && r == MSK_RES_OK; ++i)
{
if (mbConstraintLowerBounded[i] && mbConstraintUpperBounded[i])
{
if (mlbc[i] == mubc[i])
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_FX, mlbc[i], mubc[i]);
}
else
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_RA, mlbc[i], mubc[i]);
}
}
else if (mbConstraintLowerBounded[i])
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_LO, mlbc[i], +MSK_INFINITY);
}
else if (mbConstraintUpperBounded[i])
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_UP, -MSK_INFINITY, mubc[i]);
}
else
{
LOG(WARNING) << "Every constraint should not be free.";
}
}
for (int i = 0; i < mNumCone; ++i)
{
Cone& cone = mCones[i];
r = MSK_appendcone(task, MSK_CT_RQUAD, 0.0, cone.mSubscripts.size(), cone.GetMosekConeSubId());
//r = MSK_appendcone(task, MSK_CT_QUAD, 0.0, cone.mSubscripts.size(), cone.GetMosekConeSubId());
}
if (r == MSK_RES_OK)
{
MSKrescodee trmcode;
r = MSK_optimizetrm(task, &trmcode);
MSK_solutionsummary(task, MSK_STREAM_LOG);
if (r == MSK_RES_OK)
{
MSKsolstae solsta;
MSK_getsolutionstatus(task, MSK_SOL_ITR, NULL, &solsta);
double* result = new double[mNumVar];
switch (solsta)
{
case MSK_SOL_STA_OPTIMAL:
case MSK_SOL_STA_NEAR_OPTIMAL:
MSK_getsolutionslice(task, MSK_SOL_ITR, MSK_SOL_ITEM_XX, 0, mNumVar, result);
LOG(INFO) << "Optimal primal solution";
ret = true;
solution = VectorXd::Zero(mNumVar);
sol = VectorXd::Zero(mNumVar);
for (int k = 0; k < mNumVar; ++k)
{
solution[k] = result[k];
sol[k] = result[k];
}
break;
case MSK_SOL_STA_DUAL_INFEAS_CER:
case MSK_SOL_STA_PRIM_INFEAS_CER:
case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
LOG(WARNING) << "Primal or dual infeasibility certificate found.";
break;
case MSK_SOL_STA_UNKNOWN:
LOG(WARNING) << "The status of the solution could not be determined.";
break;
default:
LOG(WARNING) << "Other solution status.";
break;
}
delete[] result;
}
}
else
{
LOG(WARNING) << "Error while optimizing.";
}
if (r != MSK_RES_OK)
{
char symname[MSK_MAX_STR_LEN];
char desc[MSK_MAX_STR_LEN];
LOG(WARNING) << "An error occurred while optimizing.";
MSK_getcodedesc(r, symname, desc);
LOG(WARNING) << "Error " << symname << " - " << desc;
}
}
MSK_deletetask(&task);
MSK_deleteenv(&env);
#endif
return ret;
}
int mosekNNSolverWrapper(const Matrix &Q, const Matrix &Eq, const Matrix &b,
const Matrix &InEq, const Matrix &ib,
const Matrix &lowerBounds, const Matrix &upperBounds,
Matrix &sol, double *objVal, MosekObjectiveType objType)
{
DBGP("Mosek QP Wrapper started");
MSKrescodee r;
MSKtask_t task = NULL;
// Get the only instance of the mosek environment.
MSKenv_t env = getMosekEnv();
// Create the optimization task.
r = MSK_maketask(env, 0, 0, &task);
if (r != MSK_RES_OK) {
DBGA("Failed to create optimization task");
return -1;
}
MSK_linkfunctotaskstream(task, MSK_STREAM_LOG, NULL, printstr);
//---------------------------------------
//start inputing the problem
//prespecify number of variables to make inputting faster
r = MSK_putmaxnumvar(task, sol.rows());
//number of constraints (both equality and inequality)
if (r == MSK_RES_OK) {
r = MSK_putmaxnumcon(task, Eq.rows() + InEq.rows());
}
//make sure default value is 0 for sparse matrices
assert(Q.getDefault() == 0.0);
assert(Eq.getDefault() == 0.0);
assert(InEq.getDefault() == 0.0);
//number of non-zero entries in A
if (r == MSK_RES_OK) {
r = MSK_putmaxnumanz(task, Eq.numElements() + InEq.numElements());
}
if (r != MSK_RES_OK) {
DBGA("Failed to input variables");
MSK_deletetask(&task);
return -1;
}
//solver is sensitive to numerical problems. Scale the problem down
//we will use this value to scale down the right hand side of equality
//and inequality constraints and lower and upper bounds
//after solving, we must scale back up the solution and the value of the
//objective
double scale = b.absMax();
if (scale < 1.0e2) {
scale = 1.0;
} else {
DBGP("Mosek solver: scaling problem down by " << scale);
}
//---------------------------------------
//insert the actual variables and constraints
//append the variables
MSK_append(task, MSK_ACC_VAR, sol.rows());
//append the constraints.
MSK_append(task, MSK_ACC_CON, Eq.rows() + InEq.rows());
int i, j;
double value;
if (objType == MOSEK_OBJ_QP) {
//quadratic optimization objective
//the quadratic term
Q.sequentialReset();
while (Q.nextSequentialElement(i, j, value)) {
MSK_putqobjij(task, i, j, 2.0 * value);
}
} else if (objType == MOSEK_OBJ_LP) {
//linear objective
for (j = 0; j < Q.cols(); j++) {
if (fabs(Q.elem(0, j)) > 1.0e-5) {
MSK_putcj(task, j, Q.elem(0, j));
}
}
} else {
assert(0);
}
//variable bounds
assert(sol.rows() == lowerBounds.rows());
assert(sol.rows() == upperBounds.rows());
for (i = 0; i < sol.rows(); i++) {
if (lowerBounds.elem(i, 0) >= upperBounds.elem(i, 0)) {
if (lowerBounds.elem(i, 0) > upperBounds.elem(i, 0)) {
assert(0);
}
if (lowerBounds.elem(i, 0) == -std::numeric_limits<double>::max()) {
assert(0);
}
if (upperBounds.elem(i, 0) == std::numeric_limits<double>::max()) {
assert(0);
}
//fixed variable
DBGP(i << ": fixed " << lowerBounds.elem(i, 0) / scale);
MSK_putbound(task, MSK_ACC_VAR, i, MSK_BK_FX,
lowerBounds.elem(i, 0) / scale, upperBounds.elem(i, 0) / scale);
} else if (lowerBounds.elem(i, 0) != -std::numeric_limits<double>::max()) {
//finite lower bound
if (upperBounds.elem(i, 0) != std::numeric_limits<double>::max()) {
//two finite bounds
DBGP(i << ": finite bounds " << lowerBounds.elem(i, 0) / scale
<< " " << upperBounds.elem(i, 0) / scale);
MSK_putbound(task, MSK_ACC_VAR, i, MSK_BK_RA,
lowerBounds.elem(i, 0) / scale, upperBounds.elem(i, 0) / scale);
} else {
//lower bound
DBGP(i << ": lower bound " << lowerBounds.elem(i, 0) / scale);
MSK_putbound(task, MSK_ACC_VAR, i, MSK_BK_LO,
lowerBounds.elem(i, 0) / scale, +MSK_INFINITY);
}
} else {
//infinite lower bound
if (upperBounds.elem(i, 0) != std::numeric_limits<double>::max()) {
//upper bound
DBGP(i << ": upper bound " << upperBounds.elem(i, 0) / scale);
MSK_putbound(task, MSK_ACC_VAR, i, MSK_BK_UP,
-MSK_INFINITY, upperBounds.elem(i, 0) / scale);
} else {
//unbounded
DBGP(i << ": unbounded");
MSK_putbound(task, MSK_ACC_VAR, i, MSK_BK_FR,
-MSK_INFINITY, +MSK_INFINITY);
}
}
}
//constraints and constraint bounds
//equality constraints
Eq.sequentialReset();
while (Eq.nextSequentialElement(i, j, value)) {
MSK_putaij(task, i, j, value);
}
for (i = 0; i < Eq.rows(); i++) {
MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_FX, b.elem(i, 0) / scale, b.elem(i, 0) / scale);
}
//inequality constraints, <=
InEq.sequentialReset();
while (InEq.nextSequentialElement(i, j, value)) {
int eqi = i + Eq.rows();
MSK_putaij(task, eqi, j, value);
}
for (i = 0; i < InEq.rows(); i++) {
int eqi = i + Eq.rows();
MSK_putbound(task, MSK_ACC_CON, eqi, MSK_BK_UP, -MSK_INFINITY, ib.elem(i, 0) / scale);
}
//specify objective: minimize
MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
//give it 800 iterations, twice the default.
MSK_putintparam(task, MSK_IPAR_INTPNT_MAX_ITERATIONS, 800);
//----------------------------------
//solve the thing
DBGP("Optimization started");
r = MSK_optimize(task);
DBGP("Optimization returns");
//write problem to file
/*
static int fileNum = 0;
if (r != MSK_RES_OK) {
char filename[50];
sprintf(filename,"mosek_error_%d_%d.opf",fileNum++, r);
MSK_writedata(task, filename);
FILE *fp = fopen(filename,"a");
fprintf(fp,"\n\nEquality matrix:\n");
Eq.print(fp);
fclose(fp);
}
*/
if (r != MSK_RES_OK) {
DBGA("Mosek optimization call failed, error code " << r);
MSK_deletetask(&task);
return -1;
}
DBGP("Optimization complete");
//debug code, find out number of iterations used
//int iter;
//MSK_getintinf(task, MSK_IINF_INTPNT_ITER, &iter);
//DBGA("Iterations used: " << iter);
//find out what kind of solution we have
MSKprostae pst;
MSKsolstae sst;
MSK_getsolutionstatus(task, MSK_SOL_ITR, &pst, &sst);
int result;
if (sst == MSK_SOL_STA_OPTIMAL || sst == MSK_SOL_STA_NEAR_OPTIMAL) {
//success, we have an optimal problem
if (sst == MSK_SOL_STA_OPTIMAL) {DBGP("QP solution is optimal");}
else {DBGA("QP solution is *nearly* optimal");}
result = 0;
} else if (sst == MSK_SOL_STA_PRIM_INFEAS_CER) {
//unfeasible problem
DBGP("Mosek optimization: primal infeasible");
result = 1;
} else if (sst == MSK_SOL_STA_DUAL_INFEAS_CER) {
//unfeasible problem
DBGA("Mosek optimization: dual infeasible (primal unbounded?)");
result = 1;
} else if (sst == MSK_SOL_STA_PRIM_AND_DUAL_FEAS) {
//i think this means feasible problem, but unbounded solution
//this shouldn't happen as our Q is positive semidefinite
DBGA("QP solution is prim and dual feasible, but not optimal");
DBGA("Is Q positive semidefinite?");
result = -1;
} else {
//unknown return status
DBGA("QP fails with solution status " << sst << " and problem status " << pst);
result = -1;
}
//MSK_SOL_STA_DUAL_FEAS;
//retrieve the solutions
if (!result) {
//get the value of the objective function
MSKrealt obj, foo;
MSK_getsolutioninf(task, MSK_SOL_ITR, &pst, &sst, &obj,
&foo, &foo, &foo, &foo, &foo, &foo, &foo, &foo);
if (objType == MOSEK_OBJ_QP) {
*objVal = obj * scale * scale;
} else if (objType == MOSEK_OBJ_LP) {
*objVal = obj * scale;
} else {
assert(0);
}
double *xx = new double[sol.rows()];
MSK_getsolutionslice(task, MSK_SOL_ITR, MSK_SOL_ITEM_XX,
0, sol.rows(), xx);
for (i = 0; i < sol.rows(); i++) {
sol.elem(i, 0) = scale * xx[i];
DBGP("x" << i << ": " << xx[i]);
}
delete [] xx;
}
MSK_deletetask(&task);
return result;
}
