USING_NAMESPACE_QPOASES int sumOfSquares() { int i; /* sum of first n squares */ const int N = 100; real_t av[N]; real_t aT[N]; real_t bv[N]; real_t c; for (i = 0; i < N; i++) av[i] = i+1; for (i = 0; i < N; i++) aTv[i] = i+1; for (i = 0; i < N; i++) bv[i] = i+1; myStatic DenseMatrix a; DenseMatrixCON( &a, 1, N, N, av); myStatic DenseMatrix aT; DenseMatrixCON( &aT, N, 1, 1, aTv); a.times(1, 1.0, bv, N, 0.0, &c, 1); fprintf(stdFile, "Dot product; Error in sum of first %d squares: %9.2e\n", N, c - (1.0/6.0)*N*(N+1)*(2*N+1)); aT.transTimes(1, 1.0, bv, N, 0.0, &c, 1); fprintf(stdFile, "Transpose; Error in sum of first %d squares: %9.2e\n", N, c - (1.0/6.0)*N*(N+1)*(2*N+1)); return 0; }
/* * s e t u p C o n s t r a i n t M a t r i x */ returnValue setupConstraintMatrix( const mxArray* prhsA, int nV, int nC, DenseMatrix* A ) { real_t *A_for = 0; static real_t A_mem[NCMAX*NVMAX]; if ( prhsA == 0 ) return SUCCESSFUL_RETURN; if ( mxIsSparse( prhsA ) != 0 ) { myMexErrMsgTxt( "ERROR (qpOASES_e): Cannot handle sparse matrices!" ); return RET_INVALID_ARGUMENTS; } else { /* make a deep-copy in order to avoid modifying input data when regularising */ A_for = (real_t*) mxGetPr( prhsA ); convertFortranToC( A_for, nV,nC,A_mem ); DenseMatrixCON( A, nC,nV,nV, A_mem ); } return SUCCESSFUL_RETURN; }
/* * s e t u p H e s s i a n M a t r i x */ returnValue setupHessianMatrix( const mxArray* prhsH, int nV, DenseMatrix* H ) { real_t *H_for = 0; /*static real_t H_mem[NVMAX*NVMAX];*/ if ( prhsH == 0 ) return SUCCESSFUL_RETURN; if ( mxIsSparse( prhsH ) != 0 ) { myMexErrMsgTxt( "ERROR (qpOASES_e): Cannot handle sparse matrices!" ); return RET_INVALID_ARGUMENTS; } else { /* make a deep-copy in order to avoid modifying input data when regularising */ H_for = (real_t*) mxGetPr( prhsH ); /*qpOASES_printM( H_for,nV,nV );*/ /*convertFortranToC( H_for, nV,nV,H_mem ); not neccessary as H is symmetric! */ DenseMatrixCON( H, nV,nV,nV, H_for ); } return SUCCESSFUL_RETURN; }
int hilbert() { int i, j; real_t d, err; /* permuted 4x4 Hilbert matrix, row major format */ real_t _Av[] = {1.0/3.0, 1.0, 0.5, 0.25, 0.25, 0.5, 1.0/3.0, 0.2, 0.2, 1.0/3.0, 0.25, 1.0/6.0, 1.0/6.0, 0.25, 0.2, 1.0/7.0}; /* and its inverse, column major format */ real_t Bv[] = {240, 16, -120, -140, -2700, -120, 1200, 1680, 6480, 240, -2700, -4200, -4200, -140, 1680, 2800}; /* result */ real_t *Av = new real_t[4*4]; real_t *Cv = new real_t[4*4]; myStatic DenseMatrix A; DenseMatrixCON( &A, 4, 4, 4, Av ); for (i = 0; i < 16; i++) Av[i] = _Av[i]; A.times(4, 1.0, Bv, 4, 0.0, Cv, 4); err = 0.0; for (j = 0; j < 4; j++) { for (i = 0; i < 4; i++) { d = fabs(Cv[j*4+i] - static_cast<real_t>(i == j)); if (d > err) err = d; } } fprintf(stdFile, "Hilbert; Deviation from identity: %9.2e\n", err); delete[] Cv; A.free (); // or delete[] Av; return 0; }
int submatrix() { int i, j; real_t d, err; /* 2x3 transposed submatrix */ real_t _Asubv[] = {1.0/3.0, 0.25, 1.0, 0.5, 0.5, 1.0/3.0, 0.25, 0.2}; real_t Bsubv[] = {240, 16, -120, -140, -2700, -120, 1200, 1680}; real_t Csubv[2*2]; real_t *Asubv = new real_t[2*4]; for (i = 0; i < 8; i++) Asubv[i] = _Asubv[i]; myStatic DenseMatrix Asub; DenseMatrixCON( &Asub, 4, 2, 2, Asubv ); Asub.transTimes(2, 1.0, Bsubv, 4, 0.0, Csubv, 2); err = 0.0; for (j = 0; j < 2; j++) { for (i = 0; i < 2; i++) { d = fabs(Csubv[j*2+i] - static_cast<real_t>(i == j)); if (d > err) err = d; } } fprintf(stdFile, "Submatrix transpose; Deviation from identity: %9.2e\n", err); Asub.free (); // or delete[] Asubv; return 0; }
int indexDenseSubmatrix() { /* dense submatrices via index lists */ const int M = 20, N = 15, K = 5; int i, j, k, m, n; Indexlist rows(N), cols(N); int *rNum, *cNum; real_t err = 0.0; real_t *Av, *X, *Y, *x, *y, *xc, *yc; // prepare index lists m = (M-3)/4; n = (N-3)/4; for (i = 0; i < m; i++) { rows.addNumber(M-1 - 2*i); rows.addNumber(2*i); } for (i = 0; i < n; i++) { cols.addNumber(N-1 - 2*i); cols.addNumber(2*i); } m *= 2; n *= 2; rows.getNumberArray(&rNum); fprintf(stdFile, "Rows: "); for (i = 0; i < m; i++) fprintf(stdFile, " %2d", rNum[i]); fprintf(stdFile, "\n"); cols.getNumberArray(&cNum); fprintf(stdFile, "Cols: "); for (i = 0; i < n; i++) fprintf(stdFile, " %2d", cNum[i]); fprintf(stdFile, "\n"); // prepare input matrices Av = new real_t[M*N]; X = new real_t[n*K]; Y = new real_t[m*K]; x = new real_t[n*K]; y = new real_t[m*K]; xc = new real_t[n*K]; yc = new real_t[m*K]; myStatic DenseMatrix A; DenseMatrixCON( &A, M, N, N, Av ); for (i = 0; i < M*N; i++) Av[i] = -0.5*N*M + (real_t)i; for (i = 0; i < n*K; i++) X[i] = 1.0 / (real_t)(i+1); for (i = 0; i < m*K; i++) Y[i] = 1.0 / (real_t)(i+1); // multiply A.times(&rows, &cols, K, 1.0, X, n, 0.0, y, m); // check result for (j = 0; j < m; j++) { for (k = 0; k < K; k++) { yc[j+k*m] = -y[j+k*m]; for (i = 0; i < n; i++) yc[j+k*m] += Av[cNum[i]+rNum[j]*N] * X[i+k*n]; if (fabs(yc[j+k*m]) > err) err = fabs(yc[j+k*m]); } } fprintf(stdFile, "Indexlist submatrix; error: %9.2e\n", err); // transpose multiply A.transTimes(&rows, &cols, K, 1.0, Y, m, 0.0, x, n); // check result err = 0.0; for (j = 0; j < n; j++) { for (k = 0; k < K; k++) { xc[j+k*n] = -x[j+k*n]; for (i = 0; i < m; i++) xc[j+k*n] += Av[cNum[j]+rNum[i]*N] * Y[i+k*m]; if (fabs(xc[j+k*n]) > err) err = fabs(xc[j+k*n]); } } fprintf(stdFile, "Indexlist transpose submatrix; error: %9.2e\n", err); // check result // clean up delete[] yc; delete[] xc; delete[] y; delete[] x; delete[] Y; delete[] X; A.free (); // or delete[] Av; return 0; }
/* * s o l v e O Q P b e n c h m a r k */ returnValue solveOQPbenchmarkB( int nQP, int nV, real_t* _H, const real_t* const g, const real_t* const lb, const real_t* const ub, BooleanType isSparse, BooleanType useHotstarts, const Options* options, int maxAllowedNWSR, real_t* maxNWSR, real_t* avgNWSR, real_t* maxCPUtime, real_t* avgCPUtime, real_t* maxStationarity, real_t* maxFeasibility, real_t* maxComplementarity ) { int k; myStatic QProblemB qp; returnValue returnvalue; /* I) SETUP AUXILIARY VARIABLES: */ /* 1) Keep nWSR and store current and maximum number of * working set recalculations in temporary variables */ int nWSRcur; real_t CPUtimeLimit = *maxCPUtime; real_t CPUtimeCur = CPUtimeLimit; real_t stat, feas, cmpl; /* 2) Pointers to data of current QP ... */ const real_t* gCur; const real_t* lbCur; const real_t* ubCur; /* 3) Vectors for solution obtained by qpOASES. */ myStatic real_t x[NVMAX]; myStatic real_t y[NVMAX]; /* 4) Prepare matrix objects */ DenseMatrix *H; myStatic DenseMatrix HH; DenseMatrixCON( &HH, nV, nV, nV, _H ); H = &HH; *maxNWSR = 0; *avgNWSR = 0; *maxCPUtime = 0.0; *avgCPUtime = 0.0; *maxStationarity = 0.0; *maxFeasibility = 0.0; *maxComplementarity = 0.0; /* II) SETUP QPROBLEM OBJECT */ QProblemBCON( &qp,nV,HST_UNKNOWN ); QProblemB_setOptions( &qp,*options ); /*QProblemB_setPrintLevel( &qp,PL_LOW );*/ /* III) RUN BENCHMARK SEQUENCE: */ for( k=0; k<nQP; ++k ) { /* 1) Update pointers to current QP data. */ gCur = &( g[k*nV] ); lbCur = &( lb[k*nV] ); ubCur = &( ub[k*nV] ); /* 2) Set nWSR and maximum CPU time. */ nWSRcur = maxAllowedNWSR; CPUtimeCur = CPUtimeLimit; /* 3) Solve current QP. */ if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) ) { /* initialise */ returnvalue = QProblemB_initM( &qp,H,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur ); if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) ) return THROWERROR( returnvalue ); } else { /* hotstart */ returnvalue = QProblemB_hotstart( &qp,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur ); if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) ) return THROWERROR( returnvalue ); } /* 4) Obtain solution vectors and objective function value ... */ QProblemB_getPrimalSolution( &qp,x ); QProblemB_getDualSolution( &qp,y ); /* 5) Compute KKT residuals */ qpOASES_getKktViolationSB( nV, _H,gCur,lbCur,ubCur, x,y, &stat,&feas,&cmpl ); /* 6) update maximum values. */ if ( nWSRcur > *maxNWSR ) *maxNWSR = nWSRcur; if (stat > *maxStationarity) *maxStationarity = stat; if (feas > *maxFeasibility) *maxFeasibility = feas; if (cmpl > *maxComplementarity) *maxComplementarity = cmpl; if ( CPUtimeCur > *maxCPUtime ) *maxCPUtime = CPUtimeCur; *avgNWSR += nWSRcur; *avgCPUtime += CPUtimeCur; } *avgNWSR /= nQP; *avgCPUtime /= ((double)nQP); return SUCCESSFUL_RETURN; }