int main(int argc,char *argv[])
{
const MSKint32t numvar = 3,
numcon = 3;
MSKint32t i,j;
double c[] = {1.5, 2.5, 3.0};
MSKint32t ptrb[] = {0, 3, 6},
ptre[] = {3, 6, 9},
asub[] = { 0, 1, 2,
0, 1, 2,
0, 1, 2};
double aval[] = { 2.0, 3.0, 2.0,
4.0, 2.0, 3.0,
3.0, 3.0, 2.0};
MSKboundkeye bkc[] = {MSK_BK_UP, MSK_BK_UP, MSK_BK_UP };
double blc[] = {-MSK_INFINITY, -MSK_INFINITY, -MSK_INFINITY};
double buc[] = {100000, 50000, 60000};
MSKboundkeye bkx[] = {MSK_BK_LO, MSK_BK_LO, MSK_BK_LO};
double blx[] = {0.0, 0.0, 0.0,};
double bux[] = {+MSK_INFINITY, +MSK_INFINITY,+MSK_INFINITY};
double *xx=NULL;
MSKenv_t env;
MSKtask_t task;
MSKint32t varidx,conidx;
MSKrescodee r;
/* Create the mosek environment. */
r = MSK_makeenv(&env,NULL);
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. */
MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
/* Append the constraints. */
if (r == MSK_RES_OK)
r = MSK_appendcons(task,numcon);
/* Append the variables. */
if (r == MSK_RES_OK)
r = MSK_appendvars(task,numvar);
/* Put C. */
if (r == MSK_RES_OK)
r = MSK_putcfix(task, 0.0);
if (r == MSK_RES_OK)
for(j=0; j<numvar; ++j)
r = MSK_putcj(task,j,c[j]);
/* Put constraint bounds. */
if (r == MSK_RES_OK)
for(i=0; i<numcon; ++i)
r = MSK_putconbound(task,i,bkc[i],blc[i],buc[i]);
/* Put variable bounds. */
if (r == MSK_RES_OK)
for(j=0; j<numvar; ++j)
r = MSK_putvarbound(task,j,bkx[j],blx[j],bux[j]);
/* Put A. */
if (r == MSK_RES_OK)
if ( numcon>0 )
for(j=0; j<numvar; ++j)
r = MSK_putacol(task,
j,
ptre[j]-ptrb[j],
asub+ptrb[j],
aval+ptrb[j]);
if (r == MSK_RES_OK)
r = MSK_putobjsense(task,
MSK_OBJECTIVE_SENSE_MAXIMIZE);
if (r == MSK_RES_OK)
r = MSK_optimizetrm(task,NULL);
if (r == MSK_RES_OK)
{
xx = calloc(numvar,sizeof(double));
if ( !xx )
r = MSK_RES_ERR_SPACE;
}
if (r == MSK_RES_OK)
r = MSK_getxx(task,
MSK_SOL_BAS, /* Basic solution. */
xx);
/* Make a change to the A matrix */
if (r == MSK_RES_OK)
r = MSK_putaij(task, 0, 0, 3.0);
if (r == MSK_RES_OK)
r = MSK_optimizetrm(task,NULL);
/* Get index of new variable, this should be 3 */
if (r == MSK_RES_OK)
r = MSK_getnumvar(task,&varidx);
/* Append a new variable x_3 to the problem */
if (r == MSK_RES_OK)
r = MSK_appendvars(task,1);
/* Set bounds on new variable */
if (r == MSK_RES_OK)
r = MSK_putvarbound(task,
varidx,
MSK_BK_LO,
0,
+MSK_INFINITY);
/* Change objective */
if (r == MSK_RES_OK)
r = MSK_putcj(task,varidx,1.0);
/* Put new values in the A matrix */
if (r == MSK_RES_OK)
{
MSKint32t acolsub[] = {0, 2};
double acolval[] = {4.0, 1.0};
r = MSK_putacol(task,
varidx, /* column index */
2, /* num nz in column*/
acolsub,
acolval);
}
/* Change optimizer to free simplex and reoptimize */
if (r == MSK_RES_OK)
r = MSK_putintparam(task,MSK_IPAR_OPTIMIZER,MSK_OPTIMIZER_FREE_SIMPLEX);
if (r == MSK_RES_OK)
r = MSK_optimizetrm(task,NULL);
/* Get index of new constraint*/
if (r == MSK_RES_OK)
r = MSK_getnumcon(task,&conidx);
/* Append a new constraint */
if (r == MSK_RES_OK)
r = MSK_appendcons(task,1);
/* Set bounds on new constraint */
if (r == MSK_RES_OK)
r = MSK_putconbound(task,
conidx,
MSK_BK_UP,
-MSK_INFINITY,
30000);
/* Put new values in the A matrix */
if (r == MSK_RES_OK)
{
MSKidxt arowsub[] = {0, 1, 2, 3 };
double arowval[] = {1.0, 2.0, 1.0, 1.0};
r = MSK_putarow(task,
conidx, /* row index */
4, /* num nz in row*/
arowsub,
arowval);
}
if (r == MSK_RES_OK)
r = MSK_optimizetrm(task,NULL);
if ( xx )
free(xx);
MSK_deletetask(&task);
}
MSK_deleteenv(&env);
printf("Return code: %d (0 means no error occured.)\n",r);
return ( r );
} /* main */
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;
}
/**********************
lap: the upper RHS of the symmetric graph laplacian matrix which will be transformed
to the hessian of the non-linear part of the optimisation function
n: number of nodes (length of coords array)
ordering: array containing sequences of nodes for each level,
ie, ordering[levels[i]] is first node of (i+1)th level
level_indexes: array of starting node for each level in ordering
ie, levels[i] is index to first node of (i+1)th level
also, levels[0] is number of nodes in first level
and, levels[i]-levels[i-1] is number of nodes in ith level
and, n - levels[num_divisions-1] is number of nodes in last level
num_divisions: number of divisions between levels, ie number of levels - 1
separation: the minimum separation between nodes on different levels
***********************/
MosekEnv *mosek_init_hier(float *lap, int n, int *ordering,
int *level_indexes, int num_divisions,
float separation)
{
int count = 0;
int i, j, num_levels = num_divisions + 1;
int num_constraints;
MosekEnv *mskEnv = GNEW(MosekEnv);
DigColaLevel *levels;
int nonzero_lapsize = (n * (n - 1)) / 2;
/* vars for nodes (except x0) + dummy nodes between levels
* x0 is fixed at 0, and therefore is not included in the opt problem
* add 2 more vars for top and bottom constraints
*/
mskEnv->num_variables = n + num_divisions + 1;
logfile = fopen("quad_solve_log", "w");
levels = assign_digcola_levels(ordering, n, level_indexes, num_divisions);
#ifdef DUMP_CONSTRAINTS
print_digcola_levels(logfile, levels, num_levels);
#endif
/* nonlinear coefficients matrix of objective function */
/* int lapsize=mskEnv->num_variables+(mskEnv->num_variables*(mskEnv->num_variables-1))/2; */
mskEnv->qval = N_GNEW(nonzero_lapsize, double);
mskEnv->qsubi = N_GNEW(nonzero_lapsize, int);
mskEnv->qsubj = N_GNEW(nonzero_lapsize, int);
/* solution vector */
mskEnv->xx = N_GNEW(mskEnv->num_variables, double);
/* constraint matrix */
separation /= 2.0; /* separation between each node and it's adjacent constraint */
num_constraints = get_num_digcola_constraints(levels,
num_levels) + num_divisions + 1;
/* constraints of the form x_i - x_j >= sep so 2 non-zero entries per constraint in LHS matrix
* except x_0 (fixed at 0) constraints which have 1 nz val each.
*/
#ifdef EQUAL_WIDTH_LEVELS
num_constraints += num_divisions;
#endif
/* pointer to beginning of nonzero sequence in a column */
for (i = 0; i < n - 1; i++) {
for (j = i; j < n - 1; j++) {
mskEnv->qval[count] = -2 * lap[count + n];
assert(mskEnv->qval[count] != 0);
mskEnv->qsubi[count] = j;
mskEnv->qsubj[count] = i;
count++;
}
}
#ifdef DUMP_CONSTRAINTS
fprintf(logfile, "Q=[");
int lapcntr = n;
for (i = 0; i < mskEnv->num_variables; i++) {
if (i != 0)
fprintf(logfile, ";");
for (j = 0; j < mskEnv->num_variables; j++) {
if (j < i || i >= n - 1 || j >= n - 1) {
fprintf(logfile, "0 ");
} else {
fprintf(logfile, "%f ", -2 * lap[lapcntr++]);
}
}
}
fprintf(logfile, "]\nQ=Q-diag(diag(Q))+Q'\n");
#endif
fprintf(logfile, "\n");
/* Make the mosek environment. */
mskEnv->r = MSK_makeenv(&mskEnv->env, NULL, NULL, NULL, NULL);
/* Check whether the return code is ok. */
if (mskEnv->r == MSK_RES_OK) {
/* Directs the log stream to the user
* specified procedure 'printstr'.
*/
MSK_linkfunctoenvstream(mskEnv->env, MSK_STREAM_LOG, NULL,
printstr);
}
/* Initialize the environment. */
mskEnv->r = MSK_initenv(mskEnv->env);
if (mskEnv->r == MSK_RES_OK) {
/* Make the optimization task. */
mskEnv->r =
MSK_maketask(mskEnv->env, num_constraints,
mskEnv->num_variables, &mskEnv->task);
if (mskEnv->r == MSK_RES_OK) {
int c_ind = 0;
int c_var = n - 1;
mskEnv->r =
MSK_linkfunctotaskstream(mskEnv->task, MSK_STREAM_LOG,
NULL, printstr);
/* Resize the task. */
if (mskEnv->r == MSK_RES_OK)
mskEnv->r = MSK_resizetask(mskEnv->task, num_constraints, mskEnv->num_variables, 0, /* no cones!! */
/* each constraint applies to 2 vars */
2 * num_constraints +
num_divisions, nonzero_lapsize);
/* Append the constraints. */
if (mskEnv->r == MSK_RES_OK)
mskEnv->r = MSK_append(mskEnv->task, 1, num_constraints);
/* Append the variables. */
if (mskEnv->r == MSK_RES_OK)
mskEnv->r =
MSK_append(mskEnv->task, 0, mskEnv->num_variables);
/* Put variable bounds. */
for (j = 0;
j < mskEnv->num_variables && mskEnv->r == MSK_RES_OK; ++j)
mskEnv->r =
MSK_putbound(mskEnv->task, 0, j, MSK_BK_RA,
-MSK_INFINITY, MSK_INFINITY);
for (j = 0; j < levels[0].num_nodes && mskEnv->r == MSK_RES_OK;
j++) {
int node = levels[0].nodes[j] - 1;
if (node >= 0) {
INIT_sub_val(c_var,node);
mskEnv->r =
MSK_putavec(mskEnv->task, 1, c_ind, 2, subi, vali);
} else {
/* constraint for y0 (fixed at 0) */
mskEnv->r =
MSK_putaij(mskEnv->task, c_ind, c_var, 1.0);
}
mskEnv->r =
MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
separation, MSK_INFINITY);
c_ind++;
}
for (i = 0; i < num_divisions && mskEnv->r == MSK_RES_OK; i++) {
c_var = n + i;
for (j = 0;
j < levels[i].num_nodes && mskEnv->r == MSK_RES_OK;
j++) {
/* create separation constraint a>=b+separation */
int node = levels[i].nodes[j] - 1;
if (node >= 0) { /* no constraint for fixed node */
INIT_sub_val(node,c_var);
mskEnv->r =
MSK_putavec(mskEnv->task, 1, c_ind, 2, subi,
vali);
} else {
/* constraint for y0 (fixed at 0) */
mskEnv->r =
MSK_putaij(mskEnv->task, c_ind, c_var, -1.0);
}
mskEnv->r =
MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
separation, MSK_INFINITY);
c_ind++;
}
for (j = 0;
j < levels[i + 1].num_nodes
&& mskEnv->r == MSK_RES_OK; j++) {
int node = levels[i + 1].nodes[j] - 1;
if (node >= 0) {
INIT_sub_val(c_var,node);
mskEnv->r =
MSK_putavec(mskEnv->task, 1, c_ind, 2, subi,
vali);
} else {
/* constraint for y0 (fixed at 0) */
mskEnv->r =
MSK_putaij(mskEnv->task, c_ind, c_var, 1.0);
}
mskEnv->r =
MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
separation, MSK_INFINITY);
c_ind++;
}
}
c_var = n + i;
for (j = 0; j < levels[i].num_nodes && mskEnv->r == MSK_RES_OK;
j++) {
/* create separation constraint a>=b+separation */
int node = levels[i].nodes[j] - 1;
if (node >= 0) { /* no constraint for fixed node */
INIT_sub_val(node,c_var);
mskEnv->r =
MSK_putavec(mskEnv->task, 1, c_ind, 2, subi, vali);
} else {
/* constraint for y0 (fixed at 0) */
mskEnv->r =
MSK_putaij(mskEnv->task, c_ind, c_var, -1.0);
}
mskEnv->r =
MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
separation, MSK_INFINITY);
c_ind++;
}
/* create constraints preserving the order of dummy vars */
for (i = 0; i < num_divisions + 1 && mskEnv->r == MSK_RES_OK;
i++) {
int c_var = n - 1 + i, c_var2 = c_var + 1;
INIT_sub_val(c_var,c_var2);
mskEnv->r =
MSK_putavec(mskEnv->task, 1, c_ind, 2, subi, vali);
mskEnv->r =
MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO, 0,
MSK_INFINITY);
c_ind++;
}
#ifdef EQUAL_WIDTH_LEVELS
for (i = 1; i < num_divisions + 1 && mskEnv->r == MSK_RES_OK;
i++) {
int c_var = n - 1 + i, c_var_lo = c_var - 1, c_var_hi =
c_var + 1;
INIT_sub_val3(c_var_lo, c_var, c_var_h);
mskEnv->r =
MSK_putavec(mskEnv->task, 1, c_ind, 3, subi, vali);
mskEnv->r =
MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_FX, 0, 0);
c_ind++;
}
#endif
assert(c_ind == num_constraints);
#ifdef DUMP_CONSTRAINTS
fprintf(logfile, "A=[");
for (i = 0; i < num_constraints; i++) {
if (i != 0)
fprintf(logfile, ";");
for (j = 0; j < mskEnv->num_variables; j++) {
double aij;
MSK_getaij(mskEnv->task, i, j, &aij);
fprintf(logfile, "%f ", aij);
}
}
fprintf(logfile, "]\n");
fprintf(logfile, "b=[");
for (i = 0; i < num_constraints; i++) {
fprintf(logfile, "%f ", separation);
}
fprintf(logfile, "]\n");
#endif
if (mskEnv->r == MSK_RES_OK) {
/*
* The lower triangular part of the Q
* matrix in the objective is specified.
*/
mskEnv->r =
MSK_putqobj(mskEnv->task, nonzero_lapsize,
mskEnv->qsubi, mskEnv->qsubj,
mskEnv->qval);
}
}
}
delete_digcola_levels(levels, num_levels);
return mskEnv;
}
