export_gauss_newton_hpmpc.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00032 #include <acado/code_generation/export_gauss_newton_hpmpc.hpp>
00033 #include <acado/code_generation/export_hpmpc_interface.hpp>
00034 
00035 BEGIN_NAMESPACE_ACADO
00036 
00037 using namespace std;
00038 
00039 ExportGaussNewtonHpmpc::ExportGaussNewtonHpmpc( UserInteraction* _userInteraction,
00040                                                                                                         const std::string& _commonHeaderName
00041                                                                                                         ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00042 {}
00043 
00044 returnValue ExportGaussNewtonHpmpc::setup( )
00045 {
00046         setupInitialization();
00047 
00048         setupVariables();
00049 
00050         setupSimulation();
00051 
00052         setupObjectiveEvaluation();
00053 
00054         setupConstraintsEvaluation();
00055 
00056         setupEvaluation();
00057 
00058         setupAuxiliaryFunctions();
00059 
00060         setupQPInterface();
00061 
00062         return SUCCESSFUL_RETURN;
00063 }
00064 
00065 returnValue ExportGaussNewtonHpmpc::getDataDeclarations(        ExportStatementBlock& declarations,
00066                                                                                                                         ExportStruct dataStruct
00067                                                                                                                         ) const
00068 {
00069         returnValue status;
00070         status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00071         if (status != SUCCESSFUL_RETURN)
00072                 return status;
00073 
00074         declarations.addDeclaration(x0, dataStruct);
00075 
00076         if (Q1.isGiven() == true)
00077                 declarations.addDeclaration(qpQ, dataStruct);
00078         if (QN1.isGiven() == true)
00079                 declarations.addDeclaration(qpQf, dataStruct);
00080         if (S1.isGiven() == true)
00081                 declarations.addDeclaration(qpS, dataStruct);
00082         if (R1.isGiven() == true)
00083                 declarations.addDeclaration(qpR, dataStruct);
00084 
00085         declarations.addDeclaration(qpq, dataStruct);
00086         declarations.addDeclaration(qpqf, dataStruct);
00087         declarations.addDeclaration(qpr, dataStruct);
00088 
00089         declarations.addDeclaration(qpx, dataStruct);
00090         declarations.addDeclaration(qpu, dataStruct);
00091 
00092         declarations.addDeclaration(qpLb, dataStruct);
00093         declarations.addDeclaration(qpUb, dataStruct);
00094 
00095         declarations.addDeclaration(qpLambda, dataStruct);
00096         declarations.addDeclaration(qpMu, dataStruct);
00097         declarations.addDeclaration(qpSlacks, dataStruct);
00098 
00099         declarations.addDeclaration(nIt, dataStruct);
00100 
00101         return SUCCESSFUL_RETURN;
00102 }
00103 
00104 returnValue ExportGaussNewtonHpmpc::getFunctionDeclarations(    ExportStatementBlock& declarations
00105                                                                                                                                 ) const
00106 {
00107         declarations.addDeclaration( preparation );
00108         declarations.addDeclaration( feedback );
00109 
00110         declarations.addDeclaration( initialize );
00111         declarations.addDeclaration( initializeNodes );
00112         declarations.addDeclaration( shiftStates );
00113         declarations.addDeclaration( shiftControls );
00114         declarations.addDeclaration( getKKT );
00115         declarations.addDeclaration( getObjective );
00116 
00117         declarations.addDeclaration( evaluateStageCost );
00118         declarations.addDeclaration( evaluateTerminalCost );
00119 
00120         return SUCCESSFUL_RETURN;
00121 }
00122 
00123 returnValue ExportGaussNewtonHpmpc::getCode(    ExportStatementBlock& code
00124                                                                                                 )
00125 {
00126         qpInterface->exportCode();
00127 
00128         // Forward declaration, same as in the template file.
00129         code << "#ifdef __cplusplus\n";
00130         code << "extern \"C\"{\n";
00131         code << "#endif\n";
00132         code << "int acado_hpmpc_ip_wrapper(real_t* A, real_t* B, real_t* d, real_t* Q, real_t* Qf, real_t* S, real_t* R, real_t* q, real_t* qf, real_t* r, real_t* lb, real_t* ub, real_t* x, real_t* u, real_t* lambda, real_t* mu, real_t* slacks, int* nIt);\n";
00133         code << "#ifdef __cplusplus\n";
00134         code << "}\n";
00135         code << "#endif\n";
00136 
00137         code.addLinebreak( 2 );
00138         code.addStatement( "/******************************************************************************/\n" );
00139         code.addStatement( "/*                                                                            */\n" );
00140         code.addStatement( "/* ACADO code generation                                                      */\n" );
00141         code.addStatement( "/*                                                                            */\n" );
00142         code.addStatement( "/******************************************************************************/\n" );
00143         code.addLinebreak( 2 );
00144 
00145         int useOMP;
00146         get(CG_USE_OPENMP, useOMP);
00147         if ( useOMP )
00148         {
00149                 code.addDeclaration( state );
00150         }
00151 
00152         code.addFunction( modelSimulation );
00153 
00154         code.addFunction( evaluateStageCost );
00155         code.addFunction( evaluateTerminalCost );
00156         code.addFunction( setObjQ1Q2 );
00157         code.addFunction( setObjR1R2 );
00158         code.addFunction( setObjS1 );
00159         code.addFunction( setObjQN1QN2 );
00160         code.addFunction( setStagef );
00161         code.addFunction( evaluateObjective );
00162 
00163         code.addFunction( evaluateConstraints );
00164 
00165         code.addFunction( acc );
00166 
00167         code.addFunction( preparation );
00168         code.addFunction( feedback );
00169 
00170         code.addFunction( initialize );
00171         code.addFunction( initializeNodes );
00172         code.addFunction( shiftStates );
00173         code.addFunction( shiftControls );
00174         code.addFunction( getKKT );
00175         code.addFunction( getObjective );
00176 
00177         return SUCCESSFUL_RETURN;
00178 }
00179 
00180 
00181 unsigned ExportGaussNewtonHpmpc::getNumQPvars( ) const
00182 {
00183         return (N + 1) * NX + N * NU;
00184 }
00185 
00186 //
00187 // PROTECTED FUNCTIONS:
00188 //
00189 
00190 returnValue ExportGaussNewtonHpmpc::setupObjectiveEvaluation( void )
00191 {
00192         evaluateObjective.setup("evaluateObjective");
00193 
00194         int variableObjS;
00195         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00196 
00197         ExportVariable evLmX = zeros<double>(NX, NX);
00198         ExportVariable evLmU = zeros<double>(NU, NU);
00199 
00200         if (levenbergMarquardt > 0.0)
00201         {
00202                 DMatrix lmX = eye<double>( NX );
00203                 lmX *= levenbergMarquardt;
00204 
00205                 DMatrix lmU = eye<double>( NU );
00206                 lmU *= levenbergMarquardt;
00207 
00208                 evLmX = lmX;
00209                 evLmU = lmU;
00210         }
00211 
00212         //
00213         // Main loop that calculates Hessian and gradients
00214         //
00215 
00216         ExportIndex runObj( "runObj" );
00217         ExportForLoop loopObjective( runObj, 0, N );
00218 
00219         evaluateObjective.addIndex( runObj );
00220 
00221         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00222         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00223         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00224         loopObjective.addLinebreak( );
00225 
00226         // Evaluate the objective function
00227         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00228 
00229         // Stack the measurement function value
00230         loopObjective.addStatement(
00231                         Dy.getRows(runObj * NY, (runObj + 1) * NY) ==  objValueOut.getTranspose().getRows(0, getNY())
00232         );
00233         loopObjective.addLinebreak( );
00234 
00235         // Optionally compute derivatives
00236 
00237         ExportVariable tmpObjS, tmpFx, tmpFu;
00238         ExportVariable tmpFxEnd, tmpObjSEndTerm;
00239         tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00240         if (objS.isGiven() == true)
00241                 tmpObjS = objS;
00242         tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00243         if (objEvFx.isGiven() == true)
00244                 tmpFx = objEvFx;
00245         tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00246         if (objEvFu.isGiven() == true)
00247                 tmpFu = objEvFu;
00248         tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00249         if (objEvFxEnd.isGiven() == true)
00250                 tmpFxEnd = objEvFxEnd;
00251         tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00252         if (objSEndTerm.isGiven() == true)
00253                 tmpObjSEndTerm = objSEndTerm;
00254 
00255         unsigned indexX = getNY();
00256         ExportArgument tmpFxCall = tmpFx;
00257         if (tmpFx.isGiven() == false)
00258         {
00259                 tmpFxCall = objValueOut.getAddress(0, indexX);
00260                 indexX += objEvFx.getDim();
00261         }
00262 
00263         ExportArgument tmpFuCall = tmpFu;
00264         if (tmpFu.isGiven() == false)
00265         {
00266                 tmpFuCall = objValueOut.getAddress(0, indexX);
00267         }
00268 
00269         ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
00270 
00271         //
00272         // Optional computation of Q1, Q2
00273         //
00274         if (Q1.isGiven() == false)
00275         {
00276                 ExportVariable tmpQ1, tmpQ2;
00277                 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00278                 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00279 
00280                 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00281                 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00282                 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00283                 setObjQ1Q2.addStatement( tmpQ1 += evLmX );
00284 
00285                 loopObjective.addFunctionCall(
00286                                 setObjQ1Q2,
00287                                 tmpFxCall, objSCall,
00288                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00289                 );
00290 
00291                 loopObjective.addLinebreak( );
00292         }
00293         else if (levenbergMarquardt > 0.0)
00294                 Q1 = Q1.getGivenMatrix() + evLmX.getGivenMatrix();
00295 
00296         if (R1.isGiven() == false)
00297         {
00298                 ExportVariable tmpR1, tmpR2;
00299                 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00300                 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00301 
00302                 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00303                 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00304                 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00305                 setObjR1R2.addStatement( tmpR1 += evLmU );
00306 
00307                 loopObjective.addFunctionCall(
00308                                 setObjR1R2,
00309                                 tmpFuCall, objSCall,
00310                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00311                 );
00312 
00313                 loopObjective.addLinebreak( );
00314         }
00315         else if (levenbergMarquardt > 0.0)
00316                 R1 = R1.getGivenMatrix() + evLmU.getGivenMatrix();
00317 
00318         if (S1.isGiven() == false)
00319         {
00320                 ExportVariable tmpS1;
00321                 ExportVariable tmpS2;
00322 
00323                 tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
00324                 tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
00325 
00326                 setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
00327                 setObjS1.addVariable( tmpS2 );
00328                 setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
00329                 setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
00330 
00331                 loopObjective.addFunctionCall(
00332                                 setObjS1,
00333                                 tmpFxCall, tmpFuCall, objSCall,
00334                                 S1.getAddress(runObj * NX, 0)
00335                 );
00336         }
00337 
00338         evaluateObjective.addStatement( loopObjective );
00339 
00340         //
00341         // Evaluate the quadratic Mayer term
00342         //
00343         evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00344         evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00345 
00346         // Evaluate the objective function, last node.
00347         evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00348         evaluateObjective.addLinebreak( );
00349 
00350         evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00351         evaluateObjective.addLinebreak();
00352 
00353         if (QN1.isGiven() == false)
00354         {
00355                 ExportVariable tmpQN1, tmpQN2;
00356                 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00357                 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00358 
00359                 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00360                 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00361                 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00362                 setObjQN1QN2.addStatement( tmpQN1 += evLmX );
00363 
00364                 indexX = getNYN();
00365                 ExportArgument tmpFxEndCall = tmpFxEnd.isGiven() == true ? tmpFxEnd  : objValueOut.getAddress(0, indexX);
00366 
00367                 evaluateObjective.addFunctionCall(
00368                                 setObjQN1QN2,
00369                                 tmpFxEndCall, objSEndTerm,
00370                                 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00371                 );
00372 
00373                 evaluateObjective.addLinebreak( );
00374         }
00375         else if (levenbergMarquardt > 0.0)
00376                 QN1 = QN1.getGivenMatrix() + evLmX.getGivenMatrix();
00377 
00378         //
00379         // Hessian setup
00380         //
00381 
00382         ExportIndex index( "index" );
00383 
00384         //
00385         // Gradient setup
00386         //
00387         ExportVariable qq, rr;
00388         qq.setup("stageq", NX, 1, REAL, ACADO_LOCAL);
00389         rr.setup("stager", NU, 1, REAL, ACADO_LOCAL);
00390         setStagef.setup("setStagef", qq, rr, index);
00391 
00392         if (Q2.isGiven() == false)
00393                 setStagef.addStatement(
00394                                 qq == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) * Dy.getRows(index * NY, (index + 1) * NY)
00395                 );
00396         else
00397         {
00398                 setStagef << "(void)" << index.getFullName() << ";\n";
00399                 setStagef.addStatement(
00400                                 qq == Q2 * Dy.getRows(index * NY, (index + 1) * NY)
00401                 );
00402         }
00403         setStagef.addLinebreak();
00404 
00405         if (R2.isGiven() == false)
00406                 setStagef.addStatement(
00407                                 rr == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) * Dy.getRows(index * NY, (index + 1) * NY)
00408                 );
00409         else
00410         {
00411                 setStagef.addStatement(
00412                                 rr == R2 * Dy.getRows(index * NY, (index + 1) * NY)
00413                 );
00414         }
00415 
00416         //
00417         // Setup necessary QP variables
00418         //
00419 
00420         if (Q1.isGiven() == true)
00421         {
00422                 qpQ.setup("qpQ", N * NX, NX, REAL, ACADO_WORKSPACE);
00423                 for (unsigned blk = 0; blk < N; ++blk)
00424                         initialize.addStatement( qpQ.getSubMatrix(blk * NX, (blk + 1) * NX, 0, NX) == Q1);
00425         }
00426         else
00427         {
00428                 qpQ = Q1;
00429         }
00430 
00431         if (R1.isGiven() == true)
00432         {
00433                 qpR.setup("qpR", N * NU, NU, REAL, ACADO_WORKSPACE);
00434                 for (unsigned blk = 0; blk < N; ++blk)
00435                         initialize.addStatement( qpR.getSubMatrix(blk * NU, (blk + 1) * NU, 0, NU) == R1);
00436         }
00437         else
00438         {
00439                 qpR = R1;
00440         }
00441 
00442         if (S1.isGiven() == true)
00443         {
00444                 qpS.setup("qpS", N * NX, NU, REAL, ACADO_WORKSPACE);
00445                 if (S1.getGivenMatrix().isZero() == true)
00446                         initialize.addStatement(qpS == zeros<double>(N * NX, NU));
00447                 else
00448                         for (unsigned blk = 0; blk < N; ++blk)
00449                                 initialize.addStatement( qpS.getSubMatrix(blk * NX, (blk + 1) * NX, 0, NU) == S1);
00450         }
00451         else
00452         {
00453                 qpS = S1;
00454         }
00455 
00456         if (QN1.isGiven() == true)
00457         {
00458                 qpQf.setup("qpQf", NX, NX, REAL, ACADO_WORKSPACE);
00459                 initialize.addStatement( qpQf == QN1 );
00460         }
00461         else
00462         {
00463                 qpQf = QN1;
00464         }
00465 
00466         return SUCCESSFUL_RETURN;
00467 }
00468 
00469 returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void )
00470 {
00472         //
00473         // Setup evaluation of box constraints on states and controls
00474         //
00476 
00477         evaluateConstraints.setup("evaluateConstraints");
00478 
00479         DVector lbTmp, ubTmp;
00480 
00481         DVector lbXInf( NX );
00482         lbXInf.setAll( -INFTY );
00483 
00484         DVector ubXInf( NX );
00485         ubXInf.setAll( INFTY );
00486 
00487         DVector lbUInf( NU );
00488         lbUInf.setAll( -INFTY );
00489 
00490         DVector ubUInf( NU );
00491         ubUInf.setAll( INFTY );
00492 
00493         DVector lbValues, ubValues;
00494 
00495         for (unsigned node = 0; node < N; ++node)
00496         {
00497                 lbTmp = uBounds.getLowerBounds( node );
00498                 if ( !lbTmp.getDim() )
00499                         lbValues.append( lbUInf );
00500                 else
00501                         lbValues.append( lbTmp );
00502 
00503                 ubTmp = uBounds.getUpperBounds( node );
00504                 if ( !ubTmp.getDim() )
00505                         ubValues.append( ubUInf );
00506                 else
00507                         ubValues.append( ubTmp );
00508         }
00509 
00510         for (unsigned node = 1; node < N + 1; ++node)
00511         {
00512                 lbTmp = xBounds.getLowerBounds( node );
00513                 if ( !lbTmp.getDim() )
00514                         lbValues.append( lbXInf );
00515                 else
00516                         lbValues.append( lbTmp );
00517 
00518                 ubTmp = xBounds.getUpperBounds( node );
00519                 if ( !ubTmp.getDim() )
00520                         ubValues.append( ubXInf );
00521                 else
00522                         ubValues.append( ubTmp );
00523         }
00524 
00525         qpLb.setup("qpLb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
00526         qpUb.setup("qpUb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
00527 
00528         evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL);
00529         evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL);
00530 
00531         evaluateConstraints.addVariable( evLbValues );
00532         evaluateConstraints.addVariable( evUbValues );
00533 
00534         evaluateConstraints.addStatement( qpLb.getRows(0, N * NU) == evLbValues.getRows(0, N * NU) - u.makeColVector() );
00535         evaluateConstraints.addStatement( qpUb.getRows(0, N * NU) == evUbValues.getRows(0, N * NU) - u.makeColVector() );
00536 
00537         evaluateConstraints.addStatement( qpLb.getRows(N * NU, N * NU + N * NX) == evLbValues.getRows(N * NU, N * NU + N * NX) - x.makeColVector().getRows(NX, NX * (N + 1)) );
00538         evaluateConstraints.addStatement( qpUb.getRows(N * NU, N * NU + N * NX) == evUbValues.getRows(N * NU, N * NU + N * NX) - x.makeColVector().getRows(NX, NX * (N + 1)) );
00539 
00540         return SUCCESSFUL_RETURN;
00541 }
00542 
00543 returnValue ExportGaussNewtonHpmpc::setupVariables( )
00544 {
00545         if (initialStateFixed() == true)
00546         {
00547                 x0.setup("x0",  NX, 1, REAL, ACADO_VARIABLES);
00548                 x0.setDoc( "Current state feedback vector." );
00549         }
00550 
00551         return SUCCESSFUL_RETURN;
00552 }
00553 
00554 returnValue ExportGaussNewtonHpmpc::setupMultiplicationRoutines( )
00555 {
00556         return SUCCESSFUL_RETURN;
00557 }
00558 
00559 returnValue ExportGaussNewtonHpmpc::setupEvaluation( )
00560 {
00562         //
00563         // Setup preparation phase
00564         //
00566         preparation.setup("preparationStep");
00567         preparation.doc( "Preparation step of the RTI scheme." );
00568 
00569         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
00570         retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
00571         preparation.setReturnValue(retSim, false);
00572 
00573         preparation     << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
00574 
00575         preparation.addFunctionCall( evaluateObjective );
00576         preparation.addFunctionCall( evaluateConstraints );
00577 
00579         //
00580         // Setup feedback phase
00581         //
00583         ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
00584         ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
00585         returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
00586         feedback.setup("feedbackStep" );
00587         feedback.doc( "Feedback/estimation step of the RTI scheme." );
00588         feedback.setReturnValue( returnValueFeedbackPhase );
00589 
00590         qpx.setup("qpx", NX * (N + 1), 1, REAL, ACADO_WORKSPACE);
00591         qpu.setup("qpu", NU * N,       1, REAL, ACADO_WORKSPACE);
00592 
00593         qpq.setup("qpq", NX * N, 1, REAL, ACADO_WORKSPACE);
00594         qpqf.setup("qpqf", NX, 1, REAL, ACADO_WORKSPACE);
00595         qpr.setup("qpr", NU * N, 1, REAL, ACADO_WORKSPACE);
00596 
00597         qpLambda.setup("qpLambda", N * NX, 1, REAL, ACADO_WORKSPACE);
00598         qpMu.setup("qpMu", 2 * N * (NX + NU), 1, REAL, ACADO_WORKSPACE);
00599         qpSlacks.setup("qpSlacks", 2 * N * (NX + NU), 1, REAL, ACADO_WORKSPACE);
00600 
00601         nIt.setup("nIt", 1, 1, INT, ACADO_WORKSPACE);
00602 
00603         // State feedback
00604         feedback.addStatement( qpx.getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose() );
00605 
00606         //
00607         // Calculate objective residuals
00608         //
00609         feedback.addStatement( Dy -= y );
00610         feedback.addLinebreak();
00611         feedback.addStatement( DyN -= yN );
00612         feedback.addLinebreak();
00613 
00614         for (unsigned i = 0; i < N; ++i)
00615                 feedback.addFunctionCall(setStagef, qpq.getAddress(i * NX), qpr.getAddress(i * NU), ExportIndex( i ));
00616         feedback.addLinebreak();
00617         feedback.addStatement( qpqf == QN2 * DyN );
00618         feedback.addLinebreak();
00619 
00620         //
00621         // Here we have to add the differences....
00622         //
00623 
00624         // Call the solver
00625         feedback
00626                 << returnValueFeedbackPhase.getFullName() << " = " << "acado_hpmpc_ip_wrapper("
00627 
00628                 << evGx.getAddressString( true ) << ", "
00629                 << evGu.getAddressString( true ) << ", "
00630                 << d.getAddressString( true ) << ", "
00631 
00632                 << qpQ.getAddressString( true ) << ", "
00633                 << qpQf.getAddressString( true ) << ", "
00634                 << qpS.getAddressString( true ) << ", "
00635                 << qpR.getAddressString( true ) << ", "
00636 
00637                 << qpq.getAddressString( true ) << ", "
00638                 << qpqf.getAddressString( true ) << ", "
00639                 << qpr.getAddressString( true ) << ", "
00640 
00641                 << qpLb.getAddressString( true ) << ", "
00642                 << qpUb.getAddressString( true ) << ", "
00643 
00644                 << qpx.getAddressString( true ) << ", "
00645                 << qpu.getAddressString( true ) << ", "
00646 
00647                 << qpLambda.getAddressString( true ) << ", "
00648                 << qpMu.getAddressString( true ) << ", "
00649                 << qpSlacks.getAddressString( true ) << ", "
00650 
00651                 << nIt.getAddressString( true )
00652                 << ");\n";
00653 
00654         // Accumulate the solution, i.e. perform full Newton step
00655         feedback.addStatement( x.makeColVector() += qpx );
00656         feedback.addStatement( u.makeColVector() += qpu );
00657 
00659         //
00660         // Setup evaluation of the KKT tolerance
00661         //
00663 
00664         ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
00665         ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
00666         ExportIndex index( "index" );
00667 
00668         getKKT.setup( "getKKT" );
00669         getKKT.doc( "Get the KKT tolerance of the current iterate." );
00670         kkt.setDoc( "The KKT tolerance value." );
00671         getKKT.setReturnValue( kkt );
00672         getKKT.addVariable( tmp );
00673         getKKT.addIndex( index );
00674 
00675         getKKT.addStatement( kkt == 0.0 );
00676 
00677         // XXX At the moment this is very very specific for the NMPC case
00678 
00679         getKKT.addStatement( tmp == (qpq ^ qpx.getRows(0, N * NX)) );
00680         getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
00681         getKKT.addStatement( tmp == (qpqf ^ qpx.getRows(N * NX, (N + 1) * NX)) );
00682         getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
00683         getKKT.addStatement( tmp == (qpr ^ qpu) );
00684         getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
00685 
00686         ExportForLoop lamLoop(index, 0, N * NX);
00687         lamLoop << kkt.getFullName() << "+= fabs( " << d.get(index, 0) << " * " << qpLambda.get(index, 0) << ");\n";
00688         getKKT.addStatement( lamLoop );
00689 
00690         ExportForLoop lbLoop(index, 0, N * NU + N * NX);
00691         lbLoop << kkt.getFullName() << "+= fabs( " << qpLb.get(index, 0) << " * " << qpMu.get(index, 0) << ");\n";
00692         ExportForLoop ubLoop(index, 0, N * NU + N * NX);
00693         ubLoop << kkt.getFullName() << "+= fabs( " << qpUb.get(index, 0) << " * " << qpMu.get(index + N * NU + N * NX, 0) << ");\n";
00694 
00695         getKKT.addStatement( lbLoop );
00696         getKKT.addStatement( ubLoop );
00697 
00698         return SUCCESSFUL_RETURN;
00699 }
00700 
00701 returnValue ExportGaussNewtonHpmpc::setupQPInterface( )
00702 {
00703         //
00704         // Configure and export QP interface
00705         //
00706 
00707         string folderName;
00708         get(CG_EXPORT_FOLDER_NAME, folderName);
00709         string outFile = folderName + "/acado_hpmpc_interface.c";
00710 
00711         qpInterface = std::tr1::shared_ptr< ExportHpmpcInterface >(new ExportHpmpcInterface(outFile, commonHeaderName));
00712 
00713         int maxNumQPiterations;
00714         get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
00715 
00716         // XXX If not specified, use default value
00717         if ( maxNumQPiterations <= 0 )
00718                 maxNumQPiterations = getNumQPvars();
00719 
00720         int printLevel;
00721         get(PRINTLEVEL, printLevel);
00722 
00723         if ( (PrintLevel)printLevel >= HIGH )
00724                 printLevel = 2;
00725         else
00726                 printLevel = 0;
00727 
00728         int useSinglePrecision;
00729         get(USE_SINGLE_PRECISION, useSinglePrecision);
00730 
00731         int hotstartQP;
00732         get(HOTSTART_QP, hotstartQP);
00733 
00734         qpInterface->configure(
00735                         maxNumQPiterations,
00736                         printLevel,
00737                         useSinglePrecision,
00738                         hotstartQP
00739         );
00740 
00741         return SUCCESSFUL_RETURN;
00742 }
00743 
00744 CLOSE_NAMESPACE_ACADO


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:01