export_exact_hessian_qpdunes.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00032 #include <acado/code_generation/export_exact_hessian_qpdunes.hpp>
00033 #include <acado/code_generation/export_qpdunes_interface.hpp>
00034 
00035 BEGIN_NAMESPACE_ACADO
00036 
00037 using namespace std;
00038 
00039 ExportExactHessianQpDunes::ExportExactHessianQpDunes(   UserInteraction* _userInteraction,
00040                                                                                                         const std::string& _commonHeaderName
00041                                                                                                         ) : ExportGaussNewtonQpDunes( _userInteraction,_commonHeaderName )
00042 {}
00043 
00044 returnValue ExportExactHessianQpDunes::setup( )
00045 {
00046         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00047         setupInitialization();
00048 
00049         //
00050         // Add QP initialization call to the initialization
00051         //
00052         initialize << "for( ret = 0; ret < ACADO_N*(ACADO_NX+ACADO_NU)*(ACADO_NX+ACADO_NU)+ACADO_NX*ACADO_NX; ret++ )  acadoWorkspace.qpH[ret] = 1.0;\n";  // TODO: this is added because of a bug in qpDUNES !!
00053         ExportFunction initializeQpDunes( "initializeQpDunes" );
00054         initialize
00055                 << "ret = (int)initializeQpDunes();\n"
00056                 << "if ((return_t)ret != QPDUNES_OK) return ret;\n";
00057 
00058         cleanup.setup( "cleanupSolver" );
00059         ExportFunction cleanupQpDunes( "cleanupQpDunes" );
00060         cleanup.addFunctionCall( cleanupQpDunes );
00061         LOG( LVL_DEBUG ) << "done!" << endl;
00062 
00063         LOG( LVL_DEBUG ) << "Solver: setup setupVariables... " << endl;
00064         setupVariables();
00065         LOG( LVL_DEBUG ) << "done!" << endl;
00066 
00067         LOG( LVL_DEBUG ) << "Solver: setup setupSimulation... " << endl;
00068         setupSimulation();
00069         LOG( LVL_DEBUG ) << "done!" << endl;
00070 
00071         LOG( LVL_DEBUG ) << "Solver: setup setupObjectiveEvaluation... " << endl;
00072         setupObjectiveEvaluation();
00073         LOG( LVL_DEBUG ) << "done!" << endl;
00074 
00075         LOG( LVL_DEBUG ) << "Solver: setup setupConstraintsEvaluation... " << endl;
00076         setupConstraintsEvaluation();
00077         LOG( LVL_DEBUG ) << "done!" << endl;
00078 
00079         LOG( LVL_DEBUG ) << "Solver: setup hessian regularization... " << endl;
00080         setupHessianRegularization();
00081         LOG( LVL_DEBUG ) << "done!" << endl;
00082 
00083         LOG( LVL_DEBUG ) << "Solver: setup Evaluation... " << endl;
00084         setupEvaluation();
00085         LOG( LVL_DEBUG ) << "done!" << endl;
00086 
00087         LOG( LVL_DEBUG ) << "Solver: setup setupAuxiliaryFunctions... " << endl;
00088         setupAuxiliaryFunctions();
00089         LOG( LVL_DEBUG ) << "done!" << endl;
00090 
00091         return SUCCESSFUL_RETURN;
00092 }
00093 
00094 returnValue ExportExactHessianQpDunes::getFunctionDeclarations( ExportStatementBlock& declarations
00095                                                                                                                                 ) const
00096 {
00097         ExportGaussNewtonQpDunes::getFunctionDeclarations( declarations );
00098 
00099         declarations.addDeclaration( regularization );
00100 
00101         return SUCCESSFUL_RETURN;
00102 }
00103 
00104 returnValue ExportExactHessianQpDunes::getCode( ExportStatementBlock& code
00105                                                                                                                 )
00106 {
00107         setupQPInterface();
00108         code.addStatement( *qpInterface );
00109 
00110         code.addLinebreak( 2 );
00111         code.addStatement( "/******************************************************************************/\n" );
00112         code.addStatement( "/*                                                                            */\n" );
00113         code.addStatement( "/* ACADO code generation                                                      */\n" );
00114         code.addStatement( "/*                                                                            */\n" );
00115         code.addStatement( "/******************************************************************************/\n" );
00116         code.addLinebreak( 2 );
00117 
00118         int useOMP;
00119         get(CG_USE_OPENMP, useOMP);
00120         if ( useOMP )
00121         {
00122                 code.addDeclaration( state );
00123         }
00124 
00125         code.addFunction( modelSimulation );
00126 
00127         code.addFunction( evaluateStageCost );
00128         code.addFunction( evaluateTerminalCost );
00129         code.addFunction( setObjQ1Q2 );
00130         code.addFunction( setObjR1R2 );
00131         code.addFunction( setObjQN1QN2 );
00132         code.addFunction( setStageH );
00133         code.addFunction( setStagef );
00134         code.addFunction( evaluateObjective );
00135 
00136         code.addFunction( regularizeHessian );
00137 
00138         code.addFunction( evaluatePathConstraints );
00139 
00140         for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
00141         {
00142                 if (evaluatePointConstraints[ i ] == 0)
00143                         continue;
00144                 code.addFunction( *evaluatePointConstraints[ i ] );
00145         }
00146 
00147         code.addFunction( setStagePac );
00148         code.addFunction( evaluateConstraints );
00149 
00150         code.addFunction( acc );
00151 
00152         code.addFunction( preparation );
00153         code.addFunction( feedback );
00154 
00155         code.addFunction( initialize );
00156         code.addFunction( initializeNodes );
00157         code.addFunction( shiftStates );
00158         code.addFunction( shiftControls );
00159         code.addFunction( getKKT );
00160         code.addFunction( getObjective );
00161 
00162         code.addFunction( cleanup );
00163         code.addFunction( shiftQpData );
00164 
00165         return SUCCESSFUL_RETURN;
00166 }
00167 
00168 //
00169 // PROTECTED FUNCTIONS:
00170 //
00171 
00172 returnValue ExportExactHessianQpDunes::setupObjectiveEvaluation( void )
00173 {
00174         evaluateObjective.setup("evaluateObjective");
00175 
00176         //
00177         // A loop the evaluates objective and corresponding gradients
00178         //
00179         ExportIndex runObj( "runObj" );
00180         ExportForLoop loopObjective( runObj, 0, N );
00181 
00182         evaluateObjective.addIndex( runObj );
00183 
00184         // Interface variable to qpDUNES
00185         qpH.setup("qpH", N * (NX + NU) * (NX + NU) + NX * NX, 1, REAL, ACADO_WORKSPACE);   // --> to be used only after regularization to pass to qpDUNES
00186         qpg.setup("qpG", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
00187 
00188         // LM regularization preparation
00189 
00190         ExportVariable evLmX = zeros<double>(NX, NX);
00191         ExportVariable evLmU = zeros<double>(NU, NU);
00192 
00193         if  (levenbergMarquardt > 0.0)
00194         {
00195                 DMatrix lmX = eye<double>( NX );
00196                 lmX *= levenbergMarquardt;
00197 
00198                 DMatrix lmU = eye<double>( NU );
00199                 lmU *= levenbergMarquardt;
00200 
00201                 evLmX = lmX;
00202                 evLmU = lmU;
00203         }
00204 
00205         ExportVariable stagef;
00206         stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
00207 
00208         ExportVariable stageH;
00209         stageH.setup("stageH", NX + NU, NX + NU, REAL, ACADO_LOCAL);
00210 
00211         if( evaluateStageCost.getFunctionDim() > 0 ) {
00212                 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00213                 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00214                 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od );
00215                 loopObjective.addLinebreak( );
00216 
00217                 // Evaluate the objective function
00218                 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00219                 loopObjective.addLinebreak( );
00220 
00221                 ExportVariable tmpFxx, tmpFxu, tmpFuu;
00222                 tmpFxx.setup("tmpFxx", NX, NX, REAL, ACADO_LOCAL);
00223                 tmpFxu.setup("tmpFxu", NX, NU, REAL, ACADO_LOCAL);
00224                 tmpFuu.setup("tmpFuu", NU, NU, REAL, ACADO_LOCAL);
00225 
00226                 setStageH.setup("addObjTerm", tmpFxx, tmpFxu, tmpFuu, stageH);
00227                 setStageH.addStatement( stageH.getSubMatrix(0,NX,0,NX) += tmpFxx + evLmX );
00228                 setStageH.addStatement( stageH.getSubMatrix(0,NX,NX,NX+NU) += tmpFxu );
00229                 setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,0,NX) += tmpFxu.getTranspose() );
00230                 setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,NX,NX+NU) += tmpFuu + evLmU );
00231 
00232                 loopObjective.addFunctionCall(
00233                                 setStageH, objValueOut.getAddress(0, 1+NX+NU), objValueOut.getAddress(0, 1+NX+NU+NX*NX),
00234                                 objValueOut.getAddress(0, 1+NX+NU+NX*(NX+NU)), objS.getAddress(runObj*(NX+NU), 0) );
00235 
00236                 ExportVariable tmpDF;
00237                 tmpDF.setup("tmpDF", NX+NU, 1, REAL, ACADO_LOCAL);
00238                 setStagef.setup("addObjLinearTerm", tmpDF, stagef);
00239                 setStagef.addStatement( stagef == tmpDF.getRows(0,NX+NU) );
00240 
00241                 loopObjective.addFunctionCall(
00242                                 setStagef, objValueOut.getAddress(0, 1), qpg.getAddress(runObj * (NX+NU)) );
00243 
00244                 loopObjective.addLinebreak( );
00245         }
00246         else {
00247                 if(levenbergMarquardt > 0.0) {
00248                         setStageH.setup("addObjTerm", stageH);
00249                         setStageH.addStatement( stageH.getSubMatrix(0,NX,0,NX) += evLmX );
00250                         setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,NX,NX+NU) += evLmU );
00251 
00252                         loopObjective.addFunctionCall( setStageH, objS.getAddress(runObj*(NX+NU), 0) );
00253                 }
00254                 DMatrix D(NX+NU,1); D.setAll(0);
00255                 loopObjective.addStatement( qpg.getRows(runObj*(NX+NU), runObj*(NX+NU)+NX+NU) == D );
00256         }
00257 
00258         evaluateObjective.addStatement( loopObjective );
00259 
00260         //
00261         // Evaluate the quadratic Mayer term
00262         //
00263         if( evaluateTerminalCost.getFunctionDim() > 0 ) {
00264                 evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00265                 evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od );
00266 
00267                 // Evaluate the objective function, last node.
00268                 evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00269                 evaluateObjective.addLinebreak( );
00270 
00271                 evaluateObjective.addStatement( objSEndTerm.makeRowVector() == objValueOut.getCols(1+NX,1+NX+NX*NX) + evLmX.makeRowVector() );
00272                 evaluateObjective.addStatement( qpg.getRows(N * NX, (N + 1) * NX) == objValueOut.getCols(1,1+NX).getTranspose() );
00273 
00274                 evaluateObjective.addLinebreak( );
00275         }
00276         else {
00277                 if(levenbergMarquardt > 0.0) {
00278                         evaluateObjective.addStatement( objSEndTerm == evLmX );
00279                 }
00280                 else {
00281                         DMatrix hess(NX,NX); hess.setAll(0);
00282                         evaluateObjective.addStatement( objSEndTerm == hess );
00283                 }
00284 
00285                 DMatrix Dx(NX,1); Dx.setAll(0);
00286                 evaluateObjective.addStatement( qpg.getRows(N*NX, (N+1)*NX) == Dx );
00287         }
00288 
00289         return SUCCESSFUL_RETURN;
00290 }
00291 
00292 returnValue ExportExactHessianQpDunes::setupHessianRegularization( )
00293 {
00294         ExportVariable block( "hessian_block", NX+NU, NX+NU );
00295         regularization = ExportFunction( "acado_regularize", block );
00296         regularization.doc( "EVD-based regularization of a Hessian block." );
00297         regularization.addLinebreak();
00298 
00299         regularizeHessian.setup( "regularizeHessian" );
00300         regularizeHessian.doc( "Regularization procedure of the computed exact Hessian." );
00301 
00302         ExportIndex oInd;
00303         regularizeHessian.acquire( oInd );
00304 
00305         ExportForLoop loopObjective(oInd, 0, N);
00306         loopObjective.addFunctionCall( regularization, objS.getAddress(oInd*(NX+NU),0) );
00307         for( uint row = 0; row < NX+NU; row++ ) {
00308                 loopObjective.addStatement( qpH.getRows((oInd*(NX+NU)+row)*(NX+NU),(oInd*(NX+NU)+row+1)*(NX+NU)) == objS.getRow(oInd*(NX+NU)+row).getTranspose() );
00309         }
00310         regularizeHessian.addStatement( loopObjective );
00311 
00312         regularizeHessian.addStatement( qpH.getRows(N*(NX+NU)*(NX+NU), N*(NX+NU)*(NX+NU)+NX*NX) == objSEndTerm.makeColVector() );
00313 
00314         return SUCCESSFUL_RETURN;
00315 }
00316 
00317 CLOSE_NAMESPACE_ACADO


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