void MyQCQP::optimize()
{
// resize alpha
if(alpha != NULL)
delete []alpha;
alpha = new double[numvar];
double *c_ = new double[numvar];
for(int i = 0;i<numvar;i++) c_[i] = c[i];
MSKboundkeye *bkc_ = new MSKboundkeye[numcon];
double * blc_ = new double[numcon];
double * buc_ = new double[numcon];
for(int i = 0;i<numcon;i++)
{
bkc_[i] = bkc[i];
blc_[i] = blc[i];
buc_[i] = buc[i];
}
MSKboundkeye *bkx_ = new MSKboundkeye[numvar];
double * blx_ = new double[numvar];
double * bux_ = new double[numvar];
for(int i = 0;i<numvar;i++)
{
bkx_[i] = bkx[i];
blx_[i] = blx[i];
bux_[i] = bux[i];
}
MSKlidxt *aptrb_ = new MSKlidxt[aptrb.size()];
for(size_t i = 0;i<aptrb.size();i++) aptrb_[i] = aptrb[i];
MSKlidxt * aptre_ = new MSKlidxt[aptre.size()];
for(size_t i = 0;i<aptre.size();i++) aptre_[i] = aptre[i];
MSKidxt * asub_ = new MSKidxt[asub.size()];
for(size_t i = 0;i<asub.size();i++) asub_[i] = asub[i];
double *aval_ = new double[aval.size()];
for(size_t i = 0;i<aval.size();i++) aval_[i] = aval[i];
MSKrescodee r;
MSKenv_t env;
MSKtask_t task;
r = MSK_makeenv(&env,NULL,NULL,NULL,NULL);
r = MSK_initenv(env);
if(r == MSK_RES_OK)
{
r = MSK_maketask(env,numcon,numvar,&task);
if(r == MSK_RES_OK)
r = MSK_append(task,MSK_ACC_CON,numcon);
if(r == MSK_RES_OK)
r = MSK_append(task,MSK_ACC_VAR, numvar);
for(int j = 0;j<numvar && r== MSK_RES_OK;j++)
{
if(r == MSK_RES_OK)
r = MSK_putcj(task,j,c_[j]);
if(r == MSK_RES_OK)
r = MSK_putbound(task,MSK_ACC_VAR,j,bkx_[j],blx_[j],bux_[j]);
if(r == MSK_RES_OK)
r = MSK_putavec(task,MSK_ACC_VAR,j,aptre_[j] - aptrb_[j], asub_ + aptrb_[j],aval_+aptrb_[j]);
}
for(int i=0;i<numcon && r== MSK_RES_OK;i++)
{
r = MSK_putbound(task,MSK_ACC_CON,i,bkc_[i],blc_[i],buc_[i]);
}
delete []c_;
delete []bkx_;
delete []blx_;
delete []bux_;
delete []aptrb_;
delete []aptre_;
delete []asub_;
delete []aval_;
delete []bkc_;
delete []blc_;
delete []buc_;
for(int i=0;i<numcon-1 && r== MSK_RES_OK;i++) // numcon-1 quadratic constraints
{
int nzero = qsubi[i].size();
MSKidxt * qsubi_ = new MSKidxt[nzero];
MSKidxt * qsubj_ = new MSKidxt[nzero];
double * qval_ = new double[nzero];
for(int m = 0;m<nzero;m++)
{
qsubi_[m] = qsubi[i][m];
qsubj_[m] = qsubj[i][m];
qval_[m] = qval[i][m];
}
if(r == MSK_RES_OK)
r = MSK_putqconk(task,i,nzero,qsubi_,qsubj_,qval_);
delete []qsubi_;
delete []qsubj_;
delete []qval_;
}
if(r == MSK_RES_OK)
r = MSK_putobjsense(task,MSK_OBJECTIVE_SENSE_MINIMIZE);
if(r == MSK_RES_OK)
{
MSKrescodee trmcode;
r = MSK_optimizetrm(task,&trmcode);
MSK_getsolutionslice(task,MSK_SOL_ITR, MSK_SOL_ITEM_XX,0,numvar,alpha);
MSK_getsolutionslice(task,MSK_SOL_ITR,MSK_SOL_ITEM_SUC,0,numcon,mu);
}
MSK_deletetask(&task);
}
MSK_deleteenv(&env);
}
template <typename _Scalar> typename MosekOpt<_Scalar>::ReturnType
MosekOpt<_Scalar>::
update( bool verbose )
{
if ( _task != NULL )
{
std::cerr << "[" << __func__ << "]: " << "update can only be called once! returning." << std::endl;
return MSK_RES_ERR_UNKNOWN;
}
/* Create the optimization task. */
if ( MSK_RES_OK == _r )
{
_r = MSK_maketask( _env, this->getConstraintCount(), this->getVarCount(), &_task );
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "could not create task with " << this->getVarCount() << " vars, and " << this->getConstraintCount() << " constraints" << std::endl;
}
// redirect output
if ( MSK_RES_OK == _r )
{
_r = MSK_linkfunctotaskstream( _task, MSK_STREAM_LOG, NULL, mosekPrintStr );
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "could not create rewire output to mosekPrintStr(), continuing though..." << std::endl;
}
// Append _numCon empty constraints. The constraints will initially have no bounds.
if ( MSK_RES_OK == _r )
{
if ( verbose ) std::cout << "my: MSK_appendcons(_task,"<< this->getConstraintCount() <<");" << std::endl;
_r = MSK_appendcons( _task, this->getConstraintCount() );
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "could not append " << this->getConstraintCount() << " constraints" << std::endl;
}
// Append _numVar variables. The variables will initially be fixed at zero (x=0).
if ( MSK_RES_OK == _r )
{
if ( verbose ) std::cout << "my: MSK_appendvars(_task," << this->getVarCount() <<");" << std::endl;
_r = MSK_appendvars( _task, this->getVarCount() );
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "could not append " << this->getVarCount() << " variables" << std::endl;
}
// Optionally add a constant term to the objective.
if ( MSK_RES_OK == _r )
{
if ( verbose ) std::cout << "my: MSK_putcfix(_task," << this->getObjectiveBias() << ");" << std::endl;
_r = MSK_putcfix( _task, this->getObjectiveBias() );
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "could not add constant " << this->getObjectiveBias() << " to objective function" << std::endl;
}
// set Variables
for ( size_t j = 0; (j < this->getVarCount()) && (MSK_RES_OK == _r); ++j )
{
// set Variable j's Bounds // blx[j] <= x_j <= bux[j]
if ( MSK_RES_OK == _r )
{
_r = MSK_putvarbound( _task,
j, /* Index of variable.*/
MosekOpt<Scalar>::getBoundTypeCustom( this->getVarBoundType(j) ), /* Bound key.*/
this->getVarLowerBound(j), /* Numerical value of lower bound.*/
this->getVarUpperBound(j) ); /* Numerical value of upper bound.*/
if ( verbose ) std::cout << "my: MSK_putvarbound(_task," << j << "," << this->getVarBoundType(j) << "," << this->getVarLowerBound(j) << "," << this->getVarUpperBound(j) << ");" << std::endl;
}
// set Variable j's Type
if ( MSK_RES_OK == _r )
{
_r = MSK_putvartype( _task, j, MosekOpt<Scalar>::getVarTypeCustom(this->getVarType(j)) );
}
// set Variable j's linear coefficient in the objective function
if ( MSK_RES_OK == _r )
{
if ( verbose ) std::cout << "my: putcj(_task," << j << "," << this->getLinObjectives()[j] << ")" << std::endl;
_r = MSK_putcj( _task, j, this->getLinObjectives()[j] );
}
}
// set Quadratic Objectives
if ( MSK_RES_OK == _r )
{
const int numNonZeros = this->getQuadraticObjectives().size();
MSKint32t *qsubi = new MSKint32t[numNonZeros],
*qsubj = new MSKint32t[numNonZeros];
double *qval = new double[numNonZeros];
for ( size_t qi = 0; qi != this->getQuadraticObjectives().size(); ++qi )
{
qsubi[qi] = this->getQuadraticObjectives()[qi].row();
qsubj[qi] = this->getQuadraticObjectives()[qi].col();
qval [qi] = this->getQuadraticObjectives()[qi].value();
}
if ( verbose ) std::cout<<"my: putqobj( _task, " << numNonZeros << ",\n";
for ( size_t vi = 0; vi != numNonZeros; ++vi )
{
if ( verbose ) std::cout << qsubi[vi] << "," << qsubj[vi] << ", " << qval[vi] << std::endl;
}
if ( verbose ) std::cout << ");" << std::endl;
_r = MSK_putqobj( _task, numNonZeros, qsubi, qsubj, qval );
if ( qsubi ) { delete[] qsubi; qsubi = NULL; }
if ( qsubj ) { delete[] qsubj; qsubj = NULL; }
if ( qval ) { delete[] qval ; qval = NULL; }
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "Setting Quadratic Objectives caused error code " << (int)_r << std::endl;
} // ...Quadratic objective
// set Linear Constraints
{
typename ParentType::SparseMatrix A( this->getLinConstraintsMatrix() );
// ( this->getConstraintCount(), this->getVarCount() );
// A.setFromTriplets( this->getLinConstraints().begin(), this->getLinConstraints().end() );
std::vector<double> aval; // Linear constraints coeff matrix (sparse)
std::vector<int> asub; // Linear constraints coeff matrix indices
std::vector<int> aptrb, aptre;
for ( int row = 0; (row < A.outerSize()) && (MSK_RES_OK == _r); ++row )
{
// set Constraint Bounds for row
if ( MSK_RES_OK == _r )
{
if ( verbose ) std::cout << "my: MSK_putconbound( _task, " << row << ", "
<< MosekOpt<Scalar>::getBoundTypeCustom( this->getConstraintBoundType(row) ) << ", "
<< this->getConstraintLowerBound( row ) << ", "
<< this->getConstraintUpperBound( row ) << ")"
<< std::endl; fflush( stdout );
_r = MSK_putconbound( _task,
row, /* Index of constraint.*/
MosekOpt<Scalar>::getBoundTypeCustom(this->getConstraintBoundType(row)), /* Bound key.*/
this->getConstraintLowerBound(row), /* Numerical value of lower bound.*/
this->getConstraintUpperBound(row) ); /* Numerical value of upper bound.*/
}
// set Linear Constraint row
if ( MSK_RES_OK == _r )
{
// new line starts at index == current size
aptrb.push_back( aval.size() );
// add coeffs from new line
for ( typename ParentType::SparseMatrix::InnerIterator it(A,row); it; ++it )
{
if ( row != it.row() ) std::cerr << "[" << __func__ << "]: " << "this shouldn't happen" << std::endl;
// coeff value
aval.push_back( it.value() ); // TODO: A should be a matrix, not a vector...
// coeff subscript
asub.push_back( it.col() );
}
// new line ends at index == new size
aptre.push_back( aval.size() );
if ( verbose ) {
std::cout << "my: MSK_putarow( _task, "
<< row << ", "
<< aptre[row] - aptrb[row] << ", "
<< *(asub.data() + aptrb[row]) << ", "
<< *(aval.data() + aptrb[row]) << ");"
<< std::endl; fflush( stdout );
}
_r = MSK_putarow( _task,
row, /* Row index.*/
aptre[row] - aptrb[row], /* Number of non-zeros in row i.*/
asub.data() + aptrb[row], /* Pointer to column indexes of row i.*/
aval.data() + aptrb[row]); /* Pointer to values of row i.*/
}
} // ... for A.rows
// report error
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "Setting Lin constraints caused error code " << (int)_r << std::endl;
} // ...set Linear Constraints
// set Quadratic constraints
if ( verbose ) std::cout << "[" << __func__ << "]: " << "adding q constraints" << std::endl;
for ( size_t constr_id = 0; (constr_id != this->getQuadraticConstraints().size()) && (MSK_RES_OK == _r); ++constr_id )
{
const int numNonZeros = this->getQuadraticConstraints(constr_id).size();
MSKint32t *qsubi = new MSKint32t[numNonZeros],
*qsubj = new MSKint32t[numNonZeros];
double *qval = new double[numNonZeros];
for ( size_t qi = 0; qi != this->getQuadraticConstraints(constr_id).size(); ++qi )
{
qsubi[qi] = this->getQuadraticConstraints(constr_id)[qi].row();
qsubj[qi] = this->getQuadraticConstraints(constr_id)[qi].col();
qval [qi] = this->getQuadraticConstraints(constr_id)[qi].value();
}
if ( verbose ) std::cout<<"my: MSK_putqonk( _task, " << constr_id << ", " << numNonZeros << ",\n";
for(size_t vi=0;vi!=numNonZeros;++vi)
{
if ( verbose ) std::cout << qsubi[vi] << "," << qsubj[vi] << ", " << qval[vi] << std::endl;
}
if ( verbose ) std::cout << "); " << std::endl;
_r = MSK_putqconk(_task,
constr_id,
numNonZeros,
qsubi,
qsubj,
qval);
if ( qsubi ) { delete[] qsubi; qsubi = NULL; }
if ( qsubj ) { delete[] qsubj; qsubj = NULL; }
if ( qval ) { delete[] qval ; qval = NULL; }
if ( MSK_RES_OK != _r )
std::cerr << "[" << __func__ << "]: " << "Setting Quad constraints caused error code " << (int)_r << std::endl;
} // ...set Quadratic Constraints
// save to file
{
if ( _r == MSK_RES_OK )
{
_r = MSK_putintparam( _task, MSK_IPAR_WRITE_DATA_FORMAT, MSK_DATA_FORMAT_LP );
if ( _r == MSK_RES_OK )
{
_r = MSK_writedata( _task, "mosek.lp" );
if ( _r != MSK_RES_OK )
{
std::cerr << "[" << __func__ << "]: " << "Writedata did not work" << (int)_r << std::endl;
}
}
}
}
if ( _r == MSK_RES_OK )
{
this->_x.setZero();
this->_updated = true;
}
// return error code
return _r;
} // ...MosekOpt::update()
