export_gauss_newton_forces.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_forces.hpp>
00033 #include <acado/code_generation/export_module.hpp>
00034 
00035 #include <acado/code_generation/export_forces_interface.hpp>
00036 #include <acado/code_generation/export_forces_generator.hpp>
00037 
00038 #include <acado/code_generation/templates/templates.hpp>
00039 
00040 BEGIN_NAMESPACE_ACADO
00041 
00042 using namespace std;
00043 
00044 ExportGaussNewtonForces::ExportGaussNewtonForces(       UserInteraction* _userInteraction,
00045                                                                                                         const std::string& _commonHeaderName
00046                                                                                                         ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
00047 {
00048         qpObjPrefix = "acadoForces";
00049         qpModuleName = "forces";
00050         diagH = diagHN = false;
00051 }
00052 
00053 returnValue ExportGaussNewtonForces::setup( )
00054 {
00055         LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
00056         setupInitialization();
00057         // Add QP initialization call to the initialization
00058         ExportFunction initializeForces( "initializeForces" );
00059         initialize.addFunctionCall( initializeForces );
00060 
00061         LOG( LVL_DEBUG ) << "done!" << endl;
00062 
00063         setupVariables();
00064 
00065         setupSimulation();
00066 
00067         setupObjectiveEvaluation();
00068 
00069         setupConstraintsEvaluation();
00070 
00071         setupEvaluation();
00072 
00073         setupAuxiliaryFunctions();
00074 
00075         return SUCCESSFUL_RETURN;
00076 }
00077 
00078 returnValue ExportGaussNewtonForces::getDataDeclarations(       ExportStatementBlock& declarations,
00079                                                                                                                         ExportStruct dataStruct
00080                                                                                                                         ) const
00081 {
00082         returnValue status;
00083         status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
00084         if (status != SUCCESSFUL_RETURN)
00085                 return status;
00086 
00087         declarations.addDeclaration(x0, dataStruct);
00088 
00089         declarations.addDeclaration(lbValues, dataStruct);
00090         declarations.addDeclaration(ubValues, dataStruct);
00091 
00092         return SUCCESSFUL_RETURN;
00093 }
00094 
00095 returnValue ExportGaussNewtonForces::getFunctionDeclarations(   ExportStatementBlock& declarations
00096                                                                                                                                 ) const
00097 {
00098         declarations.addDeclaration( preparation );
00099         declarations.addDeclaration( feedback );
00100 
00101         declarations.addDeclaration( initialize );
00102         declarations.addDeclaration( initializeNodes );
00103         declarations.addDeclaration( shiftStates );
00104         declarations.addDeclaration( shiftControls );
00105         declarations.addDeclaration( getKKT );
00106         declarations.addDeclaration( getObjective );
00107 
00108         declarations.addDeclaration( evaluateStageCost );
00109         declarations.addDeclaration( evaluateTerminalCost );
00110 
00111         return SUCCESSFUL_RETURN;
00112 }
00113 
00114 returnValue ExportGaussNewtonForces::getCode(   ExportStatementBlock& code
00115                                                                                                                 )
00116 {
00117         setupQPInterface();
00118         code.addStatement( *qpInterface );
00119 
00120         code.addLinebreak( 2 );
00121         code.addStatement( "/******************************************************************************/\n" );
00122         code.addStatement( "/*                                                                            */\n" );
00123         code.addStatement( "/* ACADO code generation                                                      */\n" );
00124         code.addStatement( "/*                                                                            */\n" );
00125         code.addStatement( "/******************************************************************************/\n" );
00126         code.addLinebreak( 2 );
00127 
00128         int useOMP;
00129         get(CG_USE_OPENMP, useOMP);
00130         if ( useOMP )
00131         {
00132                 code.addDeclaration( state );
00133         }
00134 
00135         code.addFunction( modelSimulation );
00136 
00137         code.addFunction( evaluateStageCost );
00138         code.addFunction( evaluateTerminalCost );
00139         code.addFunction( setObjQ1Q2 );
00140         code.addFunction( setObjR1R2 );
00141         code.addFunction( setObjQN1QN2 );
00142         code.addFunction( setStageH );
00143         code.addFunction( setStagef );
00144         code.addFunction( evaluateObjective );
00145 
00146         code.addFunction( conSetGxGu );
00147         code.addFunction( conSetd );
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         return SUCCESSFUL_RETURN;
00163 }
00164 
00165 
00166 unsigned ExportGaussNewtonForces::getNumQPvars( ) const
00167 {
00168         return (N + 1) * NX + N * NU;
00169 }
00170 
00171 //
00172 // PROTECTED FUNCTIONS:
00173 //
00174 
00175 returnValue ExportGaussNewtonForces::setupObjectiveEvaluation( void )
00176 {
00177         evaluateObjective.setup("evaluateObjective");
00178 
00179         int variableObjS;
00180         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00181         int forceDiagHessian;
00182         get(CG_FORCE_DIAGONAL_HESSIAN, forceDiagHessian);
00183 
00184         if (S1.isGiven() == false || S1.getGivenMatrix().isZero() == false)
00185                 ACADOWARNINGTEXT(RET_NOT_IMPLEMENTED_YET,
00186                                 "Mixed control-state terms in the objective function are not supported at the moment.");
00187 
00188         diagH = false;
00189         diagHN = false;
00190         unsigned dimHRows = NX + NU;
00191         unsigned dimHCols = NX + NU;
00192         unsigned dimHNRows = NX;
00193         unsigned dimHNCols = NX;
00194         if (objS.isGiven() == true || forceDiagHessian == true)
00195                 if (objS.getGivenMatrix().isDiagonal())
00196                 {
00197                         diagH = true;
00198                         dimHCols = 1;
00199                 }
00200 
00201         if (objSEndTerm.isGiven() == true || forceDiagHessian == true)
00202                 if (objSEndTerm.getGivenMatrix().isDiagonal() == true)
00203                 {
00204                         diagHN = true;
00205                         dimHNCols = 1;
00206                 }
00207 
00208         objHessians.resize(N + 1);
00209         for (unsigned i = 0; i < N; ++i)
00210         {
00211                 objHessians[ i ].setup(string("H") + toString(i + 1), dimHRows, dimHCols, REAL, FORCES_PARAMS, false, qpObjPrefix);
00212         }
00213         objHessians[ N ].setup(string("H") + toString(N + 1), dimHNRows, dimHNCols, REAL, FORCES_PARAMS, false, qpObjPrefix);
00214 
00215         objGradients.resize(N + 1);
00216         for (unsigned i = 0; i < N; ++i)
00217         {
00218                 objGradients[ i ].setup(string("f") + toString(i + 1), NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00219         }
00220         objGradients[ N ].setup(string("f") + toString(N + 1), NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00221 
00222         //
00223         // LM regularization preparation
00224         //
00225 
00226         ExportVariable evLmX = zeros<double>(NX, NX);
00227         ExportVariable evLmU = zeros<double>(NU, NU);
00228 
00229         if (levenbergMarquardt > 0.0)
00230         {
00231                 DMatrix lmX = eye<double>( NX );
00232                 lmX *= levenbergMarquardt;
00233 
00234                 DMatrix lmU = eye<double>( NU );
00235                 lmU *= levenbergMarquardt;
00236 
00237                 evLmX = lmX;
00238                 evLmU = lmU;
00239         }
00240 
00241         //
00242         // Main loop that calculates Hessian and gradients
00243         //
00244 
00245         ExportIndex runObj( "runObj" );
00246         ExportForLoop loopObjective( runObj, 0, N );
00247 
00248         evaluateObjective.addIndex( runObj );
00249 
00250         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
00251         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
00252         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
00253         loopObjective.addLinebreak( );
00254 
00255         // Evaluate the objective function
00256         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
00257 
00258         // Stack the measurement function value
00259         loopObjective.addStatement(
00260                         Dy.getRows(runObj * NY, (runObj + 1) * NY) ==  objValueOut.getTranspose().getRows(0, getNY())
00261         );
00262         loopObjective.addLinebreak( );
00263 
00264         // Optionally compute derivatives
00265         unsigned indexX = getNY();
00266         //      unsigned indexG = indexX;
00267 
00268         ExportVariable tmpObjS, tmpFx, tmpFu;
00269         ExportVariable tmpFxEnd, tmpObjSEndTerm;
00270         tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
00271         if (objS.isGiven() == true)
00272                 tmpObjS = objS;
00273         tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
00274         if (objEvFx.isGiven() == true)
00275                 tmpFx = objEvFx;
00276         tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
00277         if (objEvFu.isGiven() == true)
00278                 tmpFu = objEvFu;
00279         tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
00280         if (objEvFxEnd.isGiven() == true)
00281                 tmpFxEnd = objEvFxEnd;
00282         tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
00283         if (objSEndTerm.isGiven() == true)
00284                 tmpObjSEndTerm = objSEndTerm;
00285 
00286         //
00287         // Optional computation of Q1, Q2
00288         //
00289         if (Q1.isGiven() == false)
00290         {
00291                 ExportVariable tmpQ1, tmpQ2;
00292                 tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
00293                 tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
00294 
00295                 setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
00296                 setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
00297                 setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
00298 
00299                 if (tmpFx.isGiven() == true)
00300                 {
00301                         if (variableObjS == YES)
00302                         {
00303                                 loopObjective.addFunctionCall(
00304                                                 setObjQ1Q2,
00305                                                 tmpFx, objS.getAddress(runObj * NY, 0),
00306                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00307                                 );
00308                         }
00309                         else
00310                         {
00311                                 loopObjective.addFunctionCall(
00312                                                 setObjQ1Q2,
00313                                                 tmpFx, objS,
00314                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00315                                 );
00316                         }
00317                 }
00318                 else
00319                 {
00320                         if (variableObjS == YES)
00321                         {
00322                                 if (objEvFx.isGiven() == true)
00323 
00324                                         loopObjective.addFunctionCall(
00325                                                         setObjQ1Q2,
00326                                                         objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00327                                                         Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00328                                         );
00329                         }
00330                         else
00331                         {
00332                                 loopObjective.addFunctionCall(
00333                                                 setObjQ1Q2,
00334                                                 objValueOut.getAddress(0, indexX), objS,
00335                                                 Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
00336                                 );
00337                         }
00338                         indexX += objEvFx.getDim();
00339                 }
00340 
00341                 loopObjective.addLinebreak( );
00342         }
00343 
00344         if (R1.isGiven() == false)
00345         {
00346                 ExportVariable tmpR1, tmpR2;
00347                 tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
00348                 tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
00349 
00350                 setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
00351                 setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
00352                 setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
00353 
00354                 if (tmpFu.isGiven() == true)
00355                 {
00356                         if (variableObjS == YES)
00357                         {
00358                                 loopObjective.addFunctionCall(
00359                                                 setObjR1R2,
00360                                                 tmpFu, objS.getAddress(runObj * NY, 0),
00361                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00362                                 );
00363                         }
00364                         else
00365                         {
00366                                 loopObjective.addFunctionCall(
00367                                                 setObjR1R2,
00368                                                 tmpFu, objS,
00369                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00370                                 );
00371                         }
00372                 }
00373                 else
00374                 {
00375                         if (variableObjS == YES)
00376                         {
00377                                 loopObjective.addFunctionCall(
00378                                                 setObjR1R2,
00379                                                 objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
00380                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00381                                 );
00382                         }
00383                         else
00384                         {
00385                                 loopObjective.addFunctionCall(
00386                                                 setObjR1R2,
00387                                                 objValueOut.getAddress(0, indexX), objS,
00388                                                 R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
00389                                 );
00390                         }
00391                 }
00392 
00393                 loopObjective.addLinebreak( );
00394         }
00395 
00396         evaluateObjective.addStatement( loopObjective );
00397 
00398         //
00399         // Evaluate the quadratic Mayer term
00400         //
00401         evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
00402         evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
00403 
00404         // Evaluate the objective function
00405         evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
00406         evaluateObjective.addLinebreak( );
00407 
00408         evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
00409         evaluateObjective.addLinebreak();
00410 
00411         if (QN1.isGiven() == false)
00412         {
00413                 indexX = getNYN();
00414 
00415                 ExportVariable tmpQN1, tmpQN2;
00416                 tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
00417                 tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
00418 
00419                 setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
00420                 setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
00421                 setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
00422 
00423                 if (tmpFxEnd.isGiven() == true)
00424                         evaluateObjective.addFunctionCall(
00425                                         setObjQN1QN2,
00426                                         tmpFxEnd, objSEndTerm,
00427                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00428                         );
00429                 else
00430                         evaluateObjective.addFunctionCall(
00431                                         setObjQN1QN2,
00432                                         objValueOut.getAddress(0, indexX), objSEndTerm,
00433                                         QN1.getAddress(0, 0), QN2.getAddress(0, 0)
00434                         );
00435 
00436                 evaluateObjective.addLinebreak( );
00437         }
00438 
00439         //
00440         // Hessian setup
00441         //
00442 
00443         ExportVariable stageH;
00444         ExportIndex index( "index" );
00445         stageH.setup("stageH", dimHRows, dimHCols, REAL, ACADO_LOCAL);
00446         setStageH.setup("setStageH", stageH, index);
00447 
00448         if (Q1.isGiven() == false)
00449         {
00450                 if (diagH == false)
00451                         setStageH.addStatement(
00452                                         stageH.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(index * NX, (index + 1) * NX, 0, NX) + evLmX
00453                         );
00454                 else
00455                         for (unsigned el = 0; el < NX; ++el)
00456                                 setStageH.addStatement(
00457                                                 stageH.getElement(el, 0) == Q1.getElement(index * NX + el, el)
00458                                 );
00459         }
00460         else
00461         {
00462                 setStageH << index.getFullName() << " = " << index.getFullName() << ";\n";
00463                 if (diagH == false)
00464                 {
00465                         setStageH.addStatement(
00466                                         stageH.getSubMatrix(0, NX, 0, NX) == Q1 + evLmX
00467                         );
00468                 }
00469                 else
00470                 {
00471                         setStageH.addStatement(
00472                                         stageH.getRows(0, NX) == Q1.getGivenMatrix().getDiag() + evLmX.getGivenMatrix().getDiag()
00473                         );
00474                 }
00475         }
00476         setStageH.addLinebreak();
00477 
00478         if (R1.isGiven() == false)
00479         {
00480                 if (diagH == false)
00481                         setStageH.addStatement(
00482                                         stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1.getSubMatrix(index * NU, (index + 1) * NU, 0, NU) + evLmU
00483                         );
00484                 else
00485                         for (unsigned el = 0; el < NU; ++el)
00486                                 setStageH.addStatement(
00487                                                 stageH.getElement(NX + el, 0) == R1.getElement(index * NU + el, el)
00488                                 );
00489         }
00490         else
00491         {
00492                 if (diagH == false)
00493                 {
00494                         setStageH.addStatement(
00495                                         stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1 + evLmU
00496                         );
00497                 }
00498                 else
00499                 {
00500                         setStageH.addStatement(
00501                                         stageH.getRows(NX, NX + NU) == R1.getGivenMatrix().getDiag() + evLmU.getGivenMatrix().getDiag()
00502                         );
00503                 }
00504         }
00505 
00506         if (Q1.isGiven() == true && R1.isGiven() == true)
00507         {
00508                 initialize <<
00509                                 setStageH.getName() << "( " << objHessians[ 0 ].getFullName() << ", " << "0" << " );\n";
00510                 initialize.addLinebreak();
00511                 if (diagHN == false)
00512                 {
00513                         initialize.addStatement(
00514                                         objHessians[ N ] == QN1 + evLmX
00515                         );
00516                 }
00517                 else
00518                 {
00519                         initialize.addStatement(
00520                                         objHessians[ N ] == QN1.getGivenMatrix().getDiag() + evLmX.getGivenMatrix().getDiag()
00521                         );
00522                 }
00523         }
00524         else
00525         {
00526                 for (unsigned i = 0; i < N; ++i)
00527                         evaluateObjective.addFunctionCall(setStageH, objHessians[ i ], ExportIndex(i));
00528                 evaluateObjective.addLinebreak();
00529                 if (diagHN == false)
00530                         evaluateObjective.addStatement(
00531                                 objHessians[ N ] == QN1 + evLmX
00532                         );
00533                 else
00534                         for (unsigned el = 0; el < NX; ++el)
00535                                 evaluateObjective.addStatement(
00536                                                 objHessians[ N ].getElement(el, 0) == QN1.getElement(el, el) + evLmX.getElement(el, el)
00537                                 );
00538         }
00539 
00540         //
00541         // Gradient setup
00542         //
00543 
00544         ExportVariable stagef;
00545         stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
00546         setStagef.setup("setStagef", stagef, index);
00547 
00548         if (Q2.isGiven() == false)
00549                 setStagef.addStatement(
00550                                 stagef.getRows(0, NX) == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) *
00551                                 Dy.getRows(index * NY, (index + 1) * NY)
00552                 );
00553         else
00554         {
00555                 setStagef <<  index.getFullName() << " = " << index.getFullName() << ";\n";
00556                 setStagef.addStatement(
00557                                 stagef.getRows(0, NX) == Q2 *
00558                                 Dy.getRows(index * NY, (index + 1) * NY)
00559                 );
00560         }
00561         setStagef.addLinebreak();
00562 
00563         if (R2.isGiven() == false)
00564                 setStagef.addStatement(
00565                                 stagef.getRows(NX, NX + NU) == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) *
00566                                 Dy.getRows(index * NY, (index + 1) * NY)
00567                 );
00568         else
00569         {
00570                 setStagef.addStatement(
00571                                 stagef.getRows(NX, NX + NU) == R2 *
00572                                 Dy.getRows(index * NY, (index + 1) * NY)
00573                 );
00574         }
00575 
00576         return SUCCESSFUL_RETURN;
00577 }
00578 
00579 returnValue ExportGaussNewtonForces::setupConstraintsEvaluation( void )
00580 {
00582         //
00583         // Setup evaluation of box constraints on states and controls
00584         //
00586 
00587         conLB.clear();
00588         conLB.resize(N + 1);
00589 
00590         conUB.clear();
00591         conUB.resize(N + 1);
00592 
00593         conLBIndices.clear();
00594         conLBIndices.resize(N + 1);
00595 
00596         conUBIndices.clear();
00597         conUBIndices.resize(N + 1);
00598 
00599         conLBValues.clear();
00600         conLBValues.resize(N + 1);
00601 
00602         conUBValues.clear();
00603         conUBValues.resize(N + 1);
00604 
00605         DVector lbTmp, ubTmp;
00606 
00607         //
00608         // Stack state constraints
00609         //
00610         unsigned numLB = 0;
00611         unsigned numUB = 0;
00612         for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
00613         {
00614                 lbTmp = xBounds.getLowerBounds( i );
00615                 ubTmp = xBounds.getUpperBounds( i );
00616 
00617                 if (isFinite( lbTmp ) == false && isFinite( ubTmp ) == false)
00618                         continue;
00619 
00620                 for (unsigned j = 0; j < lbTmp.getDim(); ++j)
00621                 {
00622                         if (acadoIsFinite( lbTmp( j ) ) == true)
00623                         {
00624                                 conLBIndices[ i ].push_back( j );
00625                                 conLBValues[ i ].push_back( lbTmp( j ) );
00626                                 numLB++;
00627                         }
00628 
00629                         if (acadoIsFinite( ubTmp( j ) ) == true)
00630                         {
00631                                 conUBIndices[ i ].push_back( j );
00632                                 conUBValues[ i ].push_back( ubTmp( j ) );
00633                                 numUB++;
00634                         }
00635                 }
00636         }
00637 
00638         //
00639         // Stack control constraints
00640         //
00641         for (unsigned i = 0; i < uBounds.getNumPoints() && i < N; ++i)
00642         {
00643                 lbTmp = uBounds.getLowerBounds( i );
00644                 ubTmp = uBounds.getUpperBounds( i );
00645 
00646                 if (isFinite( lbTmp ) == false && isFinite( ubTmp ) == false)
00647                         continue;
00648 
00649                 for (unsigned j = 0; j < lbTmp.getDim(); ++j)
00650                 {
00651                         if (acadoIsFinite( lbTmp( j ) ) == true)
00652                         {
00653                                 conLBIndices[ i ].push_back(NX + j);
00654                                 conLBValues[ i ].push_back( lbTmp( j ) );
00655                                 numLB++;
00656                         }
00657 
00658                         if (acadoIsFinite( ubTmp( j ) ) == true)
00659                         {
00660                                 conUBIndices[ i ].push_back(NX + j);
00661                                 conUBValues[ i ].push_back( ubTmp( j ) );
00662                                 numUB++;
00663                         }
00664                 }
00665         }
00666 
00667         //
00668         // Setup variables
00669         //
00670         for (unsigned i = 0; i < N + 1; ++i)
00671         {
00672                 conLB[ i ].setup(string("lb") + toString(i + 1), conLBIndices[ i ].size(), 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00673                 conUB[ i ].setup(string("ub") + toString(i + 1), conUBIndices[ i ].size(), 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00674         }
00675 
00676         int hardcodeConstraintValues;
00677         get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
00678         uint numBounds = numLB+numUB;
00679         if (!hardcodeConstraintValues && numBounds > 0)
00680         {
00681                 lbValues.setup("lbValues", numLB, 1, REAL, ACADO_VARIABLES);
00682                 lbValues.setDoc( "Lower bounds values." );
00683                 ubValues.setup("ubValues", numUB, 1, REAL, ACADO_VARIABLES);
00684                 ubValues.setDoc( "Upper bounds values." );
00685         }
00686 
00687         evaluateConstraints.setup("evaluateConstraints");
00688 
00689         //
00690         // Export evaluation of simple box constraints
00691         //
00692         uint indexB = 0;
00693         for (unsigned i = 0; i < N + 1; ++i) {
00694                 for (unsigned j = 0; j < conLBIndices[ i ].size(); ++j)
00695                 {
00696                         if( hardcodeConstraintValues ) {
00697                                 evaluateConstraints << conLB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << toString(conLBValues[ i ][ j ]) << " - ";
00698                         }
00699                         else {
00700                                 evaluateConstraints << conLB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << lbValues.get( indexB,0 ) << " - ";
00701                         }
00702                         indexB++;
00703 
00704                         if (conLBIndices[ i ][ j ] < NX)
00705                                 evaluateConstraints << x.getFullName() << "[ " << toString(i * NX + conLBIndices[ i ][ j ]) << " ];\n";
00706                         else
00707                                 evaluateConstraints << u.getFullName() << "[ " << toString(i * NU + conLBIndices[ i ][ j ] - NX) << " ];\n";
00708                 }
00709         }
00710         evaluateConstraints.addLinebreak();
00711 
00712         indexB = 0;
00713         for (unsigned i = 0; i < N + 1; ++i)
00714                 for (unsigned j = 0; j < conUBIndices[ i ].size(); ++j)
00715                 {
00716                         if( hardcodeConstraintValues ) {
00717                                 evaluateConstraints << conUB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << toString(conUBValues[ i ][ j ]) << " - ";
00718                         }
00719                         else {
00720                                 evaluateConstraints << conUB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << ubValues.get( indexB,0 ) << " - ";
00721                         }
00722                         indexB++;
00723 
00724                         if (conUBIndices[ i ][ j ] < NX)
00725                                 evaluateConstraints << x.getFullName() << "[ " << toString(i * NX + conUBIndices[ i ][ j ]) << " ];\n";
00726                         else
00727                                 evaluateConstraints << u.getFullName() << "[ " << toString(i * NU + conUBIndices[ i ][ j ] - NX) << " ];\n";
00728                 }
00729         evaluateConstraints.addLinebreak();
00730 
00732         //
00733         // Evaluation of the equality constraints
00734         //  - system dynamics only
00735         //
00737 
00738         conC.clear();
00739         conC.resize( N );
00740 
00741         // XXX FORCES works with column major format
00742 //      if (initialStateFixed() == true)
00743 //              conC[ 0 ].setup("C1", NX + NU, 2 * NX, REAL, FORCES_PARAMS, false, qpObjPrefix);
00744 //      else
00745 //              conC[ 0 ].setup("C1", NX + NU, NX, REAL, FORCES_PARAMS, false, qpObjPrefix);
00746 
00747 //      for (unsigned i = 1; i < N; ++i)
00748         for (unsigned i = 0; i < N; ++i)
00749                 conC[ i ].setup(string("C") + toString(i + 1), NX + NU, NX, REAL, FORCES_PARAMS, false, qpObjPrefix);
00750 
00751         ExportIndex index( "index" );
00752         conStageC.setup("conStageC", NX + NU, NX, REAL);
00753         conSetGxGu.setup("conSetGxGu", conStageC, index);
00754 
00755         conSetGxGu.addStatement(
00756                         conStageC.getSubMatrix(0, NX, 0, NX) ==
00757                                         evGx.getSubMatrix(index * NX, (index + 1) * NX, 0, NX).getTranspose()
00758         );
00759         conSetGxGu.addLinebreak();
00760         conSetGxGu.addStatement(
00761                         conStageC.getSubMatrix(NX, NX + NU, 0, NX) ==
00762                                         evGu.getSubMatrix(index * NX, (index + 1) * NX, 0, NU).getTranspose()
00763         );
00764 
00765 //      if (initialStateFixed() == true)
00766 //      {
00767 //              initialize.addStatement(
00768 //                              conC[ 0 ].getSubMatrix(0, NX, 0, NX) == eye( NX )
00769 //              );
00770 //              evaluateConstraints.addLinebreak();
00771 //              evaluateConstraints.addStatement(
00772 //                              conC[ 0 ].getSubMatrix(0, NX, NX, 2 * NX) == evGx.getSubMatrix(0, NX, 0, NX).getTranspose()
00773 //              );
00774 //              evaluateConstraints.addLinebreak();
00775 //              evaluateConstraints.addStatement(
00776 //                              conC[ 0 ].getSubMatrix(NX, NX + NU, NX, 2 * NX) == evGu.getSubMatrix(0, NX, 0, NU).getTranspose()
00777 //              );
00778 //              evaluateConstraints.addLinebreak();
00779 //      }
00780 
00781         unsigned start = 0; //initialStateFixed() == true ? 1 : 0;
00782         for (unsigned i = start; i < N; ++i)
00783                 evaluateConstraints.addFunctionCall(conSetGxGu, conC[ i ], ExportIndex( i ));
00784         evaluateConstraints.addLinebreak();
00785 
00786         cond.clear();
00787 
00788         unsigned dNum = initialStateFixed() == true ? N + 1 : N;
00789         cond.resize(dNum);
00790 
00791 //      if (initialStateFixed() == true)
00792 //              cond[ 0 ].setup("d1", 2 * NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00793 //      else
00794 //              cond[ 0 ].setup("d1", NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00795 
00796         for (unsigned i = 0; i < dNum; ++i)
00797                 cond[ i ].setup(string("d") + toString(i + 1), NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00798 
00799         ExportVariable staged, stagedNew;
00800 
00801         staged.setup("staged", NX, 1, REAL, ACADO_LOCAL);
00802         stagedNew.setup("stagedNew", NX, 1, REAL, ACADO_LOCAL);
00803         conSetd.setup("conSetd", stagedNew, index);
00804         conSetd.addStatement(
00805                 stagedNew == zeros<double>(NX, 1) - d.getRows(index * NX, (index + 1) * NX)
00806         );
00807 
00808 //              evaluateConstraints.addStatement(
00809 //                              cond[ 0 ].getRows(NX, 2 * NX) == dummyZero - d.getRows(0, NX)
00810 //              );
00811 //              evaluateConstraints.addLinebreak();
00812 
00813         start = initialStateFixed() == true ? 1 : 0;
00814         for (unsigned i = start; i < dNum; ++i)
00815                 evaluateConstraints.addFunctionCall(
00816                                 conSetd, cond[ i ], ExportIndex(i - 1)
00817                 );
00818 
00819         return SUCCESSFUL_RETURN;
00820 }
00821 
00822 returnValue ExportGaussNewtonForces::setupVariables( )
00823 {
00824         if (initialStateFixed() == true)
00825         {
00826                 x0.setup("x0",  NX, 1, REAL, ACADO_VARIABLES);
00827                 x0.setDoc( "Current state feedback vector." );
00828         }
00829 
00830         return SUCCESSFUL_RETURN;
00831 }
00832 
00833 returnValue ExportGaussNewtonForces::setupMultiplicationRoutines( )
00834 {
00835         return SUCCESSFUL_RETURN;
00836 }
00837 
00838 returnValue ExportGaussNewtonForces::setupEvaluation( )
00839 {
00841         //
00842         // Setup preparation phase
00843         //
00845         preparation.setup("preparationStep");
00846         preparation.doc( "Preparation step of the RTI scheme." );
00847 
00848         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
00849         retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
00850         preparation.setReturnValue(retSim, false);
00851 
00852         preparation     << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
00853 
00854         preparation.addFunctionCall( evaluateObjective );
00855         preparation.addFunctionCall( evaluateConstraints );
00856 
00858         //
00859         // Setup feedback phase
00860         //
00862         ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
00863         ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
00864         returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
00865         feedback.setup("feedbackStep" );
00866         feedback.doc( "Feedback/estimation step of the RTI scheme." );
00867         feedback.setReturnValue( returnValueFeedbackPhase );
00868 
00869         feedback.addStatement(
00870 //                      cond[ 0 ].getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
00871                         cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose()
00872         );
00873         feedback.addLinebreak();
00874 
00875         // Calculate objective residuals
00876         feedback << (Dy -= y) << (DyN -= yN);
00877         feedback.addLinebreak();
00878 
00879         for (unsigned i = 0; i < N; ++i)
00880                 feedback.addFunctionCall(setStagef, objGradients[ i ], ExportIndex( i ));
00881         feedback.addStatement( objGradients[ N ] == QN2 * DyN );
00882         feedback.addLinebreak();
00883 
00884         //
00885         // Configure output variables
00886         //
00887         std::vector< ExportVariable > vecQPVars;
00888 
00889         vecQPVars.clear();
00890         vecQPVars.resize(N + 1);
00891         for (unsigned i = 0; i < N; ++i)
00892                 vecQPVars[ i ].setup(string("out") + toString(i + 1), NX + NU, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
00893         vecQPVars[ N ].setup(string("out") + toString(N + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
00894 
00895         //
00896         // In case warm starting is enabled, give an initial guess, based on the old solution
00897         //
00898         int hotstartQP;
00899         get(HOTSTART_QP, hotstartQP);
00900 
00901         if ( hotstartQP )
00902         {
00903                 std::vector< ExportVariable > zInit;
00904 
00905                 zInit.clear();
00906                 zInit.resize(N + 1);
00907                 for (unsigned i = 0; i < N; ++i)
00908                 {
00909                         string name = "z_init_";
00910                         name = name + (i < 10 ? "0" : "") + toString( i );
00911                         zInit[ i ].setup(name, NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00912                 }
00913                 string name = "z_init_";
00914                 name = name + (N < 10 ? "0" : "") + toString( N );
00915                 zInit[ N ].setup(name, NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00916 
00917                 // TODO This should be further investigated.
00918 
00919                 //
00920                 // 1) Just use the old solution
00921                 //
00922 //              for (unsigned blk = 0; blk < N + 1; blk++)
00923 //                      feedback.addStatement(zInit[ blk ] == vecQPVars[ blk ] );
00924 
00925                 //
00926                 // 2) Initialization by shifting
00927                 //
00928 
00929 //              for (unsigned blk = 0; blk < N - 1; blk++)
00930 //                      feedback.addStatement( zInit[ blk ] == vecQPVars[blk + 1] );
00931 //              for (unsigned el = 0; el < NX; el++)
00932 //                      feedback.addStatement( zInit[N - 1].getElement(el, 0) == vecQPVars[ N ].getElement(el, 0) );
00933         }
00934 
00935         //
00936         // Call a QP solver
00937         // NOTE: we need two prefixes:
00938         // 1. module prefix
00939         // 2. structure instance prefix
00940         //
00941         ExportFunction solveQP;
00942         solveQP.setup("solve");
00943 
00944         feedback
00945                 << returnValueFeedbackPhase.getFullName() << " = "
00946                 << qpModuleName << "_" << solveQP.getName() << "( "
00947                 << "&" << qpObjPrefix << "_" << "params" << ", "
00948                 << "&" << qpObjPrefix << "_" << "output" << ", "
00949                 << "&" << qpObjPrefix << "_" << "info" << " );\n";
00950         feedback.addLinebreak();
00951 
00952         //
00953         // Here we have to add the differences....
00954         //
00955 
00956         ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
00957         ExportIndex index( "index" );
00958         acc.setup("accumulate", stageOut, index);
00959 
00960         acc.addStatement( x.getRow( index ) += stageOut.getCols(0, NX) );
00961         acc.addLinebreak();
00962         acc.addStatement( u.getRow( index ) += stageOut.getCols(NX, NX + NU) );
00963         acc.addLinebreak();
00964 
00965         for (unsigned i = 0; i < N; ++i)
00966                 feedback.addFunctionCall(acc, vecQPVars[ i ], ExportIndex( i ));
00967         feedback.addLinebreak();
00968 
00969         feedback.addStatement( x.getRow( N ) += vecQPVars[ N ].getTranspose() );
00970         feedback.addLinebreak();
00971 
00973         //
00974         // TODO Setup evaluation of KKT
00975         //
00977         ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
00978 
00979         getKKT.setup( "getKKT" );
00980         getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
00981 //      kkt.setDoc( "The KKT tolerance value." );
00982         kkt.setDoc( "1e-15." );
00983         getKKT.setReturnValue( kkt );
00984 
00985         getKKT.addStatement( kkt == 1e-15 );
00986 
00987         return SUCCESSFUL_RETURN;
00988 }
00989 
00990 returnValue ExportGaussNewtonForces::setupQPInterface( )
00991 {
00992         //
00993         // Configure and export QP interface
00994         //
00995 
00996         qpInterface = std::tr1::shared_ptr< ExportForcesInterface >(new ExportForcesInterface(FORCES_TEMPLATE, "", commonHeaderName));
00997 
00998         ExportVariable tmp1("tmp", 1, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
00999         ExportVariable tmp2("tmp", 1, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
01000         ExportVariable tmp3("tmp", 1, 1, REAL, FORCES_INFO, false, qpObjPrefix);
01001 
01002         string params = qpModuleName + "_params";
01003 
01004         string output = qpModuleName + "_output";
01005 
01006         string info = qpModuleName + "_info";
01007 
01008         string header = qpModuleName + ".h";
01009 
01010         qpInterface->configure(
01011                         header,
01012 
01013                         params,
01014                         tmp1.getDataStructString(),
01015 
01016                         output,
01017                         tmp2.getDataStructString(),
01018 
01019                         info,
01020                         tmp3.getDataStructString()
01021         );
01022 
01023         //
01024         // Configure and export MATLAB QP generator
01025         //
01026 
01027         string folderName;
01028         get(CG_EXPORT_FOLDER_NAME, folderName);
01029         string outFile = folderName + "/acado_forces_generator.m";
01030 
01031         qpGenerator = std::tr1::shared_ptr< ExportForcesGenerator >(new ExportForcesGenerator(FORCES_GENERATOR, outFile, "", "real_t", "int", 16, "%"));
01032 
01033         int maxNumQPiterations;
01034         get( MAX_NUM_QP_ITERATIONS,maxNumQPiterations );
01035 
01036         int printLevel;
01037         get(PRINTLEVEL, printLevel);
01038 
01039         // if not specified, use default value
01040         if ( maxNumQPiterations <= 0 )
01041                 maxNumQPiterations = 3 * getNumQPvars();
01042 
01043         int useOMP;
01044         get(CG_USE_OPENMP, useOMP);
01045 
01046         int hotstartQP;
01047         get(HOTSTART_QP, hotstartQP);
01048 
01049         qpGenerator->configure(
01050                         NX,
01051                         NU,
01052                         N,
01053                         conLBIndices,
01054                         conUBIndices,
01055                         (Q1.isGiven() == true && R1.isGiven() == true) ? 1 : 0,
01056                         diagH,
01057                         diagHN,
01058                         initialStateFixed(),
01059                         qpModuleName,
01060                         (PrintLevel)printLevel == HIGH ? 2 : 0,
01061                         maxNumQPiterations,
01062                         useOMP,
01063                         true,
01064                         hotstartQP
01065         );
01066 
01067         qpGenerator->exportCode();
01068 
01069         //
01070         // Export Python generator
01071         //
01072 
01073         outFile = folderName + "/acado_forces_generator.py";
01074 
01075         qpGenerator = std::tr1::shared_ptr< ExportForcesGenerator >(new ExportForcesGenerator(FORCES_GENERATOR_PYTHON, outFile, "", "real_t", "int", 16, "#"));
01076 
01077         qpGenerator->configure(
01078                         NX,
01079                         NU,
01080                         N,
01081                         conLBIndices,
01082                         conUBIndices,
01083                         (Q1.isGiven() == true && R1.isGiven() == true) ? 1 : 0, // TODO Remove this one
01084                         diagH,
01085                         diagHN,
01086                         initialStateFixed(),
01087                         qpModuleName,
01088                         (PrintLevel)printLevel == HIGH ? 2 : 0,
01089                         maxNumQPiterations,
01090                         useOMP,
01091                         false,
01092                         hotstartQP
01093         );
01094 
01095         qpGenerator->exportCode();
01096 
01097         return SUCCESSFUL_RETURN;
01098 }
01099 
01100 CLOSE_NAMESPACE_ACADO


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