export_gauss_newton_cn2.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_cn2.hpp>
00033 #include <acado/code_generation/export_qpoases_interface.hpp>
00034 
00035 using namespace std;
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 ExportGaussNewtonCN2::ExportGaussNewtonCN2(     UserInteraction* _userInteraction,
00040                                                                                         const std::string& _commonHeaderName
00041                                                                                         ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00042 {}
00043 
00044 returnValue ExportGaussNewtonCN2::setup( )
00045 {
00046         if (performFullCondensing() == false && initialStateFixed() == true)
00047                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00048         if (getNumComplexConstraints() > 0)
00049                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00050         if (performsSingleShooting() == true)
00051                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00052 
00053         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00054         setupInitialization();
00055         LOG( LVL_DEBUG ) << "done!" << endl;
00056 
00057         LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
00058         setupVariables();
00059         LOG( LVL_DEBUG ) << "done!" << endl;
00060 
00061         LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
00062         setupMultiplicationRoutines();
00063         LOG( LVL_DEBUG ) << "done!" << endl;
00064 
00065         LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
00066         setupSimulation();
00067         LOG( LVL_DEBUG ) << "done!" << endl;
00068 
00069         LOG( LVL_DEBUG ) << "Solver: setup objective evaluation... " << endl;
00070         setupObjectiveEvaluation();
00071         LOG( LVL_DEBUG ) << "done!" << endl;
00072 
00073         LOG( LVL_DEBUG ) << "Solver: setup condensing... " << endl;
00074         setupCondensing();
00075         LOG( LVL_DEBUG ) << "done!" << endl;
00076 
00077         LOG( LVL_DEBUG ) << "Solver: setup constraints... " << endl;
00078         setupConstraintsEvaluation();
00079         LOG( LVL_DEBUG ) << "done!" << endl;
00080 
00081         LOG( LVL_DEBUG ) << "Solver: setup evaluation... " << endl;
00082         setupEvaluation();
00083         LOG( LVL_DEBUG ) << "done!" << endl;
00084 
00085         LOG( LVL_DEBUG ) << "Solver: setup auxiliary functions... " << endl;
00086         setupAuxiliaryFunctions();
00087         LOG( LVL_DEBUG ) << "done!" << endl;
00088 
00089         return SUCCESSFUL_RETURN;
00090 }
00091 
00092 returnValue ExportGaussNewtonCN2::getDataDeclarations(  ExportStatementBlock& declarations,
00093                                                                                                                         ExportStruct dataStruct
00094                                                                                                                         ) const
00095 {
00096         ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00097 
00098         declarations.addDeclaration(sbar, dataStruct);
00099         declarations.addDeclaration(x0, dataStruct);
00100         declarations.addDeclaration(Dx0, dataStruct);
00101 
00102         declarations.addDeclaration(T1, dataStruct);
00103         declarations.addDeclaration(T2, dataStruct);
00104         declarations.addDeclaration(C, dataStruct);
00105 
00106         declarations.addDeclaration(W1, dataStruct);
00107         declarations.addDeclaration(W2, dataStruct);
00108         declarations.addDeclaration(E, dataStruct);
00109 
00110         declarations.addDeclaration(QDy, dataStruct);
00111         declarations.addDeclaration(w1, dataStruct);
00112         declarations.addDeclaration(w2, dataStruct);
00113 
00114         declarations.addDeclaration(lbValues, dataStruct);
00115         declarations.addDeclaration(ubValues, dataStruct);
00116         declarations.addDeclaration(lbAValues, dataStruct);
00117         declarations.addDeclaration(ubAValues, dataStruct);
00118 
00119         declarations.addDeclaration(H, dataStruct);
00120         declarations.addDeclaration(A, dataStruct);
00121         declarations.addDeclaration(g, dataStruct);
00122         declarations.addDeclaration(lb, dataStruct);
00123         declarations.addDeclaration(ub, dataStruct);
00124         declarations.addDeclaration(lbA, dataStruct);
00125         declarations.addDeclaration(ubA, dataStruct);
00126         declarations.addDeclaration(xVars, dataStruct);
00127         declarations.addDeclaration(yVars, dataStruct);
00128 
00129         // lagrange multipliers
00130         declarations.addDeclaration(mu, dataStruct);
00131 
00132         return SUCCESSFUL_RETURN;
00133 }
00134 
00135 returnValue ExportGaussNewtonCN2::getFunctionDeclarations(      ExportStatementBlock& declarations
00136                                                                                                                         ) const
00137 {
00138         declarations.addDeclaration( preparation );
00139         declarations.addDeclaration( feedback );
00140 
00141         declarations.addDeclaration( initialize );
00142         declarations.addDeclaration( initializeNodes );
00143         declarations.addDeclaration( shiftStates );
00144         declarations.addDeclaration( shiftControls );
00145         declarations.addDeclaration( getKKT );
00146         declarations.addDeclaration( getObjective );
00147 
00148         declarations.addDeclaration( evaluateStageCost );
00149         declarations.addDeclaration( evaluateTerminalCost );
00150 
00151         return SUCCESSFUL_RETURN;
00152 }
00153 
00154 returnValue ExportGaussNewtonCN2::getCode(      ExportStatementBlock& code
00155                                                                                         )
00156 {
00157         setupQPInterface();
00158 
00159         code.addLinebreak( 2 );
00160         code.addStatement( "/******************************************************************************/\n" );
00161         code.addStatement( "/*                                                                            */\n" );
00162         code.addStatement( "/* ACADO code generation                                                      */\n" );
00163         code.addStatement( "/*                                                                            */\n" );
00164         code.addStatement( "/******************************************************************************/\n" );
00165         code.addLinebreak( 2 );
00166 
00167         int useOMP;
00168         get(CG_USE_OPENMP, useOMP);
00169         if ( useOMP )
00170         {
00171                 code.addDeclaration( state );
00172         }
00173 
00174         code.addFunction( modelSimulation );
00175 
00176         code.addFunction( evaluateStageCost );
00177         code.addFunction( evaluateTerminalCost );
00178 
00179         code.addFunction( setObjQ1Q2 );
00180         code.addFunction( setObjR1R2 );
00181         code.addFunction( setObjS1 );
00182         code.addFunction( setObjQN1QN2 );
00183         code.addFunction( evaluateObjective );
00184 
00185         code.addFunction( regularizeHessian );
00186 
00187         code.addFunction( moveGxT );
00188         code.addFunction( multGxGx );
00189         code.addFunction( multGxGu );
00190         code.addFunction( moveGuE );
00191 
00192         code.addFunction( multBTW1 );
00193         code.addFunction( mac_S1T_E );
00194         code.addFunction( macBTW1_R1 );
00195         code.addFunction( multGxTGu );
00196         code.addFunction( macQEW2 );
00197 
00198         code.addFunction( macATw1QDy );
00199         code.addFunction( macBTw1 );
00200         code.addFunction( macS1TSbar );
00201         code.addFunction( macQSbarW2 );
00202         code.addFunction( macASbar );
00203         code.addFunction( expansionStep );
00204 
00205         code.addFunction( expansionStep2 );
00206 
00207         code.addFunction( mult_BT_T1 );
00208         code.addFunction( mac_ST_C );
00209         code.addFunction( multGxTGx );
00210         code.addFunction( macGxTGx );
00211 
00212         code.addFunction( copyHTH );
00213         code.addFunction( copyHTH1 );
00214 
00215         code.addFunction( multRDy );
00216         code.addFunction( multQDy );
00217 
00218         code.addFunction( multQN1Gx );
00219         code.addFunction( multQN1Gu );
00220 
00221         code.addFunction( evaluatePathConstraints );
00222 
00223         for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00224         {
00225                 if (evaluatePointConstraints[ i ] == 0)
00226                         continue;
00227                 code.addFunction( *evaluatePointConstraints[ i ] );
00228         }
00229 
00230         code.addFunction( condensePrep );
00231         code.addFunction( condenseFdb );
00232         code.addFunction( expand );
00233 
00234         code.addFunction( preparation );
00235         code.addFunction( feedback );
00236 
00237         code.addFunction( initialize );
00238         code.addFunction( initializeNodes );
00239         code.addFunction( shiftStates );
00240         code.addFunction( shiftControls );
00241         code.addFunction( getKKT );
00242         code.addFunction( getObjective );
00243 
00244         return SUCCESSFUL_RETURN;
00245 }
00246 
00247 
00248 unsigned ExportGaussNewtonCN2::getNumQPvars( ) const
00249 {
00250         if (performFullCondensing() == true)
00251                 return (N * NU);
00252 
00253         return (NX + N * NU);
00254 }
00255 
00256 unsigned ExportGaussNewtonCN2::getNumStateBounds() const
00257 {
00258         return xBoundsIdx.size();
00259 }
00260 
00261 //
00262 // PROTECTED FUNCTIONS:
00263 //
00264 
00265 returnValue ExportGaussNewtonCN2::setupObjectiveEvaluation( void )
00266 {
00267         evaluateObjective.setup("evaluateObjective");
00268 
00269         int variableObjS;
00270         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00271 
00272         //
00273         // A loop the evaluates objective and corresponding gradients
00274         //
00275         ExportIndex runObj( "runObj" );
00276         ExportForLoop loopObjective(runObj, 0, N);
00277 
00278         evaluateObjective.addIndex( runObj );
00279 
00280         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00281         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00282         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00283         loopObjective.addLinebreak( );
00284 
00285         // Evaluate the objective function
00286         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00287 
00288         // Stack the measurement function value
00289         loopObjective.addStatement(
00290                         Dy.getRows(runObj * NY, (runObj + 1) * NY) ==  objValueOut.getTranspose().getRows(0, getNY())
00291         );
00292         loopObjective.addLinebreak( );
00293 
00294         // Optionally compute derivatives
00295 
00296         ExportVariable tmpObjS, tmpFx, tmpFu;
00297         ExportVariable tmpFxEnd, tmpObjSEndTerm;
00298         tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00299         if (objS.isGiven() == true)
00300                 tmpObjS = objS;
00301         tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00302         if (objEvFx.isGiven() == true)
00303                 tmpFx = objEvFx;
00304         tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00305         if (objEvFu.isGiven() == true)
00306                 tmpFu = objEvFu;
00307         tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00308         if (objEvFxEnd.isGiven() == true)
00309                 tmpFxEnd = objEvFxEnd;
00310         tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00311         if (objSEndTerm.isGiven() == true)
00312                 tmpObjSEndTerm = objSEndTerm;
00313 
00314         unsigned indexX = getNY();
00315         ExportArgument tmpFxCall = tmpFx;
00316         if (tmpFx.isGiven() == false)
00317         {
00318                 tmpFxCall = objValueOut.getAddress(0, indexX);
00319                 indexX += objEvFx.getDim();
00320         }
00321 
00322         ExportArgument tmpFuCall = tmpFu;
00323         if (tmpFu.isGiven() == false)
00324         {
00325                 tmpFuCall = objValueOut.getAddress(0, indexX);
00326         }
00327 
00328         ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
00329 
00330         //
00331         // Optional computation of Q1, Q2
00332         //
00333         if (Q1.isGiven() == false)
00334         {
00335                 ExportVariable tmpQ1, tmpQ2;
00336                 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00337                 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00338 
00339                 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00340                 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00341                 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00342 
00343                 loopObjective.addFunctionCall(
00344                                 setObjQ1Q2,
00345                                 tmpFxCall, objSCall,
00346                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00347                 );
00348 
00349                 loopObjective.addLinebreak( );
00350         }
00351 
00352         if (R1.isGiven() == false)
00353         {
00354                 ExportVariable tmpR1, tmpR2;
00355                 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00356                 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00357 
00358                 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00359                 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00360                 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00361 
00362                 loopObjective.addFunctionCall(
00363                                 setObjR1R2,
00364                                 tmpFuCall, objSCall,
00365                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00366                 );
00367 
00368                 loopObjective.addLinebreak( );
00369         }
00370 
00371         if (S1.isGiven() == false)
00372         {
00373                 ExportVariable tmpS1;
00374                 ExportVariable tmpS2;
00375 
00376                 tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
00377                 tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
00378 
00379                 setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
00380                 setObjS1.addVariable( tmpS2 );
00381                 setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
00382                 setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
00383 
00384                 loopObjective.addFunctionCall(
00385                                 setObjS1,
00386                                 tmpFxCall, tmpFuCall, objSCall,
00387                                 S1.getAddress(runObj * NX, 0)
00388                 );
00389         }
00390 
00391         evaluateObjective.addStatement( loopObjective );
00392 
00393         //
00394         // Evaluate the quadratic Mayer term
00395         //
00396         evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00397         evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00398 
00399         // Evaluate the objective function, last node.
00400         evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00401         evaluateObjective.addLinebreak( );
00402 
00403         evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00404         evaluateObjective.addLinebreak();
00405 
00406         if (QN1.isGiven() == false)
00407         {
00408                 ExportVariable tmpQN1, tmpQN2;
00409                 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00410                 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00411 
00412                 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00413                 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00414                 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00415 
00416                 indexX = getNYN();
00417                 ExportArgument tmpFxEndCall = tmpFxEnd.isGiven() == true ? tmpFxEnd  : objValueOut.getAddress(0, indexX);
00418 
00419                 evaluateObjective.addFunctionCall(
00420                                 setObjQN1QN2,
00421                                 tmpFxEndCall, objSEndTerm,
00422                                 QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00423                 );
00424 
00425                 evaluateObjective.addLinebreak( );
00426         }
00427 
00428         return SUCCESSFUL_RETURN;
00429 }
00430 
00431 returnValue ExportGaussNewtonCN2::setupConstraintsEvaluation( void )
00432 {
00433         ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
00434 
00435         int hardcodeConstraintValues;
00436         get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00437 
00439         //
00440         // Setup the bounds on control variables
00441         //
00443 
00444         unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
00445         unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
00446 
00447         DVector lbValuesMatrix( numBounds );
00448         DVector ubValuesMatrix( numBounds );
00449 
00450         if (initialStateFixed( ) == false)
00451                 for(unsigned run1 = 0; run1 < NX; ++run1)
00452                 {
00453                         lbValuesMatrix( run1 )= xBounds.getLowerBound(0, run1);
00454                         ubValuesMatrix( run1) = xBounds.getUpperBound(0, run1);
00455                 }
00456 
00457         for(unsigned run1 = 0; run1 < N; ++run1)
00458                 for(unsigned run2 = 0; run2 < NU; ++run2)
00459                 {
00460                         lbValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getLowerBound(run1, run2);
00461                         ubValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getUpperBound(run1, run2);
00462                 }
00463 
00464 
00465         if (hardcodeConstraintValues == YES)
00466         {
00467                 lbValues.setup("lbValues", lbValuesMatrix, REAL, ACADO_VARIABLES);
00468                 ubValues.setup("ubValues", ubValuesMatrix, REAL, ACADO_VARIABLES);
00469         }
00470         else if (isFinite( lbValuesMatrix ) || isFinite( ubValuesMatrix ))
00471         {
00472                 lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
00473                 lbValues.setDoc( "Lower bounds values." );
00474                 ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
00475                 ubValues.setDoc( "Upper bounds values." );
00476 
00477                 initialize.addStatement(lbValues == lbValuesMatrix);
00478                 initialize.addStatement(ubValues == ubValuesMatrix);
00479         }
00480 
00481         ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
00482 
00483         if (performFullCondensing() == true)
00484         {
00485                 boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
00486                 boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
00487         }
00488         else
00489         {
00490                 if ( initialStateFixed( ) == true )
00491                 {
00492                         condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
00493                         condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
00494 
00495                         boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
00496                         boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
00497                 }
00498                 else
00499                 {
00500                         boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00501                         boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00502 
00503                         boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00504                         boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00505                 }
00506         }
00507         condensePrep.addLinebreak( );
00508         condenseFdb.addLinebreak( );
00509 
00511         //
00512         // Setup the bounds on state variables
00513         //
00515 
00516         if( getNumStateBounds() )
00517         {
00518                 condenseFdb.addVariable( tmp );
00519 
00520                 DVector xLowerBounds( getNumStateBounds( )), xUpperBounds( getNumStateBounds( ) );
00521                 for(unsigned i = 0; i < xBoundsIdx.size(); ++i)
00522                 {
00523                         xLowerBounds( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
00524                         xUpperBounds( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
00525                 }
00526 
00527                 unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
00528 
00529                 if (numOps < 1024)
00530                 {
00531                         for(unsigned row = 0; row < getNumStateBounds( ); ++row)
00532                         {
00533                                 unsigned conIdx = xBoundsIdx[ row ] - NX;
00534                                 if( initialStateFixed( ) == false ) {
00535                                         condensePrep.addStatement( A.getSubMatrix(row, row + 1, 0, NX ) == C.getRow( conIdx ) );
00536                                 }
00537 
00538                                 unsigned blk = conIdx / NX + 1;
00539                                 for (unsigned col = 0; col < blk; ++col)
00540                                 {
00541                                         unsigned blkRow = (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX;
00542 
00543                                         condensePrep.addStatement(
00544                                                         A.getSubMatrix(row, row + 1, offsetBounds + col * NU, offsetBounds + (col + 1) * NU ) == E.getRow( blkRow ) );
00545                                 }
00546 
00547                                 condensePrep.addLinebreak();
00548                         }
00549                 }
00550                 else
00551                 {
00552                         unsigned nXBounds = getNumStateBounds( );
00553 
00554                         DMatrix vXBoundIndices(1, nXBounds);
00555                         for (unsigned i = 0; i < nXBounds; ++i)
00556                                 vXBoundIndices(0, i) = xBoundsIdx[ i ];
00557                         ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
00558 
00559                         condensePrep.addVariable( evXBounds );
00560 
00561                         ExportIndex row, col, conIdx, blk, blkRow;
00562 
00563                         condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );
00564 
00565                         ExportForLoop lRow(row, 0, nXBounds);
00566 
00567                         lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
00568 
00569                         if( initialStateFixed( ) == false ) {
00570                                 lRow.addStatement( A.getSubMatrix(row, row + 1, 0, NX ) == C.getRow( conIdx ) );
00571                         }
00572 
00573                         lRow.addStatement( blk == conIdx / NX + 1 );
00574 
00575                         ExportForLoop lCol(col, 0, blk);
00576 
00577                         lCol.addStatement( blkRow == (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX );
00578                         lCol.addStatement(
00579                                         A.getSubMatrix(row, row + 1, offsetBounds + col * NU, offsetBounds + (col + 1) * NU ) == E.getRow( blkRow ) );
00580 
00581                         lRow.addStatement( lCol );
00582                         condensePrep.addStatement( lRow );
00583 
00584                         condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
00585                 }
00586                 condensePrep.addLinebreak( );
00587 
00588 
00589                 unsigned nXBounds = getNumStateBounds( );
00590                 if (hardcodeConstraintValues == YES)
00591                 {
00592                         lbAValues.setup("lbAValues", xLowerBounds, REAL, ACADO_VARIABLES);
00593                         ubAValues.setup("ubAValues", xUpperBounds, REAL, ACADO_VARIABLES);
00594                 }
00595                 else
00596                 {
00597                         lbAValues.setup("lbAValues", nXBounds, 1, REAL, ACADO_VARIABLES);
00598                         lbAValues.setDoc( "Lower bounds values for affine constraints." );
00599                         ubAValues.setup("ubAValues", nXBounds, 1, REAL, ACADO_VARIABLES);
00600                         ubAValues.setDoc( "Upper bounds values for affine constraints." );
00601 
00602                         initialize.addStatement(lbAValues == xLowerBounds);
00603                         initialize.addStatement(ubAValues == xUpperBounds);
00604                 }
00605 
00606                 // Shift constraint bounds by first interval
00607                 for(unsigned boundIndex = 0; boundIndex < getNumStateBounds( ); ++boundIndex)
00608                 {
00609                         unsigned row = xBoundsIdx[boundIndex];
00610 
00611                         condenseFdb.addStatement( tmp == sbar.getRow( row ) + x.makeRowVector().getCol( row ) );
00612                         condenseFdb.addStatement( lbA.getRow( boundIndex ) == lbAValues.getRow( boundIndex ) - tmp );
00613                         condenseFdb.addStatement( ubA.getRow( boundIndex ) == ubAValues.getRow( boundIndex ) - tmp );
00614                 }
00615                 condenseFdb.addLinebreak( );
00616         }
00617 
00618         return SUCCESSFUL_RETURN;
00619 }
00620 
00621 returnValue ExportGaussNewtonCN2::setupCondensing( void )
00622 {
00623         condensePrep.setup("condensePrep");
00624         condenseFdb.setup( "condenseFdb" );
00625 
00627         //
00628         // Setup local memory for preparation phase: T1, T2, W1, W2
00629         //
00631 
00632         // TODO
00633 
00635         //
00636         // Compute Hessian block H00, H10 and C
00637         //
00639 
00640         if (performFullCondensing() == false)
00641         {
00642                 LOG( LVL_DEBUG ) << "Setup condensing: H00, H10 and C" << endl;
00643 
00644                 T1.setup("T1", NX, NX, REAL, ACADO_WORKSPACE);
00645                 T2.setup("T2", NX, NX, REAL, ACADO_WORKSPACE);
00646 
00647                 condensePrep.addFunctionCall(moveGxT, evGx.getAddress(0, 0), C.getAddress(0, 0));
00648                 for (unsigned row = 1; row < N; ++row)
00649                         condensePrep.addFunctionCall(
00650                                         multGxGx, evGx.getAddress(row * NX), C.getAddress((row - 1) * NX), C.getAddress(row * NX));
00651 
00652                 /* Algorithm for computation of H10 and H00
00653 
00654                 T1 = Q_N * C_{N - 1}
00655                 for i = N - 1: 1
00656                         H_{i + 1, 0} =  B_i^T * T1
00657                         H_{i + 1, 0} += S_i^T * C_{i - 1}
00658 
00659                         T2 = A_i^T * T1
00660                         T1 = T2 + Q_i * C_{i - 1}
00661 
00662                 H_{1, 0} =  B_0^T * T1
00663                 H_{1, 0} += S_0^T
00664                 H_{0, 0} = Q_0 + A_0^T * T1
00665 
00666                  */
00667 
00668                 // H10 Block
00669                 ExportFunction* QN1Call = QN1.isGiven() == true ? &multQN1Gx : &multGxGx;
00670                 condensePrep.addFunctionCall(*QN1Call, QN1, C.getAddress((N - 1) * NX), T1);
00671                 for (unsigned row = N - 1; row > 0; --row)
00672                 {
00673                         condensePrep.addFunctionCall(mult_BT_T1, evGu.getAddress(row * NX), T1, ExportIndex( row ));
00674 
00675                         if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
00676                         {
00677                                 ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(row * NX) : S1;
00678                                 condensePrep.addFunctionCall(mac_ST_C, S1Call, C.getAddress((row - 1) * NX), ExportIndex( row ));
00679                         }
00680 
00681                         condensePrep.addFunctionCall(multGxTGx, evGx.getAddress(row * NX), T1, T2);
00682 
00683                         ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(row * NX);
00684                         condensePrep.addFunctionCall(macGxTGx, T2, Q1Call, C.getAddress((row - 1) * NX), T1);
00685                 }
00686 
00687                 condensePrep.addFunctionCall(mult_BT_T1, evGu.getAddress( 0 ), T1, ExportIndex( 0 ));
00688                 if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
00689                 {
00690                         condensePrep.addStatement(
00691                                         H.getSubMatrix(NX, NX + NU, 0, NX) += S1.getSubMatrix(0, NX, 0, NU).getTranspose()
00692                         );
00693                 }
00694 
00695                 // H00 Block
00696                 DMatrix mRegH00 = eye<double>( getNX() );
00697                 mRegH00 *= levenbergMarquardt;
00698                 condensePrep.addStatement(
00699                                 H.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(0, NX, 0, NX) + (evGx.getSubMatrix(0, NX, 0, NX).getTranspose() * T1)
00700                 );
00701                 condensePrep.addStatement( H.getSubMatrix(0, NX, 0, NX) += mRegH00 );
00702         }
00703 
00705         //
00706         // Compute Hessian block H11
00707         //
00709 
00710         LOG( LVL_DEBUG ) << "Setup condensing: H11 and E" << endl;
00711 
00712         /*
00713 
00714         This one is only for the case where we have input constraints only
00715 
00716         // Create E and H
00717 
00718         for i = 0: N - 1
00719         {
00720                 // Storage for E: (N x nx) x nu
00721 
00722                 E_0 = B_i;
00723                 for k = 1: N - i - 1
00724                         E_k = A_{i + k} * E_{k - 1};
00725 
00726                 // Two temporary matrices W1, W2 of size nx x nu
00727 
00728                 W1 = Q_N^T * E_{N - i - 1};
00729 
00730                 for k = N - 1: i + 1
00731                 {
00732                         H_{k, i} = B_k^T * W1;
00733 
00734                         W2 = A_k^T * W1;
00735                         W1 = Q_k^T * E_{k - i - 1} + W2;
00736                 }
00737                 H_{i, i} = B_i^T * W1 + R_i^T
00738         }
00739 
00740         Else, in general case:
00741 
00742         for i = 0: N - 1
00743         {
00744                 // Storage for E: (N * (N + 1) / 2 x nx) x nu
00745 
00746                 j = 1 / 2 * i * (2 * N - i + 1);
00747 
00748                 E_j = B_i;
00749                 for k = 1: N - i - 1
00750                         E_{j + k} = A_{i + k} * E_{j + k - 1};
00751 
00752                 // Two temporary matrices W1, W2 of size nx x nu
00753 
00754                 W1 = Q_N^T * E_{j + N - i - 1};
00755 
00756                 for k = N - 1: i + 1
00757                 {
00758                         H_{k, i}  = B_k^T * W1;
00759                         H_{k, i} += S_k^T * E_{j + k - i - 1};
00760 
00761                         W2 = A_k^T * W1;
00762                         W1 = Q_k^T * E_{j + k - i - 1} + W2;
00763                 }
00764                 H_{i, i} = B_i^T * W1 + R_i^T
00765         }
00766 
00767          */
00768 
00769         W1.setup("W1", NX, NU, REAL, ACADO_WORKSPACE);
00770         W2.setup("W2", NX, NU, REAL, ACADO_WORKSPACE);
00771 
00772         if (N <= 15)
00773         {
00774                 for (unsigned col = 0; col < N; ++col)
00775                 {
00776                         int offset = col * (2 * N - col + 1) / 2;
00777 
00778                         condensePrep.addComment( "Column: " + toString( col ) );
00779                         condensePrep.addFunctionCall(
00780                                         moveGuE, evGu.getAddress(col * NX), E.getAddress(offset * NX)
00781                         );
00782                         for (unsigned row = 1; row < N - col; ++row)
00783                                 condensePrep.addFunctionCall(
00784                                                 multGxGu,
00785                                                 evGx.getAddress((col + row) * NX),
00786                                                 E.getAddress((offset + row - 1) * NX), E.getAddress((offset + row) * NX)
00787                                 );
00788                         condensePrep.addLinebreak();
00789 
00790                         ExportFunction* QN1Call = QN1.isGiven() == true ? &multQN1Gu : &multGxGu;
00791                         condensePrep.addFunctionCall(
00792                                         *QN1Call, QN1, E.getAddress((offset + N - col - 1) * NX), W1
00793                         );
00794 
00795                         for (unsigned row = N - 1; col < row; --row)
00796                         {
00797                                 condensePrep.addFunctionCall(
00798                                                 multBTW1, evGu.getAddress(row * NX), W1,
00799                                                 ExportIndex( row ), ExportIndex( col )
00800                                 );
00801 
00802                                 if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
00803                                 {
00804                                         ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(row * NX) : S1;
00805 
00806                                         condensePrep.addFunctionCall(
00807                                                         mac_S1T_E,
00808                                                         S1Call, E.getAddress((offset + row - col - 1) * NX),
00809                                                         ExportIndex( row ), ExportIndex( col )
00810                                         );
00811                                 }
00812 
00813                                 condensePrep.addFunctionCall(
00814                                                 multGxTGu, evGx.getAddress(row * NX), W1, W2
00815                                 );
00816 
00817                                 ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(row * NX);
00818                                 condensePrep.addFunctionCall(
00819                                                 macQEW2, Q1Call, E.getAddress((offset + row - col - 1) * NX), W2, W1
00820                                 );
00821                         }
00822 
00823                         ExportArgument R1Call = R1.isGiven() == true ? R1 : R1.getAddress(col * NU);
00824                         condensePrep.addFunctionCall(
00825                                         macBTW1_R1, R1Call, evGu.getAddress(col * NX), W1,
00826                                         ExportIndex( col )
00827                         );
00828 
00829                         condensePrep.addLinebreak();
00830                 }
00831         }
00832         else
00833         {
00834                 // Long horizons
00835 
00836                 ExportIndex row, col, offset;
00837                 condensePrep.acquire( row ).acquire( col ).acquire( offset );
00838 
00839                 ExportForLoop cLoop(col, 0, N);
00840                 ExportForLoop fwdLoop(row, 1, N - col);
00841                 ExportForLoop adjLoop(row, N - 1, col, -1);
00842 
00843                 cLoop.addStatement( offset == col * (2 * N + 1 - col) / 2 );
00844                 cLoop.addFunctionCall(
00845                                 moveGuE, evGu.getAddress(col * NX), E.getAddress(offset * NX)
00846                 );
00847 
00848                 fwdLoop.addFunctionCall(
00849                                 multGxGu,
00850                                 evGx.getAddress((col + row) * NX),
00851                                 E.getAddress((offset + row - 1) * NX), E.getAddress((offset + row) * NX)
00852                 );
00853                 cLoop.addStatement( fwdLoop );
00854                 cLoop.addLinebreak();
00855 
00856                 ExportFunction* QN1Call = QN1.isGiven() == true ? &multQN1Gu : &multGxGu;
00857                 cLoop.addFunctionCall(
00858                                 *QN1Call, QN1, E.getAddress((offset - col + N - 1) * NX), W1
00859                 );
00860 
00861                 adjLoop.addFunctionCall(
00862                                 multBTW1, evGu.getAddress(row * NX), W1,
00863                                 row, col
00864                 );
00865 
00866                 if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
00867                 {
00868                         ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(row * NX) : S1;
00869 
00870                         adjLoop.addFunctionCall(
00871                                         mac_S1T_E,
00872                                         S1Call, E.getAddress((offset + row - col - 1) * NX),
00873                                         row, col
00874                         );
00875                 }
00876 
00877                 adjLoop.addFunctionCall(
00878                                 multGxTGu, evGx.getAddress(row * NX), W1, W2
00879                 );
00880 
00881                 ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(row * NX);
00882                 adjLoop.addFunctionCall(
00883                                 macQEW2, Q1Call, E.getAddress((offset + row - col - 1) * NX), W2, W1
00884                 );
00885 
00886                 cLoop.addStatement( adjLoop );
00887 
00888                 ExportArgument R1Call = R1.isGiven() == true ? R1 : R1.getAddress(col * NU);
00889                 cLoop.addFunctionCall(
00890                                 macBTW1_R1, R1Call, evGu.getAddress(col * NX), W1,
00891                                 ExportIndex( col )
00892                 );
00893 
00894                 condensePrep.addStatement( cLoop );
00895                 condensePrep.addLinebreak();
00896 
00897                 condensePrep.release( row ).release( col ).release( offset );
00898         }
00899 
00901 
00902         LOG( LVL_DEBUG ) << "---> Copy H11 lower part" << endl;
00903 
00904         // Copy to H11 upper lower part to upper triangular part
00905         if (N <= 20)
00906         {
00907                 for (unsigned ii = 0; ii < N; ++ii)
00908                         for(unsigned jj = 0; jj < ii; ++jj)
00909                                 condensePrep.addFunctionCall(
00910                                                 copyHTH, ExportIndex( jj ), ExportIndex( ii ));
00911 
00912                 // Copy H10 to H01
00913                 if( performFullCondensing() == false) {
00914                         for (unsigned ii = 0; ii < N; ++ii)
00915                                 condensePrep.addFunctionCall( copyHTH1, ExportIndex( ii ) );
00916                 }
00917         }
00918         else
00919         {
00920                 ExportIndex ii, jj;
00921 
00922                 condensePrep.acquire( ii );
00923                 condensePrep.acquire( jj );
00924 
00925                 ExportForLoop eLoopI(ii, 0, N);
00926                 ExportForLoop eLoopJ(jj, 0, ii);
00927 
00928                 eLoopJ.addFunctionCall(copyHTH, jj, ii);
00929                 eLoopI.addStatement( eLoopJ );
00930                 condensePrep.addStatement( eLoopI );
00931 
00932                 // Copy H10 to H01
00933                 if( performFullCondensing() == false) {
00934                         ExportForLoop eLoopK(ii, 0, N);
00935                         eLoopK.addFunctionCall(copyHTH1, ii);
00936                         condensePrep.addStatement( eLoopK );
00937                 }
00938 
00939                 condensePrep.release( ii );
00940                 condensePrep.release( jj );
00941         }
00942         condensePrep.addLinebreak();
00943 
00945         //
00946         // Compute gradient components g0 and g1
00947         //
00949 
00950         LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
00951 
00952         unsigned offset = performFullCondensing() == true ? 0 : NX;
00953 
00954         if (initialStateFixed() == true)
00955         {
00956                 condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
00957         }
00958         condenseFdb << (Dy -= y) << (DyN -= yN);
00959         condenseFdb.addLinebreak();
00960 
00961         int variableObjS;
00962         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00963 
00964         // Compute RDy
00965         if( getNY() > 0 || getNYN() ) {
00966         for(unsigned run1 = 0; run1 < N; ++run1)
00967         {
00968                 ExportArgument R2Call = R2.isGiven() == true ? R2 : R2.getAddress(run1 * NU, 0);
00969                 ExportArgument SluCall =
00970                                 objSlu.isGiven() == true || variableObjS == false ? objSlu : objSlu.getAddress(run1 * NU, 0);
00971                 condenseFdb.addFunctionCall(
00972                                 multRDy, R2Call, Dy.getAddress(run1 * NY, 0), SluCall, g.getAddress(offset + run1 * NU, 0)
00973                 );
00974         }
00975         condenseFdb.addLinebreak();
00976 
00977         // Compute QDy
00978         for(unsigned run1 = 0; run1 < N; run1++ )
00979         {
00980                 ExportArgument Q2Call = Q2.isGiven() == true ? Q2 : Q2.getAddress(run1 * NX);
00981                 ExportArgument SlxCall =
00982                                 objSlx.isGiven() == true || variableObjS == false ? objSlx : objSlx.getAddress(run1 * NX, 0);
00983                 condenseFdb.addFunctionCall(
00984                                 multQDy, Q2Call, Dy.getAddress(run1 * NY), SlxCall, QDy.getAddress(run1 * NX) );
00985         }
00986         condenseFdb.addLinebreak();
00987         ExportVariable SlxCall =
00988                                 objSlx.isGiven() == true || variableObjS == false ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX);
00989         condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == SlxCall + QN2 * DyN );
00990         condenseFdb.addLinebreak();
00991         }
00992 
00993 
00994         if (performFullCondensing() == false)
00995                 condenseFdb.addStatement(g.getRows(0, NX) == QDy.getRows(0, NX));
00996 
00997         /*
00998         if partial condensing:
00999                 g0 = q_0
01000 
01001         for k = 0: N - 1
01002                 g1_k = r_k
01003 
01004         if partial condensing:
01005                 sbar_0 = 0;
01006         else:
01007                 sbar_0 = Dx_0;
01008 
01009         sbar(1: N) = d;
01010 
01011         for k = 0: N - 1
01012                 sbar_{k + 1} += A_k sbar_k;
01013 
01014         w1 = Q_N^T * sbar_N + q_N;
01015         for k = N - 1: 1
01016         {
01017                 g1_k += B_k^T * w1;
01018                 g1_k += S_k^T * sbar_k;
01019                 w2 = A_k^T * w1 + q_k;
01020                 w1 = Q_k^T * sbar_k + w2;
01021         }
01022 
01023         g1_0 += B_0^T * w1;
01024         if partial condensing:
01025                 g_0 += A_0^T * w1
01026         else:
01027                 g1_0 += S^0^T * x0;
01028 
01029         */
01030 
01031         w1.setup("w1", NX, 1, REAL, ACADO_WORKSPACE);
01032         w2.setup("w2", NX, 1, REAL, ACADO_WORKSPACE);
01033         sbar.setup("sbar", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
01034 
01035         if( performFullCondensing() == true ) {
01036                 condenseFdb.addStatement( sbar.getRows(0, NX) == Dx0 );
01037         }
01038         else {
01039                 condenseFdb.addStatement( sbar.getRows(0, NX) == zeros<double>(NX,1) );  // Dx0 is now a variable as well !!
01040         }
01041         condensePrep.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
01042 
01043         for (unsigned i = 0; i < N; ++i)
01044                 condenseFdb.addFunctionCall(
01045                                 macASbar, evGx.getAddress(i * NX), sbar.getAddress(i * NX), sbar.getAddress((i + 1) * NX)
01046                 );
01047         condenseFdb.addLinebreak();
01048 
01049         condenseFdb.addStatement(
01050                         w1 == QDy.getRows(N * NX, (N + 1) * NX) + QN1 * sbar.getRows(N * NX, (N + 1) * NX)
01051         );
01052         for (unsigned i = N - 1; 0 < i; --i)
01053         {
01054                 condenseFdb.addFunctionCall(
01055                                 macBTw1, evGu.getAddress(i * NX), w1, g.getAddress(offset + i * NU)
01056                 );
01057 
01058                 if ((S1.isGiven() == true && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
01059                 {
01060                         ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(i * NX) : S1;
01061                         condenseFdb.addFunctionCall(macS1TSbar, S1Call, sbar.getAddress(i * NX), g.getAddress(offset + i * NU));
01062                 }
01063 
01064                 condenseFdb.addFunctionCall(
01065                                 // TODO Check indexing for QDy
01066                                 macATw1QDy, evGx.getAddress(i * NX), w1, QDy.getAddress(i * NX), w2
01067                 );
01068 
01069                 ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(i * NX);
01070                 condenseFdb.addFunctionCall(
01071                                 macQSbarW2, Q1Call, sbar.getAddress(i * NX), w2, w1);
01072         }
01073         condenseFdb.addFunctionCall(
01074                         macBTw1, evGu.getAddress( 0 ), w1, g.getAddress( offset + 0 )
01075         );
01076         if( performFullCondensing() == true ) {
01077                 if ((S1.isGiven() == true && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
01078                 {
01079                         condenseFdb.addFunctionCall(macS1TSbar, S1, sbar.getAddress(0), g);
01080                 }
01081         }
01082         else {
01083                 condenseFdb.addStatement(g.getRows(0, NX) += evGx.getRows(0, NX).getTranspose() * w1);
01084         }
01085         condenseFdb.addLinebreak();
01086 
01087 
01089         //
01090         // Expansion routine
01091         //
01093 
01094         /*
01095 
01096         // Step expansion, assuming that u_k is already updated
01097 
01098         Ds_0 = Dx_0;
01099         Ds_{1: N} = d;
01100 
01101         for i = 0: N - 1
01102         {
01103                 Ds_{k + 1} += A_k * Ds_k;       // Reuse multABarD
01104                 Ds_{k + 1} += B_k * Du_k;
01105                 s_{k + 1}  += Ds_{k + 1};
01106         }
01107 
01108          */
01109 
01110         LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
01111 
01112         expand.setup( "expand" );
01113 
01114         if (performFullCondensing() == true)
01115         {
01116                 expand.addStatement( u.makeRowVector() += xVars.getTranspose() );
01117         }
01118         else
01119         {
01120                 expand.addStatement( u.makeColVector() += xVars.getRows(NX, getNumQPvars()) );
01121         }
01122 
01123         if( performFullCondensing() == true ) {
01124                 expand.addStatement( sbar.getRows(0, NX) == Dx0 );
01125         }
01126         else {
01127                 expand.addStatement( sbar.getRows(0, NX) == xVars.getRows(0, NX) );
01128         }
01129         expand.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
01130 
01131         for (unsigned row = 0; row < N; ++row )
01132                 expand.addFunctionCall(
01133                                 expansionStep, evGx.getAddress(row * NX), evGu.getAddress(row * NX),
01134                                 xVars.getAddress(offset + row * NU), sbar.getAddress(row * NX),
01135                                 sbar.getAddress((row + 1) * NX)
01136                 );
01137 
01138         expand.addStatement( x.makeColVector() += sbar );
01139 
01140         // !! Calculation of multipliers: !!
01141         int hessianApproximation;
01142         get( HESSIAN_APPROXIMATION, hessianApproximation );
01143         bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
01144         if( secondOrder ) {
01145                 //      mu_N = lambda_N + q_N + Q_N^T * Ds_N  --> wrong in Joel's paper !!
01146                 //              for i = N - 1: 1
01147                 //                      mu_k = Q_k^T * Ds_k + A_k^T * mu_{k + 1} + S_k * Du_k + q_k
01148 
01149                 for (uint j = 0; j < NX; j++ ) {
01150                         uint item = N*NX+j;
01151                         uint IdxF = std::find(xBoundsIdx.begin(), xBoundsIdx.end(), item) - xBoundsIdx.begin();
01152                         if( IdxF != xBoundsIdx.size() ) { // INDEX FOUND
01153                                 expand.addStatement( mu.getSubMatrix(N-1,N,j,j+1) == yVars.getRow(getNumQPvars()+IdxF) );
01154                         }
01155                         else { // INDEX NOT FOUND
01156                                 expand.addStatement( mu.getSubMatrix(N-1,N,j,j+1) == 0.0 );
01157                         }
01158                 }
01159                 expand.addStatement( mu.getRow(N-1) += sbar.getRows(N*NX,(N+1)*NX).getTranspose()*QN1 );
01160                 expand.addStatement( mu.getRow(N-1) += QDy.getRows(N*NX,(N+1)*NX).getTranspose() );
01161                 for (int i = N - 1; i >= 1; i--) {
01162                         for (uint j = 0; j < NX; j++ ) {
01163                                 uint item = i*NX+j;
01164                                 uint IdxF = std::find(xBoundsIdx.begin(), xBoundsIdx.end(), item) - xBoundsIdx.begin();
01165                                 if( IdxF != xBoundsIdx.size() ) { // INDEX FOUND
01166                                         expand.addStatement( mu.getSubMatrix(i-1,i,j,j+1) == yVars.getRow(getNumQPvars()+IdxF) );
01167                                 }
01168                                 else { // INDEX NOT FOUND
01169                                         expand.addStatement( mu.getSubMatrix(i-1,i,j,j+1) == 0.0 );
01170                                 }
01171                         }
01172                         expand.addFunctionCall(
01173                                         expansionStep2, QDy.getAddress(i*NX), Q1.getAddress(i * NX), sbar.getAddress(i*NX),
01174                                         S1.getAddress(i * NX), xVars.getAddress(offset + i * NU), evGx.getAddress(i * NX),
01175                                         mu.getAddress(i-1), mu.getAddress(i) );
01176                 }
01177         }
01178 
01179         return SUCCESSFUL_RETURN;
01180 }
01181 
01182 returnValue ExportGaussNewtonCN2::setupVariables( )
01183 {
01185         //
01186         // Make index vector for state constraints
01187         //
01189 
01190         bool boxConIsFinite = false;
01191         xBoundsIdx.clear();
01192 
01193         DVector lbBox, ubBox;
01194         for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
01195         {
01196                 lbBox = xBounds.getLowerBounds( i );
01197                 ubBox = xBounds.getUpperBounds( i );
01198 
01199                 if (isFinite( lbBox ) || isFinite( ubBox ))
01200                         boxConIsFinite = true;
01201 
01202                 // This is maybe not necessary
01203                 if (boxConIsFinite == false || i == 0)
01204                         continue;
01205 
01206                 for (unsigned j = 0; j < lbBox.getDim(); ++j)
01207                 {
01208                         if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
01209                         {
01210                                 xBoundsIdx.push_back(i * lbBox.getDim() + j);
01211                         }
01212                 }
01213         }
01214 
01215         if (initialStateFixed() == true)
01216         {
01217                 x0.setup("x0",  NX, 1, REAL, ACADO_VARIABLES);
01218                 x0.setDoc( (std::string)"Current state feedback vector." );
01219                 Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
01220         }
01221 
01222         if (performFullCondensing() == false)
01223         {
01224                 C.setup("C", N*NX, NX, REAL, ACADO_WORKSPACE);
01225         }
01226         E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
01227 
01228         QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
01229 
01230         H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01231         g.setup("g",  getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01232         A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01233 
01234         lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01235         ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01236 
01237         lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
01238         ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
01239 
01240         xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01241         yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
01242 
01243         return SUCCESSFUL_RETURN;
01244 }
01245 
01246 returnValue ExportGaussNewtonCN2::setupMultiplicationRoutines( )
01247 {
01248         ExportIndex iCol( "iCol" );
01249         ExportIndex iRow( "iRow" );
01250 
01251         ExportVariable dp, dn, Gx1, Gx2, Gx3, Gx4, Gu1, Gu2, Gu3;
01252         ExportVariable R22, Dy1, RDy1, Q22, QDy1, E1, U1, U2, H101, w11, w12, w13;
01253         dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
01254         dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
01255         Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
01256         Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
01257         Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
01258         Gx4.setup("Gx4", NX, NX, REAL, ACADO_LOCAL);
01259         Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
01260         Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
01261         Gu3.setup("Gu3", NX, NU, REAL, ACADO_LOCAL);
01262         R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
01263         Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
01264         RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
01265         Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
01266         QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
01267         E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
01268         U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
01269         U2.setup("U2", NU, 1, REAL, ACADO_LOCAL);
01270         H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
01271 
01272         w11.setup("w11", NX, 1, REAL, ACADO_LOCAL);
01273         w12.setup("w12", NX, 1, REAL, ACADO_LOCAL);
01274         w13.setup("w13", NX, 1, REAL, ACADO_LOCAL);
01275 
01276         if ( Q2.isGiven() )
01277                 Q22 = Q2;
01278         if ( R2.isGiven() )
01279                 R22 = R2;
01280 
01281         // multGxd; // d_k += Gx_k * d_{k-1}
01282         multGxd.setup("multGxd", dp, Gx1, dn);
01283         multGxd.addStatement( dn += Gx1 * dp );
01284 
01285         if( performFullCondensing() == false ) {
01286                 // moveGxT
01287                 moveGxT.setup("moveGxT", Gx1, Gx2);
01288                 moveGxT.addStatement( Gx2 == Gx1 );
01289                 // multGxGx
01290                 multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
01291                 multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
01292         }
01293 
01294         // multGxGu
01295         multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
01296         multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
01297         // moveGuE
01298         moveGuE.setup("moveGuE", Gu1, Gu2);
01299         moveGuE.addStatement( Gu2 == Gu1 );
01300 
01301         unsigned offset = (performFullCondensing() == true) ? 0 : NX;
01302 
01303         // copyHTH
01304         copyHTH.setup("copyHTH", iRow, iCol);
01305         copyHTH.addStatement(
01306                         H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
01307                                         H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
01308         );
01309 
01310         if( performFullCondensing() == false ) {
01311                 // copyHTH1
01312                 copyHTH1.setup("copyHTH1", iCol);
01313                 copyHTH1.addStatement(
01314                                 H.getSubMatrix(0, NX, offset + iCol * NU, offset + (iCol + 1) * NU) ==
01315                                                 H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, 0, NX).getTranspose()
01316                 );
01317         }
01318         // multRDy
01319         ExportVariable SluCall = objSlu.isGiven() == true ? objSlu : ExportVariable("Slu", NU, 1, REAL, ACADO_LOCAL);
01320         multRDy.setup("multRDy", R22, Dy1, SluCall, RDy1);
01321         multRDy.addStatement( RDy1 ==  R22 * Dy1 );
01322         multRDy.addStatement( RDy1 +=  SluCall );
01323 
01324         // mult QDy1
01325         ExportVariable SlxCall = objSlx.isGiven() == true ? objSlx : ExportVariable("Slx", NX, 1, REAL, ACADO_LOCAL);
01326         multQDy.setup("multQDy", Q22, Dy1, SlxCall, QDy1);
01327         multQDy.addStatement( QDy1 == Q22 * Dy1 );
01328         multQDy.addStatement( QDy1 += SlxCall );
01329         // multEQDy;
01330         multEQDy.setup("multEQDy", E1, QDy1, U1);
01331         multEQDy.addStatement( U1 += (E1 ^ QDy1) );
01332         // multQETGx
01333         multQETGx.setup("multQETGx", E1, Gx1, H101);
01334         multQETGx.addStatement( H101 += (E1 ^ Gx1) );
01335 
01336         if (performsSingleShooting() == false)
01337         {
01338                 // multEDu
01339                 multEDu.setup("multEDu", E1, U1, dn);
01340                 multEDu.addStatement( dn += E1 * U1 );
01341         }
01342 
01343         if (Q1.isGiven() == true)
01344         {
01345                 // multQ1Gx
01346                 multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
01347                 multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
01348 
01349                 // multQ1Gu
01350                 multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
01351                 multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
01352 
01353                 // multQ1d
01354                 multQ1d.setup("multQ1d", Q1, dp, dn);
01355                 multQ1d.addStatement( dn == Q1 * dp );
01356         }
01357         else
01358         {
01359                 // multQ1d
01360                 multQ1d.setup("multQ1d", Gx1, dp, dn);
01361                 multQ1d.addStatement( dn == Gx1 * dp );
01362         }
01363 
01364         if (QN1.isGiven() == true)
01365         {
01366                 // multQN1Gx
01367                 multQN1Gx.setup("multQN1Gx", QN1, Gx1, Gx2);
01368                 multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
01369 
01370                 // multQN1Gu
01371                 multQN1Gu.setup("multQN1Gu", QN1, Gu1, Gu2);
01372                 multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
01373 
01374                 // multQN1d
01375                 multQN1d.setup("multQN1d", QN1, dp, dn);
01376                 multQN1d.addStatement( dn == QN1 * dp );
01377         }
01378 
01379         //
01380         // N2 condensing related
01381         //
01382 
01383         multBTW1.setup("multBTW1", Gu1, Gu2, iRow, iCol);
01384         multBTW1.addStatement(
01385                         H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
01386                                         (Gu1 ^ Gu2)
01387         );
01388 
01389         multGxTGu.setup("multGxTGu", Gx1, Gu1, Gu2);
01390         multGxTGu.addStatement( Gu2 == (Gx1 ^ Gu1) );
01391 
01392         ExportVariable Q11 = Q1.isGiven() ? Q1 : ExportVariable("Q11", NX, NX, REAL, ACADO_LOCAL);
01393 
01394         macQEW2.setup("multQEW2", Q11, Gu1, Gu2, Gu3);
01395         macQEW2.addStatement( Gu3 == Gu2 + Q11 * Gu1 );
01396 
01397         macASbar.setup("macASbar", Gx1, w11, w12);
01398         macASbar.addStatement( w12 += Gx1 * w11 );
01399 
01400         macBTw1.setup("macBTw1", Gu1, w11,      U1);
01401         macBTw1.addStatement( U1 += Gu1 ^ w11 );
01402 
01403         macATw1QDy.setup("macATw1QDy", Gx1, w11, w12, w13);
01404         macATw1QDy.addStatement( w13 == w12 + (Gx1 ^ w11) );
01405 
01406         macQSbarW2.setup("macQSbarW2", Q11, w11, w12, w13);
01407         macQSbarW2.addStatement( w13 == w12 + Q11 * w11 );
01408 
01409         expansionStep.setup("expansionStep", Gx1, Gu1, U1, w11, w12);
01410         expansionStep.addStatement( w12 += Gx1 * w11 );
01411         expansionStep.addStatement( w12 += Gu1 * U1 );
01412 
01413         //
01414         // Define LM regularization terms
01415         //
01416         DMatrix mRegH11 = eye<double>( getNU() );
01417         mRegH11 *= levenbergMarquardt;
01418 
01419         ExportVariable R11 = R1.isGiven() == true ? R1 : ExportVariable("R11", NU, NU, REAL, ACADO_LOCAL);
01420 
01421         macBTW1_R1.setup("multBTW1_R1", R11, Gu1, Gu2, iRow);
01422         macBTW1_R1.addStatement(
01423                         H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU) ==
01424                                         R11 + (Gu1 ^ Gu2)
01425         );
01426         macBTW1_R1.addStatement(
01427                         H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU) += mRegH11
01428         );
01429 
01430         ExportVariable S11 = zeros<double>(NX, NU);
01431         if (S1.isGiven() == true && S1.getGivenMatrix().isZero() == false)
01432                 S11 = S1;
01433         else if (S1.isGiven() == false)
01434                 S11 = Gu1;
01435 
01436         if ((S11.isGiven() == true && S11.getGivenMatrix().isZero() == false) || S11.isGiven() == false)
01437         {
01438                 mac_S1T_E.setup("mac_S1T_E", S11, Gu2, iRow, iCol);
01439                 mac_S1T_E.addStatement(
01440                                 H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) +=
01441                                                 (S11 ^ Gu2)
01442                 );
01443 
01444                 macS1TSbar.setup("macS1TSbar", S11, w11, U1);
01445                 macS1TSbar.addStatement( U1 += (S11 ^ w11) );
01446         }
01447 
01448         int hessianApproximation;
01449         get( HESSIAN_APPROXIMATION, hessianApproximation );
01450         bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
01451         if( secondOrder ) {
01452                 ExportVariable mu1; mu1.setup("mu1", 1, NX, REAL, ACADO_LOCAL);
01453                 ExportVariable mu2; mu2.setup("mu2", 1, NX, REAL, ACADO_LOCAL);
01454 
01455                 expansionStep2.setup("expansionStep2", QDy1, Q11, w11, S11, U1, Gx1, mu1, mu2);
01456                 expansionStep2.addStatement( mu1 += QDy1.getTranspose() );
01457                 expansionStep2.addStatement( mu1 += w11 ^ Q11.getTranspose() );
01458                 expansionStep2.addStatement( mu1 += U1 ^ S11.getTranspose() );
01459                 expansionStep2.addStatement( mu1 += mu2*Gx1 );
01460         }
01461 
01462         if (performFullCondensing() == false)
01463         {
01464                 // ExportFunction mult_BT_T1, mac_ST_C, multGxTGx, macGxTGx;
01465 
01466                 mult_BT_T1.setup("mult_BT_T1", Gu1, Gx1, iRow);
01467                 mult_BT_T1.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, 0, NX) == (Gu1 ^ Gx1) );
01468 
01469                 if ((S11.isGiven() == true && S11.getGivenMatrix().isZero() == false) || S11.isGiven() == false)
01470                 {
01471                         mac_ST_C.setup("mac_ST_C", S11, Gx1, iRow);
01472                         mac_ST_C.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, 0, NX) += (S11 ^ Gx1) );
01473                 }
01474 
01475                 multGxTGx.setup("multGxTGx", Gx1, Gx2, Gx3);
01476                 multGxTGx.addStatement( Gx3 == (Gx1 ^ Gx2) );
01477 
01478                 macGxTGx.setup("macGxTGx", Gx1, Gx2, Gx3, Gx4);
01479                 macGxTGx.addStatement( Gx4 == Gx1 + (Gx2 * Gx3) );
01480         }
01481 
01482         return SUCCESSFUL_RETURN;
01483 }
01484 
01485 returnValue ExportGaussNewtonCN2::setupEvaluation( )
01486 {
01488         //
01489         // Preparation phase
01490         //
01492 
01493         preparation.setup( "preparationStep" );
01494         preparation.doc( "Preparation step of the RTI scheme." );
01495         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
01496         retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
01497         preparation.setReturnValue(retSim, false);
01498 
01499         preparation     << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
01500 
01501         preparation.addFunctionCall( evaluateObjective );
01502         if( regularizeHessian.isDefined() ) preparation.addFunctionCall( regularizeHessian );
01503         preparation.addFunctionCall( condensePrep );
01504 
01506         //
01507         // Feedback phase
01508         //
01510 
01511         ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
01512         tmp.setDoc( "Status code of the qpOASES QP solver." );
01513 
01514         ExportFunction solve("solve");
01515         solve.setReturnValue( tmp );
01516 
01517         feedback.setup("feedbackStep");
01518         feedback.doc( "Feedback/estimation step of the RTI scheme." );
01519         feedback.setReturnValue( tmp );
01520 
01521         feedback.addFunctionCall( condenseFdb );
01522         feedback.addLinebreak();
01523 
01524         stringstream s;
01525         s << tmp.getName() << " = " << solve.getName() << "( );" << endl;
01526         feedback <<  s.str();
01527         feedback.addLinebreak();
01528 
01529         feedback.addFunctionCall( expand );
01530 
01532         //
01533         // Setup evaluation of the KKT tolerance
01534         //
01536 
01537         ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
01538         ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
01539         ExportIndex index( "index" );
01540 
01541         getKKT.setup( "getKKT" );
01542         getKKT.doc( "Get the KKT tolerance of the current iterate." );
01543         kkt.setDoc( "The KKT tolerance value." );
01544         getKKT.setReturnValue( kkt );
01545         getKKT.addVariable( prd );
01546         getKKT.addIndex( index );
01547 
01548         // ACC = |\nabla F^T * xVars|
01549         getKKT.addStatement( kkt == (g ^ xVars) );
01550         getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
01551 
01552         ExportForLoop bLoop(index, 0, getNumQPvars());
01553 
01554         bLoop.addStatement( prd == yVars.getRow( index ) );
01555         bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
01556         bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
01557         bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
01558         bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
01559         getKKT.addStatement( bLoop );
01560 
01561         if ((getNumStateBounds() + getNumComplexConstraints())> 0)
01562         {
01563                 ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
01564 
01565                 cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ));
01566                 cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
01567                 cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
01568                 cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
01569                 cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
01570 
01571                 getKKT.addStatement( cLoop );
01572         }
01573 
01574         return SUCCESSFUL_RETURN;
01575 }
01576 
01577 returnValue ExportGaussNewtonCN2::setupQPInterface( )
01578 {
01579         string folderName;
01580         get(CG_EXPORT_FOLDER_NAME, folderName);
01581         string moduleName;
01582         get(CG_MODULE_NAME, moduleName);
01583         std::string sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
01584         std::string headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
01585 
01586         int useSinglePrecision;
01587         get(USE_SINGLE_PRECISION, useSinglePrecision);
01588 
01589         int hotstartQP;
01590         get(HOTSTART_QP, hotstartQP);
01591 
01592         int covCalc;
01593         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
01594 
01595         int maxNumQPiterations;
01596         get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
01597 
01598         //
01599         // Set up export of the source file
01600         //
01601 
01602         ExportQpOasesInterface qpInterface = ExportQpOasesInterface(headerFile, sourceFile, "");
01603 
01604         qpInterface.configure(
01605                         "",
01606                         "QPOASES_HEADER",
01607                         getNumQPvars(),
01608                         getNumStateBounds() + getNumComplexConstraints(),
01609                         maxNumQPiterations,
01610                         "PL_NONE",
01611                         useSinglePrecision,
01612 
01613                         commonHeaderName,
01614                         "",
01615                         xVars.getFullName(),
01616                         yVars.getFullName(),
01617                         "", // TODO
01618 //                      sigma.getFullName(),
01619                         hotstartQP,
01620                         true,
01621                         H.getFullName(),
01622                         string(),
01623                         g.getFullName(),
01624                         A.getFullName(),
01625                         lb.getFullName(),
01626                         ub.getFullName(),
01627                         lbA.getFullName(),
01628                         ubA.getFullName()
01629         );
01630 
01631         return qpInterface.exportCode();
01632 }
01633 
01634 bool ExportGaussNewtonCN2::performFullCondensing() const
01635 {
01636         int sparseQPsolution;
01637         get(SPARSE_QP_SOLUTION, sparseQPsolution);
01638 
01639         if ((SparseQPsolutionMethods)sparseQPsolution == CONDENSING || (SparseQPsolutionMethods)sparseQPsolution == CONDENSING_N2)
01640                 return false;
01641 
01642         return true;
01643 }
01644 
01645 CLOSE_NAMESPACE_ACADO


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