export_gauss_newton_condensed.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_condensed.hpp>
00033 #include <acado/code_generation/export_qpoases_interface.hpp>
00034 #include <acado/code_generation/export_module.hpp>
00035 
00036 using namespace std;
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 ExportGaussNewtonCondensed::ExportGaussNewtonCondensed( UserInteraction* _userInteraction,
00041                                                                                                                 const std::string& _commonHeaderName
00042                                                                                                                 ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00043 {}
00044 
00045 returnValue ExportGaussNewtonCondensed::setup( )
00046 {
00047         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00048         setupInitialization();
00049         LOG( LVL_DEBUG ) << "done!" << endl;
00050 
00051         LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
00052         setupVariables();
00053         LOG( LVL_DEBUG ) << "done!" << endl;
00054 
00055         LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
00056         setupMultiplicationRoutines();
00057         LOG( LVL_DEBUG ) << "done!" << endl;
00058 
00059         LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
00060         setupSimulation();
00061         LOG( LVL_DEBUG ) << "done!" << endl;
00062 
00063         LOG( LVL_DEBUG ) << "Solver: setup arrival cost update... " << endl;
00064         setupArrivalCostCalculation();
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 ExportGaussNewtonCondensed::getDataDeclarations(    ExportStatementBlock& declarations,
00091                                                                                                                                 ExportStruct dataStruct
00092                                                                                                                                 ) const
00093 {
00094         returnValue status;
00095         status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00096         if (status != SUCCESSFUL_RETURN)
00097                 return status;
00098 
00099         declarations.addDeclaration(x0, dataStruct);
00100         declarations.addDeclaration(Dx0, dataStruct);
00101 
00102         declarations.addDeclaration(sigmaN, dataStruct);
00103 
00104         declarations.addDeclaration(T, dataStruct);
00105         declarations.addDeclaration(E, dataStruct);
00106         declarations.addDeclaration(QE, dataStruct);
00107         declarations.addDeclaration(QGx, dataStruct);
00108         declarations.addDeclaration(Qd, dataStruct);
00109         declarations.addDeclaration(QDy, dataStruct);
00110         declarations.addDeclaration(H10, dataStruct);
00111 
00112         declarations.addDeclaration(lbValues, dataStruct);
00113         declarations.addDeclaration(ubValues, dataStruct);
00114         declarations.addDeclaration(lbAValues, dataStruct);
00115         declarations.addDeclaration(ubAValues, dataStruct);
00116 
00117         if (performFullCondensing() == true)
00118                 declarations.addDeclaration(A10, dataStruct);
00119         declarations.addDeclaration(A20, dataStruct);
00120 
00121         declarations.addDeclaration(pacA01Dx0, dataStruct);
00122         declarations.addDeclaration(pocA02Dx0, dataStruct);
00123 
00124         declarations.addDeclaration(CEN, dataStruct);
00125         declarations.addDeclaration(sigmaTmp, dataStruct);
00126         declarations.addDeclaration(sigma, dataStruct);
00127 
00128         declarations.addDeclaration(H, dataStruct);
00129         declarations.addDeclaration(R, dataStruct);
00130         declarations.addDeclaration(A, dataStruct);
00131         declarations.addDeclaration(g, dataStruct);
00132         declarations.addDeclaration(lb, dataStruct);
00133         declarations.addDeclaration(ub, dataStruct);
00134         declarations.addDeclaration(lbA, dataStruct);
00135         declarations.addDeclaration(ubA, dataStruct);
00136         declarations.addDeclaration(xVars, dataStruct);
00137         declarations.addDeclaration(yVars, dataStruct);
00138 
00139         return SUCCESSFUL_RETURN;
00140 }
00141 
00142 returnValue ExportGaussNewtonCondensed::getFunctionDeclarations(        ExportStatementBlock& declarations
00143                                                                                                                                         ) const
00144 {
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( updateArrivalCost );
00157 
00158         declarations.addDeclaration( evaluateStageCost );
00159         declarations.addDeclaration( evaluateTerminalCost );
00160 
00161         return SUCCESSFUL_RETURN;
00162 }
00163 
00164 returnValue ExportGaussNewtonCondensed::getCode(        ExportStatementBlock& code
00165                                                                                                                 )
00166 {
00167         setupQPInterface();
00168 
00169         code.addLinebreak( 2 );
00170         code.addStatement( "/******************************************************************************/\n" );
00171         code.addStatement( "/*                                                                            */\n" );
00172         code.addStatement( "/* ACADO code generation                                                      */\n" );
00173         code.addStatement( "/*                                                                            */\n" );
00174         code.addStatement( "/******************************************************************************/\n" );
00175         code.addLinebreak( 2 );
00176 
00177         int useOMP;
00178         get(CG_USE_OPENMP, useOMP);
00179         if ( useOMP )
00180         {
00181                 code.addDeclaration( state );
00182         }
00183 
00184         code.addFunction( modelSimulation );
00185 
00186         code.addFunction( evaluateStageCost );
00187         code.addFunction( evaluateTerminalCost );
00188         code.addFunction( setObjQ1Q2 );
00189         code.addFunction( setObjR1R2 );
00190         code.addFunction( setObjQN1QN2 );
00191         code.addFunction( evaluateObjective );
00192 
00193         code.addFunction( multGxd );
00194         code.addFunction( moveGxT );
00195         code.addFunction( multGxGx );
00196         code.addFunction( multGxGu );
00197         code.addFunction( moveGuE );
00198         code.addFunction( setBlockH11 );
00199         code.addFunction( setBlockH11_R1 );
00200         code.addFunction( zeroBlockH11 );
00201         code.addFunction( copyHTH );
00202         code.addFunction( multQ1d );
00203         code.addFunction( multQN1d );
00204         code.addFunction( multRDy );
00205         code.addFunction( multQDy );
00206         code.addFunction( multEQDy );
00207         code.addFunction( multQETGx );
00208         code.addFunction( zeroBlockH10 );
00209         code.addFunction( multEDu );
00210         code.addFunction( multQ1Gx );
00211         code.addFunction( multQN1Gx );
00212         code.addFunction( multQ1Gu );
00213         code.addFunction( multQN1Gu );
00214         code.addFunction( zeroBlockH00 );
00215         code.addFunction( multCTQC );
00216 
00217         code.addFunction( multHxC );
00218         code.addFunction( multHxE );
00219         code.addFunction( macHxd );
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( macCTSlx );
00231         code.addFunction( macETSlu );
00232 
00233         cholSolver.getCode( code );
00234 
00235         code.addFunction( condensePrep );
00236         code.addFunction( condenseFdb );
00237         code.addFunction( expand );
00238         code.addFunction( calculateCovariance );
00239 
00240         code.addFunction( preparation );
00241         code.addFunction( feedback );
00242 
00243         code.addFunction( initialize );
00244         code.addFunction( initializeNodes );
00245         code.addFunction( shiftStates );
00246         code.addFunction( shiftControls );
00247         code.addFunction( getKKT );
00248         code.addFunction( getObjective );
00249 
00250         int useArrivalCost;
00251         get(CG_USE_ARRIVAL_COST, useArrivalCost);
00252         if (useArrivalCost == YES)
00253         {
00254                 acSolver.getCode( code );
00255                 cholObjS.getCode( code );
00256                 cholSAC.getCode( code );
00257                 code.addFunction( updateArrivalCost );
00258         }
00259 
00260         return SUCCESSFUL_RETURN;
00261 }
00262 
00263 
00264 unsigned ExportGaussNewtonCondensed::getNumQPvars( ) const
00265 {
00266         if (performFullCondensing() == true)
00267                 return (N * NU);
00268 
00269         return (NX + N * NU);
00270 }
00271 
00272 unsigned ExportGaussNewtonCondensed::getNumStateBounds() const
00273 {
00274         return xBoundsIdx.size();
00275 }
00276 
00277 //
00278 // PROTECTED FUNCTIONS:
00279 //
00280 
00281 returnValue ExportGaussNewtonCondensed::setupObjectiveEvaluation( void )
00282 {
00283         evaluateObjective.setup("evaluateObjective");
00284 
00285         int variableObjS;
00286         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00287 
00288         if (S1.getGivenMatrix().isZero() == false)
00289                 ACADOWARNINGTEXT(RET_INVALID_ARGUMENTS,
00290                                 "Mixed control-state terms in the objective function are not supported at the moment.");
00291 
00292         //
00293         // A loop the evaluates objective and corresponding gradients
00294         //
00295         ExportIndex runObj( "runObj" );
00296         ExportForLoop loopObjective( runObj, 0, N );
00297 
00298         evaluateObjective.addIndex( runObj );
00299 
00300         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00301         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00302         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00303         loopObjective.addLinebreak( );
00304 
00305         // Evaluate the objective function
00306         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00307 
00308         // Stack the measurement function value
00309         loopObjective.addStatement(
00310                         Dy.getRows(runObj * NY, (runObj + 1) * NY) ==  objValueOut.getTranspose().getRows(0, getNY())
00311         );
00312         loopObjective.addLinebreak( );
00313 
00314         // Optionally compute derivatives
00315         unsigned indexX = getNY();
00316 //      unsigned indexG = indexX;
00317 
00318         ExportVariable tmpObjS, tmpFx, tmpFu;
00319         ExportVariable tmpFxEnd, tmpObjSEndTerm;
00320         tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00321         if (objS.isGiven() == true)
00322                 tmpObjS = objS;
00323         tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00324         if (objEvFx.isGiven() == true)
00325                 tmpFx = objEvFx;
00326         tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00327         if (objEvFu.isGiven() == true)
00328                 tmpFu = objEvFu;
00329         tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00330         if (objEvFxEnd.isGiven() == true)
00331                 tmpFxEnd = objEvFxEnd;
00332         tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00333         if (objSEndTerm.isGiven() == true)
00334                 tmpObjSEndTerm = objSEndTerm;
00335 
00336         //
00337         // Optional computation of Q1, Q2
00338         //
00339         if (Q1.isGiven() == false)
00340         {
00341                 ExportVariable tmpQ1, tmpQ2;
00342                 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00343                 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00344 
00345                 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00346                 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00347                 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00348 
00349                 if (tmpFx.isGiven() == true)
00350                 {
00351                         if (variableObjS == YES)
00352                         {
00353                                 loopObjective.addFunctionCall(
00354                                                 setObjQ1Q2,
00355                                                 tmpFx, objS.getAddress(runObj * NY, 0),
00356                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00357                                 );
00358                         }
00359                         else
00360                         {
00361                                 loopObjective.addFunctionCall(
00362                                                 setObjQ1Q2,
00363                                                 tmpFx, objS,
00364                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00365                                 );
00366                         }
00367                 }
00368                 else
00369                 {
00370                         if (variableObjS == YES)
00371                         {
00372                                 loopObjective.addFunctionCall(
00373                                                 setObjQ1Q2,
00374                                                 objValueOut.getAddress(0, indexX), 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                                                 objValueOut.getAddress(0, indexX), objS,
00383                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00384                                 );
00385                         }
00386                         indexX += objEvFx.getDim();
00387                 }
00388 
00389                 loopObjective.addLinebreak( );
00390         }
00391 
00392         if (R1.isGiven() == false)
00393         {
00394                 ExportVariable tmpR1, tmpR2;
00395                 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00396                 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00397 
00398                 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00399                 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00400                 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00401 
00402                 if (tmpFu.isGiven() == true)
00403                 {
00404                         if (variableObjS == YES)
00405                         {
00406                                 loopObjective.addFunctionCall(
00407                                                 setObjR1R2,
00408                                                 tmpFu, objS.getAddress(runObj * NY, 0),
00409                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00410                                 );
00411                         }
00412                         else
00413                         {
00414                                 loopObjective.addFunctionCall(
00415                                                 setObjR1R2,
00416                                                 tmpFu, objS,
00417                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00418                                 );
00419                         }
00420                 }
00421                 else
00422                 {
00423                         if (variableObjS == YES)
00424                         {
00425                                 loopObjective.addFunctionCall(
00426                                                 setObjR1R2,
00427                                                 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00428                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00429                                 );
00430                         }
00431                         else
00432                         {
00433                                 loopObjective.addFunctionCall(
00434                                                 setObjR1R2,
00435                                                 objValueOut.getAddress(0, indexX), objS,
00436                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00437                                 );
00438                         }
00439                 }
00440 
00441                 loopObjective.addLinebreak( );
00442         }
00443 
00444         evaluateObjective.addStatement( loopObjective );
00445 
00446         //
00447         // Evaluate the quadratic Mayer term
00448         //
00449         evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00450         evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00451 
00452         // Evaluate the objective function, last node.
00453         evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00454         evaluateObjective.addLinebreak( );
00455 
00456         evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00457         evaluateObjective.addLinebreak();
00458 
00459         if (QN1.isGiven() == false)
00460         {
00461                 indexX = getNYN();
00462 
00463                 ExportVariable tmpQN1, tmpQN2;
00464                 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00465                 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00466 
00467                 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00468                 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00469                 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00470 
00471                 if (tmpFxEnd.isGiven() == true)
00472                         evaluateObjective.addFunctionCall(
00473                                         setObjQN1QN2,
00474                                         tmpFxEnd, objSEndTerm,
00475                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00476                         );
00477                 else
00478                         evaluateObjective.addFunctionCall(
00479                                         setObjQN1QN2,
00480                                         objValueOut.getAddress(0, indexX), objSEndTerm,
00481                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00482                         );
00483 
00484                 evaluateObjective.addLinebreak( );
00485         }
00486 
00487         return SUCCESSFUL_RETURN;
00488 }
00489 
00490 returnValue ExportGaussNewtonCondensed::setupConstraintsEvaluation( void )
00491 {
00492         ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
00493 
00494         int hardcodeConstraintValues;
00495         get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00496 
00498         //
00499         // Determine dimensions of constraints
00500         //
00502 
00503         unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
00504         unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
00505         unsigned numStateBounds = getNumStateBounds();
00506         unsigned numPathCon = N * dimPacH;
00507         unsigned numPointCon = dimPocH;
00508 
00510         //
00511         // Setup the bounds on independent variables
00512         //
00514 
00515         DVector lbBoundValues( numBounds );
00516         DVector ubBoundValues( numBounds );
00517 
00518         if (initialStateFixed( ) == false)
00519                 for(unsigned el = 0; el < NX; ++el)
00520                 {
00521                         lbBoundValues( el )= xBounds.getLowerBound(0, el);
00522                         ubBoundValues( el ) = xBounds.getUpperBound(0, el);
00523                 }
00524 
00525         for(unsigned node = 0; node < N; ++node)
00526                 for(unsigned el = 0; el < NU; ++el)
00527                 {
00528                         lbBoundValues(offsetBounds + node * NU + el) = uBounds.getLowerBound(node, el);
00529                         ubBoundValues(offsetBounds + node * NU + el) = uBounds.getUpperBound(node, el);
00530                 }
00531 
00532         if (hardcodeConstraintValues == YES || !(isFinite( lbBoundValues ) || isFinite( ubBoundValues )))
00533         {
00534                 lbValues.setup("lbValues", lbBoundValues, REAL, ACADO_VARIABLES);
00535                 ubValues.setup("ubValues", ubBoundValues, REAL, ACADO_VARIABLES);
00536         }
00537         else if (isFinite( lbBoundValues ) || isFinite( ubBoundValues ))
00538         {
00539                 lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
00540                 lbValues.setDoc( "Lower bounds values." );
00541                 ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
00542                 ubValues.setDoc( "Upper bounds values." );
00543 
00544                 initialize.addStatement( lbValues == lbBoundValues );
00545                 initialize.addStatement( ubValues == ubBoundValues );
00546         }
00547 
00548         ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
00549 
00550         if (performFullCondensing() == true)
00551         {
00552                 // Full condensing case
00553                 boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
00554                 boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
00555         }
00556         else
00557         {
00558                 // Partial condensing case
00559                 if (initialStateFixed() == true)
00560                 {
00561                         // MPC case
00562                         condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
00563                         condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
00564 
00565                         boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
00566                         boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
00567                 }
00568                 else
00569                 {
00570                         // MHE case
00571                         boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00572                         boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00573                         boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
00574                         boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
00575                 }
00576         }
00577         boundSetFcn->addLinebreak( );
00578 
00580         //
00581         // Determine number of affine constraints and set up structures that
00582         // holds constraint values
00583         //
00585 
00586         unsigned sizeA = numStateBounds + getNumComplexConstraints();
00587 
00588         if ( sizeA )
00589         {
00590                 DVector lbTmp, ubTmp;
00591 
00592                 if ( numStateBounds )
00593                 {
00594                         DVector lbStateBoundValues( numStateBounds );
00595                         DVector ubStateBoundValues( numStateBounds );
00596                         for (unsigned i = 0; i < numStateBounds; ++i)
00597                         {
00598                                 lbStateBoundValues( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
00599                                 ubStateBoundValues( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
00600                         }
00601 
00602                         lbTmp.append( lbStateBoundValues );
00603                         ubTmp.append( ubStateBoundValues );
00604                 }
00605 
00606                 lbTmp.append( lbPathConValues );
00607                 ubTmp.append( ubPathConValues );
00608 
00609                 lbTmp.append( lbPointConValues );
00610                 ubTmp.append( ubPointConValues );
00611 
00612                 if (hardcodeConstraintValues == true)
00613                 {
00614                         lbAValues.setup("lbAValues", lbTmp, REAL, ACADO_VARIABLES);
00615                         ubAValues.setup("ubAValues", ubTmp, REAL, ACADO_VARIABLES);
00616                 }
00617                 else
00618                 {
00619                         lbAValues.setup("lbAValues", sizeA, 1, REAL, ACADO_VARIABLES);
00620                         lbAValues.setDoc( "Lower bounds values for affine constraints." );
00621                         ubAValues.setup("ubAValues", sizeA, 1, REAL, ACADO_VARIABLES);
00622                         ubAValues.setDoc( "Upper bounds values for affine constraints." );
00623 
00624                         initialize.addStatement( lbAValues == lbTmp );
00625                         initialize.addStatement( ubAValues == ubTmp );
00626                 }
00627         }
00628 
00630         //
00631         // Setup the bounds on state variables
00632         //
00634 
00635         if ( numStateBounds )
00636         {
00637                 condenseFdb.addVariable( tmp );
00638 
00639                 unsigned offset = (performFullCondensing() == true) ? 0 : NX;
00640                 unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
00641 
00642                 if (numOps < 1024)
00643                 {
00644                         for(unsigned row = 0; row < numStateBounds; ++row)
00645                         {
00646                                 unsigned conIdx = xBoundsIdx[ row ] - NX;
00647 
00648                                 if (performFullCondensing() == false)
00649                                         condensePrep.addStatement( A.getSubMatrix(row, row + 1, 0, NX) == evGx.getRow( conIdx ) );
00650 
00651                                 unsigned blk = conIdx / NX + 1;
00652                                 for (unsigned col = 0; col < blk; ++col)
00653                                 {
00654                                         unsigned blkRow = (blk * (blk - 1) / 2 + col) * NX + conIdx % NX;
00655 
00656                                         condensePrep.addStatement(
00657                                                         A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
00658                                 }
00659 
00660                                 condensePrep.addLinebreak();
00661                         }
00662                 }
00663                 else
00664                 {
00665                         DMatrix vXBoundIndices(1, numStateBounds);
00666                         for (unsigned i = 0; i < numStateBounds; ++i)
00667                                 vXBoundIndices(0, i) = xBoundsIdx[ i ];
00668                         ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
00669 
00670                         condensePrep.addVariable( evXBounds );
00671 
00672                         ExportIndex row, col, conIdx, blk, blkRow;
00673 
00674                         condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );
00675 
00676                         ExportForLoop lRow(row, 0, numStateBounds);
00677 
00678                         lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
00679                         lRow.addStatement( blk == conIdx / NX + 1);
00680 
00681                         if (performFullCondensing() == false)
00682                                 lRow.addStatement( A.getSubMatrix(row, row + 1, 0, NX) == evGx.getRow( conIdx ) );
00683 
00684                         ExportForLoop lCol(col, 0, blk);
00685 
00686                         lCol.addStatement( blkRow == (blk * (blk - 1) / 2 + col ) * NX + conIdx % NX );
00687                         lCol.addStatement(
00688                                         A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
00689 
00690                         lRow.addStatement( lCol );
00691                         condensePrep.addStatement( lRow );
00692 
00693                         condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
00694                 }
00695                 condensePrep.addLinebreak( );
00696 
00697                 // shift constraint bounds by first interval
00698                 for(unsigned run1 = 0; run1 < numStateBounds; ++run1)
00699                 {
00700                         unsigned row = xBoundsIdx[ run1 ];
00701 
00702                         if (performFullCondensing() == true)
00703                         {
00704                                 if (performsSingleShooting() == true)
00705                                 {
00706                                         condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + evGx.getRow(row - NX) * Dx0 );
00707                                 }
00708                                 else
00709                                 {
00710                                         condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + evGx.getRow(row - NX) * Dx0 );
00711                                         condenseFdb.addStatement( tmp += d.getRow(row - NX) );
00712                                 }
00713                                 condenseFdb.addStatement( lbA.getRow( run1 ) == lbAValues.getRow( run1 ) - tmp );
00714                                 condenseFdb.addStatement( ubA.getRow( run1 ) == ubAValues.getRow( run1 ) - tmp );
00715                         }
00716                         else
00717                         {
00718                                 if (performsSingleShooting() == true)
00719                                         condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) );
00720                                 else
00721                                         condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + d.getRow(row - NX) );
00722 
00723                                 condenseFdb.addStatement( lbA.getRow( run1 ) == lbAValues.getRow( run1 ) - tmp );
00724                                 condenseFdb.addStatement( ubA.getRow( run1 ) == ubAValues.getRow( run1 ) - tmp );
00725                         }
00726                 }
00727                 condenseFdb.addLinebreak( );
00728         }
00729 
00731         //
00732         // Setup the evaluation of the path constraints
00733         //
00735 
00736         if (getNumComplexConstraints() == 0)
00737                 return SUCCESSFUL_RETURN;
00738 
00739         if ( numPathCon )
00740         {
00741                 unsigned rowOffset = numStateBounds;
00742                 unsigned colOffset = performFullCondensing() == true ? 0 : NX;
00743 
00744                 //
00745                 // Setup evaluation
00746                 //
00747                 ExportIndex runPac;
00748                 condensePrep.acquire( runPac );
00749                 ExportForLoop loopPac(runPac, 0, N);
00750 
00751                 loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
00752                 loopPac.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( runPac ) );
00753                 loopPac.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runPac ) );
00754                 loopPac.addFunctionCall( evaluatePathConstraints.getName(), conValueIn, conValueOut );
00755 
00756                 loopPac.addStatement( pacEvH.getRows( runPac * dimPacH, (runPac + 1) * dimPacH) ==
00757                                 conValueOut.getTranspose().getRows(0, dimPacH) );
00758                 loopPac.addLinebreak( );
00759 
00760                 unsigned derOffset = dimPacH;
00761 
00762                 // Optionally store derivatives
00763                 if ( pacEvHx.isGiven() == false )
00764                 {
00765                         loopPac.addStatement(
00766                                         pacEvHx.makeRowVector().
00767                                         getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX)
00768                                         == conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
00769                         );
00770 
00771                         derOffset = derOffset + dimPacH * NX;
00772                 }
00773                 if (pacEvHu.isGiven() == false )
00774                 {
00775                         loopPac.addStatement(
00776                                         pacEvHu.makeRowVector().
00777                                         getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU)
00778                                         == conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
00779                         );
00780                 }
00781 
00782                 // Add loop to the function.
00783                 condensePrep.addStatement( loopPac );
00784                 condensePrep.addLinebreak( );
00785 
00786                 // Define the multHxC multiplication routine
00787                 ExportVariable tmpA01, tmpHx, tmpGx;
00788 
00789                 if (pacEvHx.isGiven() == true)
00790                         tmpHx = pacEvHx;
00791                 else
00792                         tmpHx.setup("Hx", dimPacH, NX, REAL, ACADO_LOCAL);
00793 
00794                 tmpGx.setup("Gx", NX, NX, REAL, ACADO_LOCAL);
00795 
00796                 if (performFullCondensing() == true)
00797                 {
00798                         tmpA01.setup("A01", dimPacH, NX, REAL, ACADO_LOCAL);
00799 
00800                         multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
00801                         multHxC.addStatement( tmpA01 == tmpHx * tmpGx );
00802 
00803                         A10.setup("A01", numPathCon, NX, REAL, ACADO_WORKSPACE);
00804                 }
00805                 else
00806                 {
00807                         tmpA01.setup("A01", dimPacH, getNumQPvars(), REAL, ACADO_LOCAL);
00808                         multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
00809                         multHxC.addStatement( tmpA01.getSubMatrix(0, dimPacH, 0, NX) == tmpHx * tmpGx );
00810 
00811                         A10 = A;
00812                 }
00813 
00814                 unsigned offsetA01 = (performFullCondensing() == true) ? 0 : rowOffset;
00815 
00816                 // Define the block A_{10}(0: dimPacH, 0: NX) = H_{x}(0: dimPacH, 0: NX)
00817                 if (pacEvHx.isGiven() == true)
00818                 {
00819                         condensePrep.addStatement(
00820                                         A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
00821                                                         == pacEvHx);
00822                 }
00823                 else
00824                 {
00825                         condensePrep.addStatement(
00826                                         A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
00827                                                         == pacEvHx.getSubMatrix(0, dimPacH, 0, NX));
00828                 }
00829                 condensePrep.addLinebreak();
00830 
00831                 // Evaluate Hx * C
00832                 for (unsigned row = 0; row < N - 1; ++row)
00833                 {
00834                         if (pacEvHx.isGiven() == true)
00835                         {
00836                                 condensePrep.addFunctionCall(
00837                                                 multHxC,
00838                                                 pacEvHx,
00839                                                 evGx.getAddress(row * NX, 0),
00840                                                 A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
00841                         }
00842                         else
00843                         {
00844                                 condensePrep.addFunctionCall(
00845                                                 multHxC,
00846                                                 pacEvHx.getAddress((row + 1) * dimPacH, 0),
00847                                                 evGx.getAddress(row * NX, 0),
00848                                                 A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
00849                         }
00850                 }
00851                 condensePrep.addLinebreak();
00852 
00853                 //
00854                 // Evaluate Hx * E
00855                 //
00856                 ExportVariable tmpE;
00857                 tmpE.setup("E", NX, NU, REAL, ACADO_LOCAL);
00858                 ExportIndex iRow( "row" ), iCol( "col" );
00859 
00860                 multHxE.setup("multHxE", tmpHx, tmpE, iRow, iCol);
00861                 multHxE.addStatement(
00862                                 A.getSubMatrix( rowOffset + iRow * dimPacH,
00863                                                                 rowOffset + (iRow + 1) * dimPacH,
00864                                                                 colOffset + iCol * NU,
00865                                                                 colOffset + (iCol + 1) * NU)
00866                                 == tmpHx * tmpE );
00867 
00868                 if ( N <= 20 )
00869                 {
00870                         for (unsigned row = 0; row < N - 1; ++row)
00871                         {
00872                                 for (unsigned col = 0; col <= row; ++col)
00873                                 {
00874                                         unsigned blk = (row + 1) * row / 2 + col;
00875                                         unsigned row2 = row + 1;
00876 
00877                                         if (pacEvHx.isGiven() == true)
00878                                                 condensePrep.addFunctionCall(
00879                                                                 multHxE,
00880                                                                 pacEvHx,
00881                                                                 E.getAddress(blk * NX, 0),
00882                                                                 ExportIndex( row2 ),
00883                                                                 ExportIndex( col )
00884                                                 );
00885                                         else
00886                                                 condensePrep.addFunctionCall(
00887                                                                 multHxE,
00888                                                                 pacEvHx.getAddress((row + 1) * dimPacH, 0),
00889                                                                 E.getAddress(blk * NX, 0),
00890                                                                 ExportIndex( row2 ),
00891                                                                 ExportIndex( col )
00892                                                 );
00893                                 }
00894                         }
00895                 }
00896                 else
00897                 {
00898                         ExportIndex row, col, blk, row2;
00899                         condensePrep.acquire( row );
00900                         condensePrep.acquire( col );
00901                         condensePrep.acquire( blk );
00902                         condensePrep.acquire( row2 );
00903 
00904                         ExportForLoop eLoopI(row, 0, N - 1);
00905                         ExportForLoop eLoopJ(col, 0, row + 1);
00906 
00907                         eLoopJ.addStatement( blk == (row + 1) * row / 2 + col );
00908                         eLoopJ.addStatement( row2 == row + 1 );
00909 
00910                         if (pacEvHx.isGiven() == true)
00911                         {
00912                                 eLoopJ.addFunctionCall(
00913                                                 multHxE,
00914                                                 pacEvHx,
00915                                                 E.getAddress(blk * NX, 0),
00916                                                 row2,
00917                                                 col
00918                                 );
00919                         }
00920                         else
00921                         {
00922                                 eLoopJ.addFunctionCall(
00923                                                 multHxE,
00924                                                 pacEvHx.getAddress((row + 1) * dimPacH, 0),
00925                                                 E.getAddress(blk * NX, 0),
00926                                                 row2,
00927                                                 col
00928                                 );
00929                         }
00930 
00931                         eLoopI.addStatement( eLoopJ );
00932                         condensePrep.addStatement( eLoopI );
00933 
00934                         condensePrep.release( row );
00935                         condensePrep.release( col );
00936                         condensePrep.release( blk );
00937                         condensePrep.release( row2 );
00938                 }
00939                 condensePrep.addLinebreak();
00940 
00941                 if (pacEvHu.getDim() > 0)
00942                 {
00943                         for (unsigned i = 0; i < N; ++i)
00944                         {
00945                                 if (pacEvHu.isGiven() == true)
00946                                         initialize.addStatement(
00947                                                         A.getSubMatrix(
00948                                                                         rowOffset + i * dimPacH,
00949                                                                         rowOffset + (i + 1) * dimPacH,
00950                                                                         colOffset + i * NU,
00951                                                                         colOffset + (i + 1) * NU)
00952                                                         ==      pacEvHu
00953                                         );
00954                                 else
00955                                         condensePrep.addStatement(
00956                                                         A.getSubMatrix(
00957                                                                         rowOffset + i * dimPacH,
00958                                                                         rowOffset + (i + 1) * dimPacH,
00959                                                                         colOffset + i * NU,
00960                                                                         colOffset + (i + 1) * NU)
00961                                                         ==      pacEvHu.getSubMatrix(
00962                                                                         i * dimPacH,(i + 1) * dimPacH, 0, NU)
00963                                         );
00964                         }
00965                 }
00966 
00967                 //
00968                 // Set upper and lower bounds
00969                 //
00970                 condensePrep.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) ==
00971                                 lbAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
00972                 condensePrep.addLinebreak();
00973                 condensePrep.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) ==
00974                                 ubAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
00975                 condensePrep.addLinebreak();
00976 
00977                 if (performFullCondensing() == true)
00978                 {
00979                         pacA01Dx0.setup("pacA01Dx0", numPathCon, 1, REAL, ACADO_WORKSPACE);
00980 
00981                         condenseFdb.addStatement( pacA01Dx0 == A10 * Dx0 );
00982                         condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
00983                         condenseFdb.addLinebreak();
00984                         condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
00985                         condenseFdb.addLinebreak();
00986                 }
00987 
00988                 // Evaluate Hx * d
00989                 if ( performsSingleShooting() == false )
00990                 {
00991                         ExportVariable tmpd("tmpd", NX, 1, REAL, ACADO_LOCAL);
00992                         ExportVariable tmpLb("lbA", dimPacH, 1, REAL, ACADO_LOCAL);
00993                         ExportVariable tmpUb("ubA", dimPacH, 1, REAL, ACADO_LOCAL);
00994 
00995                         macHxd.setup("macHxd", tmpHx, tmpd, tmpLb, tmpUb);
00996                         macHxd.addStatement( pacEvHxd == tmpHx * tmpd );
00997                         macHxd.addStatement( tmpLb -= pacEvHxd );
00998                         macHxd.addStatement( tmpUb -= pacEvHxd );
00999 
01000                         for (unsigned i = 0; i < N - 1; ++i)
01001                         {
01002                                 if (pacEvHx.isGiven() == true)
01003                                 {
01004                                         condensePrep.addFunctionCall(
01005                                                         macHxd,
01006                                                         pacEvHx,
01007                                                         d.getAddress(i * NX),
01008                                                         lbA.getAddress(rowOffset + (i + 1) * dimPacH),
01009                                                         ubA.getAddress(rowOffset + (i + 1) * dimPacH)
01010                                         );
01011                                 }
01012                                 else
01013                                 {
01014                                         condensePrep.addFunctionCall(
01015                                                         macHxd,
01016                                                         pacEvHx.getAddress((i + 1) * dimPacH, 0),
01017                                                         d.getAddress(i * NX),
01018                                                         lbA.getAddress(rowOffset + (i + 1) * dimPacH),
01019                                                         ubA.getAddress(rowOffset + (i + 1) * dimPacH)
01020                                         );
01021                                 }
01022                         }
01023                         condensePrep.addLinebreak();
01024                 }
01025         }
01026 
01028         //
01029         // Setup the evaluation of the point constraints
01030         //
01032 
01033         if ( numPointCon )
01034         {
01035                 unsigned rowOffset = getNumStateBounds() + N * dimPacH;
01036                 unsigned colOffset = performFullCondensing() == true ? 0 : NX;
01037                 unsigned dim;
01038 
01039                 if (performFullCondensing() == true)
01040                         A20.setup("A02", dimPocH, NX, REAL, ACADO_WORKSPACE);
01041 
01042                 //
01043                 // Evaluate the point constraints
01044                 //
01045                 for (unsigned i = 0, intRowOffset = 0; i < N + 1; ++i)
01046                 {
01047                         if (evaluatePointConstraints[ i ] == 0)
01048                                 continue;
01049 
01050                         condensePrep.addComment( string( "Evaluating constraint on node: #" ) + toString( i ) );
01051                         condensePrep.addLinebreak();
01052 
01053                         condensePrep.addStatement(conValueIn.getCols(0, getNX()) == x.getRow( i ) );
01054                         if (i < N)
01055                         {
01056                                 condensePrep.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
01057                                 condensePrep.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
01058                         }
01059                         else
01060                                 condensePrep.addStatement( conValueIn.getCols(NX, NX + NOD) == od.getRow( i ) );
01061 
01062                         condensePrep.addFunctionCall( evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
01063                         condensePrep.addLinebreak();
01064 
01065                         if (i < N)
01066                                 dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
01067                         else
01068                                 dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
01069 
01070                         // Fill pocEvH, pocEvHx, pocEvHu
01071                         condensePrep.addStatement(
01072                                         pocEvH.getRows(intRowOffset, intRowOffset + dim)
01073                                                         == conValueOut.getTranspose().getRows(0, dim));
01074                         condensePrep.addLinebreak();
01075 
01076                         condensePrep.addStatement(
01077                                         pocEvHx.makeRowVector().getCols(intRowOffset * NX,
01078                                                         (intRowOffset + dim) * NX)
01079                                                         == conValueOut.getCols(dim, dim + dim * NX));
01080                         condensePrep.addLinebreak();
01081 
01082                         if (i < N)
01083                         {
01084                                 condensePrep.addStatement(
01085                                                 pocEvHu.makeRowVector().getCols(intRowOffset * NU,
01086                                                                 (intRowOffset + dim) * NU)
01087                                                                 == conValueOut.getCols(dim + dim * NX,
01088                                                                                 dim + dim * NX + dim * NU));
01089                                 condensePrep.addLinebreak();
01090                         }
01091 
01092                         intRowOffset += dim;
01093                 }
01094 
01095                 //
01096                 // Include point constraint data in the QP problem
01097                 //
01098                 for (unsigned row = 0, intRowOffset = 0; row < N + 1; ++row)
01099                 {
01100                         if (evaluatePointConstraints[ row ] == 0)
01101                                 continue;
01102 
01103                         condensePrep.addComment(
01104                                         string( "Evaluating multiplications of constraint functions on node: #" ) + toString( row ) );
01105                         condensePrep.addLinebreak();
01106 
01107                         if (row < N)
01108                                 dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX + NU);
01109                         else
01110                                 dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX);
01111 
01112                         if (row == 0)
01113                         {
01114                                 if (performFullCondensing() == true)
01115                                         condensePrep.addStatement(
01116                                                         A20.getSubMatrix(0, dim, 0, NX)
01117                                                                         == pocEvHx.getSubMatrix(0, dim, 0, NX));
01118                                 else
01119                                         condensePrep.addStatement(
01120                                                         A.getSubMatrix(rowOffset, rowOffset + dim, 0, NX)
01121                                                                         == pocEvHx.getSubMatrix(0, dim, 0, NX));
01122                                 condensePrep.addLinebreak();
01123 
01124                                 condensePrep.addStatement(
01125                                                 A.getSubMatrix(rowOffset, rowOffset + dim, colOffset,
01126                                                                 colOffset + NU)
01127                                                                 == pocEvHu.getSubMatrix(0, dim, 0, NU));
01128                                 condensePrep.addLinebreak();
01129                         }
01130                         else
01131                         {
01132                                 // Hx * C
01133                                 if (performFullCondensing() == true)
01134                                         condensePrep.addStatement(
01135                                                         A20.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) ==
01136                                                                         pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
01137                                                                         evGx.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
01138                                 else
01139                                         condensePrep.addStatement(
01140                                                         A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim, 0, NX) ==
01141                                                                         pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
01142                                                                         evGx.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
01143                                 condensePrep.addLinebreak();
01144 
01145                                 // Hx * E
01146                                 ExportIndex iCol, iBlk;
01147                                 condensePrep.acquire( iCol );
01148                                 condensePrep.acquire( iBlk );
01149                                 ExportForLoop eLoop(iCol, 0, row);
01150 
01151                                 // row - 1, col -> blk
01152                                 eLoop.addStatement( iBlk == row * (row - 1) / 2 + iCol );
01153                                 eLoop.addStatement(
01154                                                 A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
01155                                                                 colOffset + iCol * NU, colOffset + (iCol + 1) * NU) ==
01156                                                                                 pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
01157                                                                                 E.getSubMatrix(iBlk * NX, (iBlk + 1) * NX, 0, NU)
01158                                 );
01159 
01160                                 condensePrep.addStatement( eLoop );
01161                                 condensePrep.release( iCol );
01162                                 condensePrep.release( iBlk );
01163 
01164                                 // Hx * d, MS only
01165                                 if (performsSingleShooting() == false)
01166                                 {
01167                                         condensePrep.addStatement(
01168                                                         pocEvHxd.getRows(intRowOffset, intRowOffset + dim) ==
01169                                                                         pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
01170                                                                         d.getRows((row - 1) * NX, row * NX) );
01171                                         condensePrep.addLinebreak();
01172                                 }
01173 
01174                                 // Add Hu block to the A21 block
01175                                 if (row < N)
01176                                 {
01177                                         condensePrep.addStatement(
01178                                                         A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
01179                                                                         colOffset + row * NU, colOffset + (row + 1) * NU) ==
01180                                                                         pocEvHu.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NU));
01181                                         condensePrep.addLinebreak();
01182                                 }
01183                         }
01184 
01185                         intRowOffset += dim;
01186                 }
01187 
01188                 //
01189                 // And now setup the lbA and ubA
01190                 //
01191                 condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) ==
01192                                 lbAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
01193                 condensePrep.addLinebreak();
01194                 condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) ==
01195                                 ubAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
01196                 condensePrep.addLinebreak();
01197 
01198                 if (performFullCondensing() == true)
01199                 {
01200                         pocA02Dx0.setup("pacA02Dx0", dimPocH, 1, REAL, ACADO_WORKSPACE);
01201 
01202                         condenseFdb.addStatement( pocA02Dx0 == A20 * Dx0 );
01203                         condenseFdb.addLinebreak();
01204                         condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
01205                         condenseFdb.addLinebreak();
01206                         condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
01207                         condenseFdb.addLinebreak();
01208                 }
01209 
01210                 if (performsSingleShooting() == false)
01211                 {
01212                         condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
01213                         condensePrep.addLinebreak();
01214                         condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
01215                         condensePrep.addLinebreak();
01216                 }
01217         }
01218 
01219         return SUCCESSFUL_RETURN;
01220 }
01221 
01222 returnValue ExportGaussNewtonCondensed::setupCondensing( void )
01223 {
01224         //
01225         // Define LM regularization terms
01226         //
01227         DMatrix mRegH00 = eye<double>( getNX() );
01228         mRegH00 *= levenbergMarquardt;
01229 
01230         condensePrep.setup("condensePrep");
01231         condenseFdb.setup( "condenseFdb" );
01232 
01234         //
01235         // Create block matrices C (alias evGx) and E
01236         //
01238 
01239         LOG( LVL_DEBUG ) << "Setup condensing: create C & E matrices" << endl;
01240 
01241         // Special case, row = col = 0
01242         condensePrep.addFunctionCall(moveGuE, evGu.getAddress(0, 0), E.getAddress(0, 0) );
01243 
01244         if (N <= 20)
01245         {
01246                 unsigned row, col, prev, curr;
01247                 for (row = 1; row < N; ++row)
01248                 {
01249                         condensePrep.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T);
01250 
01251                         if (performsSingleShooting() == false)
01252                                 condensePrep.addFunctionCall(multGxd, d.getAddress((row - 1) * NX), evGx.getAddress(row * NX), d.getAddress(row * NX));
01253 
01254                         condensePrep.addFunctionCall(multGxGx, T, evGx.getAddress((row - 1) * NX, 0), evGx.getAddress(row * NX, 0));
01255                         condensePrep.addLinebreak();
01256 
01257                         for(col = 0; col < row; ++col)
01258                         {
01259                                 prev = row * (row - 1) / 2 + col;
01260                                 curr = (row + 1) * row / 2 + col;
01261 
01262                                 condensePrep.addFunctionCall(multGxGu, T, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0));
01263                         }
01264                         condensePrep.addLinebreak();
01265 
01266                         curr = (row + 1) * row / 2 + col;
01267                         condensePrep.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
01268 
01269                         condensePrep.addLinebreak();
01270                 }
01271         }
01272         else
01273         {
01274                 ExportIndex row, col, curr, prev;
01275 
01276                 condensePrep.acquire( row );
01277                 condensePrep.acquire( col );
01278                 condensePrep.acquire( curr );
01279                 condensePrep.acquire( prev );
01280 
01281                 ExportForLoop eLoopI(row, 1, N);
01282                 ExportForLoop eLoopJ(col, 0, row);
01283 
01284                 eLoopI.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T);
01285 
01286                 if (performsSingleShooting() == false)
01287                         eLoopI.addFunctionCall(multGxd, d.getAddress((row - 1) * NX), evGx.getAddress(row * NX), d.getAddress(row * NX));
01288 
01289                 eLoopI.addFunctionCall(multGxGx, T, evGx.getAddress((row - 1) * NX, 0), evGx.getAddress(row * NX, 0));
01290 
01291                 eLoopJ.addStatement( prev == row * (row - 1) / 2 + col );
01292                 eLoopJ.addStatement( curr == (row + 1) * row / 2 + col );
01293                 eLoopJ.addFunctionCall( multGxGu, T, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0) );
01294 
01295                 eLoopI.addStatement( eLoopJ );
01296                 eLoopI.addStatement( curr == (row + 1) * row / 2 + col );
01297                 eLoopI.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
01298 
01299                 condensePrep.addStatement( eLoopI );
01300                 condensePrep.addLinebreak();
01301 
01302                 condensePrep.release( row );
01303                 condensePrep.release( col );
01304                 condensePrep.release( curr );
01305                 condensePrep.release( prev );
01306         }
01307 
01308         //
01309         // Multiply Gx and E blocks with Q1.
01310         //
01311         LOG( LVL_DEBUG ) << "Setup condensing: multiply with Q1" << endl;
01312 
01313         if (performFullCondensing() == false)
01314         {
01315                 for (unsigned i = 0; i < N - 1; ++i)
01316                         if (Q1.isGiven() == true)
01317                                 condensePrep.addFunctionCall(multQ1Gx, evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
01318                         else
01319                                 condensePrep.addFunctionCall(multGxGx, Q1.getAddress((i + 1) * NX, 0), evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
01320 
01321                 if (QN1.isGiven() == true)
01322                         condensePrep.addFunctionCall(multQN1Gx, evGx.getAddress((N - 1) * NX, 0), QGx.getAddress((N - 1) * NX, 0));
01323                 else
01324                         condensePrep.addFunctionCall(multGxGx, QN1, evGx.getAddress((N - 1) * NX, 0), QGx.getAddress((N - 1) * NX, 0));
01325                 condensePrep.addLinebreak();
01326         }
01327 
01328         if (N <= 20)
01329         {
01330                 for (unsigned i = 0; i < N; ++i)
01331                         for (unsigned j = 0; j <= i; ++j)
01332                         {
01333                                 unsigned k = (i + 1) * i / 2 + j;
01334 
01335                                 if (i < N - 1)
01336                                 {
01337                                         if (Q1.isGiven() == true)
01338                                                 condensePrep.addFunctionCall(multQ1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01339                                         else
01340                                                 condensePrep.addFunctionCall(multGxGu, Q1.getAddress((i + 1) * NX, 0), E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01341                                 }
01342                                 else
01343                                 {
01344                                         if (QN1.isGiven() == true)
01345                                                 condensePrep.addFunctionCall(multQN1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01346                                         else
01347                                                 condensePrep.addFunctionCall(multGxGu, QN1, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01348                                 }
01349                         }
01350                 condensePrep.addLinebreak();
01351         }
01352         else
01353         {
01354                 ExportIndex i, j, k;
01355 
01356                 condensePrep.acquire( i );
01357                 condensePrep.acquire( j );
01358                 condensePrep.acquire( k );
01359 
01360                 ExportForLoop eLoopI(i, 0, N - 1);
01361                 ExportForLoop eLoopJ1(j, 0, i + 1);
01362                 ExportForLoop eLoopJ2(j, 0, i + 1);
01363 
01364                 eLoopJ1.addStatement( k == (i + 1) * i / 2 + j );
01365 
01366                 if (Q1.isGiven() == true)
01367                         eLoopJ1.addFunctionCall(multQ1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01368                 else
01369                         eLoopJ1.addFunctionCall(multGxGu, Q1.getAddress((i + 1) * NX, 0), E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01370 
01371                 eLoopI.addStatement( eLoopJ1 );
01372 
01373                 eLoopJ2.addStatement( k == (i + 1) * i / 2 + j );
01374 
01375                 if (QN1.isGiven() == true)
01376                         eLoopJ2.addFunctionCall(multQN1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01377                 else
01378                         eLoopJ2.addFunctionCall(multGxGu, QN1, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
01379 
01380                 condensePrep.addStatement( eLoopI );
01381                 condensePrep.addLinebreak();
01382                 condensePrep.addStatement( eLoopJ2 );
01383                 condensePrep.addLinebreak();
01384 
01385                 condensePrep.release( i );
01386                 condensePrep.release( j );
01387                 condensePrep.release( k );
01388         }
01389 
01391         //
01392         // Compute Hessian blocks H00, H10, H11
01393         //
01395 
01396         LOG( LVL_DEBUG ) << "Setup condensing: create H00, H10 & H11" << endl;
01397 
01398         LOG( LVL_DEBUG ) << "---> Create H00" << endl;
01399 
01400         //
01401         // Create H00, in case of partial condensing only
01402         //
01403         if (performFullCondensing() == false)
01404         {
01405                 condensePrep.addFunctionCall( zeroBlockH00 );
01406 
01407                 for (unsigned i = 0; i < N; ++i)
01408                         condensePrep.addFunctionCall(multCTQC, evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
01409 
01410                 condensePrep.addStatement( H00 += mRegH00 );
01411                 condensePrep.addLinebreak();
01412 
01413                 // TODO: to be checked
01414                 if (initialStateFixed() == false)
01415                 {
01416                         if (Q1.isGiven() == true)
01417                         {
01418                                 condensePrep.addStatement( H00 += Q1 );
01419                         }
01420                         else
01421                         {
01422                                 condensePrep.addStatement( H00 += Q1.getSubMatrix(0, NX, 0, NX) );
01423                         }
01424                 }
01425 
01426                 if (SAC.getDim() > 0)
01427                         condensePrep.addStatement( H00 += SAC );
01428         }
01429 
01430         LOG( LVL_DEBUG ) << "---> Create H10" << endl;
01431 
01432         //
01433         // Create H10 block
01434         //
01435         if (N <= 20)
01436         {
01437                 for (unsigned i = 0; i < N; ++i)
01438                 {
01439                         condensePrep.addFunctionCall(zeroBlockH10, H10.getAddress(i * NU));
01440 
01441                         for (unsigned j = i; j < N; ++j)
01442                         {
01443                                 unsigned k = (j + 1) * j / 2 + i;
01444 
01445                                 condensePrep.addFunctionCall(multQETGx, QE.getAddress(k * NX), evGx.getAddress(j * NX), H10.getAddress(i * NU));
01446                         }
01447                 }
01448         }
01449         else
01450         {
01451                 ExportIndex ii, jj, kk;
01452                 condensePrep.acquire( ii );
01453                 condensePrep.acquire( jj );
01454                 condensePrep.acquire( kk );
01455 
01456                 ExportForLoop eLoopI(ii, 0, N);
01457                 ExportForLoop eLoopJ(jj, ii, N);
01458 
01459                 eLoopI.addFunctionCall(zeroBlockH10, H10.getAddress(ii * NU));
01460 
01461                 eLoopJ.addStatement( kk == (jj + 1) * jj / 2 + ii );
01462                 eLoopJ.addFunctionCall( multQETGx, QE.getAddress(kk * NX), evGx.getAddress(jj * NX), H10.getAddress(ii * NU) );
01463 
01464                 eLoopI.addStatement( eLoopJ );
01465                 condensePrep.addStatement( eLoopI );
01466 
01467                 condensePrep.release( ii );
01468                 condensePrep.release( jj );
01469                 condensePrep.release( kk );
01470         }
01471         condensePrep.addLinebreak();
01472 
01473         LOG( LVL_DEBUG ) << "---> Setup H01" << endl;
01474 
01475         //
01476         // Copy H01 = H10^T in case of partial condensing
01477         //
01478         if (performFullCondensing() == false)
01479         {
01480                 condensePrep.addStatement( H.getSubMatrix(0, NX, NX, getNumQPvars()) == H10.getTranspose() );
01481                 condensePrep.addLinebreak();
01482         }
01483 
01484         LOG( LVL_DEBUG ) << "---> Create H11" << endl;
01485 
01486         //
01487         // Create H11 block
01488         //
01489         if (N <= 20)
01490         {
01491                 unsigned row, col;
01492 
01493                 for (row = 0; row < N; ++row)
01494                 {
01495                         // Diagonal block
01496                         if (R1.isGiven() == true)
01497                                 condensePrep.addFunctionCall(setBlockH11_R1, ExportIndex(row), ExportIndex(row), R1);
01498                         else
01499                                 condensePrep.addFunctionCall(setBlockH11_R1, ExportIndex(row), ExportIndex(row), R1.getAddress(row * NU));
01500 
01501                         col = row;
01502                         for(unsigned blk = col; blk < N; ++blk)
01503                         {
01504                                 unsigned indl = (blk + 1) * blk / 2 + row;
01505                                 unsigned indr = (blk + 1) * blk / 2 + col;
01506 
01507                                 condensePrep.addFunctionCall(
01508                                                 setBlockH11, ExportIndex(row), ExportIndex(col),
01509                                                 E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01510                         }
01511                         condensePrep.addLinebreak();
01512 
01513                         // The rest of the blocks in the row
01514                         for(col = row + 1; col < N; ++col)
01515                         {
01516                                 condensePrep.addFunctionCall(
01517                                                 zeroBlockH11, ExportIndex(row), ExportIndex(col)
01518                                 );
01519 
01520                                 for(unsigned blk = col; blk < N; ++blk)
01521                                 {
01522                                         unsigned indl = (blk + 1) * blk / 2 + row;
01523                                         unsigned indr = (blk + 1) * blk / 2 + col;
01524 
01525                                         condensePrep.addFunctionCall(
01526                                                         setBlockH11, ExportIndex(row), ExportIndex(col),
01527                                                         E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01528                                 }
01529                                 condensePrep.addLinebreak();
01530                         }
01531                 }
01532         }
01533         else
01534         {
01535                 ExportIndex row, col, blk, indl, indr;
01536                 condensePrep.acquire( row ).acquire( col ).acquire( blk ).acquire( indl ).acquire( indr );
01537 
01538                 ExportForLoop eLoopI(row, 0, N);
01539                 ExportForLoop eLoopK(blk, row, N);
01540                 ExportForLoop eLoopJ(col, row + 1, N);
01541                 ExportForLoop eLoopK2(blk, col, N);
01542 
01543                 // The diagonal block
01544                 // Diagonal block
01545                 if (R1.isGiven() == true)
01546                         eLoopI.addFunctionCall(setBlockH11_R1, row, row, R1);
01547                 else
01548                         eLoopI.addFunctionCall(setBlockH11_R1, row, row, R1.getAddress(row * NU));
01549 
01550                 eLoopI.addStatement( col == row );
01551                 eLoopK.addStatement( indl == (blk + 1) * blk / 2 + row );
01552                 eLoopK.addStatement( indr == (blk + 1) * blk / 2 + col );
01553                 eLoopK.addFunctionCall( setBlockH11, row, col, E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01554                 eLoopI.addStatement( eLoopK );
01555 
01556                 // The rest of the blocks in the row
01557                 eLoopJ.addFunctionCall( zeroBlockH11, row, col );
01558 
01559                 eLoopK2.addStatement( indl == (blk + 1) * blk / 2 + row );
01560                 eLoopK2.addStatement( indr == (blk + 1) * blk / 2 + col );
01561                 eLoopK2.addFunctionCall( setBlockH11, row, col, E.getAddress(indl * NX), QE.getAddress(indr * NX) );
01562                 eLoopJ.addStatement( eLoopK2 );
01563 
01564                 eLoopI.addStatement( eLoopJ );
01565                 condensePrep.addStatement( eLoopI );
01566 
01567                 condensePrep.release( row ).release( col ).release( blk ).release( indl ).release( indr );
01568         }
01569         condensePrep.addLinebreak();
01570 
01571         LOG( LVL_DEBUG ) << "---> Copy H11 lower part" << endl;
01572 
01573         unsigned offset = (performFullCondensing() == true) ? 0 : NX;
01574 
01575         // Copy to H11 upper triangular part to lower triangular part
01576         if (N <= 20)
01577         {
01578                 for (unsigned ii = 0; ii < N; ++ii)
01579                         for(unsigned jj = 0; jj < ii; ++jj)
01580                                 condensePrep.addFunctionCall(
01581                                                 copyHTH, ExportIndex( ii ), ExportIndex( jj )
01582                                 );
01583         }
01584         else
01585         {
01586                 ExportIndex ii, jj;
01587 
01588                 condensePrep.acquire( ii );
01589                 condensePrep.acquire( jj );
01590 
01591                 ExportForLoop eLoopI(ii, 0, N);
01592                 ExportForLoop eLoopJ(jj, 0, ii);
01593 
01594                 eLoopJ.addFunctionCall(copyHTH, ii, jj);
01595                 eLoopI.addStatement( eLoopJ );
01596                 condensePrep.addStatement( eLoopI );
01597 
01598                 condensePrep.release( ii );
01599                 condensePrep.release( jj );
01600         }
01601         condensePrep.addLinebreak();
01602 
01603         LOG( LVL_DEBUG ) << "---> Copy H10" << endl;
01604 
01605         //
01606         // Set block H10 in case of partial condensing
01607         //
01608         if (performFullCondensing() == false)
01609         {
01610                 condensePrep.addStatement( H.getSubMatrix(NX, getNumQPvars(), 0, NX) == H10 );
01611                 condensePrep.addLinebreak();
01612         }
01613 
01614         int externalCholesky;
01615         get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
01616         ASSERT((CondensedHessianCholeskyDecomposition)externalCholesky == INTERNAL_N3 ||
01617                         (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL)
01618 
01619         if ((CondensedHessianCholeskyDecomposition)externalCholesky == INTERNAL_N3)
01620         {
01621                 R.setup("R", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01622 
01623                 cholSolver.init(getNumQPvars(), NX, "condensing");
01624                 cholSolver.setup();
01625 
01626                 condensePrep.addStatement( R == H );
01627                 condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), R);
01628         }
01629 
01631         //
01632         // Compute gradient components g0 and g1
01633         //
01635 
01636         //
01637         // Compute d and Qd = Q1 * d
01638         //
01639         LOG( LVL_DEBUG ) << "Setup condensing: compute Qd" << endl;
01640 
01641         if (performsSingleShooting() == false)
01642         {
01643                 Qd.setup("Qd", N * NX, 1, REAL, ACADO_WORKSPACE);
01644 
01645                 for(unsigned i = 0; i < N - 1; ++i)
01646                 {
01647                         if (Q1.isGiven() == true)
01648                                 condensePrep.addFunctionCall(
01649                                                 multQ1d, Q1, d.getAddress(i * NX), Qd.getAddress(i * NX) );
01650                         else
01651                                 condensePrep.addFunctionCall(
01652                                                 multQ1d, Q1.getAddress((i + 1) * NX, 0), d.getAddress(i * NX), Qd.getAddress(i * NX) );
01653                 }
01654 
01655                 condensePrep.addFunctionCall(
01656                                 multQN1d, QN1, d.getAddress((N - 1) * NX), Qd.getAddress((N - 1) * NX) );
01657 
01658                 condensePrep.addLinebreak();
01659         }
01660 
01661         LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
01662 
01663         if (initialStateFixed() == true)
01664         {
01665                 condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
01666                 condenseFdb.addLinebreak();
01667         }
01668 
01669         condenseFdb.addStatement( Dy -=  y );
01670         condenseFdb.addStatement( DyN -=  yN );
01671         condenseFdb.addLinebreak();
01672 
01673         // Compute RDy
01674         for(unsigned run1 = 0; run1 < N; ++run1)
01675         {
01676                 if (R2.isGiven() == true)
01677                         condenseFdb.addFunctionCall(
01678                                         multRDy, R2,
01679                                         Dy.getAddress(run1 * NY, 0),
01680                                         g.getAddress(offset + run1 * NU, 0) );
01681                 else
01682                         condenseFdb.addFunctionCall(
01683                                         multRDy, R2.getAddress(run1 * NU, 0),
01684                                         Dy.getAddress(run1 * NY, 0),
01685                                         g.getAddress(offset + run1 * NU, 0) );
01686         }
01687         condenseFdb.addLinebreak();
01688 
01689         // Compute QDy
01690         // NOTE: This is just for the MHE case :: run1 starts from 0; in MPC :: from 1 ;)
01691         for(unsigned run1 = 0; run1 < N; run1++ )
01692         {
01693                 if (Q2.isGiven() == true)
01694                         condenseFdb.addFunctionCall(
01695                                         multQDy, Q2,
01696                                         Dy.getAddress(run1 * NY),
01697                                         QDy.getAddress(run1 * NX) );
01698                 else
01699                         condenseFdb.addFunctionCall(
01700                                         multQDy, Q2.getAddress(run1 * NX),
01701                                         Dy.getAddress(run1 * NY),
01702                                         QDy.getAddress(run1 * NX) );
01703         }
01704         condenseFdb.addLinebreak();
01705         condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == QN2 * DyN );
01706         condenseFdb.addLinebreak();
01707 
01708         LOG( LVL_DEBUG ) << "Setup condensing: QDy.getRows(NX, (N + 1) * NX) += Qd" << endl;
01709 
01710         if (performsSingleShooting() == false)
01711         {
01712                 condenseFdb.addStatement( QDy.getRows(NX, (N + 1) * NX) += Qd );
01713                 condenseFdb.addLinebreak();
01714         }
01715 
01716         if (performFullCondensing() == false)
01717         {
01718                 // g0 == C^T * QDy(1: N + 1, :)
01719                 condenseFdb.addStatement( g0 == (evGx ^ QDy.getRows(NX, (N + 1) * NX)) );
01720                 condenseFdb.addLinebreak();
01721 
01722                 if (initialStateFixed() == false)
01723                         condenseFdb.addStatement( g0 += QDy.getRows(0, NX) );
01724 
01725                 if (SAC.getDim() > 0)
01726                 {
01727                         // Include arrival cost
01728                         condenseFdb.addStatement( DxAC == x.getRow( 0 ).getTranspose() - xAC );
01729                         condenseFdb.addStatement( g0 +=  SAC * DxAC );
01730                 }
01731 
01732                 condenseFdb.addLinebreak();
01733         }
01734 
01735         if (N <= 20)
01736         {
01737                 for (unsigned i = 0; i < N; ++i)
01738                         for (unsigned j = i; j < N; ++j)
01739                         {
01740                                 unsigned k = (j + 1) * j / 2 + i;
01741 
01742                                 condenseFdb.addFunctionCall(
01743                                                 multEQDy, E.getAddress(k * NX, 0), QDy.getAddress((j + 1) * NX), g.getAddress(offset + i * NU) );
01744                         }
01745         }
01746         else
01747         {
01748                 ExportIndex i, j, k;
01749                 condenseFdb.acquire( i );
01750                 condenseFdb.acquire( j );
01751                 condenseFdb.acquire( k );
01752 
01753                 ExportForLoop eLoopI(i, 0, N);
01754                 ExportForLoop eLoopJ(j, i, N);
01755 
01756                 eLoopJ.addStatement( k == (j + 1) * j / 2 + i );
01757                 eLoopJ.addFunctionCall(
01758                                 multEQDy, E.getAddress(k * NX, 0), QDy.getAddress((j + 1) * NX), g.getAddress(offset + i * NU) );
01759 
01760                 eLoopI.addStatement( eLoopJ );
01761                 condenseFdb.addStatement( eLoopI );
01762 
01763                 condenseFdb.release( i );
01764                 condenseFdb.release( j );
01765                 condenseFdb.acquire( k );
01766         }
01767         condenseFdb.addLinebreak();
01768 
01769         if (performFullCondensing() == true)
01770         {
01771                 condenseFdb.addStatement( g1 += H10 * Dx0 );
01772                 condenseFdb.addLinebreak();
01773         }
01774 
01775         //
01776         // Add condensed linear terms from the objective to the gradient
01777         //
01778         if (performFullCondensing() == false && objSlx.getDim() > 0)
01779         {
01780                 condensePrep.addStatement( g0 += objSlx );
01781 
01782                 ExportVariable g00, C0, Slx0;
01783                 g00.setup("g0", NX, 1, REAL, ACADO_LOCAL);
01784                 C0.setup("C0", NX, NX, REAL, ACADO_LOCAL);
01785 
01786                 if (objSlx.isGiven() == false)
01787                         Slx0.setup("Slx0", NX, 1, REAL, ACADO_LOCAL);
01788                 else
01789                         Slx0 = objSlx;
01790 
01791                 macCTSlx.setup("macCTSlx", C0, Slx0, g00);
01792                 macCTSlx.addStatement( g00 += C0.getTranspose() * Slx0);
01793 
01794                 for (unsigned i = 0; i < N; ++i)
01795                         condensePrep.addFunctionCall(macCTSlx, evGx.getAddress(i * NX, 0), objSlx, g);
01796         }
01797 
01798         if (objSlx.getDim() > 0 || objSlu.getDim() > 0)
01799         {
01800                 offset = performFullCondensing() == true ? 0 : NX;
01801 
01802                 if (objSlu.getDim() > 0 && objSlx.getDim() == 0)
01803                 {
01804                         for (unsigned i = 0; i < N; ++i)
01805                         {
01806                                 condensePrep.addStatement(
01807                                                 g.getRows(offset + i * NU, offset + (i + 1) * NU) += objSlu
01808                                 );
01809                         }
01810                 }
01811                 else
01812                 {
01813                         ExportVariable g10, E0, Slx0, Slu0;
01814 
01815                         g10.setup("g1", NU, 1, REAL, ACADO_LOCAL);
01816                         E0.setup("E0", NX, NU, REAL, ACADO_LOCAL);
01817 
01818                         if (objSlx.isGiven() == false && objSlx.getDim() > 0)
01819                                 Slx0.setup("Slx0", NX, 1, REAL, ACADO_LOCAL);
01820                         else
01821                                 Slx0 = objSlx;
01822 
01823                         macETSlu.setup("macETSlu", E0, Slx0, g10);
01824                         macETSlu.addStatement( g10 += E0.getTranspose() * Slx0 );
01825 
01826                         if (N <= 20)
01827                         {
01828                                 for (unsigned i = 0; i < N; ++i)
01829                                         for (unsigned j = i; j < N; ++j)
01830                                         {
01831                                                 unsigned k = (j + 1) * j / 2 + i;
01832 
01833                                                 condensePrep.addFunctionCall(macETSlu, QE.getAddress(k * NX), objSlx, g.getAddress(offset + i * NU));
01834                                         }
01835                         }
01836                         else
01837                         {
01838                                 ExportIndex ii, jj, kk;
01839                                 condensePrep.acquire( ii );
01840                                 condensePrep.acquire( jj );
01841                                 condensePrep.acquire( kk );
01842 
01843                                 ExportForLoop iLoop(ii, 0, N);
01844                                 ExportForLoop jLoop(jj, ii, N);
01845 
01846                                 jLoop.addStatement( kk == (jj + 1) * jj / 2 + ii );
01847                                 jLoop.addFunctionCall(macETSlu, QE.getAddress(kk * NX), objSlx, g.getAddress(offset + ii * NU));
01848 
01849                                 iLoop.addStatement( jLoop );
01850                                 condensePrep.addStatement( iLoop );
01851 
01852                                 condensePrep.release( ii );
01853                                 condensePrep.release( jj );
01854                                 condensePrep.release( kk );
01855                         }
01856 
01857                         if (objSlu.getDim() > 0)
01858                         {
01859                                 for (unsigned i = 0; i < N; ++i)
01860                                         condensePrep.addStatement(
01861                                                         g.getRows(offset + i * NU, offset + (i + 1) * NU) += objSlu);
01862                         }
01863                 }
01864         }
01865 
01867         //
01868         // Expansion routine
01869         //
01871 
01872         LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
01873 
01874         expand.setup( "expand" );
01875 
01876         if (performFullCondensing() == true)
01877         {
01878                 expand.addStatement( u.makeRowVector() += xVars.getTranspose() );
01879                 expand.addLinebreak();
01880                 expand.addStatement( x.getRow( 0 ) += Dx0.getTranspose() );
01881         }
01882         else
01883         {
01884                 expand.addStatement( x.makeColVector().getRows(0, NX) += xVars.getRows(0, NX) );
01885                 expand.addLinebreak();
01886                 expand.addStatement( u.makeRowVector() += xVars.getTranspose().getCols(NX, getNumQPvars() ) );
01887         }
01888         expand.addLinebreak();
01889 
01890         // x += C * deltaX0
01891 
01892         if (performsSingleShooting() == false)
01893         {
01894 
01895                 if (performFullCondensing() == true)
01896                         expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += d + evGx * Dx0);
01897                 else
01898                         expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += d + evGx * xVars.getRows(0, NX) );
01899         }
01900         else
01901         {
01902                 if (performFullCondensing() == true)
01903                         expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += evGx * Dx0);
01904                 else
01905                         expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += evGx * xVars.getRows(0, NX) );
01906         }
01907 
01908         expand.addLinebreak();
01909 
01910         // x += E * deltaU
01911 
01912         offset = (performFullCondensing() == true) ? 0 : NX;
01913 
01914         if (N <= 20)
01915         {
01916                 for (unsigned i = 0; i < N; ++i)
01917                         for (unsigned j = 0; j <= i; ++j)
01918                         {
01919                                 unsigned k = (i + 1) * i / 2 + j;
01920 
01921                                 expand.addFunctionCall(
01922                                                 multEDu, E.getAddress(k * NX, 0), xVars.getAddress(offset + j * NU), x.getAddress(i + 1) );
01923                         }
01924         }
01925         else
01926         {
01927                 ExportIndex ii, jj, kk;
01928                 expand.acquire( ii );
01929                 expand.acquire( jj );
01930                 expand.acquire( kk );
01931 
01932                 ExportForLoop eLoopI(ii, 0, N);
01933                 ExportForLoop eLoopJ(jj, 0, ii + 1);
01934 
01935                 eLoopJ.addStatement( kk == (ii + 1) * ii / 2 + jj );
01936                 eLoopJ.addFunctionCall(
01937                                 multEDu, E.getAddress(kk * NX, 0), xVars.getAddress(offset + jj * NU), x.getAddress(ii + 1) );
01938 
01939                 eLoopI.addStatement( eLoopJ );
01940                 expand.addStatement( eLoopI );
01941 
01942                 expand.release( ii );
01943                 expand.release( jj );
01944                 expand.release( kk );
01945         }
01946 
01948         //
01949         // Calculation of the covariance matrix for the last state estimate
01950         //
01952 
01953         int covCalc;
01954         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
01955 
01956         if (covCalc == NO)
01957                 return SUCCESSFUL_RETURN;
01958 
01959         if (performFullCondensing() == true)
01960                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, \
01961                                 "Calculation of covariance matrix works for partial condensing only atm.");
01962 
01963         LOG( LVL_DEBUG ) << "Setup condensing: covariance matrix calculation" << endl;
01964 
01965         CEN.setup("CEN", NX, getNumQPvars(), REAL, ACADO_WORKSPACE);
01966         sigmaTmp.setup("sigmaTemp", getNumQPvars(), NX, REAL, ACADO_WORKSPACE);
01967         sigma.setup("sigma", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
01968 
01969         sigmaN.setup("sigmaN", NX, NX, REAL, ACADO_VARIABLES);
01970         sigmaN.setDoc("Covariance matrix of the last state estimate.");
01971 
01972         calculateCovariance.setup("calculateCovariance");
01973 
01974         calculateCovariance.addStatement(
01975                         CEN.getSubMatrix(0, NX, 0, NX) == evGx.getSubMatrix((N - 1) * NX, N * NX, 0, NX) );
01976         calculateCovariance.addLinebreak();
01977 
01978         ExportIndex cIndex("cIndex");
01979         ExportForLoop cLoop(cIndex, 0, N);
01980         calculateCovariance.addIndex( cIndex );
01981 
01982         unsigned blk = (N - 1 + 1) * (N - 1) / 2;
01983         cLoop.addStatement(
01984                         CEN.getSubMatrix(0, NX, NX + cIndex * NU, NX + NU + cIndex * NU) ==
01985                                         E.getSubMatrix(blk * NX + cIndex * NX, blk * NX + NX + cIndex * NX, 0, NU)
01986         );
01987         calculateCovariance.addStatement( cLoop );
01988         calculateCovariance.addLinebreak();
01989 
01990         // XXX Optimize for long horizons.
01991         calculateCovariance.addStatement(
01992                         sigmaTmp == sigma * CEN.getTranspose()
01993         );
01994         calculateCovariance.addLinebreak();
01995 
01996         calculateCovariance.addStatement(
01997                         sigmaN == CEN * sigmaTmp
01998         );
01999 
02000         return SUCCESSFUL_RETURN;
02001 }
02002 
02003 returnValue ExportGaussNewtonCondensed::setupVariables( )
02004 {
02006         //
02007         // Make index vector for state constraints
02008         //
02010 
02011         bool boxConIsFinite = false;
02012         xBoundsIdx.clear();
02013 
02014         DVector lbBox, ubBox;
02015         for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
02016         {
02017                 lbBox = xBounds.getLowerBounds( i );
02018                 ubBox = xBounds.getUpperBounds( i );
02019 
02020                 if (isFinite( lbBox ) || isFinite( ubBox ))
02021                         boxConIsFinite = true;
02022 
02023                 // This is maybe not necessary
02024                 if (boxConIsFinite == false || i == 0)
02025                         continue;
02026 
02027                 for (unsigned j = 0; j < lbBox.getDim(); ++j)
02028                 {
02029                         if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
02030                         {
02031                                 xBoundsIdx.push_back(i * lbBox.getDim() + j);
02032                         }
02033                 }
02034         }
02035 
02036         if (initialStateFixed() == true)
02037         {
02038                 x0.setup("x0",  NX, 1, REAL, ACADO_VARIABLES);
02039                 x0.setDoc( (std::string)"Current state feedback vector." );
02040                 Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
02041         }
02042 
02043         T.setup("T", NX, NX, REAL, ACADO_WORKSPACE);
02044         E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
02045         QE.setup("QE", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
02046 
02047         if (performFullCondensing() == false)
02048                 QGx.setup("QGx", N * NX, NX, REAL, ACADO_WORKSPACE);
02049 
02050         QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
02051 
02052         // Setup all QP stuff
02053 
02054         H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
02055 
02056         // Stupid aliasing to avoid copying of data
02057         if (performFullCondensing() == true)
02058         {
02059                 H11 = H;
02060         }
02061         else
02062         {
02063                 H00 = H.getSubMatrix(0, NX, 0, NX);
02064                 H11 = H.getSubMatrix(NX, getNumQPvars(), NX, getNumQPvars());
02065         }
02066 
02067         H10.setup("H10", N * NU, NX, REAL, ACADO_WORKSPACE);
02068 
02069         A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
02070 
02071         g.setup("g",  getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02072 
02073         if (performFullCondensing() == true)
02074         {
02075                 g1 = g;
02076         }
02077         else
02078         {
02079                 g0 = g.getRows(0, NX);
02080                 g1 = g.getRows(NX, getNumQPvars());
02081         }
02082 
02083         lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02084         ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02085 
02086         lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
02087         ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
02088 
02089         xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
02090         yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
02091 
02092         return SUCCESSFUL_RETURN;
02093 }
02094 
02095 returnValue ExportGaussNewtonCondensed::setupMultiplicationRoutines( )
02096 {
02097         ExportIndex iCol( "iCol" );
02098         ExportIndex iRow( "iRow" );
02099 
02100         ExportVariable dp, dn, Gx1, Gx2, Gx3, Gu1, Gu2;
02101         ExportVariable R22, R11, Dy1, RDy1, Q22, QDy1, E1, U1, H101;
02102         dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
02103         dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
02104         Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
02105         Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
02106         Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
02107         Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
02108         Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
02109         R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
02110         R11.setup("R11", NU, NU, REAL, ACADO_LOCAL);
02111         Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
02112         RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
02113         Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
02114         QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
02115         E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
02116         U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
02117         H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
02118 
02119         if ( Q2.isGiven() )
02120                 Q22 = Q2;
02121         if ( R2.isGiven() )
02122                 R22 = R2;
02123         if ( R1.isGiven() )
02124                 R11 = R1;
02125 
02126         // multGxd; // d_k += Gx_k * d_{k-1}
02127         multGxd.setup("multGxd", dp, Gx1, dn);
02128         multGxd.addStatement( dn += Gx1 * dp );
02129         // moveGxT
02130         moveGxT.setup("moveGxT", Gx1, Gx2);
02131         moveGxT.addStatement( Gx2 == Gx1 );
02132         // multGxGx
02133         multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
02134         multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
02135         // multGxGu
02136         multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
02137         multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
02138         // moveGuE
02139         moveGuE.setup("moveGuE", Gu1, Gu2);
02140         moveGuE.addStatement( Gu2 == Gu1 );
02141 
02142         unsigned offset = (performFullCondensing() == true) ? 0 : NX;
02143 
02144         // setBlockH11
02145         setBlockH11.setup("setBlockH11", iRow, iCol, Gu1, Gu2);
02146         setBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) += (Gu1 ^ Gu2) );
02147         // setBlockH11_R1
02148         DMatrix mRegH11 = eye<double>( getNU() );
02149         mRegH11 *= levenbergMarquardt;
02150 
02151         setBlockH11_R1.setup("setBlockH11_R1", iRow, iCol, R11);
02152         setBlockH11_R1.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == R11 + mRegH11 );
02153         // zeroBlockH11
02154         zeroBlockH11.setup("zeroBlockH11", iRow, iCol);
02155         zeroBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == zeros<double>(NU, NU) );
02156         // copyHTH
02157         copyHTH.setup("copyHTH", iRow, iCol);
02158         copyHTH.addStatement(
02159                         H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
02160                                         H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
02161         );
02162         // multRDy
02163         multRDy.setup("multRDy", R22, Dy1, RDy1);
02164         multRDy.addStatement( RDy1 == R22 * Dy1 );
02165         // mult QDy1
02166         multQDy.setup("multQDy", Q22, Dy1, QDy1);
02167         multQDy.addStatement( QDy1 == Q22 * Dy1 );
02168         // multEQDy;
02169         multEQDy.setup("multEQDy", E1, QDy1, U1);
02170         multEQDy.addStatement( U1 += (E1 ^ QDy1) );
02171         // multQETGx
02172         multQETGx.setup("multQETGx", E1, Gx1, H101);
02173         multQETGx.addStatement( H101 += (E1 ^ Gx1) );
02174         // zerBlockH10
02175         zeroBlockH10.setup("zeroBlockH10", H101);
02176         zeroBlockH10.addStatement( H101 == zeros<double>(NU, NX) );
02177 
02178 //      if (performsSingleShooting() == false)
02179 //      {
02180                 // multEDu
02181                 multEDu.setup("multEDu", E1, U1, dn);
02182                 multEDu.addStatement( dn += E1 * U1 );
02183 //      }
02184 
02185         if (Q1.isGiven() == true)
02186         {
02187                 // multQ1Gx
02188                 multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
02189                 multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
02190 
02191                 // multQ1Gu
02192                 multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
02193                 multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
02194 
02195                 // multQ1d
02196                 multQ1d.setup("multQ1d", Q1, dp, dn);
02197                 multQ1d.addStatement( dn == Q1 * dp );
02198         }
02199         else
02200         {
02201                 // multQ1d
02202                 multQ1d.setup("multQ1d", Gx1, dp, dn);
02203                 multQ1d.addStatement( dn == Gx1 * dp );
02204         }
02205 
02206         if (QN1.isGiven() == BT_TRUE)
02207         {
02208                 // multQN1Gu
02209                 multQN1Gu.setup("multQN1Gu", Gu1, Gu2);
02210                 multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
02211 
02212                 // multQN1Gx
02213                 multQN1Gx.setup("multQN1Gx", Gx1, Gx2);
02214                 multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
02215         }
02216 
02217         if (performsSingleShooting() == BT_FALSE)
02218         {
02219                 // multQN1d
02220                 multQN1d.setup("multQN1d", QN1, dp, dn);
02221                 multQN1d.addStatement( dn == QN1 * dp );
02222         }
02223 
02224         if (performFullCondensing() == BT_FALSE)
02225         {
02226                 // zeroBlockH00
02227                 zeroBlockH00.setup( "zeroBlockH00" );
02228                 zeroBlockH00.addStatement( H00 == zeros<double>(NX, NX) );
02229 
02230                 // multCTQC
02231                 multCTQC.setup("multCTQC", Gx1, Gx2);
02232                 multCTQC.addStatement( H00 += (Gx1 ^ Gx2) );
02233         }
02234 
02235         return SUCCESSFUL_RETURN;
02236 }
02237 
02238 returnValue ExportGaussNewtonCondensed::setupEvaluation( )
02239 {
02241         //
02242         // Preparation phase
02243         //
02245 
02246         preparation.setup( "preparationStep" );
02247         preparation.doc( "Preparation step of the RTI scheme." );
02248         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
02249         retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
02250         preparation.setReturnValue(retSim, false);
02251 
02252         preparation     << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
02253 
02254         preparation.addFunctionCall( evaluateObjective );
02255         preparation.addFunctionCall( condensePrep );
02256 
02258         //
02259         // Feedback phase
02260         //
02262 
02263         ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
02264         tmp.setDoc( "Status code of the qpOASES QP solver." );
02265 
02266         ExportFunction solve("solve");
02267         solve.setReturnValue( tmp );
02268 
02269         feedback.setup("feedbackStep");
02270         feedback.doc( "Feedback/estimation step of the RTI scheme." );
02271         feedback.setReturnValue( tmp );
02272 
02273         feedback.addFunctionCall( condenseFdb );
02274         feedback.addLinebreak();
02275 
02276         feedback << tmp.getName() << " = " << solve.getName() << "( );\n";
02277         feedback.addLinebreak();
02278 
02279         feedback.addFunctionCall( expand );
02280 
02281         int covCalc;
02282         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
02283         if (covCalc)
02284                 feedback.addFunctionCall( calculateCovariance );
02285 
02287         //
02288         // Setup evaluation of the KKT tolerance
02289         //
02291 
02292         ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
02293         ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
02294         ExportIndex index( "index" );
02295 
02296         getKKT.setup( "getKKT" );
02297         getKKT.doc( "Get the KKT tolerance of the current iterate." );
02298         kkt.setDoc( "The KKT tolerance value." );
02299         getKKT.setReturnValue( kkt );
02300         getKKT.addVariable( prd );
02301         getKKT.addIndex( index );
02302 
02303         // ACC = |\nabla F^T * xVars|
02304         getKKT.addStatement( kkt == (g ^ xVars) );
02305         getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
02306 
02307         ExportForLoop bLoop(index, 0, getNumQPvars());
02308 
02309         bLoop.addStatement( prd == yVars.getRow( index ) );
02310         bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
02311         bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
02312         bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
02313         bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
02314         getKKT.addStatement( bLoop );
02315 
02316         if ((getNumStateBounds() + getNumComplexConstraints())> 0)
02317         {
02318                 ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
02319 
02320                 cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ) );
02321                 cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
02322                 cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
02323                 cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
02324                 cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
02325 
02326                 getKKT.addStatement( cLoop );
02327         }
02328 
02329         return SUCCESSFUL_RETURN;
02330 }
02331 
02332 returnValue ExportGaussNewtonCondensed::setupQPInterface( )
02333 {
02334         string folderName;
02335         get(CG_EXPORT_FOLDER_NAME, folderName);
02336         string moduleName;
02337         get(CG_MODULE_NAME, moduleName);
02338         std::string sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
02339         std::string headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
02340 
02341         int useSinglePrecision;
02342         get(USE_SINGLE_PRECISION, useSinglePrecision);
02343 
02344         int hotstartQP;
02345         get(HOTSTART_QP, hotstartQP);
02346 
02347         int covCalc;
02348         get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
02349 
02350         int maxNumQPiterations;
02351         get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
02352 
02353         int externalCholesky;
02354         get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
02355 
02356         //
02357         // Set up export of the source file
02358         //
02359 
02360         ExportQpOasesInterface qpInterface = ExportQpOasesInterface(headerFile, sourceFile, "");
02361 
02362         qpInterface.configure(
02363                         "",
02364                         "QPOASES_HEADER",
02365                         getNumQPvars(),
02366                         getNumStateBounds() + getNumComplexConstraints(),
02367                         maxNumQPiterations,
02368                         "PL_NONE",
02369                         useSinglePrecision,
02370 
02371                         commonHeaderName,
02372                         "",
02373                         xVars.getFullName(),
02374                         yVars.getFullName(),
02375                         sigma.getFullName(),
02376                         hotstartQP,
02377                         (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL,
02378                         H.getFullName(),
02379                         R.getFullName(),
02380                         g.getFullName(),
02381                         A.getFullName(),
02382                         lb.getFullName(),
02383                         ub.getFullName(),
02384                         lbA.getFullName(),
02385                         ubA.getFullName()
02386         );
02387 
02388         return qpInterface.exportCode();
02389 }
02390 
02391 bool ExportGaussNewtonCondensed::performFullCondensing() const
02392 {
02393         int sparseQPsolution;
02394         get(SPARSE_QP_SOLUTION, sparseQPsolution);
02395 
02396         return (SparseQPsolutionMethods)sparseQPsolution == CONDENSING ? false : true;
02397 }
02398 
02399 CLOSE_NAMESPACE_ACADO


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