diff --git a/acado/code_generation/export_gauss_newton_cn2.cpp b/acado/code_generation/export_gauss_newton_cn2.cpp index 5cd534320..36cc4d74e 100644 --- a/acado/code_generation/export_gauss_newton_cn2.cpp +++ b/acado/code_generation/export_gauss_newton_cn2.cpp @@ -687,21 +687,27 @@ returnValue ExportGaussNewtonCN2::setupConstraintsEvaluation( void ) // Optionally store derivatives if ( pacEvHx.isGiven() == false ) { - loopPac.addStatement( - pacEvHx.makeRowVector(). - getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX) - == conValueOut.getCols(derOffset, derOffset + dimPacH * NX ) - ); + for (unsigned j1 = 0; j1 < dimPacH; ++j1) { + for (unsigned j2 = 0; j2 < NX; ++j2) { + loopPac.addStatement( + pacEvHx.getElement(runPac * dimPacH + j1, j2) == + conValueOut.getCol(derOffset + j1*NX+j2) + ); + } + } derOffset = derOffset + dimPacH * NX; } if (pacEvHu.isGiven() == false ) { - loopPac.addStatement( - pacEvHu.makeRowVector(). - getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU) - == conValueOut.getCols(derOffset, derOffset + dimPacH * NU ) - ); + for (unsigned j1 = 0; j1 < dimPacH; ++j1) { + for (unsigned j2 = 0; j2 < NU; ++j2) { + loopPac.addStatement( + pacEvHu.getElement(runPac * dimPacH + j1, j2) == + conValueOut.getCol(derOffset + j1*NU+j2) + ); + } + } derOffset = derOffset + dimPacH * NU; } diff --git a/acado/code_generation/export_gauss_newton_generic.cpp b/acado/code_generation/export_gauss_newton_generic.cpp index af6531bd4..17389451b 100644 --- a/acado/code_generation/export_gauss_newton_generic.cpp +++ b/acado/code_generation/export_gauss_newton_generic.cpp @@ -56,6 +56,25 @@ returnValue ExportGaussNewtonGeneric::setup( ) setupAuxiliaryFunctions(); + int useArrivalCost; + get(CG_USE_ARRIVAL_COST, useArrivalCost); + if (useArrivalCost == YES) { + ExportVariable evRet("ret", 1, 1, INT, ACADO_LOCAL, true); + + ExportVariable evReset("reset", 1, 1, INT, ACADO_LOCAL, true); + evReset.setDoc("Reset S_{AC}. Set it to 1 to initialize arrival cost calculation, " + "and later should set it to 0."); + + updateArrivalCost.init("updateArrivalCost", evReset); + updateArrivalCost.doc("Use this function to update the arrival cost."); + updateArrivalCost.setReturnValue( evRet ); + updateArrivalCost << (evRet == 0); + + acWL.setup("WL", NX, NX, REAL, ACADO_VARIABLES); + acWL.setDoc("Arrival cost term: Cholesky decomposition, lower triangular, " + " of the inverse of the state noise covariance matrix."); + } + return SUCCESSFUL_RETURN; } @@ -129,6 +148,8 @@ returnValue ExportGaussNewtonGeneric::getFunctionDeclarations( ExportStatementBl declarations.addDeclaration( evaluateStageCost ); declarations.addDeclaration( evaluateTerminalCost ); + declarations.addDeclaration( updateArrivalCost ); + return SUCCESSFUL_RETURN; } @@ -188,6 +209,12 @@ returnValue ExportGaussNewtonGeneric::getCode( ExportStatementBlock& code code.addFunction( getKKT ); code.addFunction( getObjective ); + int useArrivalCost; + get(CG_USE_ARRIVAL_COST, useArrivalCost); + if (useArrivalCost == YES) { + code.addFunction( updateArrivalCost ); + } + return SUCCESSFUL_RETURN; } @@ -404,7 +431,11 @@ returnValue ExportGaussNewtonGeneric::setupObjectiveEvaluation( void ) ExportVariable qq, rr; qq.setup("stageq", NX, 1, REAL, ACADO_LOCAL); rr.setup("stager", NU, 1, REAL, ACADO_LOCAL); - setStagef.setup("setStagef", qq, rr, index); + + + ExportVariable SlxCall = objSlx.isGiven() == true ? objSlx : ExportVariable("Slx", NX, 1, REAL, ACADO_LOCAL); + ExportVariable SluCall = objSlu.isGiven() == true ? objSlu : ExportVariable("Slu", NU, 1, REAL, ACADO_LOCAL); + setStagef.setup("setStagef", qq, rr, SlxCall, SluCall, index); if (Q2.isGiven() == false) setStagef.addStatement( @@ -417,6 +448,7 @@ returnValue ExportGaussNewtonGeneric::setupObjectiveEvaluation( void ) qq == Q2 * Dy.getRows(index * NY, (index + 1) * NY) ); } + setStagef.addStatement( qq += SlxCall ); setStagef.addLinebreak(); if (R2.isGiven() == false) @@ -429,6 +461,7 @@ returnValue ExportGaussNewtonGeneric::setupObjectiveEvaluation( void ) rr == R2 * Dy.getRows(index * NY, (index + 1) * NY) ); } + setStagef.addStatement( rr += SluCall ); // // Setup necessary QP variables @@ -687,19 +720,27 @@ returnValue ExportGaussNewtonGeneric::setupConstraintsEvaluation( void ) // Optionally store derivatives if (pacEvHx.isGiven() == false) { - loopPac.addStatement( - pacEvHx.makeRowVector().getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX) == - conValueOut.getCols(derOffset, derOffset + dimPacH * NX ) - ); + for (unsigned j1 = 0; j1 < dimPacH; ++j1) { + for (unsigned j2 = 0; j2 < NX; ++j2) { + loopPac.addStatement( + pacEvHx.getElement(runPac * dimPacH + j1, j2) == + conValueOut.getCol(derOffset + j1*NX+j2) + ); + } + } derOffset = derOffset + dimPacH * NX; } if (pacEvHu.isGiven() == false ) { - loopPac.addStatement( - pacEvHu.makeRowVector().getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU) == - conValueOut.getCols(derOffset, derOffset + dimPacH * NU ) - ); + for (unsigned j1 = 0; j1 < dimPacH; ++j1) { + for (unsigned j2 = 0; j2 < NU; ++j2) { + loopPac.addStatement( + pacEvHu.getElement(runPac * dimPacH + j1, j2) == + conValueOut.getCol(derOffset + j1*NU+j2) + ); + } + } } // Add loop to the function. @@ -856,6 +897,16 @@ returnValue ExportGaussNewtonGeneric::setupMultiplicationRoutines( ) returnValue ExportGaussNewtonGeneric::setupEvaluation( ) { + int gradientUp; + get( LIFTED_GRADIENT_UPDATE, gradientUp ); + bool gradientUpdate = (bool) gradientUp; + + int variableObjS; + get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS); + int sensitivityProp; + get( DYNAMIC_SENSITIVITY, sensitivityProp ); + bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate)); + //////////////////////////////////////////////////////////////////////////// // // Setup preparation phase @@ -914,19 +965,37 @@ returnValue ExportGaussNewtonGeneric::setupEvaluation( ) feedback.addStatement( DyN -= yN ); feedback.addLinebreak(); - for (unsigned i = 0; i < N; ++i) - feedback.addFunctionCall(setStagef, qpq.getAddress(i * NX), qpr.getAddress(i * NU), ExportIndex( i )); + for (unsigned i = 0; i < N; ++i) { + ExportArgument SlxCall = + objSlx.isGiven() == true || (variableObjS == false && !adjoint) ? objSlx : objSlx.getAddress(i * NX, 0); + ExportArgument SluCall = + objSlu.isGiven() == true || (variableObjS == false && !adjoint) ? objSlu : objSlu.getAddress(i * NU, 0); + + feedback.addFunctionCall(setStagef, qpq.getAddress(i * NX), qpr.getAddress(i * NU), SlxCall, SluCall, ExportIndex( i )); + } feedback.addLinebreak(); + ExportVariable SlxCall = + objSlx.isGiven() == true || (variableObjS == false && !adjoint) ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX); feedback.addStatement( qpqf == QN2 * DyN ); + feedback.addStatement( qpqf += SlxCall ); + feedback.addLinebreak(); - // - // Arrival cost in the MHE case - // - if (initialStateFixed() == false) +// // +// // Arrival cost in the MHE case +// // +// if (initialStateFixed() == false) +// { +// // It is assumed this is the shifted version from the previous time step! +// feedback.addStatement( DxAC == xAC - x.getRow( 0 ).getTranspose() ); +// } + + if (SAC.getDim() > 0) { - // It is assumed this is the shifted version from the previous time step! - feedback.addStatement( DxAC == xAC - x.getRow( 0 ).getTranspose() ); + // Include arrival cost + feedback.addStatement( DxAC == x.getRow( 0 ).getTranspose() - xAC ); + feedback.addStatement( Q1.getRows(0,NX) += SAC ); + feedback.addStatement( qpq.getRows(0,NX) += SAC*DxAC ); } // @@ -943,11 +1012,11 @@ returnValue ExportGaussNewtonGeneric::setupEvaluation( ) feedback.addStatement( x.makeColVector() += qpx ); feedback.addStatement( u.makeColVector() += qpu ); - if (initialStateFixed() == false) - { - // This is the arrival cost for the next time step! - feedback.addStatement( xAC == x.getRow( 1 ).getTranspose() + DxAC ); - } +// if (initialStateFixed() == false) +// { +// // This is the arrival cost for the next time step! +// feedback.addStatement( xAC == x.getRow( 1 ).getTranspose() + DxAC ); +// } //////////////////////////////////////////////////////////////////////////// // diff --git a/acado/code_generation/export_gauss_newton_hpmpc.cpp b/acado/code_generation/export_gauss_newton_hpmpc.cpp index 0681a71c7..597a4cdb5 100644 --- a/acado/code_generation/export_gauss_newton_hpmpc.cpp +++ b/acado/code_generation/export_gauss_newton_hpmpc.cpp @@ -71,6 +71,9 @@ returnValue ExportGaussNewtonHpmpc::getDataDeclarations( ExportStatementBlock& d if (status != SUCCESSFUL_RETURN) return status; + int hardcodeConstraintValues; + get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues); + declarations.addDeclaration(x0, dataStruct); if (Q1.isGiven() == true) @@ -102,6 +105,14 @@ returnValue ExportGaussNewtonHpmpc::getDataDeclarations( ExportStatementBlock& d declarations.addDeclaration(nIt, dataStruct); + if (hardcodeConstraintValues == NO) { + declarations.addDeclaration(evLbValues, dataStruct); + declarations.addDeclaration(evUbValues, dataStruct); + + declarations.addDeclaration(evLbAValues, dataStruct); + declarations.addDeclaration(evUbAValues, dataStruct); + } + return SUCCESSFUL_RETURN; } @@ -507,8 +518,8 @@ returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void ) -// int hardcodeConstraintValues; -// get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues); + int hardcodeConstraintValues; + get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues); evaluateConstraints.setup("evaluateConstraints"); @@ -567,11 +578,22 @@ returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void ) qpLb.setup("qpLb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE); qpUb.setup("qpUb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE); - evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL); - evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL); + if( hardcodeConstraintValues == YES ) { + evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL); + evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL); - evaluateConstraints.addVariable( evLbValues ); - evaluateConstraints.addVariable( evUbValues ); + evaluateConstraints.addVariable( evLbValues ); + evaluateConstraints.addVariable( evUbValues ); + } + else { + evLbValues.setup("lbValues", N * NU + N * NX, 1, REAL, ACADO_VARIABLES); + evLbValues.setDoc( "Lower bounds values." ); + evUbValues.setup("ubValues", N * NU + N * NX, 1, REAL, ACADO_VARIABLES); + evUbValues.setDoc( "Upper bounds values." ); + + initialize.addStatement( evLbValues == lbValues ); + initialize.addStatement( evUbValues == ubValues ); + } evaluateConstraints.addStatement( qpLb.getRows(0, N * NU) == evLbValues.getRows(0, N * NU) - u.makeColVector() ); evaluateConstraints.addStatement( qpUb.getRows(0, N * NU) == evUbValues.getRows(0, N * NU) - u.makeColVector() ); @@ -649,11 +671,22 @@ returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void ) lbAValues.append( pocLbStack[ N ] ); ubAValues.append( pocUbStack[ N ] ); - ExportVariable evLbAValues("lbAValues", lbAValues, STATIC_CONST_REAL); - ExportVariable evUbAValues("ubAValues", ubAValues, STATIC_CONST_REAL); + if( hardcodeConstraintValues == YES || qpDimHtot == 0 ) { + evLbAValues.setup("lbAValues", lbAValues, STATIC_CONST_REAL); + evUbAValues.setup("ubAValues", ubAValues, STATIC_CONST_REAL); - evaluateConstraints.addVariable( evLbAValues ); - evaluateConstraints.addVariable( evUbAValues ); + evaluateConstraints.addVariable( evLbAValues ); + evaluateConstraints.addVariable( evUbAValues ); + } + else { + evLbAValues.setup("lbAValues", qpDimHtot, 1, REAL, ACADO_VARIABLES); + evLbAValues.setDoc( "Lower affine bounds values." ); + evUbAValues.setup("ubAValues", qpDimHtot, 1, REAL, ACADO_VARIABLES); + evUbAValues.setDoc( "Upper affine bounds values." ); + + initialize.addStatement( evLbAValues == lbAValues ); + initialize.addStatement( evUbAValues == ubAValues ); + } // // Evaluate path constraints diff --git a/acado/code_generation/export_gauss_newton_hpmpc.hpp b/acado/code_generation/export_gauss_newton_hpmpc.hpp index f5c1495ea..7633eec52 100644 --- a/acado/code_generation/export_gauss_newton_hpmpc.hpp +++ b/acado/code_generation/export_gauss_newton_hpmpc.hpp @@ -195,7 +195,8 @@ class ExportGaussNewtonHpmpc : public ExportNLPSolver ExportVariable qpq, qpqf, qpr; ExportVariable qpx, qpu; - ExportVariable evLbValues, evUbValues; + ExportVariable evLbValues, evUbValues; + ExportVariable evLbAValues, evUbAValues; ExportVariable qpLb, qpUb; ExportVariable qpLbA, qpUbA; diff --git a/acado/code_generation/export_gauss_newton_qpdunes.cpp b/acado/code_generation/export_gauss_newton_qpdunes.cpp index d40457271..f2c2bdd9c 100644 --- a/acado/code_generation/export_gauss_newton_qpdunes.cpp +++ b/acado/code_generation/export_gauss_newton_qpdunes.cpp @@ -107,6 +107,8 @@ returnValue ExportGaussNewtonQpDunes::getDataDeclarations( ExportStatementBlock& declarations.addDeclaration(qpUb, dataStruct); declarations.addDeclaration(lbValues, dataStruct); declarations.addDeclaration(ubValues, dataStruct); + declarations.addDeclaration(evLbAValues, dataStruct); + declarations.addDeclaration(evUbAValues, dataStruct); declarations.addDeclaration(qpC, dataStruct); declarations.addDeclaration(qpc, dataStruct); @@ -734,8 +736,8 @@ returnValue ExportGaussNewtonQpDunes::setupConstraintsEvaluation( void ) if (getNumComplexConstraints() == 0) return SUCCESSFUL_RETURN; - else if(hardcodeConstraintValues == YES) - return ACADOERROR( RET_NOT_IMPLEMENTED_YET ); +// else if(hardcodeConstraintValues == NO) +// return ACADOERROR( RET_NOT_IMPLEMENTED_YET ); unsigned dimLbA = N * dimPacH; unsigned dimConA = dimLbA * (NX + NU); @@ -778,8 +780,8 @@ returnValue ExportGaussNewtonQpDunes::setupConstraintsEvaluation( void ) { if ( dimPacH ) { - lbAValues.append( lbPathConValues.block(i * NX, 0, NX, 1) ); - ubAValues.append( ubPathConValues.block(i * NX, 0, NX, 1) ); + lbAValues.append( lbPathConValues.block(i * dimPacH, 0, dimPacH, 1) ); + ubAValues.append( ubPathConValues.block(i * dimPacH, 0, dimPacH, 1) ); } lbAValues.append( pocLbStack[ i ] ); ubAValues.append( pocUbStack[ i ] ); @@ -787,11 +789,21 @@ returnValue ExportGaussNewtonQpDunes::setupConstraintsEvaluation( void ) lbAValues.append( pocLbStack[ N ] ); ubAValues.append( pocUbStack[ N ] ); - ExportVariable evLbAValues("lbAValues", lbAValues, STATIC_CONST_REAL); - ExportVariable evUbAValues("ubAValues", ubAValues, STATIC_CONST_REAL); + if (hardcodeConstraintValues == YES || dimLbA == 0) { + evLbAValues.setup("lbAValues", lbAValues, STATIC_CONST_REAL); + evUbAValues.setup("ubAValues", ubAValues, STATIC_CONST_REAL); - evaluateConstraints.addVariable( evLbAValues ); - evaluateConstraints.addVariable( evUbAValues ); + evaluateConstraints.addVariable( evLbAValues ); + evaluateConstraints.addVariable( evUbAValues ); + } else { + evLbAValues.setup("lbAValues", dimLbA, 1, REAL, ACADO_VARIABLES); + evLbAValues.setDoc( "Lower affine bounds values." ); + evUbAValues.setup("ubAValues", dimLbA, 1, REAL, ACADO_VARIABLES); + evUbAValues.setDoc( "Upper affine bounds values." ); + + initialize.addStatement( evLbAValues == lbAValues ); + initialize.addStatement( evUbAValues == ubAValues ); + } // // Evaluate path constraints diff --git a/acado/code_generation/export_gauss_newton_qpdunes.hpp b/acado/code_generation/export_gauss_newton_qpdunes.hpp index 433377318..22347db84 100644 --- a/acado/code_generation/export_gauss_newton_qpdunes.hpp +++ b/acado/code_generation/export_gauss_newton_qpdunes.hpp @@ -163,6 +163,7 @@ class ExportGaussNewtonQpDunes : public ExportNLPSolver ExportVariable qpLb, qpUb; ExportVariable lbValues, ubValues; + ExportVariable evLbAValues, evUbAValues; ExportVariable qpA; ExportVariable qpLbA, qpUbA; diff --git a/acado/code_generation/export_nlp_solver.cpp b/acado/code_generation/export_nlp_solver.cpp index 7aef19901..1fe6f3610 100644 --- a/acado/code_generation/export_nlp_solver.cpp +++ b/acado/code_generation/export_nlp_solver.cpp @@ -1862,9 +1862,10 @@ returnValue ExportNLPSolver::setupGetLSQObjective() { if ( SAC.getDim() > 0 ) { + getObjective.addStatement( DxAC == x.getRow( 0 ).getTranspose() - xAC ); getObjective.addVariable( tmpDx ); getObjective.addStatement( tmpDx == DxAC.getTranspose() * SAC ); - getObjective.addStatement( objVal += tmpDx * DxAC ); + getObjective.addStatement( objVal += tmpDx * DxAC ); } getObjective.addLinebreak( ); diff --git a/acado/code_generation/ocp_export.cpp b/acado/code_generation/ocp_export.cpp index 3062f8c9e..5e5a13c83 100644 --- a/acado/code_generation/ocp_export.cpp +++ b/acado/code_generation/ocp_export.cpp @@ -752,6 +752,8 @@ returnValue OCPexport::exportAcadoHeader( const std::string& _dirName, get(CG_USE_ARRIVAL_COST, useAC); int covCalc; get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc); + int singlePrec; + get(USE_SINGLE_PRECISION, singlePrec); int linSolver; get(LINEAR_ALGEBRA_SOLVER, linSolver); @@ -794,6 +796,8 @@ returnValue OCPexport::exportAcadoHeader( const std::string& _dirName, make_pair(toString( useAC ), "Providing interface for arrival cost."); options[ modulePrefix + "_COMPUTE_COVARIANCE_MATRIX" ] = make_pair(toString( covCalc ), "Compute covariance matrix of the last state estimate."); + options[ modulePrefix + "_SINGLE_PRECISION" ] = + make_pair(toString( singlePrec ), "Single versus double precision data type representation."); options[ modulePrefix + "_QP_NV" ] = make_pair(toString( solver->getNumQPvars() ), "Total number of QP optimization variables."); diff --git a/acado/code_generation/templates/acado_solver_mex.c.in b/acado/code_generation/templates/acado_solver_mex.c.in index 4a85c5789..d91dc7c72 100644 --- a/acado/code_generation/templates/acado_solver_mex.c.in +++ b/acado/code_generation/templates/acado_solver_mex.c.in @@ -92,6 +92,8 @@ output /** Instance of the private workspace structure. */ @MODULE_PREFIX@workspace @MODULE_NAME@Workspace; +real_t QP_violation = 0; + #if @MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_FORCES #include "forces.h" extern forces_info @MODULE_NAME@Forces_info; @@ -167,7 +169,7 @@ int getArray( const unsigned mandatory, destination = (real_t*)mxCalloc(nRows * nCols, sizeof( real_t )); if (nRows == 1 && nCols == 1) - *destination = *dPtr; + *destination = (real_t)*dPtr; else for (i = 0; i < nRows; ++i) for (j = 0; j < nCols; ++j) @@ -189,7 +191,7 @@ void setArray( mxArray* destination, unsigned i, j; if (nRows == 1 && nCols == 1) - *dPtr = *source; + *dPtr = (double)*source; else for (i = 0; i < nRows; ++i) for(j = 0; j < nCols; ++j) @@ -214,15 +216,15 @@ void mexFunction( int nlhs, real_t* uEnd = NULL; const mxArray* src = prhs[ 0 ]; - const char *infoNames[ 5 ] = {"status", "cpuTime", "kktValue", "objValue", "nIterations"}; + const char *infoNames[ 6 ] = {"status", "cpuTime", "kktValue", "objValue", "QP_iter", "QP_violation"}; mxArray* info; real_t status, cpuTime, kktValue, objValue; - double tmp[ 1 ]; + real_t tmp[ 1 ]; mxArray* shPtr; #ifndef _DSPACE @MODULE_NAME@_timer tmr; #endif - double nIterations = 0; + real_t nIterations = 0; const char *outNames[ NOO_4 ]; outNames[ 0 ] = "info"; @@ -293,6 +295,16 @@ void mexFunction( int nlhs, getArray(1, src, 0, "WN", @MODULE_NAME@Variables.WN, @MODULE_PREFIX@_NYN, @MODULE_PREFIX@_NYN); #endif /* @MODULE_PREFIX@_WEIGHTING_MATRICES_TYPE */ +#if @MODULE_PREFIX@_USE_LINEAR_TERMS == 1 +#if @MODULE_PREFIX@_WEIGHTING_MATRICES_TYPE == 1 + getArray(1, src, 0, "Wlx", @MODULE_NAME@Variables.Wlx, @MODULE_PREFIX@_NX, 1); + getArray(1, src, 0, "Wlu", @MODULE_NAME@Variables.Wlu, @MODULE_PREFIX@_NU, 1); +#elif @MODULE_PREFIX@_WEIGHTING_MATRICES_TYPE == 2 + getArray(1, src, 0, "Wlx", @MODULE_NAME@Variables.Wlx, (@MODULE_PREFIX@_N+1) * @MODULE_PREFIX@_NX, 1); + getArray(1, src, 0, "Wlu", @MODULE_NAME@Variables.Wlu, @MODULE_PREFIX@_N * @MODULE_PREFIX@_NU, 1); +#endif /* @MODULE_PREFIX@_WEIGHTING_MATRICES_TYPE */ +#endif /* @MODULE_PREFIX@_USE_LINEAR_TERMS */ + #if (@MODULE_PREFIX@_HARDCODED_CONSTRAINT_VALUES == 0) if (!initialized) @@ -423,8 +435,8 @@ void mexFunction( int nlhs, kktValue = @MODULE_NAME@_getKKT(); objValue = @MODULE_NAME@_getObjective(); -#if ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) ) - nIterations = (double)@MODULE_NAME@_getNWSR(); +#if ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_GENERIC) ) + nIterations = (real_t)@MODULE_NAME@_getNWSR(); #elif @MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_FORCES nIterations = @MODULE_NAME@Forces_info.it; #elif @MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPDUNES @@ -468,8 +480,8 @@ void mexFunction( int nlhs, kktValue = @MODULE_NAME@_getKKT(); objValue = @MODULE_NAME@_getObjective(); -#if ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) ) - nIterations = (double)@MODULE_NAME@_getNWSR(); +#if ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_GENERIC) ) + nIterations = (real_t)@MODULE_NAME@_getNWSR(); #elif @MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_FORCES nIterations = @MODULE_NAME@Forces_info.it; #endif /* @MODULE_PREFIX@_QP_SOLVER */ @@ -517,14 +529,15 @@ void mexFunction( int nlhs, #endif /* @MODULE_PREFIX@_COMPUTE_COVARIANCE_MATRIX */ /* Create the info structure. */ - info = mxCreateStructMatrix(1, 1, 5, infoNames); + info = mxCreateStructMatrix(1, 1, 6, infoNames); setArray(info, 0, "status", &status, 1, 1); setArray(info, 0, "cpuTime", &cpuTime, 1, 1); setArray(info, 0, "kktValue", &kktValue, 1, 1); setArray(info, 0, "objValue", &objValue, 1, 1); -#if ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) ) - setArray(info, 0, "nIterations", &nIterations, 1, 1); +#if ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_GENERIC) ) + setArray(info, 0, "QP_iter", &nIterations, 1, 1); + setArray(info, 0, "QP_violation", &QP_violation, 1, 1); #endif /* ( (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES) || (@MODULE_PREFIX@_QP_SOLVER == @MODULE_PREFIX@_QPOASES3) ) */ mxSetField(plhs[ 0 ], 0, "info", info); diff --git a/package.xml b/package.xml index eb3bad31c..fe37ae4dc 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ acado - 1.2.3 + 1.2.4 ACADO Toolkit Milan Vukov Rien Quirynen