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