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; }