returnValue Sensor::getDelayedOutputGrid( const VariablesGrid& _y,
VariablesGrid& _yDelayed
) const
{
// determine common time grid for delayed outputs:
Grid delayedOutputTimeGrid = lastSignal.getTimePoints( );
// make sure that last time instant of horizon lies within the grid
if ( acadoIsEqual( lastSignal.getLastTime(),_y.getLastTime( ) ) == BT_FALSE )
delayedOutputTimeGrid.addTime( _y.getLastTime( ) );
// add grids of all delayed output components
for( uint i=0; i<getNY( ); ++i )
delayedOutputTimeGrid.merge( _y.getTimePoints( ).shiftTimes( deadTimes(i) ),MM_REPLACE );
VariablesGrid tmp;
// setup common variables grid for delayed inputs
_yDelayed.init( );
for( uint i=0; i<getNY( ); ++i )
{
tmp = lastSignal( i );
tmp.merge( _y( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
tmp.refineGrid( delayedOutputTimeGrid );
_yDelayed.appendValues( tmp );
}
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;
}
