returnValue ExportGaussNewtonForces::getCode( ExportStatementBlock& code
)
{
setupQPInterface();
code.addStatement( *qpInterface );
code.addLinebreak( 2 );
code.addStatement( "/******************************************************************************/\n" );
code.addStatement( "/* */\n" );
code.addStatement( "/* ACADO code generation */\n" );
code.addStatement( "/* */\n" );
code.addStatement( "/******************************************************************************/\n" );
code.addLinebreak( 2 );
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP )
{
code.addDeclaration( state );
}
code.addFunction( modelSimulation );
code.addFunction( evaluateStageCost );
code.addFunction( evaluateTerminalCost );
code.addFunction( setObjQ1Q2 );
code.addFunction( setObjR1R2 );
code.addFunction( setObjS1 );
code.addFunction( setObjQN1QN2 );
code.addFunction( setStageH );
code.addFunction( setStagef );
code.addFunction( evaluateObjective );
code.addFunction( conSetGxGu );
code.addFunction( conSetd );
code.addFunction( evaluateConstraints );
code.addFunction( acc );
code.addFunction( preparation );
code.addFunction( feedback );
code.addFunction( initialize );
code.addFunction( initializeNodes );
code.addFunction( shiftStates );
code.addFunction( shiftControls );
code.addFunction( getKKT );
code.addFunction( getObjective );
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::getCode( ExportStatementBlock& code
)
{
if( NX1 > 0 ) {
code.addFunction( lin_input );
code.addStatement( "\n\n" );
}
if( NX2 > 0 ) {
code.addFunction( rhs );
code.addStatement( "\n\n" );
code.addFunction( diffs_rhs );
code.addStatement( "\n\n" );
}
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
code.addDeclaration( numStepsV );
code.addLinebreak( 2 );
}
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
code.addComment(String("Fixed step size:") << String(h));
code.addFunction( integrate );
if( NX2 != NX ) {
prepareFullRhs();
code.addFunction( fullRhs );
}
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonCN2::getCode( ExportStatementBlock& code
)
{
setupQPInterface();
code.addLinebreak( 2 );
code.addStatement( "/******************************************************************************/\n" );
code.addStatement( "/* */\n" );
code.addStatement( "/* ACADO code generation */\n" );
code.addStatement( "/* */\n" );
code.addStatement( "/******************************************************************************/\n" );
code.addLinebreak( 2 );
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP )
{
code.addDeclaration( state );
}
code.addFunction( modelSimulation );
code.addFunction( evaluateLSQ );
code.addFunction( evaluateLSQEndTerm );
code.addFunction( setObjQ1Q2 );
code.addFunction( setObjR1R2 );
code.addFunction( setObjS1 );
code.addFunction( setObjQN1QN2 );
code.addFunction( evaluateObjective );
code.addFunction( multGxGu );
code.addFunction( moveGuE );
code.addFunction( multBTW1 );
code.addFunction( mac_S1T_E );
code.addFunction( macBTW1_R1 );
code.addFunction( multGxTGu );
code.addFunction( macQEW2 );
code.addFunction( macATw1QDy );
code.addFunction( macBTw1 );
code.addFunction( macS1TSbar );
code.addFunction( macQSbarW2 );
code.addFunction( macASbar );
code.addFunction( expansionStep );
code.addFunction( copyHTH );
code.addFunction( multRDy );
code.addFunction( multQDy );
code.addFunction( multQN1Gu );
code.addFunction( evaluatePathConstraints );
for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
{
if (evaluatePointConstraints[ i ] == 0)
continue;
code.addFunction( *evaluatePointConstraints[ i ] );
}
code.addFunction( condensePrep );
code.addFunction( condenseFdb );
code.addFunction( expand );
code.addFunction( preparation );
code.addFunction( feedback );
code.addFunction( initialize );
code.addFunction( initializeNodes );
code.addFunction( shiftStates );
code.addFunction( shiftControls );
code.addFunction( getKKT );
code.addFunction( getObjective );
return SUCCESSFUL_RETURN;
}
returnValue ExportExactHessianQpDunes::getCode( ExportStatementBlock& code
)
{
setupQPInterface();
code.addStatement( *qpInterface );
code.addLinebreak( 2 );
code.addStatement( "/******************************************************************************/\n" );
code.addStatement( "/* */\n" );
code.addStatement( "/* ACADO code generation */\n" );
code.addStatement( "/* */\n" );
code.addStatement( "/******************************************************************************/\n" );
code.addLinebreak( 2 );
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP )
{
code.addDeclaration( state );
}
code.addFunction( modelSimulation );
code.addFunction( evaluateStageCost );
code.addFunction( evaluateTerminalCost );
code.addFunction( setObjQ1Q2 );
code.addFunction( setObjR1R2 );
code.addFunction( setObjQN1QN2 );
code.addFunction( setStageH );
code.addFunction( setStagef );
code.addFunction( evaluateObjective );
code.addFunction( regularizeHessian );
code.addFunction( evaluatePathConstraints );
for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
{
if (evaluatePointConstraints[ i ] == 0)
continue;
code.addFunction( *evaluatePointConstraints[ i ] );
}
code.addFunction( setStagePac );
code.addFunction( evaluateConstraints );
code.addFunction( acc );
code.addFunction( preparation );
code.addFunction( feedback );
code.addFunction( initialize );
code.addFunction( initializeNodes );
code.addFunction( shiftStates );
code.addFunction( shiftControls );
code.addFunction( getKKT );
code.addFunction( getObjective );
code.addFunction( cleanup );
code.addFunction( shiftQpData );
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::getCode( ExportStatementBlock& code
)
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP ) {
ExportVariable max = getAuxVariable();
max.setName( "auxVar" );
max.setDataStruct( ACADO_LOCAL );
if( NX2 > 0 ) {
rhs.setGlobalExportVariable( max );
diffs_rhs.setGlobalExportVariable( max );
}
if( NX3 > 0 ) {
rhs3.setGlobalExportVariable( max );
diffs_rhs3.setGlobalExportVariable( max );
}
getDataDeclarations( code, ACADO_LOCAL );
stringstream s;
s << "#pragma omp threadprivate( "
<< max.getFullName() << ", "
<< rk_xxx.getFullName();
if( NX1 > 0 ) {
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev1.getFullName();
s << ", " << rk_diffsNew1.getFullName();
}
if( NX2 > 0 || NXA > 0 ) {
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev2.getFullName();
s << ", " << rk_diffsNew2.getFullName();
}
if( NX3 > 0 ) {
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev3.getFullName();
s << ", " << rk_diffsNew3.getFullName();
s << ", " << rk_diffsTemp3.getFullName();
}
s << " )" << endl << endl;
code.addStatement( s.str().c_str() );
}
if( NX1 > 0 ) {
code.addFunction( lin_input );
code.addStatement( "\n\n" );
}
if( NX2 > 0 ) {
code.addFunction( rhs );
code.addStatement( "\n\n" );
code.addFunction( diffs_rhs );
code.addStatement( "\n\n" );
}
if( NX3 > 0 ) {
code.addFunction( rhs3 );
code.addStatement( "\n\n" );
code.addFunction( diffs_rhs3 );
code.addStatement( "\n\n" );
}
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
code.addDeclaration( numStepsV );
code.addLinebreak( 2 );
}
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
code.addComment(std::string("Fixed step size:") + toString(h));
code.addFunction( integrate );
return SUCCESSFUL_RETURN;
}
returnValue ExportHouseholderQR::getCode( ExportStatementBlock& code
)
{
unsigned run1, run2, run3;
if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
//
// Solve the upper triangular system of equations:
//
for (run1 = nCols; run1 > (nCols - nBacksolves); run1--)
{
for (run2 = nCols - 1; run2 > (run1 - 1); run2--)
{
solveTriangular.addStatement(
b.getRow(run1 - 1) -= A.getSubMatrix((run1 - 1), run1, run2, run2 + 1) * b.getRow(run2));
}
solveTriangular <<
"b[" << toString((run1 - 1)) << "] = b["
<< toString((run1 - 1)) << "]/A["
<< toString((run1 - 1) * nCols + (run1 - 1)) << "];\n";
}
code.addFunction(solveTriangular);
//
// Main solver function
//
solve.addStatement( determinant == 1.0 );
if( UNROLLING || nRows <= 5 )
{
// Start the factorization:
for (run1 = 0; run1 < nCols; run1++)
{
for (run2 = run1; run2 < nRows; run2++)
{
solve.addStatement(
rk_temp.getCol(run2)
== A.getSubMatrix(run2, run2 + 1, run1,
run1 + 1));
}
// calculate norm:
solve.addStatement(rk_temp.getCol(nRows) ==
rk_temp.getCols(run1, nRows) * rk_temp.getTranspose().getRows(run1, nRows));
solve << rk_temp.getFullName() << "[" << toString(nRows) << "] = sqrt("
<< rk_temp.getFullName() << "[" << toString(nRows)
<< "]);\n";
// update first element:
solve << rk_temp.getFullName() << "[" << toString(run1) << "] += ("
<< rk_temp.getFullName() << "[" << toString(run1)
<< "] < 0 ? -1 : 1)*" << rk_temp.getFullName()
<< "[" << toString(nRows) << "];\n";
// calculate norm:
solve.addStatement(rk_temp.getCol(nRows) ==
rk_temp.getCols(run1, nRows) * rk_temp.getTranspose().getRows(run1, nRows));
solve << rk_temp.getFullName() << "[" << toString(nRows) << "] = sqrt("
<< rk_temp.getFullName() << "[" << toString(nRows)
<< "]);\n";
// normalization:
for (run2 = run1; run2 < nRows; run2++)
{
solve << rk_temp.getFullName() << "[" << toString(run2) << "] = "
<< rk_temp.getFullName() << "[" << toString(run2)
<< "]/" << rk_temp.getFullName() << "["
<< toString(nRows) << "];\n";
}
// update current column:
solve.addStatement(
rk_temp.getCol(nRows)
== rk_temp.getCols(run1, nRows)
* A.getSubMatrix(run1, nRows, run1,
run1 + 1));
solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
solve.addStatement(
A.getSubMatrix(run1, run1 + 1, run1, run1 + 1) -=
rk_temp.getCol(run1) * rk_temp.getCol(nRows));
solve.addStatement( determinant == determinant * A.getElement(run1, run1) );
if (REUSE)
{
// replace zeros by results that can be reused:
for (run2 = run1; run2 < nRows - 1; run2++)
{
solve.addStatement(
A.getSubMatrix(run2 + 1, run2 + 2, run1, run1 + 1)
== rk_temp.getCol(run2));
}
}
// update following columns:
for (run2 = run1 + 1; run2 < nCols; run2++)
{
solve.addStatement(
rk_temp.getCol(nRows)
== rk_temp.getCols(run1, nRows)
* A.getSubMatrix(run1, nRows, run2,
run2 + 1));
solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
for (run3 = run1; run3 < nRows; run3++)
{
solve.addStatement(
A.getSubMatrix(run3, run3 + 1, run2, run2 + 1) -=
rk_temp.getCol(run3) * rk_temp.getCol(nRows));
}
}
// update right-hand side:
solve.addStatement(
rk_temp.getCol(nRows)
== rk_temp.getCols(run1, nRows)
* b.getRows(run1, nRows));
solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
for (run3 = run1; run3 < nRows; run3++)
{
solve.addStatement( b.getRow(run3) -= rk_temp.getCol(run3) * rk_temp.getCol(nRows));
}
if (REUSE)
{
// store last element to be reused:
solve.addStatement(
rk_temp.getCol(run1) == rk_temp.getCol(nRows - 1));
}
}
}
else
{
ExportIndex i( "i" );
ExportIndex j( "j" );
ExportIndex k( "k" );
solve.addIndex( i );
solve.addIndex( j );
solve.addIndex( k );
solve << "for( i=0; i < " << toString( nCols ) << "; i++ ) {\n";
solve << " for( j=i; j < " << toString( nRows ) << "; j++ ) {\n";
solve << " " << rk_temp.getFullName() << "[j] = A[j*" << toString( nCols ) << "+i];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[i];\n";
solve << " for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*" << rk_temp.getFullName() << "[j];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = sqrt(" << rk_temp.getFullName() << "[" << toString( nRows ) << "]);\n";
// update first element:
solve << " " << rk_temp.getFullName() << "[i] += (" << rk_temp.getFullName() << "[i] < 0 ? -1 : 1)*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[i];\n";
solve << " for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*" << rk_temp.getFullName() << "[j];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = sqrt(" << rk_temp.getFullName() << "[" << toString( nRows ) << "]);\n";
solve << " for( j=i; j < " << toString( nRows ) << "; j++ ) {\n";
solve << " " << rk_temp.getFullName() << "[j] = " << rk_temp.getFullName() << "[j]/" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*A[i*" << toString( nCols ) << "+i];\n";
solve << " for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*A[j*" << toString( nCols ) << "+i];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
solve << " A[i*" << toString( nCols ) << "+i] -= " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
solve << " " << determinant.getFullName() << " *= " << " A[i * " << toString( nCols ) << " + i];\n";
if( REUSE ) {
solve << " for( j=i; j < (" << toString( nRows ) << "-1); j++ ) {\n";
solve << " A[(j+1)*" << toString( nCols ) << "+i] = " << rk_temp.getFullName() << "[j];\n";
solve << " }\n";
}
solve << " for( j=i+1; j < " << toString( nCols ) << "; j++ ) {\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*A[i*" << toString( nCols ) << "+j];\n";
solve << " for( k=i+1; k < " << toString( nRows ) << "; k++ ) {\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[k]*A[k*" << toString( nCols ) << "+j];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
solve << " for( k=i; k < " << toString( nRows ) << "; k++ ) {\n";
solve << " A[k*" << toString( nCols ) << "+j] -= " << rk_temp.getFullName() << "[k]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
solve << " }\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*b[i];\n";
solve << " for( k=i+1; k < " << toString( nRows ) << "; k++ ) {\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[k]*b[k];\n";
solve << " }\n";
solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
solve << " for( k=i; k < " << toString( nRows ) << "; k++ ) {\n";
solve << " b[k] -= " << rk_temp.getFullName() << "[k]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
solve << " }\n";
if( REUSE ) {
solve << " " << rk_temp.getFullName() << "[i] = " << rk_temp.getFullName() << "[" << toString( nRows-1 ) << "];\n";
}
solve << "}\n";
}
solve.addLinebreak();
solve.addFunctionCall(solveTriangular, A, b);
code.addFunction( solve );
code.addLinebreak( 2 );
if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A
// update right-hand side:
for( run1 = 0; run1 < nCols; run1++ ) {
solveReuse.addStatement( rk_temp.getCol( nRows ) == A.getSubMatrix( run1+1,run1+2,run1,run1+1 )*b.getRow( run1 ) );
for( run2 = run1+1; run2 < (nRows-1); run2++ ) {
solveReuse.addStatement( rk_temp.getCol( nRows ) += A.getSubMatrix( run2+1,run2+2,run1,run1+1 )*b.getRow( run2 ) );
}
solveReuse.addStatement( rk_temp.getCol( nRows ) += rk_temp.getCol( run1 )*b.getRow( nRows-1 ) );
solveReuse << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n" ;
for( run3 = run1; run3 < (nRows-1); run3++ ) {
solveReuse.addStatement( b.getRow( run3 ) -= A.getSubMatrix( run3+1,run3+2,run1,run1+1 )*rk_temp.getCol( nRows ) );
}
solveReuse.addStatement( b.getRow( nRows-1 ) -= rk_temp.getCol( run1 )*rk_temp.getCol( nRows ) );
}
solveReuse.addLinebreak();
solveReuse.addFunctionCall( solveTriangular, A, b );
code.addFunction( solveReuse );
}
return SUCCESSFUL_RETURN;
}
