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