returnValue ExportCholeskySolver::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( chol );
declarations.addDeclaration( solve );
return SUCCESSFUL_RETURN;
}
returnValue ExplicitRungeKuttaExport::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( integrate );
declarations.addDeclaration( rhs );
declarations.addDeclaration( diffs_rhs );
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( integrate );
if( NX2 != NX ) declarations.addDeclaration( fullRhs );
else declarations.addDeclaration( rhs );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( solve );
declarations.addDeclaration( solveTriangular );
if( REUSE ) {
declarations.addDeclaration( solveReuse );
}
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
declarations.addDeclaration( rk_swap,dataStruct ); // needed for the row swaps
if( REUSE ) {
declarations.addDeclaration( rk_bPerm,dataStruct ); // reordered right-hand side
if( TRANSPOSE ) {
declarations.addDeclaration( rk_bPerm_trans,dataStruct );
}
}
return SUCCESSFUL_RETURN;
}
returnValue ExplicitRungeKuttaExport::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
if( exportRhs ) {
declarations.addDeclaration( getAuxVariable(),dataStruct );
}
declarations.addDeclaration( rk_ttt,dataStruct );
declarations.addDeclaration( rk_xxx,dataStruct );
declarations.addDeclaration( rk_kkk,dataStruct );
// declarations.addDeclaration( reset_int,dataStruct );
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 ExportGaussNewtonForces::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
returnValue status;
status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
if (status != SUCCESSFUL_RETURN)
return status;
declarations.addDeclaration(x0, dataStruct);
declarations.addDeclaration(lbValues, dataStruct);
declarations.addDeclaration(ubValues, dataStruct);
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
ExportVariable max = getAuxVariable();
declarations.addDeclaration( max,dataStruct );
declarations.addDeclaration( rk_xxx,dataStruct );
declarations.addDeclaration( reset_int,dataStruct );
declarations.addDeclaration( rk_diffsPrev1,dataStruct );
declarations.addDeclaration( rk_diffsPrev2,dataStruct );
declarations.addDeclaration( rk_diffsNew1,dataStruct );
declarations.addDeclaration( rk_diffsNew2,dataStruct );
return SUCCESSFUL_RETURN;
}
returnValue ExportIRK4StageSimplifiedNewton::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
ExportGaussElim::getFunctionDeclarations( declarations );
declarations.addDeclaration( solve_complex );
if( REUSE ) {
declarations.addDeclaration( solveReuse_complex );
}
declarations.addDeclaration( solve_full );
if( REUSE ) {
declarations.addDeclaration( solveReuse_full );
}
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( integrate );
if( NX2 != NX ) declarations.addDeclaration( fullRhs );
if( exportRhs ) {
if( NX1 > 0 ) {
declarations.addDeclaration( lin_input );
}
if( NX2 > 0 ) {
declarations.addDeclaration( rhs );
declarations.addDeclaration( diffs_rhs );
}
}
else {
Function tmpFun;
tmpFun << zeros(1,1);
ExportODEfunction tmpExport(tmpFun, getNameRHS());
declarations.addDeclaration( tmpExport );
tmpExport = ExportODEfunction(tmpFun, getNameDiffsRHS());
declarations.addDeclaration( tmpExport );
}
return SUCCESSFUL_RETURN;
}
returnValue ExportExactHessianCN2::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
ExportGaussNewtonCN2::getFunctionDeclarations( declarations );
declarations.addDeclaration( regularization );
return SUCCESSFUL_RETURN;
}
returnValue ExportIRK4StageSimplifiedNewton::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
ExportGaussElim::getDataDeclarations( declarations, dataStruct );
declarations.addDeclaration( rk_swap_complex,dataStruct ); // needed for the row swaps
if( REUSE ) {
declarations.addDeclaration( rk_bPerm_complex,dataStruct ); // reordered right-hand side
}
declarations.addDeclaration( A_mem_complex1,dataStruct );
declarations.addDeclaration( b_mem_complex1,dataStruct );
declarations.addDeclaration( A_mem_complex2,dataStruct );
declarations.addDeclaration( b_mem_complex2,dataStruct );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonCN2::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( preparation );
declarations.addDeclaration( feedback );
declarations.addDeclaration( initialize );
declarations.addDeclaration( initializeNodes );
declarations.addDeclaration( shiftStates );
declarations.addDeclaration( shiftControls );
declarations.addDeclaration( getKKT );
declarations.addDeclaration( getObjective );
declarations.addDeclaration( evaluateLSQ );
declarations.addDeclaration( evaluateLSQEndTerm );
return SUCCESSFUL_RETURN;
}
returnValue ThreeSweepsERKExport::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
AdjointERKExport::getDataDeclarations( declarations, dataStruct );
declarations.addDeclaration( rk_backward_sweep,dataStruct );
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;
}
returnValue AuxiliaryFunctionsExport::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( getAcadoVariablesX );
declarations.addDeclaration( getAcadoVariablesU );
declarations.addDeclaration( getAcadoVariablesXRef );
declarations.addDeclaration( getAcadoVariablesURef );
declarations.addDeclaration( printStates );
declarations.addDeclaration( printControls );
declarations.addDeclaration( getTime );
declarations.addDeclaration( printHeader );
return SUCCESSFUL_RETURN;
}
returnValue LiftedERKExport::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
ExplicitRungeKuttaExport::getDataDeclarations( declarations, dataStruct );
solver->getDataDeclarations( declarations,dataStruct );
declarations.addDeclaration( rk_A,dataStruct );
declarations.addDeclaration( rk_b,dataStruct );
declarations.addDeclaration( rk_auxSolver,dataStruct );
declarations.addDeclaration( rk_zzz,dataStruct );
declarations.addDeclaration( rk_zTemp,dataStruct );
declarations.addDeclaration( rk_diffZ,dataStruct );
declarations.addDeclaration( rk_delta,dataStruct );
declarations.addDeclaration( rk_prev,dataStruct );
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 ExportGaussNewtonCN2::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
declarations.addDeclaration(sbar, dataStruct);
declarations.addDeclaration(x0, dataStruct);
declarations.addDeclaration(Dx0, dataStruct);
declarations.addDeclaration(T1, dataStruct);
declarations.addDeclaration(T2, dataStruct);
declarations.addDeclaration(C, dataStruct);
declarations.addDeclaration(W1, dataStruct);
declarations.addDeclaration(W2, dataStruct);
declarations.addDeclaration(E, dataStruct);
declarations.addDeclaration(QDy, dataStruct);
declarations.addDeclaration(w1, dataStruct);
declarations.addDeclaration(w2, dataStruct);
declarations.addDeclaration(lbValues, dataStruct);
declarations.addDeclaration(ubValues, dataStruct);
declarations.addDeclaration(lbAValues, dataStruct);
declarations.addDeclaration(ubAValues, dataStruct);
declarations.addDeclaration(H, dataStruct);
declarations.addDeclaration(A, dataStruct);
declarations.addDeclaration(g, dataStruct);
declarations.addDeclaration(lb, dataStruct);
declarations.addDeclaration(ub, dataStruct);
declarations.addDeclaration(lbA, dataStruct);
declarations.addDeclaration(ubA, dataStruct);
declarations.addDeclaration(xVars, dataStruct);
declarations.addDeclaration(yVars, dataStruct);
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 ExportNLPSolver::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
declarations.addDeclaration(state, dataStruct);
declarations.addDeclaration(x, dataStruct);
declarations.addDeclaration(z, dataStruct);
declarations.addDeclaration(u, dataStruct);
declarations.addDeclaration(p, dataStruct);
declarations.addDeclaration(d, dataStruct);
declarations.addDeclaration(y, dataStruct);
declarations.addDeclaration(yN, dataStruct);
declarations.addDeclaration(Dy, dataStruct);
declarations.addDeclaration(DyN, dataStruct);
declarations.addDeclaration(evGx, dataStruct);
declarations.addDeclaration(evGu, dataStruct);
declarations.addDeclaration(objS, dataStruct);
declarations.addDeclaration(objAuxVar, dataStruct);
declarations.addDeclaration(objValueIn, dataStruct);
declarations.addDeclaration(objValueOut, dataStruct);
declarations.addDeclaration(Q1, dataStruct);
declarations.addDeclaration(Q2, dataStruct);
declarations.addDeclaration(R1, dataStruct);
declarations.addDeclaration(R2, dataStruct);
declarations.addDeclaration(objSEndTerm, dataStruct);
declarations.addDeclaration(QN1, dataStruct);
declarations.addDeclaration(QN2, dataStruct);
declarations.addDeclaration(SAC, dataStruct);
declarations.addDeclaration(xAC, dataStruct);
declarations.addDeclaration(DxAC, dataStruct);
declarations.addDeclaration(conAuxVar, dataStruct);
declarations.addDeclaration(conValueIn, dataStruct);
declarations.addDeclaration(conValueOut, dataStruct);
declarations.addDeclaration(pacEvH, dataStruct);
declarations.addDeclaration(pacEvHx, dataStruct);
declarations.addDeclaration(pacEvHu, dataStruct);
declarations.addDeclaration(pacEvHxd, dataStruct);
declarations.addDeclaration(pocEvH, dataStruct);
declarations.addDeclaration(pocEvHx, dataStruct);
declarations.addDeclaration(pocEvHu, dataStruct);
declarations.addDeclaration(pocEvHxd, dataStruct);
// Arrival cost stuff
declarations.addDeclaration(acA, dataStruct);
declarations.addDeclaration(acb, dataStruct);
declarations.addDeclaration(acP, dataStruct);
declarations.addDeclaration(acTmp, dataStruct);
declarations.addDeclaration(acWL, dataStruct);
declarations.addDeclaration(acVL, dataStruct);
declarations.addDeclaration(acHx, dataStruct);
declarations.addDeclaration(acHu, dataStruct);
declarations.addDeclaration(acXx, dataStruct);
declarations.addDeclaration(acXu, dataStruct);
declarations.addDeclaration(acXTilde, dataStruct);
declarations.addDeclaration(acHTilde, dataStruct);
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 CondensingExport::getFunctionDeclarations( ExportStatementBlock& declarations
) const
{
declarations.addDeclaration( condense1 );
declarations.addDeclaration( condense2 );
declarations.addDeclaration( expand );
declarations.addDeclaration( setupQP );
declarations.addDeclaration( multiplyQC1 );
declarations.addDeclaration( multiplyQE1 );
declarations.addDeclaration( multiplyQDX1 );
declarations.addDeclaration( multiplyRDU1 );
declarations.addDeclaration( multiplyQC2 );
declarations.addDeclaration( multiplyQE2 );
declarations.addDeclaration( multiplyQDX2 );
declarations.addDeclaration( multiplyC );
declarations.addDeclaration( multiplyE );
declarations.addDeclaration( multiplyG1 );
declarations.addDeclaration( multiplyCD1 );
declarations.addDeclaration( multiplyEU1 );
declarations.addDeclaration( multiplyG0 );
declarations.addDeclaration( multiplyH00 );
declarations.addDeclaration( multiplyH01 );
declarations.addDeclaration( multiplyH11 );
declarations.addDeclaration( getObjectiveValue );
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::getDataDeclarations( ExportStatementBlock& declarations,
ExportStruct dataStruct
) const
{
// ACADO_VARIABLES
declarations.addDeclaration( x,dataStruct );
declarations.addDeclaration( u,dataStruct );
declarations.addDeclaration( p,dataStruct );
declarations.addDeclaration( xRef,dataStruct );
declarations.addDeclaration( uRef,dataStruct );
declarations.addDeclaration( x0Ref,dataStruct );
declarations.addDeclaration( x0Ref2,dataStruct );
if ( Q.isGiven() == BT_FALSE )
declarations.addDeclaration( Q,dataStruct );
if ( R.isGiven() == BT_FALSE )
declarations.addDeclaration( R,dataStruct );
if ( QF.isGiven() == BT_FALSE )
declarations.addDeclaration( QF,dataStruct );
if ( QS.isGiven() == BT_FALSE )
declarations.addDeclaration( QS,dataStruct );
if ( QS2.isGiven() == BT_FALSE )
declarations.addDeclaration( QS2,dataStruct );
// ACADO_WORKSPACE
if ( QQF.isGiven() == BT_FALSE )
declarations.addDeclaration( QQF,dataStruct );
declarations.addDeclaration( state,dataStruct );
declarations.addDeclaration( residuum,dataStruct );
declarations.addDeclaration( g0,dataStruct );
declarations.addDeclaration( g1,dataStruct );
declarations.addDeclaration( H00,dataStruct );
declarations.addDeclaration( H01,dataStruct );
declarations.addDeclaration( H11,dataStruct );
declarations.addDeclaration( lbA,dataStruct );
declarations.addDeclaration( ubA,dataStruct );
declarations.addDeclaration( d,dataStruct );
declarations.addDeclaration( deltaX0,dataStruct );
declarations.addDeclaration( C,dataStruct );
declarations.addDeclaration( QC,dataStruct );
declarations.addDeclaration( Gx,dataStruct );
declarations.addDeclaration( E,dataStruct );
declarations.addDeclaration( QE,dataStruct );
declarations.addDeclaration( Gu,dataStruct );
declarations.addDeclaration( Dx0,dataStruct );
declarations.addDeclaration( Dx0b,dataStruct );
declarations.addDeclaration( Dx,dataStruct );
declarations.addDeclaration( QDx,dataStruct );
declarations.addDeclaration( Du,dataStruct );
declarations.addDeclaration( RDu,dataStruct );
return SUCCESSFUL_RETURN;
}