export_gauss_newton_cn2_factorization.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_gauss_newton_cn2_factorization.hpp>
33 #include <acado/code_generation/export_qpoases_interface.hpp>
34 #include <acado/code_generation/export_qpoases3_interface.hpp>
35 
36 using namespace std;
37 
38 BEGIN_NAMESPACE_ACADO
39 
40 ExportGaussNewtonCn2Factorization::ExportGaussNewtonCn2Factorization( UserInteraction* _userInteraction,
41  const std::string& _commonHeaderName
42  ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
43 {}
44 
45 returnValue ExportGaussNewtonCn2Factorization::setup( )
46 {
47  if (performFullCondensing() == false || initialStateFixed() == false || getNumComplexConstraints() > 0)
48  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
49  if (performsSingleShooting() == true)
50  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
51 
52  LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
53  setupInitialization();
54  LOG( LVL_DEBUG ) << "done!" << endl;
55 
56  LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
57  setupVariables();
58  LOG( LVL_DEBUG ) << "done!" << endl;
59 
60  LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
61  setupMultiplicationRoutines();
62  LOG( LVL_DEBUG ) << "done!" << endl;
63 
64  LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
65  setupSimulation();
66  LOG( LVL_DEBUG ) << "done!" << endl;
67 
68  LOG( LVL_DEBUG ) << "Solver: setup objective evaluation... " << endl;
69  setupObjectiveEvaluation();
70  LOG( LVL_DEBUG ) << "done!" << endl;
71 
72  LOG( LVL_DEBUG ) << "Solver: setup condensing... " << endl;
73  setupCondensing();
74  LOG( LVL_DEBUG ) << "done!" << endl;
75 
76  LOG( LVL_DEBUG ) << "Solver: setup constraints... " << endl;
77  setupConstraintsEvaluation();
78  LOG( LVL_DEBUG ) << "done!" << endl;
79 
80  LOG( LVL_DEBUG ) << "Solver: setup evaluation... " << endl;
81  setupEvaluation();
82  LOG( LVL_DEBUG ) << "done!" << endl;
83 
84  LOG( LVL_DEBUG ) << "Solver: setup auxiliary functions... " << endl;
85  setupAuxiliaryFunctions();
86  LOG( LVL_DEBUG ) << "done!" << endl;
87 
88  return SUCCESSFUL_RETURN;
89 }
90 
91 returnValue ExportGaussNewtonCn2Factorization::getDataDeclarations( ExportStatementBlock& declarations,
92  ExportStruct dataStruct
93  ) const
94 {
95  ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
96 
97  declarations.addDeclaration(sbar, dataStruct);
98  declarations.addDeclaration(x0, dataStruct);
99  declarations.addDeclaration(Dx0, dataStruct);
100 
101  declarations.addDeclaration(W1, dataStruct);
102  declarations.addDeclaration(W2, dataStruct);
103 
104  declarations.addDeclaration(D, dataStruct);
105  declarations.addDeclaration(L, dataStruct);
106 
107  declarations.addDeclaration(T1, dataStruct);
108  declarations.addDeclaration(T2, dataStruct);
109  declarations.addDeclaration(T3, dataStruct);
110 
111  declarations.addDeclaration(E, dataStruct);
112  declarations.addDeclaration(F, dataStruct);
113 
114  declarations.addDeclaration(QDy, dataStruct);
115  declarations.addDeclaration(w1, dataStruct);
116  declarations.addDeclaration(w2, dataStruct);
117 
118  declarations.addDeclaration(lbValues, dataStruct);
119  declarations.addDeclaration(ubValues, dataStruct);
120  declarations.addDeclaration(lbAValues, dataStruct);
121  declarations.addDeclaration(ubAValues, dataStruct);
122 
123  if (performFullCondensing() == true)
124  declarations.addDeclaration(A10, dataStruct);
125  declarations.addDeclaration(A20, dataStruct);
126 
127  declarations.addDeclaration(pacA01Dx0, dataStruct);
128  declarations.addDeclaration(pocA02Dx0, dataStruct);
129 
130  declarations.addDeclaration(H, dataStruct);
131  declarations.addDeclaration(U, dataStruct);
132  declarations.addDeclaration(A, dataStruct);
133  declarations.addDeclaration(g, dataStruct);
134  declarations.addDeclaration(lb, dataStruct);
135  declarations.addDeclaration(ub, dataStruct);
136  declarations.addDeclaration(lbA, dataStruct);
137  declarations.addDeclaration(ubA, dataStruct);
138  declarations.addDeclaration(xVars, dataStruct);
139  declarations.addDeclaration(yVars, dataStruct);
140 
141  return SUCCESSFUL_RETURN;
142 }
143 
144 returnValue ExportGaussNewtonCn2Factorization::getFunctionDeclarations( ExportStatementBlock& declarations
145  ) const
146 {
147  declarations.addDeclaration( preparation );
148  declarations.addDeclaration( feedback );
149 
150  declarations.addDeclaration( initialize );
151  declarations.addDeclaration( initializeNodes );
152  declarations.addDeclaration( shiftStates );
153  declarations.addDeclaration( shiftControls );
154  declarations.addDeclaration( getKKT );
155  declarations.addDeclaration( getObjective );
156 
157  declarations.addDeclaration( evaluateStageCost );
158  declarations.addDeclaration( evaluateTerminalCost );
159 
160  return SUCCESSFUL_RETURN;
161 }
162 
163 returnValue ExportGaussNewtonCn2Factorization::getCode( ExportStatementBlock& code
164  )
165 {
166  setupQPInterface();
167 
168  code.addLinebreak( 2 );
169  code.addStatement( "/******************************************************************************/\n" );
170  code.addStatement( "/* */\n" );
171  code.addStatement( "/* ACADO code generation */\n" );
172  code.addStatement( "/* */\n" );
173  code.addStatement( "/******************************************************************************/\n" );
174  code.addLinebreak( 2 );
175 
176  int useOMP;
177  get(CG_USE_OPENMP, useOMP);
178  if ( useOMP )
179  {
180  code.addDeclaration( state );
181  }
182 
183  code.addFunction( modelSimulation );
184 
185  code.addFunction( evaluateStageCost );
186  code.addFunction( evaluateTerminalCost );
187 
188  code.addFunction( setObjQ1Q2 );
189  code.addFunction( setObjR1R2 );
190  code.addFunction( setObjQN1QN2 );
191  code.addFunction( evaluateObjective );
192 
193 // code.addFunction( multGxd );
194  code.addFunction( moveGxT );
195 // code.addFunction( multGxGx );
196  code.addFunction( multGxGu );
197  code.addFunction( moveGuE );
198 
199  code.addFunction( multBTW1 );
200  code.addFunction( macBTW1_R1 );
201  code.addFunction( multGxTGu );
202  code.addFunction( macQEW2 );
203 
204  code.addFunction( mult_H_W2T_W3 );
205  code.addFunction( mac_H_W2T_W3_R );
206  code.addFunction( mac_W3_G_W1T_G );
207 
208  code.addFunction( mac_R_T2_B_D );
209  code.addFunction( move_D_U );
210  code.addFunction( mult_L_E_U );
211  code.addFunction( updateQ );
212  code.addFunction( mul_T2_A_L );
213  code.addFunction( mult_BT_T1_T2 );
214 
215 // ExportFunction mac_R_BT_F_D, mult_FT_A_L;
216 // ExportFunction updateQ2;
217 // ExportFunction mac_W1_T1_E_F;
218 
219  code.addFunction( mac_R_BT_F_D );
220  code.addFunction( mult_FT_A_L );
221  code.addFunction( updateQ2 );
222  code.addFunction( mac_W1_T1_E_F );
223  code.addFunction( move_GxT_T3 );
224 
225  cholSolver.getCode( code );
226 
227 // ExportFunction multATw1Q, macBTw1, macQSbarW2, macASbarD;
228 
229  code.addFunction( macATw1QDy );
230  code.addFunction( macBTw1 );
231  code.addFunction( macQSbarW2 );
232  code.addFunction( macASbar );
233 // code.addFunction( macASbarD2 );
234  code.addFunction( expansionStep );
235 
236 // code.addFunction( setBlockH11 );
237 // code.addFunction( zeroBlockH11 );
238  code.addFunction( copyHTH );
239 // code.addFunction( multQ1d );
240 // code.addFunction( multQN1d );
241  code.addFunction( multRDy );
242  code.addFunction( multQDy );
243 // code.addFunction( multEQDy );
244 // code.addFunction( multQETGx );
245 // code.addFunction( zeroBlockH10 );
246 // code.addFunction( multEDu );
247 // code.addFunction( multQ1Gx );
248 // code.addFunction( multQN1Gx );
249  code.addFunction( multQ1Gu );
250  code.addFunction( multQN1Gu );
251 // code.addFunction( zeroBlockH00 );
252 // code.addFunction( multCTQC );
253 
254  code.addFunction( multHxC );
255  code.addFunction( multHxE );
256  code.addFunction( macHxd );
257 
258  code.addFunction( evaluatePathConstraints );
259 
260  for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
261  {
262  if (evaluatePointConstraints[ i ] == 0)
263  continue;
264  code.addFunction( *evaluatePointConstraints[ i ] );
265  }
266 
267  code.addFunction( macCTSlx );
268  code.addFunction( macETSlu );
269 
270  code.addFunction( condensePrep );
271  code.addFunction( condenseFdb );
272  code.addFunction( expand );
273 
274  code.addFunction( preparation );
275  code.addFunction( feedback );
276 
277  code.addFunction( initialize );
278  code.addFunction( initializeNodes );
279  code.addFunction( shiftStates );
280  code.addFunction( shiftControls );
281  code.addFunction( getKKT );
282  code.addFunction( getObjective );
283 
284  return SUCCESSFUL_RETURN;
285 }
286 
287 
288 unsigned ExportGaussNewtonCn2Factorization::getNumQPvars( ) const
289 {
290  if (performFullCondensing() == true)
291  return (N * NU);
292 
293  return (NX + N * NU);
294 }
295 
296 unsigned ExportGaussNewtonCn2Factorization::getNumStateBounds() const
297 {
298  return xBoundsIdxRev.size();
299 }
300 
301 //
302 // PROTECTED FUNCTIONS:
303 //
304 
305 returnValue ExportGaussNewtonCn2Factorization::setupObjectiveEvaluation( void )
306 {
307  evaluateObjective.setup("evaluateObjective");
308 
309  int variableObjS;
310  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
311 
312  //
313  // A loop the evaluates objective and corresponding gradients
314  //
315  ExportIndex runObj( "runObj" );
316  ExportForLoop loopObjective( runObj, 0, N );
317 
318  evaluateObjective.addIndex( runObj );
319 
320  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
321  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
322  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
323  loopObjective.addLinebreak( );
324 
325  // Evaluate the objective function
326  loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
327 
328  // Stack the measurement function value
329  loopObjective.addStatement(
330  Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
331  );
332  loopObjective.addLinebreak( );
333 
334  // Optionally compute derivatives
335  unsigned indexX = getNY();
336 // unsigned indexG = indexX;
337 
338  ExportVariable tmpObjS, tmpFx, tmpFu;
339  ExportVariable tmpFxEnd, tmpObjSEndTerm;
340  tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
341  if (objS.isGiven() == true)
342  tmpObjS = objS;
343  tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
344  if (objEvFx.isGiven() == true)
345  tmpFx = objEvFx;
346  tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
347  if (objEvFu.isGiven() == true)
348  tmpFu = objEvFu;
349  tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
350  if (objEvFxEnd.isGiven() == true)
351  tmpFxEnd = objEvFxEnd;
352  tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
353  if (objSEndTerm.isGiven() == true)
354  tmpObjSEndTerm = objSEndTerm;
355 
356  //
357  // Optional computation of Q1, Q2
358  //
359  if (Q1.isGiven() == false)
360  {
361  ExportVariable tmpQ1, tmpQ2;
362  tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
363  tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
364 
365  setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
366  setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
367  setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
368 
369  if (tmpFx.isGiven() == true)
370  {
371  if (variableObjS == YES)
372  {
373  loopObjective.addFunctionCall(
374  setObjQ1Q2,
375  tmpFx, objS.getAddress(runObj * NY, 0),
376  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
377  );
378  }
379  else
380  {
381  loopObjective.addFunctionCall(
382  setObjQ1Q2,
383  tmpFx, objS,
384  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
385  );
386  }
387  }
388  else
389  {
390  if (variableObjS == YES)
391  {
392  if (objEvFx.isGiven() == true)
393 
394  loopObjective.addFunctionCall(
395  setObjQ1Q2,
396  objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
397  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
398  );
399  }
400  else
401  {
402  loopObjective.addFunctionCall(
403  setObjQ1Q2,
404  objValueOut.getAddress(0, indexX), objS,
405  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
406  );
407  }
408  indexX += objEvFx.getDim();
409  }
410 
411  loopObjective.addLinebreak( );
412  }
413 
414  if (R1.isGiven() == false)
415  {
416  ExportVariable tmpR1, tmpR2;
417  tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
418  tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
419 
420  setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
421  setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
422  setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
423 
424  if (tmpFu.isGiven() == true)
425  {
426  if (variableObjS == YES)
427  {
428  loopObjective.addFunctionCall(
429  setObjR1R2,
430  tmpFu, objS.getAddress(runObj * NY, 0),
431  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
432  );
433  }
434  else
435  {
436  loopObjective.addFunctionCall(
437  setObjR1R2,
438  tmpFu, objS,
439  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
440  );
441  }
442  }
443  else
444  {
445  if (variableObjS == YES)
446  {
447  loopObjective.addFunctionCall(
448  setObjR1R2,
449  objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
450  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
451  );
452  }
453  else
454  {
455  loopObjective.addFunctionCall(
456  setObjR1R2,
457  objValueOut.getAddress(0, indexX), objS,
458  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
459  );
460  }
461  }
462 
463  loopObjective.addLinebreak( );
464  }
465 
466  evaluateObjective.addStatement( loopObjective );
467 
468  //
469  // Evaluate the quadratic Mayer term
470  //
471  evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
472  evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
473 
474  // Evaluate the objective function
475  evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
476  evaluateObjective.addLinebreak( );
477 
478  evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
479  evaluateObjective.addLinebreak();
480 
481  if (QN1.isGiven() == false)
482  {
483  indexX = getNYN();
484 
485  ExportVariable tmpQN1, tmpQN2;
486  tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
487  tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
488 
489  setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
490  setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
491  setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
492 
493  if (tmpFxEnd.isGiven() == true)
494  evaluateObjective.addFunctionCall(
495  setObjQN1QN2,
496  tmpFxEnd, objSEndTerm,
497  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
498  );
499  else
500  evaluateObjective.addFunctionCall(
501  setObjQN1QN2,
502  objValueOut.getAddress(0, indexX), objSEndTerm,
503  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
504  );
505 
506  evaluateObjective.addLinebreak( );
507  }
508 
509  return SUCCESSFUL_RETURN;
510 }
511 
512 returnValue ExportGaussNewtonCn2Factorization::setupConstraintsEvaluation( void )
513 {
514  ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
515 
516  int hardcodeConstraintValues;
517  get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
518 
520  //
521  // Determine dimensions of constraints
522  //
524 
525  unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
526  unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
527  unsigned numStateBounds = getNumStateBounds();
528 // unsigned numPathCon = N * dimPacH;
529 // unsigned numPointCon = dimPocH;
530 
532  //
533  // Setup the bounds on independent variables
534  //
536 
537  DVector lbBoundValues( numBounds );
538  DVector ubBoundValues( numBounds );
539 
540  if (initialStateFixed( ) == false)
541  for(unsigned el = 0; el < NX; ++el)
542  {
543  lbBoundValues( el )= xBounds.getLowerBound(0, el);
544  ubBoundValues( el ) = xBounds.getUpperBound(0, el);
545  }
546 
547  // UPDATED
548  for(unsigned node = 0; node < N; ++node)
549  for(unsigned el = 0; el < NU; ++el)
550  {
551  lbBoundValues(offsetBounds + (N - 1 - node) * NU + el) = uBounds.getLowerBound(node, el);
552  ubBoundValues(offsetBounds + (N - 1 - node) * NU + el) = uBounds.getUpperBound(node, el);
553  }
554 
555  if (hardcodeConstraintValues == YES)
556  {
557  lbValues.setup("lbValues", lbBoundValues, REAL, ACADO_VARIABLES);
558  ubValues.setup("ubValues", ubBoundValues, REAL, ACADO_VARIABLES);
559  }
560  else
561  {
562  lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
563  lbValues.setDoc( "Lower bounds values." );
564  ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
565  ubValues.setDoc( "Upper bounds values." );
566  }
567 
568  ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
569 
570  if (performFullCondensing() == true)
571  {
572  ExportVariable uCol = u.makeColVector();
573  // Full condensing case
574  for (unsigned blk = 0; blk < N; ++blk)
575  {
576  boundSetFcn->addStatement( lb.getRows(blk * NU, (blk + 1) * NU) ==
577  lbValues.getRows(blk * NU, (blk + 1) * NU) - uCol.getRows((N - 1 - blk) * NU, (N - blk) * NU));
578  boundSetFcn->addStatement( ub.getRows(blk * NU, (blk + 1) * NU) ==
579  ubValues.getRows(blk * NU, (blk + 1) * NU) - uCol.getRows((N - 1 - blk) * NU, (N - blk) * NU));
580  }
581  }
582  else
583  {
584  // Partial condensing case
585  if (initialStateFixed() == true)
586  {
587  // MPC case
588  condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
589  condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
590 
591  boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
592  boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
593  }
594  else
595  {
596  // MHE case
597  boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
598  boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
599  boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
600  boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
601  }
602  }
603  boundSetFcn->addLinebreak( );
604 
606  //
607  // Determine number of affine constraints and set up structures that
608  // holds constraint values
609  //
611 
612  unsigned sizeA = numStateBounds + getNumComplexConstraints();
613 
614  if ( sizeA )
615  {
616  if (hardcodeConstraintValues == true)
617  {
618  DVector lbTmp, ubTmp;
619 
620  if ( numStateBounds )
621  {
622  DVector lbStateBoundValues( numStateBounds );
623  DVector ubStateBoundValues( numStateBounds );
624  for (unsigned i = 0; i < numStateBounds; ++i)
625  {
626  lbStateBoundValues( i ) = xBounds.getLowerBound( xBoundsIdxRev[ i ] / NX, xBoundsIdxRev[ i ] % NX );
627  ubStateBoundValues( i ) = xBounds.getUpperBound( xBoundsIdxRev[ i ] / NX, xBoundsIdxRev[ i ] % NX );
628  }
629 
630  lbTmp.append( lbStateBoundValues );
631  ubTmp.append( ubStateBoundValues );
632  }
633 
634  lbTmp.append( lbPathConValues );
635  ubTmp.append( ubPathConValues );
636 
637  lbTmp.append( lbPointConValues );
638  ubTmp.append( ubPointConValues );
639 
640 
641  lbAValues.setup("lbAValues", lbTmp, REAL, ACADO_VARIABLES);
642  ubAValues.setup("ubAValues", ubTmp, REAL, ACADO_VARIABLES);
643  }
644  else
645  {
646  lbAValues.setup("lbAValues", sizeA, 1, REAL, ACADO_VARIABLES);
647  lbAValues.setDoc( "Lower bounds values for affine constraints." );
648  ubAValues.setup("ubAValues", sizeA, 1, REAL, ACADO_VARIABLES);
649  ubAValues.setDoc( "Lower bounds values for affine constraints." );
650  }
651  }
652 
654  //
655  // Setup the bounds on state variables
656  //
658 
659  if ( numStateBounds )
660  {
661  condenseFdb.addVariable( tmp );
662 
663  unsigned offset = (performFullCondensing() == true) ? 0 : NX;
664  unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
665 
666  if (numOps < 1024)
667  {
668  for(unsigned row = 0; row < numStateBounds; ++row)
669  {
670  unsigned conIdx = xBoundsIdx[ row ];
671  unsigned blk = conIdx / NX;
672 
673  // TODO
674 // if (performFullCondensing() == false)
675 // condensePrep.addStatement( A.getSubMatrix(ii, ii + 1, 0, NX) == evGx.getRow( conIdx ) );
676 
677  for (unsigned col = blk; col < N; ++col)
678  {
679  // blk = (N - row) * (N - 1 - row) / 2 + (N - 1 - col)
680  unsigned blkRow = ((N - blk) * (N - 1 - blk) / 2 + (N - 1 - col)) * NX + conIdx % NX;
681 
682  condensePrep.addStatement(
683  A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
684  }
685 
686  condensePrep.addLinebreak();
687  }
688  }
689  else
690  {
691  DMatrix vXBoundIndices(1, numStateBounds);
692  for (unsigned i = 0; i < numStateBounds; ++i)
693  vXBoundIndices(0, i) = xBoundsIdx[ i ];
694  ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
695 
696  condensePrep.addVariable( evXBounds );
697 
698  ExportIndex row, col, conIdx, blk, blkRow, blkIdx;
699 
700  condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow ).acquire( blkIdx );
701 
702  ExportForLoop lRow(row, 0, numStateBounds);
703 
704  lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ];\n";
705  lRow.addStatement( blk == conIdx / NX );
706 
707  // TODO
708 // if (performFullCondensing() == false)
709 // eLoopI.addStatement( A.getSubMatrix(ii, ii + 1, 0, NX) == evGx.getRow( conIdx ) );
710 
711  ExportForLoop lCol(col, blk, N);
712 
713  lCol.addStatement( blkIdx == (N - blk) * (N - 1 - blk) / 2 + (N - 1 - col) );
714  lCol.addStatement( blkRow == blkIdx * NX + conIdx % NX );
715  lCol.addStatement(
716  A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
717 
718  lRow.addStatement( lCol );
719  condensePrep.addStatement( lRow );
720 
721  condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow ).release( blkIdx );
722  }
723  condensePrep.addLinebreak( );
724 
725  // Shift constraint bounds by first interval
726  // MPC case, only
727  ExportVariable xVec = x.makeRowVector();
728  for(unsigned row = 0; row < getNumStateBounds( ); ++row)
729  {
730  unsigned conIdx = xBoundsIdxRev[ row ];
731 
732  condenseFdb.addStatement( tmp == sbar.getRow( conIdx ) + xVec.getCol( conIdx ) );
733  condenseFdb.addStatement( lbA.getRow( row ) == lbAValues( row ) - tmp );
734  condenseFdb.addStatement( ubA.getRow( row ) == ubAValues( row ) - tmp );
735  }
736  condenseFdb.addLinebreak( );
737  }
738 
739  return SUCCESSFUL_RETURN;
740 }
741 
742 returnValue ExportGaussNewtonCn2Factorization::setupCondensing( void )
743 {
744  condensePrep.setup("condensePrep");
745  condenseFdb.setup( "condenseFdb" );
746 
748  //
749  // Compute Hessian block H11
750  //
752 
753  unsigned prepCacheSize =
754  2 * NX * NU +
755  NU * NU + NU * NX +
756  2 * NX * NX + NU * NX;
757 
758  int useSinglePrecision;
759  get(USE_SINGLE_PRECISION, useSinglePrecision);
760  prepCacheSize = prepCacheSize * (useSinglePrecision ? 4 : 8);
761  LOG( LVL_DEBUG ) << "---> Condensing prep. part, cache size: " << prepCacheSize << " bytes" << endl;
762 
763  ExportStruct prepCache = prepCacheSize < 16384 ? ACADO_LOCAL : ACADO_WORKSPACE;
764 
765  W1.setup("W1", NX, NU, REAL, prepCache);
766  W2.setup("W2", NX, NU, REAL, prepCache);
767 
768  D.setup("D", NU, NU, REAL, prepCache);
769  L.setup("L", NU, NX, REAL, prepCache);
770 
771  T1.setup("T1", NX, NX, REAL, prepCache);
772  T2.setup("T2", NU, NX, REAL, prepCache);
773  T3.setup("T3", NX, NX, REAL, prepCache);
774 
775  condensePrep
776  .addVariable( W1 ).addVariable( W2 )
777  .addVariable( D ).addVariable( L )
778  .addVariable( T1 ).addVariable( T2 ).addVariable( T3 );
779 
780  LOG( LVL_DEBUG ) << "---> Setup condensing: E" << endl;
782 
783  // Special case, row = col = 0
784  condensePrep.addFunctionCall(moveGuE, evGu.getAddress(0, 0), E.getAddress(0, 0) );
785 
786  if (N <= 15)
787  {
788  unsigned row, col, prev, curr;
789  for (row = 1; row < N; ++row)
790  {
791  condensePrep.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T1);
792 
793  for(col = 0; col < row; ++col)
794  {
795  prev = row * (row - 1) / 2 + col;
796  curr = (row + 1) * row / 2 + col;
797 
798  condensePrep.addFunctionCall(multGxGu, T1, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0));
799  }
800 
801  curr = (row + 1) * row / 2 + col;
802  condensePrep.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
803 
804  condensePrep.addLinebreak();
805  }
806  }
807  else
808  {
809  ExportIndex row, col, curr, prev;
810 
811  condensePrep.acquire( row ).acquire( col ).acquire( curr ).acquire( prev );
812 
813  ExportForLoop lRow(row, 1, N), lCol(col, 0, row);
814 
815  lRow.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T1);
816 
817  lCol.addStatement( prev == row * (row - 1) / 2 + col );
818  lCol.addStatement( curr == (row + 1) * row / 2 + col );
819  lCol.addFunctionCall( multGxGu, T1, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0) );
820 
821  lRow.addStatement( lCol );
822  lRow.addStatement( curr == (row + 1) * row / 2 + col );
823  lRow.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
824 
825  condensePrep.addStatement( lRow );
826 
827  condensePrep.release( row ).release( col ).release( curr ).release( prev );
828  }
829  condensePrep.addLinebreak( 2 );
830 
831  LOG( LVL_DEBUG ) << "---> Setup condensing: H11" << endl;
832 
833  /*
834 
835  Using some heuristics, W1, W2, W3 can be allocated on stack.
836 
837  The algorithm to create the Hessian becomes:
838 
839  for col = 0: N - 1
840  row = 0
841  W1 = Q(N - row) * E(N - 1 - row, N - 1 - col)
842 
843  for row = 0: col - 1
844  H(row, col) = B(N - 1 - row)^T * W1
845  W2 = A(N - (row + 1))^T * W1
846  W1 = W2 + Q(N - (row + 1)) * E(N - 1 - (row + 1), N - 1 - col)
847 
848  H(col, col) += R(N - 1 - col) + B(N - 1 - col)^T * W1
849 
850  Indexing of G matrix:
851  QE(row, col) -> block index is calculated as: blk = (row + 1) * row / 2 + col
852  G is with reversed rows and columns, say: G(row, col) = QE(N - 1 - row, N - 1 - col)
853  In more details, G(row, col) block index is calculated as:
854  blk = (N - row) * (N - 1 - row) / 2 + (N - 1 - col)
855 
856  */
857 
858  if (N <= 15)
859  {
860  for (unsigned col = 0; col < N; ++col)
861  {
862  // row = 0
863  unsigned curr = (N) * (N - 1) / 2 + (N - 1 - col);
864  if (QN1.isGiven() == true)
865  condensePrep.addFunctionCall(
866  multQN1Gu, E.getAddress(curr * NX), W1
867  );
868  else
869  condensePrep.addFunctionCall(
870  multGxGu, QN1, E.getAddress(curr * NX), W1
871  );
872 
873  for (unsigned row = 0; row < col; ++row)
874  {
875  condensePrep.addFunctionCall(
876  multBTW1, evGu.getAddress((N - 1 - row) * NX), W1, ExportIndex( row ), ExportIndex( col )
877  );
878 
879  condensePrep.addFunctionCall(
880  multGxTGu, evGx.getAddress((N - (row + 1)) * NX), W1, W2
881  );
882 
883  unsigned next = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
884  if (Q1.isGiven() == true)
885  condensePrep.addFunctionCall(
886  macQEW2, Q1, E.getAddress(next * NX), W2, W1
887  );
888  else
889  condensePrep.addFunctionCall(
890  macQEW2,
891  Q1.getAddress((N - (row + 1)) * NX), E.getAddress(next * NX), W2, W1
892  );
893  }
894 
895  if (R1.isGiven() == true)
896  condensePrep.addFunctionCall(
897  macBTW1_R1, R1, evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
898  );
899  else
900  condensePrep.addFunctionCall(
901  macBTW1_R1, R1.getAddress((N - 1 - col) * NU), evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
902  );
903 
904  condensePrep.addLinebreak();
905  }
906  }
907  else
908  {
909  // No loop unrolling...
910  ExportIndex row, col, curr, next;
911  condensePrep.acquire( row ).acquire( col ).acquire( curr ).acquire( next );
912 
913  ExportForLoop colLoop(col, 0, N);
914  colLoop << (curr == (N) * (N - 1) / 2 + (N - 1 - col));
915 
916  if (QN1.isGiven() == true)
917  colLoop.addFunctionCall(multQN1Gu, E.getAddress(curr * NX), W1);
918  else
919  colLoop.addFunctionCall(multGxGu, QN1, E.getAddress(curr * NX), W1);
920 
921  ExportForLoop rowLoop(row, 0, col);
922  rowLoop.addFunctionCall(
923  multBTW1, evGu.getAddress((N - 1 - row) * NX), W1, ExportIndex( row ), ExportIndex( col )
924  );
925 
926  rowLoop.addFunctionCall(
927  multGxTGu, evGx.getAddress((N - (row + 1)) * NX), W1, W2
928  );
929 
930  rowLoop << (next == (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col));
931  if (Q1.isGiven() == true)
932  rowLoop.addFunctionCall(macQEW2, Q1, E.getAddress(next * NX), W2, W1);
933  else
934  rowLoop.addFunctionCall(macQEW2, Q1.getAddress((N - (row + 1)) * NX), E.getAddress(next * NX), W2, W1);
935 
936  colLoop << rowLoop;
937 
938  if (R1.isGiven() == true)
939  colLoop.addFunctionCall(
940  macBTW1_R1, R1, evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
941  );
942  else
943  colLoop.addFunctionCall(
944  macBTW1_R1, R1.getAddress((N - 1 - col) * NU), evGu.getAddress((N - 1 - col) * NX), W1, ExportIndex( col )
945  );
946 
947  condensePrep << colLoop;
948  condensePrep.release( row ).release( col ).release( curr ).release( next );
949  }
950  condensePrep.addLinebreak( 2 );
951 
952  LOG( LVL_DEBUG ) << "---> Copy H11 upper to lower triangular part" << endl;
953 
954  // Copy to H11 upper triangular part to lower triangular part
955  if (N <= 15)
956  {
957  for (unsigned row = 0; row < N; ++row)
958  for(unsigned col = 0; col < row; ++col)
959  condensePrep.addFunctionCall( copyHTH, ExportIndex( row ), ExportIndex( col ) );
960  }
961  else
962  {
963  ExportIndex row, col;
964 
965  condensePrep.acquire( row ).acquire( col );
966 
967  ExportForLoop lRow(row, 0, N), lCol(col, 0, row);
968 
969  lCol.addFunctionCall(copyHTH, row, col);
970  lRow.addStatement( lCol );
971  condensePrep.addStatement( lRow );
972 
973  condensePrep.release( row ).release( col );
974  }
975  condensePrep.addLinebreak( 2 );
976 
977  LOG( LVL_DEBUG ) << "---> Factorization of the condensed Hessian" << endl;
978 
979  /*
980 
981  T1 = Q( N )
982  for blk = N - 1: 1
983  T2 = B( blk )^T * T1
984 
985  D = R( blk ) + T2 * B( blk )
986  L = T2 * A( blk )
987 
988  chol( D )
989  L = chol_solve(D, L) // L <- D^(-T) * L
990 
991  T3 = T1 * A( blk )
992  T1 = Q( blk ) + A( blk )^T * T3
993  T1 -= L^T * L
994 
995  row = N - 1 - blk
996 
997  U(row, row) = D
998  for col = row + 1: N - 1
999  U(row, col) = L * E(row + 1, col)
1000 
1001  T2 = B( 0 )^T * T1
1002  D = R( 0 ) + T2 * B( 0 )
1003  chol( D )
1004  U(N - 1, N - 1) = D
1005 
1006  ************************************************
1007  ************************************************
1008  OR, an alternative, n_x^3 free version would be:
1009 
1010  T1 = Q( N )
1011  for col = 0: N - 1
1012  F( col ) = T1 * E(0, col)
1013 
1014  for blk = N - 1: 1
1015  row = N - 1 - blk
1016 
1017  D = R( blk ) + B( blk )^T * F( row )
1018  L = F( row )^T * A( blk )
1019 
1020  chol( D )
1021  L = chol_solve(D, L) // L <- D^(-T) * L
1022 
1023  T1 = Q( blk ) - L^T * L
1024 
1025  T3 = A( blk )^T
1026 
1027  for col = row + 1: N - 1
1028  W1 = T3 * F( col )
1029  F( col ) = T1 * E(row + 1, col) + W1
1030 
1031  U(row, row) = D
1032  for col = row + 1: N - 1
1033  U(row, col) = L * E(row + 1, col)
1034 
1035  D = R( 0 ) + B( 0 )^T * F(N - 1)
1036  chol( D )
1037  U(N - 1, N - 1) = D
1038 
1039  */
1040 
1041  // The matrix where we store the factorization
1042  U.setup("U", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1043  // A helper matrix
1044  F.setup("F", N * NX, NU, REAL, ACADO_WORKSPACE);
1045 
1046  cholSolver.init(NU, NX, "condensing");
1047  cholSolver.setup();
1048 
1049  // Just for testing...
1050 // condensePrep.addStatement( U == H );
1051 // condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), U);
1052 // condensePrep.addFunctionCall(cholSolver.getSolveFunction(), U, Id);
1053 
1054 #if 0
1055  // N^2 factorization with n_x^3 terms
1056 
1057  condensePrep.addStatement( T1 == QN1 );
1058  for (unsigned blk = N - 1; blk > 0; --blk)
1059  {
1060  condensePrep.addFunctionCall(mult_BT_T1_T2, evGu.getAddress(blk * NX), T1, T2);
1061  if (R1.isGiven() == true)
1062  condensePrep.addFunctionCall(mac_R_T2_B_D, R1, T2, evGu.getAddress(blk * NX), D);
1063  else
1064  condensePrep.addFunctionCall(mac_R_T2_B_D, R1.getAddress(blk * NX), T2, evGu.getAddress(blk * NX), D);
1065  condensePrep.addFunctionCall(mul_T2_A_L, T2, evGx.getAddress(blk * NX), L);
1066 
1067  condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
1068  condensePrep.addFunctionCall(cholSolver.getSolveFunction(), D, L);
1069 
1070  if (Q1.isGiven() == true)
1071  condensePrep.addFunctionCall(updateQ, Q1, T3, evGx.getAddress(blk * NX), L, T1);
1072  else
1073  condensePrep.addFunctionCall(updateQ, Q1.getAddress(blk * NX), T3, evGx.getAddress(blk * NX), L, T1);
1074 
1075  unsigned row = N - 1 - blk;
1076 
1077  condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( row ));
1078  for (unsigned col = row + 1; col < N; ++col)
1079  {
1080  // U(row, col) = L * E(row + 1, col)
1081 
1082  // blk = (N - row) * (N - 1 - row) / 2 + (N - 1 - col)
1083 
1084  unsigned blkE = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
1085  cout << "blkE " << blkE << endl;
1086 
1087  condensePrep.addFunctionCall(mult_L_E_U, L, E.getAddress(blkE * NX), U, ExportIndex( row ), ExportIndex( col ));
1088  }
1089  }
1090  condensePrep.addFunctionCall(mult_BT_T1_T2, evGu.getAddress( 0 ), T1, T2);
1091  condensePrep.addFunctionCall(mac_R_T2_B_D, R1.getAddress( 0 ), T2, evGu.getAddress( 0 ), D);
1092  condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
1093  condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( N - 1 ));
1094 #else
1095  // N^2 factorization, n_x^3 free version
1096 
1097  // Initial value for the "updated" Q^* matrix
1098  condensePrep.addStatement( T1 == QN1 );
1099 
1100  if (N <= 15)
1101  {
1102  for (unsigned col = 0; col < N; ++col)
1103  {
1104  unsigned blkE = (N - 0) * (N - 1 - 0) / 2 + (N - 1 - col);
1105  condensePrep.addFunctionCall(
1106  multGxGu, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
1107  }
1108 
1109  for (unsigned blk = N - 1; blk > 0; --blk)
1110  {
1111  unsigned row = N - 1 - blk;
1112 
1113  // D = R( blk ) + B( blk )^T * F( row )
1114  // L = F( row )^T * A( blk )
1115  if (R1.isGiven() == true)
1116  condensePrep.addFunctionCall(
1117  mac_R_BT_F_D, R1, evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
1118  else
1119  condensePrep.addFunctionCall(
1120  mac_R_BT_F_D, R1.getAddress(blk * NX), evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
1121  condensePrep.addFunctionCall(mult_FT_A_L, F.getAddress(row * NX), evGx.getAddress(blk * NX), L);
1122 
1123  // Chol and system solve
1124  condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
1125  condensePrep.addFunctionCall(cholSolver.getSolveFunction(), D, L);
1126 
1127  // Update Q
1128  if (Q1.isGiven() == true)
1129  condensePrep.addFunctionCall(updateQ2, Q1, L, T1);
1130  else
1131  condensePrep.addFunctionCall(updateQ2, Q1.getAddress(blk * NX), L, T1);
1132 
1133  // for col = row + 1: N - 1
1134  // W1 = A( blk )^T * F( col )
1135  // F( col ) = W1 + T1 * E(row + 1, col)
1136 
1137  condensePrep.addFunctionCall(move_GxT_T3, evGx.getAddress(blk * NX), T3);
1138 
1139  for (unsigned col = row + 1; col < N; ++col)
1140  {
1141  condensePrep.addFunctionCall(multGxGu, T3, F.getAddress(col * NX), W1);
1142 
1143  unsigned blkE = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
1144  condensePrep.addFunctionCall(mac_W1_T1_E_F, W1, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
1145  }
1146 
1147  // Calculate one block-row of the factorized matrix
1148  condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( row ));
1149  for (unsigned col = row + 1; col < N; ++col)
1150  {
1151  // U(row, col) = L * E(row + 1, col)
1152 
1153  unsigned blkE = (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col);
1154  condensePrep.addFunctionCall(mult_L_E_U, L, E.getAddress(blkE * NX), U, ExportIndex( row ), ExportIndex( col ));
1155  }
1156  }
1157  }
1158  else
1159  {
1160  // Do not unroll the for-loops...
1161 
1162  ExportIndex col, blkE, blk, row;
1163 
1164  condensePrep.acquire( col ).acquire( blkE ).acquire( blk ).acquire( row );
1165 
1166  ExportForLoop colLoop(col, 0, N);
1167  colLoop << (blkE == (N - 0) * (N - 1 - 0) / 2 + (N - 1 - col));
1168  colLoop.addFunctionCall(
1169  multGxGu, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
1170  condensePrep << colLoop;
1171 
1172  ExportForLoop blkLoop(blk, N - 1, 0, -1);
1173  blkLoop << ( row == N - 1 - blk );
1174 
1175  if (R1.isGiven() == true)
1176  blkLoop.addFunctionCall(
1177  mac_R_BT_F_D, R1, evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
1178  else
1179  blkLoop.addFunctionCall(
1180  mac_R_BT_F_D, R1.getAddress(blk * NX), evGu.getAddress(blk * NX), F.getAddress(row * NX), D);
1181  blkLoop.addFunctionCall(mult_FT_A_L, F.getAddress(row * NX), evGx.getAddress(blk * NX), L);
1182 
1183  // Chol and system solve
1184  blkLoop.addFunctionCall(cholSolver.getCholeskyFunction(), D);
1185  blkLoop.addFunctionCall(cholSolver.getSolveFunction(), D, L);
1186 
1187  // Update Q
1188  if (Q1.isGiven() == true)
1189  blkLoop.addFunctionCall(updateQ2, Q1, L, T1);
1190  else
1191  blkLoop.addFunctionCall(updateQ2, Q1.getAddress(blk * NX), L, T1);
1192 
1193  blkLoop.addFunctionCall(move_GxT_T3, evGx.getAddress(blk * NX), T3);
1194 
1195  ExportForLoop colLoop2(col, row + 1, N);
1196  colLoop2.addFunctionCall(multGxGu, T3, F.getAddress(col * NX), W1);
1197  colLoop2 << (blkE == (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col));
1198  colLoop2.addFunctionCall(mac_W1_T1_E_F, W1, T1, E.getAddress(blkE * NX), F.getAddress(col * NX));
1199  blkLoop << colLoop2;
1200 
1201  // Calculate one block-row of the factorized matrix
1202  blkLoop.addFunctionCall(move_D_U, D, U, ExportIndex( row ));
1203  ExportForLoop colLoop3(col, row + 1, N);
1204  colLoop3 << (blkE == (N - (row + 1)) * (N - 1 - (row + 1)) / 2 + (N - 1 - col));
1205  colLoop3.addFunctionCall(mult_L_E_U, L, E.getAddress(blkE * NX), U, row, col);
1206  blkLoop << colLoop3;
1207 
1208  condensePrep << blkLoop;
1209  condensePrep.release( col ).release( blkE ).release( blk ).release( row );
1210  }
1211 
1212  // Calculate the bottom-right block of the factorized Hessian
1213  condensePrep.addFunctionCall(
1214  mac_R_BT_F_D, R1.getAddress(0 * NX), evGu.getAddress(0 * NX), F.getAddress((N - 1) * NX), D);
1215  condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), D);
1216  condensePrep.addFunctionCall(move_D_U, D, U, ExportIndex( N - 1 ));
1217  condensePrep.addLinebreak( 2 );
1218 
1219 #endif
1220 
1222  //
1223  // Compute gradient components g0 and g1
1224  //
1226 
1228 
1229  LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
1230 
1231  {
1232  condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
1233  condenseFdb.addLinebreak();
1234  }
1235 
1236  condenseFdb.addStatement( Dy -= y );
1237  condenseFdb.addStatement( DyN -= yN );
1238  condenseFdb.addLinebreak();
1239 
1240  // Compute RDy, UPDATED
1241  for(unsigned blk = 0; blk < N; ++blk)
1242  {
1243  if (R2.isGiven() == true)
1244  condenseFdb.addFunctionCall(
1245  multRDy, R2,
1246  Dy.getAddress(blk * NY, 0),
1247  g.getAddress((N - 1 - blk) * NU, 0) );
1248  else
1249  condenseFdb.addFunctionCall(
1250  multRDy, R2.getAddress(blk * NU, 0),
1251  Dy.getAddress(blk * NY, 0),
1252  g.getAddress((N - 1 - blk) * NU, 0) );
1253  }
1254  condenseFdb.addLinebreak();
1255 
1256  // Compute QDy
1257  // NOTE: This is just for the MHE case :: run1 starts from 0; in MPC :: from 1 ;)
1258  for(unsigned blk = 0; blk < N; blk++ )
1259  {
1260  if (Q2.isGiven() == true)
1261  condenseFdb.addFunctionCall(
1262  multQDy, Q2,
1263  Dy.getAddress(blk * NY),
1264  QDy.getAddress(blk * NX) );
1265  else
1266  condenseFdb.addFunctionCall(
1267  multQDy, Q2.getAddress(blk * NX),
1268  Dy.getAddress(blk * NY),
1269  QDy.getAddress(blk * NX) );
1270  }
1271  condenseFdb.addLinebreak();
1272  condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == QN2 * DyN );
1273  condenseFdb.addLinebreak();
1274 
1275  /*
1276 
1277  OK, we need to adapt the old N^2 algorithm: u vector is in reverse order
1278 
1279  for k = 0: N - 1
1280  g1_k = r_{N - 1 - k}; // OK, updated (look up)
1281 
1282  sbar_0 = Dx_0;
1283  sbar(1: N) = d;
1284 
1285  for k = 0: N - 1
1286  sbar_{k + 1} += A_k sbar_k; // can stay the same
1287 
1288  w1 = Q_N^T * sbar_N + q_N;
1289  for k = N - 1: 1
1290  {
1291  g1_{N - 1 - k} += B_k^T * w1;
1292  w2 = A_k^T * w1 + q_k;
1293  w1 = Q_k^T * sbar_k + w2;
1294  }
1295 
1296  g1_{N - 1} += B_0^T * w1;
1297 
1298  */
1299 
1300  sbar.setup("sbar", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
1301  w1.setup("w1", NX, 1, REAL, ACADO_WORKSPACE);
1302  w2.setup("w2", NX, 1, REAL, ACADO_WORKSPACE);
1303 
1304  condenseFdb.addStatement( sbar.getRows(0, NX) == Dx0 );
1305 
1306  if( performsSingleShooting() == false )
1307  condensePrep.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
1308 
1309  for (unsigned i = 0; i < N; ++i)
1310  condenseFdb.addFunctionCall(
1311  macASbar, evGx.getAddress(i * NX), sbar.getAddress(i * NX), sbar.getAddress((i + 1) * NX)
1312  );
1313  condenseFdb.addLinebreak();
1314 
1315  condenseFdb.addStatement(
1316  w1 == QDy.getRows(N * NX, (N + 1) * NX) + QN1 * sbar.getRows(N * NX, (N + 1) * NX)
1317  );
1318  for (unsigned i = N - 1; 0 < i; --i)
1319  {
1320  condenseFdb.addFunctionCall(
1321  macBTw1, evGu.getAddress(i * NX), w1, g.getAddress((N - 1 - i) * NU) // UPDATED
1322  );
1323  condenseFdb.addFunctionCall(
1324  macATw1QDy, evGx.getAddress(i * NX), w1, QDy.getAddress(i * NX), w2 // Proveri indexiranje za QDy
1325  );
1326  if (Q1.isGiven() == true)
1327  condenseFdb.addFunctionCall(
1328  macQSbarW2, Q1, sbar.getAddress(i * NX), w2, w1
1329  );
1330  else
1331  condenseFdb.addFunctionCall(
1332  macQSbarW2, Q1.getAddress(i * NX), sbar.getAddress(i * NX), w2, w1
1333  );
1334  }
1335  condenseFdb.addFunctionCall(
1336  macBTw1, evGu.getAddress( 0 ), w1, g.getAddress((N - 1) * NU) // UPDATED
1337  );
1338  condenseFdb.addLinebreak();
1339 
1341 
1343  //
1344  // Expansion routine
1345  //
1347 
1348  /*
1349 
1350  // Step expansion, assuming that u_k is already updated
1351 
1352  Ds_0 = Dx_0;
1353  Ds_{1: N} = d;
1354 
1355  for i = 0: N - 1
1356  {
1357  Ds_{k + 1} += A_k * Ds_k; // Reuse multABarD
1358  Ds_{k + 1} += B_k * Du_{N - 1 - k};
1359  s_{k + 1} += Ds_{k + 1};
1360  }
1361 
1362  // TODO Calculation of multipliers, modify the order of Ds and Du
1363 
1364  mu_N = lambda_N + Q_N^T * s_N
1365  for i = N - 1: 1
1366  mu_k = lambda_k + Q_k^T * s_k + A_k^T * mu_{k + 1} + q_k
1367 
1368  mu_0 = Q_0^T s_0 + A_0^T * mu_1 + q_0
1369 
1370  */
1371 
1372  LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
1373 
1374  expand.setup( "expand" );
1375 
1376  if (performFullCondensing() == true)
1377  {
1378  ExportVariable xVarsTranspose = xVars.getTranspose();
1379  for (unsigned row = 0; row < N; ++row)
1380  expand.addStatement( u.getRow( row ) += xVarsTranspose.getCols((N - 1 - row) * NU, (N - row) * NU) );
1381  }
1382  else
1383  {
1384  // TODO, does not work yet.
1385  expand.addStatement( x.makeColVector().getRows(0, NX) += xVars.getRows(0, NX) );
1386  expand.addLinebreak();
1387  expand.addStatement( u.makeColVector() += xVars.getRows(NX, getNumQPvars()) );
1388  }
1389 
1390  expand.addStatement( sbar.getRows(0, NX) == Dx0 );
1391  expand.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
1392 
1393  for (unsigned row = 0; row < N; ++row )
1394  expand.addFunctionCall(
1395  expansionStep, evGx.getAddress(row * NX), evGu.getAddress(row * NX),
1396  xVars.getAddress((N - 1 - row) * NU), sbar.getAddress(row * NX), // UPDATED
1397  sbar.getAddress((row + 1) * NX)
1398  );
1399 
1400  expand.addStatement( x.makeColVector() += sbar );
1401 
1402  return SUCCESSFUL_RETURN;
1403 }
1404 
1405 returnValue ExportGaussNewtonCn2Factorization::setupVariables( )
1406 {
1408  //
1409  // Make index vector for state constraints, reverse order
1410  //
1412 
1413  bool boxConIsFinite = false;
1414  xBoundsIdxRev.clear();
1415  xBoundsIdx.clear();
1416 
1417  DVector lbBox, ubBox;
1418  unsigned dimBox = xBounds.getNumPoints();
1419  for (int i = dimBox - 1; i >= 0; --i)
1420  {
1421  lbBox = xBounds.getLowerBounds( i );
1422  ubBox = xBounds.getUpperBounds( i );
1423 
1424  if (isFinite( lbBox ) || isFinite( ubBox ))
1425  boxConIsFinite = true;
1426 
1427  // This is maybe not necessary
1428  if (boxConIsFinite == false || i == 0)
1429  continue;
1430 
1431  for (unsigned j = 0; j < lbBox.getDim(); ++j)
1432  {
1433  if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
1434  {
1435  xBoundsIdxRev.push_back(i * lbBox.getDim() + j);
1436  xBoundsIdx.push_back((dimBox - 1 - i) * lbBox.getDim() + j);
1437  }
1438  }
1439  }
1440 
1441  if (initialStateFixed() == true)
1442  {
1443  x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
1444  x0.setDoc( (std::string)"Current state feedback vector." );
1445  Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
1446  }
1447 
1448  E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
1449  QE.setup("QE", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
1450  QGx.setup("QGx", N * NX, NX, REAL, ACADO_WORKSPACE);
1451 
1452  QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
1453 
1454  // Setup all QP stuff
1455 
1456  H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1457 
1458  // Stupid aliasing to avoid copying of data
1459  if (performFullCondensing() == true)
1460  {
1461  H11 = H;
1462  }
1463  else
1464  {
1465  H00 = H.getSubMatrix(0, NX, 0, NX);
1466  H11 = H.getSubMatrix(NX, getNumQPvars(), NX, getNumQPvars());
1467  }
1468 
1469  H10.setup("H10", N * NU, NX, REAL, ACADO_WORKSPACE);
1470 
1471  A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1472 
1473  g.setup("g", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1474 
1475  if (performFullCondensing() == true)
1476  {
1477  g1 = g;
1478  }
1479  else
1480  {
1481  g0 = g.getRows(0, NX);
1482  g1 = g.getRows(NX, getNumQPvars());
1483  }
1484 
1485  lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1486  ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1487 
1488  lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
1489  ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
1490 
1491  xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1492  yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
1493 
1494  return SUCCESSFUL_RETURN;
1495 }
1496 
1497 returnValue ExportGaussNewtonCn2Factorization::setupMultiplicationRoutines( )
1498 {
1499  ExportIndex iCol( "iCol" );
1500  ExportIndex iRow( "iRow" );
1501 
1502  ExportVariable dp, dn, Gx1, Gx2, Gx3, Gu1, Gu2, Gu3;
1503  ExportVariable R22, Dy1, RDy1, Q22, QDy1, E1, U1, U2, H101, w11, w12, w13;
1504  dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
1505  dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
1506  Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
1507  Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
1508  Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
1509  Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
1510  Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
1511  Gu3.setup("Gu3", NX, NU, REAL, ACADO_LOCAL);
1512  R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
1513  Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
1514  RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
1515  Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
1516  QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
1517  E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
1518  U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
1519  U2.setup("U2", NU, 1, REAL, ACADO_LOCAL);
1520  H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
1521 
1522  w11.setup("w11", NX, 1, REAL, ACADO_LOCAL);
1523  w12.setup("w12", NX, 1, REAL, ACADO_LOCAL);
1524  w13.setup("w13", NX, 1, REAL, ACADO_LOCAL);
1525 
1526  if ( Q2.isGiven() )
1527  Q22 = Q2;
1528  if ( R2.isGiven() )
1529  R22 = R2;
1530 
1531  // multGxd; // d_k += Gx_k * d_{k-1}
1532  multGxd.setup("multGxd", dp, Gx1, dn);
1533  multGxd.addStatement( dn += Gx1 * dp );
1534  // moveGxT
1535  moveGxT.setup("moveGxT", Gx1, Gx2);
1536  moveGxT.addStatement( Gx2 == Gx1 );
1537  // multGxGx
1538  multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
1539  multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
1540  // multGxGu
1541  multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
1542  multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
1543  // moveGuE
1544  moveGuE.setup("moveGuE", Gu1, Gu2);
1545  moveGuE.addStatement( Gu2 == Gu1 );
1546 
1547  unsigned offset = (performFullCondensing() == true) ? 0 : NX;
1548 
1549  // setBlockH11
1550  setBlockH11.setup("setBlockH11", iRow, iCol, Gu1, Gu2);
1551  setBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) += (Gu1 ^ Gu2) );
1552  // zeroBlockH11
1553  zeroBlockH11.setup("zeroBlockH11", iRow, iCol);
1554  zeroBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == zeros<double>(NU, NU) );
1555  // copyHTH
1556  copyHTH.setup("copyHTH", iRow, iCol);
1557  copyHTH.addStatement(
1558  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1559  H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
1560  );
1561  // multRDy
1562  multRDy.setup("multRDy", R22, Dy1, RDy1);
1563  multRDy.addStatement( RDy1 == R22 * Dy1 );
1564  // mult QDy1
1565  multQDy.setup("multQDy", Q22, Dy1, QDy1);
1566  multQDy.addStatement( QDy1 == Q22 * Dy1 );
1567  // multEQDy;
1568  multEQDy.setup("multEQDy", E1, QDy1, U1);
1569  multEQDy.addStatement( U1 += (E1 ^ QDy1) );
1570  // multQETGx
1571  multQETGx.setup("multQETGx", E1, Gx1, H101);
1572  multQETGx.addStatement( H101 += (E1 ^ Gx1) );
1573  // zerBlockH10
1574  zeroBlockH10.setup("zeroBlockH10", H101);
1575  zeroBlockH10.addStatement( H101 == zeros<double>(NU, NX) );
1576 
1577  if (performsSingleShooting() == false)
1578  {
1579  // multEDu
1580  multEDu.setup("multEDu", E1, U1, dn);
1581  multEDu.addStatement( dn += E1 * U1 );
1582  }
1583 
1584  if (Q1.isGiven() == true)
1585  {
1586  // multQ1Gx
1587  multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
1588  multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
1589 
1590  // multQ1Gu
1591  multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
1592  multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
1593 
1594  // multQ1d
1595  multQ1d.setup("multQ1d", Q1, dp, dn);
1596  multQ1d.addStatement( dn == Q1 * dp );
1597  }
1598  else
1599  {
1600  // multQ1d
1601  multQ1d.setup("multQ1d", Gx1, dp, dn);
1602  multQ1d.addStatement( dn == Gx1 * dp );
1603  }
1604 
1605  if (QN1.isGiven() == BT_TRUE)
1606  {
1607  // multQN1Gu
1608  multQN1Gu.setup("multQN1Gu", Gu1, Gu2);
1609  multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
1610 
1611  // multQN1Gx
1612  multQN1Gx.setup("multQN1Gx", Gx1, Gx2);
1613  multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
1614  }
1615 
1616  if (performsSingleShooting() == BT_FALSE)
1617  {
1618  // multQN1d
1619  multQN1d.setup("multQN1d", QN1, dp, dn);
1620  multQN1d.addStatement( dn == QN1 * dp );
1621  }
1622 
1623  if (performFullCondensing() == false)
1624  {
1625  // zeroBlockH00
1626  zeroBlockH00.setup( "zeroBlockH00" );
1627  zeroBlockH00.addStatement( H00 == zeros<double>(NX, NX) );
1628 
1629  // multCTQC
1630  multCTQC.setup("multCTQC", Gx1, Gx2);
1631  multCTQC.addStatement( H00 += (Gx1 ^ Gx2) );
1632  }
1633 
1634  // N2 condensing related
1635 
1636  //ExportFunction multBTW1, multGxTGu, macQEW2;
1637 
1638  // multBTW1, evGu.getAddress(row * NX), W1, row, col
1639  multBTW1.setup("multBTW1", Gu1, Gu2, iRow, iCol);
1640  multBTW1.addStatement(
1641  H.getSubMatrix(iRow * NU, (iRow + 1) * NU, iCol * NU, (iCol + 1) * NU) ==
1642  (Gu1 ^ Gu2)
1643 // (Gu2 ^ Gu1)
1644  );
1645 
1646  //
1647  // Define LM regularization terms
1648  //
1649  DMatrix mRegH00 = eye<double>( getNX() );
1650  DMatrix mRegH11 = eye<double>( getNU() );
1651 
1652  mRegH00 *= levenbergMarquardt;
1653  mRegH11 *= levenbergMarquardt;
1654 
1655  ExportVariable R11;
1656  if (R1.isGiven() == true)
1657  R11 = R1;
1658  else
1659  R11.setup("R11", NU, NU, REAL, ACADO_LOCAL);
1660 
1661  macBTW1_R1.setup("multBTW1_R1", R11, Gu1, Gu2, iRow);
1662  macBTW1_R1.addStatement(
1663  H.getSubMatrix(iRow * NU, (iRow + 1) * NU, iRow * NU, (iRow + 1) * NU) ==
1664  R11 + (Gu1 ^ Gu2)
1665  );
1666  macBTW1_R1.addStatement(
1667  H.getSubMatrix(iRow * NU, (iRow + 1) * NU, iRow * NU, (iRow + 1) * NU) += mRegH11
1668  );
1669 
1670  multGxTGu.setup("multGxTGu", Gx1, Gu1, Gu2);
1671  multGxTGu.addStatement( Gu2 == (Gx1 ^ Gu1) );
1672 
1673  // macQEW2, Q1, E.getAddress((offest + row - col - 1) * NX), W2, W1
1674  ExportVariable Q11;
1675  if (Q1.isGiven())
1676  Q11 = Q1;
1677  else
1678  Q11.setup("Q11", NX, NX, REAL, ACADO_LOCAL);
1679 
1680 
1681  macQEW2.setup("multQEW2", Q11, Gu1, Gu2, Gu3);
1682  macQEW2.addStatement( Gu3 == Gu2 + Q11 * Gu1 );
1683 
1684  // ExportFunction macATw1Q, macBTw1, macQSbarW2, macASbarD;
1685  macASbar.setup("macASbar", Gx1, w11, w12);
1686  macASbar.addStatement( w12 += Gx1 * w11 );
1687 
1688 // macASbarD2.setup("macASbarD2", Gx1, w11, w12, w13);
1689 // macASbarD2.addStatement( w13 == Gx1 * w11 );
1690 // macASbarD2.addStatement( w13 -= w12 );
1691 
1692  macBTw1.setup("macBTw1", Gu1, w11, U1);
1693  macBTw1.addStatement( U1 += Gu1 ^ w11 );
1694 
1695  // macATw1QDy, A.getAddress(i * NX), w1, QDy.getAddress(i * NX), w2
1696  macATw1QDy.setup("macATw1QDy", Gx1, w11, w12, w13);
1697  macATw1QDy.addStatement( w13 == w12 + (Gx1 ^ w11) );
1698 
1699  // macQSbarW2, Q1, sbar.getAddress(i * NX), w2, w1
1700  macQSbarW2.setup("macQSbarW2", Q11, w11, w12, w13);
1701  macQSbarW2.addStatement( w13 == w12 + Q11 * w11 );
1702 
1703  expansionStep.setup("expansionStep", Gx1, Gu1, U1, w11, w12);
1704  expansionStep.addStatement( w12 += Gx1 * w11 );
1705  expansionStep.addStatement( w12 += Gu1 * U1 );
1706 
1707  //
1708  // Hessian factorization helper routines
1709  //
1710 
1711  ExportVariable D1("D", NU, NU, REAL, ACADO_LOCAL);
1712  ExportVariable L1("L", NU, NX, REAL, ACADO_LOCAL);
1713  ExportVariable H1("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_LOCAL);
1714 
1715  ExportVariable T11("T11", NX, NX, REAL, ACADO_LOCAL);
1716  ExportVariable T22("T22", NU, NX, REAL, ACADO_LOCAL);
1717  ExportVariable T33("T33", NX, NX, REAL, ACADO_LOCAL);
1718 
1719 // ExportFunction mult_BT_T1_T2;
1720  mult_BT_T1_T2.setup("mult_BT_T1_T2", Gu1, T11, T22);
1721  mult_BT_T1_T2.addStatement( T22 == (Gu1 ^ T11) );
1722 
1723 // ExportFunction mul_T2_A_L;
1724  mul_T2_A_L.setup("mul_T2_A_L", T22, Gx1, L1);
1725  mul_T2_A_L.addStatement( L1 == T22 * Gx1 );
1726 
1727 // ExportFunction mac_R_T2_B_D;
1728  mac_R_T2_B_D.setup("mac_R_T2_B_D", R11, T22, Gu1, D1);
1729  mac_R_T2_B_D.addStatement( D1 == R11 + T22 * Gu1 );
1730 
1731 // ExportFunction move_D_H;
1732  move_D_U.setup("move_D_H", D1, H1, iRow);
1733  move_D_U.addStatement( H1.getSubMatrix(iRow * NU, (iRow + 1) * NU, iRow * NU, (iRow + 1) * NU) == D1 );
1734 
1735 // ExportFunction mult_L_E_H;
1736  mult_L_E_U.setup("mult_L_E_H", L1, Gu1, H1, iRow, iCol);
1737  mult_L_E_U.addStatement(
1738  H1.getSubMatrix(iRow * NU, (iRow + 1) * NU, iCol * NU, (iCol + 1) * NU) == L1 * Gu1
1739  );
1740 
1741 // ExportFunction updateQ; T1 = Q( blk ) + A^T * T1 * A - L^T * L
1742  // Q1, T3, evGx.getAddress(blk * NX), L, T1);
1743  updateQ.setup("updateQ", Q11, T33, Gx1, L1, T11);
1744  updateQ.addStatement( T33 == (Gx1 ^ T11) );
1745  updateQ.addStatement( T11 == Q11 + T33 * Gx1 );
1746  updateQ.addStatement( T11 -= (L1 ^ L1) );
1747 
1748 // ExportFunction mac_R_BT_F_D, mult_FT_A_L;
1749 // ExportFunction updateQ2;
1750 // ExportFunction mac_W1_T1_E_F;
1751 
1752  mac_R_BT_F_D.setup("mac_R_BT_F_D", R11, Gu1, Gu2, D1);
1753  mac_R_BT_F_D.addStatement( D1 == R11 + (Gu1 ^ Gu2) );
1754 
1755  mult_FT_A_L.setup("mult_FT_A_L", Gu1, Gx1, L1);
1756  mult_FT_A_L.addStatement( L1 == (Gu1 ^ Gx1) );
1757 
1758  updateQ2.setup("updateQ2", Q11, L1, T11);
1759  // TODO T11 == Q11 - (L1^L1) does not work properly
1760  // Revisit this...
1761  updateQ2.addStatement( T11 == Q11 );
1762  updateQ2.addStatement( T11 -= (L1 ^ L1) );
1763 
1764  mac_W1_T1_E_F.setup("mac_W1_T1_E_F", Gu1, Gx1, Gu2, Gu3);
1765  mac_W1_T1_E_F.addStatement( Gu3 == Gu1 + Gx1 * Gu2 );
1766 
1767  move_GxT_T3.setup("move_GxT_T3", Gx1, Gx2);
1768  move_GxT_T3.addStatement( Gx2 == Gx1.getTranspose() );
1769 
1770  return SUCCESSFUL_RETURN;
1771 }
1772 
1773 returnValue ExportGaussNewtonCn2Factorization::setupEvaluation( )
1774 {
1776  //
1777  // Preparation phase
1778  //
1780 
1781  preparation.setup( "preparationStep" );
1782  preparation.doc( "Preparation step of the RTI scheme." );
1783  ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
1784  retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
1785  preparation.setReturnValue(retSim, false);
1786 
1787  preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
1788 
1789  preparation.addFunctionCall( evaluateObjective );
1790  preparation.addFunctionCall( condensePrep );
1791 
1793  //
1794  // Feedback phase
1795  //
1797 
1798  ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
1799  tmp.setDoc( "Status code of the qpOASES QP solver." );
1800 
1801  ExportFunction solve("solve");
1802  solve.setReturnValue( tmp );
1803 
1804  feedback.setup("feedbackStep");
1805  feedback.doc( "Feedback/estimation step of the RTI scheme." );
1806  feedback.setReturnValue( tmp );
1807 
1808  feedback.addFunctionCall( condenseFdb );
1809  feedback.addLinebreak();
1810 
1811  stringstream s;
1812  s << tmp.getName() << " = " << solve.getName() << "( );" << endl;
1813  feedback << s.str();
1814  feedback.addLinebreak();
1815 
1816  feedback.addFunctionCall( expand );
1817 
1819  //
1820  // Setup evaluation of the KKT tolerance
1821  //
1823 
1824  ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
1825  ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
1826  ExportIndex index( "index" );
1827 
1828  getKKT.setup( "getKKT" );
1829  getKKT.doc( "Get the KKT tolerance of the current iterate." );
1830  kkt.setDoc( "The KKT tolerance value." );
1831  getKKT.setReturnValue( kkt );
1832  getKKT.addVariable( prd );
1833  getKKT.addIndex( index );
1834 
1835  // ACC = |\nabla F^T * xVars|
1836  getKKT.addStatement( kkt == (g ^ xVars) );
1837  getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
1838 
1839  ExportForLoop bLoop(index, 0, getNumQPvars());
1840 
1841  bLoop.addStatement( prd == yVars.getRow( index ) );
1842  bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
1843  bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
1844  bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
1845  bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
1846  getKKT.addStatement( bLoop );
1847 
1848  if ((getNumStateBounds() + getNumComplexConstraints())> 0)
1849  {
1850  ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
1851 
1852  cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ));
1853  cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
1854  cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
1855  cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
1856  cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
1857 
1858  getKKT.addStatement( cLoop );
1859  }
1860 
1861  return SUCCESSFUL_RETURN;
1862 }
1863 
1864 returnValue ExportGaussNewtonCn2Factorization::setupQPInterface( )
1865 {
1866  string folderName;
1867  get(CG_EXPORT_FOLDER_NAME, folderName);
1868 
1869  string moduleName;
1870  get(CG_MODULE_NAME, moduleName);
1871 
1872  int qpSolver;
1873  get(QP_SOLVER, qpSolver);
1874 
1875  std::string sourceFile, headerFile, solverDefine;
1876  ExportQpOasesInterface* qpInterface = 0;
1877 
1878  switch ( (QPSolverName)qpSolver )
1879  {
1880  case QP_QPOASES:
1881  sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
1882  headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
1883  solverDefine = "QPOASES_HEADER";
1884  qpInterface = new ExportQpOasesInterface(headerFile, sourceFile, "");
1885  break;
1886 
1887  case QP_QPOASES3:
1888  sourceFile = folderName + "/" + moduleName + "_qpoases3_interface.c";
1889  headerFile = folderName + "/" + moduleName + "_qpoases3_interface.h";
1890  solverDefine = "QPOASES3_HEADER";
1891  qpInterface = new ExportQpOases3Interface(headerFile, sourceFile, "");
1892  break;
1893 
1894  default:
1895  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
1896  "For condensed solution only qpOASES and qpOASES3 QP solver are supported");
1897  }
1898 
1899  int useSinglePrecision;
1900  get(USE_SINGLE_PRECISION, useSinglePrecision);
1901 
1902  int hotstartQP;
1903  get(HOTSTART_QP, hotstartQP);
1904 
1905  int covCalc;
1906  get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
1907 
1908  int maxNumQPiterations;
1909  get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
1910 
1911  int externalCholesky;
1912  get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
1913 
1914  //
1915  // Set up export of the source file
1916  //
1917 
1918  if ( qpInterface == 0 )
1919  return RET_UNKNOWN_BUG;
1920 
1921  qpInterface->configure(
1922  "",
1923  solverDefine,
1924  getNumQPvars(),
1925  getNumStateBounds() + getNumComplexConstraints(),
1926  maxNumQPiterations,
1927  "PL_NONE",
1928  useSinglePrecision,
1929 
1930  commonHeaderName,
1931  "",
1932  xVars.getFullName(),
1933  yVars.getFullName(),
1934  "", // TODO
1935 // sigma.getFullName(),
1936  hotstartQP,
1937  (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL,
1938  H.getFullName(),
1939  U.getFullName(),
1940  g.getFullName(),
1941  A.getFullName(),
1942  lb.getFullName(),
1943  ub.getFullName(),
1944  lbA.getFullName(),
1945  ubA.getFullName()
1946  );
1947 
1948  returnValue returnvalue = qpInterface->exportCode();
1949 
1950  if ( qpInterface != 0 )
1951  delete qpInterface;
1952 
1953  return returnvalue;
1954 }
1955 
1956 bool ExportGaussNewtonCn2Factorization::performFullCondensing() const
1957 {
1958  int sparseQPsolution;
1959  get(SPARSE_QP_SOLUTION, sparseQPsolution);
1960 
1961  if ((SparseQPsolutionMethods)sparseQPsolution == CONDENSING)
1962  return false;
1963 
1964  return true;
1965 }
1966 
1967 CLOSE_NAMESPACE_ACADO
LOG
#define LOG(level)
Just define a handy macro for getting the logger.
Definition: acado_message_handling.hpp:193
R22
#define R22
Definition: hydroscal_model.hpp:101
LVL_DEBUG
Lowest level, the debug level.
Definition: acado_types.hpp:1122
ExportNLPSolver::objEvFx
ExportVariable objEvFx
Definition: export_nlp_solver.hpp:266
ExportCholeskySolver::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: export_cholesky_solver.cpp:224
E1
const double E1
Definition: examples/code_generation/mpc_mhe/cstr.cpp:37
ExportVariable::getRow
ExportVariable getRow(const ExportIndex &idx) const
Definition: export_variable.cpp:395
RET_UNKNOWN_BUG
Definition: acado_types.hpp:845
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
ExportCholeskySolver::setup
virtual returnValue setup()
Definition: export_cholesky_solver.cpp:71
ExportNLPSolver::shiftStates
ExportFunction shiftStates
Definition: export_nlp_solver.hpp:323
SparseQPsolutionMethods
SparseQPsolutionMethods
Definition: acado_types.hpp:755
QP_SOLVER
Definition: acado_types.hpp:386
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
CG_MODULE_NAME
Definition: acado_types.hpp:335
INFTY
const double INFTY
Definition: acado_constants.hpp:65
ExportVariable::getTranspose
ExportVariable getTranspose() const
Definition: export_variable.cpp:390
ACADO_WORKSPACE
Definition: acado_types.hpp:805
ExportNLPSolver::initialStateFixed
bool initialStateFixed() const
Definition: export_nlp_solver.cpp:1589
ExportNLPSolver::xBounds
VariablesGrid xBounds
Definition: export_nlp_solver.hpp:287
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
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
export_gauss_newton_cn2_factorization.hpp
CG_COMPUTE_COVARIANCE_MATRIX
Definition: acado_types.hpp:342
ExportGaussNewtonCn2Factorization::getDataDeclarations
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: export_gauss_newton_cn2_factorization.cpp:91
ExportAlgorithm::getNX
uint getNX() const
Definition: export_algorithm.cpp:99
ExportCholeskySolver::getCholeskyFunction
const ExportFunction & getCholeskyFunction() const
Definition: export_cholesky_solver.cpp:248
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
MatrixVariablesGrid::getUpperBounds
DVector getUpperBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1393
MatrixVariablesGrid::getLowerBound
double getLowerBound(uint pointIdx, uint valueIdx) const
Definition: matrix_variables_grid.cpp:1365
ExportQpOasesInterface::configure
virtual returnValue configure(const std::string &_prefix, const std::string &_solverDefine, const int nvmax, const int ncmax, const int nwsrmax, const std::string &_printLevel, bool _useSinglePrecision, const std::string &_commonHeader, const std::string &_namespace, const std::string &_primalSolution, const std::string &_dualSolution, const std::string &_sigma, bool _hotstartQP, bool _externalCholesky, const std::string &_qpH, const std::string &_qpR, const std::string &_qpg, const std::string &_qpA, const std::string &_qplb, const std::string &_qpub, const std::string &_qplbA, const std::string &_qpubA)
Definition: export_qpoases_interface.cpp:53
MAX_NUM_QP_ITERATIONS
Definition: acado_types.hpp:387
ExportNLPSolver::objEvFxEnd
ExportVariable objEvFxEnd
Definition: export_nlp_solver.hpp:266
CONDENSING
Definition: acado_types.hpp:758
ACADO_VARIABLES
Definition: acado_types.hpp:804
ExportForLoop
Allows to export code of a for-loop.
Definition: export_for_loop.hpp:56
std::toString
string toString(T const &value)
Definition: acado_io_utils.hpp:208
QP_QPOASES
Definition: acado_types.hpp:635
ExportNLPSolver::uBounds
VariablesGrid uBounds
Definition: export_nlp_solver.hpp:286
R11
#define R11
Definition: hydroscal_model.hpp:100
H1
const double H1
Definition: examples/code_generation/mpc_mhe/cstr.cpp:40
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
MatrixVariablesGrid::getUpperBound
double getUpperBound(uint pointIdx, uint valueIdx) const
Definition: matrix_variables_grid.cpp:1414
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
ExportIndex
Defines a scalar-valued index variable to be used for exporting code.
Definition: export_index.hpp:54
ExportNLPSolver
Base class for export of NLP/OCP solvers.
Definition: export_nlp_solver.hpp:62
ExportNLPSolver::evaluateStageCost
ExportAcadoFunction evaluateStageCost
Definition: export_nlp_solver.hpp:270
ExportQpOases3Interface
A class for generating the glue code for interfacing qpOASES.
Definition: export_qpoases3_interface.hpp:49
SPARSE_QP_SOLUTION
Definition: acado_types.hpp:410
ExportNLPSolver::evaluateTerminalCost
ExportAcadoFunction evaluateTerminalCost
Definition: export_nlp_solver.hpp:271
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
USE_SINGLE_PRECISION
Definition: acado_types.hpp:426
ExportNLPSolver::setupInitialization
virtual returnValue setupInitialization()
Definition: export_nlp_solver.cpp:165
ExportAlgorithm::commonHeaderName
std::string commonHeaderName
Definition: export_algorithm.hpp:217
ExportCholeskySolver::getSolveFunction
const ExportFunction & getSolveFunction() const
Definition: export_cholesky_solver.cpp:253
ExportStruct
ExportStruct
Definition: acado_types.hpp:802
ExportNLPSolver::modelSimulation
ExportFunction modelSimulation
Definition: export_nlp_solver.hpp:240
ExportFunction::doc
virtual ExportFunction & doc(const std::string &_doc)
Definition: export_function.cpp:375
ExportAlgorithm::getNY
uint getNY() const
Definition: export_algorithm.cpp:138
CG_HARDCODE_CONSTRAINT_VALUES
Definition: acado_types.hpp:343
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
GenericVector::getDim
unsigned getDim() const
Definition: vector.hpp:172
ExportVariable::makeRowVector
ExportVariable makeRowVector() const
Definition: export_variable.cpp:442
ExportCholeskySolver::init
returnValue init(unsigned _dimA, unsigned _numColsB, const std::string &_id)
Definition: export_cholesky_solver.cpp:50
ExportNLPSolver::objValueIn
ExportVariable objValueIn
Definition: export_nlp_solver.hpp:269
ExportNLPSolver::initializeNodes
ExportFunction initializeNodes
Definition: export_nlp_solver.hpp:326
CG_USE_VARIABLE_WEIGHTING_MATRIX
Definition: acado_types.hpp:340
ExportVariable::get
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
Definition: export_variable.cpp:227
ExportQpOasesInterface::exportCode
virtual returnValue exportCode()
Definition: export_qpoases_interface.cpp:169
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
ExportNLPSolver::QN1
ExportVariable QN1
Definition: export_nlp_solver.hpp:276
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
ExportFunction
Allows to export code of an arbitrary function.
Definition: export_function.hpp:58
ExportVariable::getDim
virtual uint getDim() const
Definition: export_variable.cpp:245
ExportNLPSolver::shiftControls
ExportFunction shiftControls
Definition: export_nlp_solver.hpp:324
ExportStatementBlock::addStatement
returnValue addStatement(const ExportStatement &_statement)
Definition: export_statement_block.cpp:91
CG_USE_OPENMP
Definition: acado_types.hpp:339
ExportData::getFullName
std::string getFullName() const
Definition: export_data.cpp:121
REAL
Definition: acado_types.hpp:796
STATIC_CONST_INT
Definition: acado_types.hpp:798
ExportStatementBlock::addLinebreak
returnValue addLinebreak(uint num=1)
Definition: export_statement_block.cpp:209
ExportNLPSolver::evaluatePathConstraints
ExportAcadoFunction evaluatePathConstraints
Definition: export_nlp_solver.hpp:293
ExportAlgorithm::getNYN
uint getNYN() const
Definition: export_algorithm.cpp:148
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
GenericVector::append
GenericVector & append(const GenericVector &_arg)
Definition: vector.cpp:42
ExportFunction::setReturnValue
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
Definition: export_function.cpp:141
ExportFunction::acquire
virtual ExportFunction & acquire(ExportIndex &obj)
Definition: export_function.cpp:354
row
RowXpr row(Index i)
Definition: BlockMethods.h:725
ExportFunction::addVariable
ExportFunction & addVariable(const ExportVariable &_var)
Definition: export_function.cpp:368
acadoIsFinite
BooleanType acadoIsFinite(double x, double TOL)
Definition: acado_utils.cpp:164
Grid::getNumPoints
uint getNumPoints() const
ACADO_LOCAL
Definition: acado_types.hpp:808
ExportNLPSolver::evGu
ExportVariable evGu
Definition: export_nlp_solver.hpp:250
ExportNLPSolver::objSEndTerm
ExportVariable objSEndTerm
Definition: export_nlp_solver.hpp:265
ExportFunction::getName
std::string getName() const
Definition: export_function.cpp:164
ExportVariable::getRows
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:409
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
ExportFunction::release
virtual ExportFunction & release(const ExportIndex &obj)
Definition: export_function.cpp:361
ExportNLPSolver::getNumComplexConstraints
unsigned getNumComplexConstraints(void)
Definition: export_nlp_solver.cpp:1579
HOTSTART_QP
Definition: acado_types.hpp:388
MatrixVariablesGrid::getLowerBounds
DVector getLowerBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1344
next
Expression next(const Expression &arg)
Definition: acado_syntax.cpp:99
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
isFinite
BooleanType isFinite(const T &_value)
Definition: acado_utils.hpp:119
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
CG_EXPORT_FOLDER_NAME
Definition: acado_types.hpp:337
ExportStatementBlock::addFunction
returnValue addFunction(const ExportFunction &_function)
Definition: export_statement_block.cpp:108
export_qpoases_interface.hpp
CondensedHessianCholeskyDecomposition
CondensedHessianCholeskyDecomposition
Definition: acado_types.hpp:815
col
ColXpr col(Index i)
Definition: BlockMethods.h:708
ExportQpOasesInterface
A class for generating the glue code for interfacing qpOASES.
Definition: export_qpoases_interface.hpp:48
ExportNLPSolver::objEvFu
ExportVariable objEvFu
Definition: export_nlp_solver.hpp:266
QPSolverName
QPSolverName
Definition: acado_types.hpp:633
ExportGaussNewtonCn2Factorization::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: export_gauss_newton_cn2_factorization.cpp:163
ExportAlgorithm::getNU
uint getNU() const
Definition: export_algorithm.cpp:111
ExportGaussNewtonCn2Factorization::getFunctionDeclarations
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: export_gauss_newton_cn2_factorization.cpp:144
ExportStatementBlock
Allows to export code for a block of statements.
Definition: export_statement_block.hpp:63
ExportNLPSolver::initialize
ExportFunction initialize
Definition: export_nlp_solver.hpp:321
ExportArgument::getAddress
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
Definition: export_argument.cpp:103
QP_QPOASES3
Definition: acado_types.hpp:636
CG_CONDENSED_HESSIAN_CHOLESKY
Definition: acado_types.hpp:334
ExportNLPSolver::objValueOut
ExportVariable objValueOut
Definition: export_nlp_solver.hpp:269
EXTERNAL
Definition: acado_types.hpp:817
ExportVariable::getCol
ExportVariable getCol(const ExportIndex &idx) const
Definition: export_variable.cpp:402
ExportFunction::addIndex
ExportFunction & addIndex(const ExportIndex &_index)
Definition: export_function.cpp:347
ExportGaussNewtonCn2Factorization::ExportGaussNewtonCn2Factorization
ExportGaussNewtonCn2Factorization(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: export_gauss_newton_cn2_factorization.cpp:40
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
export_qpoases3_interface.hpp
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
ExportVariable::makeColVector
ExportVariable makeColVector() const
Definition: export_variable.cpp:448
ExportData::getName
std::string getName() const
Definition: export_data.cpp:86

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