returnValue SCPmethod::shiftVariables( double timeShift,
Vector lastX,
Vector lastXA,
Vector lastP,
Vector lastU,
Vector lastW
)
{
#ifdef SIM_DEBUG
acadoPrintf( "SCPmethod::shiftVariables\n" );
#endif
if ( acadoIsNegative( timeShift ) == BT_TRUE )
return ACADOERROR( RET_INVALID_ARGUMENTS );
// Matrix tmp;
// for( uint i=1; i<iter.getNumPoints()-1; ++i )
// {
// bandedCP.lambdaDynamic.getSubBlock( i,0, tmp );
// bandedCP.lambdaDynamic.setDense( i-1,0, tmp );
// }
// printf("shifted!\n");
needToReevaluate = BT_TRUE;
return iter.shift( timeShift, lastX, lastXA, lastP, lastU, lastW );
}
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 );
}