export_exact_hessian_cn2.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00032 #include <acado/code_generation/export_exact_hessian_cn2.hpp>
00033 #include <acado/code_generation/export_qpoases_interface.hpp>
00034 
00035 using namespace std;
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 ExportExactHessianCN2::ExportExactHessianCN2(   UserInteraction* _userInteraction,
00040                                                                                         const std::string& _commonHeaderName
00041                                                                                         ) : ExportGaussNewtonCN2( _userInteraction,_commonHeaderName )
00042 {}
00043 
00044 returnValue ExportExactHessianCN2::setup( )
00045 {
00046         std::cout << "NOTE: You are using the new (unstable) N2 condensing feature for exact Hessian based RTI..\n";
00047 
00048         if (performFullCondensing() == false && initialStateFixed() == true)
00049                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00050         if (getNumComplexConstraints() > 0)
00051                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00052         if (performsSingleShooting() == true)
00053                 return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00054 
00055         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00056         setupInitialization();
00057         LOG( LVL_DEBUG ) << "done!" << endl;
00058 
00059         LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
00060         setupVariables();
00061         LOG( LVL_DEBUG ) << "done!" << endl;
00062 
00063         LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
00064         setupMultiplicationRoutines();
00065         LOG( LVL_DEBUG ) << "done!" << endl;
00066 
00067         LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
00068         setupSimulation();
00069         LOG( LVL_DEBUG ) << "done!" << endl;
00070 
00071         LOG( LVL_DEBUG ) << "Solver: setup objective evaluation... " << endl;
00072         setupObjectiveEvaluation();
00073         LOG( LVL_DEBUG ) << "done!" << endl;
00074 
00075         LOG( LVL_DEBUG ) << "Solver: setup condensing... " << endl;
00076         setupCondensing();
00077         LOG( LVL_DEBUG ) << "done!" << endl;
00078 
00079         LOG( LVL_DEBUG ) << "Solver: setup constraints... " << endl;
00080         setupConstraintsEvaluation();
00081         LOG( LVL_DEBUG ) << "done!" << endl;
00082 
00083         LOG( LVL_DEBUG ) << "Solver: setup hessian regularization... " << endl;
00084         setupHessianRegularization();
00085         LOG( LVL_DEBUG ) << "done!" << endl;
00086 
00087         LOG( LVL_DEBUG ) << "Solver: setup evaluation... " << endl;
00088         setupEvaluation();
00089         LOG( LVL_DEBUG ) << "done!" << endl;
00090 
00091         LOG( LVL_DEBUG ) << "Solver: setup auxiliary functions... " << endl;
00092         setupAuxiliaryFunctions();
00093         LOG( LVL_DEBUG ) << "done!" << endl;
00094 
00095         return SUCCESSFUL_RETURN;
00096 }
00097 
00098 returnValue ExportExactHessianCN2::getFunctionDeclarations(     ExportStatementBlock& declarations
00099                                                                                                                         ) const
00100 {
00101         ExportGaussNewtonCN2::getFunctionDeclarations( declarations );
00102 
00103         declarations.addDeclaration( regularization );
00104 
00105         return SUCCESSFUL_RETURN;
00106 }
00107 
00108 //
00109 // PROTECTED FUNCTIONS:
00110 //
00111 
00112 returnValue ExportExactHessianCN2::setupObjectiveEvaluation( void )
00113 {
00114         evaluateObjective.setup("evaluateObjective");
00115 
00116         //
00117         // A loop the evaluates objective and corresponding gradients
00118         //
00119         ExportIndex runObj( "runObj" );
00120         ExportForLoop loopObjective( runObj, 0, N );
00121 
00122         evaluateObjective.addIndex( runObj );
00123 
00124         unsigned offset = performFullCondensing() == true ? 0 : NX;
00125 
00126         if( evaluateStageCost.getFunctionDim() > 0 ) {
00127                 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00128                 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00129                 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od );
00130                 loopObjective.addLinebreak( );
00131 
00132                 // Evaluate the objective function
00133                 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00134                 loopObjective.addLinebreak( );
00135 
00136                 ExportVariable tmpFxx, tmpFxu, tmpFuu;
00137                 tmpFxx.setup("tmpFxx", NX, NX, REAL, ACADO_LOCAL);
00138                 tmpFxu.setup("tmpFxu", NX, NU, REAL, ACADO_LOCAL);
00139                 tmpFuu.setup("tmpFuu", NU, NU, REAL, ACADO_LOCAL);
00140 
00141                 //
00142                 // Optional computation of Q1, Q2
00143                 //
00144                 ExportVariable tmpEH;
00145                 tmpEH.setup("tmpEH", NX+NU, NX+NU, REAL, ACADO_LOCAL);
00146 
00147                 setObjQ1Q2.setup("addObjTerm", tmpFxx, tmpFxu, tmpFuu, tmpEH);
00148                 setObjQ1Q2.addStatement( tmpEH.getSubMatrix(0,NX,0,NX) += tmpFxx );
00149                 setObjQ1Q2.addStatement( tmpEH.getSubMatrix(0,NX,NX,NX+NU) += tmpFxu );
00150                 setObjQ1Q2.addStatement( tmpEH.getSubMatrix(NX,NX+NU,0,NX) += tmpFxu.getTranspose() );
00151                 setObjQ1Q2.addStatement( tmpEH.getSubMatrix(NX,NX+NU,NX,NX+NU) += tmpFuu );
00152 
00153                 loopObjective.addFunctionCall(
00154                                 setObjQ1Q2, objValueOut.getAddress(0, 1+NX+NU), objValueOut.getAddress(0, 1+NX+NU+NX*NX),
00155                                 objValueOut.getAddress(0, 1+NX+NU+NX*(NX+NU)), objS.getAddress(runObj*(NX+NU), 0) );
00156 
00157                 ExportVariable tmpDx, tmpDu, tmpDF;
00158                 tmpDx.setup("tmpDx", NX, 1, REAL, ACADO_LOCAL);
00159                 tmpDu.setup("tmpDu", NU, 1, REAL, ACADO_LOCAL);
00160                 tmpDF.setup("tmpDF", NX+NU, 1, REAL, ACADO_LOCAL);
00161                 setObjR1R2.setup("addObjLinearTerm", tmpDx, tmpDu, tmpDF);
00162                 setObjR1R2.addStatement( tmpDx == tmpDF.getRows(0,NX) );
00163                 setObjR1R2.addStatement( tmpDu == tmpDF.getRows(NX,NX+NU) );
00164 
00165                 loopObjective.addFunctionCall(
00166                                 setObjR1R2, QDy.getAddress(runObj * NX), g.getAddress(offset+runObj * NU, 0), objValueOut.getAddress(0, 1) );
00167 
00168                 loopObjective.addLinebreak( );
00169         }
00170         else {
00171                 DMatrix Du(NU,1); Du.setAll(0);
00172                 DMatrix Dx(NX,1); Dx.setAll(0);
00173                 loopObjective.addStatement( g.getRows(offset+runObj*NU, offset+runObj*NU+NU) == Du );
00174                 loopObjective.addStatement( QDy.getRows(runObj*NX, runObj*NX+NX) == Dx );
00175         }
00176 
00177         evaluateObjective.addStatement( loopObjective );
00178 
00179         //
00180         // Evaluate the quadratic Mayer term
00181         //
00182         if( evaluateTerminalCost.getFunctionDim() > 0 ) {
00183                 evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00184                 evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od );
00185 
00186                 // Evaluate the objective function, last node.
00187                 evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00188                 evaluateObjective.addLinebreak( );
00189 
00190                 ExportVariable tmpFxxEnd;
00191                 tmpFxxEnd.setup("tmpFxxEnd", NX, NX, REAL, ACADO_LOCAL);
00192 
00193                 //
00194                 // Optional computation of QN1
00195                 //
00196                 ExportVariable tmpEH_N;
00197                 tmpEH_N.setup("tmpEH_N", NX, NX, REAL, ACADO_LOCAL);
00198 
00199                 setObjQN1QN2.setup("addObjEndTerm", tmpFxxEnd, tmpEH_N);
00200                 setObjQN1QN2.addStatement( tmpEH_N == tmpFxxEnd );
00201 
00202                 evaluateObjective.addFunctionCall(
00203                                 setObjQN1QN2, objValueOut.getAddress(0, 1+NX), objSEndTerm );
00204 
00205                 evaluateObjective.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == objValueOut.getCols(1,1+NX).getTranspose() );
00206 
00207                 evaluateObjective.addLinebreak( );
00208         }
00209         else {
00210                 DMatrix hess(NX,NX); hess.setAll(0);
00211                 evaluateObjective.addStatement(objSEndTerm == hess);
00212 
00213                 DMatrix Dx(NX,1); Dx.setAll(0);
00214                 evaluateObjective.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == Dx );
00215         }
00216 
00217         return SUCCESSFUL_RETURN;
00218 }
00219 
00220 returnValue ExportExactHessianCN2::setupHessianRegularization( )
00221 {
00222         ExportVariable block( "hessian_block", NX+NU, NX+NU );
00223         regularization = ExportFunction( "acado_regularize", block );
00224         regularization.doc( "EVD-based regularization of a Hessian block." );
00225         regularization.addLinebreak();
00226 
00227         regularizeHessian.setup( "regularizeHessian" );
00228         regularizeHessian.doc( "Regularization procedure of the computed exact Hessian." );
00229 
00230         ExportIndex oInd;
00231         regularizeHessian.acquire( oInd );
00232 
00233         ExportForLoop loopObjective(oInd, 0, N);
00234         loopObjective.addFunctionCall( regularization, objS.getAddress(oInd*(NX+NU),0) );
00235         loopObjective.addStatement( Q1.getRows(oInd*NX, oInd*NX+NX) == objS.getSubMatrix(oInd*(NX+NU), oInd*(NX+NU)+NX, 0, NX) );
00236         loopObjective.addStatement( S1.getRows(oInd*NX, oInd*NX+NX) == objS.getSubMatrix(oInd*(NX+NU), oInd*(NX+NU)+NX, NX, NX+NU) );
00237         loopObjective.addStatement( R1.getRows(oInd*NU, oInd*NU+NU) == objS.getSubMatrix(oInd*(NX+NU)+NX, oInd*(NX+NU)+NX+NU, NX, NX+NU) );
00238         regularizeHessian.addStatement( loopObjective );
00239 
00240         regularizeHessian.addStatement( QN1 == objSEndTerm );
00241 
00242         return SUCCESSFUL_RETURN;
00243 }
00244 
00245 CLOSE_NAMESPACE_ACADO


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