returnValue ExportCholeskySolver::getCode( ExportStatementBlock& code )
{
code.addFunction( chol );
code.addFunction( solve );
return SUCCESSFUL_RETURN;
}
returnValue ThreeSweepsERKExport::getCode( ExportStatementBlock& code
)
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP )
{
getDataDeclarations( code, ACADO_LOCAL );
code << "#pragma omp threadprivate( "
<< getAuxVariable().getFullName() << ", "
<< rk_xxx.getFullName() << ", "
<< rk_ttt.getFullName() << ", "
<< rk_kkk.getFullName() << ", "
<< rk_forward_sweep.getFullName() << ", "
<< rk_backward_sweep.getFullName()
<< " )\n\n";
}
int sensGen;
get( DYNAMIC_SENSITIVITY,sensGen );
if( exportRhs ) {
code.addFunction( rhs );
code.addFunction( diffs_rhs );
code.addFunction( diffs_sweep3 );
}
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
code.addComment(std::string("Fixed step size:") + toString(h));
code.addFunction( integrate );
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 ExportGaussNewtonBlockForces::getCode( ExportStatementBlock& code
)
{
setupQPInterface();
code.addStatement( *qpInterface );
code.addFunction( cleanup );
code.addFunction( shiftQpData );
code.addFunction( evaluateConstraints );
code.addFunction( evaluateAffineConstraints );
return ExportGaussNewtonCN2::getCode( code );
}
returnValue ExportCholeskyDecomposition::getCode( ExportStatementBlock& code
)
{
if (A.getDim() != 0)
return code.addFunction( fcn );
return SUCCESSFUL_RETURN;
}
returnValue ExportCholeskyDecomposition::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
if (A.getDim() != 0)
return declarations.addFunction( fcn );
return SUCCESSFUL_RETURN;
}
returnValue LiftedERKExport::getCode( ExportStatementBlock& code
)
{
// int printLevel;
// get( PRINTLEVEL,printLevel );
//
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "--> Exporting %s... ",fileName.getName() );
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP )
{
getDataDeclarations( code, ACADO_LOCAL );
code << "#pragma omp threadprivate( "
<< getAuxVariable().getFullName() << ", "
<< rk_xxx.getFullName() << ", "
<< rk_ttt.getFullName() << ", "
<< rk_kkk.getFullName()
<< " )\n\n";
}
int sensGen;
get( DYNAMIC_SENSITIVITY,sensGen );
if( exportRhs ) {
code.addFunction( rhs );
code.addFunction( diffs_rhs );
code.addFunction( alg_rhs );
solver->getCode( code ); // solver for the algebraic equations
}
else {
return ACADOERRORTEXT( RET_NOT_YET_IMPLEMENTED, "Externally defined dynamics not yet supported for the lifted ERK integrator!");
}
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
code.addComment(std::string("Fixed step size:") + toString(h));
code.addFunction( integrate );
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue ExplicitRungeKuttaExport::getCode( ExportStatementBlock& code
)
{
// int printLevel;
// get( PRINTLEVEL,printLevel );
//
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "--> Exporting %s... ",fileName.getName() );
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP )
{
getDataDeclarations( code, ACADO_LOCAL );
code << "#pragma omp threadprivate( "
<< getAuxVariable().getFullName() << ", "
<< rk_xxx.getFullName() << ", "
<< rk_ttt.getFullName() << ", "
<< rk_kkk.getFullName()
<< " )\n\n";
}
int sensGen;
get( DYNAMIC_SENSITIVITY,sensGen );
if( exportRhs ) {
code.addFunction( rhs );
code.addFunction( diffs_rhs );
}
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
code.addComment(std::string("Fixed step size:") + toString(h));
code.addFunction( integrate );
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::getCode( ExportStatementBlock& code
)
{
if (nRightHandSides > 0) {
if( !REUSE ) return ACADOERROR(RET_INVALID_OPTION);
setupFactorization( solve, rk_swap, determinant, string("fabs") );
code.addFunction( solve );
setupSolveReuseComplete( solveReuse, rk_bPerm );
code.addFunction( solveReuse );
if( TRANSPOSE ) {
setupSolveReuseTranspose( solveReuseTranspose, rk_bPerm_trans );
code.addFunction( solveReuseTranspose );
}
}
else {
setupSolveUpperTriangular( solveTriangular );
code.addFunction( solveTriangular );
setupSolve( solve, solveTriangular, rk_swap, determinant, string("fabs") );
code.addFunction( solve );
if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A
setupSolveReuse( solveReuse, solveTriangular, rk_bPerm );
code.addFunction( solveReuse );
}
// if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
if( TRANSPOSE ) {
setupSolveReuseTranspose( solveReuseTranspose, rk_bPerm_trans );
code.addFunction( solveReuseTranspose );
}
}
return SUCCESSFUL_RETURN;
}
returnValue AuxiliaryFunctionsExport::getCode( ExportStatementBlock& code
)
{
// int printLevel;
// get( PRINTLEVEL,printLevel );
//
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "--> Exporting %s... ",fileName.getName() );
code.addFunction( getAcadoVariablesX );
code.addFunction( getAcadoVariablesU );
code.addFunction( getAcadoVariablesXRef );
code.addFunction( getAcadoVariablesURef );
code.addFunction( printStates );
code.addFunction( printControls );
code.addFunction( getTime );
code.addFunction( printHeader );
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::getCode( ExportStatementBlock& code
)
{
// int printLevel;
// get( PRINTLEVEL,printLevel );
//
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "--> Exporting %s... ",fileName.getName() );
code.addFunction( multiplyQC1 );
code.addFunction( multiplyQE1 );
code.addFunction( multiplyQDX1 );
code.addFunction( multiplyRDU1 );
code.addFunction( multiplyQC2 );
code.addFunction( multiplyQE2 );
code.addFunction( multiplyQDX2 );
code.addFunction( multiplyC );
code.addFunction( multiplyE );
code.addFunction( multiplyCD1 );
code.addFunction( multiplyEU1 );
code.addFunction( multiplyG0 );
code.addFunction( multiplyG1 );
code.addFunction( multiplyH00 );
code.addFunction( multiplyH01 );
code.addFunction( multiplyH11 );
code.addFunction( condense1 );
code.addFunction( condense2 );
code.addFunction( expand );
code.addFunction( setupQP );
code.addFunction( getObjectiveValue );
// if ( (PrintLevel)printLevel >= HIGH )
// acadoPrintf( "done.\n" );
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 ExportIRK4StageSimplifiedNewton::getCode( ExportStatementBlock& code
)
{
if( eig.isEmpty() || transf1.isEmpty() || transf2.isEmpty() || fabs(stepsize) <= ZERO_EPS ) return ACADOERROR(RET_INVALID_OPTION);
if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
setupFactorization( solve_complex, rk_swap_complex, determinant_complex, string("cabs") );
code.addFunction( solve_complex );
if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A
setupSolveReuseComplete( solveReuse_complex, rk_bPerm_complex );
code.addFunction( solveReuse_complex );
}
ExportVariable eig_var( 1.0/stepsize*eig );
// SETUP solve_full
ExportVariable det_complex1( "det_complex1", 1, 1, REAL, ACADO_LOCAL, true );
ExportVariable det_complex2( "det_complex2", 1, 1, REAL, ACADO_LOCAL, true );
solve_full.addDeclaration( det_complex1 );
solve_full.addDeclaration( det_complex2 );
ExportIndex i( "i" );
solve_full.addIndex(i);
// form the real and complex linear system matrices
solve_full.addStatement( A_mem_complex1 == A_full );
if( implicit ) {
ExportForLoop loop1( i, 0, dim*dim );
loop1 << A_mem_complex1.getFullName() << "[i] += (" << eig_var.get(0,0) << "+" << eig_var.get(0,1) << "*I)*" << I_full.getFullName() << "[i];\n";
solve_full.addStatement( loop1 );
}
else {
ExportForLoop loop1( i, 0, dim );
loop1 << A_mem_complex1.getFullName() << "[i*" << toString(dim) << "+i] -= (" << eig_var.get(0,0) << "+" << eig_var.get(0,1) << "*I);\n";
solve_full.addStatement( loop1 );
}
solve_full.addStatement( A_mem_complex2 == A_full );
if( implicit ) {
ExportForLoop loop1( i, 0, dim*dim );
loop1 << A_mem_complex2.getFullName() << "[i] += (" << eig_var.get(1,0) << "+" << eig_var.get(1,1) << "*I)*" << I_full.getFullName() << "[i];\n";
solve_full.addStatement( loop1 );
}
else {
ExportForLoop loop1( i, 0, dim );
loop1 << A_mem_complex2.getFullName() << "[i*" << toString(dim) << "+i] -= (" << eig_var.get(1,0) << "+" << eig_var.get(1,1) << "*I);\n";
solve_full.addStatement( loop1 );
}
// factorize the real and complex linear systems
solve_full.addFunctionCall(getNameSolveComplexFunction(),A_mem_complex1,rk_perm_full.getAddress(0,0));
solve_full.addFunctionCall(getNameSolveComplexFunction(),A_mem_complex2,rk_perm_full.getAddress(1,0));
code.addFunction( solve_full );
// SETUP solveReuse_full
if( REUSE ) {
solveReuse_full.addIndex(i);
// transform the right-hand side
transformRightHandSide( solveReuse_full, i );
// solveReuse the real and complex linear systems
solveReuse_full.addFunctionCall(getNameSolveComplexReuseFunction(),A_mem_complex1,b_mem_complex1,rk_perm_full.getAddress(0,0));
solveReuse_full.addFunctionCall(getNameSolveComplexReuseFunction(),A_mem_complex2,b_mem_complex2,rk_perm_full.getAddress(1,0));
// transform back to the solution
transformSolution( solveReuse_full, i );
code.addFunction( solveReuse_full );
}
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 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;
}
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;
}