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