int rien_feedbackStep( )
{
int tmp;
acado_timer tmr;
acado_tic( &tmr );
acado_condenseFdb( );
condenseTime += acado_toc( &tmr );
acado_tic( &tmr );
tmp = acado_solve( );
qpTime = acado_toc( &tmr );
acado_tic( &tmr );
acado_expand( );
condenseTime += acado_toc( &tmr );
return tmp;
}
int rien_preparationStep( )
{
int ret;
acado_timer tmr;
acado_tic( &tmr );
ret = acado_modelSimulation();
simTime = acado_toc( &tmr );
acado_evaluateObjective( );
acado_tic( &tmr );
acado_regularizeHessian( );
regularizeTime = acado_toc( &tmr );
acado_tic( &tmr );
acado_condensePrep( );
condenseTime = acado_toc( &tmr );
return ret;
}
base::Vector6d NMPC::calcControl(const base::Vector6d &reference,
const base::VectorXd &pose_samples,
const base::Vector6d &efforts)
{
/* Get the time before start of the loop */
acado_timer t;
acado_tic( &t );
calcTrajectory(reference);
updateStates(pose_samples, efforts);
/* Prepare first step */
acado_preparationStep();
/* The "real-time iterations" loop */
for(int iter = 0; iter < num_steps_; ++iter)
{
acado_feedbackStep( );
// Optional: shift the initialization (look at acado_common.h)
// shiftStates(2, 0, 0);
// shiftControls( 0 );
/* Prepare for the next step */
acado_preparationStep();
}
base::Vector6d control = base::Vector6d::Zero();
for (int i = 0; i < kNumControls; i++)
control[i] = acadoVariables.u[i];
control_derivative_ = control;
/* Read the elapsed time */
elapsed_time_ = acado_toc( &t );
return control;
}
/** The MEX interface function. */
void mexFunction( int nlhs,
mxArray *plhs[],
int nrhs,
const mxArray *prhs[]
)
{
static unsigned initialized = 0;
unsigned ctrl;
int ctrlIndex, i, j;
unsigned strategy;
unsigned initType;
real_t* xEnd = NULL;
real_t* uEnd = NULL;
const mxArray* src = prhs[ 0 ];
const char *infoNames[ 9 ] = {"status", "cpuTime", "simTime", "qpTime", "condenseTime", "regularizeTime", "kktValue", "objValue", "nIterations"};
mxArray* info;
double status, cpuTime, kktValue, objValue;
double tmp[ 1 ];
mxArray* shPtr;
acado_timer tmr;
double nIterations = 0;
const char *outNames[ NOO_4 ];
outNames[ 0 ] = "info";
outNames[ 1 ] = "x";
outNames[ 2 ] = "u";
outNames[ 3 ] = "mu";
#if ACADO_NXA
outNames[ NOO ] = "z";
#endif
#if ACADO_USE_ARRIVAL_COST == 1
outNames[ NOO_2 ] = "xAC";
outNames[NOO_2 + 1] = "SAC";
#endif
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
outNames[ NOO_3 ] = "sigmaN";
#endif
if (nrhs != 1)
mexErrMsgTxt(
"This function requires exactly one input: a structure with parameters.");
if (nlhs != 1)
mexErrMsgTxt(
"This function returns one output.");
if( !mxIsStruct( src ) )
mexErrMsgTxt("The function argument must be a structure.");
/* Get the control flag. */
if (getArray(0, src, 0, "control", tmp, 1, 1) == 0)
ctrl = (unsigned)tmp[ 0 ];
else
ctrl = 0;
/* Get the initialization flag. */
if (getArray(0, src, 0, "initialization", tmp, 1, 1) == 0)
initType = (unsigned)tmp[ 0 ];
else
initType = 0;
/* Copy MATLAB arrays to C arrays. */
getArray(1, src, 0, "x", acadoVariables.x, ACADO_N + 1, ACADO_NX);
getArray(1, src, 0, "u", acadoVariables.u, ACADO_N, ACADO_NU);
getArray(1, src, 0, "mu", acadoVariables.mu, ACADO_N, ACADO_NX);
#if ACADO_NXA
getArray(1, src, 0, "z", acadoVariables.z, ACADO_N, ACADO_NXA);
#endif
#if ACADO_NOD
getArray(1, src, 0, "od", acadoVariables.od, ACADO_N + 1, ACADO_NOD);
#endif
#if ACADO_INITIAL_STATE_FIXED
getArray(1, src, 0, "x0", acadoVariables.x0, ACADO_NX, 1);
#endif /* ACADO_INITIAL_STATE_FIXED */
#if (ACADO_HARDCODED_CONSTRAINT_VALUES == 0) && ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
if (!initialized)
{
acado_initializeSolver();
}
/* Bounds */
#if ACADO_INITIAL_STATE_FIXED == 1
getArray(1, src, 0, "lbValues", acadoVariables.lbValues, ACADO_N * ACADO_NU, 1);
getArray(1, src, 0, "ubValues", acadoVariables.ubValues, ACADO_N * ACADO_NU, 1);
#else
getArray(1, src, 0, "lbValues", acadoVariables.lbValues, ACADO_NX + ACADO_N * ACADO_NU, 1);
getArray(1, src, 0, "ubValues", acadoVariables.ubValues, ACADO_NX + ACADO_N * ACADO_NU, 1);
#endif /* ACADO_INITIAL_STATE_FIXED == 0 */
#if QPOASES_NCMAX > 0
/* Affine constraints */
getArray(1, src, 0, "lbAValues", acadoVariables.lbAValues, QPOASES_NCMAX, 1);
getArray(1, src, 0, "ubAValues", acadoVariables.ubAValues, QPOASES_NCMAX, 1);
#endif /* QPOASES_NCMAX > 0 */
#endif /* (ACADO_HARDCODED_CONSTRAINT_VALUES == 0) && ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
#if (ACADO_QP_SOLVER == ACADO_QPDUNES)
if (!initialized) {
acado_initializeSolver();
}
#endif
#if ACADO_USE_ARRIVAL_COST == 1
getArray(1, src, 0, "xAC", acadoVariables.xAC, ACADO_NX, 1);
getArray(1, src, 0, "SAC", acadoVariables.SAC, ACADO_NX, ACADO_NX);
getArray(1, src, 0, "WL", acadoVariables.WL, ACADO_NX, ACADO_NX);
#endif
/* Shifting strategy */
shPtr = mxGetField(src, 0, "shifting");
if (shPtr != NULL)
{
if( !mxIsStruct( shPtr ) )
mexErrMsgTxt("Field \"shifting\" must be defined with a structure.");
/* Get the shifting strategy flag */
getArray(1, shPtr, 0, "strategy", tmp, 1, 1);
strategy = (unsigned)tmp[ 0 ];
if (strategy > 2)
mexErrMsgTxt("Valid options for the shifting strategy are 1 or 2.");
getArray(0, shPtr, 0, "xEnd", xEnd, ACADO_NX, 1);
getArray(0, shPtr, 0, "uEnd", uEnd, ACADO_NU, 1);
}
else
strategy = 0;
acado_tic( &tmr );
/* Call solver */
switch ( ctrl )
{
case 0:
/* Simple operational mode. Run one RTI with optional shifting. */
if ( !initialized )
{
memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
#if ACADO_HARDCODED_CONSTRAINT_VALUES == 1
acado_initializeSolver();
#endif /* ACADO_HARDCODED_CONSTRAINT_VALUES == 1 */
/*for( i = 0; i < ACADO_N*ACADO_RK_NIS; i++ ) {
for( j = 0; j < ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA); j++ ) {
acadoWorkspace.rk_A_traj[i*ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA)*ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA)+j*ACADO_RK_NSTAGES*(ACADO_NX+ACADO_NXA)+j] = 1.0;
}
}*/
if (initType == 1)
{
acado_initializeNodesByForwardSimulation();
}
#if ACADO_USE_ARRIVAL_COST == 1
acado_updateArrivalCost( 1 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */
initialized = 1;
}
else if (strategy == 1 || strategy == 2)
{
#if ACADO_USE_ARRIVAL_COST == 1
acado_updateArrivalCost( 0 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */
acado_shiftStates(strategy, xEnd, uEnd);
acado_shiftControls(uEnd);
}
rien_preparationStep();
status = (double)rien_feedbackStep();
kktValue = acado_getKKT();
objValue = acado_getObjective();
#if ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
nIterations = (double)acado_getNWSR();
#endif /* ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
break;
case 1:
/* Initialize */
memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
acado_initializeSolver();
if (initType == 1)
{
acado_initializeNodesByForwardSimulation();
}
#if ACADO_USE_ARRIVAL_COST == 1
acado_updateArrivalCost( 1 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */
break;
case 2:
/* Preparation step */
rien_preparationStep();
break;
case 3:
/* Feedback step */
status = (double)rien_feedbackStep();
kktValue = acado_getKKT();
objValue = acado_getObjective();
#if ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
nIterations = (double)acado_getNWSR();
#endif /* ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
break;
case 4:
/* Shifting */
#if ACADO_USE_ARRIVAL_COST == 1
acado_updateArrivalCost( 0 );
#endif /* ACADO_USE_ARRIVAL_COST == 1 */
acado_shiftStates(strategy, xEnd, uEnd);
acado_shiftControls( uEnd );
break;
default:
/* Return an error */
mexErrMsgTxt("Unknown control code.");
}
cpuTime = acado_toc( &tmr );
/* Prepare return argument */
plhs[ 0 ] = mxCreateStructMatrix(1, 1, NOO_4, outNames);
setArray(plhs[ 0 ], 0, "x", acadoVariables.x, ACADO_N + 1, ACADO_NX);
setArray(plhs[ 0 ], 0, "u", acadoVariables.u, ACADO_N, ACADO_NU);
setArray(plhs[ 0 ], 0, "mu", acadoVariables.mu, ACADO_N, ACADO_NX);
#if ACADO_NXA > 0
setArray(plhs[ 0 ], 0, "z", acadoVariables.z, ACADO_N, ACADO_NXA);
#endif
#if ACADO_USE_ARRIVAL_COST == 1
setArray(plhs[ 0 ], 0, "xAC", acadoVariables.xAC, ACADO_NX, 1);
setArray(plhs[ 0 ], 0, "SAC", acadoVariables.SAC, ACADO_NX, ACADO_NX);
#endif /* ACADO_USE_ARRIVAL_COST */
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
setArray(plhs[ 0 ], 0, "sigmaN", acadoVariables.sigmaN, ACADO_NX, ACADO_NX);
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
/* Create the info structure. */
info = mxCreateStructMatrix(1, 1, 9, infoNames);
setArray(info, 0, "status", &status, 1, 1);
setArray(info, 0, "cpuTime", &cpuTime, 1, 1);
setArray(info, 0, "simTime", &simTime, 1, 1);
setArray(info, 0, "qpTime", &qpTime, 1, 1);
setArray(info, 0, "condenseTime", &condenseTime, 1, 1);
setArray(info, 0, "regularizeTime", ®ularizeTime, 1, 1);
setArray(info, 0, "kktValue", &kktValue, 1, 1);
setArray(info, 0, "objValue", &objValue, 1, 1);
#if ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) )
setArray(info, 0, "nIterations", &nIterations, 1, 1);
#endif /* ( (ACADO_QP_SOLVER == ACADO_QPOASES) || (ACADO_QP_SOLVER == ACADO_QPOASES3) ) */
mxSetField(plhs[ 0 ], 0, "info", info);
/* Cleanup of the allocated memory */
FREE( xEnd );
FREE( uEnd );
}