97 lines
3.1 KiB
Plaintext
97 lines
3.1 KiB
Plaintext
/******************************************************************************/
|
|
/* */
|
|
/* qpDUNES interface data and functions */
|
|
/* */
|
|
/******************************************************************************/
|
|
|
|
#include <qpDUNES.h>
|
|
|
|
qpData_t qpData;
|
|
qpOptions_t qpOptions;
|
|
|
|
#if @DIAG_H@
|
|
#define QPDUNES_LS_HOMOTOPY_GRID_SEARCH 0
|
|
#endif
|
|
|
|
#include "@MODULE_NAME@_auxiliary_functions.h"
|
|
real_t prepareQPtime, updateQPtime, solveQPtime;
|
|
|
|
int initializeQpDunes( void )
|
|
{
|
|
return_t statusFlag;
|
|
int kk;
|
|
|
|
@QP_ND_ARRAY@
|
|
|
|
qpOptions = qpDUNES_setupDefaultOptions();
|
|
qpOptions.maxIter = @MAX_ITER@;
|
|
qpOptions.printLevel = @PRINT_LEVEL@;
|
|
qpOptions.stationarityTolerance = 1.e-6;
|
|
qpOptions.regParam = 1.e-6;
|
|
qpOptions.newtonHessDiagRegTolerance = 1.e-8;
|
|
qpOptions.lsType = @DIAG_H@ ? QPDUNES_LS_ACCELERATED_GRADIENT_BISECTION_LS : QPDUNES_LS_HOMOTOPY_GRID_SEARCH;
|
|
/* qpOptions.lsType = QPDUNES_LS_BACKTRACKING_LS; */
|
|
qpOptions.lineSearchReductionFactor = 0.1;
|
|
qpOptions.lineSearchMaxStepSize = 1.;
|
|
qpOptions.maxNumLineSearchIterations = 25;
|
|
qpOptions.maxNumLineSearchRefinementIterations = 25;
|
|
/* qpOptions.regType = QPDUNES_REG_SINGULAR_DIRECTIONS; */
|
|
qpOptions.regType = QPDUNES_REG_LEVENBERG_MARQUARDT;
|
|
|
|
qpDUNES_setup(&qpData, @ACADO_N@, @ACADO_NX@, @ACADO_NU@, nD, &( qpOptions ));
|
|
|
|
for (kk = 0; kk < @ACADO_N@; ++kk)
|
|
{
|
|
qpData.intervals[ kk ]->H.sparsityType = @DIAG_H@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE;
|
|
}
|
|
qpData.intervals[ @ACADO_N@ ]->H.sparsityType = @DIAG_HN@ ? QPDUNES_DIAGONAL : QPDUNES_DENSE;
|
|
|
|
statusFlag = qpDUNES_init(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@);
|
|
|
|
return (int)statusFlag;
|
|
}
|
|
|
|
void cleanupQpDunes( void )
|
|
{
|
|
qpDUNES_cleanup( &qpData );
|
|
}
|
|
|
|
int prepareQpDunes( void )
|
|
{
|
|
return_t statusFlag;
|
|
@MODULE_NAME@_timer tmr;
|
|
|
|
@MODULE_NAME@_tic( &tmr );
|
|
statusFlag = qpDUNES_updateData(&qpData, @QP_H@, @QP_G@, @QP_C@, @QP_c@, @QP_LB@, @QP_UB@, @QP_D@, @QP_LBA@, @QP_UBA@);
|
|
if (statusFlag != QPDUNES_OK)
|
|
return (int)statusFlag;
|
|
prepareQPtime = @MODULE_NAME@_toc( &tmr );
|
|
|
|
return (int)statusFlag;
|
|
}
|
|
|
|
int solveQpDunes( void )
|
|
{
|
|
return_t statusFlag;
|
|
@MODULE_NAME@_timer tmr;
|
|
|
|
@MODULE_NAME@_tic( &tmr );
|
|
if ( @INITIAL_STATE_FIXED@ )
|
|
statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ 0 ], 0, 0, 0, 0, @QP_LB_0@, @QP_UB_0@, 0, 0, 0, 0);
|
|
else
|
|
statusFlag = qpDUNES_updateIntervalData(&qpData, qpData.intervals[ @ACADO_N@ ], 0, @QP_G_N@, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
if (statusFlag != QPDUNES_OK)
|
|
return (int)statusFlag;
|
|
updateQPtime = @MODULE_NAME@_toc( &tmr );
|
|
|
|
@MODULE_NAME@_tic( &tmr );
|
|
statusFlag = qpDUNES_solve( &qpData );
|
|
|
|
qpDUNES_getPrimalSol(&qpData, @QP_PRIMAL@);
|
|
qpDUNES_getDualSol(&qpData, @QP_LAMBDA@, @QP_MU@);
|
|
solveQPtime = @MODULE_NAME@_toc( &tmr );
|
|
|
|
return (int)statusFlag;
|
|
}
|
|
|