export_nlp_solver.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
32 #include <acado/code_generation/export_nlp_solver.hpp>
33 
34 #include <acado/objective/objective.hpp>
35 #include <acado/ocp/ocp.hpp>
36 
37 BEGIN_NAMESPACE_ACADO
38 
39 using namespace std;
40 
41 ExportNLPSolver::ExportNLPSolver( UserInteraction* _userInteraction,
42  const std::string& _commonHeaderName
43  ) : ExportAlgorithm(_userInteraction, _commonHeaderName),
44  cholObjS(_userInteraction, _commonHeaderName),
45  cholSAC(_userInteraction, _commonHeaderName),
46  acSolver(userInteraction, _commonHeaderName)
47 
48 {
49  levenbergMarquardt = 0.0;
50 
51  dimPacH = 0;
52  dimPocH = 0;
53 }
54 
55 returnValue ExportNLPSolver::setIntegratorExport( IntegratorExportPtr const _integrator
56  )
57 {
58  integrator = _integrator;
59  return SUCCESSFUL_RETURN;
60 }
61 
62 returnValue ExportNLPSolver::setLevenbergMarquardt( double _levenbergMarquardt
63  )
64 {
65  if ( _levenbergMarquardt < 0.0 )
66  {
67  ACADOWARNINGTEXT(RET_INVALID_ARGUMENTS, "Levenberg-Marquardt regularization factor must be positive!");
68  levenbergMarquardt = 0.0;
69  }
70  else
71  {
72  levenbergMarquardt = _levenbergMarquardt;
73  }
74 
75  return SUCCESSFUL_RETURN;
76 }
77 
78 bool ExportNLPSolver::performsSingleShooting( ) const
79 {
80  int discretizationType;
81  get(DISCRETIZATION_TYPE, discretizationType);
82 
83  if ( discretizationType == SINGLE_SHOOTING )
84  return true;
85 
86  return false;
87 }
88 
89 returnValue ExportNLPSolver::getDataDeclarations( ExportStatementBlock& declarations,
90  ExportStruct dataStruct
91  ) const
92 {
93  declarations.addDeclaration(state, dataStruct);
94  declarations.addDeclaration(x, dataStruct);
95  declarations.addDeclaration(z, dataStruct);
96  declarations.addDeclaration(u, dataStruct);
97  declarations.addDeclaration(od, dataStruct);
98  declarations.addDeclaration(d, dataStruct);
99 
100  declarations.addDeclaration(y, dataStruct);
101  declarations.addDeclaration(yN, dataStruct);
102  declarations.addDeclaration(Dy, dataStruct);
103  declarations.addDeclaration(DyN, dataStruct);
104 
105  declarations.addDeclaration(evGx, dataStruct);
106  declarations.addDeclaration(evGu, dataStruct);
107 
108  declarations.addDeclaration(objg, dataStruct);
109  declarations.addDeclaration(objS, dataStruct);
110  declarations.addDeclaration(objSEndTerm, dataStruct);
111  declarations.addDeclaration(objSlx, dataStruct);
112  declarations.addDeclaration(objSlu, dataStruct);
113 
114  declarations.addDeclaration(objAuxVar, dataStruct);
115  declarations.addDeclaration(objValueIn, dataStruct);
116  declarations.addDeclaration(objValueOut, dataStruct);
117 
118  declarations.addDeclaration(Q1, dataStruct);
119  declarations.addDeclaration(Q2, dataStruct);
120 
121  declarations.addDeclaration(R1, dataStruct);
122  declarations.addDeclaration(R2, dataStruct);
123 
124  declarations.addDeclaration(S1, dataStruct);
125 
126  declarations.addDeclaration(QN1, dataStruct);
127  declarations.addDeclaration(QN2, dataStruct);
128 
129  declarations.addDeclaration(SAC, dataStruct);
130  declarations.addDeclaration(xAC, dataStruct);
131  declarations.addDeclaration(DxAC, dataStruct);
132 
133  declarations.addDeclaration(conAuxVar, dataStruct);
134  declarations.addDeclaration(conValueIn, dataStruct);
135  declarations.addDeclaration(conValueOut, dataStruct);
136 
137  declarations.addDeclaration(pacEvH, dataStruct);
138  declarations.addDeclaration(pacEvHx, dataStruct);
139  declarations.addDeclaration(pacEvHu, dataStruct);
140  declarations.addDeclaration(pacEvDDH, dataStruct);
141  declarations.addDeclaration(pacEvHxd, dataStruct);
142 
143  declarations.addDeclaration(pocEvH, dataStruct);
144  declarations.addDeclaration(pocEvHx, dataStruct);
145  declarations.addDeclaration(pocEvHu, dataStruct);
146  declarations.addDeclaration(pocEvHxd, dataStruct);
147 
148  // Arrival cost stuff
149  declarations.addDeclaration(acA, dataStruct);
150  declarations.addDeclaration(acb, dataStruct);
151  declarations.addDeclaration(acP, dataStruct);
152  declarations.addDeclaration(acTmp, dataStruct);
153  declarations.addDeclaration(acWL, dataStruct);
154  declarations.addDeclaration(acVL, dataStruct);
155  declarations.addDeclaration(acHx, dataStruct);
156  declarations.addDeclaration(acHu, dataStruct);
157  declarations.addDeclaration(acXx, dataStruct);
158  declarations.addDeclaration(acXu, dataStruct);
159  declarations.addDeclaration(acXTilde, dataStruct);
160  declarations.addDeclaration(acHTilde, dataStruct);
161 
162  return SUCCESSFUL_RETURN;
163 }
164 
165 returnValue ExportNLPSolver::setupInitialization()
166 {
167  string moduleName;
168  get(CG_MODULE_NAME, moduleName);
169 
171  //
172  // Setup the main initialization function.
173  //
175 
176  ExportVariable retInit("ret", 1, 1, INT, ACADO_LOCAL, true);
177  retInit.setDoc("=0: OK, otherwise an error code of a QP solver.");
178  initialize.setup( "initializeSolver" );
179  initialize.doc( "Solver initialization. Must be called once before any other function call." );
180  initialize.setReturnValue(retInit);
181 
182  initialize.addComment( "This is a function which must be called once before any other function call!" );
183  initialize.addLinebreak( 2 );
184 
185  initialize << (retInit == 0);
186  initialize.addLinebreak();
187  initialize << "memset(&" << moduleName << "Workspace, 0, sizeof( " << moduleName << "Workspace ));" << "\n";
188 // initialize << "memset(&" << moduleName << "Variables, 0, sizeof( " << moduleName << "Variables ));" << "\n";
189 
190  return SUCCESSFUL_RETURN;
191 }
192 
193 returnValue ExportNLPSolver::setupSimulation( void )
194 {
195  // \todo Implement free parameters and support for DAEs
196 
197  //
198  // By default, here will be defined model simulation suitable for sparse QP solver.
199  // Condensing based QP solvers should redefine/extend model simulation
200  //
201 
202  string moduleName;
203  get(CG_MODULE_NAME, moduleName);
204 
205  int hessianApproximation;
206  get( HESSIAN_APPROXIMATION, hessianApproximation );
207  int sensitivityProp;
208  get( DYNAMIC_SENSITIVITY, sensitivityProp );
209 
210  modelSimulation.setup( "modelSimulation" );
211  ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
212  modelSimulation.setReturnValue(retSim, false);
213  modelSimulation.addStatement(retSim == 0);
214  ExportIndex run;
215  modelSimulation.acquire( run );
216  ExportForLoop loop(run, 0, getN());
217 
218  int useOMP;
219  get(CG_USE_OPENMP, useOMP);
220 
221  x.setup("x", (getN() + 1), getNX(), REAL, ACADO_VARIABLES);
222  x.setDoc( string("Matrix containing ") + toString(getN() + 1) + " differential variable vectors." );
223  z.setup("z", getN(), getNXA(), REAL, ACADO_VARIABLES);
224  z.setDoc( string("Matrix containing ") + toString( N ) + " algebraic variable vectors." );
225  u.setup("u", getN(), getNU(), REAL, ACADO_VARIABLES);
226  u.setDoc( string("Matrix containing ") + toString( N ) + " control variable vectors." );
227  od.setup("od", getN() + 1, getNOD(), REAL, ACADO_VARIABLES);
228  od.setDoc( string("Matrix containing ") + toString(getN() + 1) + " online data vectors." );
229 
230  if (performsSingleShooting() == false)
231  {
232  d.setup("d", getN() * getNX(), 1, REAL, ACADO_WORKSPACE);
233  }
234 
235  int gradientUp;
236  get( LIFTED_GRADIENT_UPDATE, gradientUp );
237  bool gradientUpdate = (bool) gradientUp;
238 
239  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
240  bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate));
241 
242  uint symH = 0;
243  if( (ExportSensitivityType) sensitivityProp == SYMMETRIC || (ExportSensitivityType) sensitivityProp == SYMMETRIC_FB || (secondOrder && (ExportSensitivityType) sensitivityProp == BACKWARD) ) symH = (NX+NU)*(NX+NU+1)/2;
244  else if( (ExportSensitivityType) sensitivityProp == FORWARD_OVER_BACKWARD && gradientUpdate ) symH = (NX+NU)*(NX+NU); // TODO: this is a quick fix for the dimensions in case of FOB lifted collocation integrators
245  else if( (ExportSensitivityType) sensitivityProp == FORWARD_OVER_BACKWARD ) symH = NX*(NX+NU)+NU*NU;
246 
247  evGx.setup("evGx", N * NX, NX, REAL, ACADO_WORKSPACE);
248  evGu.setup("evGu", N * NX, NU, REAL, ACADO_WORKSPACE);
249 
250  if( secondOrder || adjoint ) mu.setup("mu", N, NX, REAL, ACADO_VARIABLES);
251 
252  ExportStruct dataStructWspace;
253  dataStructWspace = (useOMP && performsSingleShooting() == false) ? ACADO_LOCAL : ACADO_WORKSPACE;
254  state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNU() + getNOD(), REAL, dataStructWspace);
255  if( secondOrder && !gradientUpdate ) {
256  state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNX() + symH + getNU() + getNOD(), REAL, dataStructWspace);
257  }
258  else if( secondOrder && gradientUpdate ) {
259  state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNX() + symH + getNX() + getNU() + getNU() + getNOD(), REAL, dataStructWspace);
260  }
261  else if( adjoint ) {
262  state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNX() + getNX() + getNU() + getNU() + getNOD(), REAL, dataStructWspace);
263  }
264 
265  unsigned indexZ = NX + NXA;
266  if( secondOrder || adjoint ) indexZ = indexZ + NX; // because of the first order adjoint direction
267  unsigned indexGxx = indexZ + NX * NX;
268  unsigned indexGzx = indexGxx + NXA * NX;
269  unsigned indexGxu = indexGzx + NX * NU;
270  unsigned indexGzu = indexGxu + NXA * NU;
271  unsigned indexH = indexGzu;
272  if( secondOrder ) indexH = indexGzu + symH; // because of the second order derivatives
273  unsigned indexG = indexH;
274  if( (secondOrder && gradientUpdate) || adjoint ) indexG = indexH + NX+NU;
275  unsigned indexU = indexG + NU;
276  unsigned indexOD = indexU + NOD;
277 
279  //
280  // Code for model simulation
281  //
283  if (performsSingleShooting() == true)
284  {
285  modelSimulation.addStatement( state.getCols(0, NX) == x.getRow( 0 ) );
286  modelSimulation.addStatement( state.getCols(NX, NX + NXA) == z.getRow( 0 ) );
287  modelSimulation.addStatement( state.getCols(indexG, indexU) == u.getRow( 0 ) );
288  modelSimulation.addStatement( state.getCols(indexU, indexOD) == od.getRow( 0 ) );
289  modelSimulation.addLinebreak( );
290  }
291 
292  if ( useOMP )
293  {
294 
295  modelSimulation
296  << "#pragma omp parallel for private(" << run.getName() << ", " << state.getFullName()
297  << ") shared(" << evGx.getDataStructString() << ", "
298  << x.getDataStructString() << ")\n";
299  }
300 
301  if (performsSingleShooting() == false)
302  {
303  loop.addStatement( state.getCols(0, NX) == x.getRow( run ) );
304  loop.addStatement( state.getCols(NX, NX + NXA) == z.getRow( run ) );
305  }
306  loop.addLinebreak( );
307 
308  // Fill in the input vector
309  if( secondOrder || adjoint ) {
310  loop.addStatement( state.getCols(NX+NXA, 2*NX+NXA) == mu.getRow( run ) );
311  }
312  loop.addStatement( state.getCols(indexG, indexU) == u.getRow( run ) );
313  loop.addStatement( state.getCols(indexU, indexOD) == od.getRow( run ) );
314  loop.addLinebreak( );
315 
316  // Integrate the model
317  // TODO make that function calls can accept constant defined scalars
318  int intMode;
319  get( IMPLICIT_INTEGRATOR_MODE, intMode );
320  if ( integrator->equidistantControlGrid() )
321  {
322  if( (ImplicitIntegratorMode)intMode == LIFTED || (ImplicitIntegratorMode)intMode == LIFTED_FEEDBACK ) {
323  loop << retSim.getFullName() << " = "
324  << moduleName << "_integrate" << "(" << state.getFullName()
325  << ", " << run.getFullName() << ");\n";
326  }
327  else if (performsSingleShooting() == false)
328  loop << retSim.getFullName() << " = "
329  << moduleName << "_integrate" << "(" << state.getFullName() << ", 1);\n";
330  else
331  loop << retSim.getFullName() << " = " << moduleName << "_integrate"
332  << "(" << state.getFullName() << ", "
333  << run.getFullName() << " == 0"
334  << ");\n";
335  }
336  else
337  {
338  if (performsSingleShooting() == false)
339  loop << retSim.getFullName() << " = "
340  << moduleName << "_integrate"
341  << "(" << state.getFullName() << ", 1, " << run.getFullName() << ");\n";
342  else
343  loop << retSim.getFullName() << " = "
344  << moduleName << "_integrate"
345  << "(" << state.getFullName() << ", "
346  << run.getFullName() << " == 0"
347  << ", " << run.getFullName() << ");\n";
348  }
349  loop.addLinebreak( );
350 // if (useOMP == 0)
351 // {
352 // // TODO In case we use OpenMP more sophisticated solution has to be found.
353 // loop << "if (" << retSim.getFullName() << " != 0) return " << retSim.getFullName() << ";";
354 // loop.addLinebreak( );
355 // }
356 
357  if ( performsSingleShooting() == true )
358  {
359  // Single shooting case: prepare for the next iteration
360  loop.addStatement( x.getRow(run + 1) == state.getCols(0, NX) );
361  loop.addLinebreak( );
362  }
363  else
364  {
365  // Multiple shootin', compute residuum
366  loop.addStatement( d.getTranspose().getCols(run * NX, (run + 1) * NX) == state.getCols( 0,getNX() ) - x.getRow( run+1 ) );
367  loop.addLinebreak( );
368  }
369 
370  loop.addStatement( z.getRow( run ) == state.getCols(NX, NX + NXA) );
371 
372  // Stack sensitivities
373  // \todo Upgrade this code later to stack Z sens
374  loop.addStatement(
375  evGx.makeRowVector().getCols(run * NX * NX, (run + 1) * NX * NX) == state.getCols(indexZ, indexGxx)
376  );
377  loop.addLinebreak();
378 
379  loop.addStatement(
380  evGu.makeRowVector().getCols(run * NX * NU, (run + 1) * NX * NU) == state.getCols(indexGzx, indexGxu)
381  );
382 
383  // TODO: write this in exported loops (RIEN)
384  if( secondOrder && ((ExportSensitivityType) sensitivityProp == SYMMETRIC || (ExportSensitivityType) sensitivityProp == SYMMETRIC_FB || (ExportSensitivityType) sensitivityProp == BACKWARD) ) {
385  for( uint i = 0; i < NX+NU; i++ ) {
386  for( uint j = 0; j <= i; j++ ) {
387  loop.addStatement( objS.getElement(run*(NX+NU)+i,j) == -1.0*state.getCol(indexGzu + i*(i+1)/2+j) );
388  if( i != j) {
389  loop.addStatement( objS.getElement(run*(NX+NU)+j,i) == objS.getElement(run*(NX+NU)+i,j) );
390  }
391  }
392  }
393  }
394  else if( secondOrder && (ExportSensitivityType) sensitivityProp == FORWARD_OVER_BACKWARD && gradientUpdate ) {
395  for( uint i = 0; i < NX+NU; i++ ) {
396  for( uint j = 0; j < NX+NU; j++ ) {
397  loop.addStatement( objS.getElement(run*(NX+NU)+i,j) == -1.0*state.getCol(indexGzu + i*(NX+NU)+j) );
398  }
399  }
400  }
401  else if( secondOrder && (ExportSensitivityType) sensitivityProp == FORWARD_OVER_BACKWARD ) {
402  for( uint i = 0; i < NX; i++ ) {
403  for( uint j = 0; j < NX; j++ ) {
404  loop.addStatement( objS.getElement(run*(NX+NU)+i,j) == -1.0*state.getCol(indexGzu + i*NX+j) );
405  }
406  }
407  for( uint i = 0; i < NU; i++ ) {
408  for( uint j = 0; j < NX; j++ ) {
409  loop.addStatement( objS.getElement(run*(NX+NU)+NX+i,j) == -1.0*state.getCol(indexGzu + NX*NX+i*NX+j) );
410  loop.addStatement( objS.getElement(run*(NX+NU)+j,NX+i) == objS.getElement(run*(NX+NU)+NX+i,j) );
411  }
412  }
413  for( uint i = 0; i < NU; i++ ) {
414  for( uint j = 0; j < NU; j++ ) {
415  loop.addStatement( objS.getElement(run*(NX+NU)+NX+i,NX+j) == -1.0*state.getCol(indexGzu + NX*(NX+NU)+i*NU+j) );
416  }
417  }
418  }
419  else if( secondOrder ) return ACADOERRORTEXT(RET_INVALID_OPTION, "Only SYMMETRIC or FORWARD_OVER_BACKWARD options supported for Exact Hessian based RTI.");
420 
421  // GRADIENT UPDATE: in case of lifted collocation integrators
422  if( secondOrder && gradientUpdate ) {
423  for( uint i = 0; i < NX+NU; i++ ) {
424  loop.addStatement( objg.getRow(run*(NX+NU)+i) == -1.0*state.getCol(indexH+i) );
425  }
426  }
427  else if( adjoint ) {
428  for( uint i = 0; i < NX; i++ ) {
429  loop.addStatement( objSlx.getRow(run*NX+i) == -1.0*state.getCol(indexH+i) );
430  }
431  for( uint i = 0; i < NU; i++ ) {
432  loop.addStatement( objSlu.getRow(run*NU+i) == -1.0*state.getCol(indexH+NX+i) );
433  }
434  }
435 
436  // XXX This should be revisited at some point
437  // modelSimulation.release( run );
438 
439  modelSimulation.addStatement( loop );
440 
441  return SUCCESSFUL_RETURN;
442 }
443 
444 
445 returnValue ExportNLPSolver::setObjective(const Objective& _objective)
446 {
447  if( _objective.getNumMayerTerms() == 0 && _objective.getNumLagrangeTerms() == 0 ) {
448  return setLSQObjective( _objective );
449  }
450  else {
451  return setGeneralObjective( _objective );
452  }
453 }
454 
455 
456 returnValue ExportNLPSolver::setGeneralObjective(const Objective& _objective)
457 {
459  // ONLY ACADO AD SUPPORTED FOR NOW
461 
462  Function objF, objFEndTerm;
463  DifferentialState dummy0;
464  Control dummy1;
465  dummy0.clearStaticCounters();
466  dummy1.clearStaticCounters();
467 
468  DifferentialState vX("", NX, 1);
469  Control vU("", NU, 1);
470 
471  diagonalH = false;
472  diagonalHN = false;
473 
474  int gradientUp;
475  get( LIFTED_GRADIENT_UPDATE, gradientUp );
476  bool gradientUpdate = (bool) gradientUp;
477  int hessianApproximation;
478  get( HESSIAN_APPROXIMATION, hessianApproximation );
479  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
480  if( secondOrder ) {
481  objS.setup("EH", N * (NX + NU), NX + NU, REAL, ACADO_WORKSPACE); // EXACT HESSIAN
482  }
483  if( secondOrder && gradientUpdate ) {
484  objg.setup("Eg", N * (NX + NU), 1, REAL, ACADO_WORKSPACE); // GRADIENT CONTRIBUTION
485  }
486 
487  int qpSolution;
488  get( SPARSE_QP_SOLUTION, qpSolution );
489  if( (SparseQPsolutionMethods)qpSolution != SPARSE_SOLVER ) {
490  S1.setup("S1", NX * N, NU, REAL, ACADO_WORKSPACE);
491  Q1.setup("Q1", NX * N, NX, REAL, ACADO_WORKSPACE);
492  R1.setup("R1", NU * N, NU, REAL, ACADO_WORKSPACE);
493  QN1.setup("QN1", NX, NX, REAL, ACADO_WORKSPACE);
494  }
495 
496  objValueIn.setup("objValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
497  // -----------------
498  // Lagrange Term:
499  setNY( 0 );
500  if( _objective.getNumLagrangeTerms() ) {
501  _objective.getLagrangeTerm(0, objF);
502 
503  objS.setup("EH", N * (NX+NU), NX+NU, REAL, ACADO_WORKSPACE); // EXACT HESSIAN
504 
505  Expression expF;
506  objF.getExpression( expF );
507 
508  // FIRST ORDER DERIVATIVES
509  Expression expFx, expFu, expDF, expDDF, S, lambda, arg, dl;
510  S = eye<double>(NX+NU);
511  lambda = 1;
512  arg << vX;
513  arg << vU;
514 
515  expDDF = symmetricDerivative( expF, arg, S, lambda, &expDF, &dl );
516 
517  expFx = expDF.getCols(0, NX).transpose();
518  expFu = expDF.getCols(NX, NX+NU).transpose();
519 
520  Function Fx, Fu;
521  Fx << expFx;
522  Fu << expFu;
523 
524 // if (Fx.isConstant() == true)
525 // {
526 // EvaluationPoint epFx( Fx );
527 //
528 // DVector vFx = Fx.evaluate( epFx );
529 //
530 // objEvFx.setup("evFx", Eigen::Map<DMatrix>(vFx.data(), 1, NX), REAL, ACADO_WORKSPACE);
531 // }
532 // else
533 // {
534  objF << expFx;
535 
536  objEvFx.setup("evFx", 1, NX, REAL, ACADO_WORKSPACE);
537 // }
538 
539 // if (Fu.isConstant() == true)
540 // {
541 // EvaluationPoint epFu( Fu );
542 //
543 // DVector vFu = Fu.evaluate( epFu );
544 //
545 // objEvFu.setup("evFu", Eigen::Map<DMatrix>(vFu.data(), 1, NU), REAL, ACADO_WORKSPACE);
546 // }
547 // else
548 // {
549  objF << expFu;
550 
551  objEvFu.setup("evFu", 1, NU, REAL, ACADO_WORKSPACE);
552 // }
553 
554  // SECOND ORDER DERIVATIVES
555  Expression expFxx;
556  Expression expFxu;
557  Expression expFuu;
558 
559  expFxx = expDDF.getSubMatrix(0,NX,0,NX);
560  expFxu = expDDF.getSubMatrix(0,NX,NX,NX+NU);
561  expFuu = expDDF.getSubMatrix(NX,NX+NU,NX,NX+NU);
562 
563  Function Fxx, Fxu, Fuu;
564  Fxx << expFxx;
565  Fxu << expFxu;
566  Fuu << expFuu;
567 
568 // if (Fxx.isConstant() == true)
569 // {
570 // EvaluationPoint epFxx( Fxx );
571 //
572 // DVector vFxx = Fxx.evaluate( epFxx );
573 //
574 // objEvFxx.setup("evFxx", Eigen::Map<DMatrix>(vFxx.data(), NX, NX), REAL, ACADO_WORKSPACE);
575 // Q1 = vFxx.data();
576 // }
577 // else
578 // {
579  objF << expFxx;
580 
581  objEvFxx.setup("evFxx", NX, NX, REAL, ACADO_WORKSPACE);
582 // }
583 
584 // if (Fxu.isConstant() == true)
585 // {
586 // EvaluationPoint epFxu( Fxu );
587 //
588 // DVector vFxu = Fxu.evaluate( epFxu );
589 //
590 // objEvFxu.setup("evFxu", Eigen::Map<DMatrix>(vFxu.data(), NX, NU), REAL, ACADO_WORKSPACE);
591 // S1 = vFxu.data();
592 // }
593 // else
594 // {
595  objF << expFxu;
596 
597  objEvFxu.setup("evFxu", NX, NU, REAL, ACADO_WORKSPACE);
598 // }
599 
600 // if (Fuu.isConstant() == true)
601 // {
602 // EvaluationPoint epFuu( Fuu );
603 //
604 // DVector vFuu = Fuu.evaluate( epFuu );
605 //
606 // objEvFuu.setup("evFuu", Eigen::Map<DMatrix>(vFuu.data(), NU, NU), REAL, ACADO_WORKSPACE);
607 // R1 = vFuu.data();
608 // }
609 // else
610 // {
611  objF << expFuu;
612 
613  objEvFuu.setup("evFuu", NU, NU, REAL, ACADO_WORKSPACE);
614 // }
615 
616  // Set the separate aux variable for the evaluation of the objective.
617  objAuxVar.setup("objAuxVar", objF.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
618  evaluateStageCost.init(objF, "evaluateLagrange", NX, 0, NU);
619  evaluateStageCost.setGlobalExportVariable( objAuxVar );
620  evaluateStageCost.setPrivate( true );
621 
622  objValueOut.setup("objValueOut", 1, objF.getDim(), REAL, ACADO_WORKSPACE);
623  }
624 
625  // -----------------
626  // Mayer Term:
627  setNYN( 0 );
628  if( _objective.getNumMayerTerms() ) {
629  _objective.getMayerTerm(0, objFEndTerm);
630 
631  if (objFEndTerm.getNU() > 0)
632  return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost function must not depend on controls.");
633 
634  objSEndTerm.setup("EH_N", NX, NX, REAL, ACADO_WORKSPACE); // EXACT HESSIAN
635 
636  Expression expFEndTerm;
637  objFEndTerm.getExpression( expFEndTerm );
638 
639  // FIRST ORDER DERIVATIVES
640 
641  Expression expFEndTermX, expFEndTermXX, S, lambda, dl;
642  S = eye<double>(NX);
643  lambda = 1;
644 
645  expFEndTermXX = symmetricDerivative( expFEndTerm, vX, S, lambda, &expFEndTermX, &dl );
646 
647  Function FEndTermX;
648  FEndTermX << expFEndTermX.transpose();
649 
650 // if (FEndTermX.isConstant() == true)
651 // {
652 // EvaluationPoint epFEndTermX( FEndTermX );
653 //
654 // DVector vFx = FEndTermX.evaluate( epFEndTermX );
655 //
656 // objEvFxEnd.setup("evFxEnd", Eigen::Map<DMatrix>(vFx.data(), 1, NX), REAL, ACADO_WORKSPACE);
657 // }
658 // else
659 // {
660  objFEndTerm << expFEndTermX;
661 
662  objEvFxEnd.setup("evFxEnd", 1, NX, REAL, ACADO_WORKSPACE);
663 // }
664 
665  // SECOND ORDER DERIVATIVES
666 
667  Function FEndTermXX;
668  FEndTermXX << expFEndTermXX;
669 
670 // if (FEndTermXX.isConstant() == true)
671 // {
672 // EvaluationPoint epFEndTermXX( FEndTermXX );
673 //
674 // DVector vFxx = FEndTermXX.evaluate( epFEndTermXX );
675 //
676 // objEvFxxEnd.setup("evFxxEnd", Eigen::Map<DMatrix>(vFxx.data(), NX, NX), REAL, ACADO_WORKSPACE);
677 // QN1 = vFxx.data();
678 // }
679 // else
680 // {
681  objFEndTerm << expFEndTermXX;
682 
683  objEvFxxEnd.setup("evFxxEnd", NX, NX, REAL, ACADO_WORKSPACE);
684 // }
685 
686  unsigned objFEndTermSize = objFEndTerm.getGlobalExportVariableSize();
687  if ( objFEndTermSize > objAuxVar.getDim() )
688  {
689  objAuxVar.setup("objAuxVar", objFEndTermSize, 1, REAL, ACADO_WORKSPACE);
690  }
691 
692  evaluateTerminalCost.init(objFEndTerm, "evaluateMayer", NX, 0, 0);
693  evaluateTerminalCost.setGlobalExportVariable( objAuxVar );
694  evaluateTerminalCost.setPrivate( true );
695 
696  if (objFEndTerm.getDim() > objF.getDim())
697  {
698  objValueOut.setup("objValueOut", 1, objFEndTerm.getDim(), REAL, ACADO_WORKSPACE);
699  }
700 
701 
702 // setupObjectiveLinearTerms( _objective );
703  objSlx = zeros<double>(NX, 1);
704  objSlu = zeros<double>(NU, 1);
705  objSlx.setDoc("Linear term weighting vector for states.");
706  objSlu.setDoc("Linear term weighting vector for controls.");
707 //
708 // setupResidualVariables();
709  }
710 
711  return SUCCESSFUL_RETURN;
712 }
713 
714 
715 returnValue ExportNLPSolver::setLSQObjective(const Objective& _objective)
716 {
717  int variableObjS;
718  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
719  int useArrivalCost;
720  get(CG_USE_ARRIVAL_COST, useArrivalCost);
721 
722  int hessianApproximation;
723  get( HESSIAN_APPROXIMATION, hessianApproximation );
724  if((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN) {
725  return ACADOERRORTEXT(RET_NOT_YET_IMPLEMENTED, "The Exact Hessian based RTI solver is not yet implemented for least squares objectives (use lagrange and mayer terms instead)!");
726  }
727 
729  //
730  // Check first if we are dealing with external functions
731  //
733 
734  LsqExternElements lsqExternElements;
735  _objective.getLSQTerms( lsqExternElements );
736 
737  LsqExternElements lsqExternEndTermElements;
738  _objective.getLSQEndTerms( lsqExternEndTermElements );
739 
740  if (lsqExternElements.size() > 0 || lsqExternEndTermElements.size() > 0)
741  {
742  if (lsqExternElements.size() != 1 || lsqExternEndTermElements.size() != 1)
743  return ACADOERROR( RET_INVALID_ARGUMENTS );
744  if (lsqExternElements[ 0 ].W.isSquare() == false || lsqExternElements[ 0 ].W.isSquare() == false)
745  return ACADOERROR( RET_INVALID_ARGUMENTS );
746 
747  setNY( lsqExternElements[ 0 ].W.getNumRows() );
748  setNYN( lsqExternEndTermElements[ 0 ].W.getNumRows() );
749 
750  if (variableObjS == YES)
751  {
752  objS.setup("W", N * NY, NY, REAL, ACADO_VARIABLES);
753  }
754  else if (lsqExternElements[ 0 ].givenW == false)
755  {
756  objS.setup("W", lsqExternElements[ 0 ].W, REAL, ACADO_VARIABLES, false, "", false);
757  }
758  else
759  {
760  objS.setup("W", lsqExternElements[ 0 ].W, REAL, ACADO_VARIABLES);
761  }
762 
763  objSEndTerm.setup("WN", lsqExternEndTermElements[ 0 ].W,
764  REAL, ACADO_VARIABLES, false, "", lsqExternEndTermElements[ 0 ].givenW);
765 
766  int forceDiagHessian;
767  get(CG_FORCE_DIAGONAL_HESSIAN, forceDiagHessian);
768 
769  diagonalH = diagonalHN = forceDiagHessian ? true : false;
770 
771  objEvFx.setup("evFx", NY, NX, REAL, ACADO_WORKSPACE);
772  objEvFu.setup("evFu", NY, NU, REAL, ACADO_WORKSPACE);
773  objEvFxEnd.setup("evFx", NYN, NX, REAL, ACADO_WORKSPACE);
774 
775  Q1.setup("Q1", NX * N, NX, REAL, ACADO_WORKSPACE);
776  Q2.setup("Q2", NX * N, NY, REAL, ACADO_WORKSPACE);
777 
778  R1.setup("R1", NU * N, NU, REAL, ACADO_WORKSPACE);
779  R2.setup("R2", NU * N, NY, REAL, ACADO_WORKSPACE);
780 
781  S1.setup("S1", NX * N, NU, REAL, ACADO_WORKSPACE);
782 
783  QN1.setup("QN1", NX, NX, REAL, ACADO_WORKSPACE);
784  QN2.setup("QN2", NX, NYN, REAL, ACADO_WORKSPACE);
785 
786  objValueIn.setup("objValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
787  objValueOut.setup("objValueOut", 1,
788  NY < NYN ? NYN * (1 + NX + NU): NY * (1 + NX + NU), REAL, ACADO_WORKSPACE);
789 
790  evaluateStageCost = ExportAcadoFunction(lsqExternElements[ 0 ].h);
791  evaluateTerminalCost = ExportAcadoFunction(lsqExternEndTermElements[ 0 ].h);
792 
793  setupObjectiveLinearTerms( _objective );
794 
795  setupResidualVariables();
796 
797  return SUCCESSFUL_RETURN;
798  }
799 
801  //
802  // ... or we use ACADO AD
803  //
805 
806  Function objF, objFEndTerm;
807 
808  LsqElements lsqElements;
809  LsqElements lsqEndTermElements;
810 
811  _objective.getLSQTerms( lsqElements );
812  _objective.getLSQEndTerms( lsqEndTermElements );
813 
814  if( lsqElements.size() == 0 )
815  return ACADOERRORTEXT(RET_INITIALIZE_FIRST, "Objective function is not initialized.");
816  if (lsqElements.size() > 1 || lsqEndTermElements.size() > 1)
817  return ACADOERRORTEXT(RET_INITIALIZE_FIRST,
818  "Current implementation of code generation module\n"
819  "supports only one LSQ term definition per one OCP." );
820  if (lsqElements[0].h.getDim() == 0)
821  return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "Objective function must include cost elements.");
822 
823  if (lsqElements[ 0 ].W.isSquare() == false)
824  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Weighting matrices must be square.");
825  if (lsqElements[ 0 ].W.getNumRows() != (unsigned)lsqElements[ 0 ].h.getDim())
826  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Wrong dimensions of the weighting matrix.");
827 
828  if ( lsqEndTermElements.size() == 0 )
829  return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost must be defined");
830  if (lsqEndTermElements[0].h.getDim() == 0)
831  return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost must include cost elements.");
832 
833  if (lsqEndTermElements[ 0 ].W.isSquare() == false)
834  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Weighting matrices must be square.");
835  if (lsqEndTermElements[ 0 ].W.getNumRows() != (unsigned)lsqEndTermElements[ 0 ].h.getDim())
836  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Wrong dimensions of the weighting matrix.");
837 
838  objF = lsqElements[ 0 ].h;
839  setNY( objF.getDim() );
840 
841  DifferentialState dummy0;
842  Control dummy1;
843  dummy0.clearStaticCounters();
844  dummy1.clearStaticCounters();
845 
846  DifferentialState vX("", NX, 1);
847  Control vU("", NU, 1);
848 
850  //
851  // Setup the Lagrange LSQ terms
852  //
854 
855  // Setup the S matrix
856  if (lsqElements[ 0 ].givenW == false)
857  {
858  if ( variableObjS == YES )
859  {
860  // TODO Sparsity of this guy should be done in an efficient way one day,
861  // most probably after isolating objective handling in a separate
862  // class.
863  objS.setup("W", N * NY, NY, REAL, ACADO_VARIABLES);
864  }
865  else
866  {
867  objS.setup("W", lsqElements[ 0 ].W, REAL, ACADO_VARIABLES, false, "", false);
868  }
869  }
870  else
871  {
872  if (lsqElements[ 0 ].W.isPositiveSemiDefinite() == false)
873  return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
874 
875  objS.setup("W", lsqElements[ 0 ].W, REAL, ACADO_VARIABLES);
876  }
877 
878  Expression expF;
879  objF.getExpression( expF );
880 
881  Expression expFx;
882  Expression expFu;
883 
884  expFx = forwardDerivative(expF, vX);
885  expFu = forwardDerivative(expF, vU);
886 
887  Function Fx, Fu;
888  Fx << expFx;
889  Fu << expFu;
890 
891  if (Fx.isConstant() == true)
892  {
893  EvaluationPoint epFx( Fx );
894 
895  DVector vFx = Fx.evaluate( epFx );
896 
897  objEvFx.setup("evFx", Eigen::Map<DMatrix>(vFx.data(), NY, NX), REAL, ACADO_WORKSPACE);
898  }
899  else
900  {
901  objF << expFx;
902 
903  objEvFx.setup("evFx", NY, NX, REAL, ACADO_WORKSPACE);
904  }
905 
906 // objF << expFx;
907 // evFx.setup("evFx", NY, NX, REAL, ACADO_WORKSPACE);
908 
909  if (Fu.isConstant() == true)
910  {
911  EvaluationPoint epFu( Fu );
912 
913  DVector vFu = Fu.evaluate( epFu );
914 
915  objEvFu.setup("evFu", Eigen::Map<DMatrix>(vFu.data(), NY, NU), REAL, ACADO_WORKSPACE);
916  }
917  else
918  {
919  objF << expFu;
920 
921  objEvFu.setup("evFu", NY, NU, REAL, ACADO_WORKSPACE);
922  }
923 
924 // objF << expFu;
925 // evFu.setup("evFu", NY, NU, REAL, ACADO_WORKSPACE);
926 
927  //
928  // Initialize the export of the LSQ function which evaluates the
929  // objective and (possibly) its derivatives.
930  //
931 
932  // Set the separate aux variable for the evaluation of the objective.
933 
934  objAuxVar.setup("objAuxVar", objF.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
935  evaluateStageCost.init(objF, "evaluateLSQ", NX, 0, NU);
936  evaluateStageCost.setGlobalExportVariable( objAuxVar );
937  evaluateStageCost.setPrivate( true );
938 
939  objValueIn.setup("objValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
940  objValueOut.setup("objValueOut", 1, objF.getDim(), REAL, ACADO_WORKSPACE);
941 
942  //
943  // Optional pre-computing of Q1, Q2, R1, R2 matrices
944  //
945 
946  if (objS.isGiven() == true && objEvFx.isGiven() == true)
947  {
948  if (useArrivalCost)
949  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
950 
951  // Precompute Q1 and Q2;
952 
953  DMatrix m1(NX,NX), m2(NX, NY);
954 
955  m2 = objEvFx.getGivenMatrix().transpose() * objS.getGivenMatrix();
956  m1 = m2 * objEvFx.getGivenMatrix();
957 
958  Q1 = m1;
959  if ( m1 == m2 )
960  {
961  Q2 = Q1;
962  }
963  else
964  {
965  Q2 = m2;
966  }
967  }
968  else if (Fx.isOneOrZero() == NE_ZERO)
969  {
970  if (useArrivalCost)
971  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
972 
973  Q1 = zeros<double>(NX, NX);
974  Q2 = zeros<double>(NX, NY);
975  }
976  else
977  {
978  Q1.setup("Q1", NX * N, NX, REAL, ACADO_WORKSPACE);
979  Q2.setup("Q2", NX * N, NY, REAL, ACADO_WORKSPACE);
980  }
981 
982  if (objS.isGiven() == true && objEvFu.isGiven() == true)
983  {
984  // Precompute R1 and R2
985 
986  DMatrix m2 = objEvFu.getGivenMatrix().transpose() * objS.getGivenMatrix();
987  DMatrix m1 = m2 * objEvFu.getGivenMatrix();
988 
989  R1 = m1;
990  if (m1 == m2)
991  {
992  R2 = R1;
993  }
994  else
995  {
996  R2 = m2;
997  }
998  }
999  else if (Fu.isOneOrZero() == NE_ZERO)
1000  {
1001  R1 = zeros<double>(NU, NU);
1002  R2 = zeros<double>(NU, NY);
1003  }
1004  else
1005  {
1006  R1.setup("R1", NU * N, NU, REAL, ACADO_WORKSPACE);
1007  R2.setup("R2", NU * N, NY, REAL, ACADO_WORKSPACE);
1008  }
1009 
1010  // Check for sparsity of the stage Hessian
1011  // Dependency pattern of Fx
1012  DMatrix depFx = objEvFx.isGiven() == true ? objEvFx.getGivenMatrix() : expFx.getSparsityPattern();
1013  // Dependency pattern of Fu
1014  DMatrix depFu = objEvFu.isGiven() == true ? objEvFu.getGivenMatrix() : expFu.getSparsityPattern();
1015 
1016  DMatrix depQ = depFx.transpose() * lsqElements[ 0 ].W * depFx;
1017  DMatrix depR = depFu.transpose() * lsqElements[ 0 ].W * depFu;
1018  DMatrix depS = depFx.transpose() * lsqElements[ 0 ].W * depFu;
1019 
1020  if (depQ.isDiagonal() && depR.isDiagonal() && depS.isZero())
1021  diagonalH = true;
1022  else
1023  diagonalH = false;
1024 
1025  if (depS.isZero() == true)
1026  {
1027  S1 = zeros<double>(NX, NU);
1028  }
1029  else if (objS.isGiven() == true && objEvFu.isGiven() == true && objEvFx.isGiven() == true)
1030  {
1031  S1 = objEvFx.getGivenMatrix().transpose() * objS.getGivenMatrix() * objEvFu.getGivenMatrix();
1032  }
1033  else
1034  {
1035  S1.setup("S1", NX * N, NU, REAL, ACADO_WORKSPACE);
1036  }
1037 
1039  //
1040  // Setup the quadratic Mayer term stuff
1041  //
1043 
1044  objFEndTerm = lsqEndTermElements[ 0 ].h;
1045 
1046  if (objFEndTerm.getNU() > 0)
1047  return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "The terminal cost function must not depend on controls.");
1048 
1049  setNYN( objFEndTerm.getDim() );
1050 
1051  // Setup the SN matrix
1052  if (lsqEndTermElements[ 0 ].givenW == false)
1053  {
1054  objSEndTerm.setup("WN", lsqEndTermElements[ 0 ].W, REAL, ACADO_VARIABLES, false, "", false);
1055  }
1056  else
1057  {
1058  if (lsqEndTermElements[ 0 ].W.isPositiveDefinite() == false)
1059  return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
1060 
1061  objSEndTerm.setup("WN", lsqEndTermElements[ 0 ].W, REAL, ACADO_VARIABLES);
1062  }
1063 
1064  Expression expFEndTerm;
1065  objFEndTerm.getExpression( expFEndTerm );
1066 
1067  Expression expFEndTermX;
1068 
1069  expFEndTermX = forwardDerivative(expFEndTerm, vX);
1070 
1071  Function FEndTermX;
1072  FEndTermX << expFEndTermX;
1073 
1074  if (FEndTermX.isConstant() == true)
1075  {
1076  EvaluationPoint epFEndTermX( FEndTermX );
1077 
1078  DVector vFx = FEndTermX.evaluate( epFEndTermX );
1079 
1080  objEvFxEnd.setup("evFxEnd", Eigen::Map<DMatrix>(vFx.data(), NYN, NX), REAL, ACADO_WORKSPACE);
1081  }
1082  else
1083  {
1084  objFEndTerm << expFEndTermX;
1085 
1086  objEvFxEnd.setup("evFxEnd", NYN, NX, REAL, ACADO_WORKSPACE);
1087  }
1088 
1089 // objFEndTerm << expFEndTermX;
1090 // objEvFxEnd.setup("evFxEnd", NYN, NX, REAL, ACADO_WORKSPACE);
1091 
1092  unsigned objFEndTermSize = objFEndTerm.getGlobalExportVariableSize();
1093  if ( objFEndTermSize > objAuxVar.getDim() )
1094  {
1095  objAuxVar.setup(objAuxVar.getName(), objFEndTermSize, 1, REAL, objAuxVar.getDataStruct());
1096  }
1097 
1098  evaluateTerminalCost.init(objFEndTerm, "evaluateLSQEndTerm", NX, 0, 0);
1099  evaluateTerminalCost.setGlobalExportVariable( objAuxVar );
1100  evaluateTerminalCost.setPrivate( true );
1101 
1102  if (objFEndTerm.getDim() > objF.getDim())
1103  {
1104  objValueOut.setup("objValueOut", 1, objFEndTerm.getDim(), REAL, ACADO_WORKSPACE);
1105  }
1106 
1107  if (objSEndTerm.isGiven() == true && objEvFxEnd.isGiven() == true)
1108  {
1109  // Precompute
1110 
1111  DMatrix m2, m1;
1112 
1113  m2 = objEvFxEnd.getTranspose().getGivenMatrix() * objSEndTerm.getGivenMatrix();
1114  m1 = m2 * objEvFxEnd.getGivenMatrix();
1115 
1116  QN1 = m1;
1117  if (m1 == m2)
1118  QN2 = m1;
1119  else
1120  QN2 = m2;
1121  }
1122  else if (FEndTermX.isOneOrZero() == NE_ZERO)
1123  {
1124  QN1 = zeros<double>(NX, NX);
1125  QN2 = zeros<double>(NX, NYN);
1126  }
1127  else
1128  {
1129  QN1.setup("QN1", NX, NX, REAL, ACADO_WORKSPACE);
1130  QN2.setup("QN2", NX, NYN, REAL, ACADO_WORKSPACE);
1131  }
1132 
1133  DMatrix depFxEnd = objEvFxEnd.isGiven() == true ? objEvFxEnd.getGivenMatrix() : expFEndTermX.getSparsityPattern();
1134  DMatrix depQN = depFxEnd.transpose() * lsqEndTermElements[ 0 ].W * depFxEnd;
1135  diagonalHN = depQN.isDiagonal() ? true : false;
1136 
1137  LOG( LVL_DEBUG ) << "diag H_{0: N-1}: " << diagonalH << ", diag H_N: " << diagonalHN << endl;
1138 
1139  // Both are given or none is given; otherwise give an error.
1140  if (getNYN() && (objS.isGiven() ^ objSEndTerm.isGiven()))
1141  return ACADOERRORTEXT(RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT, "All weighting matrices have to be defined (or all undefined)");
1142 
1143  setupObjectiveLinearTerms( _objective );
1144 
1145  setupResidualVariables();
1146 
1147  return SUCCESSFUL_RETURN;
1148 }
1149 
1150 returnValue ExportNLPSolver::setupObjectiveLinearTerms(const Objective& _objective)
1151 {
1152  int variableObjS;
1153  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
1154 
1156  //
1157  // Setup the linear terms
1158  //
1160 
1161  LsqLinearElements lsqLinearElements;
1162  _objective.getLSQLinearTerms( lsqLinearElements );
1163 
1164  if (lsqLinearElements.size() > 0)
1165  {
1166  ASSERT_RETURN(lsqLinearElements.size() == 1);
1167 
1168  if (variableObjS == YES)
1169  {
1170  objSlx.setup("Wlx", (N + 1) * NX, 1, REAL, ACADO_VARIABLES);
1171  objSlu.setup("Wlu", N * NU, 1, REAL, ACADO_VARIABLES);
1172  }
1173  else
1174  {
1175  ASSERT_RETURN( lsqLinearElements[ 0 ].Wlx.getDim() == NX );
1176  ASSERT_RETURN( lsqLinearElements[ 0 ].Wlu.getDim() == NU );
1177 
1178  if (lsqLinearElements[ 0 ].givenW == false)
1179  {
1180  objSlx.setup("Wlx", lsqLinearElements[ 0 ].Wlx, REAL, ACADO_VARIABLES, false, "", false);
1181  objSlu.setup("Wlu", lsqLinearElements[ 0 ].Wlu, REAL, ACADO_VARIABLES, false, "", false);
1182  }
1183  else
1184  {
1185  objSlx.setup("Wlx", lsqLinearElements[ 0 ].Wlx, REAL, ACADO_VARIABLES);
1186  objSlu.setup("Wlu", lsqLinearElements[ 0 ].Wlu, REAL, ACADO_VARIABLES);
1187  }
1188  }
1189  }
1190  else
1191  {
1192  objSlx = zeros<double>(NX, 1);
1193  objSlu = zeros<double>(NU, 1);
1194  }
1195 
1196  int gradientUp;
1197  get( LIFTED_GRADIENT_UPDATE, gradientUp );
1198  bool gradientUpdate = (bool) gradientUp;
1199 
1200  int sensitivityProp;
1201  get( DYNAMIC_SENSITIVITY, sensitivityProp );
1202  bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate));
1203  if( adjoint ) {
1204  objSlx.setup("Wlx", (N+1) * NX, 1, REAL, ACADO_WORKSPACE);
1205  objSlu.setup("Wlu", N * NU, 1, REAL, ACADO_WORKSPACE);
1206  }
1207 
1208  objSlx.setDoc("Linear term weighting vector for states.");
1209  objSlu.setDoc("Linear term weighting vector for controls.");
1210 
1211  return SUCCESSFUL_RETURN;
1212 }
1213 
1214 returnValue ExportNLPSolver::setupResidualVariables()
1215 {
1216  y.setup("y", getN() * getNY(), 1, REAL, ACADO_VARIABLES);
1217  y.setDoc( string("Matrix containing ") + toString( N ) +
1218  " reference/measurement vectors of size " + toString( NY ) + " for first " + toString( N ) + " nodes." );
1219  yN.setup("yN", getNYN(), 1, REAL, ACADO_VARIABLES);
1220  yN.setDoc( string("Reference/measurement vector for the ") + toString(N + 1) + ". node." );
1221  Dy.setup("Dy", getN() * getNY(), 1, REAL,ACADO_WORKSPACE);
1222  DyN.setup("DyN", getNYN(), 1, REAL, ACADO_WORKSPACE);
1223 
1224  return SUCCESSFUL_RETURN;
1225 }
1226 
1227 returnValue ExportNLPSolver::setConstraints(const OCP& _ocp)
1228 {
1230  //
1231  // Extract box constraints
1232  //
1234 
1235  Grid grid;
1236  Constraint constraints;
1237 
1238  _ocp.getGrid( grid );
1239  _ocp.getConstraint( constraints );
1240 
1241  VariablesGrid ugrid(NU, grid);
1242  VariablesGrid xgrid(NX, grid);
1243 
1244  OCPiterate tmp;
1245  tmp.init(&xgrid, 0, 0, &ugrid, 0);
1246 
1247  constraints.getBounds( tmp );
1248 
1249  bool boxConIsFinite = false;
1250  DVector lbTmp;
1251  DVector ubTmp;
1252 
1253  //
1254  // Extract box constraints on inputs
1255  //
1256  for (unsigned i = 0; i < tmp.u->getNumPoints(); ++i)
1257  {
1258  lbTmp = tmp.u->getLowerBounds( i );
1259  ubTmp = tmp.u->getUpperBounds( i );
1260 
1261  if ((ubTmp >= lbTmp) == false)
1262  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Some lower bounds are bigger than upper bounds?");
1263 
1264  if (isFinite( lbTmp ) || isFinite( ubTmp ))
1265  boxConIsFinite = true;
1266  }
1267 
1268  if (boxConIsFinite == true)
1269  uBounds = *(tmp.u);
1270  else
1271  uBounds.init();
1272 
1273  //
1274  // Extract box constraints on states
1275  //
1276  boxConIsFinite = false;
1277  for (unsigned i = 0; i < tmp.x->getNumPoints(); ++i)
1278  {
1279  lbTmp = tmp.x->getLowerBounds( i );
1280  ubTmp = tmp.x->getUpperBounds( i );
1281 
1282  if ((ubTmp >= lbTmp) == false)
1283  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Some lower bounds are bigger than upper bounds?");
1284 
1285  if (isFinite( lbTmp ) || isFinite( ubTmp ))
1286  boxConIsFinite = true;
1287  }
1288 
1289  if ( boxConIsFinite == true )
1290  xBounds = *(tmp.x);
1291  else
1292  xBounds.init();
1293 
1295  //
1296  // Intermezzo - reset static counters
1297  //
1299 
1300  DifferentialState dummy0;
1301  Control dummy1;
1302  dummy0.clearStaticCounters();
1303  dummy1.clearStaticCounters();
1304 
1305  DifferentialState vX("", NX, 1);
1306  Control vU("", NU, 1);
1307 
1308  int hessianApproximation;
1309  get( HESSIAN_APPROXIMATION, hessianApproximation );
1310  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
1311 
1313  //
1314  // Extract path constraints; pac prefix
1315  //
1317 
1318  conAuxVar.setName( "conAuxVar" );
1319  conAuxVar.setDataStruct( ACADO_WORKSPACE );
1320 
1321  Function pacH;
1322 
1323  DMatrix pacLBMatrix, pacUBMatrix;
1324  constraints.getPathConstraints(pacH, pacLBMatrix, pacUBMatrix);
1325 
1326  dimPacH = pacH.getDim();
1327 // std::cout << "pacH.getDim(): " << pacH.getDim() << std::endl;
1328 
1329  if (dimPacH != 0)
1330  {
1331  DifferentialState lambda("", dimPacH, 1);
1332 
1333  lbPathConValues = pacLBMatrix.getRows(0, N - 1).makeVector();
1334  ubPathConValues = pacUBMatrix.getRows(0, N - 1).makeVector();
1335 
1336  Expression expPacH, expPacHx, expPacHu, expPacDH, expPacDDH;
1337  pacH.getExpression( expPacH );
1338 
1339  // FIRST ORDER DERIVATIVES
1340  Expression S, arg, dl;
1341  S = eye<double>(NX+NU);
1342  arg << vX;
1343  arg << vU;
1344 
1345  expPacDDH = symmetricDerivative( expPacH, arg, S, lambda, &expPacDH, &dl );
1346 
1347  expPacHx = expPacDH.getCols(0, NX);
1348  expPacHu = expPacDH.getCols(NX, NX+NU);
1349 
1350 // expPacHx = forwardDerivative(expPacH, vX);
1351 // expPacHu = forwardDerivative(expPacH, vU);
1352 
1353 // expPacHxx = expPacDDH.getSubMatrix(0,NX,0,NX);
1354 // expPacHxu = expPacDDH.getSubMatrix(0,NX,NX,NX+NU);
1355 // expPacHuu = expPacDDH.getSubMatrix(NX,NX+NU,NX,NX+NU);
1356 
1357  Function pacHx, pacHu, pacDDH;
1358  pacHx << expPacHx;
1359  pacHu << expPacHu;
1360 
1361  pacDDH << expPacDDH;
1362 
1363  // Set dimension of residual
1364  pacEvH.setup("evH", N * dimPacH, 1, REAL, ACADO_WORKSPACE);
1365 
1366  // Check derivative of path constraints w.r.t. x
1367  if (pacHx.isConstant())
1368  {
1369  EvaluationPoint epPacHx( pacHx );
1370  DVector v = pacHx.evaluate( epPacHx );
1371 
1372  if (v.isZero() == false)
1373  {
1374  pacEvHx.setup("evHx", Eigen::Map<DMatrix>(v.data(), dimPacH, NX), REAL, ACADO_WORKSPACE);
1375  }
1376  }
1377  else
1378  {
1379  pacH << expPacHx;
1380 
1381  pacEvHx.setup("evHx", N * dimPacH, NX, REAL, ACADO_WORKSPACE);
1382  }
1383 
1384  // Check derivative of path constraints w.r.t. u
1385  if (pacHu.isConstant())
1386  {
1387  EvaluationPoint epPacHu( pacHu );
1388  DVector v = pacHu.evaluate( epPacHu );
1389 
1390  if (v.isZero() == false)
1391  {
1392  pacEvHu.setup("evHu", Eigen::Map<DMatrix>(v.data(), dimPacH, NU), REAL, ACADO_WORKSPACE);
1393  }
1394  }
1395  else
1396  {
1397  pacH << expPacHu;
1398 
1399  pacEvHu.setup("evHu", N * dimPacH, NU, REAL, ACADO_WORKSPACE);
1400  }
1401 
1402  uint dimL = 0;
1403  // Check second order derivatives of path constraints
1404  if (pacDDH.isConstant())
1405  {
1406  EvaluationPoint epPacDDH( pacDDH );
1407  DVector v = pacDDH.evaluate( epPacDDH );
1408 
1409  if (v.isZero() == false)
1410  {
1411  pacEvDDH.setup("evDDH", Eigen::Map<DMatrix>(v.data(), (NX+NU), (NX+NU)), REAL, ACADO_WORKSPACE);
1412  }
1413  }
1414  else if( secondOrder )
1415  {
1416  dimL = dimPacH;
1417  pacH << expPacDDH;
1418 
1419  pacEvDDH.setup("evDDH", (NX+NU), (NX+NU), REAL, ACADO_WORKSPACE);
1420  }
1421 
1422  if (performsSingleShooting() == false)
1423  {
1424  pacEvHxd.setup("evHxd", dimPacH, 1, REAL, ACADO_WORKSPACE);
1425  }
1426 
1427  conAuxVar.setup("conAuxVar", pacH.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
1428  conValueIn.setup("conValueIn", 1, NX + dimL + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
1429  conValueOut.setup("conValueOut", 1, pacH.getDim(), REAL, ACADO_WORKSPACE);
1430 
1431  evaluatePathConstraints.init(pacH, "evaluatePathConstraints", NX+dimL, 0, NU, NP, 0, NOD);
1432  evaluatePathConstraints.setGlobalExportVariable( conAuxVar );
1433  }
1434 
1436  //
1437  // Extract point constraints; poc prefix
1438  //
1440 
1441 
1442  evaluatePointConstraints.resize(N + 1);
1443 
1444  unsigned dimPocHMax = 0;
1445 
1446  pocLbStack.resize(N + 1);
1447  pocUbStack.resize(N + 1);
1448 
1449  // Setup the point constraints
1450  for (unsigned i = 0; i < N + 1; ++i)
1451  {
1452  Function pocH, pocH2;
1453  Expression expPocH, expPocH2, expPocHx, expPocHu;
1454  DMatrix pocLBMatrix, pocUBMatrix, pocLBMatrix2, pocUBMatrix2;
1455 
1456  // Get the point constraint
1457  constraints.getPointConstraint(i, pocH2, pocLBMatrix2, pocUBMatrix2);
1458 
1459  // Extract and stack the point constraint if it exists
1460  if ( pocH2.getDim() )
1461  {
1462 // std::cout << "i: " << i << ", pocH.getDim(): " << pocH2.getDim() << std::endl;
1463  if (pocH2.getNU() > 0 && i == N)
1464  {
1465  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "The terminal (point) constraint must not depend on controls.");
1466  }
1467 
1468  // Extract the function expression and stack its Jacobians w.r.t.
1469  // x and u
1470  pocH2.getExpression( expPocH2 );
1471 
1472  // XXX AFAIK, this is not bullet-proof!
1473  for (unsigned j = 0; j < (uint)pocH2.getDim(); ++j) {
1474  expPocH2 = Expression(*pocH2.getExpression(j));
1475  VariableType tmpType = expPocH2.getVariableType();
1476 // std::cout << "j: " << j << ", tmpType: " << tmpType << std::endl;
1477  if( tmpType == VT_UNKNOWN || tmpType == VT_INTERMEDIATE_STATE ) {
1478  expPocH << expPocH2;
1479  pocLBMatrix.appendCols(pocLBMatrix2.getCol(j));
1480  pocUBMatrix.appendCols(pocUBMatrix2.getCol(j));
1481  }
1482  }
1483 // std::cout << "expPocH.getDim(): " << expPocH.getDim() << std::endl;
1484  if( expPocH.getDim() == 0 ) continue;
1485  else {
1486  pocH << expPocH;
1487  }
1488 
1489  expPocHx = forwardDerivative(expPocH, vX);
1490  pocH << expPocHx;
1491 
1492  if (i < N)
1493  {
1494  expPocHu = forwardDerivative(expPocH, vU);
1495  pocH << expPocHu;
1496  }
1497 
1498  // Stack the new function
1499  evaluatePointConstraints[ i ] = std::shared_ptr< ExportAcadoFunction >(new ExportAcadoFunction);
1500 
1501  std::string pocFName;
1502 
1503  pocFName = "evaluatePointConstraint" + toString( i );
1504 
1505  if (i < N)
1506  {
1507  evaluatePointConstraints[ i ]->init(pocH, pocFName, NX, 0, NU, NP, 0, NOD);
1508  }
1509  else
1510  {
1511  evaluatePointConstraints[ i ]->init(pocH, pocFName, NX, 0, 0, NP, 0, NOD);
1512  }
1513 
1514  // Determine the maximum function dimension
1515  if ( dimPocHMax < (unsigned)pocH.getDim() )
1516  {
1517  dimPocHMax = pocH.getDim();
1518  }
1519 
1520  // TODO This is too specific for condensing, thus should be moved to condensing class.
1521  // Stack the lower and upper bounds
1522  lbPointConValues.append( pocLBMatrix.getRow( 0 ) );
1523  ubPointConValues.append( pocUBMatrix.getRow( 0 ) );
1524 
1525  pocLbStack[ i ] = pocLBMatrix.getRow( 0 );
1526  pocUbStack[ i ] = pocUBMatrix.getRow( 0 );
1527  }
1528  }
1529 
1530 // std::cout << "lb dim: " << pocLB.getDim() << std::endl;
1531 // std::cout << "ub dim: " << pocUB.getDim() << std::endl;
1532 
1533  dimPocH = lbPointConValues.getDim();
1534 
1535  if ( dimPocH != 0 )
1536  {
1537  unsigned pocAuxVarDim = 0;
1538 
1539  ExportVariable pocAuxVarTemp;
1540 
1541  for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
1542  {
1543  if ( !evaluatePointConstraints[ i ] )
1544  continue;
1545 
1546  pocAuxVarTemp = evaluatePointConstraints[ i ]->getGlobalExportVariable();
1547 
1548  pocAuxVarDim = pocAuxVarDim < pocAuxVarTemp.getDim() ? pocAuxVarTemp.getDim() : pocAuxVarDim;
1549 
1550  evaluatePointConstraints[ i ]->setGlobalExportVariable( conAuxVar );
1551  }
1552 
1553  int conAuxVarDim =
1554  (conAuxVar.getDim() < pocAuxVarDim) ? pocAuxVarDim : conAuxVar.getDim();
1555  conAuxVar.setup("conAuxVar", conAuxVarDim, 1, REAL, ACADO_WORKSPACE);
1556 
1557  conValueIn.setup("conValueIn", 1, NX + 0 + NU + NOD, REAL, ACADO_WORKSPACE);
1558 
1559  unsigned conValueOutDim =
1560  (dimPocHMax < conValueOut.getDim()) ? conValueOut.getDim() : dimPocHMax;
1561  conValueOut.setup("conValueOut", 1, conValueOutDim, REAL, ACADO_WORKSPACE);
1562 
1563  pocEvH.setup("pocEvH", dimPocH, 1, REAL, ACADO_WORKSPACE);
1564  pocEvHx.setup("pocEvHx", dimPocH, NX, REAL, ACADO_WORKSPACE);
1565 
1566  // For this guy we actually need less... but no worry for now
1567  pocEvHu.setup("pocEvHu", dimPocH, NU, REAL, ACADO_WORKSPACE);
1568 
1569  // Setup one more variable for MS:
1570  if (performsSingleShooting() == false)
1571  {
1572  pocEvHxd.setup("pocEvHxd", dimPocH, 1, REAL, ACADO_WORKSPACE);
1573  }
1574  }
1575 
1576  return SUCCESSFUL_RETURN;
1577 }
1578 
1579 unsigned ExportNLPSolver::getNumComplexConstraints( void )
1580 {
1581  return N * dimPacH + dimPocH;
1582 }
1583 
1584 unsigned ExportNLPSolver::getNumPathConstraints( void )
1585 {
1586  return dimPacH;
1587 }
1588 
1589 bool ExportNLPSolver::initialStateFixed() const
1590 {
1591  int fixInitialState;
1592  get(FIX_INITIAL_STATE, fixInitialState);
1593 
1594  return (bool)fixInitialState;
1595 }
1596 
1597 bool ExportNLPSolver::usingLinearTerms() const
1598 {
1599  if (objSlx.isGiven() == false && objSlu.isGiven() == false)
1600  return true;
1601  // Otherwise they are hard-coded and we don't need this indicator
1602  return false;
1603 }
1604 
1605 returnValue ExportNLPSolver::setupAuxiliaryFunctions()
1606 {
1607  string moduleName;
1608  get(CG_MODULE_NAME, moduleName);
1609 
1611  //
1612  // Shift controls
1613  //
1615  ExportVariable uEnd("uEnd", NU, 1, REAL, ACADO_LOCAL);
1616  uEnd.setDoc( "Value for the u vector on the second to last node. If =0 the old value is used." );
1617  ExportIndex index( "index" );
1618  shiftControls.setup("shiftControls", uEnd);
1619  shiftControls.addIndex( index );
1620  shiftControls.doc( "Shift controls vector by one interval." );
1621 
1622  ExportForLoop uLoop(index, 0, N - 1);
1623  uLoop.addStatement( u.getRow( index ) == u.getRow(index + 1) );
1624 
1625  shiftControls.addStatement( uLoop );
1626  shiftControls.addLinebreak( );
1627  shiftControls.addStatement( "if (uEnd != 0)\n{\n" );
1628  shiftControls.addStatement(u.getRow(N - 1) == uEnd.getTranspose());
1629  shiftControls.addStatement( "}\n" );
1630 
1632  //
1633  // Shift states
1634  //
1636  ExportVariable xEnd("xEnd", NX, 1, REAL, ACADO_LOCAL);
1637  xEnd.setDoc( "Value for the x vector on the last node. If =0 the old value is used." );
1638  ExportIndex strategy( "strategy" );
1639  strategy.setDoc( string("Shifting strategy: 1. Initialize node ") + toString(N + 1) + " with xEnd." \
1640  " 2. Initialize node " + toString(N + 1) + " by forward simulation." );
1641  // TODO Think about adding zEnd here at some point...
1642  shiftStates.setup("shiftStates", strategy, xEnd, uEnd);
1643  shiftStates.addIndex( index );
1644  if (NXA == 0)
1645  shiftStates.doc( "Shift differential variables vector by one interval." );
1646  else
1647  shiftStates.doc( "Shift differential variables vector and algebraic variables vector by one interval." );
1648 
1649  ExportForLoop xLoop(index, 0, N);
1650  xLoop.addStatement( x.getRow( index ) == x.getRow(index + 1) );
1651  shiftStates.addStatement( xLoop );
1652 
1653  if (NXA > 0)
1654  {
1655  ExportForLoop zLoop(index, 0, N - 1);
1656  zLoop.addStatement( z.getRow( index ) == z.getRow(index + 1) );
1657  shiftStates.addStatement( zLoop );
1658  }
1659 
1660  shiftStates.addLinebreak( );
1661  shiftStates.addStatement( "if (strategy == 1 && xEnd != 0)\n{\n" );
1662  shiftStates.addStatement( x.getRow( N ) == xEnd.getTranspose() );
1663  shiftStates.addStatement( "}\n" );
1664  shiftStates.addStatement( "else if (strategy == 2) \n{\n" );
1665 
1666  int gradientUp;
1667  get( LIFTED_GRADIENT_UPDATE, gradientUp );
1668  bool gradientUpdate = (bool) gradientUp;
1669 
1670  int hessianApproximation;
1671  get( HESSIAN_APPROXIMATION, hessianApproximation );
1672  int sensitivityProp;
1673  get( DYNAMIC_SENSITIVITY, sensitivityProp );
1674  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
1675  bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate));
1676 
1677  uint symH = 0;
1678  if( (ExportSensitivityType) sensitivityProp == SYMMETRIC || (ExportSensitivityType) sensitivityProp == SYMMETRIC_FB || (secondOrder && (ExportSensitivityType) sensitivityProp == BACKWARD) ) symH = (NX+NU)*(NX+NU+1)/2;
1679  else if( (ExportSensitivityType) sensitivityProp == FORWARD_OVER_BACKWARD && gradientUpdate ) symH = (NX+NU)*(NX+NU); // TODO: this is a quick fix for the dimensions in case of FOB lifted collocation integrators
1680  else if( (ExportSensitivityType) sensitivityProp == FORWARD_OVER_BACKWARD ) symH = NX*(NX+NU)+NU*NU;
1681 
1682 
1683  unsigned indexZ = NX + NXA;
1684  if( secondOrder || adjoint ) indexZ = indexZ + NX; // because of the first order adjoint direction
1685  unsigned indexGxx = indexZ + NX * NX;
1686  unsigned indexGzx = indexGxx + NXA * NX;
1687  unsigned indexGxu = indexGzx + NX * NU;
1688  unsigned indexGzu = indexGxu + NXA * NU;
1689  unsigned indexH = indexGzu;
1690  if( secondOrder ) indexH = indexGzu + symH; // because of the second order derivatives
1691  unsigned indexG = indexH;
1692  if( (secondOrder && gradientUpdate) || adjoint ) indexG = indexH + NX+NU;
1693  unsigned indexU = indexG + NU;
1694  unsigned indexOD = indexU + NOD;
1695 
1696  shiftStates.addStatement( state.getCols(0, NX) == x.getRow( N ) );
1697  shiftStates.addStatement( state.getCols(NX, NX + NXA) == z.getRow(N - 1) );
1698  shiftStates.addStatement( "if (uEnd != 0)\n{\n" );
1699  shiftStates.addStatement( state.getCols(indexG, indexU) == uEnd.getTranspose() );
1700  shiftStates.addStatement( "}\n" );
1701  shiftStates.addStatement( "else\n{\n" );
1702  shiftStates.addStatement( state.getCols(indexG, indexU) == u.getRow(N - 1) );
1703  shiftStates.addStatement( "}\n" );
1704  shiftStates.addStatement( state.getCols(indexU, indexOD) == od.getRow( N ) );
1705  shiftStates.addLinebreak( );
1706 
1707  if ( integrator->equidistantControlGrid() )
1708  {
1709  shiftStates << moduleName << "_integrate" << "(" << state.getFullName() << ", 1);\n";
1710  }
1711  else
1712  {
1713  shiftStates << moduleName << "_integrate" << "(" << state.getFullName() << ", 1, " << toString(N - 1) << ");\n";
1714  }
1715 
1716  shiftStates.addLinebreak( );
1717  shiftStates.addStatement( x.getRow( N ) == state.getCols(0, NX) );
1718  if ( NXA )
1719  {
1720  shiftStates.addLinebreak();
1721  shiftStates.addStatement(z.getRow(N - 1) == state.getCols(NX, NX + NXA));
1722  }
1723 
1724 
1725  shiftStates.addStatement( "}\n" );
1726 
1728  //
1729  // Initialize nodes by a forward simulation
1730  //
1732  initializeNodes.setup("initializeNodesByForwardSimulation");
1733  initializeNodes.addIndex( index );
1734  initializeNodes.doc( "Initialize shooting nodes by a forward simulation starting from the first node." );
1735 
1736  ExportForLoop iLoop(index, 0, N);
1737 
1738 
1739  iLoop.addStatement( state.getCols(0, NX) == x.getRow( index ) );
1740  if ( NXA )
1741  {
1742  iLoop << std::string("if (") << index.getFullName() << std::string(" > 0){");
1743  iLoop.addStatement( state.getCols(NX, NX + NXA) == z.getRow(index - 1) );
1744  iLoop << std::string("}\n");
1745  }
1746  iLoop.addStatement( state.getCols(indexG, indexU) == u.getRow( index ) );
1747  iLoop.addStatement( state.getCols(indexU, indexOD) == od.getRow( index ) );
1748  iLoop.addLinebreak( );
1749 
1750  if ( integrator->equidistantControlGrid() )
1751  {
1752  iLoop << moduleName << "_integrate"
1753  << "(" << state.getFullName() << ", "
1754  << index.getFullName() << " == 0"
1755  << ");\n";
1756  }
1757  else
1758  {
1759  iLoop << moduleName << "_integrate"
1760  << "(" << state.getFullName() << ", "
1761  << index.getFullName() << " == 0"
1762  << ", " << index.getFullName() << ");\n";
1763  }
1764 
1765  iLoop.addLinebreak();
1766  iLoop.addStatement( x.getRow(index + 1) == state.getCols(0, NX) );
1767 
1768  // Store improved initial guess from the integrator
1769  iLoop.addStatement( z.getRow(index) == state.getCols(NX, NX + NXA) );
1770 
1771  initializeNodes.addStatement( iLoop );
1772 
1773  return setupGetObjective();
1774 }
1775 
1776 
1777 returnValue ExportNLPSolver::setupGetObjective( )
1778 {
1779  if( getNY() > 0 || getNYN() > 0 ) {
1780  return setupGetLSQObjective( );
1781  }
1782  else {
1783  return setupGetGeneralObjective( );
1784  }
1785 }
1786 
1787 
1788 returnValue ExportNLPSolver::setupGetLSQObjective() {
1790  //
1791  // Objective value calculation
1792  //
1794 
1795  getObjective.setup( "getObjective" );
1796  getObjective.doc( "Calculate the objective value." );
1797  ExportVariable objVal("objVal", 1, 1, REAL, ACADO_LOCAL, true);
1798  objVal.setDoc( "Value of the objective function." );
1799  getObjective.setReturnValue( objVal );
1800 
1801  ExportVariable tmpDx("tmpDx", 1, NX, REAL, ACADO_LOCAL );
1802  ExportVariable tmpDy("tmpDy", 1, getNY(), REAL, ACADO_LOCAL );
1803  ExportVariable tmpDyN("tmpDyN", 1, getNYN(), REAL, ACADO_LOCAL );
1804 
1805  getObjective.addVariable( tmpDy );
1806  getObjective.addVariable( tmpDyN );
1807 
1808  ExportIndex oInd;
1809  getObjective.acquire( oInd );
1810 
1811  // Recalculate objective
1812 
1813  ExportForLoop loopObjective(oInd, 0, N);
1814 
1815  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( oInd ) );
1816  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( oInd ) );
1817  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( oInd ) );
1818  loopObjective.addLinebreak( );
1819 
1820  // Evaluate the objective function
1821  loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
1822 
1823  // Stack the measurement function value
1824  loopObjective.addStatement(
1825  Dy.getRows(oInd * NY, (oInd + 1) * NY) ==
1826  objValueOut.getTranspose().getRows(0, getNY()) - y.getRows(oInd * NY, (oInd + 1) * NY)
1827  );
1828 
1829  getObjective.addStatement( loopObjective );
1830 
1831  getObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
1832  getObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
1833 
1834  // Evaluate the objective function
1835  getObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
1836 
1837  getObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) - yN.getTranspose() );
1838 
1839  getObjective.addStatement( objVal == 0 );
1840 
1841  ExportForLoop oLoop(oInd, 0, N);
1842 
1843  int variableObjS;
1844  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
1845 
1846  if (variableObjS == NO)
1847  {
1848  oLoop.addStatement( tmpDy == Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * objS );
1849  oLoop.addStatement( objVal += Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * tmpDy.getTranspose() );
1850  }
1851  else
1852  {
1853  oLoop.addStatement( tmpDy == Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * objS.getSubMatrix(oInd * NY, (oInd + 1) * NY, 0, NY) );
1854  oLoop.addStatement( objVal += Dy.getTranspose().getCols(oInd * NY, (oInd + 1) * NY) * tmpDy.getTranspose() );
1855  }
1856 
1857  getObjective.addStatement( oLoop );
1858  getObjective.addLinebreak( );
1859 
1860  getObjective.addStatement( tmpDyN == DyN.getTranspose() * objSEndTerm );
1861  getObjective.addStatement( objVal += DyN.getTranspose() * tmpDyN.getTranspose() );
1862 
1863  if ( SAC.getDim() > 0 )
1864  {
1865  getObjective.addVariable( tmpDx );
1866  getObjective.addStatement( tmpDx == DxAC.getTranspose() * SAC );
1867  getObjective.addStatement( objVal += tmpDx * DxAC );
1868  }
1869  getObjective.addLinebreak( );
1870 
1871  getObjective.addStatement( "objVal *= 0.5;\n" );
1872 
1873  return SUCCESSFUL_RETURN;
1874 }
1875 
1876 returnValue ExportNLPSolver::setupGetGeneralObjective() {
1878  //
1879  // Objective value calculation
1880  //
1882 
1883  getObjective.setup( "getObjective" );
1884  getObjective.doc( "Calculate the objective value." );
1885  ExportVariable objVal("objVal", 1, 1, REAL, ACADO_LOCAL, true);
1886  objVal.setDoc( "Value of the objective function." );
1887  getObjective.setReturnValue( objVal );
1888 
1889  ExportIndex oInd;
1890  getObjective.acquire( oInd );
1891 
1892  // Recalculate objective
1893  getObjective.addStatement( objVal == 0 );
1894 
1895  if( evaluateStageCost.getFunctionDim() > 0 ) {
1896  ExportForLoop loopObjective(oInd, 0, N);
1897 
1898  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( oInd ) );
1899  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( oInd ) );
1900  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( oInd ) );
1901  loopObjective.addLinebreak( );
1902 
1903  // Evaluate the objective function
1904  loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
1905 
1906  // Stack the measurement function value
1907  loopObjective.addStatement( objVal += objValueOut.getCol(0) );
1908 
1909  getObjective.addStatement( loopObjective );
1910  }
1911  if( evaluateTerminalCost.getFunctionDim() > 0 ) {
1912  getObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
1913  getObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
1914 
1915  // Evaluate the objective function
1916  getObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
1917 
1918  getObjective.addStatement( objVal += objValueOut.getCol(0) );
1919  }
1920 
1921  return SUCCESSFUL_RETURN;
1922 }
1923 
1924 unsigned ExportNLPSolver::weightingMatricesType( void ) const
1925 {
1926  if (objS.isGiven() == true && objSEndTerm.isGiven() == true)
1927  return 0;
1928 
1929  // get the option for variable objS matrix.
1930  int variableObjS;
1931  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
1932  if ( variableObjS )
1933  return 2;
1934 
1935  return 1;
1936 }
1937 
1938 returnValue ExportNLPSolver::setupArrivalCostCalculation()
1939 {
1940  int useArrivalCost;
1941  get(CG_USE_ARRIVAL_COST, useArrivalCost);
1942  if (useArrivalCost == NO)
1943  return SUCCESSFUL_RETURN;
1944 
1945  SAC.setup("SAC", NX, NX, REAL, ACADO_VARIABLES);
1946  SAC.setDoc("Arrival cost term: inverse of the covariance matrix.");
1947  xAC.setup("xAC", NX, 1, REAL, ACADO_VARIABLES);
1948  xAC.setDoc("Arrival cost term: a priori state estimate.");
1949  DxAC.setup("DxAC", NX, 1, REAL, ACADO_WORKSPACE);
1950 
1951  string moduleName;
1952  get(CG_MODULE_NAME, moduleName);
1953 
1954  ExportVariable evRet("ret", 1, 1, INT, ACADO_LOCAL, true);
1955 
1956  ExportVariable evReset("reset", 1, 1, INT, ACADO_LOCAL, true);
1957  evReset.setDoc("Reset S_{AC}. Set it to 1 to initialize arrival cost calculation, "
1958  "and later should set it to 0.");
1959 
1960  updateArrivalCost.init("updateArrivalCost", evReset);
1961  updateArrivalCost.doc("Use this function to update the arrival cost.");
1962  updateArrivalCost.setReturnValue( evRet );
1963  updateArrivalCost << (evRet == 0);
1964 
1965  const unsigned AM = 2 * NX + NY;
1966  const unsigned AN = 2 * NX + NU; // A bit different from my implementation
1967 
1968  acA.setup("acA", AM, AN, REAL, ACADO_WORKSPACE);
1969  acb.setup("acb", AM, 1, REAL, ACADO_WORKSPACE);
1970  acP.setup("acP", NX, NX, REAL, ACADO_WORKSPACE);
1971 
1972  acWL.setup("WL", NX, NX, REAL, ACADO_VARIABLES);
1973  acWL.setDoc("Arrival cost term: Cholesky decomposition, lower triangular, "
1974  " of the inverse of the state noise covariance matrix.");
1975  acVL.setup("acVL", NY, NY, REAL, ACADO_WORKSPACE);
1976 
1977  acHx.setup("acHx", NY, NX, REAL, ACADO_WORKSPACE);
1978  acHu.setup("acHu", NY, NU, REAL, ACADO_WORKSPACE);
1979  acXx.setup("acXx", NX, NX, REAL, ACADO_WORKSPACE);
1980  acXu.setup("acXu", NX, NU, REAL, ACADO_WORKSPACE);
1981 
1982  acXTilde.setup("acXTilde", NX, 1, REAL, ACADO_WORKSPACE);
1983  acHTilde.setup("acHTilde", NY, 1, REAL, ACADO_WORKSPACE);
1984 
1985  //
1986  // Perform a hard reset if necessary
1987  // This is a bit messy because the update code needs upper triangular P
1988  // matrix, but Cholesky function returns lower triangular. One way to
1989  // avoid this is to be add an option to ExportCholeskyDecomposition to
1990  // ---> export lower or upper triangular matrix.
1991  //
1992  cholSAC.init("cholSAC", NX);
1993  cholSAC.setup();
1994 
1995  updateArrivalCost << "\nif ( " << evReset.getName() << " )\n{\n";
1996  updateArrivalCost.addStatement( acXx == SAC );
1997  updateArrivalCost.addFunctionCall(cholSAC.getName(), acXx);
1998  updateArrivalCost << (acP == acXx.getTranspose());
1999  updateArrivalCost << std::string( "return 0;\n}\n\n" );
2000 
2001  //
2002  // Evaluate model @ the first node
2003  //
2004 
2005  unsigned indexZ = NX + NXA;
2006  unsigned indexGxx = indexZ + NX * NX;
2007  unsigned indexGzx = indexGxx + NXA * NX;
2008  unsigned indexGxu = indexGzx + NX * NU;
2009  unsigned indexGzu = indexGxu + NXA * NU;
2010  unsigned indexU = indexGzu + NU;
2011  unsigned indexNOD = indexU + NOD;
2012 
2013  updateArrivalCost.addStatement( state.getCols(0, NX) == x.getRow( 0 ) );
2014  updateArrivalCost.addStatement( state.getCols(NX, NX + NXA) == z.getRow( 0 ) );
2015  updateArrivalCost.addStatement( state.getCols(indexGzu, indexU) == u.getRow( 0 ) );
2016  updateArrivalCost.addStatement( state.getCols(indexU, indexNOD) == od.getRow( 0 ) );
2017 
2018  if (integrator->equidistantControlGrid())
2019  updateArrivalCost << moduleName << "_integrate" << "(" << state.getFullName() << ", 1);\n";
2020  else
2021  updateArrivalCost << moduleName << "_integrate" << "(" << state.getFullName() << ", 1, " << toString(0) << ");\n";
2022  updateArrivalCost.addLinebreak( );
2023 
2024  //
2025  // Evaluate objective function @ the first node
2026  //
2027 
2028  updateArrivalCost.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( 0 ) );
2029  updateArrivalCost.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( 0 ) );
2030  updateArrivalCost.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( 0 ) );
2031 
2032  updateArrivalCost.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
2033  updateArrivalCost.addLinebreak( );
2034 
2035  //
2036  // Cholesky decomposition of the term objS
2037  //
2038  if (objS.isGiven() == true)
2039  {
2040  DMatrix m = objS.getGivenMatrix();
2041  DMatrix mChol = m.llt().matrixL();
2042 
2043  initialize << (acVL == mChol);
2044  }
2045  else
2046  {
2047  cholObjS.init("cholObjS", NY);
2048  cholObjS.setup();
2049 
2050  int variableObjS;
2051  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
2052 
2053  if ( variableObjS )
2054  {
2055  updateArrivalCost << (acVL == objS.getSubMatrix(0, NY, 0, NY));
2056  updateArrivalCost.addFunctionCall(cholObjS.getName(), acVL);
2057  }
2058  else
2059  {
2060  updateArrivalCost << (acVL == objS);
2061  updateArrivalCost.addFunctionCall(cholObjS.getName(), acVL);
2062  }
2063  }
2064 
2065  //
2066  // Create acA and acb
2067  //
2068 
2069  /*
2070  PL is square root form already.
2071 
2072  A = ||PL zeros(nx, nu) zeros(nx, nx) ||
2073  ||-VL * Hx -VL * Hu zeros(ny, nx) ||
2074  ||-WL * Xx -WL * Xu WL ||
2075 
2076  Since A is initialized to 0, we should use use -= operator
2077  */
2078 
2079  // Clear A and b
2080  updateArrivalCost
2081  << (acA == zeros<double>(AM, AN))
2082  << (acb == zeros<double>(AM, 1))
2083  << std::string( "\n" );
2084 
2085  // Copy products to the matrices
2086  updateArrivalCost
2087  << (acXx.makeRowVector() == state.getCols(indexZ, indexGxx))
2088  << (acXu.makeRowVector() == state.getCols(indexGzx, indexGxu));
2089 
2090  unsigned ind = NY;
2091  if (objEvFx.isGiven() == true)
2092  {
2093  initialize << (acHx == objEvFx);
2094  }
2095  else
2096  {
2097  updateArrivalCost << (acHx.makeRowVector() == objValueOut.getCols(ind, ind + NY * NX));
2098  ind += NY * NX;
2099  }
2100 
2101  if (objEvFu.isGiven() == true)
2102  {
2103  initialize << (acHu == objEvFu);
2104  }
2105  else
2106  {
2107  updateArrivalCost << (acHu.makeRowVector() == objValueOut.getCols(ind, ind + NY * NU));
2108  }
2109 
2110  // acVL and acWL are lower triangular matrices
2111  // acP is ALWAYS upper triangular!
2112 
2113  updateArrivalCost
2114  << (acA.getSubMatrix(0, NX, 0, NX) == acP)
2115 
2116  << (acA.getSubMatrix(NX, NX + NY, 0, NX) -= (acVL ^ acHx))
2117  << (acA.getSubMatrix(NX, NX + NY, NX, NX + NU) -= (acVL ^ acHu))
2118 
2119  << (acA.getSubMatrix(NX + NY, NX + NY + NX, 0, NX) -= (acWL ^ acXx))
2120  << (acA.getSubMatrix(NX + NY, NX + NY + NX, NX, NX + NU) -= (acWL ^ acXu))
2121  << (acA.getSubMatrix(NX + NY, NX + NY + NX, NX + NU, NX + NU + NX) == acWL.getTranspose());
2122 
2123  /*
2124 
2125  x1 is output from the integrator
2126  h is evaluated obj @ node 0
2127  x and u are current solutions
2128  xL and uL are previous arrival cost values.
2129 
2130  x_tilde = x1 - np.dot(Xx,x) - np.dot(Xu,u)
2131  h_tilde = h - np.dot(Hx,x) - np.dot(Hu,u)
2132 
2133  res = np.bmat([ -np.dot(PL, xL),
2134  np.dot(VL, yL - h_tilde),
2135  -np.dot(WL, x_tilde) ])
2136 
2137  */
2138 
2139  updateArrivalCost
2140  << (acXTilde == state.getTranspose().getRows(0, NX))
2141  << (acXTilde -= acXx * x.getRow( 0 ).getTranspose())
2142  << (acXTilde -= acXu * u.getRow( 0 ).getTranspose());
2143 
2144  updateArrivalCost
2145  << (acHTilde == y.getRows(0, NY))
2146  << (acHTilde -= objValueOut.getTranspose().getRows(0, NY))
2147  << (acHTilde += acHx * x.getRow( 0 ).getTranspose())
2148  << (acHTilde += acHu * u.getRow( 0 ).getTranspose());
2149 
2150  // Inverted signs from Mario's implementation
2151  updateArrivalCost
2152  << (acb.getRows(0, NX) == (acP * xAC))
2153  << (acb.getRows(NX, NX + NY) -= (acVL ^ acHTilde))
2154  << (acb.getRows(NX + NY, NX + NY + NX) == (acWL ^ acXTilde));
2155 
2156  //
2157  // Solver the linear system
2158  // We need first NX back-solves to get solution of this linear system...
2159  //
2160  acSolver.init(AM, AN, NX, false, false, std::string("ac"));
2161  acTmp = acSolver.getGlobalExportVariable( 1 );
2162  updateArrivalCost.addFunctionCall(acSolver.getNameSolveFunction(), acA, acb, acTmp);
2163 
2164  //
2165  // Get the solution of the linear system
2166  //
2167 
2168  updateArrivalCost << (xAC == acb.getRows(NX + NU, 2 * NX + NU));
2169 
2170  // Get the update acP, upper triangular part
2171  for (unsigned row = 0; row < NX; ++row)
2172  for (unsigned col = row; col < NX; ++col)
2173  updateArrivalCost << (acP.getElement(row, col) == acA.getElement(NX + NU + row, NX + NU + col));
2174 
2175  // Calculate the weighting matrix which is used outside.
2176  updateArrivalCost << (SAC == (acP ^ acP));
2177 
2178  return SUCCESSFUL_RETURN;
2179 }
2180 
2181 CLOSE_NAMESPACE_ACADO
ExportNLPSolver::acHTilde
ExportVariable acHTilde
Definition: export_nlp_solver.hpp:340
Objective::getLSQLinearTerms
returnValue getLSQLinearTerms(LsqLinearElements &_elements) const
Definition: objective.cpp:607
GenericMatrix::getCol
GenericVector< T > getCol(unsigned _idx) const
Definition: matrix.hpp:205
LOG
#define LOG(level)
Just define a handy macro for getting the logger.
Definition: acado_message_handling.hpp:193
Objective::getNumLagrangeTerms
uint getNumLagrangeTerms() const
Definition: objective.cpp:627
LVL_DEBUG
Lowest level, the debug level.
Definition: acado_types.hpp:1122
ExportNLPSolver::acTmp
ExportVariable acTmp
Definition: export_nlp_solver.hpp:338
ExportNLPSolver::objEvFx
ExportVariable objEvFx
Definition: export_nlp_solver.hpp:266
HessianApproximationMode
HessianApproximationMode
Definition: acado_types.hpp:611
symmetricDerivative
Expression symmetricDerivative(const Expression &arg1, const Expression &arg2, const Expression &forward_seed, const Expression &backward_seed, Expression *forward_result, Expression *backward_result)
Definition: acado_syntax.cpp:159
OCPiterate
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
FORWARD_OVER_BACKWARD
Definition: integrator_export_types.hpp:69
Function
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
SINGLE_SHOOTING
Definition: acado_types.hpp:202
ExportNLPSolver::conValueOut
ExportVariable conValueOut
Definition: export_nlp_solver.hpp:296
ExportVariable::getRow
ExportVariable getRow(const ExportIndex &idx) const
Definition: export_variable.cpp:395
Objective::getLSQEndTerms
returnValue getLSQEndTerms(LsqElements &_elements) const
Definition: objective.cpp:586
ExportAlgorithm::getN
uint getN() const
Definition: export_algorithm.cpp:128
ExportNLPSolver::acXu
ExportVariable acXu
Definition: export_nlp_solver.hpp:340
OCPiterate::x
VariablesGrid * x
Definition: ocp_iterate.hpp:186
ExportNLPSolver::objS
ExportVariable objS
Definition: export_nlp_solver.hpp:265
ExportNLPSolver::getDataDeclarations
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: export_nlp_solver.cpp:89
ExportNLPSolver::objSlu
ExportVariable objSlu
Definition: export_nlp_solver.hpp:278
ExportNLPSolver::integrator
IntegratorExportPtr integrator
Definition: export_nlp_solver.hpp:238
ExportNLPSolver::DxAC
ExportVariable DxAC
Definition: export_nlp_solver.hpp:343
ExportNLPSolver::shiftStates
ExportFunction shiftStates
Definition: export_nlp_solver.hpp:323
ExportNLPSolver::pacEvHx
ExportVariable pacEvHx
Definition: export_nlp_solver.hpp:301
ExportAlgorithm::getNXA
uint getNXA() const
Definition: export_algorithm.cpp:105
Constraint::getPathConstraints
returnValue getPathConstraints(Function &function_, DMatrix &lb_, DMatrix &ub_) const
Definition: constraint.cpp:1228
LIFTED_FEEDBACK
Definition: acado_types.hpp:163
IntegratorExportPtr
std::shared_ptr< IntegratorExport > IntegratorExportPtr
Definition: integrator_export.hpp:501
LsqLinearElements
std::vector< LsqLinearData > LsqLinearElements
Definition: objective.hpp:98
SparseQPsolutionMethods
SparseQPsolutionMethods
Definition: acado_types.hpp:755
ExportNLPSolver::setupArrivalCostCalculation
returnValue setupArrivalCostCalculation()
Definition: export_nlp_solver.cpp:1938
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
CG_MODULE_NAME
Definition: acado_types.hpp:335
ASSERT_RETURN
#define ASSERT_RETURN(x)
Definition: acado_debugging.hpp:52
FIX_INITIAL_STATE
Definition: acado_types.hpp:420
ExportVariable::getTranspose
ExportVariable getTranspose() const
Definition: export_variable.cpp:390
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:104
GenericMatrix::appendCols
GenericMatrix & appendCols(const GenericMatrix &_arg)
Definition: matrix.cpp:83
ACADO_WORKSPACE
Definition: acado_types.hpp:805
ExportNLPSolver::pocEvH
ExportVariable pocEvH
Definition: export_nlp_solver.hpp:313
OCPiterate::u
VariablesGrid * u
Definition: ocp_iterate.hpp:189
ExportNLPSolver::initialStateFixed
bool initialStateFixed() const
Definition: export_nlp_solver.cpp:1589
ExportNLPSolver::xBounds
VariablesGrid xBounds
Definition: export_nlp_solver.hpp:287
ExportNLPSolver::pocEvHx
ExportVariable pocEvHx
Definition: export_nlp_solver.hpp:314
ExportVariable::setup
ExportVariable & setup(const std::string &_name, uint _nRows=1, uint _nCols=1, ExportType _type=REAL, ExportStruct _dataStruct=ACADO_LOCAL, bool _callItByValue=false, const std::string &_prefix=std::string())
Definition: export_variable.cpp:148
ExportVariable::isGiven
bool isGiven(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
Definition: export_variable.cpp:213
ExportFunction::init
returnValue init(const std::string &_name="defaultFunctionName", const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
Definition: export_function.cpp:76
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
ExportNLPSolver::setLevenbergMarquardt
returnValue setLevenbergMarquardt(double _levenbergMarquardt)
Definition: export_nlp_solver.cpp:62
ExportAcadoFunction
Allows to export code of an ACADO function.
Definition: export_acado_function.hpp:50
HESSIAN_APPROXIMATION
Definition: acado_types.hpp:376
Constraint::getPointConstraint
returnValue getPointConstraint(const unsigned index, Function &function_, DMatrix &lb_, DMatrix &ub_) const
Definition: constraint.cpp:1233
ExportAlgorithm::getNX
uint getNX() const
Definition: export_algorithm.cpp:99
ExportNLPSolver::setupGetLSQObjective
virtual returnValue setupGetLSQObjective()
Definition: export_nlp_solver.cpp:1788
Constraint
Stores and evaluates the constraints of optimal control problems.
Definition: constraint.hpp:60
VariablesGrid
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Definition: variables_grid.hpp:56
RET_INVALID_OPTION
Definition: acado_types.hpp:889
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
Function::evaluate
DVector evaluate(const EvaluationPoint &x, const int &number=0)
Definition: function.cpp:520
GenericMatrix::makeVector
GenericMatrix & makeVector()
Definition: matrix.cpp:124
ExportNLPSolver::conValueIn
ExportVariable conValueIn
Definition: export_nlp_solver.hpp:295
ExportNLPSolver::objEvFxx
ExportVariable objEvFxx
Definition: export_nlp_solver.hpp:267
forwardDerivative
Expression forwardDerivative(const Expression &arg1, const Expression &arg2)
Definition: acado_syntax.cpp:106
MatrixVariablesGrid::getUpperBounds
DVector getUpperBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1393
ExportNLPSolver::pacEvHu
ExportVariable pacEvHu
Definition: export_nlp_solver.hpp:301
ExportNLPSolver::pocLbStack
std::vector< DVector > pocLbStack
Definition: export_nlp_solver.hpp:311
ExportStatementBlock::addComment
returnValue addComment(const std::string &_comment)
Definition: export_statement_block.cpp:228
ExportNLPSolver::setLSQObjective
returnValue setLSQObjective(const Objective &_objective)
Definition: export_nlp_solver.cpp:715
VT_INTERMEDIATE_STATE
Definition: acado_types.hpp:105
EvaluationPoint
Allows to setup function evaluation points.
Definition: evaluation_point.hpp:58
Function::getNU
int getNU() const
Definition: function.cpp:222
IMPLICIT_INTEGRATOR_MODE
Definition: acado_types.hpp:344
ExportAlgorithm::getNOD
uint getNOD() const
Definition: export_algorithm.cpp:122
ExportNLPSolver::setConstraints
returnValue setConstraints(const OCP &_ocp)
Definition: export_nlp_solver.cpp:1227
ExportNLPSolver::objEvFxEnd
ExportVariable objEvFxEnd
Definition: export_nlp_solver.hpp:266
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
RET_NONPOSITIVE_WEIGHT
Definition: acado_types.hpp:1091
LIFTED
Definition: acado_types.hpp:162
OCPiterate::init
returnValue init(const VariablesGrid *const _x, const VariablesGrid *const _xa, const VariablesGrid *const _p, const VariablesGrid *const _u, const VariablesGrid *const _w)
Definition: ocp_iterate.cpp:112
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
ExportNLPSolver::acP
ExportVariable acP
Definition: export_nlp_solver.hpp:338
ACADO_VARIABLES
Definition: acado_types.hpp:804
ExportVariable::getGivenMatrix
const DMatrix & getGivenMatrix() const
Definition: export_variable.cpp:460
ExportForLoop
Allows to export code of a for-loop.
Definition: export_for_loop.hpp:56
ExportNLPSolver::xAC
ExportVariable xAC
Definition: export_nlp_solver.hpp:343
Expression::getCols
Expression getCols(const uint &colIdx1, const uint &colIdx2) const
Definition: expression.cpp:582
std::toString
string toString(T const &value)
Definition: acado_io_utils.hpp:208
ExportSensitivityType
ExportSensitivityType
Definition: integrator_export_types.hpp:64
ExportData::setName
returnValue setName(const std::string &_name)
Definition: export_data.cpp:61
ExportNLPSolver::uBounds
VariablesGrid uBounds
Definition: export_nlp_solver.hpp:286
ImplicitIntegratorMode
ImplicitIntegratorMode
Definition: acado_types.hpp:158
ExportVariable::getElement
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
Definition: export_variable.cpp:434
ExportNLPSolver::objSlx
ExportVariable objSlx
Definition: export_nlp_solver.hpp:278
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
OCP::getConstraint
returnValue getConstraint(Constraint &constraint_) const
Definition: ocp.cpp:331
ExportNLPSolver::acWL
ExportVariable acWL
Definition: export_nlp_solver.hpp:340
ExportNLPSolver::objg
ExportVariable objg
Definition: export_nlp_solver.hpp:265
ExportNLPSolver::DyN
ExportVariable DyN
Definition: export_nlp_solver.hpp:260
ExportNLPSolver::state
ExportVariable state
Definition: export_nlp_solver.hpp:242
ExportVariable::getSubMatrix
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
Definition: export_variable.cpp:425
ExportNLPSolver::evGx
ExportVariable evGx
Definition: export_nlp_solver.hpp:249
Function::getExpression
returnValue getExpression(Expression &expression) const
Definition: function.cpp:139
ExportNLPSolver::acA
ExportVariable acA
Definition: export_nlp_solver.hpp:338
DISCRETIZATION_TYPE
Definition: acado_types.hpp:383
ExportIndex
Defines a scalar-valued index variable to be used for exporting code.
Definition: export_index.hpp:54
ExportNLPSolver::weightingMatricesType
unsigned weightingMatricesType(void) const
Definition: export_nlp_solver.cpp:1924
VariableType
VariableType
Definition: acado_types.hpp:95
ExportNLPSolver::evaluateStageCost
ExportAcadoFunction evaluateStageCost
Definition: export_nlp_solver.hpp:270
NE_ZERO
Definition: acado_types.hpp:66
ExportNLPSolver::acXTilde
ExportVariable acXTilde
Definition: export_nlp_solver.hpp:340
INEXACT
Definition: integrator_export_types.hpp:72
ExportNLPSolver::acb
ExportVariable acb
Definition: export_nlp_solver.hpp:338
Constraint::getBounds
virtual returnValue getBounds(const OCPiterate &iter)
Definition: constraint.cpp:1211
SPARSE_QP_SOLUTION
Definition: acado_types.hpp:410
Expression
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
ExportAlgorithm
Allows to export automatically generated algorithms for fast model predictive control.
Definition: export_algorithm.hpp:61
ExportNLPSolver::evaluateTerminalCost
ExportAcadoFunction evaluateTerminalCost
Definition: export_nlp_solver.hpp:271
ExportNLPSolver::usingLinearTerms
bool usingLinearTerms() const
Definition: export_nlp_solver.cpp:1597
RET_INVALID_OBJECTIVE_FOR_CODE_EXPORT
Definition: acado_types.hpp:1104
DYNAMIC_SENSITIVITY
Definition: acado_types.hpp:380
CG_USE_ARRIVAL_COST
Definition: acado_types.hpp:338
ExportFunction::setup
ExportFunction & setup(const std::string &_name="defaultFunctionName", const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
Definition: export_function.cpp:101
YES
#define YES
Definition: acado_types.hpp:51
ExportData::setDoc
virtual returnValue setDoc(const std::string &_doc)
Definition: export_data.cpp:140
ExportNLPSolver::setupInitialization
virtual returnValue setupInitialization()
Definition: export_nlp_solver.cpp:165
ExportNLPSolver::setupGetObjective
virtual returnValue setupGetObjective()
Definition: export_nlp_solver.cpp:1777
ExportNLPSolver::updateArrivalCost
ExportFunction updateArrivalCost
Definition: export_nlp_solver.hpp:331
ExportStruct
ExportStruct
Definition: acado_types.hpp:802
ExportNLPSolver::modelSimulation
ExportFunction modelSimulation
Definition: export_nlp_solver.hpp:240
Function::getGlobalExportVariableSize
unsigned getGlobalExportVariableSize() const
Definition: function.cpp:600
ExportNLPSolver::acVL
ExportVariable acVL
Definition: export_nlp_solver.hpp:340
ExportFunction::doc
virtual ExportFunction & doc(const std::string &_doc)
Definition: export_function.cpp:375
ExportAlgorithm::getNY
uint getNY() const
Definition: export_algorithm.cpp:138
ExportNLPSolver::objEvFxxEnd
ExportVariable objEvFxxEnd
Definition: export_nlp_solver.hpp:267
ExportNLPSolver::getNumPathConstraints
unsigned getNumPathConstraints(void)
Definition: export_nlp_solver.cpp:1584
ExportNLPSolver::cholSAC
ExportCholeskyDecomposition cholSAC
Definition: export_nlp_solver.hpp:334
ExportVariable::getCols
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:417
INT
Definition: acado_types.hpp:795
ExportNLPSolver::getObjective
ExportFunction getObjective
Definition: export_nlp_solver.hpp:325
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
Objective::getNumMayerTerms
uint getNumMayerTerms() const
Definition: objective.cpp:614
EXACT_HESSIAN
Definition: acado_types.hpp:619
GenericVector::getDim
unsigned getDim() const
Definition: vector.hpp:172
ExportNLPSolver::setupGetGeneralObjective
virtual returnValue setupGetGeneralObjective()
Definition: export_nlp_solver.cpp:1876
Function::getDim
int getDim() const
ExportVariable::makeRowVector
ExportVariable makeRowVector() const
Definition: export_variable.cpp:442
ExportNLPSolver::objValueIn
ExportVariable objValueIn
Definition: export_nlp_solver.hpp:269
ExportNLPSolver::initializeNodes
ExportFunction initializeNodes
Definition: export_nlp_solver.hpp:326
LsqElements
std::vector< LsqData > LsqElements
Definition: objective.hpp:62
CG_USE_VARIABLE_WEIGHTING_MATRIX
Definition: acado_types.hpp:340
ExportNLPSolver::acHx
ExportVariable acHx
Definition: export_nlp_solver.hpp:340
ExportNLPSolver::pacEvHxd
ExportVariable pacEvHxd
Definition: export_nlp_solver.hpp:301
ExportNLPSolver::performsSingleShooting
bool performsSingleShooting() const
Definition: export_nlp_solver.cpp:78
ExportNLPSolver::QN2
ExportVariable QN2
Definition: export_nlp_solver.hpp:276
ExportNLPSolver::setupSimulation
virtual returnValue setupSimulation(void)
Definition: export_nlp_solver.cpp:193
ExportData::getDataStruct
ExportStruct getDataStruct() const
Definition: export_data.cpp:109
NO
#define NO
Definition: acado_types.hpp:53
VariablesGrid::init
returnValue init()
Definition: variables_grid.cpp:208
ExportNLPSolver::QN1
ExportVariable QN1
Definition: export_nlp_solver.hpp:276
ExportFunction::setPrivate
virtual ExportFunction & setPrivate(bool _set=true)
Definition: export_function.cpp:382
ExportNLPSolver::evaluatePointConstraints
std::vector< std::shared_ptr< ExportAcadoFunction > > evaluatePointConstraints
Definition: export_nlp_solver.hpp:308
UserInteraction
Encapsulates all user interaction for setting options, logging data and plotting results.
Definition: user_interaction.hpp:57
OCP
Data class for defining optimal control problems.
Definition: ocp.hpp:89
ExportVariable::getDim
virtual uint getDim() const
Definition: export_variable.cpp:245
SYMMETRIC_FB
Definition: integrator_export_types.hpp:71
ExportNLPSolver::shiftControls
ExportFunction shiftControls
Definition: export_nlp_solver.hpp:324
Objective::getMayerTerm
returnValue getMayerTerm(uint index, Function &mayerTerm) const
Definition: objective.cpp:620
ExportData::setDataStruct
returnValue setDataStruct(ExportStruct _dataStruct)
Definition: export_data.cpp:80
ExportStatementBlock::addStatement
returnValue addStatement(const ExportStatement &_statement)
Definition: export_statement_block.cpp:91
v
#define v
VT_UNKNOWN
Definition: acado_types.hpp:110
ExportNLPSolver::SAC
ExportVariable SAC
Definition: export_nlp_solver.hpp:343
CG_USE_OPENMP
Definition: acado_types.hpp:339
ExportData::getFullName
std::string getFullName() const
Definition: export_data.cpp:121
Eigen::PlainObjectBase::data
EIGEN_STRONG_INLINE const Scalar * data() const
Definition: PlainObjectBase.h:209
REAL
Definition: acado_types.hpp:796
Expression::transpose
Expression transpose() const
Definition: expression.cpp:811
ExportStatementBlock::addLinebreak
returnValue addLinebreak(uint num=1)
Definition: export_statement_block.cpp:209
ExportNLPSolver::setGeneralObjective
returnValue setGeneralObjective(const Objective &_objective)
Definition: export_nlp_solver.cpp:456
ExportNLPSolver::evaluatePathConstraints
ExportAcadoFunction evaluatePathConstraints
Definition: export_nlp_solver.hpp:293
ExportAlgorithm::getNYN
uint getNYN() const
Definition: export_algorithm.cpp:148
GenericVector::append
GenericVector & append(const GenericVector &_arg)
Definition: vector.cpp:42
ACADOWARNINGTEXT
#define ACADOWARNINGTEXT(retval, text)
Definition: acado_message_handling.hpp:126
ExportNLPSolver::setupObjectiveLinearTerms
returnValue setupObjectiveLinearTerms(const Objective &_objective)
Definition: export_nlp_solver.cpp:1150
ExportNLPSolver::setIntegratorExport
returnValue setIntegratorExport(IntegratorExportPtr const _integrator)
Definition: export_nlp_solver.cpp:55
ExportNLPSolver::acHu
ExportVariable acHu
Definition: export_nlp_solver.hpp:340
ExportFunction::setReturnValue
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
Definition: export_function.cpp:141
row
RowXpr row(Index i)
Definition: BlockMethods.h:725
ExportFunction::acquire
virtual ExportFunction & acquire(ExportIndex &obj)
Definition: export_function.cpp:354
ExportNLPSolver::conAuxVar
ExportVariable conAuxVar
Definition: export_nlp_solver.hpp:294
export_nlp_solver.hpp
Expression::getVariableType
VariableType getVariableType() const
ExportNLPSolver::objEvFuu
ExportVariable objEvFuu
Definition: export_nlp_solver.hpp:267
ExportFunction::addVariable
ExportFunction & addVariable(const ExportVariable &_var)
Definition: export_function.cpp:368
ExportNLPSolver::objAuxVar
ExportVariable objAuxVar
Definition: export_nlp_solver.hpp:269
ExportNLPSolver::ExportNLPSolver
ExportNLPSolver(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: export_nlp_solver.cpp:41
Grid::getNumPoints
uint getNumPoints() const
ExportAcadoFunction::setGlobalExportVariable
returnValue setGlobalExportVariable(const ExportVariable &var)
Definition: export_acado_function.cpp:185
ExportAlgorithm::setNY
void setNY(uint NY_)
Definition: export_algorithm.cpp:133
ExportNLPSolver::objEvFxu
ExportVariable objEvFxu
Definition: export_nlp_solver.hpp:267
ACADO_LOCAL
Definition: acado_types.hpp:808
ExportNLPSolver::acSolver
ExportHouseholderQR acSolver
Definition: export_nlp_solver.hpp:336
ExportNLPSolver::evGu
ExportVariable evGu
Definition: export_nlp_solver.hpp:250
ExportNLPSolver::objSEndTerm
ExportVariable objSEndTerm
Definition: export_nlp_solver.hpp:265
ExportVariable::getRows
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:409
SYMMETRIC
Definition: integrator_export_types.hpp:70
ExportStatementBlock::addDeclaration
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
Definition: export_statement_block.cpp:158
ExportNLPSolver::setupAuxiliaryFunctions
returnValue setupAuxiliaryFunctions()
Definition: export_nlp_solver.cpp:1605
ExportNLPSolver::getNumComplexConstraints
unsigned getNumComplexConstraints(void)
Definition: export_nlp_solver.cpp:1579
MatrixVariablesGrid::getLowerBounds
DVector getLowerBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1344
LsqExternElements
std::vector< LsqExternData > LsqExternElements
Definition: objective.hpp:81
Objective::getLagrangeTerm
returnValue getLagrangeTerm(uint index, Function &lagrangeTerm) const
Definition: objective.cpp:633
OCP::getGrid
returnValue getGrid(Grid &grid_) const
Definition: ocp.cpp:329
ExportLinearSolver::getNameSolveFunction
const std::string getNameSolveFunction()
Definition: linear_solver_export.cpp:175
Expression::getSubMatrix
Expression getSubMatrix(const uint &rowIdx1, const uint &rowIdx2, const uint &colIdx1, const uint &colIdx2) const
Definition: expression.cpp:601
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
ExpressionType::clearStaticCounters
returnValue clearStaticCounters()
Definition: expression.hpp:398
isFinite
BooleanType isFinite(const T &_value)
Definition: acado_utils.hpp:119
LIFTED_GRADIENT_UPDATE
Definition: acado_types.hpp:346
Function::isOneOrZero
NeutralElement isOneOrZero()
Definition: function.cpp:307
ExportNLPSolver::pocEvHxd
ExportVariable pocEvHxd
Definition: export_nlp_solver.hpp:314
ExportNLPSolver::pocUbStack
std::vector< DVector > pocUbStack
Definition: export_nlp_solver.hpp:311
ExportNLPSolver::setObjective
returnValue setObjective(const Objective &_objective)
Definition: export_nlp_solver.cpp:445
col
ColXpr col(Index i)
Definition: BlockMethods.h:708
Function::isConstant
BooleanType isConstant()
Definition: function.cpp:360
ExportNLPSolver::objEvFu
ExportVariable objEvFu
Definition: export_nlp_solver.hpp:266
RET_NOT_YET_IMPLEMENTED
Definition: acado_types.hpp:847
ExportNLPSolver::setupResidualVariables
returnValue setupResidualVariables()
Definition: export_nlp_solver.cpp:1214
RET_INITIALIZE_FIRST
Definition: acado_types.hpp:1053
ExportCholeskyDecomposition::init
returnValue init(const std::string &_name, unsigned _dim, bool _unrolling=false)
Definition: export_cholesky_decomposition.cpp:48
ExportNLPSolver::pacEvH
ExportVariable pacEvH
Definition: export_nlp_solver.hpp:300
ExportNLPSolver::cholObjS
ExportCholeskyDecomposition cholObjS
Definition: export_nlp_solver.hpp:333
CG_FORCE_DIAGONAL_HESSIAN
Definition: acado_types.hpp:333
ExportNLPSolver::pocEvHu
ExportVariable pocEvHu
Definition: export_nlp_solver.hpp:314
SPARSE_SOLVER
Definition: acado_types.hpp:757
ExportNLPSolver::pacEvDDH
ExportVariable pacEvDDH
Definition: export_nlp_solver.hpp:302
ExportAlgorithm::getNU
uint getNU() const
Definition: export_algorithm.cpp:111
ExportStatementBlock
Allows to export code for a block of statements.
Definition: export_statement_block.hpp:63
ExportAlgorithm::setNYN
void setNYN(uint NYN_)
Definition: export_algorithm.cpp:143
ExportNLPSolver::initialize
ExportFunction initialize
Definition: export_nlp_solver.hpp:321
S
#define S
Definition: rocket_with_templates_ocp.hpp:41
GenericMatrix::getRows
GenericMatrix getRows(unsigned _start, unsigned _end) const
Definition: matrix.hpp:235
ExportLinearSolver::init
returnValue init(const uint newDim, const bool &reuse=true, const bool &unrolling=false)
Definition: linear_solver_export.cpp:62
ExportNLPSolver::objValueOut
ExportVariable objValueOut
Definition: export_nlp_solver.hpp:269
ExportNLPSolver::acXx
ExportVariable acXx
Definition: export_nlp_solver.hpp:340
ExportVariable::getCol
ExportVariable getCol(const ExportIndex &idx) const
Definition: export_variable.cpp:402
ExportAcadoFunction::init
returnValue init(const Function &_f, const std::string &_name="acadoFcn", const uint _numX=0, const uint _numXA=0, const uint _numU=0, const uint _numP=0, const uint _numDX=0, const uint _numOD=0)
Definition: export_acado_function.cpp:83
Objective
Stores and evaluates the objective function of optimal control problems.
Definition: objective.hpp:123
GenericMatrix::getRow
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
Objective::getLSQTerms
returnValue getLSQTerms(LsqElements &_elements) const
Definition: objective.cpp:579
ExportFunction::addIndex
ExportFunction & addIndex(const ExportIndex &_index)
Definition: export_function.cpp:347
ExportData::getDataStructString
std::string getDataStructString() const
Definition: export_data.cpp:115
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106
ExportVariable
Defines a matrix-valued variable to be used for exporting code.
Definition: export_variable.hpp:58
ACADOERRORTEXT
#define ACADOERRORTEXT(retval, text)
Definition: acado_message_handling.hpp:110
ExportStatementBlock::addFunctionCall
returnValue addFunctionCall(const std::string &_fName, const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
Definition: export_statement_block.cpp:115
ExportHouseholderQR::getGlobalExportVariable
virtual ExportVariable getGlobalExportVariable(const uint factor) const
Definition: householder_qr_export.cpp:338
BACKWARD
Definition: integrator_export_types.hpp:68
ExportData::getName
std::string getName() const
Definition: export_data.cpp:86
Expression::getDim
uint getDim() const

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:34