void singlePlanner::executeTrajectory(vector<Vector> &_bestTraj, vector<Vector> &_bestTrajRoot, const string &color) { updatePlanner(); _bestTraj = generateTrajectory(); _bestTrajRoot = getBestTrajRoot(); printTrajectory(); if (running_mode=="single") { displayPlan(color); logTrajectory(); } }
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 ); }