export_gauss_newton_cn2_factorization.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_factorization.hpp>
00033 #include <acado/code_generation/export_qpoases_interface.hpp>
00034 
00035 using namespace std;
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 ExportGaussNewtonCn2Factorization::ExportGaussNewtonCn2Factorization(   UserInteraction* _userInteraction,
00040                                                                                                                                                 const std::string& _commonHeaderName
00041                                                                                                                                                 ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00042 {}
00043 
00044 returnValue ExportGaussNewtonCn2Factorization::setup( )
00045 {
00046         if (performFullCondensing() == false || initialStateFixed() == false || getNumComplexConstraints() > 0)
00047                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00048         if (performsSingleShooting() == true)
00049                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00050 
00051         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00052         setupInitialization();
00053         LOG( LVL_DEBUG ) << "done!" << endl;
00054 
00055         LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
00056         setupVariables();
00057         LOG( LVL_DEBUG ) << "done!" << endl;
00058 
00059         LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
00060         setupMultiplicationRoutines();
00061         LOG( LVL_DEBUG ) << "done!" << endl;
00062 
00063         LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
00064         setupSimulation();
00065         LOG( LVL_DEBUG ) << "done!" << endl;
00066 
00067         LOG( LVL_DEBUG ) << "Solver: setup objective evaluation... " << endl;
00068         setupObjectiveEvaluation();
00069         LOG( LVL_DEBUG ) << "done!" << endl;
00070 
00071         LOG( LVL_DEBUG ) << "Solver: setup condensing... " << endl;
00072         setupCondensing();
00073         LOG( LVL_DEBUG ) << "done!" << endl;
00074 
00075         LOG( LVL_DEBUG ) << "Solver: setup constraints... " << endl;
00076         setupConstraintsEvaluation();
00077         LOG( LVL_DEBUG ) << "done!" << endl;
00078 
00079         LOG( LVL_DEBUG ) << "Solver: setup evaluation... " << endl;
00080         setupEvaluation();
00081         LOG( LVL_DEBUG ) << "done!" << endl;
00082 
00083         LOG( LVL_DEBUG ) << "Solver: setup auxiliary functions... " << endl;
00084         setupAuxiliaryFunctions();
00085         LOG( LVL_DEBUG ) << "done!" << endl;
00086 
00087         return SUCCESSFUL_RETURN;
00088 }
00089 
00090 returnValue ExportGaussNewtonCn2Factorization::getDataDeclarations(     ExportStatementBlock& declarations,
00091                                                                                                                                         ExportStruct dataStruct
00092                                                                                                                                         ) const
00093 {
00094         ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00095 
00096         declarations.addDeclaration(sbar, dataStruct);
00097         declarations.addDeclaration(x0, dataStruct);
00098         declarations.addDeclaration(Dx0, dataStruct);
00099 
00100         declarations.addDeclaration(W1, dataStruct);
00101         declarations.addDeclaration(W2, dataStruct);
00102 
00103         declarations.addDeclaration(D, dataStruct);
00104         declarations.addDeclaration(L, dataStruct);
00105 
00106         declarations.addDeclaration(T1, dataStruct);
00107         declarations.addDeclaration(T2, dataStruct);
00108         declarations.addDeclaration(T3, dataStruct);
00109 
00110         declarations.addDeclaration(E, dataStruct);
00111         declarations.addDeclaration(F, dataStruct);
00112 
00113         declarations.addDeclaration(QDy, dataStruct);
00114         declarations.addDeclaration(w1, dataStruct);
00115         declarations.addDeclaration(w2, dataStruct);
00116 
00117         declarations.addDeclaration(lbValues, dataStruct);
00118         declarations.addDeclaration(ubValues, dataStruct);
00119         declarations.addDeclaration(lbAValues, dataStruct);
00120         declarations.addDeclaration(ubAValues, dataStruct);
00121 
00122         if (performFullCondensing() == true)
00123                 declarations.addDeclaration(A10, dataStruct);
00124         declarations.addDeclaration(A20, dataStruct);
00125 
00126         declarations.addDeclaration(pacA01Dx0, dataStruct);
00127         declarations.addDeclaration(pocA02Dx0, dataStruct);
00128 
00129         declarations.addDeclaration(H, dataStruct);
00130         declarations.addDeclaration(U, dataStruct);
00131         declarations.addDeclaration(A, dataStruct);
00132         declarations.addDeclaration(g, dataStruct);
00133         declarations.addDeclaration(lb, dataStruct);
00134         declarations.addDeclaration(ub, dataStruct);
00135         declarations.addDeclaration(lbA, dataStruct);
00136         declarations.addDeclaration(ubA, dataStruct);
00137         declarations.addDeclaration(xVars, dataStruct);
00138         declarations.addDeclaration(yVars, dataStruct);
00139 
00140         return SUCCESSFUL_RETURN;
00141 }
00142 
00143 returnValue ExportGaussNewtonCn2Factorization::getFunctionDeclarations( ExportStatementBlock& declarations
00144                                                                                                                         ) const
00145 {
00146         declarations.addDeclaration( preparation );
00147         declarations.addDeclaration( feedback );
00148 
00149         declarations.addDeclaration( initialize );
00150         declarations.addDeclaration( initializeNodes );
00151         declarations.addDeclaration( shiftStates );
00152         declarations.addDeclaration( shiftControls );
00153         declarations.addDeclaration( getKKT );
00154         declarations.addDeclaration( getObjective );
00155 
00156         declarations.addDeclaration( evaluateStageCost );
00157         declarations.addDeclaration( evaluateTerminalCost );
00158 
00159         return SUCCESSFUL_RETURN;
00160 }
00161 
00162 returnValue ExportGaussNewtonCn2Factorization::getCode( ExportStatementBlock& code
00163                                                                                         )
00164 {
00165         setupQPInterface();
00166 
00167         code.addLinebreak( 2 );
00168         code.addStatement( "/******************************************************************************/\n" );
00169         code.addStatement( "/*                                                                            */\n" );
00170         code.addStatement( "/* ACADO code generation                                                      */\n" );
00171         code.addStatement( "/*                                                                            */\n" );
00172         code.addStatement( "/******************************************************************************/\n" );
00173         code.addLinebreak( 2 );
00174 
00175         int useOMP;
00176         get(CG_USE_OPENMP, useOMP);
00177         if ( useOMP )
00178         {
00179                 code.addDeclaration( state );
00180         }
00181 
00182         code.addFunction( modelSimulation );
00183 
00184         code.addFunction( evaluateStageCost );
00185         code.addFunction( evaluateTerminalCost );
00186 
00187         code.addFunction( setObjQ1Q2 );
00188         code.addFunction( setObjR1R2 );
00189         code.addFunction( setObjQN1QN2 );
00190         code.addFunction( evaluateObjective );
00191 
00192 //      code.addFunction( multGxd );
00193         code.addFunction( moveGxT );
00194 //      code.addFunction( multGxGx );
00195         code.addFunction( multGxGu );
00196         code.addFunction( moveGuE );
00197 
00198         code.addFunction( multBTW1 );
00199         code.addFunction( macBTW1_R1 );
00200         code.addFunction( multGxTGu );
00201         code.addFunction( macQEW2 );
00202 
00203         code.addFunction( mult_H_W2T_W3 );
00204         code.addFunction( mac_H_W2T_W3_R );
00205         code.addFunction( mac_W3_G_W1T_G );
00206 
00207         code.addFunction( mac_R_T2_B_D );
00208         code.addFunction( move_D_U );
00209         code.addFunction( mult_L_E_U );
00210         code.addFunction( updateQ );
00211         code.addFunction( mul_T2_A_L );
00212         code.addFunction( mult_BT_T1_T2 );
00213 
00214 //      ExportFunction mac_R_BT_F_D, mult_FT_A_L;
00215 //              ExportFunction updateQ2;
00216 //              ExportFunction mac_W1_T1_E_F;
00217 
00218         code.addFunction( mac_R_BT_F_D );
00219         code.addFunction( mult_FT_A_L );
00220         code.addFunction( updateQ2 );
00221         code.addFunction( mac_W1_T1_E_F );
00222         code.addFunction( move_GxT_T3 );
00223 
00224         cholSolver.getCode( code );
00225 
00226 //      ExportFunction multATw1Q, macBTw1, macQSbarW2, macASbarD;
00227 
00228         code.addFunction( macATw1QDy );
00229         code.addFunction( macBTw1 );
00230         code.addFunction( macQSbarW2 );
00231         code.addFunction( macASbar );
00232 //      code.addFunction( macASbarD2 );
00233         code.addFunction( expansionStep );
00234 
00235 //      code.addFunction( setBlockH11 );
00236 //      code.addFunction( zeroBlockH11 );
00237         code.addFunction( copyHTH );
00238 //      code.addFunction( multQ1d );
00239 //      code.addFunction( multQN1d );
00240         code.addFunction( multRDy );
00241         code.addFunction( multQDy );
00242 //      code.addFunction( multEQDy );
00243 //      code.addFunction( multQETGx );
00244 //      code.addFunction( zeroBlockH10 );
00245 //      code.addFunction( multEDu );
00246 //      code.addFunction( multQ1Gx );
00247 //      code.addFunction( multQN1Gx );
00248         code.addFunction( multQ1Gu );
00249         code.addFunction( multQN1Gu );
00250 //      code.addFunction( zeroBlockH00 );
00251 //      code.addFunction( multCTQC );
00252 
00253         code.addFunction( multHxC );
00254         code.addFunction( multHxE );
00255         code.addFunction( macHxd );
00256 
00257         code.addFunction( evaluatePathConstraints );
00258 
00259         for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00260         {
00261                 if (evaluatePointConstraints[ i ] == 0)
00262                         continue;
00263                 code.addFunction( *evaluatePointConstraints[ i ] );
00264         }
00265 
00266         code.addFunction( macCTSlx );
00267         code.addFunction( macETSlu );
00268 
00269         code.addFunction( condensePrep );
00270         code.addFunction( condenseFdb );
00271         code.addFunction( expand );
00272 
00273         code.addFunction( preparation );
00274         code.addFunction( feedback );
00275 
00276         code.addFunction( initialize );
00277         code.addFunction( initializeNodes );
00278         code.addFunction( shiftStates );
00279         code.addFunction( shiftControls );
00280         code.addFunction( getKKT );
00281         code.addFunction( getObjective );
00282 
00283         return SUCCESSFUL_RETURN;
00284 }
00285 
00286 
00287 unsigned ExportGaussNewtonCn2Factorization::getNumQPvars( ) const
00288 {
00289         if (performFullCondensing() == true)
00290                 return (N * NU);
00291 
00292         return (NX + N * NU);
00293 }
00294 
00295 unsigned ExportGaussNewtonCn2Factorization::getNumStateBounds() const
00296 {
00297         return xBoundsIdxRev.size();
00298 }
00299 
00300 //
00301 // PROTECTED FUNCTIONS:
00302 //
00303 
00304 returnValue ExportGaussNewtonCn2Factorization::setupObjectiveEvaluation( void )
00305 {
00306         evaluateObjective.setup("evaluateObjective");
00307 
00308         int variableObjS;
00309         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00310 
00311         //
00312         // A loop the evaluates objective and corresponding gradients
00313         //
00314         ExportIndex runObj( "runObj" );
00315         ExportForLoop loopObjective( runObj, 0, N );
00316 
00317         evaluateObjective.addIndex( runObj );
00318 
00319         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00320         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00321         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00322         loopObjective.addLinebreak( );
00323 
00324         // Evaluate the objective function
00325         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00326 
00327         // Stack the measurement function value
00328         loopObjective.addStatement(
00329                         Dy.getRows(runObj * NY, (runObj + 1) * NY) ==  objValueOut.getTranspose().getRows(0, getNY())
00330         );
00331         loopObjective.addLinebreak( );
00332 
00333         // Optionally compute derivatives
00334         unsigned indexX = getNY();
00335 //      unsigned indexG = indexX;
00336 
00337         ExportVariable tmpObjS, tmpFx, tmpFu;
00338         ExportVariable tmpFxEnd, tmpObjSEndTerm;
00339         tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00340         if (objS.isGiven() == true)
00341                 tmpObjS = objS;
00342         tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00343         if (objEvFx.isGiven() == true)
00344                 tmpFx = objEvFx;
00345         tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00346         if (objEvFu.isGiven() == true)
00347                 tmpFu = objEvFu;
00348         tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00349         if (objEvFxEnd.isGiven() == true)
00350                 tmpFxEnd = objEvFxEnd;
00351         tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00352         if (objSEndTerm.isGiven() == true)
00353                 tmpObjSEndTerm = objSEndTerm;
00354 
00355         //
00356         // Optional computation of Q1, Q2
00357         //
00358         if (Q1.isGiven() == false)
00359         {
00360                 ExportVariable tmpQ1, tmpQ2;
00361                 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00362                 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00363 
00364                 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00365                 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00366                 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00367 
00368                 if (tmpFx.isGiven() == true)
00369                 {
00370                         if (variableObjS == YES)
00371                         {
00372                                 loopObjective.addFunctionCall(
00373                                                 setObjQ1Q2,
00374                                                 tmpFx, objS.getAddress(runObj * NY, 0),
00375                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00376                                 );
00377                         }
00378                         else
00379                         {
00380                                 loopObjective.addFunctionCall(
00381                                                 setObjQ1Q2,
00382                                                 tmpFx, objS,
00383                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00384                                 );
00385                         }
00386                 }
00387                 else
00388                 {
00389                         if (variableObjS == YES)
00390                         {
00391                                 if (objEvFx.isGiven() == true)
00392 
00393                                         loopObjective.addFunctionCall(
00394                                                         setObjQ1Q2,
00395                                                         objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00396                                                         Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00397                                         );
00398                         }
00399                         else
00400                         {
00401                                 loopObjective.addFunctionCall(
00402                                                 setObjQ1Q2,
00403                                                 objValueOut.getAddress(0, indexX), objS,
00404                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00405                                 );
00406                         }
00407                         indexX += objEvFx.getDim();
00408                 }
00409 
00410                 loopObjective.addLinebreak( );
00411         }
00412 
00413         if (R1.isGiven() == false)
00414         {
00415                 ExportVariable tmpR1, tmpR2;
00416                 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00417                 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00418 
00419                 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00420                 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00421                 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00422 
00423                 if (tmpFu.isGiven() == true)
00424                 {
00425                         if (variableObjS == YES)
00426                         {
00427                                 loopObjective.addFunctionCall(
00428                                                 setObjR1R2,
00429                                                 tmpFu, objS.getAddress(runObj * NY, 0),
00430                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00431                                 );
00432                         }
00433                         else
00434                         {
00435                                 loopObjective.addFunctionCall(
00436                                                 setObjR1R2,
00437                                                 tmpFu, objS,
00438                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00439                                 );
00440                         }
00441                 }
00442                 else
00443                 {
00444                         if (variableObjS == YES)
00445                         {
00446                                 loopObjective.addFunctionCall(
00447                                                 setObjR1R2,
00448                                                 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00449                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00450                                 );
00451                         }
00452                         else
00453                         {
00454                                 loopObjective.addFunctionCall(
00455                                                 setObjR1R2,
00456                                                 objValueOut.getAddress(0, indexX), objS,
00457                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00458                                 );
00459                         }
00460                 }
00461 
00462                 loopObjective.addLinebreak( );
00463         }
00464 
00465         evaluateObjective.addStatement( loopObjective );
00466 
00467         //
00468         // Evaluate the quadratic Mayer term
00469         //
00470         evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00471         evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00472 
00473         // Evaluate the objective function
00474         evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00475         evaluateObjective.addLinebreak( );
00476 
00477         evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00478         evaluateObjective.addLinebreak();
00479 
00480         if (QN1.isGiven() == false)
00481         {
00482                 indexX = getNYN();
00483 
00484                 ExportVariable tmpQN1, tmpQN2;
00485                 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00486                 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00487 
00488                 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00489                 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00490                 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00491 
00492                 if (tmpFxEnd.isGiven() == true)
00493                         evaluateObjective.addFunctionCall(
00494                                         setObjQN1QN2,
00495                                         tmpFxEnd, objSEndTerm,
00496                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00497                         );
00498                 else
00499                         evaluateObjective.addFunctionCall(
00500                                         setObjQN1QN2,
00501                                         objValueOut.getAddress(0, indexX), objSEndTerm,
00502                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00503                         );
00504 
00505                 evaluateObjective.addLinebreak( );
00506         }
00507 
00508         return SUCCESSFUL_RETURN;
00509 }
00510 
00511 returnValue ExportGaussNewtonCn2Factorization::setupConstraintsEvaluation( void )
00512 {
00513         ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
00514 
00515         int hardcodeConstraintValues;
00516         get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00517 
00519         //
00520         // Determine dimensions of constraints
00521         //
00523 
00524         unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
00525         unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
00526         unsigned numStateBounds = getNumStateBounds();
00527 //      unsigned numPathCon = N * dimPacH;
00528 //      unsigned numPointCon = dimPocH;
00529 
00531         //
00532         // Setup the bounds on independent variables
00533         //
00535 
00536         DVector lbBoundValues( numBounds );
00537         DVector ubBoundValues( numBounds );
00538 
00539         if (initialStateFixed( ) == false)
00540                 for(unsigned el = 0; el < NX; ++el)
00541                 {
00542                         lbBoundValues( el )= xBounds.getLowerBound(0, el);
00543                         ubBoundValues( el ) = xBounds.getUpperBound(0, el);
00544                 }
00545 
00546         // UPDATED
00547         for(unsigned node = 0; node < N; ++node)
00548                 for(unsigned el = 0; el < NU; ++el)
00549                 {
00550                         lbBoundValues(offsetBounds + (N - 1 - node) * NU + el) = uBounds.getLowerBound(node, el);
00551                         ubBoundValues(offsetBounds + (N - 1 - node) * NU + el) = uBounds.getUpperBound(node, el);
00552                 }
00553 
00554         if (hardcodeConstraintValues == YES)
00555         {
00556                 lbValues.setup("lbValues", lbBoundValues, REAL, ACADO_VARIABLES);
00557                 ubValues.setup("ubValues", ubBoundValues, REAL, ACADO_VARIABLES);
00558         }
00559         else
00560         {
00561                 lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
00562                 lbValues.setDoc( "Lower bounds values." );
00563                 ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
00564                 ubValues.setDoc( "Upper bounds values." );
00565         }
00566 
00567         ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
00568 
00569         if (performFullCondensing() == true)
00570         {
00571                 ExportVariable uCol = u.makeColVector();
00572                 // Full condensing case
00573                 for (unsigned blk = 0; blk < N; ++blk)
00574                 {
00575                         boundSetFcn->addStatement( lb.getRows(blk * NU, (blk + 1) * NU) ==
00576                                         lbValues.getRows(blk * NU, (blk + 1) * NU) - uCol.getRows((N - 1 - blk) * NU, (N - blk) * NU));
00577                         boundSetFcn->addStatement( ub.getRows(blk * NU, (blk + 1) * NU) ==
00578                                         ubValues.getRows(blk * NU, (blk + 1) * NU) - uCol.getRows((N - 1 - blk) * NU, (N - blk) * NU));
00579                 }
00580         }
00581         else
00582         {
00583                 // Partial condensing case
00584                 if (initialStateFixed() == true)
00585                 {
00586                         // MPC case
00587                         condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
00588                         condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
00589 
00590                         boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
00591                         boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
00592                 }
00593                 else
00594                 {
00595                         // MHE case
00596                         boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00597                         boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00598                         boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00599                         boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00600                 }
00601         }
00602         boundSetFcn->addLinebreak( );
00603 
00605         //
00606         // Determine number of affine constraints and set up structures that
00607         // holds constraint values
00608         //
00610 
00611         unsigned sizeA = numStateBounds + getNumComplexConstraints();
00612 
00613         if ( sizeA )
00614         {
00615                 if (hardcodeConstraintValues == true)
00616                 {
00617                         DVector lbTmp, ubTmp;
00618 
00619                         if ( numStateBounds )
00620                         {
00621                                 DVector lbStateBoundValues( numStateBounds );
00622                                 DVector ubStateBoundValues( numStateBounds );
00623                                 for (unsigned i = 0; i < numStateBounds; ++i)
00624                                 {
00625                                         lbStateBoundValues( i ) = xBounds.getLowerBound( xBoundsIdxRev[ i ] / NX, xBoundsIdxRev[ i ] % NX );
00626                                         ubStateBoundValues( i ) = xBounds.getUpperBound( xBoundsIdxRev[ i ] / NX, xBoundsIdxRev[ i ] % NX );
00627                                 }
00628 
00629                                 lbTmp.append( lbStateBoundValues );
00630                                 ubTmp.append( ubStateBoundValues );
00631                         }
00632 
00633                         lbTmp.append( lbPathConValues );
00634                         ubTmp.append( ubPathConValues );
00635 
00636                         lbTmp.append( lbPointConValues );
00637                         ubTmp.append( ubPointConValues );
00638 
00639 
00640                         lbAValues.setup("lbAValues", lbTmp, REAL, ACADO_VARIABLES);
00641                         ubAValues.setup("ubAValues", ubTmp, REAL, ACADO_VARIABLES);
00642                 }
00643                 else
00644                 {
00645                         lbAValues.setup("lbAValues", sizeA, 1, REAL, ACADO_VARIABLES);
00646                         lbAValues.setDoc( "Lower bounds values for affine constraints." );
00647                         ubAValues.setup("ubAValues", sizeA, 1, REAL, ACADO_VARIABLES);
00648                         ubAValues.setDoc( "Lower bounds values for affine constraints." );
00649                 }
00650         }
00651 
00653         //
00654         // Setup the bounds on state variables
00655         //
00657 
00658         if ( numStateBounds )
00659         {
00660                 condenseFdb.addVariable( tmp );
00661 
00662                 unsigned offset = (performFullCondensing() == true) ? 0 : NX;
00663                 unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
00664 
00665                 if (numOps < 1024)
00666                 {
00667                         for(unsigned row = 0; row < numStateBounds; ++row)
00668                         {
00669                                 unsigned conIdx = xBoundsIdx[ row ];
00670                                 unsigned blk = conIdx / NX;
00671 
00672                                 // TODO
00673 //                              if (performFullCondensing() == false)
00674 //                                      condensePrep.addStatement( A.getSubMatrix(ii, ii + 1, 0, NX) == evGx.getRow( conIdx ) );
00675 
00676                                 for (unsigned col = blk; col < N; ++col)
00677                                 {
00678                                         // blk = (N - row) * (N - 1 - row) / 2 + (N - 1 - col)
00679                                         unsigned blkRow = ((N - blk) * (N - 1 - blk) / 2 + (N - 1 - col)) * NX + conIdx % NX;
00680 
00681                                         condensePrep.addStatement(
00682                                                         A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
00683                                 }
00684 
00685                                 condensePrep.addLinebreak();
00686                         }
00687                 }
00688                 else
00689                 {
00690                         DMatrix vXBoundIndices(1, numStateBounds);
00691                         for (unsigned i = 0; i < numStateBounds; ++i)
00692                                 vXBoundIndices(0, i) = xBoundsIdx[ i ];
00693                         ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
00694 
00695                         condensePrep.addVariable( evXBounds );
00696 
00697                         ExportIndex row, col, conIdx, blk, blkRow, blkIdx;
00698 
00699                         condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow ).acquire( blkIdx );
00700 
00701                         ExportForLoop lRow(row, 0, numStateBounds);
00702 
00703                         lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ];\n";
00704                         lRow.addStatement( blk == conIdx / NX );
00705 
00706                         // TODO
00707 //                      if (performFullCondensing() == false)
00708 //                              eLoopI.addStatement( A.getSubMatrix(ii, ii + 1, 0, NX) == evGx.getRow( conIdx ) );
00709 
00710                         ExportForLoop lCol(col, blk, N);
00711 
00712                         lCol.addStatement( blkIdx == (N - blk) * (N - 1 - blk) / 2 + (N - 1 - col) );
00713                         lCol.addStatement( blkRow == blkIdx * NX + conIdx % NX );
00714                         lCol.addStatement(
00715                                         A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
00716 
00717                         lRow.addStatement( lCol );
00718                         condensePrep.addStatement( lRow );
00719 
00720                         condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow ).release( blkIdx );
00721                 }
00722                 condensePrep.addLinebreak( );
00723 
00724                 // Shift constraint bounds by first interval
00725                 // MPC case, only
00726                 ExportVariable xVec = x.makeRowVector();
00727                 for(unsigned row = 0; row < getNumStateBounds( ); ++row)
00728                 {
00729                         unsigned conIdx = xBoundsIdxRev[ row ];
00730 
00731                         condenseFdb.addStatement( tmp == sbar.getRow( conIdx ) + xVec.getCol( conIdx ) );
00732                         condenseFdb.addStatement( lbA.getRow( row ) == lbAValues( row ) - tmp );
00733                         condenseFdb.addStatement( ubA.getRow( row ) == ubAValues( row ) - tmp );
00734                 }
00735                 condenseFdb.addLinebreak( );
00736         }
00737 
00738         return SUCCESSFUL_RETURN;
00739 }
00740 
00741 returnValue ExportGaussNewtonCn2Factorization::setupCondensing( void )
00742 {
00743         condensePrep.setup("condensePrep");
00744         condenseFdb.setup( "condenseFdb" );
00745 
00747         //
00748         // Compute Hessian block H11
00749         //
00751 
00752         unsigned prepCacheSize =
00753                         2 * NX * NU +
00754                         NU * NU + NU * NX +
00755                         2 * NX * NX + NU * NX;
00756 
00757         int useSinglePrecision;
00758         get(USE_SINGLE_PRECISION, useSinglePrecision);
00759         prepCacheSize = prepCacheSize * (useSinglePrecision ? 4 : 8);
00760         LOG( LVL_DEBUG ) << "---> Condensing prep. part, cache size: " << prepCacheSize << " bytes" << endl;
00761 
00762         ExportStruct prepCache = prepCacheSize < 16384 ? ACADO_LOCAL : ACADO_WORKSPACE;
00763 
00764         W1.setup("W1", NX, NU, REAL, prepCache);
00765         W2.setup("W2", NX, NU, REAL, prepCache);
00766 
00767         D.setup("D", NU, NU, REAL, prepCache);
00768         L.setup("L", NU, NX, REAL, prepCache);
00769 
00770         T1.setup("T1", NX, NX, REAL, prepCache);
00771         T2.setup("T2", NU, NX, REAL, prepCache);
00772         T3.setup("T3", NX, NX, REAL, prepCache);
00773 
00774         condensePrep
00775                         .addVariable( W1 ).addVariable( W2 )
00776                         .addVariable( D ).addVariable( L )
00777                         .addVariable( T1 ).addVariable( T2 ).addVariable( T3 );
00778 
00779         LOG( LVL_DEBUG ) << "---> Setup condensing: E" << endl;
00781 
00782         // Special case, row = col = 0
00783         condensePrep.addFunctionCall(moveGuE, evGu.getAddress(0, 0), E.getAddress(0, 0) );
00784 
00785         if (N <= 15)
00786         {
00787                 unsigned row, col, prev, curr;
00788                 for (row = 1; row < N; ++row)
00789                 {
00790                         condensePrep.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T1);
00791 
00792                         for(col = 0; col < row; ++col)
00793                         {
00794                                 prev = row * (row - 1) / 2 + col;
00795                                 curr = (row + 1) * row / 2 + col;
00796 
00797                                 condensePrep.addFunctionCall(multGxGu, T1, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0));
00798                         }
00799 
00800                         curr = (row + 1) * row / 2 + col;
00801                         condensePrep.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
00802 
00803                         condensePrep.addLinebreak();
00804                 }
00805         }
00806         else
00807         {
00808                 ExportIndex row, col, curr, prev;
00809 
00810                 condensePrep.acquire( row ).acquire( col ).acquire( curr ).acquire( prev );
00811 
00812                 ExportForLoop lRow(row, 1, N), lCol(col, 0, row);
00813 
00814                 lRow.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T1);
00815 
00816                 lCol.addStatement( prev == row * (row - 1) / 2 + col );
00817                 lCol.addStatement( curr == (row + 1) * row / 2 + col );
00818                 lCol.addFunctionCall( multGxGu, T1, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0) );
00819 
00820                 lRow.addStatement( lCol );
00821                 lRow.addStatement( curr == (row + 1) * row / 2 + col );
00822                 lRow.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
00823 
00824                 condensePrep.addStatement( lRow );
00825 
00826                 condensePrep.release( row ).release( col ).release( curr ).release( prev );
00827         }
00828         condensePrep.addLinebreak( 2 );
00829 
00830         LOG( LVL_DEBUG ) << "---> Setup condensing: H11" << endl;
00831 
00832         /*
00833 
00834         Using some heuristics, W1, W2, W3 can be allocated on stack.
00835 
00836         The algorithm to create the Hessian becomes:
00837 
00838         for col = 0: N - 1
00839                 row = 0
00840                 W1 = Q(N - row) * E(N - 1 - row, N - 1 - col)
00841 
00842                 for row = 0: col - 1
00843                         H(row, col) = B(N - 1 - row)^T * W1
00844                         W2 = A(N - (row + 1))^T * W1
00845                         W1 = W2 + Q(N - (row + 1)) * E(N - 1 - (row + 1), N - 1 - col)
00846 
00847                 H(col, col) += R(N - 1 - col) + B(N - 1 - col)^T * W1
00848 
00849         Indexing of G matrix:
00850                 QE(row, col) -> block index is calculated as: blk = (row + 1) * row / 2 + col
00851                 G is with reversed rows and columns, say: G(row, col) = QE(N - 1 - row, N - 1 - col)
00852                 In more details, G(row, col) block index is calculated as:
00853                         blk = (N - row) * (N - 1 - row) / 2 + (N - 1 - col)
00854 
00855          */
00856 
00857         if (N <= 15)
00858         {
00859                 for (unsigned col = 0; col < N; ++col)
00860                 {
00861                         // row = 0
00862                         unsigned curr = (N) * (N - 1) / 2 + (N - 1 - col);
00863                         if (QN1.isGiven() == true)
00864                                 condensePrep.addFunctionCall(
00865                                                 multQN1Gu, E.getAddress(curr * NX), W1
00866                                 );
00867                         else
00868                                 condensePrep.addFunctionCall(
00869                                                 multGxGu, QN1, E.getAddress(curr * NX), W1
00870                                 );
00871 
00872                         for (unsigned row = 0; row < col; ++row)
00873                         {
00874                                 condensePrep.addFunctionCall(
00875                                                 multBTW1, evGu.getAddress((N - 1 - row) * NX), W1, ExportIndex( row ), ExportIndex( col )
00876                                 );
00877 
00878                                 condensePrep.addFunctionCall(
00879                                                 multGxTGu, evGx.getAddress((N - (row + 1)) * NX), W1, W2
00880                                 );
00881 
00882                                 unsigned next = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
00883                                 if (Q1.isGiven() == true)
00884                                         condensePrep.addFunctionCall(
00885                                                         macQEW2, Q1, E.getAddress(next * NX), W2, W1
00886                                         );
00887                                 else
00888                                         condensePrep.addFunctionCall(
00889                                                         macQEW2,
00890                                                         Q1.getAddress((N - (row + 1)) * NX), E.getAddress(next * NX), W2, W1
00891                                         );
00892                         }
00893 
00894                         if (R1.isGiven() == true)
00895                                 condensePrep.addFunctionCall(
00896                                                 macBTW1_R1, R1, evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
00897                                 );
00898                         else
00899                                 condensePrep.addFunctionCall(
00900                                                 macBTW1_R1, R1.getAddress((N - 1 - col) * NU), evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
00901                                 );
00902 
00903                         condensePrep.addLinebreak();
00904                 }
00905         }
00906         else
00907         {
00908                 // No loop unrolling...
00909                 ExportIndex row, col, curr, next;
00910                 condensePrep.acquire( row ).acquire( col ).acquire( curr ).acquire( next );
00911 
00912                 ExportForLoop colLoop(col, 0, N);
00913                 colLoop << (curr == (N) * (N - 1) / 2 + (N - 1 - col));
00914 
00915                 if (QN1.isGiven() == true)
00916                         colLoop.addFunctionCall(multQN1Gu, E.getAddress(curr * NX), W1);
00917                 else
00918                         colLoop.addFunctionCall(multGxGu, QN1, E.getAddress(curr * NX), W1);
00919 
00920                 ExportForLoop rowLoop(row, 0, col);
00921                 rowLoop.addFunctionCall(
00922                                 multBTW1, evGu.getAddress((N - 1 - row) * NX), W1, ExportIndex( row ), ExportIndex( col )
00923                 );
00924 
00925                 rowLoop.addFunctionCall(
00926                                 multGxTGu, evGx.getAddress((N - (row + 1)) * NX), W1, W2
00927                 );
00928 
00929                 rowLoop << (next == (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col));
00930                 if (Q1.isGiven() == true)
00931                         rowLoop.addFunctionCall(macQEW2, Q1, E.getAddress(next * NX), W2, W1);
00932                 else
00933                         rowLoop.addFunctionCall(macQEW2, Q1.getAddress((N - (row + 1)) * NX), E.getAddress(next * NX), W2, W1);
00934 
00935                 colLoop << rowLoop;
00936 
00937                 if (R1.isGiven() == true)
00938                         colLoop.addFunctionCall(
00939                                         macBTW1_R1, R1, evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
00940                         );
00941                 else
00942                         colLoop.addFunctionCall(
00943                                         macBTW1_R1, R1.getAddress((N - 1 - col) * NU), evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
00944                         );
00945 
00946                 condensePrep << colLoop;
00947                 condensePrep.release( row ).release( col ).release( curr ).release( next );
00948         }
00949         condensePrep.addLinebreak( 2 );
00950 
00951         LOG( LVL_DEBUG ) << "---> Copy H11 upper to lower triangular part" << endl;
00952 
00953         // Copy to H11 upper triangular part to lower triangular part
00954         if (N <= 15)
00955         {
00956                 for (unsigned row = 0; row < N; ++row)
00957                         for(unsigned col = 0; col < row; ++col)
00958                                 condensePrep.addFunctionCall( copyHTH, ExportIndex( row ), ExportIndex( col ) );
00959         }
00960         else
00961         {
00962                 ExportIndex row, col;
00963 
00964                 condensePrep.acquire( row ).acquire( col );
00965 
00966                 ExportForLoop lRow(row, 0, N), lCol(col, 0, row);
00967 
00968                 lCol.addFunctionCall(copyHTH, row, col);
00969                 lRow.addStatement( lCol );
00970                 condensePrep.addStatement( lRow );
00971 
00972                 condensePrep.release( row ).release( col );
00973         }
00974         condensePrep.addLinebreak( 2 );
00975 
00976         LOG( LVL_DEBUG ) << "---> Factorization of the condensed Hessian" << endl;
00977 
00978         /*
00979 
00980         T1 = Q( N )
00981         for blk = N - 1: 1
00982                 T2 = B( blk )^T * T1
00983 
00984                 D = R( blk ) + T2 * B( blk )
00985                 L = T2 * A( blk )
00986 
00987                 chol( D )
00988                 L = chol_solve(D, L) // L <- D^(-T) * L
00989 
00990                 T3 = T1 * A( blk )
00991                 T1 = Q( blk ) + A( blk )^T * T3
00992                 T1 -= L^T * L
00993 
00994                 row = N - 1 - blk
00995 
00996                 U(row, row) = D
00997                 for col = row + 1: N - 1
00998                         U(row, col) = L * E(row + 1, col)
00999 
01000         T2 = B( 0 )^T * T1
01001         D = R( 0 ) + T2 * B( 0 )
01002         chol( D )
01003         U(N - 1, N - 1) = D
01004 
01005         ************************************************
01006         ************************************************
01007         OR, an alternative, n_x^3 free version would be:
01008 
01009         T1 = Q( N )
01010         for col = 0: N - 1
01011                 F( col ) = T1 * E(0, col)
01012 
01013         for blk = N - 1: 1
01014                 row = N - 1 - blk
01015 
01016                 D = R( blk ) + B( blk )^T * F( row )
01017                 L = F( row )^T * A( blk )
01018 
01019                 chol( D )
01020                 L = chol_solve(D, L) // L <- D^(-T) * L
01021 
01022                 T1 = Q( blk ) - L^T * L
01023 
01024                 T3 = A( blk )^T
01025 
01026                 for col = row + 1: N - 1
01027                         W1 = T3 * F( col )
01028                         F( col ) = T1 * E(row + 1, col) + W1
01029 
01030                 U(row, row) = D
01031                 for col = row + 1: N - 1
01032                         U(row, col) = L * E(row + 1, col)
01033 
01034         D = R( 0 ) + B( 0 )^T * F(N - 1)
01035         chol( D )
01036         U(N - 1, N - 1) = D
01037 
01038          */
01039 
01040         // The matrix where we store the factorization
01041         U.setup("U", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01042         // A helper matrix
01043         F.setup("F", N * NX, NU, REAL, ACADO_WORKSPACE);
01044 
01045         cholSolver.init(NU, NX, "condensing");
01046         cholSolver.setup();
01047 
01048         // Just for testing...
01049 //      condensePrep.addStatement( U == H );
01050 //      condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), U);
01051 //      condensePrep.addFunctionCall(cholSolver.getSolveFunction(), U, Id);
01052 
01053 #if 0
01054         // N^2 factorization with n_x^3 terms
01055 
01056         condensePrep.addStatement( T1 == QN1 );
01057         for (unsigned blk = N - 1; blk > 0; --blk)
01058         {
01059                 condensePrep.addFunctionCall(mult_BT_T1_T2, evGu.getAddress(blk * NX), T1, T2);
01060                 if (R1.isGiven() == true)
01061                         condensePrep.addFunctionCall(mac_R_T2_B_D, R1, T2, evGu.getAddress(blk * NX), D);
01062                 else
01063                         condensePrep.addFunctionCall(mac_R_T2_B_D, R1.getAddress(blk * NX), T2, evGu.getAddress(blk * NX), D);
01064                 condensePrep.addFunctionCall(mul_T2_A_L, T2, evGx.getAddress(blk * NX), L);
01065 
01066                 condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
01067                 condensePrep.addFunctionCall(cholSolver.getSolveFunction(), D, L);
01068 
01069                 if (Q1.isGiven() == true)
01070                         condensePrep.addFunctionCall(updateQ, Q1, T3, evGx.getAddress(blk * NX), L, T1);
01071                 else
01072                         condensePrep.addFunctionCall(updateQ, Q1.getAddress(blk * NX), T3, evGx.getAddress(blk * NX), L, T1);
01073 
01074                 unsigned row = N - 1 - blk;
01075 
01076                 condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( row ));
01077                 for (unsigned col = row + 1; col < N; ++col)
01078                 {
01079                         // U(row, col) = L * E(row + 1, col)
01080 
01081                         // blk = (N - row) * (N - 1 - row) / 2 + (N - 1 - col)
01082 
01083                         unsigned blkE = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
01084                         cout << "blkE " << blkE << endl;
01085 
01086                         condensePrep.addFunctionCall(mult_L_E_U, L, E.getAddress(blkE * NX), U, ExportIndex( row ), ExportIndex( col ));
01087                 }
01088         }
01089         condensePrep.addFunctionCall(mult_BT_T1_T2, evGu.getAddress( 0 ), T1, T2);
01090         condensePrep.addFunctionCall(mac_R_T2_B_D, R1.getAddress( 0 ), T2, evGu.getAddress( 0 ), D);
01091         condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
01092         condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( N - 1 ));
01093 #else
01094         // N^2 factorization, n_x^3 free version
01095 
01096         // Initial value for the "updated" Q^* matrix
01097         condensePrep.addStatement( T1 == QN1 );
01098 
01099         if (N <= 15)
01100         {
01101                 for (unsigned col = 0; col < N; ++col)
01102                 {
01103                         unsigned blkE = (N - 0) * (N - 1 - 0) / 2 + (N - 1 - col);
01104                         condensePrep.addFunctionCall(
01105                                         multGxGu, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
01106                 }
01107 
01108                 for (unsigned blk = N - 1; blk > 0; --blk)
01109                 {
01110                         unsigned row = N - 1 - blk;
01111 
01112                         // D = R( blk ) + B( blk )^T * F( row )
01113                         // L = F( row )^T * A( blk )
01114                         if (R1.isGiven() == true)
01115                                 condensePrep.addFunctionCall(
01116                                                 mac_R_BT_F_D, R1, evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
01117                         else
01118                                 condensePrep.addFunctionCall(
01119                                                 mac_R_BT_F_D, R1.getAddress(blk * NX), evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
01120                         condensePrep.addFunctionCall(mult_FT_A_L, F.getAddress(row * NX), evGx.getAddress(blk * NX), L);
01121 
01122                         // Chol and system solve
01123                         condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
01124                         condensePrep.addFunctionCall(cholSolver.getSolveFunction(), D, L);
01125 
01126                         // Update Q
01127                         if (Q1.isGiven() == true)
01128                                 condensePrep.addFunctionCall(updateQ2, Q1, L, T1);
01129                         else
01130                                 condensePrep.addFunctionCall(updateQ2, Q1.getAddress(blk * NX), L, T1);
01131 
01132                         // for col = row + 1: N - 1
01133                         //       W1 = A( blk )^T * F( col )
01134                         //       F( col ) = W1 + T1 * E(row + 1, col)
01135 
01136                         condensePrep.addFunctionCall(move_GxT_T3, evGx.getAddress(blk * NX), T3);
01137 
01138                         for (unsigned col = row + 1; col < N; ++col)
01139                         {
01140                                 condensePrep.addFunctionCall(multGxGu, T3, F.getAddress(col * NX), W1);
01141 
01142                                 unsigned blkE = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
01143                                 condensePrep.addFunctionCall(mac_W1_T1_E_F, W1, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
01144                         }
01145 
01146                         // Calculate one block-row of the factorized matrix
01147                         condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( row ));
01148                         for (unsigned col = row + 1; col < N; ++col)
01149                         {
01150                                 // U(row, col) = L * E(row + 1, col)
01151 
01152                                 unsigned blkE = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
01153                                 condensePrep.addFunctionCall(mult_L_E_U, L, E.getAddress(blkE * NX), U, ExportIndex( row ), ExportIndex( col ));
01154                         }
01155                 }
01156         }
01157         else
01158         {
01159                 // Do not unroll the for-loops...
01160 
01161                 ExportIndex col, blkE, blk, row;
01162 
01163                 condensePrep.acquire( col ).acquire( blkE ).acquire( blk ).acquire( row );
01164 
01165                 ExportForLoop colLoop(col, 0, N);
01166                 colLoop << (blkE == (N - 0) * (N - 1 - 0) / 2 + (N - 1 - col));
01167                 colLoop.addFunctionCall(
01168                                 multGxGu, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
01169                 condensePrep << colLoop;
01170 
01171                 ExportForLoop blkLoop(blk, N - 1, 0, -1);
01172                 blkLoop << ( row == N - 1 - blk );
01173 
01174                 if (R1.isGiven() == true)
01175                         blkLoop.addFunctionCall(
01176                                         mac_R_BT_F_D, R1, evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
01177                 else
01178                         blkLoop.addFunctionCall(
01179                                         mac_R_BT_F_D, R1.getAddress(blk * NX), evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
01180                 blkLoop.addFunctionCall(mult_FT_A_L, F.getAddress(row * NX), evGx.getAddress(blk * NX), L);
01181 
01182                 // Chol and system solve
01183                 blkLoop.addFunctionCall(cholSolver.getCholeskyFunction(), D);
01184                 blkLoop.addFunctionCall(cholSolver.getSolveFunction(), D, L);
01185 
01186                 // Update Q
01187                 if (Q1.isGiven() == true)
01188                         blkLoop.addFunctionCall(updateQ2, Q1, L, T1);
01189                 else
01190                         blkLoop.addFunctionCall(updateQ2, Q1.getAddress(blk * NX), L, T1);
01191 
01192                 blkLoop.addFunctionCall(move_GxT_T3, evGx.getAddress(blk * NX), T3);
01193 
01194                 ExportForLoop colLoop2(col, row + 1, N);
01195                 colLoop2.addFunctionCall(multGxGu, T3, F.getAddress(col * NX), W1);
01196                 colLoop2 << (blkE == (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col));
01197                 colLoop2.addFunctionCall(mac_W1_T1_E_F, W1, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
01198                 blkLoop << colLoop2;
01199 
01200                 // Calculate one block-row of the factorized matrix
01201                 blkLoop.addFunctionCall(move_D_U, D, U, ExportIndex( row ));
01202                 ExportForLoop colLoop3(col, row + 1, N);
01203                 colLoop3 << (blkE == (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col));
01204                 colLoop3.addFunctionCall(mult_L_E_U, L, E.getAddress(blkE * NX), U, row, col);
01205                 blkLoop << colLoop3;
01206 
01207                 condensePrep << blkLoop;
01208                 condensePrep.release( col ).release( blkE ).release( blk ).release( row );
01209         }
01210 
01211         // Calculate the bottom-right block of the factorized Hessian
01212         condensePrep.addFunctionCall(
01213                         mac_R_BT_F_D, R1.getAddress(0 * NX), evGu.getAddress(0 * NX), F.getAddress((N - 1) * NX), D);
01214         condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
01215         condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( N - 1 ));
01216         condensePrep.addLinebreak( 2 );
01217 
01218 #endif
01219 
01221         //
01222         // Compute gradient components g0 and g1
01223         //
01225 
01227 
01228         LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
01229 
01230         {
01231                 condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
01232                 condenseFdb.addLinebreak();
01233         }
01234 
01235         condenseFdb.addStatement( Dy -= y );
01236         condenseFdb.addStatement( DyN -= yN );
01237         condenseFdb.addLinebreak();
01238 
01239         // Compute RDy, UPDATED
01240         for(unsigned blk = 0; blk < N; ++blk)
01241         {
01242                 if (R2.isGiven() == true)
01243                         condenseFdb.addFunctionCall(
01244                                         multRDy, R2,
01245                                         Dy.getAddress(blk * NY, 0),
01246                                         g.getAddress((N - 1 - blk) * NU, 0) );
01247                 else
01248                         condenseFdb.addFunctionCall(
01249                                         multRDy, R2.getAddress(blk * NU, 0),
01250                                         Dy.getAddress(blk * NY, 0),
01251                                         g.getAddress((N - 1 - blk) * NU, 0) );
01252         }
01253         condenseFdb.addLinebreak();
01254 
01255         // Compute QDy
01256         // NOTE: This is just for the MHE case :: run1 starts from 0; in MPC :: from 1 ;)
01257         for(unsigned blk = 0; blk < N; blk++ )
01258         {
01259                 if (Q2.isGiven() == true)
01260                         condenseFdb.addFunctionCall(
01261                                         multQDy, Q2,
01262                                         Dy.getAddress(blk * NY),
01263                                         QDy.getAddress(blk * NX) );
01264                 else
01265                         condenseFdb.addFunctionCall(
01266                                         multQDy, Q2.getAddress(blk * NX),
01267                                         Dy.getAddress(blk * NY),
01268                                         QDy.getAddress(blk * NX) );
01269         }
01270         condenseFdb.addLinebreak();
01271         condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == QN2 * DyN );
01272         condenseFdb.addLinebreak();
01273 
01274         /*
01275 
01276         OK, we need to adapt the old N^2 algorithm: u vector is in reverse order
01277 
01278         for k = 0: N - 1
01279                 g1_k = r_{N - 1 - k}; // OK, updated (look up)
01280 
01281         sbar_0 = Dx_0;
01282         sbar(1: N) = d;
01283 
01284         for k = 0: N - 1
01285                 sbar_{k + 1} += A_k sbar_k; // can stay the same
01286 
01287         w1 = Q_N^T * sbar_N + q_N;
01288         for k = N - 1: 1
01289         {
01290                 g1_{N - 1 - k} += B_k^T * w1;
01291                 w2 = A_k^T * w1 + q_k;
01292                 w1 = Q_k^T * sbar_k + w2;
01293         }
01294 
01295         g1_{N - 1} += B_0^T * w1;
01296 
01297         */
01298 
01299         sbar.setup("sbar", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
01300         w1.setup("w1", NX, 1, REAL, ACADO_WORKSPACE);
01301         w2.setup("w2", NX, 1, REAL, ACADO_WORKSPACE);
01302 
01303         condenseFdb.addStatement( sbar.getRows(0, NX) == Dx0 );
01304 
01305         if( performsSingleShooting() == false )
01306                 condensePrep.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
01307 
01308         for (unsigned i = 0; i < N; ++i)
01309                 condenseFdb.addFunctionCall(
01310                                 macASbar, evGx.getAddress(i * NX), sbar.getAddress(i * NX), sbar.getAddress((i + 1) * NX)
01311                 );
01312         condenseFdb.addLinebreak();
01313 
01314         condenseFdb.addStatement(
01315                         w1 == QDy.getRows(N * NX, (N + 1) * NX) + QN1 * sbar.getRows(N * NX, (N + 1) * NX)
01316         );
01317         for (unsigned i = N - 1; 0 < i; --i)
01318         {
01319                 condenseFdb.addFunctionCall(
01320                                 macBTw1, evGu.getAddress(i * NX), w1, g.getAddress((N - 1 - i) * NU) // UPDATED
01321                 );
01322                 condenseFdb.addFunctionCall(
01323                                 macATw1QDy, evGx.getAddress(i * NX), w1, QDy.getAddress(i * NX), w2 // Proveri indexiranje za QDy
01324                 );
01325                 if (Q1.isGiven() == true)
01326                         condenseFdb.addFunctionCall(
01327                                         macQSbarW2, Q1, sbar.getAddress(i * NX), w2, w1
01328                         );
01329                 else
01330                         condenseFdb.addFunctionCall(
01331                                         macQSbarW2, Q1.getAddress(i * NX), sbar.getAddress(i * NX), w2, w1
01332                         );
01333         }
01334         condenseFdb.addFunctionCall(
01335                         macBTw1, evGu.getAddress( 0 ), w1, g.getAddress((N - 1) * NU) // UPDATED
01336         );
01337         condenseFdb.addLinebreak();
01338 
01340 
01342         //
01343         // Expansion routine
01344         //
01346 
01347         /*
01348 
01349         // Step expansion, assuming that u_k is already updated
01350 
01351         Ds_0 = Dx_0;
01352         Ds_{1: N} = d;
01353 
01354         for i = 0: N - 1
01355         {
01356                 Ds_{k + 1} += A_k * Ds_k;       // Reuse multABarD
01357                 Ds_{k + 1} += B_k * Du_{N - 1 - k};
01358                 s_{k + 1}  += Ds_{k + 1};
01359         }
01360 
01361         // TODO Calculation of multipliers, modify the order of Ds and Du
01362 
01363         mu_N = lambda_N + Q_N^T * s_N
01364         for i = N - 1: 1
01365                 mu_k = lambda_k + Q_k^T * s_k + A_k^T * mu_{k + 1} + q_k
01366 
01367         mu_0 = Q_0^T s_0 + A_0^T * mu_1 + q_0
01368 
01369          */
01370 
01371         LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
01372 
01373         expand.setup( "expand" );
01374 
01375         if (performFullCondensing() == true)
01376         {
01377                 ExportVariable xVarsTranspose = xVars.getTranspose();
01378                 for (unsigned row = 0; row < N; ++row)
01379                         expand.addStatement( u.getRow( row ) += xVarsTranspose.getCols((N - 1 - row) * NU, (N - row) * NU) );
01380         }
01381         else
01382         {
01383                 // TODO, does not work yet.
01384                 expand.addStatement( x.makeColVector().getRows(0, NX) += xVars.getRows(0, NX) );
01385                 expand.addLinebreak();
01386                 expand.addStatement( u.makeColVector() += xVars.getRows(NX, getNumQPvars()) );
01387         }
01388 
01389         expand.addStatement( sbar.getRows(0, NX) == Dx0 );
01390         expand.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
01391 
01392         for (unsigned row = 0; row < N; ++row )
01393                 expand.addFunctionCall(
01394                                 expansionStep, evGx.getAddress(row * NX), evGu.getAddress(row * NX),
01395                                 xVars.getAddress((N - 1 - row) * NU), sbar.getAddress(row * NX), // UPDATED
01396                                 sbar.getAddress((row + 1) * NX)
01397                 );
01398 
01399         expand.addStatement( x.makeColVector() += sbar );
01400 
01401         return SUCCESSFUL_RETURN;
01402 }
01403 
01404 returnValue ExportGaussNewtonCn2Factorization::setupVariables( )
01405 {
01407         //
01408         // Make index vector for state constraints, reverse order
01409         //
01411 
01412         bool boxConIsFinite = false;
01413         xBoundsIdxRev.clear();
01414         xBoundsIdx.clear();
01415 
01416         DVector lbBox, ubBox;
01417         unsigned dimBox = xBounds.getNumPoints();
01418         for (int i = dimBox - 1; i >= 0; --i)
01419         {
01420                 lbBox = xBounds.getLowerBounds( i );
01421                 ubBox = xBounds.getUpperBounds( i );
01422 
01423                 if (isFinite( lbBox ) || isFinite( ubBox ))
01424                         boxConIsFinite = true;
01425 
01426                 // This is maybe not necessary
01427                 if (boxConIsFinite == false || i == 0)
01428                         continue;
01429 
01430                 for (unsigned j = 0; j < lbBox.getDim(); ++j)
01431                 {
01432                         if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
01433                         {
01434                                 xBoundsIdxRev.push_back(i * lbBox.getDim() + j);
01435                                 xBoundsIdx.push_back((dimBox - 1 - i) * lbBox.getDim() + j);
01436                         }
01437                 }
01438         }
01439 
01440         if (initialStateFixed() == true)
01441         {
01442                 x0.setup("x0",  NX, 1, REAL, ACADO_VARIABLES);
01443                 x0.setDoc( (std::string)"Current state feedback vector." );
01444                 Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
01445         }
01446 
01447         E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
01448         QE.setup("QE", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
01449         QGx.setup("QGx", N * NX, NX, REAL, ACADO_WORKSPACE);
01450 
01451         QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
01452 
01453         // Setup all QP stuff
01454 
01455         H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01456 
01457         // Stupid aliasing to avoid copying of data
01458         if (performFullCondensing() == true)
01459         {
01460                 H11 = H;
01461         }
01462         else
01463         {
01464                 H00 = H.getSubMatrix(0, NX, 0, NX);
01465                 H11 = H.getSubMatrix(NX, getNumQPvars(), NX, getNumQPvars());
01466         }
01467 
01468         H10.setup("H10", N * NU, NX, REAL, ACADO_WORKSPACE);
01469 
01470         A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01471 
01472         g.setup("g",  getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01473 
01474         if (performFullCondensing() == true)
01475         {
01476                 g1 = g;
01477         }
01478         else
01479         {
01480                 g0 = g.getRows(0, NX);
01481                 g1 = g.getRows(NX, getNumQPvars());
01482         }
01483 
01484         lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01485         ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01486 
01487         lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
01488         ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
01489 
01490         xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
01491         yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
01492 
01493         return SUCCESSFUL_RETURN;
01494 }
01495 
01496 returnValue ExportGaussNewtonCn2Factorization::setupMultiplicationRoutines( )
01497 {
01498         ExportIndex iCol( "iCol" );
01499         ExportIndex iRow( "iRow" );
01500 
01501         ExportVariable dp, dn, Gx1, Gx2, Gx3, Gu1, Gu2, Gu3;
01502         ExportVariable R22, Dy1, RDy1, Q22, QDy1, E1, U1, U2, H101, w11, w12, w13;
01503         dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
01504         dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
01505         Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
01506         Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
01507         Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
01508         Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
01509         Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
01510         Gu3.setup("Gu3", NX, NU, REAL, ACADO_LOCAL);
01511         R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
01512         Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
01513         RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
01514         Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
01515         QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
01516         E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
01517         U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
01518         U2.setup("U2", NU, 1, REAL, ACADO_LOCAL);
01519         H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
01520 
01521         w11.setup("w11", NX, 1, REAL, ACADO_LOCAL);
01522         w12.setup("w12", NX, 1, REAL, ACADO_LOCAL);
01523         w13.setup("w13", NX, 1, REAL, ACADO_LOCAL);
01524 
01525         if ( Q2.isGiven() )
01526                 Q22 = Q2;
01527         if ( R2.isGiven() )
01528                 R22 = R2;
01529 
01530         // multGxd; // d_k += Gx_k * d_{k-1}
01531         multGxd.setup("multGxd", dp, Gx1, dn);
01532         multGxd.addStatement( dn += Gx1 * dp );
01533         // moveGxT
01534         moveGxT.setup("moveGxT", Gx1, Gx2);
01535         moveGxT.addStatement( Gx2 == Gx1 );
01536         // multGxGx
01537         multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
01538         multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
01539         // multGxGu
01540         multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
01541         multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
01542         // moveGuE
01543         moveGuE.setup("moveGuE", Gu1, Gu2);
01544         moveGuE.addStatement( Gu2 == Gu1 );
01545 
01546         unsigned offset = (performFullCondensing() == true) ? 0 : NX;
01547 
01548         // setBlockH11
01549         setBlockH11.setup("setBlockH11", iRow, iCol, Gu1, Gu2);
01550         setBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) += (Gu1 ^ Gu2) );
01551         // zeroBlockH11
01552         zeroBlockH11.setup("zeroBlockH11", iRow, iCol);
01553         zeroBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == zeros<double>(NU, NU) );
01554         // copyHTH
01555         copyHTH.setup("copyHTH", iRow, iCol);
01556         copyHTH.addStatement(
01557                         H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
01558                                         H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
01559         );
01560         // multRDy
01561         multRDy.setup("multRDy", R22, Dy1, RDy1);
01562         multRDy.addStatement( RDy1 == R22 * Dy1 );
01563         // mult QDy1
01564         multQDy.setup("multQDy", Q22, Dy1, QDy1);
01565         multQDy.addStatement( QDy1 == Q22 * Dy1 );
01566         // multEQDy;
01567         multEQDy.setup("multEQDy", E1, QDy1, U1);
01568         multEQDy.addStatement( U1 += (E1 ^ QDy1) );
01569         // multQETGx
01570         multQETGx.setup("multQETGx", E1, Gx1, H101);
01571         multQETGx.addStatement( H101 += (E1 ^ Gx1) );
01572         // zerBlockH10
01573         zeroBlockH10.setup("zeroBlockH10", H101);
01574         zeroBlockH10.addStatement( H101 == zeros<double>(NU, NX) );
01575 
01576         if (performsSingleShooting() == false)
01577         {
01578                 // multEDu
01579                 multEDu.setup("multEDu", E1, U1, dn);
01580                 multEDu.addStatement( dn += E1 * U1 );
01581         }
01582 
01583         if (Q1.isGiven() == true)
01584         {
01585                 // multQ1Gx
01586                 multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
01587                 multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
01588 
01589                 // multQ1Gu
01590                 multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
01591                 multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
01592 
01593                 // multQ1d
01594                 multQ1d.setup("multQ1d", Q1, dp, dn);
01595                 multQ1d.addStatement( dn == Q1 * dp );
01596         }
01597         else
01598         {
01599                 // multQ1d
01600                 multQ1d.setup("multQ1d", Gx1, dp, dn);
01601                 multQ1d.addStatement( dn == Gx1 * dp );
01602         }
01603 
01604         if (QN1.isGiven() == BT_TRUE)
01605         {
01606                 // multQN1Gu
01607                 multQN1Gu.setup("multQN1Gu", Gu1, Gu2);
01608                 multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
01609 
01610                 // multQN1Gx
01611                 multQN1Gx.setup("multQN1Gx", Gx1, Gx2);
01612                 multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
01613         }
01614 
01615         if (performsSingleShooting() == BT_FALSE)
01616         {
01617                 // multQN1d
01618                 multQN1d.setup("multQN1d", QN1, dp, dn);
01619                 multQN1d.addStatement( dn == QN1 * dp );
01620         }
01621 
01622         if (performFullCondensing() == false)
01623         {
01624                 // zeroBlockH00
01625                 zeroBlockH00.setup( "zeroBlockH00" );
01626                 zeroBlockH00.addStatement( H00 == zeros<double>(NX, NX) );
01627 
01628                 // multCTQC
01629                 multCTQC.setup("multCTQC", Gx1, Gx2);
01630                 multCTQC.addStatement( H00 += (Gx1 ^ Gx2) );
01631         }
01632 
01633         // N2 condensing related
01634 
01635         //ExportFunction multBTW1, multGxTGu, macQEW2;
01636 
01637         // multBTW1, evGu.getAddress(row * NX), W1, row, col
01638         multBTW1.setup("multBTW1", Gu1, Gu2, iRow, iCol);
01639         multBTW1.addStatement(
01640                         H.getSubMatrix(iRow * NU, (iRow + 1) * NU, iCol * NU, (iCol + 1) * NU) ==
01641                                         (Gu1 ^ Gu2)
01642 //                                      (Gu2 ^ Gu1)
01643         );
01644 
01645         //
01646         // Define LM regularization terms
01647         //
01648         DMatrix mRegH00 = eye<double>( getNX() );
01649         DMatrix mRegH11 = eye<double>( getNU() );
01650 
01651         mRegH00 *= levenbergMarquardt;
01652         mRegH11 *= levenbergMarquardt;
01653 
01654         ExportVariable R11;
01655         if (R1.isGiven() == true)
01656                 R11 = R1;
01657         else
01658                 R11.setup("R11", NU, NU, REAL, ACADO_LOCAL);
01659 
01660         macBTW1_R1.setup("multBTW1_R1", R11, Gu1, Gu2, iRow);
01661         macBTW1_R1.addStatement(
01662                         H.getSubMatrix(iRow * NU, (iRow + 1) * NU, iRow * NU, (iRow + 1) * NU) ==
01663                                         R11 + (Gu1 ^ Gu2)
01664                         );
01665         macBTW1_R1.addStatement(
01666                         H.getSubMatrix(iRow * NU, (iRow + 1) * NU, iRow * NU, (iRow + 1) * NU) += mRegH11
01667         );
01668 
01669         multGxTGu.setup("multGxTGu", Gx1, Gu1, Gu2);
01670         multGxTGu.addStatement( Gu2 == (Gx1 ^ Gu1) );
01671 
01672         // macQEW2, Q1, E.getAddress((offest + row - col - 1) * NX), W2, W1
01673         ExportVariable Q11;
01674         if (Q1.isGiven())
01675                 Q11 = Q1;
01676         else
01677                 Q11.setup("Q11", NX, NX, REAL, ACADO_LOCAL);
01678 
01679 
01680         macQEW2.setup("multQEW2", Q11, Gu1, Gu2, Gu3);
01681         macQEW2.addStatement( Gu3 == Gu2 + Q11 * Gu1 );
01682 
01683         // ExportFunction macATw1Q, macBTw1, macQSbarW2, macASbarD;
01684         macASbar.setup("macASbar", Gx1, w11, w12);
01685         macASbar.addStatement( w12 += Gx1 * w11 );
01686 
01687 //      macASbarD2.setup("macASbarD2", Gx1, w11, w12, w13);
01688 //      macASbarD2.addStatement( w13 == Gx1 * w11 );
01689 //      macASbarD2.addStatement( w13 -= w12 );
01690 
01691         macBTw1.setup("macBTw1", Gu1, w11,      U1);
01692         macBTw1.addStatement( U1 += Gu1 ^ w11 );
01693 
01694         // macATw1QDy, A.getAddress(i * NX), w1, QDy.getAddress(i * NX), w2
01695         macATw1QDy.setup("macATw1QDy", Gx1, w11, w12, w13);
01696         macATw1QDy.addStatement( w13 == w12 + (Gx1 ^ w11) );
01697 
01698         // macQSbarW2, Q1, sbar.getAddress(i * NX), w2, w1
01699         macQSbarW2.setup("macQSbarW2", Q11, w11, w12, w13);
01700         macQSbarW2.addStatement( w13 == w12 + Q11 * w11 );
01701 
01702         expansionStep.setup("expansionStep", Gx1, Gu1, U1, w11, w12);
01703         expansionStep.addStatement( w12 += Gx1 * w11 );
01704         expansionStep.addStatement( w12 += Gu1 * U1 );
01705 
01706         //
01707         // Hessian factorization helper routines
01708         //
01709 
01710         ExportVariable D1("D", NU, NU, REAL, ACADO_LOCAL);
01711         ExportVariable L1("L", NU, NX, REAL, ACADO_LOCAL);
01712         ExportVariable H1("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_LOCAL);
01713 
01714         ExportVariable T11("T11", NX, NX, REAL, ACADO_LOCAL);
01715         ExportVariable T22("T22", NU, NX, REAL, ACADO_LOCAL);
01716         ExportVariable T33("T33", NX, NX, REAL, ACADO_LOCAL);
01717 
01718 //      ExportFunction mult_BT_T1_T2;
01719         mult_BT_T1_T2.setup("mult_BT_T1_T2", Gu1, T11, T22);
01720         mult_BT_T1_T2.addStatement( T22 == (Gu1 ^ T11) );
01721 
01722 //      ExportFunction mul_T2_A_L;
01723         mul_T2_A_L.setup("mul_T2_A_L", T22, Gx1, L1);
01724         mul_T2_A_L.addStatement( L1 == T22 * Gx1 );
01725 
01726 //      ExportFunction mac_R_T2_B_D;
01727         mac_R_T2_B_D.setup("mac_R_T2_B_D", R11, T22, Gu1, D1);
01728         mac_R_T2_B_D.addStatement( D1 == R11 + T22 * Gu1 );
01729 
01730 //      ExportFunction move_D_H;
01731         move_D_U.setup("move_D_H", D1, H1, iRow);
01732         move_D_U.addStatement( H1.getSubMatrix(iRow * NU, (iRow + 1) * NU, iRow * NU, (iRow + 1) * NU) == D1 );
01733 
01734 //      ExportFunction mult_L_E_H;
01735         mult_L_E_U.setup("mult_L_E_H", L1, Gu1, H1, iRow, iCol);
01736         mult_L_E_U.addStatement(
01737                         H1.getSubMatrix(iRow * NU, (iRow + 1) * NU, iCol * NU, (iCol + 1) * NU) == L1 * Gu1
01738         );
01739 
01740 //      ExportFunction updateQ; T1 = Q( blk ) + A^T * T1 * A - L^T * L
01741         // Q1, T3, evGx.getAddress(blk * NX), L, T1);
01742         updateQ.setup("updateQ", Q11, T33, Gx1, L1, T11);
01743         updateQ.addStatement( T33 == (Gx1 ^ T11) );
01744         updateQ.addStatement( T11 == Q11 + T33 * Gx1 );
01745         updateQ.addStatement( T11 -= (L1 ^ L1) );
01746 
01747 //      ExportFunction mac_R_BT_F_D, mult_FT_A_L;
01748 //      ExportFunction updateQ2;
01749 //      ExportFunction mac_W1_T1_E_F;
01750 
01751         mac_R_BT_F_D.setup("mac_R_BT_F_D", R11, Gu1, Gu2, D1);
01752         mac_R_BT_F_D.addStatement( D1 == R11 + (Gu1 ^ Gu2) );
01753 
01754         mult_FT_A_L.setup("mult_FT_A_L", Gu1, Gx1, L1);
01755         mult_FT_A_L.addStatement( L1 == (Gu1 ^ Gx1) );
01756 
01757         updateQ2.setup("updateQ2", Q11, L1, T11);
01758         // TODO T11 == Q11 - (L1^L1) does not work properly
01759         //      Revisit this...
01760         updateQ2.addStatement( T11 == Q11 );
01761         updateQ2.addStatement( T11 -= (L1 ^ L1) );
01762 
01763         mac_W1_T1_E_F.setup("mac_W1_T1_E_F", Gu1, Gx1, Gu2, Gu3);
01764         mac_W1_T1_E_F.addStatement( Gu3 == Gu1 + Gx1 * Gu2 );
01765 
01766         move_GxT_T3.setup("move_GxT_T3", Gx1, Gx2);
01767         move_GxT_T3.addStatement( Gx2 == Gx1.getTranspose() );
01768 
01769         return SUCCESSFUL_RETURN;
01770 }
01771 
01772 returnValue ExportGaussNewtonCn2Factorization::setupEvaluation( )
01773 {
01775         //
01776         // Preparation phase
01777         //
01779 
01780         preparation.setup( "preparationStep" );
01781         preparation.doc( "Preparation step of the RTI scheme." );
01782         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
01783         retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
01784         preparation.setReturnValue(retSim, false);
01785 
01786         preparation     << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
01787 
01788         preparation.addFunctionCall( evaluateObjective );
01789         preparation.addFunctionCall( condensePrep );
01790 
01792         //
01793         // Feedback phase
01794         //
01796 
01797         ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
01798         tmp.setDoc( "Status code of the qpOASES QP solver." );
01799 
01800         ExportFunction solve("solve");
01801         solve.setReturnValue( tmp );
01802 
01803         feedback.setup("feedbackStep");
01804         feedback.doc( "Feedback/estimation step of the RTI scheme." );
01805         feedback.setReturnValue( tmp );
01806 
01807         feedback.addFunctionCall( condenseFdb );
01808         feedback.addLinebreak();
01809 
01810         stringstream s;
01811         s << tmp.getName() << " = " << solve.getName() << "( );" << endl;
01812         feedback <<  s.str();
01813         feedback.addLinebreak();
01814 
01815         feedback.addFunctionCall( expand );
01816 
01818         //
01819         // Setup evaluation of the KKT tolerance
01820         //
01822 
01823         ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
01824         ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
01825         ExportIndex index( "index" );
01826 
01827         getKKT.setup( "getKKT" );
01828         getKKT.doc( "Get the KKT tolerance of the current iterate." );
01829         kkt.setDoc( "The KKT tolerance value." );
01830         getKKT.setReturnValue( kkt );
01831         getKKT.addVariable( prd );
01832         getKKT.addIndex( index );
01833 
01834         // ACC = |\nabla F^T * xVars|
01835         getKKT.addStatement( kkt == (g ^ xVars) );
01836         getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
01837 
01838         ExportForLoop bLoop(index, 0, getNumQPvars());
01839 
01840         bLoop.addStatement( prd == yVars.getRow( index ) );
01841         bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
01842         bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
01843         bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
01844         bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
01845         getKKT.addStatement( bLoop );
01846 
01847         if ((getNumStateBounds() + getNumComplexConstraints())> 0)
01848         {
01849                 ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
01850 
01851                 cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ));
01852                 cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
01853                 cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
01854                 cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
01855                 cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
01856 
01857                 getKKT.addStatement( cLoop );
01858         }
01859 
01860         return SUCCESSFUL_RETURN;
01861 }
01862 
01863 returnValue ExportGaussNewtonCn2Factorization::setupQPInterface( )
01864 {
01865         string folderName;
01866         get(CG_EXPORT_FOLDER_NAME, folderName);
01867         string moduleName;
01868         get(CG_MODULE_NAME, moduleName);
01869         std::string sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
01870         std::string headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
01871 
01872         int useSinglePrecision;
01873         get(USE_SINGLE_PRECISION, useSinglePrecision);
01874 
01875         int hotstartQP;
01876         get(HOTSTART_QP, hotstartQP);
01877 
01878         int covCalc;
01879         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
01880 
01881         int maxNumQPiterations;
01882         get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
01883 
01884         int externalCholesky;
01885         get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
01886 
01887         //
01888         // Set up export of the source file
01889         //
01890 
01891         ExportQpOasesInterface qpInterface = ExportQpOasesInterface(headerFile, sourceFile, "");
01892 
01893         qpInterface.configure(
01894                         "",
01895                         "QPOASES_HEADER",
01896                         getNumQPvars(),
01897                         getNumStateBounds() + getNumComplexConstraints(),
01898                         maxNumQPiterations,
01899                         "PL_NONE",
01900                         useSinglePrecision,
01901 
01902                         commonHeaderName,
01903                         "",
01904                         xVars.getFullName(),
01905                         yVars.getFullName(),
01906                         "", // TODO
01907 //                      sigma.getFullName(),
01908                         hotstartQP,
01909                         (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL,
01910                         H.getFullName(),
01911                         U.getFullName(),
01912                         g.getFullName(),
01913                         A.getFullName(),
01914                         lb.getFullName(),
01915                         ub.getFullName(),
01916                         lbA.getFullName(),
01917                         ubA.getFullName()
01918         );
01919 
01920         return qpInterface.exportCode();
01921 }
01922 
01923 bool ExportGaussNewtonCn2Factorization::performFullCondensing() const
01924 {
01925         int sparseQPsolution;
01926         get(SPARSE_QP_SOLUTION, sparseQPsolution);
01927 
01928         if ((SparseQPsolutionMethods)sparseQPsolution == CONDENSING)
01929                 return false;
01930 
01931         return true;
01932 }
01933 
01934 CLOSE_NAMESPACE_ACADO

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