export_nlp_solver.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_nlp_solver.hpp>
00033 
00034 #include <acado/objective/objective.hpp>
00035 #include <acado/ocp/ocp.hpp>
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 using namespace std;
00040 
00041 ExportNLPSolver::ExportNLPSolver(       UserInteraction* _userInteraction,
00042                                                                         const std::string& _commonHeaderName
00043                                                                         ) : ExportAlgorithm(_userInteraction, _commonHeaderName),
00044                                                                                         cholObjS(_userInteraction, _commonHeaderName),
00045                                                                                         cholSAC(_userInteraction, _commonHeaderName),
00046                                                                                         acSolver(userInteraction, _commonHeaderName)
00047 
00048 {
00049         levenbergMarquardt = 0.0;
00050 
00051         dimPacH = 0;
00052         dimPocH = 0;
00053 }
00054 
00055 returnValue ExportNLPSolver::setIntegratorExport(       IntegratorExportPtr const _integrator
00056                                                                                                         )
00057 {
00058         integrator = _integrator;
00059         return SUCCESSFUL_RETURN;
00060 }
00061 
00062 returnValue ExportNLPSolver::setLevenbergMarquardt(     double _levenbergMarquardt
00063                                                                                                                 )
00064 {
00065         if ( _levenbergMarquardt < 0.0 )
00066         {
00067                 ACADOWARNINGTEXT(RET_INVALID_ARGUMENTS, "Levenberg-Marquardt regularization factor must be positive!");
00068                 levenbergMarquardt = 0.0;
00069         }
00070         else
00071         {
00072                 levenbergMarquardt = _levenbergMarquardt;
00073         }
00074 
00075         return SUCCESSFUL_RETURN;
00076 }
00077 
00078 bool ExportNLPSolver::performsSingleShooting( ) const
00079 {
00080         int discretizationType;
00081         get(DISCRETIZATION_TYPE, discretizationType);
00082 
00083         if ( discretizationType == SINGLE_SHOOTING )
00084                 return true;
00085 
00086         return false;
00087 }
00088 
00089 returnValue ExportNLPSolver::getDataDeclarations(       ExportStatementBlock& declarations,
00090                                                                                                         ExportStruct dataStruct
00091                                                                                                         ) const
00092 {
00093         declarations.addDeclaration(state, dataStruct);
00094         declarations.addDeclaration(x, dataStruct);
00095         declarations.addDeclaration(z, dataStruct);
00096         declarations.addDeclaration(u, dataStruct);
00097         declarations.addDeclaration(od, dataStruct);
00098         declarations.addDeclaration(d, dataStruct);
00099 
00100         declarations.addDeclaration(y, dataStruct);
00101         declarations.addDeclaration(yN, dataStruct);
00102         declarations.addDeclaration(Dy, dataStruct);
00103         declarations.addDeclaration(DyN, dataStruct);
00104 
00105         declarations.addDeclaration(evGx, dataStruct);
00106         declarations.addDeclaration(evGu, dataStruct);
00107 
00108         declarations.addDeclaration(objS, dataStruct);
00109         declarations.addDeclaration(objSEndTerm, dataStruct);
00110         declarations.addDeclaration(objSlx, dataStruct);
00111         declarations.addDeclaration(objSlu, dataStruct);
00112 
00113         declarations.addDeclaration(objAuxVar, dataStruct);
00114         declarations.addDeclaration(objValueIn, dataStruct);
00115         declarations.addDeclaration(objValueOut, dataStruct);
00116 
00117         declarations.addDeclaration(Q1, dataStruct);
00118         declarations.addDeclaration(Q2, dataStruct);
00119 
00120         declarations.addDeclaration(R1, dataStruct);
00121         declarations.addDeclaration(R2, dataStruct);
00122 
00123         declarations.addDeclaration(S1, dataStruct);
00124 
00125         declarations.addDeclaration(QN1, dataStruct);
00126         declarations.addDeclaration(QN2, dataStruct);
00127 
00128         declarations.addDeclaration(SAC, dataStruct);
00129         declarations.addDeclaration(xAC, dataStruct);
00130         declarations.addDeclaration(DxAC, dataStruct);
00131 
00132         declarations.addDeclaration(conAuxVar, dataStruct);
00133         declarations.addDeclaration(conValueIn, dataStruct);
00134         declarations.addDeclaration(conValueOut, dataStruct);
00135 
00136         declarations.addDeclaration(pacEvH, dataStruct);
00137         declarations.addDeclaration(pacEvHx, dataStruct);
00138         declarations.addDeclaration(pacEvHu, dataStruct);
00139         declarations.addDeclaration(pacEvHxd, dataStruct);
00140 
00141         declarations.addDeclaration(pocEvH, dataStruct);
00142         declarations.addDeclaration(pocEvHx, dataStruct);
00143         declarations.addDeclaration(pocEvHu, dataStruct);
00144         declarations.addDeclaration(pocEvHxd, dataStruct);
00145 
00146         // Arrival cost stuff
00147         declarations.addDeclaration(acA, dataStruct);
00148         declarations.addDeclaration(acb, dataStruct);
00149         declarations.addDeclaration(acP, dataStruct);
00150         declarations.addDeclaration(acTmp, dataStruct);
00151         declarations.addDeclaration(acWL, dataStruct);
00152         declarations.addDeclaration(acVL, dataStruct);
00153         declarations.addDeclaration(acHx, dataStruct);
00154         declarations.addDeclaration(acHu, dataStruct);
00155         declarations.addDeclaration(acXx, dataStruct);
00156         declarations.addDeclaration(acXu, dataStruct);
00157         declarations.addDeclaration(acXTilde, dataStruct);
00158         declarations.addDeclaration(acHTilde, dataStruct);
00159 
00160         return SUCCESSFUL_RETURN;
00161 }
00162 
00163 returnValue ExportNLPSolver::setupInitialization()
00164 {
00166         //
00167         // Setup the main initialization function.
00168         //
00170 
00171         ExportVariable retInit("ret", 1, 1, INT, ACADO_LOCAL, true);
00172         retInit.setDoc("=0: OK, otherwise an error code of a QP solver.");
00173         initialize.setup( "initializeSolver" );
00174         initialize.doc( "Solver initialization. Must be called once before any other function call." );
00175         initialize.setReturnValue(retInit);
00176 
00177         initialize.addComment( "This is a function which must be called once before any other function call!" );
00178         initialize.addLinebreak( 2 );
00179 
00180         initialize << (retInit == 0);
00181         initialize.addLinebreak();
00182         initialize      << "memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));" << "\n";
00183 //      initialize      << "memset(&acadoVariables, 0, sizeof( acadoVariables ));" << "\n";
00184 
00185         return SUCCESSFUL_RETURN;
00186 }
00187 
00188 returnValue ExportNLPSolver::setupSimulation( void )
00189 {
00190         // \todo Implement free parameters and support for DAEs
00191 
00192         //
00193         // By default, here will be defined model simulation suitable for sparse QP solver.
00194         // Condensing based QP solvers should redefine/extend model simulation
00195         //
00196 
00197         int hessianApproximation;
00198         get( HESSIAN_APPROXIMATION, hessianApproximation );
00199 
00200         modelSimulation.setup( "modelSimulation" );
00201         ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
00202         modelSimulation.setReturnValue(retSim, false);
00203         modelSimulation.addStatement(retSim == 0);
00204         ExportIndex run;
00205         modelSimulation.acquire( run );
00206         ExportForLoop loop(run, 0, getN());
00207 
00208         int useOMP;
00209         get(CG_USE_OPENMP, useOMP);
00210 
00211         x.setup("x", (getN() + 1), getNX(), REAL, ACADO_VARIABLES);
00212         x.setDoc( string("Matrix containing ") + toString(getN() + 1) + " differential variable vectors." );
00213         z.setup("z", getN(), getNXA(), REAL, ACADO_VARIABLES);
00214         z.setDoc( string("Matrix containing ") + toString( N ) + " algebraic variable vectors." );
00215         u.setup("u", getN(), getNU(), REAL, ACADO_VARIABLES);
00216         u.setDoc( string("Matrix containing ") + toString( N ) + " control variable vectors." );
00217         od.setup("od", getN() + 1, getNOD(), REAL, ACADO_VARIABLES);
00218         od.setDoc( string("Matrix containing ") + toString(getN() + 1) + " online data vectors." );
00219 
00220         if (performsSingleShooting() == false)
00221         {
00222                 d.setup("d", getN() * getNX(), 1, REAL, ACADO_WORKSPACE);
00223         }
00224 
00225         uint symH = (NX+NU)*(NX+NU+1)/2;
00226 
00227         evGx.setup("evGx", N * NX, NX, REAL, ACADO_WORKSPACE);
00228         evGu.setup("evGu", N * NX, NU, REAL, ACADO_WORKSPACE);
00229 
00230         bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
00231 
00232         if( secondOrder ) mu.setup("mu", N, NX, REAL, ACADO_VARIABLES);
00233 
00234         ExportStruct dataStructWspace;
00235         dataStructWspace = (useOMP && performsSingleShooting() == false) ? ACADO_LOCAL : ACADO_WORKSPACE;
00236         state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNU() + getNOD(), REAL, dataStructWspace);
00237         if( secondOrder ) {
00238                 state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNX() + symH + getNU() + getNOD(), REAL, dataStructWspace);
00239         }
00240 
00241         unsigned indexZ   = NX + NXA;
00242         if( secondOrder ) indexZ = indexZ + NX;         // because of the first order adjoint direction
00243         unsigned indexGxx = indexZ + NX * NX;
00244         unsigned indexGzx = indexGxx + NXA * NX;
00245         unsigned indexGxu = indexGzx + NX * NU;
00246         unsigned indexGzu = indexGxu + NXA * NU;
00247         unsigned indexH = indexGzu;
00248         if( secondOrder ) indexH = indexGzu + symH;     // because of the second order derivatives
00249         unsigned indexU   = indexH + NU;
00250         unsigned indexOD   = indexU + NOD;
00251 
00253         //
00254         // Code for model simulation
00255         //
00257         if (performsSingleShooting() == true)
00258         {
00259                 modelSimulation.addStatement( state.getCols(0, NX)                              == x.getRow( 0 ) );
00260                 modelSimulation.addStatement( state.getCols(NX, NX + NXA)               == z.getRow( 0 ) );
00261                 modelSimulation.addStatement( state.getCols(indexH, indexU)     == u.getRow( 0 ) );
00262                 modelSimulation.addStatement( state.getCols(indexU, indexOD)    == od.getRow( 0 ) );
00263                 modelSimulation.addLinebreak( );
00264         }
00265 
00266         if ( useOMP )
00267         {
00268 
00269                 modelSimulation
00270                         << "#pragma omp parallel for private(" << run.getName() << ", " << state.getFullName()
00271                                 << ") shared(" << evGx.getDataStructString() << ", "
00272                                 << x.getDataStructString() << ")\n";
00273         }
00274 
00275         if (performsSingleShooting() == false)
00276         {
00277                 loop.addStatement( state.getCols(0, NX)                 == x.getRow( run ) );
00278                 loop.addStatement( state.getCols(NX, NX + NXA)  == z.getRow( run ) );
00279         }
00280         loop.addLinebreak( );
00281 
00282         // Fill in the input vector
00283         if( secondOrder ) {
00284                 loop.addStatement( state.getCols(NX+NXA, 2*NX+NXA)      == mu.getRow( run ) );
00285         }
00286         loop.addStatement( state.getCols(indexH, indexU)        == u.getRow( run ) );
00287         loop.addStatement( state.getCols(indexU, indexOD)       == od.getRow( run ) );
00288         loop.addLinebreak( );
00289 
00290         // Integrate the model
00291         // TODO make that function calls can accept constant defined scalars
00292         if ( integrator->equidistantControlGrid() )
00293         {
00294                 if (performsSingleShooting() == false)
00295                         loop    << retSim.getFullName() << " = "
00296                                          << "integrate" << "(" << state.getFullName() << ", 1);\n";
00297                 else
00298                         loop    << retSim.getFullName() << " = " << "integrate"
00299                                         << "(" << state.getFullName() << ", "
00300                                         << run.getFullName() << " == 0"
00301                                         << ");\n";
00302         }
00303         else
00304         {
00305                 if (performsSingleShooting() == false)
00306                         loop    << retSim.getFullName() << " = "
00307                                         << "integrate"
00308                                         << "(" << state.getFullName() << ", 1, " << run.getFullName() << ");\n";
00309                 else
00310                         loop    << retSim.getFullName() << " = "
00311                                         << "integrate"
00312                                         << "(" << state.getFullName() << ", "
00313                                         << run.getFullName() << " == 0"
00314                                         << ", " << run.getFullName() << ");\n";
00315         }
00316         loop.addLinebreak( );
00317         if (useOMP == 0)
00318         {
00319                 // TODO In case we use OpenMP more sophisticated solution has to be found.
00320                 loop << "if (" << retSim.getFullName() << " != 0) return " << retSim.getFullName() << ";";
00321                 loop.addLinebreak( );
00322         }
00323 
00324         if ( performsSingleShooting() == true )
00325         {
00326                 // Single shooting case: prepare for the next iteration
00327                 loop.addStatement( x.getRow(run + 1) == state.getCols(0, NX) );
00328                 loop.addLinebreak( );
00329         }
00330         else
00331         {
00332                 // Multiple shootin', compute residuum
00333                 loop.addStatement( d.getTranspose().getCols(run * NX, (run  + 1) * NX) == state.getCols( 0,getNX() ) - x.getRow( run+1 ) );
00334                 loop.addLinebreak( );
00335         }
00336 
00337         loop.addStatement( z.getRow( run ) == state.getCols(NX, NX + NXA) );
00338 
00339         // Stack sensitivities
00340         // \todo Upgrade this code later to stack Z sens
00341         loop.addStatement(
00342                         evGx.makeRowVector().getCols(run * NX * NX, (run + 1) * NX * NX) == state.getCols(indexZ, indexGxx)
00343         );
00344         loop.addLinebreak();
00345 
00346         loop.addStatement(
00347                         evGu.makeRowVector().getCols(run * NX * NU, (run + 1) * NX * NU) == state.getCols(indexGzx, indexGxu)
00348         );
00349 
00350         // TODO: write this in exported loops (RIEN)
00351         if( secondOrder ) {
00352                 for( uint i = 0; i < NX+NU; i++ ) {
00353                         for( uint j = 0; j <= i; j++ ) {
00354                                 loop.addStatement( objS.getElement(run*(NX+NU)+i,j) == -1.0*state.getCol(indexGzu + i*(i+1)/2+j) );
00355                                 if( i != j) {
00356                                         loop.addStatement( objS.getElement(run*(NX+NU)+j,i) == objS.getElement(run*(NX+NU)+i,j) );
00357                                 }
00358                         }
00359                 }
00360         }
00361 
00362         // XXX This should be revisited at some point
00363         //      modelSimulation.release( run );
00364 
00365         modelSimulation.addStatement( loop );
00366 
00367         return SUCCESSFUL_RETURN;
00368 }
00369 
00370 
00371 returnValue ExportNLPSolver::setObjective(const Objective& _objective)
00372 {
00373         if( _objective.getNumMayerTerms() == 0 && _objective.getNumLagrangeTerms() == 0 ) {
00374                 return setLSQObjective( _objective );
00375         }
00376         else {
00377                 return setGeneralObjective( _objective );
00378         }
00379 }
00380 
00381 
00382 returnValue ExportNLPSolver::setGeneralObjective(const Objective& _objective)
00383 {
00385         //   ONLY ACADO AD SUPPORTED FOR NOW
00387 
00388         Function objF, objFEndTerm;
00389         DifferentialState dummy0;
00390         Control dummy1;
00391         dummy0.clearStaticCounters();
00392         dummy1.clearStaticCounters();
00393 
00394         DifferentialState vX("", NX, 1);
00395         Control vU("", NU, 1);
00396 
00397         diagonalH = false;
00398         diagonalHN = false;
00399 
00400         int hessianApproximation;
00401         get( HESSIAN_APPROXIMATION, hessianApproximation );
00402         bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
00403         if( secondOrder ) {
00404                 objS.setup("EH", N * (NX + NU), NX + NU, REAL, ACADO_WORKSPACE);  // EXACT HESSIAN
00405         }
00406 
00407         int qpSolution;
00408         get( SPARSE_QP_SOLUTION, qpSolution );
00409         if( (SparseQPsolutionMethods)qpSolution != SPARSE_SOLVER ) {
00410                 S1.setup("S1", NX * N, NU, REAL, ACADO_WORKSPACE);
00411                 Q1.setup("Q1", NX * N, NX, REAL, ACADO_WORKSPACE);
00412                 R1.setup("R1", NU * N, NU, REAL, ACADO_WORKSPACE);
00413                 QN1.setup("QN1", NX, NX, REAL, ACADO_WORKSPACE);
00414         }
00415 
00416         objValueIn.setup("objValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
00417         // -----------------
00418         //   Lagrange Term:
00419         setNY( 0 );
00420         if( _objective.getNumLagrangeTerms() ) {
00421                 _objective.getLagrangeTerm(0, objF);
00422 
00423                 objS.setup("EH", N * (NX+NU), NX+NU, REAL, ACADO_WORKSPACE);  // EXACT HESSIAN
00424 
00425                 Expression expF;
00426                 objF.getExpression( expF );
00427 
00428                 // FIRST ORDER DERIVATIVES
00429                 Expression expFx, expFu, expDF, expDDF, S, lambda, arg, dl;
00430                 S = eye<double>(NX+NU);
00431                 lambda = 1;
00432                 arg << vX;
00433                 arg << vU;
00434 
00435                 expDDF = symmetricDerivative( expF, arg, S, lambda, &expDF, &dl );
00436 
00437                 expFx = expDF.getCols(0, NX).transpose();
00438                 expFu = expDF.getCols(NX, NX+NU).transpose();
00439 
00440                 Function Fx, Fu;
00441                 Fx << expFx;
00442                 Fu << expFu;
00443 
00444 //              if (Fx.isConstant() == true)
00445 //              {
00446 //                      EvaluationPoint epFx( Fx );
00447 //
00448 //                      DVector vFx = Fx.evaluate( epFx );
00449 //
00450 //                      objEvFx.setup("evFx", Eigen::Map<DMatrix>(vFx.data(), 1, NX), REAL, ACADO_WORKSPACE);
00451 //              }
00452 //              else
00453 //              {
00454                         objF << expFx;
00455 
00456                         objEvFx.setup("evFx", 1, NX, REAL, ACADO_WORKSPACE);
00457 //              }
00458 
00459 //              if (Fu.isConstant() == true)
00460 //              {
00461 //                      EvaluationPoint epFu( Fu );
00462 //
00463 //                      DVector vFu = Fu.evaluate( epFu );
00464 //
00465 //                      objEvFu.setup("evFu", Eigen::Map<DMatrix>(vFu.data(), 1, NU), REAL, ACADO_WORKSPACE);
00466 //              }
00467 //              else
00468 //              {
00469                         objF << expFu;
00470 
00471                         objEvFu.setup("evFu", 1, NU, REAL, ACADO_WORKSPACE);
00472 //              }
00473 
00474                 // SECOND ORDER DERIVATIVES
00475                 Expression expFxx;
00476                 Expression expFxu;
00477                 Expression expFuu;
00478 
00479                 expFxx = expDDF.getSubMatrix(0,NX,0,NX);
00480                 expFxu = expDDF.getSubMatrix(0,NX,NX,NX+NU);
00481                 expFuu = expDDF.getSubMatrix(NX,NX+NU,NX,NX+NU);
00482 
00483                 Function Fxx, Fxu, Fuu;
00484                 Fxx << expFxx;
00485                 Fxu << expFxu;
00486                 Fuu << expFuu;
00487 
00488 //              if (Fxx.isConstant() == true)
00489 //              {
00490 //                      EvaluationPoint epFxx( Fxx );
00491 //
00492 //                      DVector vFxx = Fxx.evaluate( epFxx );
00493 //
00494 //                      objEvFxx.setup("evFxx", Eigen::Map<DMatrix>(vFxx.data(), NX, NX), REAL, ACADO_WORKSPACE);
00495 //                      Q1 = vFxx.data();
00496 //              }
00497 //              else
00498 //              {
00499                         objF << expFxx;
00500 
00501                         objEvFxx.setup("evFxx", NX, NX, REAL, ACADO_WORKSPACE);
00502 //              }
00503 
00504 //              if (Fxu.isConstant() == true)
00505 //              {
00506 //                      EvaluationPoint epFxu( Fxu );
00507 //
00508 //                      DVector vFxu = Fxu.evaluate( epFxu );
00509 //
00510 //                      objEvFxu.setup("evFxu", Eigen::Map<DMatrix>(vFxu.data(), NX, NU), REAL, ACADO_WORKSPACE);
00511 //                      S1 = vFxu.data();
00512 //              }
00513 //              else
00514 //              {
00515                         objF << expFxu;
00516 
00517                         objEvFxu.setup("evFxu", NX, NU, REAL, ACADO_WORKSPACE);
00518 //              }
00519 
00520 //              if (Fuu.isConstant() == true)
00521 //              {
00522 //                      EvaluationPoint epFuu( Fuu );
00523 //
00524 //                      DVector vFuu = Fuu.evaluate( epFuu );
00525 //
00526 //                      objEvFuu.setup("evFuu", Eigen::Map<DMatrix>(vFuu.data(), NU, NU), REAL, ACADO_WORKSPACE);
00527 //                      R1 = vFuu.data();
00528 //              }
00529 //              else
00530 //              {
00531                         objF << expFuu;
00532 
00533                         objEvFuu.setup("evFuu", NU, NU, REAL, ACADO_WORKSPACE);
00534 //              }
00535 
00536                 // Set the separate aux variable for the evaluation of the objective.
00537                 objAuxVar.setup("objAuxVar", objF.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
00538                 evaluateStageCost.init(objF, "evaluateLagrange", NX, 0, NU);
00539                 evaluateStageCost.setGlobalExportVariable( objAuxVar );
00540                 evaluateStageCost.setPrivate( true );
00541 
00542                 objValueOut.setup("objValueOut", 1, objF.getDim(), REAL, ACADO_WORKSPACE);
00543         }
00544 
00545         // -----------------
00546         //   Mayer Term:
00547         setNYN( 0 );
00548         if( _objective.getNumMayerTerms() ) {
00549                 _objective.getMayerTerm(0, objFEndTerm);
00550 
00551                 if (objFEndTerm.getNU() > 0)
00552                         return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost function must not depend on controls.");
00553 
00554                 objSEndTerm.setup("EH_N", NX, NX, REAL, ACADO_WORKSPACE);  // EXACT HESSIAN
00555 
00556                 Expression expFEndTerm;
00557                 objFEndTerm.getExpression( expFEndTerm );
00558 
00559                 // FIRST ORDER DERIVATIVES
00560 
00561                 Expression expFEndTermX, expFEndTermXX, S, lambda, dl;
00562                 S = eye<double>(NX);
00563                 lambda = 1;
00564 
00565                 expFEndTermXX = symmetricDerivative( expFEndTerm, vX, S, lambda, &expFEndTermX, &dl );
00566 
00567                 Function FEndTermX;
00568                 FEndTermX << expFEndTermX.transpose();
00569 
00570 //              if (FEndTermX.isConstant() == true)
00571 //              {
00572 //                      EvaluationPoint epFEndTermX( FEndTermX );
00573 //
00574 //                      DVector vFx = FEndTermX.evaluate( epFEndTermX );
00575 //
00576 //                      objEvFxEnd.setup("evFxEnd", Eigen::Map<DMatrix>(vFx.data(), 1, NX), REAL, ACADO_WORKSPACE);
00577 //              }
00578 //              else
00579 //              {
00580                         objFEndTerm << expFEndTermX;
00581 
00582                         objEvFxEnd.setup("evFxEnd", 1, NX, REAL, ACADO_WORKSPACE);
00583 //              }
00584 
00585                 // SECOND ORDER DERIVATIVES
00586 
00587                 Function FEndTermXX;
00588                 FEndTermXX << expFEndTermXX;
00589 
00590 //              if (FEndTermXX.isConstant() == true)
00591 //              {
00592 //                      EvaluationPoint epFEndTermXX( FEndTermXX );
00593 //
00594 //                      DVector vFxx = FEndTermXX.evaluate( epFEndTermXX );
00595 //
00596 //                      objEvFxxEnd.setup("evFxxEnd", Eigen::Map<DMatrix>(vFxx.data(), NX, NX), REAL, ACADO_WORKSPACE);
00597 //                      QN1 = vFxx.data();
00598 //              }
00599 //              else
00600 //              {
00601                         objFEndTerm << expFEndTermXX;
00602 
00603                         objEvFxxEnd.setup("evFxxEnd", NX, NX, REAL, ACADO_WORKSPACE);
00604 //              }
00605 
00606                 unsigned objFEndTermSize = objFEndTerm.getGlobalExportVariableSize();
00607                 if ( objFEndTermSize > objAuxVar.getDim() )
00608                 {
00609                         objAuxVar.setup("objAuxVar", objFEndTermSize, 1, REAL, ACADO_WORKSPACE);
00610                 }
00611 
00612                 evaluateTerminalCost.init(objFEndTerm, "evaluateMayer", NX, 0, 0);
00613                 evaluateTerminalCost.setGlobalExportVariable( objAuxVar );
00614                 evaluateTerminalCost.setPrivate( true );
00615 
00616                 if (objFEndTerm.getDim() > objF.getDim())
00617                 {
00618                         objValueOut.setup("objValueOut", 1, objFEndTerm.getDim(), REAL, ACADO_WORKSPACE);
00619                 }
00620 
00621 
00622 //              setupObjectiveLinearTerms( _objective );
00623                 objSlx = zeros<double>(NX, 1);
00624                 objSlu = zeros<double>(NU, 1);
00625                 objSlx.setDoc("Linear term weighting vector for states.");
00626                 objSlu.setDoc("Linear term weighting vector for controls.");
00627 //
00628 //              setupResidualVariables();
00629         }
00630 
00631         return SUCCESSFUL_RETURN;
00632 }
00633 
00634 
00635 returnValue ExportNLPSolver::setLSQObjective(const Objective& _objective)
00636 {
00637         int variableObjS;
00638         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
00639         int useArrivalCost;
00640         get(CG_USE_ARRIVAL_COST, useArrivalCost);
00641 
00643         //
00644         // Check first if we are dealing with external functions
00645         //
00647 
00648         LsqExternElements lsqExternElements;
00649         _objective.getLSQTerms( lsqExternElements );
00650 
00651         LsqExternElements lsqExternEndTermElements;
00652         _objective.getLSQEndTerms( lsqExternEndTermElements );
00653 
00654         if (lsqExternElements.size() > 0 || lsqExternEndTermElements.size() > 0)
00655         {
00656                 if (lsqExternElements.size() != 1 || lsqExternEndTermElements.size() != 1)
00657                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00658                 if (lsqExternElements[ 0 ].W.isSquare() == false || lsqExternElements[ 0 ].W.isSquare() == false)
00659                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00660 
00661                 setNY( lsqExternElements[ 0 ].W.getNumRows() );
00662                 setNYN( lsqExternEndTermElements[ 0 ].W.getNumRows() );
00663 
00664                 if (variableObjS == YES)
00665                 {
00666                         objS.setup("W", N * NY, NY, REAL, ACADO_VARIABLES);
00667                 }
00668                 else if (lsqExternElements[ 0 ].givenW == false)
00669                 {
00670                         objS.setup("W", lsqExternElements[ 0 ].W, REAL, ACADO_VARIABLES, false, "", false);
00671                 }
00672                 else
00673                 {
00674                         objS.setup("W", lsqExternElements[ 0 ].W, REAL, ACADO_VARIABLES);
00675                 }
00676 
00677                 objSEndTerm.setup("WN", lsqExternEndTermElements[ 0 ].W,
00678                                 REAL, ACADO_VARIABLES, false, "", lsqExternEndTermElements[ 0 ].givenW);
00679 
00680                 int forceDiagHessian;
00681                 get(CG_FORCE_DIAGONAL_HESSIAN, forceDiagHessian);
00682 
00683                 diagonalH = diagonalHN = forceDiagHessian ? true : false;
00684 
00685                 objEvFx.setup("evFx", NY, NX, REAL, ACADO_WORKSPACE);
00686                 objEvFu.setup("evFu", NY, NU, REAL, ACADO_WORKSPACE);
00687                 objEvFxEnd.setup("evFx", NYN, NX, REAL, ACADO_WORKSPACE);
00688 
00689                 Q1.setup("Q1", NX * N, NX, REAL, ACADO_WORKSPACE);
00690                 Q2.setup("Q2", NX * N, NY, REAL, ACADO_WORKSPACE);
00691 
00692                 R1.setup("R1", NU * N, NU, REAL, ACADO_WORKSPACE);
00693                 R2.setup("R2", NU * N, NY, REAL, ACADO_WORKSPACE);
00694 
00695 //              S1.setup("S1", NX * N, NU, REAL, ACADO_WORKSPACE);
00696                 S1 = zeros<double>(NX, NU);
00697 
00698                 QN1.setup("QN1", NX, NX, REAL, ACADO_WORKSPACE);
00699                 QN2.setup("QN2", NX, NYN, REAL, ACADO_WORKSPACE);
00700 
00701                 objValueIn.setup("objValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
00702                 objValueOut.setup("objValueOut", 1,
00703                                 NY < NYN ? NYN * (1 + NX + NU): NY * (1 + NX + NU), REAL, ACADO_WORKSPACE);
00704 
00705                 evaluateStageCost = ExportAcadoFunction(lsqExternElements[ 0 ].h);
00706                 evaluateTerminalCost = ExportAcadoFunction(lsqExternEndTermElements[ 0 ].h);
00707 
00708                 setupObjectiveLinearTerms( _objective );
00709 
00710                 setupResidualVariables();
00711 
00712                 return SUCCESSFUL_RETURN;
00713         }
00714 
00716         //
00717         // ... or we use ACADO AD
00718         //
00720 
00721         Function objF, objFEndTerm;
00722 
00723         LsqElements lsqElements;
00724         LsqElements lsqEndTermElements;
00725 
00726         _objective.getLSQTerms( lsqElements );
00727         _objective.getLSQEndTerms( lsqEndTermElements );
00728 
00729         if(     lsqElements.size() == 0 )
00730                 return ACADOERRORTEXT(RET_INITIALIZE_FIRST, "Objective function is not initialized.");
00731         if (lsqElements.size() > 1 || lsqEndTermElements.size() > 1)
00732                 return ACADOERRORTEXT(RET_INITIALIZE_FIRST,
00733                                 "Current implementation of code generation module\n"
00734                                 "supports only one LSQ term definition per one OCP." );
00735 
00736         if (lsqElements[ 0 ].W.isSquare() == false)
00737                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Weighting matrices must be square.");
00738         if (lsqElements[ 0 ].W.getNumRows() != (unsigned)lsqElements[ 0 ].h.getDim())
00739                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Wrong dimensions of the weighting matrix.");
00740 
00741         if ( lsqEndTermElements.size() == 0 )
00742                 return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost must be defined");
00743 
00744         if (lsqEndTermElements[ 0 ].W.isSquare() == false)
00745                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Weighting matrices must be square.");
00746         if (lsqEndTermElements[ 0 ].W.getNumRows() != (unsigned)lsqEndTermElements[ 0 ].h.getDim())
00747                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Wrong dimensions of the weighting matrix.");
00748 
00749         objF = lsqElements[ 0 ].h;
00750         setNY( objF.getDim() );
00751 
00752         DifferentialState dummy0;
00753         Control dummy1;
00754         dummy0.clearStaticCounters();
00755         dummy1.clearStaticCounters();
00756 
00757         DifferentialState vX("", NX, 1);
00758         Control vU("", NU, 1);
00759 
00761         //
00762         // Setup the Lagrange LSQ terms
00763         //
00765 
00766         // Setup the S matrix
00767         if (lsqElements[ 0 ].givenW == false)
00768         {
00769                 if ( variableObjS == YES )
00770                 {
00771                         // TODO Sparsity of this guy should be done in an efficient way one day,
00772                         //      most probably after isolating objective handling in a separate
00773                         //      class.
00774                         objS.setup("W", N * NY, NY, REAL, ACADO_VARIABLES);
00775                 }
00776                 else
00777                 {
00778                         objS.setup("W", lsqElements[ 0 ].W, REAL, ACADO_VARIABLES, false, "", false);
00779                 }
00780         }
00781         else
00782         {
00783                 if (lsqElements[ 0 ].W.isPositiveSemiDefinite() == false)
00784                         return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
00785 
00786                 objS.setup("W", lsqElements[ 0 ].W, REAL, ACADO_VARIABLES);
00787         }
00788 
00789         Expression expF;
00790         objF.getExpression( expF );
00791 
00792         Expression expFx;
00793         Expression expFu;
00794 
00795         expFx = forwardDerivative(expF, vX);
00796         expFu = forwardDerivative(expF, vU);
00797 
00798         Function Fx, Fu;
00799         Fx << expFx;
00800         Fu << expFu;
00801 
00802         if (Fx.isConstant() == true)
00803         {
00804                 EvaluationPoint epFx( Fx );
00805 
00806                 DVector vFx = Fx.evaluate( epFx );
00807 
00808                 objEvFx.setup("evFx", Eigen::Map<DMatrix>(vFx.data(), NY, NX), REAL, ACADO_WORKSPACE);
00809         }
00810         else
00811         {
00812                 objF << expFx;
00813 
00814                 objEvFx.setup("evFx", NY, NX, REAL, ACADO_WORKSPACE);
00815         }
00816 
00817 //      objF << expFx;
00818 //      evFx.setup("evFx", NY, NX, REAL, ACADO_WORKSPACE);
00819 
00820         if (Fu.isConstant() == true)
00821         {
00822                 EvaluationPoint epFu( Fu );
00823 
00824                 DVector vFu = Fu.evaluate( epFu );
00825 
00826                 objEvFu.setup("evFu", Eigen::Map<DMatrix>(vFu.data(), NY, NU), REAL, ACADO_WORKSPACE);
00827         }
00828         else
00829         {
00830                 objF << expFu;
00831 
00832                 objEvFu.setup("evFu", NY, NU, REAL, ACADO_WORKSPACE);
00833         }
00834 
00835 //      objF << expFu;
00836 //      evFu.setup("evFu", NY, NU, REAL, ACADO_WORKSPACE);
00837 
00838         //
00839         // Initialize the export of the LSQ function which evaluates the
00840         // objective and (possibly) its derivatives.
00841         //
00842 
00843         // Set the separate aux variable for the evaluation of the objective.
00844 
00845         objAuxVar.setup("objAuxVar", objF.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
00846         evaluateStageCost.init(objF, "evaluateLSQ", NX, 0, NU);
00847         evaluateStageCost.setGlobalExportVariable( objAuxVar );
00848         evaluateStageCost.setPrivate( true );
00849 
00850         objValueIn.setup("objValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
00851         objValueOut.setup("objValueOut", 1, objF.getDim(), REAL, ACADO_WORKSPACE);
00852 
00853         //
00854         // Optional pre-computing of Q1, Q2, R1, R2 matrices
00855         //
00856 
00857         if (objS.isGiven() == true && objEvFx.isGiven() == true)
00858         {
00859                 if (useArrivalCost)
00860                         return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00861 
00862                 // Precompute Q1 and Q2;
00863 
00864                 DMatrix m1(NX,NX), m2(NX, NY);
00865 
00866                 m2 = objEvFx.getGivenMatrix().transpose() * objS.getGivenMatrix();
00867                 m1 = m2 * objEvFx.getGivenMatrix();
00868 
00869                 Q1 = m1;
00870                 if ( m1 == m2 )
00871                 {
00872                         Q2 = Q1;
00873                 }
00874                 else
00875                 {
00876                         Q2 = m2;
00877                 }
00878         }
00879         else if (Fx.isOneOrZero() == NE_ZERO)
00880         {
00881                 if (useArrivalCost)
00882                         return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00883 
00884                 Q1 = zeros<double>(NX, NX);
00885                 Q2 = zeros<double>(NX, NY);
00886         }
00887         else
00888         {
00889                 Q1.setup("Q1", NX * N, NX, REAL, ACADO_WORKSPACE);
00890                 Q2.setup("Q2", NX * N, NY, REAL, ACADO_WORKSPACE);
00891         }
00892 
00893         if (objS.isGiven() == true && objEvFu.isGiven() == true)
00894         {
00895                 // Precompute R1 and R2
00896 
00897                 DMatrix m2 = objEvFu.getGivenMatrix().transpose() * objS.getGivenMatrix();
00898                 DMatrix m1 = m2 * objEvFu.getGivenMatrix();
00899 
00900                 R1 = m1;
00901                 if (m1 == m2)
00902                 {
00903                         R2 = R1;
00904                 }
00905                 else
00906                 {
00907                         R2 = m2;
00908                 }
00909         }
00910         else if (Fu.isOneOrZero() == NE_ZERO)
00911         {
00912                 R1 = zeros<double>(NU, NU);
00913                 R2 = zeros<double>(NU, NY);
00914         }
00915         else
00916         {
00917                 R1.setup("R1", NU * N, NU, REAL, ACADO_WORKSPACE);
00918                 R2.setup("R2", NU * N, NY, REAL, ACADO_WORKSPACE);
00919         }
00920 
00921         // Check for sparsity of the stage Hessian
00922         // Dependency pattern of Fx
00923         DMatrix depFx = objEvFx.isGiven() == true ? objEvFx.getGivenMatrix() : expFx.getSparsityPattern();
00924         // Dependency pattern of Fu
00925         DMatrix depFu = objEvFu.isGiven() == true ? objEvFu.getGivenMatrix() : expFu.getSparsityPattern();
00926 
00927         DMatrix depQ = depFx.transpose() * lsqElements[ 0 ].W * depFx;
00928         DMatrix depR = depFu.transpose() * lsqElements[ 0 ].W * depFu;
00929         DMatrix depS = depFx.transpose() * lsqElements[ 0 ].W * depFu;
00930 
00931         if (depQ.isDiagonal() && depR.isDiagonal() && depS.isZero())
00932                 diagonalH = true;
00933         else
00934                 diagonalH = false;
00935 
00936         if (depS.isZero() == true)
00937         {
00938                 S1 = zeros<double>(NX, NU);
00939         }
00940         else if (objS.isGiven() == true && objEvFu.isGiven() == true && objEvFx.isGiven() == true)
00941         {
00942                 S1 = objEvFx.getGivenMatrix().transpose() * objS.getGivenMatrix() * objEvFu.getGivenMatrix();
00943         }
00944         else
00945         {
00946                 S1.setup("S1", NX * N, NU, REAL, ACADO_WORKSPACE);
00947         }
00948 
00950         //
00951         // Setup the quadratic Mayer term stuff
00952         //
00954 
00955         objFEndTerm = lsqEndTermElements[ 0 ].h;
00956 
00957         if (objFEndTerm.getNU() > 0)
00958                 return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost function must not depend on controls.");
00959 
00960         setNYN( objFEndTerm.getDim() );
00961 
00962         // Setup the SN matrix
00963         if (lsqEndTermElements[ 0 ].givenW == false)
00964         {
00965                 objSEndTerm.setup("WN", lsqEndTermElements[ 0 ].W, REAL, ACADO_VARIABLES, false, "", false);
00966         }
00967         else
00968         {
00969                 if (lsqEndTermElements[ 0 ].W.isPositiveDefinite() == false)
00970                         return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
00971 
00972                 objSEndTerm.setup("WN", lsqEndTermElements[ 0 ].W, REAL, ACADO_VARIABLES);
00973         }
00974 
00975         Expression expFEndTerm;
00976         objFEndTerm.getExpression( expFEndTerm );
00977 
00978         Expression expFEndTermX;
00979 
00980         expFEndTermX = forwardDerivative(expFEndTerm, vX);
00981 
00982         Function FEndTermX;
00983         FEndTermX << expFEndTermX;
00984 
00985         if (FEndTermX.isConstant() == true)
00986         {
00987                 EvaluationPoint epFEndTermX( FEndTermX );
00988 
00989                 DVector vFx = FEndTermX.evaluate( epFEndTermX );
00990 
00991                 objEvFxEnd.setup("evFxEnd", Eigen::Map<DMatrix>(vFx.data(), NYN, NX), REAL, ACADO_WORKSPACE);
00992         }
00993         else
00994         {
00995                 objFEndTerm << expFEndTermX;
00996 
00997                 objEvFxEnd.setup("evFxEnd", NYN, NX, REAL, ACADO_WORKSPACE);
00998         }
00999 
01000 //      objFEndTerm << expFEndTermX;
01001 //      objEvFxEnd.setup("evFxEnd", NYN, NX, REAL, ACADO_WORKSPACE);
01002 
01003         unsigned objFEndTermSize = objFEndTerm.getGlobalExportVariableSize();
01004         if ( objFEndTermSize > objAuxVar.getDim() )
01005         {
01006                 objAuxVar.setup(objAuxVar.getName(), objFEndTermSize, 1, REAL, objAuxVar.getDataStruct());
01007         }
01008 
01009         evaluateTerminalCost.init(objFEndTerm, "evaluateLSQEndTerm", NX, 0, 0);
01010         evaluateTerminalCost.setGlobalExportVariable( objAuxVar );
01011         evaluateTerminalCost.setPrivate( true );
01012 
01013         if (objFEndTerm.getDim() > objF.getDim())
01014         {
01015                 objValueOut.setup("objValueOut", 1, objFEndTerm.getDim(), REAL, ACADO_WORKSPACE);
01016         }
01017 
01018         if (objSEndTerm.isGiven() == true && objEvFxEnd.isGiven() == true)
01019         {
01020                 // Precompute
01021 
01022                 DMatrix m2, m1;
01023 
01024                 m2 = objEvFxEnd.getTranspose().getGivenMatrix() * objSEndTerm.getGivenMatrix();
01025                 m1 = m2 * objEvFxEnd.getGivenMatrix();
01026 
01027                 QN1 = m1;
01028                 if (m1 ==  m2)
01029                         QN2 = m1;
01030                 else
01031                         QN2 = m2;
01032         }
01033         else if (FEndTermX.isOneOrZero() == NE_ZERO)
01034         {
01035                 QN1 = zeros<double>(NX, NX);
01036                 QN2 = zeros<double>(NX, NYN);
01037         }
01038         else
01039         {
01040                 QN1.setup("QN1", NX, NX, REAL, ACADO_WORKSPACE);
01041                 QN2.setup("QN2", NX, NYN, REAL, ACADO_WORKSPACE);
01042         }
01043 
01044         DMatrix depFxEnd = objEvFxEnd.isGiven() == true ? objEvFxEnd.getGivenMatrix() : expFEndTermX.getSparsityPattern();
01045         DMatrix depQN = depFxEnd.transpose() * lsqEndTermElements[ 0 ].W * depFxEnd;
01046         diagonalHN = depQN.isDiagonal() ? true : false;
01047 
01048         LOG( LVL_DEBUG ) << "diag H_{0: N-1}: " << diagonalH << ", diag H_N: " << diagonalHN << endl;
01049 
01050         // Both are given or none is given; otherwise give an error.
01051         if (getNYN() && (objS.isGiven() ^ objSEndTerm.isGiven()))
01052                 return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "All weighting matrices have to be defined (or all undefined)");
01053 
01054         setupObjectiveLinearTerms( _objective );
01055 
01056         setupResidualVariables();
01057 
01058         return SUCCESSFUL_RETURN;
01059 }
01060 
01061 returnValue ExportNLPSolver::setupObjectiveLinearTerms(const Objective& _objective)
01062 {
01063         int variableObjS;
01064         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
01065 
01067         //
01068         // Setup the linear terms
01069         //
01071 
01072         LsqLinearElements lsqLinearElements;
01073         _objective.getLSQLinearTerms( lsqLinearElements );
01074 
01075         if (lsqLinearElements.size() > 0)
01076         {
01077                 ASSERT_RETURN(lsqLinearElements.size() == 1);
01078 
01079                 if (variableObjS == YES)
01080                 {
01081                         objSlx.setup("Wlx", (N + 1) * NX, 1, REAL, ACADO_VARIABLES);
01082                         objSlu.setup("Wlu", N * NU, 1, REAL, ACADO_VARIABLES);
01083                 }
01084                 else
01085                 {
01086                         ASSERT_RETURN( lsqLinearElements[ 0 ].Wlx.getDim() == NX );
01087                         ASSERT_RETURN( lsqLinearElements[ 0 ].Wlu.getDim() == NU );
01088 
01089                         if (lsqLinearElements[ 0 ].givenW == false)
01090                         {
01091                                 objSlx.setup("Wlx", lsqLinearElements[ 0 ].Wlx, REAL, ACADO_VARIABLES, false, "", false);
01092                                 objSlu.setup("Wlu", lsqLinearElements[ 0 ].Wlu, REAL, ACADO_VARIABLES, false, "", false);
01093                         }
01094                         else
01095                         {
01096                                 objSlx.setup("Wlx", lsqLinearElements[ 0 ].Wlx, REAL, ACADO_VARIABLES);
01097                                 objSlu.setup("Wlu", lsqLinearElements[ 0 ].Wlu, REAL, ACADO_VARIABLES);
01098                         }
01099                 }
01100         }
01101         else
01102         {
01103                 objSlx = zeros<double>(NX, 1);
01104                 objSlu = zeros<double>(NU, 1);
01105         }
01106 
01107         objSlx.setDoc("Linear term weighting vector for states.");
01108         objSlu.setDoc("Linear term weighting vector for controls.");
01109 
01110         return SUCCESSFUL_RETURN;
01111 }
01112 
01113 returnValue ExportNLPSolver::setupResidualVariables()
01114 {
01115         y.setup("y",  getN() * getNY(), 1, REAL, ACADO_VARIABLES);
01116         y.setDoc( string("Matrix containing ") + toString( N ) +
01117                         " reference/measurement vectors of size " + toString( NY ) + " for first " + toString( N ) + " nodes." );
01118         yN.setup("yN", getNYN(), 1, REAL, ACADO_VARIABLES);
01119         yN.setDoc( string("Reference/measurement vector for the ") + toString(N + 1) + ". node." );
01120         Dy.setup("Dy", getN() * getNY(), 1, REAL,ACADO_WORKSPACE);
01121         DyN.setup("DyN", getNYN(), 1, REAL, ACADO_WORKSPACE);
01122 
01123         return SUCCESSFUL_RETURN;
01124 }
01125 
01126 returnValue ExportNLPSolver::setConstraints(const OCP& _ocp)
01127 {
01129         //
01130         // Extract box constraints
01131         //
01133 
01134         Grid grid;
01135         Constraint constraints;
01136 
01137         _ocp.getGrid( grid );
01138         _ocp.getConstraint( constraints );
01139 
01140         VariablesGrid ugrid(NU, grid);
01141         VariablesGrid xgrid(NX, grid);
01142 
01143         OCPiterate tmp;
01144         tmp.init(&xgrid, 0, 0, &ugrid, 0);
01145 
01146         constraints.getBounds( tmp );
01147 
01148         bool boxConIsFinite = false;
01149         DVector lbTmp;
01150         DVector ubTmp;
01151 
01152         //
01153         // Extract box constraints on inputs
01154         //
01155         for (unsigned i = 0; i < tmp.u->getNumPoints(); ++i)
01156         {
01157                 lbTmp = tmp.u->getLowerBounds( i );
01158                 ubTmp = tmp.u->getUpperBounds( i );
01159 
01160                 if ((ubTmp >= lbTmp) == false)
01161                         return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Some lower bounds are bigger than upper bounds?");
01162 
01163                 if (isFinite( lbTmp ) || isFinite( ubTmp ))
01164                         boxConIsFinite = true;
01165         }
01166 
01167         if (boxConIsFinite == true)
01168                 uBounds = *(tmp.u);
01169         else
01170                 uBounds.init();
01171 
01172         //
01173         // Extract box constraints on states
01174         //
01175         boxConIsFinite = false;
01176         for (unsigned i = 0; i < tmp.x->getNumPoints(); ++i)
01177         {
01178                 lbTmp = tmp.x->getLowerBounds( i );
01179                 ubTmp = tmp.x->getUpperBounds( i );
01180 
01181                 if ((ubTmp >= lbTmp) == false)
01182                         return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Some lower bounds are bigger than upper bounds?");
01183 
01184                 if (isFinite( lbTmp ) || isFinite( ubTmp ))
01185                         boxConIsFinite = true;
01186         }
01187 
01188         if ( boxConIsFinite == true )
01189                 xBounds = *(tmp.x);
01190         else
01191                 xBounds.init();
01192 
01194         //
01195         // Intermezzo - reset static counters
01196         //
01198 
01199         DifferentialState dummy0;
01200         Control dummy1;
01201         dummy0.clearStaticCounters();
01202         dummy1.clearStaticCounters();
01203 
01204         DifferentialState vX("", NX, 1);
01205         Control vU("", NU, 1);
01206 
01208         //
01209         // Extract path constraints; pac prefix
01210         //
01212 
01213         conAuxVar.setName( "conAuxVar" );
01214         conAuxVar.setDataStruct( ACADO_WORKSPACE );
01215 
01216         Function pacH;
01217 
01218         DMatrix pacLBMatrix, pacUBMatrix;
01219         constraints.getPathConstraints(pacH, pacLBMatrix, pacUBMatrix);
01220 
01221         dimPacH = pacH.getDim();
01222 
01223         if (dimPacH != 0)
01224         {
01225                 lbPathConValues = pacLBMatrix.getRows(0, N - 1).makeVector();
01226                 ubPathConValues = pacUBMatrix.getRows(0, N - 1).makeVector();
01227 
01228                 Expression expPacH, expPacHx, expPacHu;
01229                 pacH.getExpression( expPacH );
01230 
01231                 expPacHx = forwardDerivative(expPacH, vX);
01232                 expPacHu = forwardDerivative(expPacH, vU);
01233 
01234                 Function pacHx, pacHu;
01235                 pacHx << expPacHx;
01236                 pacHu << expPacHu;
01237 
01238                 // Set dimension of residual
01239                 pacEvH.setup("evH", N * dimPacH, 1, REAL, ACADO_WORKSPACE);
01240 
01241                 // Check derivative of path constraints w.r.t. x
01242                 if (pacHx.isConstant())
01243                 {
01244                         EvaluationPoint epPacHx( pacHx );
01245                         DVector v = pacHx.evaluate( epPacHx );
01246 
01247                         if (v.isZero() == false)
01248                         {
01249                                 pacEvHx.setup("evHx", Eigen::Map<DMatrix>(v.data(), dimPacH, NX), REAL, ACADO_WORKSPACE);
01250                         }
01251                 }
01252                 else
01253                 {
01254                         pacH << expPacHx;
01255 
01256                         pacEvHx.setup("evHx", N * dimPacH, NX, REAL, ACADO_WORKSPACE);
01257                 }
01258 
01259                 // Check derivative of path constraints w.r.t. u
01260                 if (pacHu.isConstant())
01261                 {
01262                         EvaluationPoint epPacHu( pacHu );
01263                         DVector v = pacHu.evaluate( epPacHu );
01264 
01265                         if (v.isZero() == false)
01266                         {
01267                                 pacEvHu.setup("evHu", Eigen::Map<DMatrix>(v.data(), dimPacH, NU), REAL, ACADO_WORKSPACE);
01268                         }
01269                 }
01270                 else
01271                 {
01272                         pacH << expPacHu;
01273 
01274                         pacEvHu.setup("evHu", N * dimPacH, NU, REAL, ACADO_WORKSPACE);
01275                 }
01276 
01277                 if (performsSingleShooting() == false)
01278                 {
01279                         pacEvHxd.setup("evHxd", dimPacH, 1, REAL, ACADO_WORKSPACE);
01280                 }
01281 
01282                 conAuxVar.setup("conAuxVar", pacH.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
01283                 conValueIn.setup("conValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
01284                 conValueOut.setup("conValueOut", 1, pacH.getDim(), REAL, ACADO_WORKSPACE);
01285 
01286                 evaluatePathConstraints.init(pacH, "evaluatePathConstraints", NX, 0, NU, NP, 0, NOD);
01287                 evaluatePathConstraints.setGlobalExportVariable( conAuxVar );
01288         }
01289 
01291         //
01292         // Extract point constraints; poc prefix
01293         //
01295 
01296         Function pocH;
01297         Expression expPocH, expPocHx, expPocHu;
01298         DMatrix pocLBMatrix, pocUBMatrix;
01299 
01300         evaluatePointConstraints.resize(N + 1);
01301 
01302         unsigned dimPocHMax = 0;
01303 
01304         pocLbStack.resize(N + 1);
01305         pocUbStack.resize(N + 1);
01306 
01307         // Setup the point constraints
01308         for (unsigned i = 0; i < N + 1; ++i)
01309         {
01310                 // Get the point constraint
01311                 constraints.getPointConstraint(i, pocH, pocLBMatrix, pocUBMatrix);
01312 
01313                 // Extract and stack the point constraint if it exists
01314                 if ( pocH.getDim() )
01315                 {
01316                         if (pocH.getNU() > 0 && i == N)
01317                         {
01318                                 return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "The terminal (point) constraint must not depend on controls.");
01319                         }
01320 
01321                         // Extract the function expression and stack its Jacobians w.r.t.
01322                         // x and u
01323                         pocH.getExpression( expPocH );
01324 
01325                         // XXX AFAIK, this is not bullet-proof!
01326 //                      if (expPocH.getVariableType() != VT_INTERMEDIATE_STATE)
01327                         if (expPocH.getVariableType() != VT_UNKNOWN && expPocH.getVariableType() != VT_INTERMEDIATE_STATE)
01328                                 continue;
01329 
01330                         expPocHx = forwardDerivative(expPocH, vX);
01331                         pocH << expPocHx;
01332 
01333                         if (i < N)
01334                         {
01335                                 expPocHu = forwardDerivative(expPocH, vU);
01336                                 pocH << expPocHu;
01337                         }
01338 
01339                         // Stack the new function
01340                         evaluatePointConstraints[ i ] = std::tr1::shared_ptr< ExportAcadoFunction >(new ExportAcadoFunction);
01341 
01342                         std::string pocFName;
01343 
01344                         pocFName = "evaluatePointConstraint" + toString( i );
01345 
01346                         if (i < N)
01347                         {
01348                                 evaluatePointConstraints[ i ]->init(pocH, pocFName, NX, 0, NU, NP, 0, NOD);
01349                         }
01350                         else
01351                         {
01352                                 evaluatePointConstraints[ i ]->init(pocH, pocFName, NX, 0, 0, NP, 0, NOD);
01353                         }
01354 
01355                         // Determine the maximum function dimension
01356                         if ( dimPocHMax < (unsigned)pocH.getDim() )
01357                         {
01358                                 dimPocHMax =  pocH.getDim();
01359                         }
01360 
01361                         // TODO This is too specific for condensing, thus should be moved to condensing class.
01362                         // Stack the lower and upper bounds
01363                         lbPointConValues.append( pocLBMatrix.getRow( 0 ) );
01364                         ubPointConValues.append( pocUBMatrix.getRow( 0 ) );
01365 
01366                         pocLbStack[ i ] = pocLBMatrix.getRow( 0 );
01367                         pocUbStack[ i ] = pocUBMatrix.getRow( 0 );
01368                 }
01369         }
01370 
01371 //      std::cout << "lb dim: " << pocLB.getDim() << std::endl;
01372 //      std::cout << "ub dim: " << pocUB.getDim() << std::endl;
01373 
01374         dimPocH = lbPointConValues.getDim();
01375 
01376         if ( dimPocH != 0 )
01377         {
01378                 unsigned pocAuxVarDim = 0;
01379 
01380                 ExportVariable pocAuxVarTemp;
01381 
01382                 for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
01383                 {
01384                         if ( !evaluatePointConstraints[ i ] )
01385                                 continue;
01386 
01387                         pocAuxVarTemp = evaluatePointConstraints[ i ]->getGlobalExportVariable();
01388 
01389                         pocAuxVarDim = pocAuxVarDim < pocAuxVarTemp.getDim() ? pocAuxVarTemp.getDim() : pocAuxVarDim;
01390 
01391                         evaluatePointConstraints[ i ]->setGlobalExportVariable( conAuxVar );
01392                 }
01393 
01394                 int conAuxVarDim =
01395                                 (conAuxVar.getDim() < pocAuxVarDim) ? pocAuxVarDim : conAuxVar.getDim();
01396                 conAuxVar.setup("conAuxVar", conAuxVarDim, 1, REAL, ACADO_WORKSPACE);
01397 
01398                 conValueIn.setup("conValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
01399 
01400                 unsigned conValueOutDim =
01401                                 (dimPocHMax < conValueOut.getDim()) ? conValueOut.getDim() : dimPocHMax;
01402                 conValueOut.setup("conValueOut", 1, conValueOutDim, REAL, ACADO_WORKSPACE);
01403 
01404                 pocEvH.setup("pocEvH", dimPocH, 1, REAL, ACADO_WORKSPACE);
01405                 pocEvHx.setup("pocEvHx", dimPocH, NX, REAL, ACADO_WORKSPACE);
01406 
01407                 // For this guy we actually need less... but no worry for now
01408                 pocEvHu.setup("pocEvHu", dimPocH, NU, REAL, ACADO_WORKSPACE);
01409 
01410                 // Setup one more variable for MS:
01411                 if (performsSingleShooting() == false)
01412                 {
01413                         pocEvHxd.setup("pocEvHxd", dimPocH, 1, REAL, ACADO_WORKSPACE);
01414                 }
01415         }
01416 
01417         return SUCCESSFUL_RETURN;
01418 }
01419 
01420 unsigned ExportNLPSolver::getNumComplexConstraints( void )
01421 {
01422         return N * dimPacH + dimPocH;
01423 }
01424 
01425 bool ExportNLPSolver::initialStateFixed() const
01426 {
01427         int fixInitialState;
01428         get(FIX_INITIAL_STATE, fixInitialState);
01429 
01430         return (bool)fixInitialState;
01431 }
01432 
01433 bool ExportNLPSolver::usingLinearTerms() const
01434 {
01435         if (objSlx.isGiven() == false && objSlu.isGiven() == false)
01436                 return true;
01437         // Otherwise they are hard-coded and we don't need this indicator
01438         return false;
01439 }
01440 
01441 returnValue ExportNLPSolver::setupAuxiliaryFunctions()
01442 {
01444         //
01445         // Shift controls
01446         //
01448         ExportVariable uEnd("uEnd", NU, 1, REAL, ACADO_LOCAL);
01449         uEnd.setDoc( "Value for the u vector on the second to last node. If =0 the old value is used." );
01450         ExportIndex index( "index" );
01451         shiftControls.setup("shiftControls", uEnd);
01452         shiftControls.addIndex( index );
01453         shiftControls.doc( "Shift controls vector by one interval." );
01454 
01455         ExportForLoop uLoop(index, 0, N - 1);
01456         uLoop.addStatement( u.getRow( index ) == u.getRow(index + 1) );
01457 
01458         shiftControls.addStatement( uLoop );
01459         shiftControls.addLinebreak( );
01460         shiftControls.addStatement( "if (uEnd != 0)\n{\n" );
01461         shiftControls.addStatement(u.getRow(N - 1) == uEnd.getTranspose());
01462         shiftControls.addStatement( "}\n" );
01463 
01465         //
01466         // Shift states
01467         //
01469         ExportVariable xEnd("xEnd", NX, 1, REAL, ACADO_LOCAL);
01470         xEnd.setDoc( "Value for the x vector on the last node. If =0 the old value is used." );
01471         ExportIndex strategy( "strategy" );
01472         strategy.setDoc( string("Shifting strategy: 1. Initialize node ") + toString(N + 1) + " with xEnd." \
01473                         " 2. Initialize node " + toString(N + 1) + " by forward simulation." );
01474         // TODO Think about adding zEnd here at some point...
01475         shiftStates.setup("shiftStates", strategy, xEnd, uEnd);
01476         shiftStates.addIndex( index );
01477         if (NXA == 0)
01478                 shiftStates.doc( "Shift differential variables vector by one interval." );
01479         else
01480                 shiftStates.doc( "Shift differential variables vector and algebraic variables vector by one interval." );
01481 
01482         ExportForLoop xLoop(index, 0, N);
01483         xLoop.addStatement( x.getRow( index ) == x.getRow(index + 1) );
01484         shiftStates.addStatement( xLoop );
01485 
01486         if (NXA > 0)
01487         {
01488                 ExportForLoop zLoop(index, 0, N - 1);
01489                 zLoop.addStatement( z.getRow( index ) == z.getRow(index + 1) );
01490                 shiftStates.addStatement( zLoop );
01491         }
01492 
01493         shiftStates.addLinebreak( );
01494         shiftStates.addStatement( "if (strategy == 1 && xEnd != 0)\n{\n" );
01495         shiftStates.addStatement( x.getRow( N ) == xEnd.getTranspose() );
01496         shiftStates.addStatement( "}\n" );
01497         shiftStates.addStatement( "else if (strategy == 2) \n{\n" );
01498 
01499         uint symH = (NX+NU)*(NX+NU+1)/2;
01500 
01501         int hessianApproximation;
01502         get( HESSIAN_APPROXIMATION, hessianApproximation );
01503         bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
01504 
01505         unsigned indexZ   = NX + NXA;
01506         if( secondOrder ) indexZ = indexZ + NX;         // because of the first order adjoint direction
01507         unsigned indexGxx = indexZ + NX * NX;
01508         unsigned indexGzx = indexGxx + NXA * NX;
01509         unsigned indexGxu = indexGzx + NX * NU;
01510         unsigned indexGzu = indexGxu + NXA * NU;
01511         unsigned indexH = indexGzu;
01512         if( secondOrder ) indexH = indexGzu + symH;     // because of the second order derivatives
01513         unsigned indexU   = indexH + NU;
01514         unsigned indexOD   = indexU + NOD;
01515 
01516         shiftStates.addStatement( state.getCols(0, NX) == x.getRow( N ) );
01517         shiftStates.addStatement( state.getCols(NX, NX + NXA) == z.getRow(N - 1) );
01518         shiftStates.addStatement( "if (uEnd != 0)\n{\n" );
01519         shiftStates.addStatement( state.getCols(indexH, indexU) == uEnd.getTranspose() );
01520         shiftStates.addStatement( "}\n" );
01521         shiftStates.addStatement( "else\n{\n" );
01522         shiftStates.addStatement( state.getCols(indexH, indexU) == u.getRow(N - 1) );
01523         shiftStates.addStatement( "}\n" );
01524         shiftStates.addStatement( state.getCols(indexU, indexOD) == od.getRow( N ) );
01525         shiftStates.addLinebreak( );
01526 
01527         if ( integrator->equidistantControlGrid() )
01528         {
01529                 shiftStates << "integrate" << "(" << state.getFullName() << ", 1);\n";
01530         }
01531         else
01532         {
01533                 shiftStates << "integrate" << "(" << state.getFullName() << ", 1, " << toString(N - 1) << ");\n";
01534         }
01535 
01536         shiftStates.addLinebreak( );
01537         shiftStates.addStatement( x.getRow( N ) == state.getCols(0, NX) );
01538         if ( NXA )
01539         {
01540                 shiftStates.addLinebreak();
01541                 shiftStates.addStatement(z.getRow(N - 1) == state.getCols(NX, NX + NXA));
01542         }
01543 
01544 
01545         shiftStates.addStatement( "}\n" );
01546 
01548         //
01549         // Initialize nodes by a forward simulation
01550         //
01552         initializeNodes.setup("initializeNodesByForwardSimulation");
01553         initializeNodes.addIndex( index );
01554         initializeNodes.doc( "Initialize shooting nodes by a forward simulation starting from the first node." );
01555 
01556         ExportForLoop iLoop(index, 0, N);
01557 
01558 
01559         iLoop.addStatement( state.getCols(0, NX)                == x.getRow( index ) );
01560         if ( NXA )
01561         {
01562                 iLoop << std::string("if (") << index.getFullName() << std::string(" > 0){");
01563                 iLoop.addStatement( state.getCols(NX, NX + NXA) == z.getRow(index - 1) );
01564                 iLoop << std::string("}\n");
01565         }
01566         iLoop.addStatement( state.getCols(indexH, indexU)       == u.getRow( index ) );
01567         iLoop.addStatement( state.getCols(indexU, indexOD)      == od.getRow( index ) );
01568         iLoop.addLinebreak( );
01569 
01570         if ( integrator->equidistantControlGrid() )
01571         {
01572                 iLoop << "integrate"
01573                                 << "(" << state.getFullName() << ", "
01574                                 << index.getFullName() << " == 0"
01575                                 << ");\n";
01576         }
01577         else
01578         {
01579                 iLoop << "integrate"
01580                                 << "(" << state.getFullName() << ", "
01581                                 << index.getFullName() << " == 0"
01582                                 << ", " << index.getFullName() << ");\n";
01583         }
01584 
01585         iLoop.addLinebreak();
01586         iLoop.addStatement( x.getRow(index + 1) == state.getCols(0, NX) );
01587 
01588         // Store improved initial guess from the integrator
01589         iLoop.addStatement( z.getRow(index) == state.getCols(NX, NX + NXA) );
01590 
01591         initializeNodes.addStatement( iLoop );
01592 
01593         return setupGetObjective();
01594 }
01595 
01596 
01597 returnValue ExportNLPSolver::setupGetObjective(  )
01598 {
01599         if( getNY() > 0 || getNYN() > 0 ) {
01600                 return setupGetLSQObjective( );
01601         }
01602         else {
01603                 return setupGetGeneralObjective( );
01604         }
01605 }
01606 
01607 
01608 returnValue ExportNLPSolver::setupGetLSQObjective() {
01610         //
01611         // Objective value calculation
01612         //
01614 
01615         getObjective.setup( "getObjective" );
01616         getObjective.doc( "Calculate the objective value." );
01617         ExportVariable objVal("objVal", 1, 1, REAL, ACADO_LOCAL, true);
01618         objVal.setDoc( "Value of the objective function." );
01619         getObjective.setReturnValue( objVal );
01620 
01621         ExportVariable tmpDx("tmpDx", 1, NX, REAL, ACADO_LOCAL );
01622         ExportVariable tmpDy("tmpDy", 1, getNY(), REAL, ACADO_LOCAL );
01623         ExportVariable tmpDyN("tmpDyN", 1, getNYN(), REAL, ACADO_LOCAL );
01624 
01625         getObjective.addVariable( tmpDy );
01626         getObjective.addVariable( tmpDyN );
01627 
01628         ExportIndex oInd;
01629         getObjective.acquire( oInd );
01630 
01631         // Recalculate objective
01632 
01633         ExportForLoop loopObjective(oInd, 0, N);
01634 
01635         loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( oInd ) );
01636         loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( oInd ) );
01637         loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( oInd ) );
01638         loopObjective.addLinebreak( );
01639 
01640         // Evaluate the objective function
01641         loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
01642 
01643         // Stack the measurement function value
01644         loopObjective.addStatement(
01645                         Dy.getRows(oInd * NY, (oInd + 1) * NY) ==
01646                                         objValueOut.getTranspose().getRows(0, getNY()) - y.getRows(oInd * NY, (oInd + 1) * NY)
01647         );
01648 
01649         getObjective.addStatement( loopObjective );
01650 
01651         getObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
01652         getObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
01653 
01654         // Evaluate the objective function
01655         getObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
01656 
01657         getObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) - yN.getTranspose() );
01658 
01659         getObjective.addStatement( objVal == 0 );
01660 
01661         ExportForLoop oLoop(oInd, 0, N);
01662 
01663         int variableObjS;
01664         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
01665 
01666         if (variableObjS == NO)
01667         {
01668                 oLoop.addStatement( tmpDy == Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * objS );
01669                 oLoop.addStatement( objVal += Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * tmpDy.getTranspose() );
01670         }
01671         else
01672         {
01673                 oLoop.addStatement( tmpDy == Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * objS.getSubMatrix(oInd * NY, (oInd + 1) * NY, 0, NY) );
01674                 oLoop.addStatement( objVal += Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * tmpDy.getTranspose() );
01675         }
01676 
01677         getObjective.addStatement( oLoop );
01678         getObjective.addLinebreak( );
01679 
01680         getObjective.addStatement( tmpDyN == DyN.getTranspose() * objSEndTerm );
01681         getObjective.addStatement( objVal += DyN.getTranspose() * tmpDyN.getTranspose() );
01682 
01683         if ( SAC.getDim() > 0 )
01684         {
01685                 getObjective.addVariable( tmpDx );
01686                 getObjective.addStatement( tmpDx == DxAC.getTranspose() * SAC );
01687                 getObjective.addStatement( objVal +=  tmpDx * DxAC );
01688         }
01689         getObjective.addLinebreak( );
01690 
01691         getObjective.addStatement( "objVal *= 0.5;\n" );
01692 
01693         return SUCCESSFUL_RETURN;
01694 }
01695 
01696 returnValue ExportNLPSolver::setupGetGeneralObjective() {
01698         //
01699         // Objective value calculation
01700         //
01702 
01703         getObjective.setup( "getObjective" );
01704         getObjective.doc( "Calculate the objective value." );
01705         ExportVariable objVal("objVal", 1, 1, REAL, ACADO_LOCAL, true);
01706         objVal.setDoc( "Value of the objective function." );
01707         getObjective.setReturnValue( objVal );
01708 
01709         ExportIndex oInd;
01710         getObjective.acquire( oInd );
01711 
01712         // Recalculate objective
01713         getObjective.addStatement( objVal == 0 );
01714 
01715         if( evaluateStageCost.getFunctionDim() > 0 ) {
01716                 ExportForLoop loopObjective(oInd, 0, N);
01717 
01718                 loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( oInd ) );
01719                 loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( oInd ) );
01720                 loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( oInd ) );
01721                 loopObjective.addLinebreak( );
01722 
01723                 // Evaluate the objective function
01724                 loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
01725 
01726                 // Stack the measurement function value
01727                 loopObjective.addStatement( objVal += objValueOut.getCol(0) );
01728 
01729                 getObjective.addStatement( loopObjective );
01730         }
01731         if( evaluateTerminalCost.getFunctionDim() > 0 ) {
01732                 getObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
01733                 getObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
01734 
01735                 // Evaluate the objective function
01736                 getObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
01737 
01738                 getObjective.addStatement( objVal += objValueOut.getCol(0) );
01739         }
01740 
01741         return SUCCESSFUL_RETURN;
01742 }
01743 
01744 unsigned ExportNLPSolver::weightingMatricesType( void ) const
01745 {
01746         if (objS.isGiven() == true && objSEndTerm.isGiven() == true)
01747                 return 0;
01748 
01749         // get the option for variable objS matrix.
01750         int variableObjS;
01751         get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
01752         if ( variableObjS )
01753                 return 2;
01754 
01755         return 1;
01756 }
01757 
01758 returnValue ExportNLPSolver::setupArrivalCostCalculation()
01759 {
01760         int useArrivalCost;
01761         get(CG_USE_ARRIVAL_COST, useArrivalCost);
01762         if (useArrivalCost == NO)
01763                 return SUCCESSFUL_RETURN;
01764 
01765         if ( useArrivalCost )
01766         {
01767                 SAC.setup("SAC", NX, NX, REAL, ACADO_VARIABLES);
01768                 SAC.setDoc("Arrival cost term: inverse of the covariance matrix.");
01769                 xAC.setup("xAC", NX, 1, REAL, ACADO_VARIABLES);
01770                 xAC.setDoc("Arrival cost term: a priori state estimate.");
01771                 DxAC.setup("DxAC", NX, 1, REAL, ACADO_WORKSPACE);
01772         }
01773 
01774         ExportVariable evRet("ret", 1, 1, INT, ACADO_LOCAL, true);
01775 
01776         ExportVariable evReset("reset", 1, 1, INT, ACADO_LOCAL, true);
01777         evReset.setDoc("Reset S_{AC}. Set it to 1 to initialize arrival cost calculation, "
01778                                    "and later should set it to 0.");
01779 
01780         updateArrivalCost.init("updateArrivalCost", evReset);
01781         updateArrivalCost.doc("Use this function to update the arrival cost.");
01782         updateArrivalCost.setReturnValue( evRet );
01783         updateArrivalCost << (evRet == 0);
01784 
01785         const unsigned AM = 2 * NX + NY;
01786         const unsigned AN = 2 * NX + NU; // A bit different from my implementation
01787 
01788         acA.setup("acA", AM, AN, REAL, ACADO_WORKSPACE);
01789         acb.setup("acb", AM, 1,  REAL, ACADO_WORKSPACE);
01790         acP.setup("acP", NX, NX, REAL, ACADO_WORKSPACE);
01791 
01792         acWL.setup("WL", NX, NX, REAL, ACADO_VARIABLES);
01793         acWL.setDoc("Arrival cost term: Cholesky decomposition, lower triangular, "
01794                                 " of the inverse of the state noise covariance matrix.");
01795         acVL.setup("acVL", NY, NY, REAL, ACADO_WORKSPACE);
01796 
01797         acHx.setup("acHx", NY, NX, REAL, ACADO_WORKSPACE);
01798         acHu.setup("acHu", NY, NU, REAL, ACADO_WORKSPACE);
01799         acXx.setup("acXx", NX, NX, REAL, ACADO_WORKSPACE);
01800         acXu.setup("acXu", NX, NU, REAL, ACADO_WORKSPACE);
01801 
01802         acXTilde.setup("acXTilde", NX, 1, REAL, ACADO_WORKSPACE);
01803         acHTilde.setup("acHTilde", NY, 1, REAL, ACADO_WORKSPACE);
01804 
01805         //
01806         // Perform a hard reset if necessary
01807         // This is a bit messy because the update code needs upper triangular P
01808         // matrix, but Cholesky function returns lower triangular. One way to
01809         // avoid this is to be add an option to ExportCholeskyDecomposition to
01810         // ---> export lower or upper triangular matrix.
01811         //
01812         cholSAC.init("cholSAC", NX);
01813         cholSAC.setup();
01814 
01815         updateArrivalCost << "\nif ( " << evReset.getName() << " )\n{\n";
01816         updateArrivalCost.addStatement( acXx == SAC );
01817         updateArrivalCost.addFunctionCall(cholSAC.getName(), acXx);
01818         updateArrivalCost << (acP == acXx.getTranspose());
01819         updateArrivalCost << std::string( "return 0;\n}\n\n" );
01820 
01821         //
01822         // Evaluate model @ the first node
01823         //
01824 
01825         unsigned indexZ   = NX + NXA;
01826         unsigned indexGxx = indexZ + NX * NX;
01827         unsigned indexGzx = indexGxx + NXA * NX;
01828         unsigned indexGxu = indexGzx + NX * NU;
01829         unsigned indexGzu = indexGxu + NXA * NU;
01830         unsigned indexU   = indexGzu + NU;
01831         unsigned indexNOD   = indexU + NOD;
01832 
01833         updateArrivalCost.addStatement( state.getCols(0, NX) == x.getRow( 0 ) );
01834         updateArrivalCost.addStatement( state.getCols(NX, NX + NXA) == z.getRow( 0 ) );
01835         updateArrivalCost.addStatement( state.getCols(indexGzu, indexU) == u.getRow( 0 ) );
01836         updateArrivalCost.addStatement( state.getCols(indexU, indexNOD) == od.getRow( 0 ) );
01837 
01838         if (integrator->equidistantControlGrid())
01839                 updateArrivalCost << "integrate" << "(" << state.getFullName() << ", 1);\n";
01840         else
01841                 updateArrivalCost << "integrate" << "(" << state.getFullName() << ", 1, " << toString(0) << ");\n";
01842         updateArrivalCost.addLinebreak( );
01843 
01844         //
01845         // Evaluate objective function @ the first node
01846         //
01847 
01848         updateArrivalCost.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( 0 ) );
01849         updateArrivalCost.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( 0 ) );
01850         updateArrivalCost.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( 0 ) );
01851 
01852         updateArrivalCost.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
01853         updateArrivalCost.addLinebreak( );
01854 
01855         //
01856         // Cholesky decomposition of the term objS
01857         //
01858         if (objS.isGiven() == true)
01859         {
01860                 DMatrix m = objS.getGivenMatrix();
01861                 DMatrix mChol = m.llt().matrixL();
01862 
01863                 initialize << (acVL == mChol);
01864         }
01865         else
01866         {
01867                 cholObjS.init("cholObjS", NY);
01868                 cholObjS.setup();
01869 
01870                 int variableObjS;
01871                 get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
01872 
01873                 if ( variableObjS )
01874                 {
01875                         updateArrivalCost << (acVL == objS.getSubMatrix(0, NY, 0, NY));
01876                         updateArrivalCost.addFunctionCall(cholObjS.getName(), acVL);
01877                 }
01878                 else
01879                 {
01880                         updateArrivalCost << (acVL == objS);
01881                         updateArrivalCost.addFunctionCall(cholObjS.getName(), acVL);
01882                 }
01883         }
01884 
01885         //
01886         // Create acA and acb
01887         //
01888 
01889         /*
01890                 PL is square root form already.
01891 
01892                 A = ||PL         zeros(nx, nu)  zeros(nx, nx) ||
01893                 ||-VL * Hx   -VL * Hu       zeros(ny, nx) ||
01894                 ||-WL * Xx   -WL * Xu       WL            ||
01895 
01896         Since A is initialized to 0, we should use use -= operator
01897          */
01898 
01899         // Clear A and b
01900         updateArrivalCost
01901                 << (acA == zeros<double>(AM, AN))
01902                 << (acb == zeros<double>(AM, 1))
01903                 << std::string( "\n" );
01904 
01905         // Copy products to the matrices
01906         updateArrivalCost
01907                 << (acXx.makeRowVector() == state.getCols(indexZ, indexGxx))
01908                 << (acXu.makeRowVector() == state.getCols(indexGzx, indexGxu));
01909 
01910         unsigned ind = NY;
01911         if (objEvFx.isGiven() == true)
01912         {
01913                 initialize << (acHx == objEvFx);
01914         }
01915         else
01916         {
01917                 updateArrivalCost << (acHx.makeRowVector() == objValueOut.getCols(ind, ind + NY * NX));
01918                 ind += NY * NX;
01919         }
01920 
01921         if (objEvFu.isGiven() == true)
01922         {
01923                 initialize << (acHu == objEvFu);
01924         }
01925         else
01926         {
01927                 updateArrivalCost << (acHu.makeRowVector() == objValueOut.getCols(ind, ind + NY * NU));
01928         }
01929 
01930         // acVL and acWL are lower triangular matrices
01931         // acP is ALWAYS upper triangular!
01932 
01933         updateArrivalCost
01934                 << (acA.getSubMatrix(0, NX, 0, NX) == acP)
01935 
01936                 << (acA.getSubMatrix(NX, NX + NY, 0, NX) -= (acVL ^ acHx))
01937                 << (acA.getSubMatrix(NX, NX + NY, NX, NX + NU) -= (acVL ^ acHu))
01938 
01939                 << (acA.getSubMatrix(NX + NY, NX + NY + NX, 0, NX) -= (acWL ^ acXx))
01940                 << (acA.getSubMatrix(NX + NY, NX + NY + NX, NX, NX + NU) -= (acWL ^ acXu))
01941                 << (acA.getSubMatrix(NX + NY, NX + NY + NX, NX + NU, NX + NU + NX) == acWL.getTranspose());
01942 
01943         /*
01944 
01945         x1 is output from the integrator
01946         h  is evaluated obj @ node 0
01947         x and u are current solutions
01948         xL and uL are previous arrival cost values.
01949 
01950         x_tilde = x1 - np.dot(Xx,x) - np.dot(Xu,u)
01951     h_tilde =  h - np.dot(Hx,x) - np.dot(Hu,u)
01952 
01953         res = np.bmat([ -np.dot(PL, xL),
01954                      np.dot(VL, yL - h_tilde),
01955                     -np.dot(WL, x_tilde) ])
01956 
01957          */
01958 
01959         updateArrivalCost
01960                 << (acXTilde == state.getTranspose().getRows(0, NX) - acXx * x.getRow( 0 ).getTranspose())
01961                 << (acXTilde -= acXu * u.getRow( 0 ).getTranspose());
01962 
01963         updateArrivalCost
01964                 << (acHTilde == y.getRows(0, NY) - objValueOut.getTranspose().getRows(0, NY))
01965                 << (acHTilde += acHx * x.getRow( 0 ).getTranspose())
01966                 << (acHTilde += acHu * u.getRow( 0 ).getTranspose());
01967 
01968         // Inverted signs from Mario's implementation
01969         updateArrivalCost
01970                 << (acb.getRows(0, NX) == (acP * xAC))
01971                 << (acb.getRows(NX, NX + NY) -= (acVL ^ acHTilde))
01972                 << (acb.getRows(NX + NY, NX + NY + NX) == (acWL ^ acXTilde));
01973 
01974         //
01975         // Solver the linear system
01976         // We need first NX back-solves to get solution of this linear system...
01977         //
01978         acSolver.init(AM, AN, NX, false, false, std::string("ac"));
01979         acTmp = acSolver.getGlobalExportVariable( 1 );
01980         updateArrivalCost.addFunctionCall(acSolver.getNameSolveFunction(), acA, acb, acTmp);
01981 
01982         //
01983         // Get the solution of the linear system
01984         //
01985 
01986         updateArrivalCost << (xAC == acb.getRows(NX + NU, 2 * NX + NU));
01987 
01988         // Get the update acP, upper triangular part
01989         for (unsigned row = 0; row < NX; ++row)
01990                 for (unsigned col = row; col < NX; ++col)
01991                         updateArrivalCost << (acP.getElement(row, col) == acA.getElement(NX + NU + row, NX + NU + col));
01992 
01993         // Calculate the weighting matrix which is used outside.
01994         updateArrivalCost << (SAC == (acP ^ acP));
01995 
01996         return SUCCESSFUL_RETURN;
01997 }
01998 
01999 CLOSE_NAMESPACE_ACADO


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