returnValue ExportGaussElim::setupSolveReuseComplete( ExportFunction& _solveReuse, ExportVariable& _bPerm ) {
ExportIndex run1( "i" );
ExportIndex run2( "j" );
ExportIndex tmp_index1( "index1" );
ExportIndex tmp_index2( "index2" );
ExportVariable tmp( "tmp_var", 1, 1, _bPerm.getType(), ACADO_LOCAL, true );
_solveReuse.addIndex( run1 );
_solveReuse.addIndex( run2 );
_solveReuse.addIndex( tmp_index1 );
_solveReuse.addIndex( tmp_index2 );
_solveReuse.addDeclaration(tmp);
uint run3;
if (nRightHandSides <= 0)
return ACADOERROR(RET_INVALID_OPTION);
ExportForLoop loop1( run1, 0, dim );
loop1 << run2.getName() << " = " << rk_perm.getFullName() << "[" << run1.getName() << "]*" << toString(nRightHandSides) << ";\n";
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
loop1 << _bPerm.get( run1,run3 ) << " = b[" << run2.getName() << "+" << toString(run3) << "];\n";
}
_solveReuse.addStatement( loop1 );
ExportForLoop loop2( run2, 1, dim ); // row run2
loop2.addStatement( tmp_index1 == run2*nRightHandSides );
ExportForLoop loop3( run1, 0, run2 ); // column run1
loop3.addStatement( tmp_index2 == run1*nRightHandSides );
loop3.addStatement( tmp == A.getElement(run2,run1) );
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
// loop3.addStatement( _bPerm.getElement( run2,run3 ) += tmp * _bPerm.getElement( run1,run3 ) );
loop3 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] += " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index2.getName() << "+" << toString(run3) << "];\n";
}
loop2.addStatement( loop3 );
_solveReuse.addStatement( loop2 );
// Solve the upper triangular system of equations:
ExportForLoop loop4( run1, dim-1, -1, -1 );
loop4.addStatement( tmp_index1 == run1*nRightHandSides );
ExportForLoop loop5( run2, dim-1, run1, -1 );
loop5.addStatement( tmp_index2 == run2*nRightHandSides );
loop5.addStatement( tmp == A.getElement( run1,run2 ) );
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
// loop5.addStatement( _bPerm.getElement( run1,run3 ) -= tmp * _bPerm.getElement( run2,run3 ) );
loop5 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] -= " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index2.getName() << "+" << toString(run3) << "];\n";
}
loop4.addStatement( loop5 );
loop4 << tmp.getName() << " = 1.0/A[" << run1.getName() << "*" << toString(dim+1) << "];\n";
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
// loop4 << _bPerm.get( run1,run3 ) << " = " << _bPerm.get( run1,run3 ) << "*" << tmp.getName() << ";\n";
loop4 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] = " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "];\n";
}
_solveReuse.addStatement( loop4 );
_solveReuse.addStatement( b == _bPerm );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveReuseTranspose( ExportFunction& _solveReuse, ExportVariable& _bPerm ) {
ExportIndex run1( "i" );
ExportIndex run2( "j" );
ExportVariable tmp( "tmp_var", 1, 1, _bPerm.getType(), ACADO_LOCAL, true );
_solveReuse.addIndex( run1 );
_solveReuse.addIndex( run2 );
_solveReuse.addDeclaration(tmp);
_solveReuse.addStatement( _bPerm == b_trans );
ExportForLoop loop2( run2, 0, dim ); // row run2
ExportForLoop loop3( run1, 0, run2 ); // column run1
loop3.addStatement( _bPerm.getRow(run2) -= A.getElement(run1,run2)*_bPerm.getRow(run1) );
loop2.addStatement( loop3 );
loop2 << tmp.getName() << " = 1.0/A[" << run2.getName() << "*" << toString(dim+1) << "];\n";
loop2.addStatement( _bPerm.getRow(run2) == _bPerm.getRow(run2)*tmp );
_solveReuse.addStatement( loop2 );
// Solve the upper triangular system of equations:
ExportForLoop loop4( run1, dim-1, -1, -1 );
ExportForLoop loop5( run2, dim-1, run1, -1 );
loop5.addStatement( _bPerm.getRow(run1) += A.getElement(run2,run1)*_bPerm.getRow(run2) );
loop4.addStatement( loop5 );
_solveReuse.addStatement( loop4 );
// The permutation now happens HERE!
ExportForLoop loop1( run1, 0, dim );
loop1 << run2.getName() << " = " << rk_perm.getFullName() << "[" << run1.getName() << "];\n";
loop1.addStatement( b_trans.getRow(run2) == _bPerm.getRow(run1) );
_solveReuse.addStatement( loop1 );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveUpperTriangular( ExportFunction& _solveTriangular ) {
if (nRightHandSides > 0)
return ACADOERROR(RET_INVALID_OPTION);
uint run1, run2;
// Solve the upper triangular system of equations:
for( run1 = dim; run1 > 0; run1--) {
for( run2 = dim-1; run2 > (run1-1); run2--) {
_solveTriangular.addStatement( b.getRow( (run1-1) ) -= A.getSubMatrix( (run1-1),(run1-1)+1,run2,run2+1 ) * b.getRow( run2 ) );
}
_solveTriangular << "b[" << toString( (run1-1) ) << "] = b[" << toString( (run1-1) ) << "]/A[" << toString( (run1-1)*dim+(run1-1) ) << "];\n";
}
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveReuse( ExportFunction& _solveReuse, ExportFunction& _solveTriangular, ExportVariable& _bPerm ) {
uint run1, run2;
if (nRightHandSides > 0)
return ACADOERROR(RET_INVALID_OPTION);
for( run1 = 0; run1 < dim; run1++ ) {
_solveReuse << _bPerm.get( run1,0 ) << " = b[" << rk_perm.getFullName() << "[" << toString( run1 ) << "]];\n";
}
for( run2 = 1; run2 < dim; run2++ ) { // row run2
for( run1 = 0; run1 < run2; run1++ ) { // column run1
_solveReuse << _bPerm.get( run2,0 ) << " += A[" << toString( run2*dim+run1 ) << "]*" << _bPerm.getFullName() << "[" << toString( run1 ) << "];\n";
}
_solveReuse.addLinebreak();
}
_solveReuse.addLinebreak();
_solveReuse.addFunctionCall( _solveTriangular, A, _bPerm );
_solveReuse.addStatement( b == _bPerm );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonCN2::setupConstraintsEvaluation( void )
{
ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
int hardcodeConstraintValues;
get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
////////////////////////////////////////////////////////////////////////////
//
// Setup the bounds on control variables
//
////////////////////////////////////////////////////////////////////////////
unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
DVector lbValuesMatrix( numBounds );
DVector ubValuesMatrix( numBounds );
if (initialStateFixed( ) == false)
for(unsigned run1 = 0; run1 < NX; ++run1)
{
lbValuesMatrix( run1 )= xBounds.getLowerBound(0, run1);
ubValuesMatrix( run1) = xBounds.getUpperBound(0, run1);
}
for(unsigned run1 = 0; run1 < N; ++run1)
for(unsigned run2 = 0; run2 < NU; ++run2)
{
lbValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getLowerBound(run1, run2);
ubValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getUpperBound(run1, run2);
}
if (hardcodeConstraintValues == YES)
{
lbValues.setup("lbValues", lbValuesMatrix, REAL, ACADO_VARIABLES);
ubValues.setup("ubValues", ubValuesMatrix, REAL, ACADO_VARIABLES);
}
else if (isFinite( lbValuesMatrix ) || isFinite( ubValuesMatrix ))
{
lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
lbValues.setDoc( "Lower bounds values." );
ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
ubValues.setDoc( "Upper bounds values." );
initialize.addStatement(lbValues == lbValuesMatrix);
initialize.addStatement(ubValues == ubValuesMatrix);
}
ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
if (performFullCondensing() == true)
{
boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
}
else
{
if ( initialStateFixed( ) == true )
{
condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
}
else
{
boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
}
}
condensePrep.addLinebreak( );
condenseFdb.addLinebreak( );
////////////////////////////////////////////////////////////////////////////
//
// Setup the bounds on state variables
//
////////////////////////////////////////////////////////////////////////////
if( getNumStateBounds() )
{
condenseFdb.addVariable( tmp );
DVector xLowerBounds( getNumStateBounds( )), xUpperBounds( getNumStateBounds( ) );
for(unsigned i = 0; i < xBoundsIdx.size(); ++i)
{
xLowerBounds( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
xUpperBounds( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
}
unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
if (numOps < 1024)
{
for(unsigned row = 0; row < getNumStateBounds( ); ++row)
{
unsigned conIdx = xBoundsIdx[ row ] - NX;
unsigned blk = conIdx / NX + 1;
for (unsigned col = 0; col < blk; ++col)
{
unsigned blkRow = (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX;
condensePrep.addStatement(
A.getSubMatrix(row, row + 1, col * NU, (col + 1) * NU ) == E.getRow( blkRow ) );
}
condensePrep.addLinebreak();
}
}
else
{
unsigned nXBounds = getNumStateBounds( );
DMatrix vXBoundIndices(1, nXBounds);
for (unsigned i = 0; i < nXBounds; ++i)
vXBoundIndices(0, i) = xBoundsIdx[ i ];
ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
condensePrep.addVariable( evXBounds );
ExportIndex row, col, conIdx, blk, blkRow;
condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );
ExportForLoop lRow(row, 0, nXBounds);
lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
lRow.addStatement( blk == conIdx / NX + 1 );
ExportForLoop lCol(col, 0, blk);
lCol.addStatement( blkRow == (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX );
lCol.addStatement(
A.getSubMatrix(row, row + 1, col * NU, (col + 1) * NU ) == E.getRow( blkRow ) );
lRow.addStatement( lCol );
condensePrep.addStatement( lRow );
condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
}
condensePrep.addLinebreak( );
unsigned nXBounds = getNumStateBounds( );
if (hardcodeConstraintValues == YES)
{
lbAValues.setup("lbAValues", xLowerBounds, REAL, ACADO_VARIABLES);
ubAValues.setup("ubAValues", xUpperBounds, REAL, ACADO_VARIABLES);
}
else
{
lbAValues.setup("lbAValues", nXBounds, 1, REAL, ACADO_VARIABLES);
lbAValues.setDoc( "Lower bounds values for affine constraints." );
ubAValues.setup("ubAValues", nXBounds, 1, REAL, ACADO_VARIABLES);
ubAValues.setDoc( "Upper bounds values for affine constraints." );
initialize.addStatement(lbAValues == xLowerBounds);
initialize.addStatement(ubAValues == xUpperBounds);
}
// Shift constraint bounds by first interval
for(unsigned boundIndex = 0; boundIndex < getNumStateBounds( ); ++boundIndex)
{
unsigned row = xBoundsIdx[boundIndex];
condenseFdb.addStatement( tmp == sbar.getRow( row ) + x.makeRowVector().getCol( row ) );
condenseFdb.addStatement( lbA.getRow( boundIndex ) == lbAValues.getRow( boundIndex ) - tmp );
condenseFdb.addStatement( ubA.getRow( boundIndex ) == ubAValues.getRow( boundIndex ) - tmp );
}
condenseFdb.addLinebreak( );
}
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupFactorization( ExportFunction& _solve, ExportVariable& _swap, ExportVariable& _determinant, const string& absF ) {
uint run1, run2, run3;
ExportIndex i( "i" );
_solve.addIndex( i );
ExportIndex j( "j" );
ExportIndex k( "k" );
ExportVariable indexMax( "indexMax", 1, 1, INT, ACADO_LOCAL, true );
ExportVariable intSwap( "intSwap", 1, 1, INT, ACADO_LOCAL, true );
ExportVariable valueMax( "valueMax", 1, 1, REAL, ACADO_LOCAL, true );
ExportVariable temp( "temp", 1, 1, REAL, ACADO_LOCAL, true );
if( !UNROLLING ) {
_solve.addIndex( j );
_solve.addIndex( k );
_solve.addDeclaration( indexMax );
if( REUSE ) _solve.addDeclaration( intSwap );
_solve.addDeclaration( valueMax );
_solve.addDeclaration( temp );
}
// initialise rk_perm (the permutation vector)
if( REUSE ) {
ExportForLoop loop1( i,0,dim );
loop1 << rk_perm.get( 0,i ) << " = " << i.getName() << ";\n";
_solve.addStatement( loop1 );
}
_solve.addStatement( _determinant == 1 );
if( UNROLLING || dim <= 5 ) {
// Start the factorization:
for( run1 = 0; run1 < (dim-1); run1++ ) {
// Search for pivot in column run1:
for( run2 = run1+1; run2 < dim; run2++ ) {
// add the test (if or else if):
stringstream test;
if( run2 == (run1+1) ) {
test << "if(";
} else {
test << "else if(";
}
test << absF << "(A[" << toString( run2*dim+run1 ) << "]) > " << absF << "(A[" << toString( run1*dim+run1 ) << "])";
for( run3 = run1+1; run3 < dim; run3++ ) {
if( run3 != run2) {
test << " && " << absF << "(A[" << toString( run2*dim+run1 ) << "]) > " << absF << "(A[" << toString( run3*dim+run1 ) << "])";
}
}
test << ") {\n";
_solve.addStatement( test.str() );
// do the row swaps:
// for A:
for( run3 = 0; run3 < dim; run3++ ) {
_solve.addStatement( _swap == A.getSubMatrix( run1,run1+1,run3,run3+1 ) );
_solve.addStatement( A.getSubMatrix( run1,run1+1,run3,run3+1 ) == A.getSubMatrix( run2,run2+1,run3,run3+1 ) );
_solve.addStatement( A.getSubMatrix( run2,run2+1,run3,run3+1 ) == _swap );
}
if( REUSE ) { // rk_perm also needs to be updated if it needs to be possible to reuse the factorization
_solve.addStatement( intSwap == rk_perm.getCol( run1 ) );
_solve.addStatement( rk_perm.getCol( run1 ) == rk_perm.getCol( run2 ) );
_solve.addStatement( rk_perm.getCol( run2 ) == intSwap );
}
_solve.addStatement( "}\n" );
}
// potentially needed row swaps are done
_solve.addLinebreak();
// update of the next rows:
for( run2 = run1+1; run2 < dim; run2++ ) {
_solve << "A[" << toString( run2*dim+run1 ) << "] = -A[" << toString( run2*dim+run1 ) << "]/A[" << toString( run1*dim+run1 ) << "];\n";
_solve.addStatement( A.getSubMatrix( run2,run2+1,run1+1,dim ) += A.getSubMatrix( run2,run2+1,run1,run1+1 ) * A.getSubMatrix( run1,run1+1,run1+1,dim ) );
_solve.addLinebreak();
}
_solve.addStatement( _determinant == _determinant*A.getSubMatrix(run1,run1+1,run1,run1+1) );
_solve.addLinebreak();
}
_solve.addStatement( _determinant == _determinant*A.getSubMatrix(dim-1,dim,dim-1,dim) );
_solve.addLinebreak();
}
else { // without UNROLLING:
_solve << "for( i=0; i < (" << toString( dim-1 ) << "); i++ ) {\n";
_solve << " indexMax = i;\n";
_solve << " valueMax = " << absF << "(A[i*" << toString( dim ) << "+i]);\n";
_solve << " for( j=(i+1); j < " << toString( dim ) << "; j++ ) {\n";
_solve << " temp = " << absF << "(A[j*" << toString( dim ) << "+i]);\n";
_solve << " if( temp > valueMax ) {\n";
_solve << " indexMax = j;\n";
_solve << " valueMax = temp;\n";
_solve << " }\n";
_solve << " }\n";
_solve << " if( indexMax > i ) {\n";
ExportForLoop loop2( k,0,dim );
loop2 << " " << _swap.getFullName() << " = A[i*" << toString( dim ) << "+" << k.getName() << "];\n";
loop2 << " A[i*" << toString( dim ) << "+" << k.getName() << "] = A[indexMax*" << toString( dim ) << "+" << k.getName() << "];\n";
loop2 << " A[indexMax*" << toString( dim ) << "+" << k.getName() << "] = " << _swap.getFullName() << ";\n";
_solve.addStatement( loop2 );
if( REUSE ) {
_solve << " " << intSwap.getFullName() << " = " << rk_perm.getFullName() << "[i];\n";
_solve << " " << rk_perm.getFullName() << "[i] = " << rk_perm.getFullName() << "[indexMax];\n";
_solve << " " << rk_perm.getFullName() << "[indexMax] = " << intSwap.getFullName() << ";\n";
}
_solve << " }\n";
_solve << " " << _determinant.getFullName() << " *= A[i*" << toString( dim ) << "+i];\n";
_solve << " for( j=i+1; j < " << toString( dim ) << "; j++ ) {\n";
_solve << " A[j*" << toString( dim ) << "+i] = -A[j*" << toString( dim ) << "+i]/A[i*" << toString( dim ) << "+i];\n";
_solve << " for( k=i+1; k < " << toString( dim ) << "; k++ ) {\n";
_solve << " A[j*" << toString( dim ) << "+k] += A[j*" << toString( dim ) << "+i] * A[i*" << toString( dim ) << "+k];\n";
_solve << " }\n";
_solve << " }\n";
_solve << "}\n";
_solve << _determinant.getFullName() << " *= A[" << toString( (dim-1)*dim+(dim-1) ) << "];\n";
}
_solve << _determinant.getFullName() << " = " << absF << "(" << _determinant.getFullName() << ");\n";
return SUCCESSFUL_RETURN;
}