/* identitical to same function within the class Process! */
returnValue Actuator::checkInputConsistency( const VariablesGrid& _u,
const VariablesGrid& _p
) const
{
if ( _u.getNumPoints( ) < 2 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( _u.getNumRows( ) != getNU( ) )
return ACADOERROR( RET_CONTROL_DIMENSION_MISMATCH );
if ( _p.isEmpty( ) == BT_TRUE )
{
if ( getNP( ) > 0 )
return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
}
else
{
if ( _p.getNumPoints( ) < 2 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( _p.getNumRows( ) != getNP( ) )
return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
if ( acadoIsEqual( _u.getFirstTime( ),_p.getFirstTime( ) ) == BT_FALSE )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( acadoIsEqual( _u.getLastTime( ),_p.getLastTime( ) ) == BT_FALSE )
return ACADOERROR( RET_INVALID_ARGUMENTS );
}
return SUCCESSFUL_RETURN;
}
returnValue Actuator::addActuatorNoise( VariablesGrid& _u,
VariablesGrid& _p
) const
{
if ( hasNoise( ) == BT_FALSE )
return SUCCESSFUL_RETURN;
// generate current noise
VariablesGrid currentNoise;
if ( generateNoise( _u.getFirstTime(),_u.getLastTime(),currentNoise ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_GENERATING_NOISE_FAILED );
// determine common grid
Grid commonGrid, tmpGrid;
currentNoise.getGrid( tmpGrid );
_u.getGrid( commonGrid );
commonGrid & tmpGrid;
// adapt input grids and add noise
_u.refineGrid( commonGrid );
currentNoise.refineGrid( commonGrid );
_u += currentNoise.getValuesSubGrid( 0,getNU()-1 );
if ( _p.isEmpty( ) == BT_FALSE )
{
_p.refineGrid( commonGrid );
_p += currentNoise.getValuesSubGrid( getNU(),getNU()+getNP()-1 );
}
return SUCCESSFUL_RETURN;
}
returnValue Actuator::setParameterDeadTimes( const Vector& _deadTimes
)
{
if ( _deadTimes.getDim( ) != getNP( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( _deadTimes.getMin( ) < 0.0 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( deadTimes.getDim( ) == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
for( uint i=0; i<getNP(); ++i )
deadTimes( getNU()+i ) = _deadTimes( i );
return SUCCESSFUL_RETURN;
}
returnValue Actuator::setParameterNoise( const Noise& _noise,
double _noiseSamplingTime
)
{
if ( _noise.getDim( ) != getNP( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
for( uint i=0; i<getNP( ); ++i )
{
if ( additiveNoise[ getNU()+i ] != 0 )
delete additiveNoise[ getNU()+i ];
additiveNoise[ getNU()+i ] = _noise.clone( i );
}
noiseSamplingTimes.setAll( _noiseSamplingTime );
return SUCCESSFUL_RETURN;
}
returnValue Actuator::setParameterDeadTimes( double _deadTime
)
{
if ( _deadTime < 0.0 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( deadTimes.getDim( ) == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
for( uint i=0; i<getNP(); ++i )
deadTimes( getNU()+i ) = _deadTime;
return SUCCESSFUL_RETURN;
}
returnValue Actuator::setParameterNoise( uint idx,
const Noise& _noise,
double _noiseSamplingTime
)
{
if ( ( idx >= getNP( ) ) || ( _noise.getDim( ) != 1 ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( additiveNoise[ getNU()+idx ] != 0 )
delete additiveNoise[ getNU()+idx ];
additiveNoise[ getNU()+idx ] = _noise.clone( );
if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
ACADOWARNING( RET_NO_DIFFERENT_NOISE_SAMPLING_FOR_DISCRETE );
noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::setupEvaluation( )
{
x.setup( "x", (getN()+1), getNX(), REAL,ACADO_VARIABLES );
u.setup( "u", getN(), getNU(), REAL,ACADO_VARIABLES );
p.setup( "p", 1, getNP(), REAL,ACADO_VARIABLES );
state.setup ( "state", 1,getNX()*(getNX()+getNU()+1) + getNU() +getNP(), REAL,ACADO_WORKSPACE );
if ( isInitialStateFixed( ) == BT_FALSE )
{
x0Ref.setup ( "x0Ref", 1, getNX(), REAL,ACADO_VARIABLES );
x0Ref2.setup( "x0Ref2", 1, getNX(), REAL,ACADO_VARIABLES );
}
E.setup ( "E", getN()*getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
lbA.setup( "lbA", getNumStateBounds(), 1, REAL,ACADO_WORKSPACE );
ubA.setup( "ubA", getNumStateBounds(), 1, REAL,ACADO_WORKSPACE );
Matrix zeroXU = zeros( getNX(),getNU() );
Matrix idX = eye( getNX() );
//
// setupQP
//
setupQP.setup( "setupQP" );
if ( performsSingleShooting() == BT_TRUE )
setupQP.addStatement( state.getCols( 0,getNX() ) == x.getRow(0) );
// TODO: this part should be preinitialized. More specifically, E & QE matrices should be preinitializes to 0.
uint run1, run2;
for( run1 = 0; run1 < getN()-1; run1++ )
for( run2 = 1+run1; run2 < getN(); run2++ )
setupQP.addStatement( E.getSubMatrix( run1*getNX(),(run1+1)*getNX(), run2*getNU(),(run2+1)*getNU() ) == zeroXU );
// Write state bounds to the file
// TODO: Since the bounds are fixed in this case, they should be preinitialized
if( getNumStateBounds( ) > 0 )
{
Vector xLowerBounds(nxBounds), xUpperBounds(nxBounds);
for( run1 = 0; run1 < nxBounds; run1++ )
{
xLowerBounds(run1) = xBounds.getLowerBound( xBoundsIdx[run1]/getNX(),xBoundsIdx[run1]%getNX() );
xUpperBounds(run1) = xBounds.getUpperBound( xBoundsIdx[run1]/getNX(),xBoundsIdx[run1]%getNX() );
}
setupQP.addStatement( lbA == xLowerBounds );
setupQP.addStatement( ubA == xUpperBounds );
}
setupQP.addLinebreak( );
if ( isInitialStateFixed( ) == BT_FALSE )
{
setupQP.addStatement( Dx0 == x.getRow(0) - x0Ref );
setupQP.addStatement( Dx0b == x.getRow(0) - x0Ref2 );
setupQP.addLinebreak( );
}
// compute QQF if necessary
if ( QQF.isGiven( ) == BT_FALSE )
setupQP.addStatement( QQF == Q + QF );
ExportIndex reset( String("reset") );
setupQP.addIndex( reset );
setupQP.addStatement( reset == 1 );
ExportIndex run( String("run1") );
ExportForLoop loop( run, 0,getN() );
setupQP.addIndex( run );
if ( performsSingleShooting() == BT_FALSE ) {
loop.addStatement( reset == 1 );
loop.addStatement( state.getCols( 0,getNX() ) == x.getRow( run ) );
}
// no free parameters implemented yet!
uint uIdx = getNX() * ( 1+getNX() );
uint pIdx = getNX() * ( 1+getNX()+getNU() );
uIdx = pIdx;
pIdx = pIdx + getNU();
loop.addStatement( state.getCols( uIdx,pIdx ) == u.getRow( run ) );
loop.addStatement( state.getCols( pIdx,pIdx+getNP() ) == p );
loop.addLinebreak( );
if ( integrator->equidistantControlGrid() )
{
loop.addFunctionCall( "integrate", state, reset.makeArgument() );
}
else
{
loop.addFunctionCall( "integrate", state, reset.makeArgument(), run.makeArgument() );
}
if( performsSingleShooting() ) loop.addStatement( reset == 0 );
if ( performsSingleShooting() == BT_TRUE )
{
loop.addStatement( x.getRow( run+1 ) == state.getCols( 0,getNX() ) );
}
else
{
//
// Multiple shooting
// TODO: Check the sign here
//
loop.addStatement( residuum.getRow( run ) == state.getCols( 0,getNX() ) - x.getRow( run+1 ) );
}
loop.addLinebreak( );
loop.addFunctionCall( condense1, run.makeArgument(),state );
setupQP.addStatement( loop );
setupQP.addLinebreak( );
setupQP.addFunctionCall( condense2 );
////////////////////////////////////////////////////////////////////////////
//
// Get objective value
//
////////////////////////////////////////////////////////////////////////////
ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, BT_TRUE);
ExportVariable tmpDx("tmpDx", 1, getNX(), REAL, ACADO_LOCAL );
ExportVariable tmpDu("tmpDu", 1, getNU(), REAL, ACADO_LOCAL );
getObjectiveValue.setup( "getObjectiveValue" );
getObjectiveValue.setReturnValue( tmp );
getObjectiveValue.addVariable( tmpDx );
getObjectiveValue.addVariable( tmpDu );
getObjectiveValue.addStatement( tmp == 0.0 );
getObjectiveValue.addLinebreak( );
for (unsigned i = 0; i < getN(); ++i)
{
getObjectiveValue.addStatement( tmpDx == Dx.getRow( i ) * Q );
getObjectiveValue.addStatement( tmp += Dx.getRow( i ) * tmpDx.getTranspose() );
}
getObjectiveValue.addLinebreak( );
for (unsigned i = 0; i < getN(); ++i)
{
getObjectiveValue.addStatement( tmpDu == Du.getRow( i ) * R );
getObjectiveValue.addStatement( tmp += Du.getRow( i ) * tmpDu.getTranspose() );
}
getObjectiveValue.addLinebreak( );
getObjectiveValue.addStatement( tmp == tmp * Matrix( 0.5 ) );
return SUCCESSFUL_RETURN;
}
returnValue Actuator::getDelayedInputGrids( const VariablesGrid& _u,
const VariablesGrid& _p,
VariablesGrid& _uDelayed,
VariablesGrid& _pDelayed
) const
{
// determine common time grid for delayed inputs:
Grid delayedInputTimeGrid = lastSignal.getTimePoints( );
// make sure that last time instant of horizon lies within the grid
if ( acadoIsEqual( lastSignal.getLastTime(),_u.getLastTime( ) ) == BT_FALSE )
delayedInputTimeGrid.addTime( _u.getLastTime( ) );
// delayedInputTimeGrid.print();
// add grids of all delayed input components
for( uint i=0; i<getNU( ); ++i )
delayedInputTimeGrid = delayedInputTimeGrid & ( _u.getTimePoints( ).shiftTimes( deadTimes(i) ) );
// _u.getTimePoints( ).print();
// _u.getTimePoints( ).shiftTimes( deadTimes(0) ).print();
// delayedInputTimeGrid.print();
if ( _p.isEmpty( ) == BT_FALSE )
{
for( uint i=0; i<getNP( ); ++i )
delayedInputTimeGrid = delayedInputTimeGrid & ( _p.getTimePoints( ).shiftTimes( deadTimes(getNU()+i) ) );
}
VariablesGrid tmp;
// setup common variables grid for delayed inputs
_uDelayed.init( );
_pDelayed.init( );
for( uint i=0; i<getNU( ); ++i )
{
// tmp.print("tmp");
tmp = lastSignal( i );
// tmp.print("tmp");
tmp.merge( _u( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
// tmp.print("tmp");
tmp.refineGrid( delayedInputTimeGrid );
// tmp.print("tmp");
_uDelayed.appendValues( tmp );
}
if ( _p.isEmpty( ) == BT_FALSE )
{
for( uint i=0; i<getNP( ); ++i )
{
tmp = lastSignal( getNU()+i );
tmp.merge( _p( i ).shiftTimes( deadTimes(getNU()+i) ),MM_REPLACE,BT_FALSE );
tmp.refineGrid( delayedInputTimeGrid );
_pDelayed.appendValues( tmp );
}
}
return SUCCESSFUL_RETURN;
}
returnValue SimulationEnvironment::step( )
{
/* Consistency check. */
if ( getStatus( ) != BS_READY )
return ACADOERROR( RET_BLOCK_NOT_READY );
++nSteps;
acadoPrintf( "\n*** Simulation Loop No. %d (starting at time %.3f) ***\n",nSteps,simulationClock.getTime( ) );
/* Perform one single simulation loop */
Vector u, p;
Vector uPrevious, pPrevious;
if ( getNU( ) > 0 )
feedbackControl.evaluate( simulationClock.getTime( ),uPrevious );
if ( getNP( ) > 0 )
feedbackParameter.evaluate( simulationClock.getTime( ),pPrevious );
VariablesGrid y;
Vector yPrevious;
if ( getNY( ) > 0 )
processOutput.evaluate( simulationClock.getTime( ),yPrevious );
// step controller
// yPrevious.print("controller input y");
if ( controller->step( simulationClock.getTime( ),yPrevious ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
double compDelay = determineComputationalDelay( controller->getPreviousRealRuntime( ) );
double nextSamplingInstant = controller->getNextSamplingInstant( simulationClock.getTime( ) );
nextSamplingInstant = round( nextSamplingInstant * 1.0e6 ) / 1.0e6;
// obtain new controls and parameters
if ( controller->getU( u ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// u.print("controller output u");
if ( controller->getP( p ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
if ( acadoIsEqual( simulationClock.getTime( ),endTime ) == BT_TRUE )
{
simulationClock.init( nextSamplingInstant );
return SUCCESSFUL_RETURN;
}
if ( fabs( compDelay ) < 100.0*EPS )
{
// step process without computational delay
if ( process->step( simulationClock.getTime( ),nextSamplingInstant,u,p ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// Obtain current process output
if ( process->getY( y ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// y.print("process output y");
// update history
if ( getNU( ) > 0 )
feedbackControl. add( simulationClock.getTime( ),nextSamplingInstant,u );
if ( getNP( ) > 0 )
feedbackParameter.add( simulationClock.getTime( ),nextSamplingInstant,p );
if ( getNY( ) > 0 )
processOutput. add( y,IM_LINEAR );
}
else
{
// step process WITH computational delay
if ( simulationClock.getTime( )+compDelay > nextSamplingInstant )
return ACADOERROR( RET_COMPUTATIONAL_DELAY_TOO_BIG );
Grid delayGrid( 3 );
delayGrid.setTime( simulationClock.getTime( ) );
delayGrid.setTime( simulationClock.getTime( )+compDelay );
delayGrid.setTime( nextSamplingInstant );
VariablesGrid uDelayed( u.getDim( ),delayGrid,VT_CONTROL );
uDelayed.setVector( 0,uPrevious );
uDelayed.setVector( 1,u );
uDelayed.setVector( 2,u );
VariablesGrid pDelayed( p.getDim( ),delayGrid,VT_PARAMETER );
pDelayed.setVector( 0,pPrevious );
pDelayed.setVector( 1,p );
pDelayed.setVector( 2,p );
if ( process->step( uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// Obtain current process output
if ( process->getY( y ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// update history
if ( getNU( ) > 0 )
feedbackControl. add( uDelayed,IM_CONSTANT );
if ( getNP( ) > 0 )
feedbackParameter.add( pDelayed,IM_CONSTANT );
if ( getNY( ) > 0 )
processOutput. add( y,IM_LINEAR );
}
// update simulation clock
simulationClock.init( nextSamplingInstant );
return SUCCESSFUL_RETURN;
}
returnValue SimulationEnvironment::init( const Vector &x0_,
const Vector &p_
)
{
// 1) initialise all sub-blocks and evaluate process at start time
Vector uStart, pStart;
VariablesGrid yStart;
if ( controller != 0 )
{
if ( controller->init( startTime,x0_,p_ ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
if ( controller->getU( uStart ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
if ( controller->getP( pStart ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
}
else
return ACADOERROR( RET_NO_CONTROLLER_SPECIFIED );
if ( process != 0 )
{
if ( process->init( startTime,x0_,uStart,pStart ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
if ( process->getY( yStart ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
}
else
return ACADOERROR( RET_NO_PROCESS_SPECIFIED );
simulationClock.init( startTime );
// 2) consistency checks
if ( ( process != 0 ) && ( controller != 0 ) )
{
if ( process->getNY( ) != controller->getNY( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
// printf( "%d %d\n",process->getNU( ),controller->getNU( ) );
if ( process->getNU( ) != controller->getNU( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
if ( process->getNP( ) > controller->getNP( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
}
// initialise history...
if ( getNU( ) > 0 )
feedbackControl. add( startTime-10.0*EPS,startTime,uStart );
if ( getNP( ) > 0 )
feedbackParameter.add( startTime-10.0*EPS,startTime,pStart );
if ( getNY( ) > 0 )
processOutput. add( startTime-10.0*EPS,startTime,yStart.getVector(0) );
// ... and update block status
setStatus( BS_READY );
return SUCCESSFUL_RETURN;
}
returnValue FunctionEvaluationTree::exportCode( FILE *file,
const char *fcnName,
const char *realString,
int precision,
uint _numX,
uint _numXA,
uint _numU,
uint _numP,
uint _numDX
) const{
int run1;
int nni = 0;
for (run1 = 0; run1 < n; run1++)
if (lhs_comp[run1] + 1 > nni)
nni = lhs_comp[run1] + 1;
unsigned numX = _numX > 0 ? _numX : getNX();
unsigned numXA = _numXA > 0 ? _numXA : getNXA();
unsigned numU = _numU > 0 ? _numU : getNU();
unsigned numP = _numP > 0 ? _numP : getNP();
unsigned numDX = _numDX > 0 ? _numDX : getNDX();
acadoFPrintf(file, "void %s(const %s* in, %s* out)\n{\n", fcnName,
realString, realString);
if( numX > 0 ){
acadoFPrintf(file,"const %s* xd = in;\n", realString );
}
if( numXA > 0 ){
acadoFPrintf(file,"const %s* xa = in + %d;\n", realString,numX );
}
if( getNU() > 0 ){
acadoFPrintf(file,"const %s* u = in + %d;\n", realString,numX+numXA );
}
if( getNUI() > 0 ){
acadoFPrintf(file,"const %s* v = in + %d;\n", realString,numX+numXA+numU );
}
if( numP > 0 ){
acadoFPrintf(file,"const %s* p = in + %d;\n", realString,numX+numXA+numU+getNUI() );
}
if( getNPI() > 0 ){
acadoFPrintf(file,"const %s* q = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP );
}
if( getNW() > 0 ){
acadoFPrintf(file,"const %s* w = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP+getNPI() );
}
if( numDX > 0 ){
acadoFPrintf(file,"const %s* dx = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP+getNPI()+getNW() );
}
if( getNT() > 0 ){
acadoFPrintf(file,"const %s* t = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP+getNPI()+getNW()+numDX );
}
if (n > 0)
{
acadoFPrintf(file, "/* Vector of auxiliary variables; number of elements: %d. */\n", n);
acadoFPrintf(file, "%s* a = %s;\n", realString, globalExportVariableName.getName() );
acadoFPrintf(file, "\n/* Compute intermediate quantities; */\n");
}
Stream *auxVarIndividualNames = new Stream[nni];
for( run1 = 0; run1 < n; run1++ )
auxVarIndividualNames[lhs_comp[run1]] << "a" << "[" << run1 << "]";
// Export intermediate quantities
for (run1 = 0; run1 < n; run1++)
{
// Convert the name for intermediate variables for subexpressions
sub[run1]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames);
acadoFPrintf(file, "%s[%d] = ", "a", run1);
file << *sub[run1];
acadoFPrintf(file, ";\n");
}
acadoFPrintf(file,"\n/* Compute outputs: */\n");
// Export output quantities
for (run1 = 0; run1 < dim; run1++)
{
// Convert names for interm. quantities for output expressions
f[run1]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames);
acadoFPrintf(file, "out[%d] = ", run1);
file << *f[run1];
acadoFPrintf(file, ";\n");
}
acadoFPrintf(file,"}\n\n");
delete [] auxVarIndividualNames;
return SUCCESSFUL_RETURN;
}
returnValue FunctionEvaluationTree::exportCode( FILE *file,
const char *fcnName,
const char *realString,
int precision,
uint _numX,
uint _numXA,
uint _numU
) const{
int run1;
int nni = 0;
for( run1 = 0; run1 < n; run1++ )
if( lhs_comp[run1]+1 > nni )
nni = lhs_comp[run1]+1;
uint numX;
if( _numX > 0 ) {
numX = _numX;
}
else {
numX = getNX();
}
uint numXA;
if( _numXA > 0 ) {
numXA = _numXA;
}
else {
numXA = getNXA();
}
uint numU;
if( _numU > 0 ) {
numU = _numU;
}
else {
numU = getNU();
}
acadoFPrintf(file,"void %s( const %s* acado_x, %s* acado_f ){\n", fcnName,realString,realString );
if( numX > 0 ){
acadoFPrintf(file,"const %s* acado_xd = acado_x;\n", realString );
}
if( numXA > 0 ){
acadoFPrintf(file,"const %s* acado_xa = acado_x + %d;\n", realString,numX );
}
if( getNU() > 0 ){
acadoFPrintf(file,"const %s* acado_u = acado_x + %d;\n", realString,numX+numXA );
}
if( getNUI() > 0 ){
acadoFPrintf(file,"const %s* acado_v = acado_x + %d;\n", realString,numX+numXA+numU );
}
if( getNP() > 0 ){
acadoFPrintf(file,"const %s* acado_p = acado_x + %d;\n", realString,numX+numXA+numU+getNUI() );
}
if( getNPI() > 0 ){
acadoFPrintf(file,"const %s* acado_q = acado_x + %d;\n", realString,numX+numXA+numU+getNUI()+getNP() );
}
if( getNW() > 0 ){
acadoFPrintf(file,"const %s* acado_w = acado_x + %d;\n", realString,numX+numXA+numU+getNUI()+getNP()+getNPI() );
}
if( getNDX() > 0 ){
acadoFPrintf(file,"const %s* acado_dx = acado_x + %d;\n", realString,numX+numXA+numU+getNUI()+getNP()+getNPI()+getNW() );
}
acadoFPrintf(file,"\n");
acadoFPrintf(file,"/* COMPUTE INTERMEDIATE STATES: */\n");
acadoFPrintf(file,"/* ---------------------------- */\n");
//
// This is a (not so quick) hack to have a flexible name for interm.
// quantities, but helps. It general case it should be done with
// ExportVariable -- to be able to set a working structure, too. So:
//
// TODO: setGlobalExportvariable should set the full name once....
//
String auxVarName;
if ( auxVariableStructName.isEmpty() )
{
auxVarName = auxVariableName;
}
else
{
auxVarName = auxVariableStructName;
auxVarName += ".";
auxVarName += auxVariableName;
}
Stream *auxVarIndividualNames = new Stream[nni];
for( run1 = 0; run1 < n; run1++ )
auxVarIndividualNames[lhs_comp[run1]] << auxVarName << "[" << run1 << "]";
// Export intermediate quantities
for( run1 = 0; run1 < n; run1++ )
{
// Convert the name for intermediate variables for subexpressions
sub[ run1 ]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames );
acadoFPrintf(file,"%s[%d] = ", auxVarName.getName(), run1 );
file << *sub[run1];
acadoFPrintf(file,";\n");
}
acadoFPrintf(file,"\n");
acadoFPrintf(file,"/* COMPUTE OUTPUT: */\n");
acadoFPrintf(file,"/* --------------- */\n");
// Export output quantities
for( run1 = 0; run1 < dim; run1++ )
{
// Convert names for interm. quantities for output expressions
f[ run1 ]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames );
acadoFPrintf(file,"acado_f[%d] = ", run1 );
file << *f[run1];
acadoFPrintf(file,";\n");
}
acadoFPrintf(file,"}\n\n");
delete [] auxVarIndividualNames;
return SUCCESSFUL_RETURN;
}
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( );
}
