ExportVariable FunctionEvaluationTree::getGlobalExportVariable( ) const
{
// int run1;
// int nni = 0;
// for( run1 = 0; run1 < n; run1++ )
// if( lhs_comp[run1]+1 > nni )
// nni = lhs_comp[run1]+1;
// TODO fix this in a correct way
if (auxVariableStructName.isEmpty())
return ExportVariable(auxVariableName, n, 1, REAL, ACADO_LOCAL);
else
return ExportVariable(auxVariableName, n, 1, REAL, ACADO_WORKSPACE);
}
void CCfgBuffVar::SetVar(const char *p_szValue)
{
if (strcmp(p_szValue,m_szValue) == 0)
{ return;
}
LOG("CCfgBuffVar::SetVar: f=%s, g=%s var=%s val= %s -> %s", m_szFile, m_szGroup,m_szVar,
m_szValue, p_szValue);
strncpy(m_szValue, p_szValue, sizeof(m_szValue) - 1);
m_szValue[sizeof(m_szValue) - 1] = 0; // just for safety
ExportVariable(m_szFile,m_szGroup,m_szVar,m_szValue);
}
BEGIN_NAMESPACE_ACADO
//
// PUBLIC MEMBER FUNCTIONS:
//
ExportLinearSolver::ExportLinearSolver( UserInteraction* _userInteraction,
const String& _commonHeaderName
) : ExportAlgorithm(_userInteraction, _commonHeaderName)
{
REUSE = BT_TRUE;
UNROLLING = BT_FALSE;
dim = nRows = nCols = nBacksolves = 0;
determinant = ExportVariable("det", 1, 1, REAL, ACADO_LOCAL, BT_TRUE);
}
returnValue SIMexport::exportAcadoHeader( const String& _dirName,
const String& _fileName,
const String& _realString,
const String& _intString,
int _precision
) const
{
int qpSolver;
get( QP_SOLVER,qpSolver );
int operatingSystem;
get( OPERATING_SYSTEM,operatingSystem );
int useSinglePrecision;
get( USE_SINGLE_PRECISION,useSinglePrecision );
int fixInitialState;
get( FIX_INITIAL_STATE,fixInitialState );
String fileName( _dirName );
fileName << "/" << _fileName;
ExportFile acadoHeader( fileName,"", _realString,_intString,_precision );
acadoHeader.addStatement( "#include <stdio.h>\n" );
acadoHeader.addStatement( "#include <math.h>\n" );
if ( (OperatingSystem)operatingSystem == OS_WINDOWS )
{
acadoHeader.addStatement( "#include <windows.h>\n" );
}
else
{
// OS_UNIX
acadoHeader.addStatement( "#include <time.h>\n" );
acadoHeader.addStatement( "#include <sys/stat.h>\n" );
acadoHeader.addStatement( "#include <sys/time.h>\n" );
}
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "#ifndef ACADO_H\n" );
acadoHeader.addStatement( "#define ACADO_H\n" );
acadoHeader.addLinebreak( );
switch ( (QPSolverName)qpSolver )
{
case QP_CVXGEN:
acadoHeader.addStatement( "#define USE_CVXGEN\n" );
acadoHeader.addStatement( "#include \"cvxgen/solver.h\"\n" );
acadoHeader.addLinebreak( 2 );
if ( (BooleanType)useSinglePrecision == BT_TRUE )
acadoHeader.addStatement( "typedef float real_t;\n" );
else
acadoHeader.addStatement( "typedef double real_t;\n" );
acadoHeader.addLinebreak( 2 );
break;
case QP_QPOASES:
acadoHeader.addStatement( "#ifndef __MATLAB__\n" );
acadoHeader.addStatement( "#ifdef __cplusplus\n" );
acadoHeader.addStatement( "extern \"C\"\n" );
acadoHeader.addStatement( "{\n" );
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addStatement( "#include \"qpoases/solver.hpp\"\n" );
acadoHeader.addLinebreak( 2 );
break;
case QP_QPOASES3:
acadoHeader.addStatement( "#include \"qpoases3/solver.h\"\n" );
acadoHeader.addLinebreak( 2 );
break;
case QP_NONE:
if ( (BooleanType)useSinglePrecision == BT_TRUE )
acadoHeader.addStatement( "typedef float real_t;\n" );
else
acadoHeader.addStatement( "typedef double real_t;\n" );
acadoHeader.addLinebreak( 2 );
break;
default:
return ACADOERROR( RET_INVALID_OPTION );
}
Vector nMeasV = getNumMeas();
Vector nOutV = getDimOutputs();
if( nMeasV.getDim() != nOutV.getDim() ) return ACADOERROR( RET_INVALID_OPTION );
//
// Some common defines
//
acadoHeader.addComment( "COMMON DEFINITIONS: " );
acadoHeader.addComment( "--------------------------------" );
acadoHeader.addLinebreak( 2 );
if( (uint)nOutV.getDim() > 0 ) {
acadoHeader.addComment( "Dimension of the output functions" );
acadoHeader.addDeclaration( ExportVariable( "NOUT",nOutV,STATIC_CONST_INT ) );
acadoHeader.addComment( "Measurements of the output functions per shooting interval" );
acadoHeader.addDeclaration( ExportVariable( "NMEAS",nMeasV,STATIC_CONST_INT ) );
}
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "Number of control intervals" );
acadoHeader.addStatement( (String)"#define ACADO_N " << getN() << "\n");
acadoHeader.addComment( "Number of differential states" );
acadoHeader.addStatement( (String)"#define ACADO_NX " << getNX() << "\n" );
acadoHeader.addComment( "Number of differential state derivatives" );
acadoHeader.addStatement( (String)"#define ACADO_NDX " << getNDX() << "\n" );
acadoHeader.addComment( "Number of algebraic states" );
acadoHeader.addStatement( (String)"#define ACADO_NXA " << getNXA() << "\n" );
acadoHeader.addComment( "Number of controls" );
acadoHeader.addStatement( (String)"#define ACADO_NU " << getNU() << "\n" );
acadoHeader.addComment( "Number of parameters" );
acadoHeader.addStatement( (String)"#define ACADO_NP " << getNP() << "\n" );
acadoHeader.addComment( "Number of output functions" );
acadoHeader.addStatement( (String)"#define NUM_OUTPUTS " << (uint)nOutV.getDim() << "\n" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "GLOBAL VARIABLES: " );
acadoHeader.addComment( "--------------------------------" );
ExportStatementBlock tempHeader;
if ( collectDataDeclarations( tempHeader,ACADO_VARIABLES ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
acadoHeader.addStatement( "typedef struct ACADOvariables_ {\n" );
acadoHeader.addStatement( tempHeader );
#ifdef WIN32
if( tempHeader.getNumStatements() == 0 ) {
acadoHeader.addStatement( "int dummy; \n" );
}
#endif
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "} ACADOvariables;\n" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "GLOBAL WORKSPACE: " );
acadoHeader.addComment( "--------------------------------" );
acadoHeader.addStatement( "typedef struct ACADOworkspace_ {\n" );
if ( collectDataDeclarations( acadoHeader,ACADO_WORKSPACE ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "} ACADOworkspace;\n" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "GLOBAL FORWARD DECLARATIONS: " );
acadoHeader.addComment( "-------------------------------------" );
if ( collectFunctionDeclarations( acadoHeader ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
acadoHeader.addComment( "-------------------------------------" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "EXTERN DECLARATIONS: " );
acadoHeader.addComment( "-------------------------------------" );
acadoHeader.addStatement( "extern ACADOworkspace acadoWorkspace;\n" );
acadoHeader.addStatement( "extern ACADOvariables acadoVariables;\n" );
acadoHeader.addComment( "-------------------------------------" );
switch ( (QPSolverName) qpSolver )
{
case QP_CVXGEN:
break;
case QP_QPOASES:
acadoHeader.addStatement( "#ifndef __MATLAB__\n");
acadoHeader.addStatement( "#ifdef __cplusplus\n" );
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "} /* extern \"C\" */\n" );
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addStatement( "#endif\n" );
break;
case QP_QPOASES3:
break;
case QP_NONE:
break;
default:
return ACADOERROR( RET_INVALID_OPTION );
}
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addLinebreak( );
acadoHeader.addComment( "END OF FILE." );
acadoHeader.addLinebreak( );
return acadoHeader.exportCode( );
}
returnValue DiscreteTimeExport::setup( )
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
// non equidistant integration grids not implemented for NARX integrators
if( !equidistant ) return ACADOERROR( RET_INVALID_OPTION );
String fileName( "integrator.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
ExportIndex run( "run" );
ExportIndex i( "i" );
ExportIndex j( "j" );
ExportIndex k( "k" );
ExportIndex tmp_index("tmp_index");
uint diffsDim = NX*(NX+NU);
uint inputDim = NX*(NX+NU+1) + NU + NP;
// setup INTEGRATE function
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, BT_TRUE );
rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
if( equidistantControlGrid() ) {
integrate = ExportFunction( "integrate", rk_eta, reset_int );
}
else {
integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
}
integrate.setReturnValue( error_code );
integrate.addIndex( run );
integrate.addIndex( i );
integrate.addIndex( j );
integrate.addIndex( k );
integrate.addIndex( tmp_index );
rhs_in = ExportVariable( "x", inputDim-diffsDim, 1, REAL, ACADO_LOCAL );
rhs_out = ExportVariable( "f", NX, 1, REAL, ACADO_LOCAL );
fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
rk_xxx = ExportVariable( "rk_xxx", 1, inputDim-diffsDim, REAL, structWspace );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX1, NX1+NU, REAL, structWspace );
rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX2, NX1+NX2+NU, REAL, structWspace );
}
rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2, NX1+NX2+NU, REAL, structWspace );
ExportVariable numInt( "numInts", 1, 1, INT );
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
integrate.addStatement( String( "int " ) << numInt.getName() << " = " << numStepsV.getName() << "[" << rk_index.getName() << "];\n" );
}
integrate.addStatement( rk_xxx.getCols( NX,inputDim-diffsDim ) == rk_eta.getCols( NX+diffsDim,inputDim ) );
integrate.addLinebreak( );
// integrator loop:
ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
ExportStatementBlock *loop;
if( equidistantControlGrid() ) {
loop = &tmpLoop;
}
else {
loop = &integrate;
loop->addStatement( String("for(") << run.getName() << " = 0; " << run.getName() << " < " << numInt.getName() << "; " << run.getName() << "++ ) {\n" );
}
loop->addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
// Set rk_diffsPrev:
loop->addStatement( String("if( run > 0 ) {\n") );
if( NX1 > 0 ) {
ExportForLoop loopTemp1( i,0,NX1 );
loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX1 ) );
if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
loop->addStatement( loopTemp1 );
}
if( NX2 > 0 ) {
ExportForLoop loopTemp2( i,0,NX2 );
loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+NXA+NX1*NX,i*NX+NX+NXA+NX1*NX+NX1+NX2 ) );
if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX1*NU,i*NU+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
loop->addStatement( loopTemp2 );
}
loop->addStatement( String("}\n") );
}
// evaluate states:
if( NX1 > 0 ) {
loop->addFunctionCall( lin_input.getName(), rk_xxx, rk_eta.getAddress(0,0) );
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameRHS(), rk_xxx, rk_eta.getAddress(0,NX1) );
}
// evaluate sensitivities:
if( NX1 > 0 ) {
for( uint i1 = 0; i1 < NX1; i1++ ) {
for( uint i2 = 0; i2 < NX1; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,i2,i2+1) == A11(i1,i2) );
}
for( uint i2 = 0; i2 < NU; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,NX1+i2,NX1+i2+1) == B11(i1,i2) );
}
}
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameDiffsRHS(), rk_xxx, rk_diffsNew2.getAddress(0,0) );
}
// computation of the sensitivities using chain rule:
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "if( run == 0 ) {\n" ) );
}
// PART 1
updateInputSystem(loop, i, j, tmp_index);
// PART 2
updateImplicitSystem(loop, i, j, tmp_index);
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "}\n" ) );
loop->addStatement( String( "else {\n" ) );
// PART 1
propagateInputSystem(loop, i, j, k, tmp_index);
// PART 2
propagateImplicitSystem(loop, i, j, k, tmp_index);
loop->addStatement( String( "}\n" ) );
}
// end of the integrator loop.
if( !equidistantControlGrid() ) {
loop->addStatement( "}\n" );
}
else {
integrate.addStatement( *loop );
}
// PART 1
if( NX1 > 0 ) {
Matrix zeroR = zeros(1, NX2);
ExportForLoop loop1( i,0,NX1 );
loop1.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1,i*NX+NX+NXA+NX ) == zeroR );
integrate.addStatement( loop1 );
}
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
