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 Curve::add( double tStart, double tEnd, const Function ¶meterization_ ){ uint run1; // CHECK WHETHER "tStart < tEnd": // ------------------------------------------------ if( acadoIsStrictlyGreater( tStart,tEnd ) == BT_TRUE ) return ACADOERROR(RET_TIME_INTERVAL_NOT_VALID); // CHECK WHETHER THE INPUT VECTOR IS EMPTY: // ------------------------------------------------ if( parameterization_.getDim() == 0 ) return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH); if( isEmpty() == BT_FALSE ){ // IF THE CURVE IS NOT EMPTY THE DIMENSIONS MUST BE CHECKED: // --------------------------------------------------------- if( getDim() != (int) parameterization_.getDim() ) return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH); // CHECK WHETHER THE CURVE HAS NO GAP's: // --------------------------------------------------------- if( acadoIsEqual( tStart,grid->getLastTime() ) == BT_FALSE ) { ASSERT(1==0); return ACADOERROR(RET_TIME_INTERVAL_NOT_VALID); } // APPEND THE NEW TIME INTERVAL TO THE GRID: // --------------------------------------------------------- double *times = new double[nIntervals+2]; for( run1 = 0; run1 < nIntervals+1; run1++ ) times[run1] = grid->getTime(run1); times[nIntervals+1] = tEnd; delete grid; grid = new Grid( nIntervals+2, times ); nIntervals++; delete[] times; // --------------------------------------------------------- } else{ // SETUP A NEW GRID: // ---------------------------------------------------- grid = new Grid( tStart, tEnd, 2 ); nIntervals++; // ---------------------------------------------------- } // CHECK WHETHER THE FUNCTION ITSELF IS VALID: // ------------------------------------------- if( parameterization_.getNX () != 0 || parameterization_.getNXA() != 0 || parameterization_.getNP () != 0 || parameterization_.getNPI() != 0 || parameterization_.getNU () != 0 || parameterization_.getNUI() != 0 || parameterization_.getNW() != 0 ){ return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH); } // SET THE DIMENSION OF THE CURVE: // (the correctness of the dimension has already been cecked) // ---------------------------------------------------------- dim = parameterization_.getDim(); // ALLOCATE MEMORY FOR THE NEW PIECE OF CURVE: // ------------------------------------------- parameterization = (Function**)realloc(parameterization,nIntervals*sizeof(Function*)); // MAKE A DEEP COPY OF THE PARAMETERIZATION: // ----------------------------------------- parameterization[nIntervals-1] = new Function(parameterization_); // RETURN: // ----------------------------------------- return SUCCESSFUL_RETURN; }
// uses a simple O(n^2) algorithm for sorting returnValue VariablesGrid::merge( const VariablesGrid& arg, MergeMethod _mergeMethod, BooleanType keepOverlap ) { if ( ( keepOverlap == BT_FALSE ) && ( _mergeMethod == MM_DUPLICATE ) ) return ACADOERROR( RET_INVALID_ARGUMENTS ); // nothing to do if arg or object itself is empty if ( arg.getNumPoints( ) == 0 ) return SUCCESSFUL_RETURN; if ( getNumPoints( ) == 0 ) { *this = arg; return SUCCESSFUL_RETURN; } // use append if grids do not overlap if ( acadoIsSmaller( getLastTime( ),arg.getFirstTime( ) ) == BT_TRUE ) return appendTimes( arg,_mergeMethod ); // construct merged grid VariablesGrid mergedGrid; uint j = 0; BooleanType overlapping = BT_FALSE; for( uint i=0; i<getNumPoints( ); ++i ) { if ( keepOverlap == BT_FALSE ) overlapping = arg.isInInterval( getTime(i) ); // add all grid points of argument grid that are smaller // then current one of original grid while ( ( j < arg.getNumPoints( ) ) && ( acadoIsStrictlySmaller( arg.getTime( j ),getTime( i ) ) == BT_TRUE ) ) { if ( ( overlapping == BT_FALSE ) || ( ( overlapping == BT_TRUE ) && ( _mergeMethod == MM_REPLACE ) ) ) { mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) ); } ++j; } // merge current grid points if they are at equal times if ( acadoIsEqual( arg.getTime( j ),getTime( i ) ) == BT_TRUE ) { switch ( _mergeMethod ) { case MM_KEEP: mergedGrid.addMatrix( *(values[i]),getTime( i ) ); break; case MM_REPLACE: mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) ); break; case MM_DUPLICATE: mergedGrid.addMatrix( *(values[i]),getTime( i ) ); mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) ); break; } ++j; } else { // add current grid point of original grid if ( ( overlapping == BT_FALSE ) || ( ( overlapping == BT_TRUE ) && ( _mergeMethod == MM_KEEP ) ) ) { mergedGrid.addMatrix( *(values[i]),getTime( i ) );//arg. } } } // add all remaining grid points of argument grid while ( j < arg.getNumPoints( ) ) { if ( acadoIsStrictlyGreater( arg.getTime(j),getLastTime() ) == BT_TRUE ) mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) ); ++j; } // merged grid becomes current grid *this = mergedGrid; 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; }