NMPC::NMPC(bool position_control, int num_steps,
const ReferenceInterpolation &reference_interpolation)
:position_control_(position_control),
num_steps_(num_steps),
reference_interpolation_(reference_interpolation),
running_weights_(base::MatrixXd::Identity(kNumOutputs, kNumOutputs)),
terminal_weights_(base::MatrixXd::Identity(kNumOutputsN, kNumOutputsN)),
control_derivative_(base::Vector6d::Zero()),
elapsed_time_(0)
{
if (num_steps_ <= 0)
throw std::runtime_error("The number of steps should be a positive "
"value.");
// Initialize the solver
acado_initializeSolver();
// Initialize the control
for (int i = 0; i < kNumControls * kHorizon; ++i)
acadoVariables.u[ i ] = 0.0;
// Initialize the states
for (int i = 0; i < kNumDiffStates * (kHorizon + 1); ++i)
acadoVariables.x[ i ] = 0.0;
// Initialize the measurements/reference
for (int i = 0; i < kNumOutputs * kHorizon; i++)
acadoVariables.y[ i ] = 0.0;
acadoVariables.yN[ 0 ] = 0.0;
// Initialize the current state feedback
for (int i = 0; i < kNumDiffStates; ++i)
acadoVariables.x0[ i ] = 0.0;
setWeights(running_weights_, terminal_weights_);
}
/** 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 );
}
