void print_ecos_xequil(ecos_bb_pwork* prob){
#if EQUILIBRATE > 0
int i; PRINTTEXT("ecos->xequil: ");
for (i=0; i<prob->ecos_prob->n; ++i) PRINTTEXT("%.2f ", prob->ecos_prob->xequil[i] );
#else
PRINTTEXT("ecos->xequil: 1, (equilibration dissabled) ");
#endif
PRINTTEXT("\n");
}
idxint ECOS_BB_solve(ecos_bb_pwork* prob) {
idxint curr_node_idx = 0;
#if MI_PRINTLEVEL > 0
if (prob->stgs->verbose){
PRINTTEXT("Iter\tLower Bound\tUpper Bound\tGap\n");
PRINTTEXT("================================================\n");
}
#endif
/* Initialize to root node and execute steps 1 on slide 6 */
/* of http://stanford.edu/class/ee364b/lectures/bb_slides.pdf*/
prob->iter = 0;
initialize_root(prob);
/*print_node(prob, curr_node_idx);*/
get_bounds(curr_node_idx, prob);
prob->global_L = prob->nodes[curr_node_idx].L;
prob->global_U = prob->nodes[curr_node_idx].U;
while ( should_continue(prob, curr_node_idx) ){
#if MI_PRINTLEVEL > 0
if (prob->stgs->verbose){ print_progress(prob); }
#endif
++(prob->iter);
/* Step 2*/
/* Branch replaces nodes[curr_node_idx] with leftNode*/
/* and nodes[prob->iter] with rightNode */
branch(curr_node_idx, prob);
/* Step 3*/
get_bounds(curr_node_idx, prob);
get_bounds(prob->iter, prob);
/* Step 4*/
prob->global_L = get_global_L(prob);
curr_node_idx = get_next_node(prob);
}
load_solution(prob);
#if MI_PRINTLEVEL > 0
if (prob->stgs->verbose){ print_progress(prob); }
#endif
return get_ret_code(prob);
}
/*
* Updates the solver's lb and ub contraints for integer variables
* to the bounds specified by the node
*/
void set_prob(ecos_bb_pwork* prob, char* bool_node_id, pfloat* int_node_id){
idxint i;
for(i=0; i<prob->num_bool_vars; ++i){
switch(bool_node_id[i]){
case MI_ONE:
ecos_updateDataEntry_h(prob->ecos_prob, 2*i, -1.0); /*ecos_prob->h[2*i] = -(1.0); // -x <= -1 <=> x >= 1*/
ecos_updateDataEntry_h(prob->ecos_prob, 2*i+1, 1.0);/*ecos_prob->h[2*i + 1] = 1.0;*/
break;
case MI_ZERO:
ecos_updateDataEntry_h(prob->ecos_prob, 2*i, 0.0);/*ecos_prob->h[2*i] = 0.0;*/
ecos_updateDataEntry_h(prob->ecos_prob, 2*i+1, 0.0);/*ecos_prob->h[2*i + 1] = 0.0;*/
break;
case MI_STAR:
ecos_updateDataEntry_h(prob->ecos_prob, 2*i, 0.0);/*ecos_prob->h[2*i] = 0.0;*/
ecos_updateDataEntry_h(prob->ecos_prob, 2*i+1, 1.0);/*ecos_prob->h[2*i + 1] = 1.0;*/
break;
default:
#if MI_PRINTLEVEL > 1
PRINTTEXT("Illegal boolean setting arguments passed: %u \n", bool_node_id[i]);
#endif
break;
}
}
/* Set integer bounds */
for(i=0; i<prob->num_int_vars; ++i){
ecos_updateDataEntry_h(prob->ecos_prob, 2*(i + prob->num_bool_vars) , int_node_id[2*i] );
ecos_updateDataEntry_h(prob->ecos_prob, 2*(i + prob->num_bool_vars)+1 , int_node_id[2*i+1] );
}
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose){ print_ecos_h(prob); }
#endif
}
/**
* Prints a dense integer matrix.
*/
void printDenseMatrix_i(idxint *M, idxint dim1, idxint dim2, char *name)
{
idxint i,j;
PRINTTEXT("%s = \n\t",name);
for( i=0; i<dim1; i++ ){
for( j=0; j<dim2; j++ ){
if( j<dim2-1 )
PRINTTEXT("%d, ",(int)M[i*dim2+j]);
else
PRINTTEXT("%d; ",(int)M[i*dim2+j]);
}
if( i<dim1-1){
PRINTTEXT("\n\t");
}
}
PRINTTEXT("\n");
}
void branch(idxint curr_node_idx, ecos_bb_pwork* prob){
idxint i, split_idx = prob->nodes[curr_node_idx].split_idx;
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose) {
PRINTTEXT("Branching->\t");
print_node(prob, curr_node_idx);
}
#endif
/* Create right node*/
prob->nodes[prob->iter].L = prob->nodes[curr_node_idx].L;
prob->nodes[prob->iter].U = prob->nodes[curr_node_idx].U;
prob->nodes[prob->iter].status = MI_NOT_SOLVED;
/* Copy over the node id*/
for(i=0; i < prob->num_bool_vars; ++i)
get_bool_node_id(prob->iter, prob)[i] = get_bool_node_id(curr_node_idx, prob)[i];
for(i=0; i < prob->num_int_vars*2; ++i)
get_int_node_id(prob->iter, prob)[i] = get_int_node_id(curr_node_idx, prob)[i];
if (split_idx < prob->num_bool_vars){
get_bool_node_id(curr_node_idx, prob)[split_idx] = MI_ZERO;
get_bool_node_id(prob->iter, prob)[split_idx] = MI_ONE;
}else{
split_idx -= prob->num_bool_vars;
/* Left branch constrain UB */
get_int_node_id(curr_node_idx, prob)[split_idx*2 + 1] =
pfloat_floor( prob->nodes[curr_node_idx].split_val, prob->stgs->integer_tol );
/* Right branch constrain LB */
get_int_node_id(prob->iter, prob)[split_idx*2 ] =
-pfloat_ceil( prob->nodes[curr_node_idx].split_val, prob->stgs->integer_tol );
}
prob->nodes[curr_node_idx].status = MI_NOT_SOLVED;
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose) {
PRINTTEXT(" Left-> \t "); print_node(prob, curr_node_idx);
PRINTTEXT(" Right->\t "); print_node(prob, prob->iter);
}
#endif
}
void printProgress(stats* info)
{
if( info->iter == 0 )
{
/* print header at very first iteration */
#if PRINTLEVEL == 2
PRINTTEXT("\nECOS %s - (c) A. Domahidi, ETH Zurich & embotech 2012-14. Support: [email protected]\n\n", ECOS_VERSION);
#endif
#if defined _WIN32 || defined _WIN64
PRINTTEXT("It pcost dcost gap pres dres k/t mu step IR\n");
PRINTTEXT("%2d %+5.3e %+5.3e %+2.0e %2.0e %2.0e %2.0e %2.0e N/A %d %d -\n",(int)info->iter, info->pcost, info->dcost, info->gap, info->pres, info->dres, info->kapovert, info->mu, (int)info->nitref1, (int)info->nitref2);
#else
PRINTTEXT("It pcost dcost gap pres dres k/t mu step IR\n");
PRINTTEXT("%2d %c%+5.3e %c%+5.3e %c%+2.0e %c%2.0e %c%2.0e %c%2.0e %c%2.0e N/A %d %d -\n",(int)info->iter, 32, info->pcost, 32, info->dcost, 32, info->gap, 32, info->pres, 32, info->dres, 32, info->kapovert, 32, info->mu, (int)info->nitref1, (int)info->nitref2);
#endif
} else {
#if defined _WIN32 || defined _WIN64
PRINTTEXT("%2d %+5.3e %+5.3e %+2.0e %2.0e %2.0e %2.0e %2.0e %6.4f %d %d %d\n",(int)info->iter, info->pcost, info->dcost, info->gap, info->pres, info->dres, info->kapovert, info->mu, info->step, (int)info->nitref1, (int)info->nitref2, (int)info->nitref3);
#else
PRINTTEXT("%2d %c%+5.3e%c %+5.3e %c %+2.0e%c %2.0e%c %2.0e%c %2.0e%c %2.0e%c %6.4f %d %d %d\n",(int)info->iter, 32,info->pcost, 32,info->dcost, 32, info->gap, 32, info->pres, 32, info->dres, 32, info->kapovert, 32, info->mu, 32, info->step, (int)info->nitref1, (int)info->nitref2, (int)info->nitref3);
#endif
}
/* enable to flush printf in Matlab immediately */
#ifdef MATLAB_MEX_FILE
#if defined MATLAB_FLUSH_PRINTS
mexEvalString("pause(0.0001);");
#endif
#endif
}
void print_node(ecos_bb_pwork* prob, idxint i){
if (i==-1){
int j; PRINTTEXT("Node info: TMP, Partial id:");
for(j=0; j < prob->num_bool_vars; ++j)
PRINTTEXT("%i ", prob->tmp_bool_node_id[j] );
PRINTTEXT(" | ");
for(j=0; j < prob->num_int_vars; ++j)
PRINTTEXT("(%.2f, %.2f) ", -prob->tmp_int_node_id[2*j], prob->tmp_int_node_id[2*j+1] );
PRINTTEXT("\n");
}else{
int j; PRINTTEXT("Node info: %u : %.2f : %.2f : %u : %.2f Partial id:", prob->nodes[i].status,
prob->nodes[i].L, prob->nodes[i].U, (int)prob->nodes[i].split_idx, prob->nodes[i].split_val);
for(j=0; j < prob->num_bool_vars; ++j)
PRINTTEXT("%i ", get_bool_node_id(i, prob)[j] );
PRINTTEXT(" | ");
for(j=0; j < prob->num_int_vars; ++j)
PRINTTEXT("(%.2f, %.2f) ", -get_int_node_id(i, prob)[2*j], get_int_node_id(i, prob)[2*j+1] );
PRINTTEXT("\n");
}
}
void printProgress(stats* info)
{
if( info->iter == 0 )
{
/* print header at very first iteration */
#if PRINTLEVEL == 2
PRINTTEXT("\nECOS - (c) A. Domahidi, Automatic Control Laboratory, ETH Zurich, 2012-13.\n\n");
#endif
PRINTTEXT("It pcost dcost gap pres dres k/t mu step IR\n");
#if defined WIN32 || defined _WIN64
PRINTTEXT("%2d %+5.3e %+5.3e %+2.0e %2.0e %2.0e %2.0e %2.0e N/A %d %d -\n",(int)info->iter, info->pcost, info->dcost, info->gap, info->pres, info->dres, info->kapovert, info->mu, (int)info->nitref1, (int)info->nitref2);
#else
PRINTTEXT("%2d %c%+5.3e %c%+5.3e %c%+2.0e %c%2.0e %c%2.0e %c%2.0e %c%2.0e N/A %d %d -\n",(int)info->iter, 32, info->pcost, 32, info->dcost, 32, info->gap, 32, info->pres, 32, info->dres, 32, info->kapovert, 32, info->mu, (int)info->nitref1, (int)info->nitref2);
#endif
} else {
#if defined WIN32 || defined _WIN64
PRINTTEXT("%2d %+5.3e %+5.3e %+2.0e %2.0e %2.0e %2.0e %2.0e %6.4f %d %d %d\n",(int)info->iter, info->pcost, info->dcost, info->gap, info->pres, info->dres, info->kapovert, info->mu, info->step, (int)info->nitref1, (int)info->nitref2, (int)info->nitref3);
#else
PRINTTEXT("%2d %c%+5.3e%c %+5.3e %c %+2.0e%c %2.0e%c %2.0e%c %2.0e%c %2.0e %6.4f %d %d %d\n",(int)info->iter, 32,info->pcost, 32,info->dcost, 32, info->gap, 32, info->pres, 32, info->dres, 32, info->kapovert, 32, info->mu, info->step, (int)info->nitref1, (int)info->nitref2, (int)info->nitref3);
#endif
}
/* enable to flush printf in Matlab immediately */
#if PRINTLEVEL > 0
#ifdef MATLAB_MEX_FILE
#if defined MATLAB_FLUSH_PRINTS
mexEvalString("drawnow;");
#endif
#endif
#endif
}
void deleteLastProgressLine( stats* info )
{
idxint i;
idxint offset = 0;
if( info->kapovert < 0 ) offset++;
if( info->mu < 0) offset++;
if( info->pres < 0 ) offset++;
if (info->dres < 0 ) offset++;
for (i=0; i<82+offset; i++) {
PRINTTEXT("%c",8);
}
}
/* use LOAD and SPCONVERT to read in the file in MATLAB */
void dumpSparseMatrix(spmat* M, char* fn)
{
idxint j, i, row_strt, row_stop;
idxint k = 0;
FILE *f = fopen(fn,"w");
if( f != NULL ){
for(j=0; j<M->n; j++){
row_strt = M->jc[j];
row_stop = M->jc[j+1];
if (row_strt == row_stop)
continue;
else {
for(i=row_strt; i<row_stop; i++ ){
fprintf(f,"%d\t%d\t%20.18e\n", (int)M->ir[i]+1, (int)j+1, M->pr[k++]);
}
}
}
fprintf(f,"%d\t%d\t%20.18e\n", (int)M->m, (int)M->n, 0.0);
fclose(f);
PRINTTEXT("File %s successfully written.\n", fn);
} else {
PRINTTEXT("Error during writing file %s.\n", fn);
}
}
/*
* Prints a sparse matrix.
*/
void printSparseMatrix(spmat* M)
{
idxint j, i, row_strt, row_stop;
idxint k = 0;
for(j=0; j<M->n; j++){
row_strt = M->jc[j];
row_stop = M->jc[j+1];
if (row_strt == row_stop)
continue;
else {
for(i=row_strt; i<row_stop; i++ ){
PRINTTEXT("\t(%3u,%3u) = %g\n", (int)M->ir[i]+1, (int)j+1, M->pr[k++]);
}
}
}
}
/*
* Updates statistics.
*/
void updateStatistics(pwork* w)
{
pfloat nry, nrz;
stats* info = w->info;
/* mu = (s'*z + kap*tau) / (D+1) where s'*z is the duality gap */
info->gap = ddot(w->m, w->s, w->z);
info->mu = (info->gap + w->kap*w->tau) / (w->D + 1);
info->kapovert = w->kap / w->tau;
info->pcost = w->cx / w->tau;
info->dcost = -(w->hz + w->by) / w->tau;
/* relative duality gap */
if( info->pcost < 0 ){ info->relgap = info->gap / (-info->pcost); }
else if( info->dcost > 0 ){ info->relgap = info->gap / info->dcost; }
else info->relgap = NAN;
/* residuals */
nry = w->p > 0 ? norm2(w->ry, w->p)/w->resy0 : 0.0;
nrz = norm2(w->rz, w->m)/w->resz0;
info->pres = MAX(nry, nrz) / w->tau;
info->dres = norm2(w->rx, w->n)/w->resx0 / w->tau;
/* infeasibility measures
*
* CVXOPT uses the following:
* info->pinfres = w->hz + w->by < 0 ? w->hresx / w->resx0 / (-w->hz - w->by) : NAN;
* info->dinfres = w->cx < 0 ? MAX(w->hresy/w->resy0, w->hresz/w->resz0) / (-w->cx) : NAN;
*/
info->pinfres = w->hz + w->by < 0 ? w->hresx/w->resx0 : NAN;
info->dinfres = w->cx < 0 ? MAX(w->hresy/w->resy0, w->hresz/w->resz0) : NAN;
#if PRINTLEVEL > 2
PRINTTEXT("TAU=%6.4e KAP=%6.4e PINFRES=%6.4e DINFRES=%6.4e\n",w->tau,w->kap,info->pinfres, info->dinfres );
#endif
}
/*
* Function to return the next var to split on
*/
void get_branch_var(ecos_bb_pwork* prob, idxint* split_idx, pfloat* split_val){
idxint i;
pfloat x, y, d, ambiguity;
d = 1.0;
for (i=0; i<(prob->num_bool_vars + prob->num_int_vars); ++i){
if (i < prob->num_bool_vars ){
y = prob->ecos_prob->x[ prob->bool_vars_idx[i] ];
x = y;
} else {
y = prob->ecos_prob->x[ prob->int_vars_idx[i-prob->num_bool_vars] ];
x = y - pfloat_floor(y, prob->stgs->integer_tol );
}
ambiguity = abs_2(x-0.5);
if ( ambiguity < d){
*split_idx = i;
*split_val = y;
d = ambiguity;
}
}
#if MI_PRINTLEVEL > 1
PRINTTEXT("split_idx:%u, split_val:%f\n", *split_idx, *split_val);
#endif
}
/*
* Initializes the solver.
*/
idxint init(pwork* w)
{
idxint i, j, k, l, KKT_FACTOR_RETURN_CODE;
idxint* Pinv = w->KKT->Pinv;
pfloat rx, ry, rz;
#if PROFILING > 1
timer tfactor, tkktsolve;
#endif
/* set regularization parameter */
w->KKT->delta = w->stgs->delta;
/* Initialize KKT matrix */
kkt_init(w->KKT->PKPt, w->KKT->PK, w->C);
#if DEBUG > 0
dumpSparseMatrix(w->KKT->PKPt, "PKPt0.txt");
#endif
/* initialize RHS1 */
k = 0; j = 0;
for( i=0; i<w->n; i++ ){ w->KKT->RHS1[w->KKT->Pinv[k++]] = 0; }
for( i=0; i<w->p; i++ ){ w->KKT->RHS1[w->KKT->Pinv[k++]] = w->b[i]; }
for( i=0; i<w->C->lpc->p; i++ ){ w->KKT->RHS1[w->KKT->Pinv[k++]] = w->h[i]; j++; }
for( l=0; l<w->C->nsoc; l++ ){
for( i=0; i < w->C->soc[l].p; i++ ){ w->KKT->RHS1[w->KKT->Pinv[k++]] = w->h[j++]; }
#if CONEMODE == 0
w->KKT->RHS1[w->KKT->Pinv[k++]] = 0;
w->KKT->RHS1[w->KKT->Pinv[k++]] = 0;
#endif
}
#if PRINTLEVEL > 2
PRINTTEXT("Written %d entries of RHS1\n", (int)k);
#endif
/* initialize RHS2 */
for( i=0; i<w->n; i++ ){ w->KKT->RHS2[w->KKT->Pinv[i]] = -w->c[i]; }
for( i=w->n; i<w->KKT->PKPt->n; i++ ){ w->KKT->RHS2[w->KKT->Pinv[i]] = 0; }
/* get scalings of problem data */
rx = norm2(w->c, w->n); w->resx0 = MAX(1, rx);
ry = norm2(w->b, w->p); w->resy0 = MAX(1, ry);
rz = norm2(w->h, w->m); w->resz0 = MAX(1, rz);
/* Factor KKT matrix - this is needed in all 3 linear system solves */
#if PROFILING > 1
tic(&tfactor);
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta, &w->info->tfactor_t1, &w->info->tfactor_t2);
w->info->tfactor += toc(&tfactor);
#else
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta);
#endif
/* check if factorization was successful, exit otherwise */
if( KKT_FACTOR_RETURN_CODE != KKT_OK ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nProblem in factoring KKT system, aborting.");
#endif
return ECOS_FATAL;
}
/*
* PRIMAL VARIABLES:
* - solve xhat = arg min ||Gx-h||_2^2 such that Ax = b
* - r = h - G*xhat
* These two equations are solved by
*
* [ 0 A' G' ] [ xhat ] [ 0 ]
* [ A 0 0 ] [ y ] = [ b ]
* [ G 0 -I ] [ -r ] [ h ]
*
* and then take shat = r if alphap < 0, zbar + (1+alphap)*e otherwise
* where alphap = inf{ alpha | sbar + alpha*e >= 0 }
*/
/* Solve for RHS [0; b; h] */
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref1 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS1, w->KKT->dx1, w->KKT->dy1, w->KKT->dz1, w->n, w->p, w->m, w->C, 1, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
#if DEBUG > 0
#if PRINTLEVEL > 3
printDenseMatrix(w->KKT->dx1, w->n, 1, "dx1_init");
printDenseMatrix(w->KKT->dy1, w->p, 1, "dy1_init");
printDenseMatrix(w->KKT->dz1, w->m, 1, "dz1_init");
#endif
#endif
/* Copy out initial value of x */
for( i=0; i<w->n; i++ ){ w->x[i] = w->KKT->dx1[i]; }
/* Copy out -r into temporary variable */
for( i=0; i<w->m; i++ ){ w->KKT->work1[i] = -w->KKT->dz1[i]; }
/* Bring variable to cone */
bring2cone(w->C, w->KKT->work1, w->s );
/*
* dual variables
* solve (yhat,zbar) = arg min ||z||_2^2 such that G'*z + A'*y + c = 0
*
* we can solve this by
*
* [ 0 A' G' ] [ x ] [ -c ]
* [ A 0 0 ] [ yhat ] = [ 0 ]
* [ G 0 -I ] [ zbar ] [ 0 ]
*
* and then take zhat = zbar if alphad < 0, zbar + (1+alphad)*e otherwise
* where alphad = inf{ alpha | zbar + alpha*e >= 0 }
*/
/* Solve for RHS [-c; 0; 0] */
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref2 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS2, w->KKT->dx2, w->KKT->dy2, w->KKT->dz2, w->n, w->p, w->m, w->C, 1, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
#if DEBUG > 0
#if PRINTLEVEL > 3
printDenseMatrix(w->KKT->dx2, w->n, 1, "dx2_init");
printDenseMatrix(w->KKT->dy2, w->p, 1, "dy2_init");
printDenseMatrix(w->KKT->dz2, w->m, 1, "dz2_init");
#endif
#endif
/* Copy out initial value of y */
for( i=0; i<w->p; i++ ){ w->y[i] = w->KKT->dy2[i]; }
/* Bring variable to cone */
bring2cone(w->C, w->KKT->dz2, w->z );
/* Prepare RHS1 - before this line RHS1 = [0; b; h], after it holds [-c; b; h] */
for( i=0; i<w->n; i++){ w->KKT->RHS1[Pinv[i]] = -w->c[i]; }
/*
* other variables
*/
w->kap = 1.0;
w->tau = 1.0;
w->info->step = 0;
w->info->step_aff = 0;
w->info->dinf = 0;
w->info->pinf = 0;
return 0;
}
/*
* Builds KKT matrix.
* We store and operate only on the upper triangular part.
* Replace by or use in codegen.
*
* INPUT: spmat* Gt - pointer to G'
* spmat* At - pointer to A'
* cone* C - pointer to cone struct
*
* OUTPUT: idxint* Sign - pointer to vector of signs for regularization
* spmat* K - pointer to unpermuted upper triangular part of KKT matrix
*/
void createKKT_U(spmat* Gt, spmat* At, cone* C, idxint** S, spmat** K)
{
idxint i, j, k, l, r, row_stop, row, cone_strt, ks, conesize;
idxint n = Gt->m;
idxint m = Gt->n;
idxint p = At ? At->n : 0;
idxint nK, nnzK;
pfloat *Kpr;
idxint *Kjc, *Kir;
idxint *Sign;
/* Dimension of KKT matrix
* = n (number of variables)
* + p (number of equality constraints)
* + m (number of inequality constraints)
* + 2*C->nsoc (expansion of SOC scalings)
*/
nK = n + p + m;
#if CONEMODE == 0
nK += 2*C->nsoc;
#endif
/* Number of non-zeros in KKT matrix
* = At->nnz (nnz of equality constraint matrix A)
* + Gt->nnz (nnz of inequality constraint matrix)
* + C->lpc.p (nnz of LP cone)
* + 3*[sum(C->soc[i].p)+1] (nnz of expanded soc scalings)
*/
nnzK = (At ? At->nnz : 0) + Gt->nnz + C->lpc->p;
#if STATICREG == 1
nnzK += n+p;
#endif
for( i=0; i<C->nsoc; i++ ){
#if CONEMODE == 0
nnzK += 3*C->soc[i].p+1;
#else
nnzK += (C->soc[i].p*(C->soc[i].p+1))/2;
#endif
}
#if PRINTLEVEL > 2
PRINTTEXT("Non-zeros in KKT matrix: %d\n", (int) nnzK);
#endif
/* Allocate memory for KKT matrix */
Kpr = (pfloat *)MALLOC(nnzK*sizeof(pfloat));
Kir = (idxint *)MALLOC(nnzK*sizeof(idxint));
Kjc = (idxint *)MALLOC((nK+1)*sizeof(idxint));
/* Allocate memory for sign vector */
Sign = (idxint *)MALLOC(nK*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for sign vector\n");
#endif
/* Set signs for regularization of (1,1) block */
for( ks=0; ks < n; ks++ ){
Sign[ks] = +1; /* (1,1) block */
}
for( ks=n; ks < n+p; ks++){
Sign[ks] = -1; /* (2,2) block */
}
#if CONEMODE == 0
for( ks=n+p; ks < n+p+C->lpc->p; ks++){
Sign[ks] = -1; /* (3,3) block: LP cone */
}
ks = n+p+C->lpc->p;
for( l=0; l<C->nsoc; l++){
for (i=0; i<C->soc[l].p; i++) {
Sign[ks++] = -1; /* (3,3) block: SOC, D */
}
Sign[ks++] = -1; /* (3,3) block: SOC, v */
Sign[ks++] = +1; /* (3,3) block: SOC, u */
}
#else
for (ks=n+p; ks < n+p+m; ks++) {
Sign[ks] = -1; /* (3,3) block has -1 sign if all dense */
}
#endif
#if DEBUG > 0
if (ks!=nK) {
PRINTTEXT("ks = %d, whereas nK = %d - exiting.", (int)ks, (int)nK);
exit(-1);
}
#endif
/* count the number of non-zero entries in K */
k = 0;
/* (1,1) block: the first n columns are empty */
#if STATICREG == 0
for (j=0; j<n; j++) {
Kjc[j] = 0;
}
#else
for (j=0; j<n; j++) {
Kjc[j] = j;
Kir[j] = j;
Kpr[k++] = DELTASTAT;
}
#endif
/* Fill upper triangular part of K with values */
/* (1,2) block: A' */
i = 0; /* counter for non-zero entries in A or G, respectively */
for( j=0; j<p; j++ ){
/* A' */
row = At->jc[j];
row_stop = At->jc[j+1];
if( row <= row_stop ){
Kjc[n+j] = k;
while( row++ < row_stop ){
Kir[k] = At->ir[i];
Kpr[k] = At->pr[i];
k++; i++;
}
}
#if STATICREG == 1
Kir[k] = n+j;
Kpr[k++] = -DELTASTAT;
#endif
}
/* (1,3) and (3,3) block: [G'; 0; -Vinit]
* where
*
* Vinit = blkdiag(I, blkdiag(I,1,-1), ..., blkdiag(I,1,-1));
* ^ #number of second-order cones ^
*
* Note that we have to prepare the (3,3) block accordingly
* (put zeros for init but store indices that are used in KKT_update
* of cone module)
*/
/* LP cone */
i = 0;
for( j=0; j < C->lpc->p; j++ ){
/* copy in G' */
row = Gt->jc[j];
row_stop = Gt->jc[j+1];
if( row <= row_stop ){
Kjc[n+p+j] = k;
while( row++ < row_stop ){
Kir[k] = Gt->ir[i];
Kpr[k] = Gt->pr[i];
k++; i++;
}
}
/* -I for LP-cone */
C->lpc->kkt_idx[j] = k;
Kir[k] = n+p+j;
Kpr[k] = -1.0;
k++;
}
#if CONEMODE == 0
/* Second-order cones - copy in G' and set up the scaling matrix
* which has the following structure:
*
*
* [ * 0 * ]
* [ * * * ]
* [ * * * ] [ I v u ] I: identity of size conesize
* - [ * * * ] = -[ u' 1 0 ] v: vector of size conesize - 1
* [ * * * ] [ v' 0' -1 ] u: vector of size conesize
* [ * * * ]
* [ * * * ]
* [ 0 * * * * * * 1 0 ]
* [ * * * * * * * 0 -1 ]
*
* NOTE: only the upper triangular part (with the diagonal elements)
* is copied in here.
*/
cone_strt = C->lpc->p;
for( l=0; l < C->nsoc; l++ ){
/* size of the cone */
conesize = C->soc[l].p;
/* go column-wise about it */
for( j=0; j < conesize; j++ ){
row = Gt->jc[cone_strt+j];
row_stop = Gt->jc[cone_strt+j+1];
if( row <= row_stop ){
Kjc[n+p+cone_strt+2*l+j] = k;
while( row++ < row_stop ){
Kir[k] = Gt->ir[i];
Kpr[k++] = Gt->pr[i++];
}
}
/* diagonal D */
Kir[k] = n+p+cone_strt + 2*l + j;
Kpr[k] = -1.0;
C->soc[l].Didx[j] = k;
k++;
}
/* v */
Kjc[n+p+cone_strt+2*l+conesize] = k;
for (r=1; r<conesize; r++) {
Kir[k] = n+p+cone_strt + 2*l + r;
Kpr[k] = 0;
k++;
}
Kir[k] = n+p+cone_strt + 2*l + conesize;
Kpr[k] = -1;
k++;
/* u */
Kjc[n+p+cone_strt+2*l+conesize+1] = k;
for (r=0; r<conesize; r++) {
Kir[k] = n+p+cone_strt + 2*l + r;
Kpr[k] = 0;
k++;
}
Kir[k] = n+p+cone_strt + 2*l + conesize + 1;
Kpr[k] = +1;
k++;
/* prepare index for next cone */
cone_strt += C->soc[l].p;
}
#else
/* Second-order cones - copy in G' and set up the scaling matrix
* which has a dense structure (only upper half part is shown):
*
* [ a b*q' ]
* - W^2 = -V = eta^2 [ b*q I + c*(q*q') ]
*
* where I: identity of size conesize
* q: vector of size consize - 1
* a,b,c: scalars
*
* NOTE: only the upper triangular part (with the diagonal elements)
* is copied in here.
*/
cone_strt = C->lpc->p;
for( l=0; l < C->nsoc; l++ ){
/* size of the cone */
conesize = C->soc[l].p;
/* go column-wise about it */
for( j=0; j < conesize; j++ ){
row = Gt->jc[cone_strt+j];
row_stop = Gt->jc[cone_strt+j+1];
if( row <= row_stop ){
Kjc[n+p+cone_strt+j] = k;
while( row++ < row_stop ){
Kir[k] = Gt->ir[i];
Kpr[k++] = Gt->pr[i++];
}
}
/* first elements - record where this column starts */
Kir[k] = n+p+cone_strt;
Kpr[k] = -1.0;
C->soc[l].colstart[j] = k;
k++;
/* the rest of the column */
for (r=1; r<=j; r++) {
Kir[k] = n+p+cone_strt+r;
Kpr[k] = -1.0;
k++;
}
}
/* prepare index for next cone */
cone_strt += C->soc[l].p;
}
#endif
#if PRINTLEVEL > 2
PRINTTEXT("CREATEKKT: Written %d KKT entries\n", (int)k);
PRINTTEXT("CREATEKKT: nK=%d and ks=%d\n",(int)nK,(int)ks);
PRINTTEXT("CREATEKKT: Size of KKT matrix: %d\n", (int)nK);
#endif
/* return Sign vector and KKT matrix */
*S = Sign;
*K = createSparseMatrix(nK, nK, nnzK, Kjc, Kir, Kpr);
}
/*
* Sets up all data structures needed.
* Replace by codegen
*/
pwork* ECOS_setup(idxint n, idxint m, idxint p, idxint l, idxint ncones, idxint* q,
pfloat* Gpr, idxint* Gjc, idxint* Gir,
pfloat* Apr, idxint* Ajc, idxint* Air,
pfloat* c, pfloat* h, pfloat* b)
{
idxint i, j, k, cidx, conesize, lnz, amd_result, nK, *Ljc, *Lir, *P, *Pinv, *Sign;
pwork* mywork;
double Control [AMD_CONTROL], Info [AMD_INFO];
pfloat rx, ry, rz, *Lpr;
spmat *At, *Gt, *KU;
#if PROFILING > 0
timer tsetup;
#endif
#if PROFILING > 1
timer tcreatekkt;
timer tmattranspose;
timer tordering;
#endif
#if PROFILING > 0
tic(&tsetup);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("\n");
PRINTTEXT(" *******************************************************************************\n");
PRINTTEXT(" * ECOS: Embedded Conic Solver - Sparse Interior Point method for SOCPs *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * NOTE: The solver is based on L. Vandenberghe's 'The CVXOPT linear and quad- *\n");
PRINTTEXT(" * ratic cone program solvers', March 20, 2010. Available online: *\n");
PRINTTEXT(" * [http://abel.ee.ucla.edu/cvxopt/documentation/coneprog.pdf] *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * This code uses T.A. Davis' sparse LDL package and AMD code. *\n");
PRINTTEXT(" * [http://www.cise.ufl.edu/research/sparse] *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * Written during a summer visit at Stanford University with S. Boyd. *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * (C) Alexander Domahidi, Automatic Control Laboratory, ETH Zurich, 2012-13. *\n");
PRINTTEXT(" * Email: [email protected] *\n");
PRINTTEXT(" *******************************************************************************\n");
PRINTTEXT("\n\n");
PRINTTEXT("PROBLEM SUMMARY:\n");
PRINTTEXT(" Primal variables (n): %d\n", (int)n);
PRINTTEXT("Equality constraints (p): %d\n", (int)p);
PRINTTEXT(" Conic variables (m): %d\n", (int)m);
PRINTTEXT("- - - - - - - - - - - - - - -\n");
PRINTTEXT(" Size of LP cone: %d\n", (int)l);
PRINTTEXT(" Number of SOCs: %d\n", (int)ncones);
for( i=0; i<ncones; i++ ){
PRINTTEXT(" Size of SOC #%02d: %d\n", (int)(i+1), (int)q[i]);
}
#endif
/* get work data structure */
mywork = (pwork *)MALLOC(sizeof(pwork));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for WORK struct\n");
#endif
/* dimensions */
mywork->n = n;
mywork->m = m;
mywork->p = p;
mywork->D = l + ncones;
#if PRINTLEVEL > 2
PRINTTEXT("Set dimensions\n");
#endif
/* variables */
mywork->x = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->y = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->z = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->s = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->lambda = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dsaff_by_W = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dsaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->saff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->zaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->W_times_dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for variables\n");
#endif
/* best iterates so far */
mywork->best_x = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->best_y = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->best_z = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->best_s = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->best_info = (stats *)MALLOC(sizeof(stats));
/* cones */
mywork->C = (cone *)MALLOC(sizeof(cone));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for cone struct\n");
#endif
/* LP cone */
mywork->C->lpc = (lpcone *)MALLOC(sizeof(lpcone));
mywork->C->lpc->p = l;
if( l > 0 ){
mywork->C->lpc->w = (pfloat *)MALLOC(l*sizeof(pfloat));
mywork->C->lpc->v = (pfloat *)MALLOC(l*sizeof(pfloat));
mywork->C->lpc->kkt_idx = (idxint *)MALLOC(l*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for LP cone\n");
#endif
} else {
mywork->C->lpc->w = NULL;
mywork->C->lpc->v = NULL;
mywork->C->lpc->kkt_idx = NULL;
#if PRINTLEVEL > 2
PRINTTEXT("No LP cone present, pointers filled with NULL\n");
#endif
}
/* Second-order cones */
mywork->C->soc = (socone *)MALLOC(ncones*sizeof(socone));
mywork->C->nsoc = ncones;
cidx = 0;
for( i=0; i<ncones; i++ ){
conesize = (idxint)q[i];
mywork->C->soc[i].p = conesize;
mywork->C->soc[i].a = 0;
mywork->C->soc[i].eta = 0;
mywork->C->soc[i].q = (pfloat *)MALLOC((conesize-1)*sizeof(pfloat));
mywork->C->soc[i].skbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
mywork->C->soc[i].zkbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
#if CONEMODE == 0
mywork->C->soc[i].Didx = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
#if CONEMODE > 0
mywork->C->soc[i].colstart = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
cidx += conesize;
}
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for second-order cones\n");
#endif
/* info struct */
mywork->info = (stats *)MALLOC(sizeof(stats));
#if PROFILING > 1
mywork->info->tfactor = 0;
mywork->info->tkktsolve = 0;
mywork->info->tfactor_t1 = 0;
mywork->info->tfactor_t2 = 0;
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for info struct\n");
#endif
#if defined EQUILIBRATE && EQUILIBRATE > 0
/* equilibration vector */
mywork->xequil = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->Aequil = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->Gequil = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for equilibration vectors\n");
#endif
#endif
/* settings */
mywork->stgs = (settings *)MALLOC(sizeof(settings));
mywork->stgs->maxit = MAXIT;
mywork->stgs->gamma = GAMMA;
mywork->stgs->delta = DELTA;
mywork->stgs->eps = EPS;
mywork->stgs->nitref = NITREF;
mywork->stgs->abstol = ABSTOL;
mywork->stgs->feastol = FEASTOL;
mywork->stgs->reltol = RELTOL;
mywork->stgs->abstol_inacc = ATOL_INACC;
mywork->stgs->feastol_inacc = FTOL_INACC;
mywork->stgs->reltol_inacc = RTOL_INACC;
mywork->stgs->verbose = VERBOSE;
#if PRINTLEVEL > 2
PRINTTEXT("Written settings\n");
#endif
mywork->c = c;
mywork->h = h;
mywork->b = b;
#if PRINTLEVEL > 2
PRINTTEXT("Hung pointers for c, h and b into WORK struct\n");
#endif
/* Store problem data */
if(Apr && Ajc && Air) {
mywork->A = createSparseMatrix(p, n, Ajc[n], Ajc, Air, Apr);
} else {
mywork->A = NULL;
}
if (Gpr && Gjc && Gir) {
mywork->G = createSparseMatrix(m, n, Gjc[n], Gjc, Gir, Gpr);
} else {
/* create an empty sparse matrix */
mywork->G = createSparseMatrix(m, n, 0, Gjc, Gir, Gpr);
}
#if defined EQUILIBRATE && EQUILIBRATE > 0
set_equilibration(mywork);
#if PRINTLEVEL > 2
PRINTTEXT("Done equilibrating\n");
#endif
#endif
#if PROFILING > 1
mywork->info->ttranspose = 0;
tic(&tmattranspose);
#endif
if(mywork->A)
At = transposeSparseMatrix(mywork->A);
else
At = NULL;
#if PROFILING > 1
mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Transposed A\n");
#endif
#if PROFILING > 1
tic(&tmattranspose);
#endif
Gt = transposeSparseMatrix(mywork->G);
#if PROFILING > 1
mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Transposed G\n");
#endif
/* set up KKT system */
#if PROFILING > 1
tic(&tcreatekkt);
#endif
createKKT_U(Gt, At, mywork->C, &Sign, &KU);
#if PROFILING > 1
mywork->info->tkktcreate = toc(&tcreatekkt);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Created upper part of KKT matrix K\n");
#endif
/*
* Set up KKT system related data
* (L comes later after symbolic factorization)
*/
nK = KU->n;
#if DEBUG > 0
dumpSparseMatrix(KU, "KU0.txt");
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Dimension of KKT matrix: %d\n", (int)nK);
PRINTTEXT("Non-zeros in KKT matrix: %d\n", (int)KU->nnz);
#endif
/* allocate memory in KKT system */
mywork->KKT = (kkt *)MALLOC(sizeof(kkt));
mywork->KKT->D = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->Parent = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Pinv = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->work1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work3 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work4 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work5 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work6 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->Flag = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Pattern = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Lnz = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->RHS1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->RHS2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->dx1 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
mywork->KKT->dx2 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
mywork->KKT->dy1 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
mywork->KKT->dy2 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
mywork->KKT->dz1 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
mywork->KKT->dz2 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
mywork->KKT->Sign = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->PKPt = newSparseMatrix(nK, nK, KU->nnz);
mywork->KKT->PK = (idxint *)MALLOC(KU->nnz*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Created memory for KKT-related data\n");
#endif
/* calculate ordering of KKT matrix using AMD */
P = (idxint *)MALLOC(nK*sizeof(idxint));
#if PROFILING > 1
tic(&tordering);
#endif
AMD_defaults(Control);
amd_result = AMD_order(nK, KU->jc, KU->ir, P, Control, Info);
#if PROFILING > 1
mywork->info->torder = toc(&tordering);
#endif
if( amd_result == AMD_OK ){
#if PRINTLEVEL > 2
PRINTTEXT("AMD ordering successfully computed.\n");
AMD_info(Info);
#endif
} else {
#if PRINTLEVEL > 2
PRINTTEXT("Problem in AMD ordering, exiting.\n");
AMD_info(Info);
#endif
return NULL;
}
/* calculate inverse permutation and permutation mapping of KKT matrix */
pinv(nK, P, mywork->KKT->Pinv);
Pinv = mywork->KKT->Pinv;
#if DEBUG > 0
dumpDenseMatrix_i(P, nK, 1, "P.txt");
dumpDenseMatrix_i(mywork->KKT->Pinv, nK, 1, "PINV.txt");
#endif
permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, mywork->KKT->PK);
/* permute sign vector */
for( i=0; i<nK; i++ ){ mywork->KKT->Sign[Pinv[i]] = Sign[i]; }
#if PRINTLEVEL > 3
PRINTTEXT("P = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)P[i]); }
PRINTTEXT("];\n");
PRINTTEXT("Pinv = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)Pinv[i]); }
PRINTTEXT("];\n");
PRINTTEXT("Sign = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)Sign[i]); }
PRINTTEXT("];\n");
PRINTTEXT("SignP = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)mywork->KKT->Sign[i]); }
PRINTTEXT("];\n");
#endif
/* symbolic factorization */
Ljc = (idxint *)MALLOC((nK+1)*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Allocated memory for cholesky factor L\n");
#endif
LDL_symbolic2(
mywork->KKT->PKPt->n, /* A and L are n-by-n, where n >= 0 */
mywork->KKT->PKPt->jc, /* input of size n+1, not modified */
mywork->KKT->PKPt->ir, /* input of size nz=Ap[n], not modified */
Ljc, /* output of size n+1, not defined on input */
mywork->KKT->Parent, /* output of size n, not defined on input */
mywork->KKT->Lnz, /* output of size n, not defined on input */
mywork->KKT->Flag /* workspace of size n, not defn. on input or output */
);
/* assign memory for L */
lnz = Ljc[nK];
#if PRINTLEVEL > 2
PRINTTEXT("Nonzeros in L, excluding diagonal: %d\n", (int)lnz) ;
#endif
Lir = (idxint *)MALLOC(lnz*sizeof(idxint));
Lpr = (pfloat *)MALLOC(lnz*sizeof(pfloat));
mywork->KKT->L = createSparseMatrix(nK, nK, lnz, Ljc, Lir, Lpr);
#if PRINTLEVEL > 2
PRINTTEXT("Created Cholesky factor of K in KKT struct\n");
#endif
/* permute KKT matrix - we work on this one from now on */
permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, NULL);
#if DEBUG > 0
dumpSparseMatrix(mywork->KKT->PKPt, "PKPt.txt");
#endif
#if CONEMODE > 0
/* zero any off-diagonal elements in (permuted) scalings in KKT matrix */
for (i=0; i<mywork->C->nsoc; i++) {
for (j=1; j<mywork->C->soc[i].p; j++) {
for (k=0; k<j; k++) {
mywork->KKT->PKPt->pr[mywork->KKT->PK[mywork->C->soc[i].colstart[j]+k]] = 0;
}
}
}
#endif
#if DEBUG > 0
dumpSparseMatrix(mywork->KKT->PKPt, "PKPt0.txt");
#endif
/* set up RHSp for initialization */
k = 0; j = 0;
for( i=0; i<n; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = 0; }
for( i=0; i<p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = b[i]; }
for( i=0; i<l; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[i]; j++; }
for( l=0; l<ncones; l++ ){
for( i=0; i < mywork->C->soc[l].p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[j++]; }
#if CONEMODE == 0
mywork->KKT->RHS1[Pinv[k++]] = 0;
mywork->KKT->RHS1[Pinv[k++]] = 0;
#endif
}
#if PRINTLEVEL > 2
PRINTTEXT("Written %d entries of RHS1\n", (int)k);
#endif
/* set up RHSd for initialization */
for( i=0; i<n; i++ ){ mywork->KKT->RHS2[Pinv[i]] = -c[i]; }
for( i=n; i<nK; i++ ){ mywork->KKT->RHS2[Pinv[i]] = 0; }
/* get scalings of problem data */
rx = norm2(c, n); mywork->resx0 = MAX(1, rx);
ry = norm2(b, p); mywork->resy0 = MAX(1, ry);
rz = norm2(h, m); mywork->resz0 = MAX(1, rz);
/* get memory for residuals */
mywork->rx = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->ry = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->rz = (pfloat *)MALLOC(m*sizeof(pfloat));
/* clean up */
mywork->KKT->P = P;
FREE(Sign);
if(At) freeSparseMatrix(At);
freeSparseMatrix(Gt);
freeSparseMatrix(KU);
#if PROFILING > 0
mywork->info->tsetup = toc(&tsetup);
#endif
return mywork;
}
/*
* Main solver routine.
*/
idxint ECOS_solve(pwork* w)
{
idxint i, initcode, KKT_FACTOR_RETURN_CODE;
pfloat dtau_denom, dtauaff, dkapaff, sigma, dtau, dkap, bkap, pres_prev;
idxint exitcode = ECOS_FATAL;
#if PROFILING > 0
timer tsolve;
#endif
#if PROFILING > 1
timer tfactor, tkktsolve;
#endif
#if PROFILING > 0
/* start timer */
tic(&tsolve);
#endif
/* Initialize solver */
initcode = init(w);
if( initcode == ECOS_FATAL ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nFatal error during initialization, aborting.");
#endif
return ECOS_FATAL;
}
/* MAIN INTERIOR POINT LOOP ---------------------------------------------------------------------- */
for( w->info->iter = 0; w->info->iter <= w->stgs->maxit; w->info->iter++ ){
/* Compute residuals */
computeResiduals(w);
/* Update statistics */
updateStatistics(w);
#if PRINTLEVEL > 1
/* Print info */
if( w->stgs->verbose ) printProgress(w->info);
#endif
/* SAFEGUARD: Backtrack to old iterate if the update was bad such that the primal residual PRES has
* increased by a factor of SAFEGUARD.
* If the safeguard is activated, the solver quits with the flag ECOS_NUMERICS.
*/
if( w->info->iter > 0 && w->info->pres > SAFEGUARD*pres_prev ){
#if PRINTLEVEL > 1
if( w->stgs->verbose ) PRINTTEXT("\nNUMERICAL PROBLEMS, recovering iterate %d and stopping.\n", (int)w->info->iter-1);
#endif
/* Backtrack */
for( i=0; i < w->n; i++ ){ w->x[i] -= w->info->step * w->KKT->dx2[i]; }
for( i=0; i < w->p; i++ ){ w->y[i] -= w->info->step * w->KKT->dy2[i]; }
for( i=0; i < w->m; i++ ){ w->z[i] -= w->info->step * w->KKT->dz2[i]; }
for( i=0; i < w->m; i++ ){ w->s[i] -= w->info->step * w->dsaff[i]; }
w->kap -= w->info->step * dkap;
w->tau -= w->info->step * dtau;
exitcode = ECOS_NUMERICS;
computeResiduals(w);
updateStatistics(w);
break;
}
pres_prev = w->info->pres;
/* Check termination criteria and exit if necessary */
/* Optimal? */
if( ( ( -w->cx > 0 ) || ( -w->by - w->hz > 0) ) &&
w->info->pres < w->stgs->feastol && w->info->dres < w->stgs->feastol &&
( w->info->gap < w->stgs->abstol || w->info->relgap < w->stgs->reltol ) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nOPTIMAL (within feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", w->stgs->feastol, w->stgs->reltol, w->stgs->abstol);
#endif
exitcode = ECOS_OPTIMAL;
break;
}
/* Primal infeasible? */
else if( ((w->info->pinfres != NAN) && (w->info->pinfres < w->stgs->feastol)) ||
((w->tau < w->stgs->feastol) && (w->kap < w->stgs->feastol && w->info->pinfres < w->stgs->feastol)) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nPRIMAL INFEASIBLE (within feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", w->stgs->feastol, w->stgs->reltol, w->stgs->abstol);
#endif
w->info->pinf = 1;
w->info->dinf = 0;
exitcode = ECOS_PINF;
break;
}
/* Dual infeasible? */
else if( (w->info->dinfres != NAN) && (w->info->dinfres < w->stgs->feastol) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nUNBOUNDED (within feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", w->stgs->feastol, w->stgs->reltol, w->stgs->abstol);
#endif
w->info->pinf = 0;
w->info->dinf = 1;
exitcode = ECOS_DINF;
break;
}
/* Did the line search c**k up? (zero step length) */
else if( w->info->iter > 0 && w->info->step == STEPMIN*GAMMA ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nNo further progress possible (- numerics?), exiting.");
#endif
exitcode = ECOS_NUMERICS;
break;
}
/* MAXIT reached? */
else if( w->info->iter == w->stgs->maxit ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nMaximum number of iterations reached, exiting.");
#endif
exitcode = ECOS_MAXIT;
break;
}
/* Compute scalings */
if( updateScalings(w->C, w->s, w->z, w->lambda) == OUTSIDE_CONE ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nSlacks or multipliers leaving the positive orthant (- numerics ?), exiting.\n");
#endif
return ECOS_OUTCONE;
}
/* Update KKT matrix with scalings */
kkt_update(w->KKT->PKPt, w->KKT->PK, w->C);
#if DEBUG > 0
dumpSparseMatrix(w->KKT->PKPt,"PKPt_updated.txt");
#endif
/* factor KKT matrix */
#if PROFILING > 1
tic(&tfactor);
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta, &w->info->tfactor_t1, &w->info->tfactor_t2);
w->info->tfactor += toc(&tfactor);
#else
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta);
#endif
/* Solve for RHS1, which is used later also in combined direction */
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref1 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS1, w->KKT->dx1, w->KKT->dy1, w->KKT->dz1, w->n, w->p, w->m, w->C, 0, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
/* AFFINE SEARCH DIRECTION (predictor, need dsaff and dzaff only) */
RHS_affine(w);
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref2 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS2, w->KKT->dx2, w->KKT->dy2, w->KKT->dz2, w->n, w->p, w->m, w->C, 0, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
/* dtau_denom = kap/tau - (c'*x1 + by1 + h'*z1); */
dtau_denom = w->kap/w->tau - ddot(w->n, w->c, w->KKT->dx1) - ddot(w->p, w->b, w->KKT->dy1) - ddot(w->m, w->h, w->KKT->dz1);
/* dtauaff = (dt + c'*x2 + by2 + h'*z2) / dtau_denom; */
dtauaff = (w->rt - w->kap + ddot(w->n, w->c, w->KKT->dx2) + ddot(w->p, w->b, w->KKT->dy2) + ddot(w->m, w->h, w->KKT->dz2)) / dtau_denom;
/* dzaff = dz2 + dtau_aff*dz1 */
for( i=0; i<w->m; i++ ){ w->W_times_dzaff[i] = w->KKT->dz2[i] + dtauaff*w->KKT->dz1[i]; }
scale(w->W_times_dzaff, w->C, w->W_times_dzaff);
/* W\dsaff = -W*dzaff -lambda; */
for( i=0; i<w->m; i++ ){ w->dsaff_by_W[i] = -w->W_times_dzaff[i] - w->lambda[i]; }
/* dkapaff = -(bkap + kap*dtauaff)/tau; bkap = kap*tau*/
dkapaff = -w->kap - w->kap/w->tau*dtauaff;
/* Line search on W\dsaff and W*dzaff */
w->info->step_aff = lineSearch(w->lambda, w->dsaff_by_W, w->W_times_dzaff, w->tau, dtauaff, w->kap, dkapaff, w->C, w->KKT);
/* Centering parameter */
sigma = 1.0 - w->info->step_aff;
sigma = sigma*sigma*sigma;
if( sigma > SIGMAMAX ) sigma = SIGMAMAX;
if( sigma < SIGMAMIN ) sigma = SIGMAMIN;
w->info->sigma = sigma;
/* COMBINED SEARCH DIRECTION */
RHS_combined(w);
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref3 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS2, w->KKT->dx2, w->KKT->dy2, w->KKT->dz2, w->n, w->p, w->m, w->C, 0, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
/* bkap = kap*tau + dkapaff*dtauaff - sigma*info.mu; */
bkap = w->kap*w->tau + dkapaff*dtauaff - sigma*w->info->mu;
/* dtau = ((1-sigma)*rt - bkap/tau + c'*x2 + by2 + h'*z2) / dtau_denom; */
dtau = ((1-sigma)*w->rt - bkap/w->tau + ddot(w->n, w->c, w->KKT->dx2) + ddot(w->p, w->b, w->KKT->dy2) + ddot(w->m, w->h, w->KKT->dz2)) / dtau_denom;
/* dx = x2 + dtau*x1; dy = y2 + dtau*y1; dz = z2 + dtau*z1; */
for( i=0; i < w->n; i++ ){ w->KKT->dx2[i] += dtau*w->KKT->dx1[i]; }
for( i=0; i < w->p; i++ ){ w->KKT->dy2[i] += dtau*w->KKT->dy1[i]; }
for( i=0; i < w->m; i++ ){ w->KKT->dz2[i] += dtau*w->KKT->dz1[i]; }
/* ds_by_W = -(lambda \ bs + conelp_timesW(scaling,dz,dims)); */
/* note that ath this point w->dsaff_by_W holds already (lambda \ ds) */
scale(w->KKT->dz2, w->C, w->W_times_dzaff);
for( i=0; i < w->m; i++ ){ w->dsaff_by_W[i] = -(w->dsaff_by_W[i] + w->W_times_dzaff[i]); }
/* dkap = -(bkap + kap*dtau)/tau; */
dkap = -(bkap + w->kap*dtau)/w->tau;
/* Line search on combined direction */
w->info->step = lineSearch(w->lambda, w->dsaff_by_W, w->W_times_dzaff, w->tau, dtau, w->kap, dkap, w->C, w->KKT) * w->stgs->gamma;
/* ds = W*ds_by_W */
scale(w->dsaff_by_W, w->C, w->dsaff);
/* Update variables */
for( i=0; i < w->n; i++ ){ w->x[i] += w->info->step * w->KKT->dx2[i]; }
for( i=0; i < w->p; i++ ){ w->y[i] += w->info->step * w->KKT->dy2[i]; }
for( i=0; i < w->m; i++ ){ w->z[i] += w->info->step * w->KKT->dz2[i]; }
for( i=0; i < w->m; i++ ){ w->s[i] += w->info->step * w->dsaff[i]; }
w->kap += w->info->step * dkap;
w->tau += w->info->step * dtau;
}
/* scale variables back */
backscale(w);
/* stop timer */
#if PROFILING > 0
w->info->tsolve = toc(&tsolve);
#endif
#if PRINTLEVEL > 0
#if PROFILING > 0
if( w->stgs->verbose ) PRINTTEXT("\nRuntime: %f seconds.", w->info->tsetup + w->info->tsolve);
#endif
if( w->stgs->verbose ) PRINTTEXT("\n\n");
#endif
return exitcode;
}
/*
* Initializes the solver.
*
* This function assumes that the KKT matrix K is already in the form
*
* [0 A' G']
* K = [A 0 0 ]
* [G 0 -I ]
*
* The preprocessor/codegen takes care of this, we just have to be aware
* of this implicit assumption.
*/
idxint init(pwork* w)
{
idxint i, KKT_FACTOR_RETURN_CODE;
idxint* Pinv = w->KKT->Pinv;
#if PROFILING > 1
timer tfactor, tkktsolve;
#endif
w->KKT->delta = w->stgs->delta;
/* Factor KKT matrix - this is needed in all 3 linear system solves */
#if PROFILING > 1
tic(&tfactor);
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta, &w->info->tfactor_t1, &w->info->tfactor_t2);
w->info->tfactor += toc(&tfactor);
#else
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta);
#endif
/* check if factorization was successful, exit otherwise */
if( KKT_FACTOR_RETURN_CODE != KKT_OK ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nProblem in factoring KKT system, aborting.");
#endif
return ECOS_FATAL;
}
/*
* PRIMAL VARIABLES:
* - solve xhat = arg min ||Gx-h||_2^2 such that Ax = b
* - r = h - G*xhat
* These two equations are solved by
*
* [ 0 A' G' ] [ xhat ] [ 0 ]
* [ A 0 0 ] [ y ] = [ b ]
* [ G 0 -I ] [ -r ] [ h ]
*
* and then take shat = r if alphap < 0, zbar + (1+alphap)*e otherwise
* where alphap = inf{ alpha | sbar + alpha*e >= 0 }
*/
/* Solve for RHS [0; b; h] */
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref1 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS1, w->KKT->dx1, w->KKT->dy1, w->KKT->dz1, w->n, w->p, w->m, w->C, 1, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
#if DEBUG > 0
#if PRINTLEVEL > 3
printDenseMatrix(w->KKT->dx1, w->n, 1, "dx1_init");
printDenseMatrix(w->KKT->dy1, w->p, 1, "dy1_init");
printDenseMatrix(w->KKT->dz1, w->m, 1, "dz1_init");
#endif
#endif
/* Copy out initial value of x */
for( i=0; i<w->n; i++ ){ w->x[i] = w->KKT->dx1[i]; }
/* Copy out -r into temporary variable */
for( i=0; i<w->m; i++ ){ w->KKT->work1[i] = -w->KKT->dz1[i]; }
/* Bring variable to cone */
bring2cone(w->C, w->KKT->work1, w->s );
/*
* dual variables
* solve (yhat,zbar) = arg min ||z||_2^2 such that G'*z + A'*y + c = 0
*
* we can solve this by
*
* [ 0 A' G' ] [ x ] [ -c ]
* [ A 0 0 ] [ yhat ] = [ 0 ]
* [ G 0 -I ] [ zbar ] [ 0 ]
*
* and then take zhat = zbar if alphad < 0, zbar + (1+alphad)*e otherwise
* where alphad = inf{ alpha | zbar + alpha*e >= 0 }
*/
/* Solve for RHS [-c; 0; 0] */
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref2 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS2, w->KKT->dx2, w->KKT->dy2, w->KKT->dz2, w->n, w->p, w->m, w->C, 1, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
#if DEBUG > 0
#if PRINTLEVEL > 3
printDenseMatrix(w->KKT->dx2, w->n, 1, "dx2_init");
printDenseMatrix(w->KKT->dy2, w->p, 1, "dy2_init");
printDenseMatrix(w->KKT->dz2, w->m, 1, "dz2_init");
#endif
#endif
/* Copy out initial value of y */
for( i=0; i<w->p; i++ ){ w->y[i] = w->KKT->dy2[i]; }
/* Bring variable to cone */
bring2cone(w->C, w->KKT->dz2, w->z );
/* Prepare RHS1 - before this line RHS1 = [0; b; h], after it holds [-c; b; h] */
for( i=0; i<w->n; i++){ w->KKT->RHS1[Pinv[i]] = -w->c[i]; }
/*
* other variables
*/
w->kap = 1.0;
w->tau = 1.0;
w->info->step = 0;
w->info->step_aff = 0;
w->info->dinf = 0;
w->info->pinf = 0;
return 0;
}
/*
* Main solver routine.
*/
idxint ECOS_solve(pwork* w)
{
idxint i, initcode, KKT_FACTOR_RETURN_CODE;
pfloat dtau_denom, dtauaff, dkapaff, sigma, dtau, dkap, bkap, pres_prev;
idxint exitcode = ECOS_FATAL, interrupted;
#if DEBUG
char fn[20];
#endif
#if (defined _WIN32 || defined _WIN64 )
/* sets width of exponent for floating point numbers to 2 instead of 3 */
unsigned int old_output_format = _set_output_format(_TWO_DIGIT_EXPONENT);
#endif
#if PROFILING > 0
timer tsolve;
#endif
#if PROFILING > 1
timer tfactor, tkktsolve;
#endif
#if PROFILING > 0
/* start timer */
tic(&tsolve);
#endif
/* initialize ctrl-c support */
init_ctrlc();
/* Initialize solver */
initcode = init(w);
if( initcode == ECOS_FATAL ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nFatal error during initialization, aborting.");
#endif
return ECOS_FATAL;
}
/* MAIN INTERIOR POINT LOOP ---------------------------------------------------------------------- */
for( w->info->iter = 0; w->info->iter <= w->stgs->maxit ; w->info->iter++ ){
/* Compute residuals */
computeResiduals(w);
/* Update statistics */
updateStatistics(w);
#if PRINTLEVEL > 1
/* Print info */
if( w->stgs->verbose ) printProgress(w->info);
#endif
/* SAFEGUARD: Backtrack to best previously seen iterate if
*
* - the update was bad such that the primal residual PRES has increased by a factor of SAFEGUARD, or
* - the gap became negative
*
* If the safeguard is activated, the solver tests if reduced precision has been reached, and reports
* accordingly. If not even reduced precision is reached, ECOS returns the flag ECOS_NUMERICS.
*/
if( w->info->iter > 0 && (w->info->pres > SAFEGUARD*pres_prev || w->info->gap < 0) ){
#if PRINTLEVEL > 1
if( w->stgs->verbose ) deleteLastProgressLine( w->info );
if( w->stgs->verbose ) PRINTTEXT("Unreliable search direction detected, recovering best iterate (%d) and stopping.\n", (int)w->best_info->iter);
#endif
restoreBestIterate( w );
/* Determine whether we have reached at least reduced accuracy */
exitcode = checkExitConditions( w, ECOS_INACC_OFFSET );
/* if not, exit anyways */
if( exitcode == ECOS_NOT_CONVERGED_YET ){
exitcode = ECOS_NUMERICS;
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nNUMERICAL PROBLEMS (reached feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", MAX(w->info->dres, w->info->pres), w->info->relgap, w->info->gap);
#endif
break;
} else {
break;
}
}
pres_prev = w->info->pres;
/* Check termination criteria to full precision and exit if necessary */
exitcode = checkExitConditions( w, 0 );
interrupted = check_ctrlc();
if( exitcode == ECOS_NOT_CONVERGED_YET ){
/*
* Full precision has not been reached yet. Check for two more cases of exit:
* (i) min step size, in which case we assume we won't make progress any more, and
* (ii) maximum number of iterations reached
* If these two are not fulfilled, another iteration will be made.
*/
/* Did the line search c**k up? (zero step length) */
if( w->info->iter > 0 && w->info->step == STEPMIN*GAMMA ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) deleteLastProgressLine( w->info );
if( w->stgs->verbose ) PRINTTEXT("No further progress possible, recovering best iterate (%d) and stopping.", (int)w->best_info->iter );
#endif
restoreBestIterate( w );
/* Determine whether we have reached reduced precision */
exitcode = checkExitConditions( w, ECOS_INACC_OFFSET );
if( exitcode == ECOS_NOT_CONVERGED_YET ){
exitcode = ECOS_NUMERICS;
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nNUMERICAL PROBLEMS (reached feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", MAX(w->info->dres, w->info->pres), w->info->relgap, w->info->gap);
#endif
}
break;
}
/* MAXIT reached? */
else if( interrupted || w->info->iter == w->stgs->maxit ){
#if PRINTLEVEL > 0
const char *what = interrupted ? "SIGINT intercepted" : "Maximum number of iterations reached";
#endif
/* Determine whether current iterate is better than what we had so far */
if( compareStatistics( w->info, w->best_info) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose )
PRINTTEXT("%s, stopping.\n",what);
#endif
} else
{
#if PRINTLEVEL > 0
if( w->stgs->verbose )
PRINTTEXT("%s, recovering best iterate (%d) and stopping.\n", what, (int)w->best_info->iter);
#endif
restoreBestIterate( w );
}
/* Determine whether we have reached reduced precision */
exitcode = checkExitConditions( w, ECOS_INACC_OFFSET );
if( exitcode == ECOS_NOT_CONVERGED_YET ){
exitcode = interrupted ? ECOS_SIGINT : ECOS_MAXIT;
#if PRINTLEVEL > 0
if( w->stgs->verbose ) {
const char* what = interrupted ? "INTERRUPTED" : "RAN OUT OF ITERATIONS";
PRINTTEXT("\n%s (reached feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", what, MAX(w->info->dres, w->info->pres), w->info->relgap, w->info->gap);
}
#endif
}
break;
}
} else {
/* Full precision has been reached, stop solver */
break;
}
/* SAFEGUARD:
* Check whether current iterate is worth keeping as the best solution so far,
* before doing another iteration
*/
if (w->info->iter == 0) {
/* we're at the first iterate, so there's nothing to compare yet */
saveIterateAsBest( w );
} else if( compareStatistics( w->info, w->best_info) ){
/* PRINTTEXT("Better solution found, saving as best so far \n"); */
saveIterateAsBest( w );
}
/* Compute scalings */
if( updateScalings(w->C, w->s, w->z, w->lambda) == OUTSIDE_CONE ){
/* SAFEGUARD: we have to recover here */
#if PRINTLEVEL > 0
if( w->stgs->verbose ) deleteLastProgressLine( w->info );
if( w->stgs->verbose ) PRINTTEXT("Slacks/multipliers leaving the cone, recovering best iterate (%d) and stopping.\n", (int)w->best_info->iter);
#endif
restoreBestIterate( w );
/* Determine whether we have reached at least reduced accuracy */
exitcode = checkExitConditions( w, ECOS_INACC_OFFSET );
if( exitcode == ECOS_NOT_CONVERGED_YET ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) PRINTTEXT("\nNUMERICAL PROBLEMS (reached feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", MAX(w->info->dres, w->info->pres), w->info->relgap, w->info->gap);
#endif
return ECOS_OUTCONE;
} else {
break;
}
}
/* Update KKT matrix with scalings */
kkt_update(w->KKT->PKPt, w->KKT->PK, w->C);
#if DEBUG > 0
/* DEBUG: Store matrix to be factored */
sprintf(fn, "PKPt_updated_%02i.txt", (int)w->info->iter);
dumpSparseMatrix(w->KKT->PKPt, fn);
#endif
/* factor KKT matrix */
#if PROFILING > 1
tic(&tfactor);
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta, &w->info->tfactor_t1, &w->info->tfactor_t2);
w->info->tfactor += toc(&tfactor);
#else
KKT_FACTOR_RETURN_CODE = kkt_factor(w->KKT, w->stgs->eps, w->stgs->delta);
#endif
#if DEBUG > 0
/* DEBUG: store factor */
sprintf(fn, "PKPt_factor_%02i.txt", (int)w->info->iter);
dumpSparseMatrix(w->KKT->L, fn);
#endif
/* Solve for RHS1, which is used later also in combined direction */
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref1 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS1, w->KKT->dx1, w->KKT->dy1, w->KKT->dz1, w->n, w->p, w->m, w->C, 0, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
#if DEBUG > 0 && PRINTLEVEL > 2
/* Print result of linear system solve */
printDenseMatrix(w->KKT->dx1, 1, 5, "dx1(1:5)");
printDenseMatrix(w->KKT->dy1, 1, 5, "dy1(1:5)");
printDenseMatrix(w->KKT->dz1, 1, 5, "dz1(1:5)");
#endif
/* AFFINE SEARCH DIRECTION (predictor, need dsaff and dzaff only) */
RHS_affine(w);
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref2 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS2, w->KKT->dx2, w->KKT->dy2, w->KKT->dz2, w->n, w->p, w->m, w->C, 0, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
/* dtau_denom = kap/tau - (c'*x1 + by1 + h'*z1); */
dtau_denom = w->kap/w->tau - eddot(w->n, w->c, w->KKT->dx1) - eddot(w->p, w->b, w->KKT->dy1) - eddot(w->m, w->h, w->KKT->dz1);
/* dtauaff = (dt + c'*x2 + by2 + h'*z2) / dtau_denom; */
dtauaff = (w->rt - w->kap + eddot(w->n, w->c, w->KKT->dx2) + eddot(w->p, w->b, w->KKT->dy2) + eddot(w->m, w->h, w->KKT->dz2)) / dtau_denom;
/* dzaff = dz2 + dtau_aff*dz1 */
for( i=0; i<w->m; i++ ){ w->W_times_dzaff[i] = w->KKT->dz2[i] + dtauaff*w->KKT->dz1[i]; }
scale(w->W_times_dzaff, w->C, w->W_times_dzaff);
/* W\dsaff = -W*dzaff -lambda; */
for( i=0; i<w->m; i++ ){ w->dsaff_by_W[i] = -w->W_times_dzaff[i] - w->lambda[i]; }
/* dkapaff = -(bkap + kap*dtauaff)/tau; bkap = kap*tau*/
dkapaff = -w->kap - w->kap/w->tau*dtauaff;
/* Line search on W\dsaff and W*dzaff */
w->info->step_aff = lineSearch(w->lambda, w->dsaff_by_W, w->W_times_dzaff, w->tau, dtauaff, w->kap, dkapaff, w->C, w->KKT);
/* Centering parameter */
sigma = 1.0 - w->info->step_aff;
sigma = sigma*sigma*sigma;
if( sigma > SIGMAMAX ) sigma = SIGMAMAX;
if( sigma < SIGMAMIN ) sigma = SIGMAMIN;
w->info->sigma = sigma;
/* COMBINED SEARCH DIRECTION */
RHS_combined(w);
#if PROFILING > 1
tic(&tkktsolve);
#endif
w->info->nitref3 = kkt_solve(w->KKT, w->A, w->G, w->KKT->RHS2, w->KKT->dx2, w->KKT->dy2, w->KKT->dz2, w->n, w->p, w->m, w->C, 0, w->stgs->nitref);
#if PROFILING > 1
w->info->tkktsolve += toc(&tkktsolve);
#endif
/* bkap = kap*tau + dkapaff*dtauaff - sigma*info.mu; */
bkap = w->kap*w->tau + dkapaff*dtauaff - sigma*w->info->mu;
/* dtau = ((1-sigma)*rt - bkap/tau + c'*x2 + by2 + h'*z2) / dtau_denom; */
dtau = ((1-sigma)*w->rt - bkap/w->tau + eddot(w->n, w->c, w->KKT->dx2) + eddot(w->p, w->b, w->KKT->dy2) + eddot(w->m, w->h, w->KKT->dz2)) / dtau_denom;
/* dx = x2 + dtau*x1; dy = y2 + dtau*y1; dz = z2 + dtau*z1; */
for( i=0; i < w->n; i++ ){ w->KKT->dx2[i] += dtau*w->KKT->dx1[i]; }
for( i=0; i < w->p; i++ ){ w->KKT->dy2[i] += dtau*w->KKT->dy1[i]; }
for( i=0; i < w->m; i++ ){ w->KKT->dz2[i] += dtau*w->KKT->dz1[i]; }
/* ds_by_W = -(lambda \ bs + conelp_timesW(scaling,dz,dims)); */
/* note that ath this point w->dsaff_by_W holds already (lambda \ ds) */
scale(w->KKT->dz2, w->C, w->W_times_dzaff);
for( i=0; i < w->m; i++ ){ w->dsaff_by_W[i] = -(w->dsaff_by_W[i] + w->W_times_dzaff[i]); }
/* dkap = -(bkap + kap*dtau)/tau; */
dkap = -(bkap + w->kap*dtau)/w->tau;
/* Line search on combined direction */
w->info->step = lineSearch(w->lambda, w->dsaff_by_W, w->W_times_dzaff, w->tau, dtau, w->kap, dkap, w->C, w->KKT) * w->stgs->gamma;
/* ds = W*ds_by_W */
scale(w->dsaff_by_W, w->C, w->dsaff);
/* Update variables */
for( i=0; i < w->n; i++ ){ w->x[i] += w->info->step * w->KKT->dx2[i]; }
for( i=0; i < w->p; i++ ){ w->y[i] += w->info->step * w->KKT->dy2[i]; }
for( i=0; i < w->m; i++ ){ w->z[i] += w->info->step * w->KKT->dz2[i]; }
for( i=0; i < w->m; i++ ){ w->s[i] += w->info->step * w->dsaff[i]; }
w->kap += w->info->step * dkap;
w->tau += w->info->step * dtau;
}
/* scale variables back */
backscale(w);
/* stop timer */
#if PROFILING > 0
w->info->tsolve = toc(&tsolve);
#endif
#if PRINTLEVEL > 0
#if PROFILING > 0
if( w->stgs->verbose ) PRINTTEXT("\nRuntime: %f seconds.", w->info->tsetup + w->info->tsolve);
#endif
if( w->stgs->verbose ) PRINTTEXT("\n\n");
#endif
remove_ctrlc();
return exitcode;
}
void print_progress(ecos_bb_pwork* prob){
PRINTTEXT("%u \t%.2f \t\t%.2f \t\t%.2f\n", (int)prob->iter, prob->global_L, prob->global_U, prob->global_U-prob->global_L);
}
/**
* Solves the permuted KKT system and returns the unpermuted search directions.
*
* On entry, the factorization of the permuted KKT matrix, PKPt,
* is assumed to be up to date (call kkt_factor beforehand to achieve this).
* The right hand side, Pb, is assumed to be already permuted.
*
* On exit, the resulting search directions are written into dx, dy and dz,
* where these variables are permuted back to the original ordering.
*
* KKT->nitref iterative refinement steps are applied to solve the linear system.
*
* Returns the number of iterative refinement steps really taken.
*/
idxint kkt_solve(kkt* KKT, spmat* A, spmat* G, pfloat* Pb, pfloat* dx, pfloat* dy, pfloat* dz, idxint n, idxint p, idxint m, cone* C, idxint isinit, idxint nitref)
{
#if CONEMODE == 0
#define MTILDE (m+2*C->nsoc)
#else
#define MTILDE (m)
#endif
idxint i, k, l, j, kk, kItRef;
#if (defined STATICREG) && (STATICREG > 0)
idxint dzoffset;
#endif
idxint* Pinv = KKT->Pinv;
pfloat* Px = KKT->work1;
pfloat* dPx = KKT->work2;
pfloat* e = KKT->work3;
pfloat* Pe = KKT->work4;
pfloat* truez = KKT->work5;
pfloat* Gdx = KKT->work6;
pfloat* ex = e;
pfloat* ey = e + n;
pfloat* ez = e + n+p;
pfloat bnorm = 1.0 + norminf(Pb, n+p+MTILDE);
pfloat nex = 0;
pfloat ney = 0;
pfloat nez = 0;
pfloat nerr;
pfloat nerr_prev;
pfloat error_threshold = bnorm*LINSYSACC;
idxint nK = KKT->PKPt->n;
/* forward - diagonal - backward solves: Px holds solution */
LDL_lsolve2(nK, Pb, KKT->L->jc, KKT->L->ir, KKT->L->pr, Px );
LDL_dsolve(nK, Px, KKT->D);
LDL_ltsolve(nK, Px, KKT->L->jc, KKT->L->ir, KKT->L->pr);
#if PRINTLEVEL > 2
if( p > 0 ){
PRINTTEXT("\nIR: it ||ex|| ||ey|| ||ez|| (threshold: %4.2e)\n", error_threshold);
PRINTTEXT(" --------------------------------------------------\n");
} else {
PRINTTEXT("\nIR: it ||ex|| ||ez|| (threshold: %4.2e)\n", error_threshold);
PRINTTEXT(" -----------------------------------------\n");
}
#endif
/* iterative refinement */
for( kItRef=0; kItRef <= nitref; kItRef++ ){
/* unpermute x & copy into arrays */
unstretch(n, p, C, Pinv, Px, dx, dy, dz);
/* compute error term */
k=0; j=0;
/* 1. error on dx*/
#if (defined STATICREG) && (STATICREG > 0)
/* ex = bx - A'*dy - G'*dz - DELTASTAT*dx */
for( i=0; i<n; i++ ){ ex[i] = Pb[Pinv[k++]] - DELTASTAT*dx[i]; }
#else
/* ex = bx - A'*dy - G'*dz */
for( i=0; i<n; i++ ){ ex[i] = Pb[Pinv[k++]]; }
#endif
if(A) sparseMtVm(A, dy, ex, 0, 0);
sparseMtVm(G, dz, ex, 0, 0);
nex = norminf(ex,n);
/* error on dy */
if( p > 0 ){
#if (defined STATICREG) && (STATICREG > 0)
/* ey = by - A*dx + DELTASTAT*dy */
for( i=0; i<p; i++ ){ ey[i] = Pb[Pinv[k++]] + DELTASTAT*dy[i]; }
#else
/* ey = by - A*dx */
for( i=0; i<p; i++ ){ ey[i] = Pb[Pinv[k++]]; }
#endif
sparseMV(A, dx, ey, -1, 0);
ney = norminf(ey,p);
}
/* --> 3. ez = bz - G*dx + V*dz_true */
kk = 0; j=0;
#if (defined STATICREG) && (STATICREG > 0)
dzoffset=0;
#endif
sparseMV(G, dx, Gdx, 1, 1);
for( i=0; i<C->lpc->p; i++ ){
#if (defined STATICREG) && (STATICREG > 0)
ez[kk++] = Pb[Pinv[k++]] - Gdx[j++] + DELTASTAT*dz[dzoffset++];
#else
ez[kk++] = Pb[Pinv[k++]] - Gdx[j++];
#endif
}
for( l=0; l<C->nsoc; l++ ){
for( i=0; i<C->soc[l].p; i++ ){
#if (defined STATICREG) && (STATICREG > 0)
ez[kk++] = i<(C->soc[l].p-1) ? Pb[Pinv[k++]] - Gdx[j++] + DELTASTAT*dz[dzoffset++] : Pb[Pinv[k++]] - Gdx[j++] - DELTASTAT*dz[dzoffset++];
#else
ez[kk++] = Pb[Pinv[k++]] - Gdx[j++];
#endif
}
#if CONEMODE == 0
ez[kk] = 0;
ez[kk+1] = 0;
k += 2;
kk += 2;
#endif
}
for( i=0; i<MTILDE; i++) { truez[i] = Px[Pinv[n+p+i]]; }
if( isinit == 0 ){
scale2add(truez, ez, C);
} else {
vadd(MTILDE, truez, ez);
}
nez = norminf(ez,MTILDE);
#if PRINTLEVEL > 2
if( p > 0 ){
PRINTTEXT(" %2d %3.1e %3.1e %3.1e\n", (int)kItRef, nex, ney, nez);
} else {
PRINTTEXT(" %2d %3.1e %3.1e\n", (int)kItRef, nex, nez);
}
#endif
/* maximum error (infinity norm of e) */
nerr = MAX( nex, nez);
if( p > 0 ){ nerr = MAX( nerr, ney ); }
/* CHECK WHETHER REFINEMENT BROUGHT DECREASE - if not undo and quit! */
if( kItRef > 0 && nerr > nerr_prev ){
/* undo refinement */
for( i=0; i<nK; i++ ){ Px[i] -= dPx[i]; }
kItRef--;
break;
}
/* CHECK WHETHER TO REFINE AGAIN */
if( kItRef == nitref || ( nerr < error_threshold ) || ( kItRef > 0 && nerr_prev < IRERRFACT*nerr ) ){
break;
}
nerr_prev = nerr;
/* permute */
for( i=0; i<nK; i++) { Pe[Pinv[i]] = e[i]; }
/* forward - diagonal - backward solves: dPx holds solution */
LDL_lsolve2(nK, Pe, KKT->L->jc, KKT->L->ir, KKT->L->pr, dPx);
LDL_dsolve(nK, dPx, KKT->D);
LDL_ltsolve(nK, dPx, KKT->L->jc, KKT->L->ir, KKT->L->pr);
/* add refinement to Px */
for( i=0; i<nK; i++ ){ Px[i] += dPx[i]; }
}
#if PRINTLEVEL > 2
PRINTTEXT("\n");
#endif
/* copy solution out into the different arrays, permutation included */
unstretch(n, p, C, Pinv, Px, dx, dy, dz);
return kItRef;
}
void print_stats(ecos_bb_pwork* prob){
PRINTTEXT("\tPcost: %.2f \n", prob->info->pcost);
PRINTTEXT("\tDcost: %.2f \n", prob->info->dcost);
PRINTTEXT("\tE_Pcost: %.2f \n", prob->ecos_prob->info->pcost);
PRINTTEXT("\tE_Dcost: %.2f \n", prob->ecos_prob->info->dcost);
}
int makeOptStep(emb_optimizer optim, emb_opt_context opt_context, const double xcur[], const double uprev[], double ucur[])
{
emb_size_type nx = getOptModelStateCount(optim);
emb_size_type nu = getOptModelInputCount(optim);
int i, j, status;
static unsigned firstRun = 1;
double feedback[ ACADO_NX ];
/* Fill the reference for states */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NX; ++j)
acadoVariables.xRef[i * ACADO_NX + j] = xref[ j ];
/* Fill the reference for controls -- always zero :) */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.uRef[i * ACADO_NU + j] = 0.0;
if ( firstRun )
{
/* Print header */
PRINTTEXT( "\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
"Copyright (C) 2008-2011 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.\n"
"Developed within the Optimization in Engineering Center (OPTEC) under\n"
"supervision of Moritz Diehl. All rights reserved.\n\n"
"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
"General Public License 3 in the hope that it will be useful,\n"
"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
"GNU Lesser General Public License for more details.\n\n" );
/* Initialize the shooting nodes */
for (i = 0; i < ACADO_N + 1; ++i)
{
acadoVariables.x[i * ACADO_NX + 0] = 0.0;
acadoVariables.x[i * ACADO_NX + 1] = 0.0;
acadoVariables.x[i * ACADO_NX + 2] = 0.75;
acadoVariables.x[i * ACADO_NX + 3] = 0.0;
acadoVariables.x[i * ACADO_NX + 4] = 0.0;
acadoVariables.x[i * ACADO_NX + 5] = 3.14159265359 / 4.0;
acadoVariables.x[i * ACADO_NX + 6] = 0.0;
acadoVariables.x[i * ACADO_NX + 7] = 0.0;
}
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.u[i * ACADO_NU + j] = 0.0;
firstRun = 0;
}
else
{
/* Shift the states and the controls */
shiftStates( 0 );
shiftControls( 0 );
}
/* Execute the preparation phase */
preparationStep();
/* Prepare the feedback signal */
feedback[ 0 ] = xcur[ 0 ];
feedback[ 1 ] = xcur[ 2 ];
feedback[ 2 ] = xcur[ 1 ];
feedback[ 3 ] = xcur[ 3 ];
feedback[ 4 ] = xcur[ 4 ];
feedback[ 5 ] = xcur[ 5 ];
feedback[ 6 ] = xcur[ 6 ];
feedback[ 7 ] = xcur[ 7 ];
/* Execute the feedback phase */
status = feedbackStep( feedback );
PRINTTEXT("KKT tolerance: %1.5e\n", getKKT());
if ( status )
{
PRINTTEXT("Problem in the QP solver detected. QP error code is: %d\n", status);
return 1;
}
/* Assign to input */
for( i = 0; i < ACADO_NU; i++ ) {
ucur[ i ] = acadoVariables.u[ i ];
}
return 0;
}
void print_ecos_c(ecos_bb_pwork* prob){
int i; PRINTTEXT("ecos->c: ");
for (i=0; i<prob->ecos_prob->n; ++i) PRINTTEXT("%.2f ", prob->ecos_prob->c[i] );
PRINTTEXT("\n");
}
/*
* This function is reponsible for checking the exit/convergence conditions of ECOS.
* If one of the exit conditions is met, ECOS displays an exit message and returns
* the corresponding exit code. The calling function must then make sure that ECOS
* is indeed correctly exited, so a call to this function should always be followed
* by a break statement.
*
* If mode == 0, normal precisions are checked.
*
* If mode != 0, reduced precisions are checked, and the exit display is augmented
* by "Close to". The exitcodes returned are increased by the value
* of mode.
*
* The primal and dual infeasibility flags w->info->pinf and w->info->dinf are raised
* according to the outcome of the test.
*
* If none of the exit tests are met, the function returns ECOS_NOT_CONVERGED_YET.
* This should not be an exitflag that is ever returned to the outside world.
*/
idxint checkExitConditions(pwork* w, idxint mode)
{
pfloat feastol;
pfloat abstol;
pfloat reltol;
/* set accuracy against which to check */
if( mode == 0) {
/* check convergence against normal precisions */
feastol = w->stgs->feastol;
abstol = w->stgs->abstol;
reltol = w->stgs->reltol;
} else {
/* check convergence against reduced precisions */
feastol = w->stgs->feastol_inacc;
abstol = w->stgs->abstol_inacc;
reltol = w->stgs->reltol_inacc;
}
/* Optimal? */
if( ( -w->cx > 0 || -w->by - w->hz >= -abstol ) &&
( w->info->pres < feastol && w->info->dres < feastol ) &&
( w->info->gap < abstol || w->info->relgap < reltol ) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) {
if( mode == 0) {
PRINTTEXT("\nOPTIMAL (within feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", MAX(w->info->dres, w->info->pres), w->info->relgap, w->info->gap);
} else {
PRINTTEXT("\nClose to OPTIMAL (within feastol=%3.1e, reltol=%3.1e, abstol=%3.1e).", MAX(w->info->dres, w->info->pres), w->info->relgap, w->info->gap);
}
}
#endif
w->info->pinf = 0;
w->info->dinf = 0;
return ECOS_OPTIMAL + mode;
}
/* Dual infeasible? */
else if( (w->info->dinfres != NAN) && (w->info->dinfres < feastol) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) {
if( mode == 0) {
PRINTTEXT("\nUNBOUNDED (within feastol=%3.1e).", w->info->dinfres );
} else {
PRINTTEXT("\nClose to UNBOUNDED (within feastol=%3.1e).", w->info->dinfres );
}
}
#endif
w->info->pinf = 0;
w->info->dinf = 1;
return ECOS_DINF + mode;
}
/* Primal infeasible? */
else if( (w->info->pinfres != NAN && w->info->pinfres < feastol) ||
( w->tau < w->stgs->feastol && w->kap < w->stgs->feastol && w->info->pinfres < w->stgs->feastol) ){
#if PRINTLEVEL > 0
if( w->stgs->verbose ) {
if( mode == 0) {
PRINTTEXT("\nPRIMAL INFEASIBLE (within feastol=%3.1e).", w->info->pinfres );
} else {
PRINTTEXT("\nClose to PRIMAL INFEASIBLE (within feastol=%3.1e).", w->info->pinfres );
}
}
#endif
w->info->pinf = 1;
w->info->dinf = 0;
return ECOS_PINF + mode;
}
/* Indicate if none of the above criteria are met */
else {
return ECOS_NOT_CONVERGED_YET;
}
}
int makeOptStep(emb_optimizer optim, emb_opt_context opt_context, const double xcur[], const double uprev[], double ucur[])
{
emb_size_type nx = getOptModelStateCount(optim);
emb_size_type nu = getOptModelInputCount(optim);
int i, j, status;
static unsigned firstRun = 1;
double feedback[ ACADO_NX ];
/* Fill the reference for states */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NX; ++j)
acadoVariables.xRef[i * ACADO_NX + j] = xref[ j ];
/* Fill the reference for controls -- always zero :) */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.uRef[i * ACADO_NU + j] = 0.0;
if ( firstRun )
{
/* Initialize the shooting nodes */
for (i = 0; i < ACADO_N + 1; ++i)
{
/* Choose point for model linearization */
acadoVariables.x[i * ACADO_NX + 0] = 0.0;
acadoVariables.x[i * ACADO_NX + 1] = 0.0;
acadoVariables.x[i * ACADO_NX + 2] = 0.75;
acadoVariables.x[i * ACADO_NX + 3] = 0.0;
acadoVariables.x[i * ACADO_NX + 4] = 0.0;
acadoVariables.x[i * ACADO_NX + 5] = 0.0;
acadoVariables.x[i * ACADO_NX + 6] = 0.0;
acadoVariables.x[i * ACADO_NX + 7] = 0.0;
}
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.u[i * ACADO_NU + j] = 0.0;
firstRun = 0;
/* Linearize the model */
preparationStep();
}
/* Prepare the feedback signal */
feedback[ 0 ] = xcur[ 0 ];
feedback[ 1 ] = xcur[ 2 ];
feedback[ 2 ] = xcur[ 1 ];
feedback[ 3 ] = xcur[ 3 ];
feedback[ 4 ] = xcur[ 4 ];
feedback[ 5 ] = xcur[ 5 ];
feedback[ 6 ] = xcur[ 6 ];
feedback[ 7 ] = xcur[ 7 ];
/* Execute the feedback phase */
status = feedbackStep( feedback );
if ( status )
{
PRINTTEXT("qpOASES problem detected. Error code %d\n", status);
return 1;
}
/* Assign to input */
ucur[ 0 ] = acadoVariables.u[ 0 ];
ucur[ 1 ] = acadoVariables.u[ 1 ];
return 0;
}
void get_bounds(idxint node_idx, ecos_bb_pwork* prob){
idxint i, ret_code, branchable, viable_rounded_sol;
viable_rounded_sol = 0;
set_prob( prob, get_bool_node_id(node_idx,prob), get_int_node_id(node_idx, prob) );
ret_code = ECOS_solve(prob->ecos_prob);
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose){ print_ecos_solution(prob); }
if (ret_code != ECOS_OPTIMAL && ret_code != ECOS_PINF && ret_code != ECOS_DINF){
PRINTTEXT("1Exit code: %u\n", ret_code);
}
#endif
if (ret_code >= 0 ||
ret_code == ECOS_MAXIT ||
ret_code == ECOS_NUMERICS )
{
prob->nodes[node_idx].L = eddot(prob->ecos_prob->n, prob->ecos_prob->x, prob->ecos_prob->c);
/* Figure out if x is already an integer solution
if the solution had no numerical errors*/
branchable = 1;
for (i=0; i<prob->num_bool_vars; ++i){
prob->tmp_bool_node_id[i] = (char) pfloat_round( prob->ecos_prob->x[prob->bool_vars_idx[i]] );
branchable &= float_eqls( prob->ecos_prob->x[i] , (pfloat) prob->tmp_bool_node_id[i], prob->stgs->integer_tol );
}
for (i=0; i<prob->num_int_vars; ++i){
prob->tmp_int_node_id[2 * i + 1] = pfloat_round(prob->ecos_prob->x[prob->int_vars_idx[i]] );
prob->tmp_int_node_id[2*i] = -(prob->tmp_int_node_id[2*i + 1]);
branchable &= float_eqls( prob->ecos_prob->x[i] , prob->tmp_int_node_id[2*i + 1], prob->stgs->integer_tol );
}
branchable = !branchable;
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose){ PRINTTEXT("Is branchable: %u\n",branchable); }
#endif
if (branchable){ /* pfloat_round and check feasibility*/
get_branch_var(prob, &(prob->nodes[node_idx].split_idx), &(prob->nodes[node_idx].split_val) );
prob->nodes[node_idx].status = MI_SOLVED_BRANCHABLE;
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose){ print_node(prob,-1); PRINTTEXT("Rounded Solution:\n"); }
#endif
set_prob(prob, prob->tmp_bool_node_id, prob->tmp_int_node_id);
ret_code = ECOS_solve(prob->ecos_prob);
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose){ print_ecos_solution(prob); }
if (ret_code != ECOS_OPTIMAL && ret_code != ECOS_PINF && ret_code != ECOS_DINF){
PRINTTEXT("2Exit code: %u\n", ret_code);
}
#endif
if (ret_code == ECOS_OPTIMAL){
/* Use the node's U as tmp storage */
prob->nodes[node_idx].U = eddot(prob->ecos_prob->n, prob->ecos_prob->x, prob->ecos_prob->c);
viable_rounded_sol = 1;
}
}else{ /* This is already an integer solution*/
prob->nodes[node_idx].status = MI_SOLVED_NON_BRANCHABLE;
prob->nodes[node_idx].U = eddot(prob->ecos_prob->n, prob->ecos_prob->x, prob->ecos_prob->c);
}
if (prob->nodes[node_idx].U < prob->global_U){
#if MI_PRINTLEVEL > 1
if (prob->stgs->verbose){
PRINTTEXT("New optimal solution, U: %.2f\n", prob->nodes[node_idx].U);
print_ecos_xequil(prob);
print_ecos_c(prob);
print_ecos_solution(prob);
}
#endif
store_solution(prob);
prob->global_U = prob->nodes[node_idx].U;
}
if (viable_rounded_sol){
/* Reset the node's U back to INF because it was not originally feasible */
prob->nodes[node_idx].U = INFINITY;
}
}else { /*Assume node infeasible*/
prob->nodes[node_idx].L = INFINITY;
prob->nodes[node_idx].U = INFINITY;
prob->nodes[node_idx].status = MI_SOLVED_NON_BRANCHABLE;
}
}
