returnValue Controller::obtainEstimates( double currentTime, const Vector& _y, Vector& xEst, Vector& pEst ) { /* 1) Call Estimator */ RealClock clock; Vector xaEst, uEst, wEst; clock.reset(); clock.start(); if ( estimator != 0 ) { if ( estimator->step( currentTime,_y ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); if ( estimator->getOutputs( xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); } else { /* If no estimator is specified, assume full state feedback. */ xEst = _y; } clock.stop(); logCollection.setLast( LOG_TIME_ESTIMATOR,clock.getTime() ); // step internal reference trajectory if ( referenceTrajectory != 0 ) { if ( referenceTrajectory->step( currentTime,_y,xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); } return SUCCESSFUL_RETURN; }
returnValue SCPmethod::prepareNextStep( ) { returnValue returnvalue; RealClock clockLG; BlockMatrix oldLagrangeGradient; BlockMatrix newLagrangeGradient; // cout <<"bandedCP.dynResiduum (possibly shifted) = \n"); // bandedCP.dynResiduum.print(); // cout <<"bandedCP.lambdaDynamic (possibly shifted) = \n"); // bandedCP.lambdaDynamic.print(); // Coumpute the "old" Lagrange Gradient with the latest multipliers: // ----------------------------------------------------------------- clockLG.reset( ); clockLG.start( ); returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),oldIter,bandedCP, oldLagrangeGradient ); if( returnvalue != SUCCESSFUL_RETURN ) ACADOERROR( RET_NLP_STEP_FAILED ); clockLG.stop( ); // Linearize the NLP system at the new point: // ------------------------------------------ int printLevel; get( PRINTLEVEL,printLevel ); #ifdef SIM_DEBUG /* printf("preparation step \n"); (iter.x->getVector(0)).print("iter.x(0)"); (iter.u->getVector(0)).print("iter.u(0)"); (iter.x->getVector(1)).print("iter.x(1)"); (iter.u->getVector(1)).print("iter.u(1)");*/ #endif if ( (PrintLevel)printLevel >= HIGH ) cout << "--> Computing new linearization of NLP system ...\n"; clock.reset( ); clock.start( ); if ( needToReevaluate == BT_TRUE ) { // needs to re-evaluate due to eval->evaluate call within merit function evaluation! eval->clearDynamicDiscretization( ); if ( eval->evaluate( iter,bandedCP ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_NLP_STEP_FAILED ); needToReevaluate = BT_FALSE; } returnvalue = eval->evaluateSensitivities( iter,bandedCP ); if( returnvalue != SUCCESSFUL_RETURN ) ACADOERROR( RET_NLP_STEP_FAILED ); clock.stop( ); setLast( LOG_TIME_SENSITIVITIES,clock.getTime() ); if ( (PrintLevel)printLevel >= HIGH ) cout << "<-- Computing new linearization of NLP system done.\n"; //bandedCP.objectiveGradient.print(); // Coumpute the "new" Lagrange Gradient with the latest multipliers: // ----------------------------------------------------------------- clockLG.start( ); returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),iter,bandedCP, newLagrangeGradient ); if( returnvalue != SUCCESSFUL_RETURN ) ACADOERROR( RET_NLP_STEP_FAILED ); clockLG.stop( ); setLast( LOG_TIME_LAGRANGE_GRADIENT,clockLG.getTime() ); // Compute the next Hessian: // ------------------------- if ( (PrintLevel)printLevel >= HIGH ) cout << "--> Computing or approximating Hessian matrix ...\n"; clock.reset( ); clock.start( ); returnvalue = computeHessianMatrix( oldLagrangeGradient,newLagrangeGradient ); if( returnvalue != SUCCESSFUL_RETURN ) ACADOERROR( RET_NLP_STEP_FAILED ); clock.stop( ); setLast( LOG_TIME_HESSIAN_COMPUTATION,clock.getTime() ); if ( (PrintLevel)printLevel >= HIGH ) cout << "<-- Computing or approximating Hessian matrix done.\n"; // CONDENSE THE KKT-SYSTEM: // ------------------------ if ( isInRealTimeMode == BT_TRUE ) { if ( bandedCPsolver->prepareSolve( bandedCP ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_NLP_STEP_FAILED ); } stopClockAndPrintRuntimeProfile( ); return CONVERGENCE_NOT_YET_ACHIEVED; }
returnValue Controller::step( double currentTime, const Vector& _y, const VariablesGrid& _yRef ) { /* Consistency check. */ if ( getStatus( ) != BS_READY ) return ACADOERROR( RET_BLOCK_NOT_READY ); if ( controlLaw == 0 ) return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED ); // start real runtime measurement realClock.reset( ); realClock.start( ); RealClock clock; //printf( "RTI? %d!!\n", controlLaw->isInRealTimeMode( ) ); if ( controlLaw->isInRealTimeMode( ) == BT_TRUE ) { clock.reset(); clock.start(); if ( feedbackStep( currentTime,_y ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); double nextTime = currentTime; if ( controlLaw != 0 ) nextTime += controlLaw->getSamplingTime( ); if ( preparationStep( nextTime,_yRef ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); clock.stop(); logCollection.setLast( LOG_TIME_CONTROL_LAW,clock.getTime() ); } else { /* 1) Call Estimator */ clock.reset(); clock.start(); Vector xEst, pEst; if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); clock.stop(); logCollection.setLast( LOG_TIME_ESTIMATOR,clock.getTime() ); /* 2) Evaluate reference trajectory */ clock.reset(); clock.start(); VariablesGrid yRef( _yRef ); getCurrentReference( currentTime,yRef ); if ( controlLaw->step( currentTime,xEst,pEst,yRef ) != SUCCESSFUL_RETURN ) return ACADOERROR( RET_CONTROLLER_STEP_FAILED ); clock.stop(); logCollection.setLast( LOG_TIME_CONTROL_LAW,clock.getTime() ); } // stop real runtime measurement realClock.stop( ); logCollection.setLast( LOG_TIME_CONTROLLER,realClock.getTime() ); return SUCCESSFUL_RETURN; }