returnValue ModelData::setIntegrationGrid( const Grid& _ocpGrid, const uint _numSteps
)
{
uint i;
N = _ocpGrid.getNumIntervals();
BooleanType equidistantControl = _ocpGrid.isEquidistant();
double T = _ocpGrid.getLastTime() - _ocpGrid.getFirstTime();
double h = T/((double)_numSteps);
Vector stepsVector( N );
if (integrationGrid.isEmpty() == BT_TRUE)
{
for( i = 0; i < stepsVector.getDim(); i++ )
{
stepsVector(i) = (int) acadoRound((_ocpGrid.getTime(i+1)-_ocpGrid.getTime(i))/h);
}
if( equidistantControl )
{
// Setup fixed integrator grid for equidistant control grid
integrationGrid = Grid( 0.0, ((double) T)/((double) N), (int) ceil((double)_numSteps/((double) N) - 10.0*EPS) + 1 );
}
else
{
// Setup for non equidistant control grid
// NOTE: This grid defines only one integration step because the control
// grid is non equidistant.
integrationGrid = Grid( 0.0, h, 2 );
numSteps = stepsVector;
}
}
return SUCCESSFUL_RETURN;
}
returnValue ShootingMethod::evaluate( OCPiterate &iter
)
{
// iter.print(); ASSERT( 1==0 );
// INTRODUCE SOME AUXILIARY VARIABLES:
// -----------------------------------
ASSERT( iter.x != 0 );
uint run1;
double tStart, tEnd;
Vector x ; nx = iter.getNX ();
Vector xa; na = iter.getNXA();
Vector p ; np = iter.getNP ();
Vector u ; nu = iter.getNU ();
Vector w ; nw = iter.getNW ();
VariablesGrid xAll;
VariablesGrid xaAll;
residuum = *iter.x;
residuum.setAll( 0.0 );
iter.getInitialData( x, xa, p, u, w );
// iter.print();
// RUN A LOOP OVER ALL INTERVALS OF THE UNION GRID:
// ------------------------------------------------
for( run1 = 0; run1 < unionGrid.getNumIntervals(); run1++ ){
integrator[run1]->setOptions( getOptions( 0 ) ); // ??
int freezeIntegrator;
get( FREEZE_INTEGRATOR, freezeIntegrator );
if ( (BooleanType)freezeIntegrator == BT_TRUE )
integrator[run1]->freezeAll();
tStart = unionGrid.getTime( run1 );
tEnd = unionGrid.getTime( run1+1 );
// printf("tStart = %e, tEnd = %e\n",tStart,tEnd );
// x.print("x");
// u.print("u");
// p.print("p");
// integrator[run1]->set( INTEGRATOR_PRINTLEVEL, MEDIUM );
Grid evaluationGrid;
iter.x->getSubGrid( tStart,tEnd,evaluationGrid );
Grid outputGrid;
if ( acadoIsNegative( integrator[run1]->getDifferentialEquationSampleTime( ) ) == BT_TRUE )
outputGrid.init( tStart,tEnd,getNumEvaluationPoints() );
else
outputGrid.init( tStart,tEnd, 1+acadoRound( (tEnd-tStart)/integrator[run1]->getDifferentialEquationSampleTime() ) );
if ( integrator[run1]->integrate( outputGrid&evaluationGrid, x, xa, p, u, w ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_INTEGRATE_SYSTEM );
Vector xOld;
Vector pOld = p;
if ( evaluationGrid.getNumPoints( ) <= 2 )
{
integrator[run1]->getX ( x );
integrator[run1]->getXA( xa );
xOld = x;
iter.updateData( tEnd, x, xa, p, u, w );
}
else
{
integrator[run1]->getX ( xAll );
integrator[run1]->getXA( xaAll );
xOld = xAll.getLastVector( );
for( uint run2=1; run2<outputGrid.getNumPoints(); ++run2 )
{
if ( evaluationGrid.hasTime( outputGrid.getTime(run2) ) == BT_TRUE )
{
x = xAll.getVector(run2);
xa = xaAll.getVector(run2);
iter.updateData( outputGrid.getTime(run2), x, xa, p, u, w );
}
}
}
if ( iter.isInSimulationMode( ) == BT_FALSE )
p = pOld; // should be changed later...
residuum.setVector( run1, xOld - x );
}
// LOG THE RESULTS:
// ----------------
return logTrajectory( iter );
}
