export_gauss_newton_qpdunes2.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_qpdunes2.hpp>
00033 
00034 #include <sstream>
00035 
00036 BEGIN_NAMESPACE_ACADO
00037 
00038 using namespace std;
00039 
00040 ExportGaussNewtonQpDunes2::ExportGaussNewtonQpDunes2(   UserInteraction* _userInteraction,
00041                                                                                                         const std::string& _commonHeaderName
00042                                                                                                         ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00043 {}
00044 
00045 returnValue ExportGaussNewtonQpDunes2::setup( )
00046 {
00047         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00048         setupInitialization();
00049 
00050         //
00051         // Add QP initialization call to the initialization
00052         //
00053         ExportFunction initializeQpDunes( "initializeQpDunes" );
00054         initialize
00055                 << "ret = (int)initializeQpDunes();\n"
00056                 << "if ((return_t)ret != QPDUNES_OK) return ret;\n";
00057 
00058         cleanup.setup( "cleanupSolver" );
00059         ExportFunction cleanupQpDunes( "cleanupQpDunes" );
00060         cleanup.addFunctionCall( cleanupQpDunes );
00061 
00062         LOG( LVL_DEBUG ) << "done!" << endl;
00063 
00064         setupVariables();
00065 
00066         setupSimulation();
00067 
00068         setupObjectiveEvaluation();
00069 
00070         setupConstraintsEvaluation();
00071 
00072         setupEvaluation();
00073 
00074         setupAuxiliaryFunctions();
00075 
00076         return SUCCESSFUL_RETURN;
00077 }
00078 
00079 returnValue ExportGaussNewtonQpDunes2::getDataDeclarations(     ExportStatementBlock& declarations,
00080                                                                                                                         ExportStruct dataStruct
00081                                                                                                                         ) const
00082 {
00083         returnValue status;
00084         status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00085         if (status != SUCCESSFUL_RETURN)
00086                 return status;
00087 
00088         declarations.addDeclaration(x0, dataStruct);
00089 
00090         declarations.addDeclaration(qpH, dataStruct);
00091         declarations.addDeclaration(qpg, dataStruct);
00092         declarations.addDeclaration(qpLb0, dataStruct);
00093         declarations.addDeclaration(qpUb0, dataStruct);
00094         declarations.addDeclaration(qpLb, dataStruct);
00095         declarations.addDeclaration(qpUb, dataStruct);
00096         declarations.addDeclaration(qpC, dataStruct);
00097         declarations.addDeclaration(qpc, dataStruct);
00098 
00099         declarations.addDeclaration(qpA, dataStruct);
00100         declarations.addDeclaration(qpLbA, dataStruct);
00101         declarations.addDeclaration(qpUbA, dataStruct);
00102 
00103         declarations.addDeclaration(qpPrimal, dataStruct);
00104         declarations.addDeclaration(qpLambda, dataStruct);
00105         declarations.addDeclaration(qpMu, dataStruct);
00106 
00107         return SUCCESSFUL_RETURN;
00108 }
00109 
00110 returnValue ExportGaussNewtonQpDunes2::getFunctionDeclarations( ExportStatementBlock& declarations
00111                                                                                                                                 ) const
00112 {
00113         declarations.addDeclaration( preparation );
00114         declarations.addDeclaration( feedback );
00115 
00116         declarations.addDeclaration( initialize );
00117         declarations.addDeclaration( initializeNodes );
00118         declarations.addDeclaration( shiftStates );
00119         declarations.addDeclaration( shiftControls );
00120         declarations.addDeclaration( getKKT );
00121         declarations.addDeclaration( getObjective );
00122 
00123         declarations.addDeclaration( cleanup );
00124         declarations.addDeclaration( shiftQpData );
00125 
00126         declarations.addDeclaration( evaluateStageCost );
00127         declarations.addDeclaration( evaluateTerminalCost );
00128 
00129         return SUCCESSFUL_RETURN;
00130 }
00131 
00132 returnValue ExportGaussNewtonQpDunes2::getCode( ExportStatementBlock& code
00133                                                                                                                 )
00134 {
00135         setupQPInterface();
00136         code.addStatement( *qpInterface );
00137 
00138         code.addLinebreak( 2 );
00139         code.addStatement( "/******************************************************************************/\n" );
00140         code.addStatement( "/*                                                                            */\n" );
00141         code.addStatement( "/* ACADO code generation                                                      */\n" );
00142         code.addStatement( "/*                                                                            */\n" );
00143         code.addStatement( "/******************************************************************************/\n" );
00144         code.addLinebreak( 2 );
00145 
00146         int useOMP;
00147         get(CG_USE_OPENMP, useOMP);
00148         if ( useOMP )
00149         {
00150                 code.addDeclaration( state );
00151         }
00152 
00153         code.addFunction( modelSimulation );
00154 
00155         code.addFunction( evaluateStageCost );
00156         code.addFunction( evaluateTerminalCost );
00157         code.addFunction( setObjQ1Q2 );
00158         code.addFunction( setObjR1R2 );
00159         code.addFunction( setObjQN1QN2 );
00160         code.addFunction( setStageH );
00161         code.addFunction( setStagef );
00162         code.addFunction( evaluateObjective );
00163 
00164         code.addFunction( evaluatePathConstraints );
00165 
00166         for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00167         {
00168                 if (evaluatePointConstraints[ i ] == 0)
00169                         continue;
00170                 code.addFunction( *evaluatePointConstraints[ i ] );
00171         }
00172 
00173         code.addFunction( setStagePac );
00174         code.addFunction( evaluateConstraints );
00175 
00176         code.addFunction( acc );
00177 
00178         code.addFunction( preparation );
00179         code.addFunction( feedback );
00180 
00181         code.addFunction( initialize );
00182         code.addFunction( initializeNodes );
00183         code.addFunction( shiftStates );
00184         code.addFunction( shiftControls );
00185         code.addFunction( getKKT );
00186         code.addFunction( getObjective );
00187 
00188         code.addFunction( cleanup );
00189         code.addFunction( shiftQpData );
00190 
00191         return SUCCESSFUL_RETURN;
00192 }
00193 
00194 
00195 unsigned ExportGaussNewtonQpDunes2::getNumQPvars( ) const
00196 {
00197         return (N + 1) * NX + N * NU;
00198 }
00199 
00200 //
00201 // PROTECTED FUNCTIONS:
00202 //
00203 
00204 returnValue ExportGaussNewtonQpDunes2::setupObjectiveEvaluation( void )
00205 {
00206         evaluateObjective.setup("evaluateObjective");
00207 
00208         int variableObjS;
00209         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00210 
00211 //      if (S1.isGiven() == false or S1.getGivenMatrix().isZero() == false)
00212 //              return ACADOFATALTEXT(RET_INVALID_ARGUMENTS,
00213 //                              "Mixed control-state terms in the objective function are not supported at the moment.");
00214 
00215         qpH.setup("qpH", N * (NX + NU) * (NX + NU) + NX * NX, 1, REAL, ACADO_WORKSPACE);
00216         qpg.setup("qpG", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
00217 
00218         //
00219         // LM regularization preparation
00220         //
00221 
00222         ExportVariable evLmX = zeros<double>(NX, NX);
00223         ExportVariable evLmU = zeros<double>(NU, NU);
00224 
00225         if  (levenbergMarquardt > 0.0)
00226         {
00227                 DMatrix lmX = eye<double>( NX );
00228                 lmX *= levenbergMarquardt;
00229 
00230                 DMatrix lmU = eye<double>( NU );
00231                 lmU *= levenbergMarquardt;
00232 
00233                 evLmX = lmX;
00234                 evLmU = lmU;
00235         }
00236 
00237         //
00238         // Main loop that calculates Hessian and gradients
00239         //
00240 
00241         ExportIndex runObj( "runObj" );
00242         ExportForLoop loopObjective( runObj, 0, N );
00243 
00244         evaluateObjective.addIndex( runObj );
00245 
00246         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00247         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00248         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00249         loopObjective.addLinebreak( );
00250 
00251         // Evaluate the objective function
00252         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00253 
00254         // Stack the measurement function value
00255         loopObjective.addStatement(
00256                         Dy.getRows(runObj * NY, (runObj + 1) * NY) ==  objValueOut.getTranspose().getRows(0, getNY())
00257         );
00258         loopObjective.addLinebreak( );
00259 
00260         // Optionally compute derivatives
00261         unsigned indexX = getNY();
00262         //      unsigned indexG = indexX;
00263 
00264         ExportVariable tmpObjS, tmpFx, tmpFu;
00265         ExportVariable tmpFxEnd, tmpObjSEndTerm;
00266         tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00267         if (objS.isGiven() == true)
00268                 tmpObjS = objS;
00269         tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00270         if (objEvFx.isGiven() == true)
00271                 tmpFx = objEvFx;
00272         tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00273         if (objEvFu.isGiven() == true)
00274                 tmpFu = objEvFu;
00275         tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00276         if (objEvFxEnd.isGiven() == true)
00277                 tmpFxEnd = objEvFxEnd;
00278         tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00279         if (objSEndTerm.isGiven() == true)
00280                 tmpObjSEndTerm = objSEndTerm;
00281 
00282         //
00283         // Optional computation of Q1, Q2
00284         //
00285         if (Q1.isGiven() == false)
00286         {
00287                 ExportVariable tmpQ1, tmpS1, tmpQ2;
00288                 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00289                 tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
00290                 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00291 
00292                 setObjQ1Q2.setup("setObjQ1S1Q2", tmpFx, tmpObjS, tmpQ1, tmpS1, tmpQ2);
00293                 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00294                 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00295                 setObjQ1Q2.addStatement( tmpS1 == tmpQ2 * tmpFu );
00296 
00297                 if (tmpFx.isGiven() == true)
00298                 {
00299                         if (variableObjS == YES)
00300                         {
00301                                 loopObjective.addFunctionCall(
00302                                                 setObjQ1Q2,
00303                                                 tmpFx, objS.getAddress(runObj * NY, 0),
00304                                                 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00305                                 );
00306                         }
00307                         else
00308                         {
00309                                 loopObjective.addFunctionCall(
00310                                                 setObjQ1Q2,
00311                                                 tmpFx, objS,
00312                                                 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00313                                 );
00314                         }
00315                 }
00316                 else
00317                 {
00318                         if (variableObjS == YES)
00319                         {
00320                                 if (objEvFx.isGiven() == true)
00321 
00322                                         loopObjective.addFunctionCall(
00323                                                         setObjQ1Q2,
00324                                                         objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00325                                                         Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00326                                         );
00327                         }
00328                         else
00329                         {
00330                                 loopObjective.addFunctionCall(
00331                                                 setObjQ1Q2,
00332                                                 objValueOut.getAddress(0, indexX), objS,
00333                                                 Q1.getAddress(runObj * NX, 0), S1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00334                                 );
00335                         }
00336                         indexX += objEvFx.getDim();
00337                 }
00338 
00339                 loopObjective.addLinebreak( );
00340         }
00341 
00342         if (R1.isGiven() == false)
00343         {
00344                 ExportVariable tmpR1, tmpR2;
00345                 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00346                 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00347 
00348                 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00349                 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00350                 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00351 
00352                 if (tmpFu.isGiven() == true)
00353                 {
00354                         if (variableObjS == YES)
00355                         {
00356                                 loopObjective.addFunctionCall(
00357                                                 setObjR1R2,
00358                                                 tmpFu, objS.getAddress(runObj * NY, 0),
00359                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00360                                 );
00361                         }
00362                         else
00363                         {
00364                                 loopObjective.addFunctionCall(
00365                                                 setObjR1R2,
00366                                                 tmpFu, objS,
00367                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00368                                 );
00369                         }
00370                 }
00371                 else
00372                 {
00373                         if (variableObjS == YES)
00374                         {
00375                                 loopObjective.addFunctionCall(
00376                                                 setObjR1R2,
00377                                                 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00378                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00379                                 );
00380                         }
00381                         else
00382                         {
00383                                 loopObjective.addFunctionCall(
00384                                                 setObjR1R2,
00385                                                 objValueOut.getAddress(0, indexX), objS,
00386                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00387                                 );
00388                         }
00389                 }
00390 
00391                 loopObjective.addLinebreak( );
00392         }
00393 
00394         evaluateObjective.addStatement( loopObjective );
00395 
00396         //
00397         // Evaluate the quadratic Mayer term
00398         //
00399         evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00400         evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00401 
00402         // Evaluate the objective function
00403         evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00404         evaluateObjective.addLinebreak( );
00405 
00406         evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00407         evaluateObjective.addLinebreak();
00408 
00409         if (QN1.isGiven() == false)
00410         {
00411                 indexX = getNYN();
00412 
00413                 ExportVariable tmpQN1, tmpQN2;
00414                 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00415                 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00416 
00417                 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00418                 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00419                 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00420 
00421                 if (tmpFxEnd.isGiven() == true)
00422                         evaluateObjective.addFunctionCall(
00423                                         setObjQN1QN2,
00424                                         tmpFxEnd, objSEndTerm,
00425                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00426                         );
00427                 else
00428                         evaluateObjective.addFunctionCall(
00429                                         setObjQN1QN2,
00430                                         objValueOut.getAddress(0, indexX), objSEndTerm,
00431                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00432                         );
00433 
00434                 evaluateObjective.addLinebreak( );
00435         }
00436 
00437         //
00438         // Hessian setup
00439         //
00440 
00441         ExportVariable stageH;
00442         ExportIndex index( "index" );
00443         stageH.setup("stageH", NX + NU, NX + NU, REAL, ACADO_LOCAL);
00444         setStageH.setup("setStageH", stageH, index);
00445 
00446         if (Q1.isGiven() == false)
00447                 setStageH.addStatement(
00448                                 stageH.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(index * NX, (index + 1) * NX, 0, NX) + evLmX
00449                 );
00450         else
00451         {
00452                 setStageH.addStatement( index == index );
00453                 setStageH.addStatement(
00454                                 stageH.getSubMatrix(0, NX, 0, NX) == Q1 + evLmX
00455                 );
00456         }
00457         setStageH.addLinebreak();
00458 
00459         if (S1.isGiven() == false) {
00460                 setStageH.addStatement(
00461                                 stageH.getSubMatrix(0, NX, NX, NX+NU) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU)
00462                 );
00463                 setStageH.addStatement(
00464                                 stageH.getSubMatrix(NX, NX+NU, 0, NX) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU).getTranspose()
00465                 );
00466         }
00467         else
00468         {
00469                 setStageH.addStatement( index == index );
00470                 setStageH.addStatement(
00471                                 stageH.getSubMatrix(0, NX, NX, NX+NU) == S1
00472                 );
00473                 setStageH.addStatement(
00474                                 stageH.getSubMatrix(NX, NX+NU, 0, NX) == S1.getTranspose()
00475                 );
00476         }
00477         setStageH.addLinebreak();
00478 
00479         if (R1.isGiven() == false)
00480                 setStageH.addStatement(
00481                                 stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1.getSubMatrix(index * NU, (index + 1) * NU, 0, NU) + evLmU
00482                 );
00483         else
00484                 setStageH.addStatement(
00485                                 stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1 + evLmU
00486                 );
00487 
00488         if (Q1.isGiven() == true && R1.isGiven() == true)
00489         {
00490                 for (unsigned i = 0; i < N; ++i)
00491                 {
00492                         initialize.addFunctionCall(
00493                                         setStageH, qpH.getAddress(i * (NX + NU) * (NX + NU)), ExportIndex( i ));
00494                 }
00495                 initialize.addLinebreak();
00496                 initialize.addStatement(
00497                                 qpH.getTranspose().getCols(N * (NX + NU) * (NX + NU), N * (NX + NU) * (NX + NU) + NX * NX) == QN1.makeRowVector() + evLmX.makeRowVector()
00498                 );
00499         }
00500         else
00501         {
00502                 ACADOWARNINGTEXT(RET_NOT_YET_IMPLEMENTED, "Under dev, has to be tested!");
00503 
00504                 for (unsigned i = 0; i < N; ++i)
00505                 {
00506                         evaluateObjective.addFunctionCall(
00507                                         setStageH, qpH.getAddress(i * (NX + NU) * (NX + NU)), ExportIndex( i ));
00508                 }
00509                 evaluateObjective.addLinebreak();
00510                 evaluateObjective.addStatement(
00511                                 qpH.getTranspose().getCols(N * (NX + NU) * (NX + NU), N * (NX + NU) * (NX + NU) + NX * NX) == QN1.makeRowVector() + evLmX.makeRowVector()
00512                 );
00513         }
00514 
00515         //
00516         // Gradient setup
00517         //
00518 
00519         ExportVariable stagef;
00520         stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
00521         setStagef.setup("setStagef", stagef, index);
00522 
00523         if (Q2.isGiven() == false)
00524                 setStagef.addStatement(
00525                                 stagef.getRows(0, NX) == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) *
00526                                 Dy.getRows(index * NY, (index + 1) * NY)
00527                 );
00528         else
00529         {
00530                 setStagef.addStatement( index == index );
00531                 setStagef.addStatement(
00532                                 stagef.getRows(0, NX) == Q2 *
00533                                 Dy.getRows(index * NY, (index + 1) * NY)
00534                 );
00535         }
00536         setStagef.addLinebreak();
00537 
00538         if (R2.isGiven() == false)
00539                 setStagef.addStatement(
00540                                 stagef.getRows(NX, NX + NU) == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) *
00541                                 Dy.getRows(index * NY, (index + 1) * NY)
00542                 );
00543         else
00544         {
00545                 setStagef.addStatement(
00546                                 stagef.getRows(NX, NX + NU) == R2 *
00547                                 Dy.getRows(index * NY, (index + 1) * NY)
00548                 );
00549         }
00550 
00551         return SUCCESSFUL_RETURN;
00552 }
00553 
00554 returnValue ExportGaussNewtonQpDunes2::setupConstraintsEvaluation( void )
00555 {
00557         //
00558         // Setup evaluation of box constraints on states and controls
00559         //
00561 
00562         // XXX This is for the NMPC case! TODO MHE!
00563         qpLb0.setup("qpLb0", 1, NX + NU, REAL, ACADO_WORKSPACE);
00564         qpUb0.setup("qpUb0", 1, NX + NU, REAL, ACADO_WORKSPACE);
00565         qpLb.setup("qpLb", 1, N * (NX + NU) + NX, REAL, ACADO_WORKSPACE);
00566         qpUb.setup("qpUb", 1, N * (NX + NU) + NX, REAL, ACADO_WORKSPACE);
00567 
00568         DVector lbTmp, ubTmp;
00569         DVector lbXValues, ubXValues;
00570         DVector lbUValues, ubUValues;
00571 
00572         DVector lbXInf( NX );
00573         lbXInf.setAll( -INFTY );
00574 
00575         DVector ubXInf( NX );
00576         ubXInf.setAll( INFTY );
00577 
00578         //
00579         // Stack state bounds
00580         //
00581         for (unsigned i = 0; i < N + 1; ++i)
00582         {
00583                 lbTmp = xBounds.getLowerBounds( i );
00584                 if ( !lbTmp.getDim() )
00585                         lbXValues.append( lbXInf );
00586                 else
00587                         lbXValues.append( lbTmp );
00588 
00589                 ubTmp = xBounds.getUpperBounds( i );
00590                 if ( !ubTmp.getDim() )
00591                         ubXValues.append( ubXInf );
00592                 else
00593                         ubXValues.append( ubTmp );
00594         }
00595 
00596         ExportVariable evLbXValues("lbXValues", lbXValues, STATIC_CONST_REAL, ACADO_LOCAL);
00597         ExportVariable evUbXValues("ubXValues", ubXValues, STATIC_CONST_REAL, ACADO_LOCAL);
00598 
00599         //
00600         // Stack control constraints
00601         //
00602         for (unsigned i = 0; i < N; ++i)
00603         {
00604                 lbTmp = uBounds.getLowerBounds( i );
00605                 lbUValues.append( lbTmp );
00606 
00607                 ubTmp = uBounds.getUpperBounds( i );
00608                 ubUValues.append( ubTmp );
00609         }
00610 
00611         ExportVariable evLbUValues("lbUValues", lbUValues, STATIC_CONST_REAL, ACADO_LOCAL);
00612         ExportVariable evUbUValues("ubUValues", ubUValues, STATIC_CONST_REAL, ACADO_LOCAL);
00613 
00614         //
00615         // Export evaluation of simple box constraints
00616         //
00617         evaluateConstraints.setup("evaluateConstraints");
00618         evaluateConstraints.addVariable( evLbXValues );
00619         evaluateConstraints.addVariable( evUbXValues );
00620         evaluateConstraints.addVariable( evLbUValues );
00621         evaluateConstraints.addVariable( evUbUValues );
00622 
00623         evaluateConstraints.addStatement(
00624                         qpLb0.getCols(NX, NX + NU) == evLbUValues.getTranspose().getCols(0, NU) - u.getRow( 0 )
00625         );
00626         evaluateConstraints.addStatement(
00627                         qpUb0.getCols(NX, NX + NU) == evUbUValues.getTranspose().getCols(0, NU) - u.getRow( 0 )
00628         );
00629 
00630         ExportIndex ind( "ind" );
00631         evaluateConstraints.addIndex( ind );
00632         ExportForLoop lbLoop(ind, 0, N);
00633         ExportForLoop ubLoop(ind, 0, N);
00634 
00635         lbLoop.addStatement(
00636                         qpLb.getCols(ind * (NX + NU), ind * (NX + NU) + NX) ==
00637                                         evLbXValues.getTranspose().getCols(ind * NX, (ind + 1) * NX) - x.getRow( ind )
00638         );
00639         lbLoop.addStatement(
00640                         qpLb.getCols(ind * (NX + NU) + NX, (ind + 1) * (NX + NU)) ==
00641                                         evLbUValues.getTranspose().getCols(ind * NU, (ind + 1) * NU) - u.getRow( ind )
00642         );
00643 
00644         ubLoop.addStatement(
00645                         qpUb.getCols(ind * (NX + NU), ind * (NX + NU) + NX) ==
00646                                         evUbXValues.getTranspose().getCols(ind * NX, (ind + 1) * NX) - x.getRow( ind )
00647         );
00648         ubLoop.addStatement(
00649                         qpUb.getCols(ind * (NX + NU) + NX, (ind + 1) * (NX + NU)) ==
00650                                         evUbUValues.getTranspose().getCols(ind * NU, (ind + 1) * NU) - u.getRow( ind )
00651         );
00652 
00653         evaluateConstraints.addStatement( lbLoop );
00654         evaluateConstraints.addStatement( ubLoop );
00655         evaluateConstraints.addLinebreak();
00656 
00657         evaluateConstraints.addStatement(
00658                         qpLb.getCols(N * (NX + NU), N * (NX + NU) + NX) ==
00659                                         evLbXValues.getTranspose().getCols(N * NX, (N + 1) * NX) - x.getRow( N )
00660         );
00661         evaluateConstraints.addStatement(
00662                         qpUb.getCols(N * (NX + NU), N * (NX + NU) + NX) ==
00663                                         evUbXValues.getTranspose().getCols(N * NX, (N + 1) * NX) - x.getRow( N )
00664         );
00665         evaluateConstraints.addLinebreak();
00666 
00668         //
00669         // Evaluation of the equality constraints
00670         //  - system dynamics only
00671         //
00673 
00674         //
00675         // Set QP C matrix
00676         //
00677 
00678         qpC.setup("qpC", N * NX, NX + NU, REAL, ACADO_WORKSPACE);
00679         qpc.setup("qpc", N * NX, 1, REAL, ACADO_WORKSPACE);
00680 
00681         ExportForLoop cLoop(ind, 0, N);
00682 
00683         cLoop.addStatement(
00684                         qpC.getSubMatrix(ind * NX, (ind + 1) * NX, 0, NX) ==
00685                                         evGx.getSubMatrix(ind * NX, (ind + 1) * NX, 0, NX)
00686         );
00687         cLoop.addStatement(
00688                         qpC.getSubMatrix(ind * NX, (ind + 1) * NX, NX, NX + NU) ==
00689                                         evGu.getSubMatrix(ind * NX, (ind + 1) * NX, 0, NU)
00690         );
00691 
00692         evaluateConstraints.addStatement( cLoop );
00693         evaluateConstraints.addLinebreak();
00694 
00695         // TODO MHE case; consult Janick
00696 
00697         //
00698         // Set QP c vector, ONLY MS is supported for now, so we do not perform checks atm
00699         //
00700 
00701         evaluateConstraints.addStatement(
00702                         qpc == d
00703         );
00704         evaluateConstraints.addLinebreak();
00705 
00707         //
00708         // Setup evaluation of path and point constraints
00709         //
00711 
00712         if (getNumComplexConstraints() == 0)
00713                 return SUCCESSFUL_RETURN;
00714 
00715         unsigned dimLbA  = N * dimPacH;
00716         unsigned dimConA = dimLbA * (NX + NU);
00717 
00718         qpConDim.resize(N + 1, 0);
00719         for (unsigned i = 0; i < N; ++i)
00720                 qpConDim[ i ] += dimPacH;
00721 
00722         for (unsigned i = 0; i < N; ++i)
00723                 if (evaluatePointConstraints[ i ])
00724                 {
00725                         unsigned dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
00726 
00727                         dimLbA  += dim;
00728                         dimConA += dim * (NX + NU);
00729 
00730                         qpConDim[ i ] += dim;
00731                 }
00732 
00733         if (evaluatePointConstraints[ N ])
00734         {
00735                 unsigned dim = evaluatePointConstraints[ N ]->getFunctionDim() / (1 + NX);
00736                 dimLbA  += dim;
00737                 dimConA += dim * NX;
00738 
00739                 qpConDim[ N ] += dim;
00740         }
00741 
00742         qpA.setup("qpA", dimConA, 1, REAL, ACADO_WORKSPACE);
00743         qpLbA.setup("qpLbA", dimLbA, 1, REAL, ACADO_WORKSPACE);
00744         qpUbA.setup("qpUbA", dimLbA, 1, REAL, ACADO_WORKSPACE);
00745 
00746         //
00747         // Setup constraint values for the whole horizon.
00748         //
00749         DVector lbAValues;
00750         DVector ubAValues;
00751 
00752         for (unsigned i = 0; i < N; ++i)
00753         {
00754                 if ( dimPacH )
00755                 {
00756                         lbAValues.append( lbPathConValues.block(i * NX, 0, NX, 1) );
00757                         ubAValues.append( ubPathConValues.block(i * NX, 0, NX, 1) );
00758                 }
00759                 lbAValues.append( pocLbStack[ i ] );
00760                 ubAValues.append( pocUbStack[ i ] );
00761         }
00762         lbAValues.append( pocLbStack[ N ] );
00763         ubAValues.append( pocUbStack[ N ] );
00764 
00765         ExportVariable evLbAValues("lbAValues", lbAValues, STATIC_CONST_REAL, ACADO_LOCAL);
00766         ExportVariable evUbAValues("ubAValues", ubAValues, STATIC_CONST_REAL, ACADO_LOCAL);
00767 
00768         evaluateConstraints.addVariable( evLbAValues );
00769         evaluateConstraints.addVariable( evUbAValues );
00770 
00771         //
00772         // Evaluate path constraints
00773         //
00774 
00775         if ( dimPacH )
00776         {
00777                 ExportIndex runPac;
00778                 evaluateConstraints.acquire( runPac );
00779                 ExportForLoop loopPac(runPac, 0, N);
00780 
00781                 loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
00782                 loopPac.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( runPac ) );
00783                 loopPac.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runPac ) );
00784                 loopPac.addFunctionCall( evaluatePathConstraints.getName(), conValueIn, conValueOut );
00785 
00786                 loopPac.addStatement( pacEvH.getRows( runPac * dimPacH, (runPac + 1) * dimPacH) ==
00787                                 conValueOut.getTranspose().getRows(0, dimPacH) );
00788                 loopPac.addLinebreak( );
00789 
00790                 unsigned derOffset = dimPacH;
00791 
00792                 // Optionally store derivatives
00793                 if (pacEvHx.isGiven() == false)
00794                 {
00795                         loopPac.addStatement(
00796                                         pacEvHx.makeRowVector().getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX) ==
00797                                                         conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
00798                         );
00799 
00800                         derOffset = derOffset + dimPacH * NX;
00801                 }
00802                 if (pacEvHu.isGiven() == false )
00803                 {
00804                         loopPac.addStatement(
00805                                         pacEvHu.makeRowVector().getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU) ==
00806                                                         conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
00807                         );
00808                 }
00809 
00810                 // Add loop to the function.
00811                 evaluateConstraints.addStatement( loopPac );
00812                 evaluateConstraints.release( runPac );
00813                 evaluateConstraints.addLinebreak( );
00814         }
00815 
00816         //
00817         // Evaluate point constraints
00818         //
00819 
00820         for (unsigned i = 0, intRowOffset = 0, dim = 0; i < N + 1; ++i)
00821         {
00822                 if (evaluatePointConstraints[ i ] == 0)
00823                         continue;
00824 
00825                 evaluateConstraints.addComment(
00826                                 string( "Evaluating constraint on node: #" ) + toString( i )
00827                 );
00828 
00829                 evaluateConstraints.addStatement(conValueIn.getCols(0, getNX()) == x.getRow( i ) );
00830                 if (i < N)
00831                 {
00832                         evaluateConstraints.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
00833                         evaluateConstraints.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
00834                 }
00835                 else
00836                         evaluateConstraints.addStatement( conValueIn.getCols(NX, NX + NOD) == od.getRow( i ) );
00837 
00838                 evaluateConstraints.addFunctionCall(
00839                                 evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
00840                 evaluateConstraints.addLinebreak();
00841 
00842                 if (i < N)
00843                         dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
00844                 else
00845                         dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
00846 
00847                 // Fill pocEvH, pocEvHx, pocEvHu
00848                 evaluateConstraints.addStatement(
00849                                 pocEvH.getRows(intRowOffset, intRowOffset + dim) ==
00850                                                 conValueOut.getTranspose().getRows(0, dim));
00851                 evaluateConstraints.addLinebreak();
00852 
00853                 evaluateConstraints.addStatement(
00854                                 pocEvHx.makeRowVector().getCols(intRowOffset * NX, (intRowOffset + dim) * NX)
00855                                                 == conValueOut.getCols(dim, dim + dim * NX));
00856                 evaluateConstraints.addLinebreak();
00857 
00858                 if (i < N)
00859                 {
00860                         evaluateConstraints.addStatement(
00861                                         pocEvHu.makeRowVector().getCols(intRowOffset * NU, (intRowOffset + dim) * NU)
00862                                                         == conValueOut.getCols(dim + dim * NX, dim + dim * NX + dim * NU));
00863                         evaluateConstraints.addLinebreak();
00864                 }
00865 
00866                 intRowOffset += dim;
00867         }
00868 
00869         //
00870         // Copy data to QP solver structures
00871         //
00872 
00873         // TODO Make a function which is gonna set blocks of A, lbA, ubA for path constraints
00874         //      Here, an argument should be a offset from the beginning on the corresponding
00875         //      matrix or vector.
00876 
00877         ExportVariable tLbAValues, tUbAValues, tPacA;
00878         ExportIndex offsetPac("offset"), indPac( "ind" );
00879 
00880         tLbAValues.setup("lbAValues", dimPacH, 1, REAL, ACADO_LOCAL);
00881         tUbAValues.setup("ubAValues", dimPacH, 1, REAL, ACADO_LOCAL);
00882         tPacA.setup("tPacA", dimPacH, NX + NU, REAL, ACADO_LOCAL);
00883 
00884         setStagePac.setup("setStagePac", offsetPac, indPac, tPacA, tLbAValues, tUbAValues);
00885 
00886         if (pacEvHx.isGiven() == true)
00887                 setStagePac << (tPacA.getSubMatrix(0, dimPacH, 0, NX) == pacEvHx);
00888         else
00889                 setStagePac << (tPacA.getSubMatrix(0, dimPacH, 0, NX) ==
00890                                 pacEvHx.getSubMatrix(indPac * dimPacH, indPac * dimPacH + dimPacH, 0 , NX));
00891 
00892         if (pacEvHu.isGiven() == true)
00893                 setStagePac << (tPacA.getSubMatrix(0, dimPacH, NX, NX + NU) == pacEvHu);
00894         else
00895                 setStagePac << (tPacA.getSubMatrix(0, dimPacH, NX, NX + NU) ==
00896                                 pacEvHu.getSubMatrix(indPac * dimPacH, indPac * dimPacH + dimPacH, 0 , NU));
00897 
00898         setStagePac
00899                 << (qpLbA.getRows(offsetPac, offsetPac + dimPacH) == tLbAValues - pacEvH.getRows(indPac * dimPacH, indPac * dimPacH + dimPacH))
00900                 << (qpUbA.getRows(offsetPac, offsetPac + dimPacH) == tUbAValues - pacEvH.getRows(indPac * dimPacH, indPac * dimPacH + dimPacH));
00901 
00902         ExportVariable tPocA;
00903         tPocA.setup("tPocA", conValueOut.getDim(), NX + NU, REAL, ACADO_LOCAL);
00904         if ( dimPocH )
00905                 evaluateConstraints.addVariable( tPocA );
00906 
00907         unsigned offsetEval = 0;
00908         unsigned offsetPoc = 0;
00909         for (unsigned i = 0; i < N; ++i)
00910         {
00911                 if ( dimPacH )
00912                 {
00913                         evaluateConstraints.addFunctionCall(
00914                                         setStagePac,
00915                                         ExportIndex( offsetEval ), ExportIndex( i ),
00916                                         qpA.getAddress(offsetEval * (NX + NU)),
00917                                         evLbAValues.getAddress( offsetEval ), evUbAValues.getAddress( offsetEval )
00918                         );
00919 
00920                         offsetEval += dimPacH;
00921                 }
00922 
00923                 if ( evaluatePointConstraints[ i ] )
00924                 {
00925                         unsigned dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
00926 
00927                         evaluateConstraints.addLinebreak();
00928 
00929                         evaluateConstraints
00930                                 << (tPocA.getSubMatrix(0, dim, 0, NX) == pocEvHx.getSubMatrix(offsetPoc, offsetPoc + dim, 0, NX))
00931                                 << (tPocA.getSubMatrix(0, dim, NX, NX + NU) == pocEvHu.getSubMatrix(offsetPoc, offsetPoc + dim, 0, NU))
00932                                 << (qpA.getRows(offsetEval * (NX + NU), (offsetEval + dim) * (NX + NU)) == tPocA.makeColVector().getRows(0, dim * (NX + NU)))
00933                                 << (qpLbA.getRows(offsetEval, offsetEval + dim) ==
00934                                                 evLbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim))
00935                                 << (qpUbA.getRows(offsetEval, offsetEval + dim) ==
00936                                                 evUbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim));
00937 
00938                         offsetEval += dim;
00939                         offsetPoc += dim;
00940                 }
00941         }
00942 
00943         if ( evaluatePointConstraints[ N ] )
00944         {
00945                 unsigned dim = evaluatePointConstraints[ N ]->getFunctionDim() / (1 + NX);
00946 
00947                 evaluateConstraints
00948                         << (qpA.getRows(offsetEval * (NX + NU), offsetEval * (NX + NU) + dim * NX) ==
00949                                         pocEvHx.makeColVector().getRows(offsetPoc * (NX + NU), offsetPoc * (NX + NU) + dim * NX))
00950                         << (qpLbA.getRows(offsetEval, offsetEval + dim) ==
00951                                         evLbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim))
00952                         << (qpUbA.getRows(offsetEval, offsetEval + dim) ==
00953                                         evUbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim));
00954         }
00955 
00956         return SUCCESSFUL_RETURN;
00957 }
00958 
00959 returnValue ExportGaussNewtonQpDunes2::setupVariables( )
00960 {
00961         if (initialStateFixed() == true)
00962         {
00963                 x0.setup("x0",  NX, 1, REAL, ACADO_VARIABLES);
00964                 x0.setDoc( (std::string)"Current state feedback vector." );
00965         }
00966 
00967         return SUCCESSFUL_RETURN;
00968 }
00969 
00970 returnValue ExportGaussNewtonQpDunes2::setupMultiplicationRoutines( )
00971 {
00972         return SUCCESSFUL_RETURN;
00973 }
00974 
00975 returnValue ExportGaussNewtonQpDunes2::setupEvaluation( )
00976 {
00977         stringstream ss;
00978 
00980         //
00981         // Setup preparation phase
00982         //
00984         preparation.setup("preparationStep");
00985         preparation.doc( "Preparation step of the RTI scheme." );
00986 
00987         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
00988         retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
00989         preparation.setReturnValue(retSim, false);
00990 
00991         preparation     << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
00992 
00993         preparation.addFunctionCall( evaluateObjective );
00994         preparation.addFunctionCall( evaluateConstraints );
00995 
00997         //
00998         // Setup feedback phase
00999         //
01001         ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
01002         ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
01003         returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
01004         feedback.setup("feedbackStep" );
01005         feedback.doc( "Feedback/estimation step of the RTI scheme." );
01006         feedback.setReturnValue( returnValueFeedbackPhase );
01007 
01008         // XXX This is the MPC case!
01009         feedback.addStatement(
01010                         qpLb0.getTranspose().getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
01011         );
01012         feedback.addStatement(
01013                         qpUb0.getCols(0, NX) == qpLb0.getCols(0, NX)
01014         );
01015         feedback.addLinebreak();
01016 
01017         //
01018         // Calculate objective residuals
01019         //
01020         feedback.addStatement( Dy -= y );
01021         feedback.addLinebreak();
01022         feedback.addStatement( DyN -= yN );
01023         feedback.addLinebreak();
01024 
01025         for (unsigned i = 0; i < N; ++i)
01026                 feedback.addFunctionCall(setStagef, qpg.getAddress(i * (NX + NU)), ExportIndex( i ));
01027         feedback.addStatement( qpg.getRows(N * (NX + NU), N * (NX + NU) + NX) == QN2 * DyN );
01028         feedback.addLinebreak();
01029 
01030         feedback << returnValueFeedbackPhase.getFullName() << " = solveQpDunes();\n";
01031 
01032         //
01033         // Here we have to add the differences....
01034         //
01035 
01036         qpPrimal.setup("qpPrimal", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
01037         qpLambda.setup("qpLambda", N * NX, 1, REAL, ACADO_WORKSPACE);
01038 
01039         // XXX Janick, how to interpret those guys?
01040         qpMu.setup("qpMu", 2 * N * (NX + NU) + 2 * NX, 1, REAL, ACADO_WORKSPACE);
01041 
01042         // NOTE: Again, we are performing MS only
01043 
01044         ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
01045         ExportIndex index( "index" );
01046         acc.setup("accumulate", stageOut, index);
01047 
01048         acc     << (x.getRow( index ) += stageOut.getCols(0, NX))
01049                 << (u.getRow( index ) += stageOut.getCols(NX, NX + NU));
01050 
01051         for (unsigned i = 0; i < N; ++i)
01052                 feedback.addFunctionCall(acc, qpPrimal.getAddress(i * (NX + NU)), ExportIndex( i ));
01053         feedback.addLinebreak();
01054         feedback.addStatement(
01055                         x.getRow( N ) += qpPrimal.getTranspose().getCols(N * (NX + NU), N * (NX + NU) + NX)
01056         );
01057         feedback.addLinebreak();
01058 
01060         //
01061         // Shifting of QP data
01062         //
01064 
01065         shiftQpData.setup( "shiftQpData" );
01066         ss.str( string() );
01067         ss      << "qpDUNES_shiftLambda( &qpData );" << endl
01068                 << "qpDUNES_shiftIntervals( &qpData );" << endl;
01069         shiftQpData.addStatement( ss.str().c_str() );
01070 
01072         //
01073         // TODO Setup evaluation of KKT
01074         //
01076         ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
01077 
01078         getKKT.setup( "getKKT" );
01079         getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
01080 //      kkt.setDoc( "The KKT tolerance value." );
01081         kkt.setDoc( "0." );
01082         getKKT.setReturnValue( kkt );
01083 
01084         getKKT.addStatement( kkt == 0 );
01085 
01086         return SUCCESSFUL_RETURN;
01087 }
01088 
01089 returnValue ExportGaussNewtonQpDunes2::setupQPInterface( )
01090 {
01091         //
01092         // Configure and export QP interface
01093         //
01094 
01095         qpInterface = std::tr1::shared_ptr< ExportQpDunesInterface >(new ExportQpDunesInterface("", commonHeaderName));
01096 
01097         int maxNumQPiterations;
01098         get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
01099 
01100         // XXX If not specified, use default value
01101         if ( maxNumQPiterations <= 0 )
01102                 maxNumQPiterations = getNumQPvars();
01103 
01104         int printLevel;
01105         get(PRINTLEVEL, printLevel);
01106 
01107         if ( (PrintLevel)printLevel >= HIGH )
01108                 printLevel = 2;
01109         else
01110                 printLevel = 0;
01111 
01112         qpInterface->configure(
01113                         maxNumQPiterations,
01114                         printLevel,
01115                         qpH.getFullName(),
01116                         qpg.getFullName(),
01117                         " 0 ",
01118                         qpC.getFullName(),
01119                         qpc.getFullName(),
01120                         qpA.getFullName(),
01121                         qpLb0.getFullName(),
01122                         qpUb0.getFullName(),
01123                         qpLb.getFullName(),
01124                         qpUb.getFullName(),
01125                         qpLbA.getFullName(),
01126                         qpUbA.getFullName(),
01127                         qpPrimal.getFullName(),
01128                         qpLambda.getFullName(),
01129                         qpMu.getFullName(),
01130                         qpConDim,
01131                         "1",
01132                         "1",
01133                         "1"
01134         );
01135 
01136         return SUCCESSFUL_RETURN;
01137 }
01138 
01139 CLOSE_NAMESPACE_ACADO


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