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


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