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;
}
uint CondensingExport::getNumQPvars( ) const
{
if ( performsFullCondensing() == BT_TRUE )
return getNU()*getN();
else
return getNX() + getNU()*getN();
}
/* 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::setControlDeadTimes( const Vector& _deadTimes
)
{
if ( _deadTimes.getDim( ) != getNU( ) )
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<getNU(); ++i )
deadTimes( i ) = _deadTimes( i );
return SUCCESSFUL_RETURN;
}
returnValue Actuator::setControlNoise( const Noise& _noise,
double _noiseSamplingTime
)
{
if ( _noise.getDim( ) != getNU( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
for( uint i=0; i<getNU( ); ++i )
{
if ( additiveNoise[i] != 0 )
delete additiveNoise[i];
additiveNoise[i] = _noise.clone( i );
}
noiseSamplingTimes.setAll( _noiseSamplingTime );
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 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 FeedforwardLaw::init( double startTime,
const Vector &x0_,
const Vector &p_,
const VariablesGrid& _yRef
)
{
u.init(getNU());
u.setZero();
p.init( 0 );
p.setZero( );
setStatus( BS_READY );
return SUCCESSFUL_RETURN;
}
returnValue Actuator::setControlDeadTime( uint idx,
double _deadTime
)
{
if ( idx >= getNU( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( _deadTime < 0.0 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( deadTimes.getDim( ) == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
deadTimes( idx ) = _deadTime;
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::getFirstControl( Vector& u0_ ) const
{
#ifdef SIM_DEBUG
acadoPrintf( "SCPmethod::getFirstControl\n" );
#endif
if( iter.u == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
u0_ = iter.u->getVector( 0 );
if ( hasPerformedStep == BT_FALSE )
{
Vector deltaU0( getNU() );
bandedCPsolver->getFirstControl( deltaU0 );
u0_ += deltaU0;
}
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 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 CondensingExport::setupMultiplicationRoutines( )
{
ExportVariable QQ = deepcopy( Q );
QQ.setDataStruct( ACADO_LOCAL );
ExportVariable QC1( "QC1", getNX(),getNX() );
ExportVariable QE1( "QE1", getNX(),getNU()*getN() );
ExportVariable Dx1( "Dx1", getNX(),1 );
ExportVariable QDx1("QDx1", getNX(),1 );
ExportVariable Du1( "Du1", getNU(),1 );
ExportVariable RDu1("RDu1", getNU(),1 );
ExportVariable Gx ( "Gx", getNX(),getNX() );
ExportVariable Gu ( "Gu", getNX(),getNU() );
ExportVariable C1 ( "C1", getNX(),getNX() );
ExportVariable E1 ( "E1", getNX(),getNU()*getN() );
ExportVariable d1 ( "d1", getNX(),1 );
ExportVariable u1 ( "u1", getNU(),1 );
ExportVariable C ( "C", getNX()*getN(),getNX() );
ExportVariable QC ( "QC", getNX()*getN(),getNX() );
ExportVariable E ( "E", getNX()*getN(),getNU()*getN() );
ExportVariable QE ( "QE", getNX()*getN(),getNU()*getN() );
ExportVariable QDx( "QDx", getNX()*getN(),1 );
ExportVariable g0 ( "g0", getNX(),1 );
ExportVariable g1 ( "g1", getNU()*getN(),1 );
ExportVariable H00( "H00", getNX(),getNX() );
ExportVariable H01( "H01", getNX(),getNU()*getN() );
ExportVariable H11( "H11", getNU()*getN(),getNU()*getN() );
multiplyQC1.setup( "multiplyQC1", QQ,C1,QC1 );
multiplyQC1.addStatement( QC1 == QQ*C1 );
multiplyQE1.setup( "multiplyQE1", QQ,E1,QE1 );
multiplyQE1.addStatement( QE1 == QQ*E1 );
multiplyQDX1.setup( "multiplyQDX1", QQ,Dx1,QDx1 );
multiplyQDX1.addStatement( QDx1 == QQ*Dx1 );
multiplyRDU1.setup( "multiplyRDU1", R,Du1,RDu1 );
multiplyRDU1.addStatement( RDu1 == R*Du1 );
multiplyQC2.setup( "multiplyQC2", QQF,C1,QC1 );
multiplyQC2.addStatement( QC1 == QQF*C1 );
multiplyQE2.setup( "multiplyQE2", QQF,E1,QE1 );
multiplyQE2.addStatement( QE1 == QQF*E1 );
multiplyQDX2.setup( "multiplyQDX2", QQF,Dx1,QDx1 );
multiplyQDX2.addStatement( QDx1 == QQF*Dx1 );
multiplyC.setup( "multiplyC", Gx,C1,C1("C1_new") );
multiplyC.addStatement( C1("C1_new") == Gx*C1 );
multiplyE.setup( "multiplyE", Gx,E1,E1("E1_new") );
multiplyE.addStatement( E1("E1_new") == Gx*E1 );
if ( performsSingleShooting() == BT_FALSE )
{
multiplyCD1.setup( "multiplyCD1", Gx,d1,d1("d1_new") );
multiplyCD1.addStatement( d1("d1_new") == Gx*d1 );
multiplyEU1.setup( "multiplyEU1", Gu,u1,d1("d1_new") );
multiplyEU1.addStatement( d1("d1_new") += Gu*u1 );
}
if ( performsFullCondensing() == BT_FALSE )
{
multiplyG0.setup( "multiplyG0", C,QDx,g0 );
multiplyG0.addStatement( g0 == (C^QDx) );
}
multiplyG1.setup( "multiplyG1", E,QDx,g1 );
multiplyG1.addStatement( g1 == (E^QDx) );
if ( performsFullCondensing() == BT_FALSE )
{
multiplyH00.setup( "multiplyH00", C,QC,H00 );
multiplyH00.addStatement( H00 == (C^QC) );
}
multiplyH01.setup( "multiplyH01", C,QE,H01 );
multiplyH01.addStatement( H01 == (C^QE) );
multiplyH11.setup( "multiplyH11", E,QE,H11 );
multiplyH11.addStatement( (H11 == (E^QE)) );
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::setupCondensing( )
{
uint run1;
x.setup( "x", getN()+1, getNX(), REAL,ACADO_VARIABLES );
xRef.setup( "xRef", getN(), getNX(), REAL,ACADO_VARIABLES );
uRef.setup( "uRef", getN(), getNU(), REAL,ACADO_VARIABLES );
Gx.setup( "Gx", getNX(), getNX(), REAL,ACADO_WORKSPACE );
Gu.setup( "Gu", getNX(), getNU(), REAL,ACADO_WORKSPACE );
ExportVariable yy( "yy", 1, getNX()*(getNX()+getNU()+1)+getNU() );
ExportVariable Q0 = QS;
ExportVariable Q0b = QS2;
if ( performsSingleShooting() == BT_FALSE )
{
residuum.setup( "residuum", getN(), getNX(), REAL,ACADO_WORKSPACE );
d.setup( "d", getN(), getNX(), REAL,ACADO_WORKSPACE );
}
deltaX0.setup( "deltaX0", getNX(), 1, REAL,ACADO_WORKSPACE );
C.setup ( "C", getN()*getNX(), getNX(), REAL,ACADO_WORKSPACE );
QC.setup ( "QC", getN()*getNX(), getNX(), REAL,ACADO_WORKSPACE );
E.setup ( "E", getN()*getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
QE.setup ( "QE", getN()*getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
if ( isInitialStateFixed( ) == BT_FALSE )
{
Dx0.setup ( "Dx0", 1, getNX(), REAL,ACADO_WORKSPACE );
Dx0b.setup( "Dx0b", 1, getNX(), REAL,ACADO_WORKSPACE );
}
Dx.setup ( "Dx", getN(), getNX(), REAL,ACADO_WORKSPACE );
QDx.setup ( "QDx", getN()*getNX(), 1, REAL,ACADO_WORKSPACE );
Du.setup ( "Du", getN(), getNU(), REAL,ACADO_WORKSPACE );
RDu.setup ( "RDu", getN(), getNU(), REAL,ACADO_WORKSPACE );
if ( performsFullCondensing() == BT_FALSE )
{
g0.setup ( "g0", getNX() , 1, REAL,ACADO_WORKSPACE );
H00.setup ( "H00", getNX() , getNX(), REAL,ACADO_WORKSPACE );
}
g1.setup ( "g1", getN()*getNU(), 1, REAL,ACADO_WORKSPACE );
H01.setup ( "H01", getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
H11.setup ( "H11", getN()*getNU(), getN()*getNU(), REAL,ACADO_WORKSPACE );
deltaU.setup( "vars.x", getNumQPvars(),1 );
ExportIndex index( String("index") );
////////////////////////////////////////////////////////////////////////////
//
// First condensing routine
//
////////////////////////////////////////////////////////////////////////////
condense1.setup( "condense1", index.makeArgument(),yy );
// TO BE TESTED
if ( performsSingleShooting() == BT_TRUE )
condense1.addStatement( Dx.getRow(index) == yy.getCols( 0,getNX() ) - xRef.getRow(index) );
else
condense1.addStatement( Dx.getRow(index) == x.getRow( index + 1 ) - xRef.getRow(index) );
uint uIdx = getNX()*(getNX()+getNU()+1);
condense1.addStatement( Du.getRow(index) == yy.getCols( uIdx,uIdx+getNU() ) - uRef.getRow(index) );
condense1.addStatement( Gx.makeRowVector() == yy.getCols( getNX(),getNX()+getNX()*getNX() ) );
uIdx = getNX()*(getNX()+1);
condense1.addStatement( Gu.makeRowVector() == yy.getCols( uIdx,uIdx+getNX()*getNU() ) );
condense1.addStatement( "if( index != 0 ){\n" );
if ( performsSingleShooting() == BT_FALSE )
{
condense1.addFunctionCall( multiplyCD1, Gx, d.getAddress(index - 1), d.getAddress(index) );
// TODO: check this once again
// condense1.addStatement( d.getRow( index ) += residuum.getRow( index-1 ) );
condense1.addStatement( d.getRow( index ) += residuum.getRow( index ) );
}
condense1.addFunctionCall( multiplyC, Gx,C.getAddress((index-1)*getNX(),0), C.getAddress(index*getNX(),0) );
condense1.addFunctionCall( multiplyE, Gx,E.getAddress((index-1)*getNX(),0), E.getAddress(index*getNX(),0) );
condense1.addStatement( "}\nelse{\n" );
if ( performsSingleShooting() == BT_FALSE )
{
condense1.addStatement( d.getRow( 0 ) == residuum.getRow( 0 ) );
}
condense1.addStatement( C.getRows( 0,getNX() ) == Gx );
condense1.addStatement( "}\n" );
condense1.addStatement( E.getSubMatrix( index*getNX(),(index+1)*getNX(), index*getNU(),(index+1)*getNU() ) == Gu );
////////////////////////////////////////////////////////////////////////////
//
// Second condensing routine
//
////////////////////////////////////////////////////////////////////////////
condense2.setup( "condense2" );
//
// Create matrices QC and QE, vectors QDx, RDu
//
for( run1 = 0; run1 < getN()-1; run1++ )
{
condense2.addFunctionCall( multiplyQC1, Q, C.getAddress(run1*getNX(),0), QC.getAddress(run1*getNX(),0) );
condense2.addFunctionCall( multiplyQE1, Q, E.getAddress(run1*getNX(),0), QE.getAddress(run1*getNX(),0) );
condense2.addFunctionCall( multiplyQDX1, Q, Dx.getAddress(run1), QDx.getAddress(run1*getNX()) );
condense2.addFunctionCall( multiplyRDU1, R, Du.getAddress(run1), RDu.getAddress(run1) );
}
condense2.addFunctionCall( multiplyQC2, QQF, C.getAddress((getN()-1)*getNX(),0), QC.getAddress((getN()-1)*getNX(),0) );
condense2.addFunctionCall( multiplyQE2, QQF, E.getAddress((getN()-1)*getNX(),0), QE.getAddress((getN()-1)*getNX(),0) );
condense2.addFunctionCall( multiplyQDX2, QQF, Dx.getAddress(getN()-1), QDx.getAddress((getN()-1)*getNX()) );
condense2.addFunctionCall( multiplyRDU1, R, Du.getAddress(getN()-1), RDu.getAddress(getN()-1) );
//
// Condense gradient
//
if ( performsFullCondensing() == BT_FALSE )
{
condense2.addFunctionCall( multiplyG0, C, QDx, g0 );
if ( isInitialStateFixed( ) == BT_FALSE )
condense2.addStatement( g0 += Q0 * Dx0.makeColVector() );
if ( isInitialStateFixed( ) == BT_FALSE )
condense2.addStatement( g0 += Q0b * Dx0b.makeColVector() );
}
if ( performsSingleShooting() == BT_TRUE )
{
condense2.addFunctionCall( multiplyG1, E, QDx, g1 );
}
else
{
condense2.addStatement( Dx == Dx + d );
condense2.addFunctionCall(multiplyG1, QE, Dx, g1 );
}
condense2.addStatement( g1 += RDu.makeColVector() );
//
// Condense Hessian
//
if ( performsFullCondensing() == BT_FALSE )
{
condense2.addFunctionCall( multiplyH00, C,QC,H00 );
Matrix regH = eye( getNX() );
regH *= levenbergMarquardt;
if ( isInitialStateFixed( ) == BT_FALSE )
{
condense2.addStatement( H00 += Q0 + regH );
condense2.addStatement( H00 += Q0b );
}
else
condense2.addStatement( H00 += regH );
}
condense2.addFunctionCall( multiplyH01, C, QE, H01 );
condense2.addFunctionCall( multiplyH11, E, QE, H11 );
Matrix regH = eye( getNU() );
regH *= levenbergMarquardt;
for( run1 = 0; run1 < getN(); run1++ )
condense2.addStatement( H11.getSubMatrix( run1*getNU(),(run1+1)*getNU(), run1*getNU(),(run1+1)*getNU() ) += R + regH );
////////////////////////////////////////////////////////////////////////////
//
// Expanding routine
// TODO: create multiplication routines maybe...
//
////////////////////////////////////////////////////////////////////////////
if ( performsSingleShooting() == BT_FALSE )
{
expand.setup( "expand" );
if ( performsFullCondensing() == BT_TRUE )
{
// d += C * deltaX0
expand.addStatement( d.makeColVector() += C * deltaX0 );
// d += E * deltaU
expand.addStatement( d.makeColVector() += E * deltaU );
}
else
{
// d += C * deltaX0
expand.addStatement( d.makeColVector() += C * deltaU.getRows(0, getNX()) );
// d += E * deltaU
expand.addStatement( d.makeColVector() += E * deltaU.getRows(getNX(), getNumQPvars()) );
}
// x(:, 1: N + 1) += d
expand.addStatement( x.getRows(1, getN() + 1) += d );
}
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 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 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 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;
}
