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 );
/* Do nothing if controller is disabled */
if ( isEnabled == BT_FALSE )
{
logCollection.setLast( LOG_TIME_CONTROLLER,0.0 );
logCollection.setLast( LOG_TIME_CONTROL_LAW,0.0 );
logCollection.setLast( LOG_TIME_ESTIMATOR,0.0 );
return SUCCESSFUL_RETURN;
}
if ( feedbackStep( currentTime,_y,_yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
double nextTime = currentTime + controlLaw->getSamplingTime( );
if ( preparationStep( nextTime,_yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::step( const Vector& x0_,
const Vector& p_
)
{
if ( numberOfSteps == 0 )
replot( PLOT_AT_START );
if ( feedbackStep( x0_,p_ ) != SUCCESSFUL_RETURN )
return RET_NLP_STEP_FAILED;
if ( performCurrentStep( ) == CONVERGENCE_ACHIEVED )
return CONVERGENCE_ACHIEVED;
returnValue returnValue = prepareNextStep( );
if ( ( returnValue != CONVERGENCE_ACHIEVED ) && ( returnValue != CONVERGENCE_NOT_YET_ACHIEVED ) )
return RET_NLP_STEP_FAILED;
return returnValue;
}
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;
}
int makeOptStep(emb_optimizer optim, emb_opt_context opt_context, const double xcur[], const double uprev[], double ucur[])
{
emb_size_type nx = getOptModelStateCount(optim);
emb_size_type nu = getOptModelInputCount(optim);
int i, j, status;
static unsigned firstRun = 1;
double feedback[ ACADO_NX ];
/* Fill the reference for states */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NX; ++j)
acadoVariables.xRef[i * ACADO_NX + j] = xref[ j ];
/* Fill the reference for controls -- always zero :) */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.uRef[i * ACADO_NU + j] = 0.0;
if ( firstRun )
{
/* Print header */
PRINTTEXT( "\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
"Copyright (C) 2008-2011 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.\n"
"Developed within the Optimization in Engineering Center (OPTEC) under\n"
"supervision of Moritz Diehl. All rights reserved.\n\n"
"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
"General Public License 3 in the hope that it will be useful,\n"
"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
"GNU Lesser General Public License for more details.\n\n" );
/* Initialize the shooting nodes */
for (i = 0; i < ACADO_N + 1; ++i)
{
acadoVariables.x[i * ACADO_NX + 0] = 0.0;
acadoVariables.x[i * ACADO_NX + 1] = 0.0;
acadoVariables.x[i * ACADO_NX + 2] = 0.75;
acadoVariables.x[i * ACADO_NX + 3] = 0.0;
acadoVariables.x[i * ACADO_NX + 4] = 0.0;
acadoVariables.x[i * ACADO_NX + 5] = 3.14159265359 / 4.0;
acadoVariables.x[i * ACADO_NX + 6] = 0.0;
acadoVariables.x[i * ACADO_NX + 7] = 0.0;
}
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.u[i * ACADO_NU + j] = 0.0;
firstRun = 0;
}
else
{
/* Shift the states and the controls */
shiftStates( 0 );
shiftControls( 0 );
}
/* Execute the preparation phase */
preparationStep();
/* Prepare the feedback signal */
feedback[ 0 ] = xcur[ 0 ];
feedback[ 1 ] = xcur[ 2 ];
feedback[ 2 ] = xcur[ 1 ];
feedback[ 3 ] = xcur[ 3 ];
feedback[ 4 ] = xcur[ 4 ];
feedback[ 5 ] = xcur[ 5 ];
feedback[ 6 ] = xcur[ 6 ];
feedback[ 7 ] = xcur[ 7 ];
/* Execute the feedback phase */
status = feedbackStep( feedback );
PRINTTEXT("KKT tolerance: %1.5e\n", getKKT());
if ( status )
{
PRINTTEXT("Problem in the QP solver detected. QP error code is: %d\n", status);
return 1;
}
/* Assign to input */
for( i = 0; i < ACADO_NU; i++ ) {
ucur[ i ] = acadoVariables.u[ i ];
}
return 0;
}
int makeOptStep(emb_optimizer optim, emb_opt_context opt_context, const double xcur[], const double uprev[], double ucur[])
{
emb_size_type nx = getOptModelStateCount(optim);
emb_size_type nu = getOptModelInputCount(optim);
int i, j, status;
static unsigned firstRun = 1;
double feedback[ ACADO_NX ];
/* Fill the reference for states */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NX; ++j)
acadoVariables.xRef[i * ACADO_NX + j] = xref[ j ];
/* Fill the reference for controls -- always zero :) */
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.uRef[i * ACADO_NU + j] = 0.0;
if ( firstRun )
{
/* Initialize the shooting nodes */
for (i = 0; i < ACADO_N + 1; ++i)
{
/* Choose point for model linearization */
acadoVariables.x[i * ACADO_NX + 0] = 0.0;
acadoVariables.x[i * ACADO_NX + 1] = 0.0;
acadoVariables.x[i * ACADO_NX + 2] = 0.75;
acadoVariables.x[i * ACADO_NX + 3] = 0.0;
acadoVariables.x[i * ACADO_NX + 4] = 0.0;
acadoVariables.x[i * ACADO_NX + 5] = 0.0;
acadoVariables.x[i * ACADO_NX + 6] = 0.0;
acadoVariables.x[i * ACADO_NX + 7] = 0.0;
}
for (i = 0; i < ACADO_N; ++i)
for (j = 0; j < ACADO_NU; ++j)
acadoVariables.u[i * ACADO_NU + j] = 0.0;
firstRun = 0;
/* Linearize the model */
preparationStep();
}
/* Prepare the feedback signal */
feedback[ 0 ] = xcur[ 0 ];
feedback[ 1 ] = xcur[ 2 ];
feedback[ 2 ] = xcur[ 1 ];
feedback[ 3 ] = xcur[ 3 ];
feedback[ 4 ] = xcur[ 4 ];
feedback[ 5 ] = xcur[ 5 ];
feedback[ 6 ] = xcur[ 6 ];
feedback[ 7 ] = xcur[ 7 ];
/* Execute the feedback phase */
status = feedbackStep( feedback );
if ( status )
{
PRINTTEXT("qpOASES problem detected. Error code %d\n", status);
return 1;
}
/* Assign to input */
ucur[ 0 ] = acadoVariables.u[ 0 ];
ucur[ 1 ] = acadoVariables.u[ 1 ];
return 0;
}
