export_gauss_newton_condensed.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_condensed.hpp>
33 #include <acado/code_generation/export_qpoases_interface.hpp>
34 #include <acado/code_generation/export_qpoases3_interface.hpp>
35 #include <acado/code_generation/export_module.hpp>
36 
37 using namespace std;
38 
39 BEGIN_NAMESPACE_ACADO
40 
41 ExportGaussNewtonCondensed::ExportGaussNewtonCondensed( UserInteraction* _userInteraction,
42  const std::string& _commonHeaderName
43  ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
44 {}
45 
46 returnValue ExportGaussNewtonCondensed::setup( )
47 {
48  LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
49  setupInitialization();
50  LOG( LVL_DEBUG ) << "done!" << endl;
51 
52  LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
53  setupVariables();
54  LOG( LVL_DEBUG ) << "done!" << endl;
55 
56  LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
57  setupMultiplicationRoutines();
58  LOG( LVL_DEBUG ) << "done!" << endl;
59 
60  LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
61  setupSimulation();
62  LOG( LVL_DEBUG ) << "done!" << endl;
63 
64  LOG( LVL_DEBUG ) << "Solver: setup arrival cost update... " << endl;
65  setupArrivalCostCalculation();
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 ExportGaussNewtonCondensed::getDataDeclarations( ExportStatementBlock& declarations,
92  ExportStruct dataStruct
93  ) const
94 {
95  returnValue status;
96  status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
97  if (status != SUCCESSFUL_RETURN)
98  return status;
99 
100  declarations.addDeclaration(x0, dataStruct);
101  declarations.addDeclaration(Dx0, dataStruct);
102 
103  declarations.addDeclaration(sigmaN, dataStruct);
104 
105  declarations.addDeclaration(T, dataStruct);
106  declarations.addDeclaration(E, dataStruct);
107  declarations.addDeclaration(QE, dataStruct);
108  declarations.addDeclaration(QGx, dataStruct);
109  declarations.addDeclaration(Qd, dataStruct);
110  declarations.addDeclaration(QDy, dataStruct);
111  declarations.addDeclaration(H10, dataStruct);
112 
113  declarations.addDeclaration(lbValues, dataStruct);
114  declarations.addDeclaration(ubValues, dataStruct);
115  declarations.addDeclaration(lbAValues, dataStruct);
116  declarations.addDeclaration(ubAValues, dataStruct);
117 
118  if (performFullCondensing() == true)
119  declarations.addDeclaration(A10, dataStruct);
120  declarations.addDeclaration(A20, dataStruct);
121 
122  declarations.addDeclaration(pacA01Dx0, dataStruct);
123  declarations.addDeclaration(pocA02Dx0, dataStruct);
124 
125  declarations.addDeclaration(CEN, dataStruct);
126  declarations.addDeclaration(sigmaTmp, dataStruct);
127  declarations.addDeclaration(sigma, dataStruct);
128 
129  declarations.addDeclaration(H, dataStruct);
130  declarations.addDeclaration(R, dataStruct);
131  declarations.addDeclaration(A, dataStruct);
132  declarations.addDeclaration(g, dataStruct);
133  declarations.addDeclaration(lb, dataStruct);
134  declarations.addDeclaration(ub, dataStruct);
135  declarations.addDeclaration(lbA, dataStruct);
136  declarations.addDeclaration(ubA, dataStruct);
137  declarations.addDeclaration(xVars, dataStruct);
138  declarations.addDeclaration(yVars, dataStruct);
139 
140  return SUCCESSFUL_RETURN;
141 }
142 
143 returnValue ExportGaussNewtonCondensed::getFunctionDeclarations( ExportStatementBlock& declarations
144  ) const
145 {
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( updateArrivalCost );
158 
159  declarations.addDeclaration( evaluateStageCost );
160  declarations.addDeclaration( evaluateTerminalCost );
161 
162  return SUCCESSFUL_RETURN;
163 }
164 
165 returnValue ExportGaussNewtonCondensed::getCode( ExportStatementBlock& code
166  )
167 {
168  setupQPInterface();
169 
170  code.addLinebreak( 2 );
171  code.addStatement( "/******************************************************************************/\n" );
172  code.addStatement( "/* */\n" );
173  code.addStatement( "/* ACADO code generation */\n" );
174  code.addStatement( "/* */\n" );
175  code.addStatement( "/******************************************************************************/\n" );
176  code.addLinebreak( 2 );
177 
178  int useOMP;
179  get(CG_USE_OPENMP, useOMP);
180  if ( useOMP )
181  {
182  code.addDeclaration( state );
183  }
184 
185  code.addFunction( modelSimulation );
186 
187  code.addFunction( evaluateStageCost );
188  code.addFunction( evaluateTerminalCost );
189  code.addFunction( setObjQ1Q2 );
190  code.addFunction( setObjR1R2 );
191  code.addFunction( setObjQN1QN2 );
192  code.addFunction( evaluateObjective );
193 
194  code.addFunction( multGxd );
195  code.addFunction( moveGxT );
196  code.addFunction( multGxGx );
197  code.addFunction( multGxGu );
198  code.addFunction( moveGuE );
199  code.addFunction( setBlockH11 );
200  code.addFunction( setBlockH11_R1 );
201  code.addFunction( zeroBlockH11 );
202  code.addFunction( copyHTH );
203  code.addFunction( multQ1d );
204  code.addFunction( multQN1d );
205  code.addFunction( multRDy );
206  code.addFunction( multQDy );
207  code.addFunction( multEQDy );
208  code.addFunction( multQETGx );
209  code.addFunction( zeroBlockH10 );
210  code.addFunction( multEDu );
211  code.addFunction( multQ1Gx );
212  code.addFunction( multQN1Gx );
213  code.addFunction( multQ1Gu );
214  code.addFunction( multQN1Gu );
215  code.addFunction( zeroBlockH00 );
216  code.addFunction( multCTQC );
217 
218  code.addFunction( multHxC );
219  code.addFunction( multHxE );
220  code.addFunction( macHxd );
221 
222  code.addFunction( evaluatePathConstraints );
223 
224  for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
225  {
226  if (evaluatePointConstraints[ i ] == 0)
227  continue;
228  code.addFunction( *evaluatePointConstraints[ i ] );
229  }
230 
231  code.addFunction( macCTSlx );
232  code.addFunction( macETSlu );
233 
234  cholSolver.getCode( code );
235 
236  code.addFunction( condensePrep );
237  code.addFunction( condenseFdb );
238  code.addFunction( expand );
239  code.addFunction( calculateCovariance );
240 
241  code.addFunction( preparation );
242  code.addFunction( feedback );
243 
244  code.addFunction( initialize );
245  code.addFunction( initializeNodes );
246  code.addFunction( shiftStates );
247  code.addFunction( shiftControls );
248  code.addFunction( getKKT );
249  code.addFunction( getObjective );
250 
251  int useArrivalCost;
252  get(CG_USE_ARRIVAL_COST, useArrivalCost);
253  if (useArrivalCost == YES)
254  {
255  acSolver.getCode( code );
256  cholObjS.getCode( code );
257  cholSAC.getCode( code );
258  code.addFunction( updateArrivalCost );
259  }
260 
261  return SUCCESSFUL_RETURN;
262 }
263 
264 
265 unsigned ExportGaussNewtonCondensed::getNumQPvars( ) const
266 {
267  if (performFullCondensing() == true)
268  return (N * NU);
269 
270  return (NX + N * NU);
271 }
272 
273 unsigned ExportGaussNewtonCondensed::getNumStateBounds() const
274 {
275  return xBoundsIdx.size();
276 }
277 
278 //
279 // PROTECTED FUNCTIONS:
280 //
281 
282 returnValue ExportGaussNewtonCondensed::setupObjectiveEvaluation( void )
283 {
284  evaluateObjective.setup("evaluateObjective");
285 
286  int variableObjS;
287  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
288 
289  if (S1.getGivenMatrix().isZero() == false)
290  ACADOWARNINGTEXT(RET_INVALID_ARGUMENTS,
291  "Mixed control-state terms in the objective function are not supported at the moment.");
292 
293  //
294  // A loop the evaluates objective and corresponding gradients
295  //
296  ExportIndex runObj( "runObj" );
297  ExportForLoop loopObjective( runObj, 0, N );
298 
299  evaluateObjective.addIndex( runObj );
300 
301  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
302  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
303  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
304  loopObjective.addLinebreak( );
305 
306  // Evaluate the objective function
307  loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
308 
309  // Stack the measurement function value
310  loopObjective.addStatement(
311  Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
312  );
313  loopObjective.addLinebreak( );
314 
315  // Optionally compute derivatives
316  unsigned indexX = getNY();
317 // unsigned indexG = indexX;
318 
319  ExportVariable tmpObjS, tmpFx, tmpFu;
320  ExportVariable tmpFxEnd, tmpObjSEndTerm;
321  tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
322  if (objS.isGiven() == true)
323  tmpObjS = objS;
324  tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
325  if (objEvFx.isGiven() == true)
326  tmpFx = objEvFx;
327  tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
328  if (objEvFu.isGiven() == true)
329  tmpFu = objEvFu;
330  tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
331  if (objEvFxEnd.isGiven() == true)
332  tmpFxEnd = objEvFxEnd;
333  tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
334  if (objSEndTerm.isGiven() == true)
335  tmpObjSEndTerm = objSEndTerm;
336 
337  //
338  // Optional computation of Q1, Q2
339  //
340  if (Q1.isGiven() == false)
341  {
342  ExportVariable tmpQ1, tmpQ2;
343  tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
344  tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
345 
346  setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
347  setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
348  setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
349 
350  if (tmpFx.isGiven() == true)
351  {
352  if (variableObjS == YES)
353  {
354  loopObjective.addFunctionCall(
355  setObjQ1Q2,
356  tmpFx, objS.getAddress(runObj * NY, 0),
357  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
358  );
359  }
360  else
361  {
362  loopObjective.addFunctionCall(
363  setObjQ1Q2,
364  tmpFx, objS,
365  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
366  );
367  }
368  }
369  else
370  {
371  if (variableObjS == YES)
372  {
373  loopObjective.addFunctionCall(
374  setObjQ1Q2,
375  objValueOut.getAddress(0, indexX), 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  objValueOut.getAddress(0, indexX), objS,
384  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
385  );
386  }
387  indexX += objEvFx.getDim();
388  }
389 
390  loopObjective.addLinebreak( );
391  }
392 
393  if (R1.isGiven() == false)
394  {
395  ExportVariable tmpR1, tmpR2;
396  tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
397  tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
398 
399  setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
400  setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
401  setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
402 
403  if (tmpFu.isGiven() == true)
404  {
405  if (variableObjS == YES)
406  {
407  loopObjective.addFunctionCall(
408  setObjR1R2,
409  tmpFu, objS.getAddress(runObj * NY, 0),
410  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
411  );
412  }
413  else
414  {
415  loopObjective.addFunctionCall(
416  setObjR1R2,
417  tmpFu, objS,
418  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
419  );
420  }
421  }
422  else
423  {
424  if (variableObjS == YES)
425  {
426  loopObjective.addFunctionCall(
427  setObjR1R2,
428  objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
429  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
430  );
431  }
432  else
433  {
434  loopObjective.addFunctionCall(
435  setObjR1R2,
436  objValueOut.getAddress(0, indexX), objS,
437  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
438  );
439  }
440  }
441 
442  loopObjective.addLinebreak( );
443  }
444 
445  evaluateObjective.addStatement( loopObjective );
446 
447  //
448  // Evaluate the quadratic Mayer term
449  //
450  evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
451  evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
452 
453  // Evaluate the objective function, last node.
454  evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
455  evaluateObjective.addLinebreak( );
456 
457  evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
458  evaluateObjective.addLinebreak();
459 
460  if (QN1.isGiven() == false)
461  {
462  indexX = getNYN();
463 
464  ExportVariable tmpQN1, tmpQN2;
465  tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
466  tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
467 
468  setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
469  setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
470  setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
471 
472  if (tmpFxEnd.isGiven() == true)
473  evaluateObjective.addFunctionCall(
474  setObjQN1QN2,
475  tmpFxEnd, objSEndTerm,
476  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
477  );
478  else
479  evaluateObjective.addFunctionCall(
480  setObjQN1QN2,
481  objValueOut.getAddress(0, indexX), objSEndTerm,
482  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
483  );
484 
485  evaluateObjective.addLinebreak( );
486  }
487 
488  return SUCCESSFUL_RETURN;
489 }
490 
491 returnValue ExportGaussNewtonCondensed::setupConstraintsEvaluation( void )
492 {
493  ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
494 
495  int hardcodeConstraintValues;
496  get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
497 
499  //
500  // Determine dimensions of constraints
501  //
503 
504  unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
505  unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
506  unsigned numStateBounds = getNumStateBounds();
507  unsigned numPathCon = N * dimPacH;
508  unsigned numPointCon = dimPocH;
509 
511  //
512  // Setup the bounds on independent variables
513  //
515 
516  DVector lbBoundValues( numBounds );
517  DVector ubBoundValues( numBounds );
518 
519  if (initialStateFixed( ) == false)
520  for(unsigned el = 0; el < NX; ++el)
521  {
522  lbBoundValues( el )= xBounds.getLowerBound(0, el);
523  ubBoundValues( el ) = xBounds.getUpperBound(0, el);
524  }
525 
526  for(unsigned node = 0; node < N; ++node)
527  for(unsigned el = 0; el < NU; ++el)
528  {
529  lbBoundValues(offsetBounds + node * NU + el) = uBounds.getLowerBound(node, el);
530  ubBoundValues(offsetBounds + node * NU + el) = uBounds.getUpperBound(node, el);
531  }
532 
533  if (hardcodeConstraintValues == YES || !(isFinite( lbBoundValues ) || isFinite( ubBoundValues )))
534  {
535  lbValues.setup("lbValues", lbBoundValues, REAL, ACADO_VARIABLES);
536  ubValues.setup("ubValues", ubBoundValues, REAL, ACADO_VARIABLES);
537  }
538  else if (isFinite( lbBoundValues ) || isFinite( ubBoundValues ))
539  {
540  lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
541  lbValues.setDoc( "Lower bounds values." );
542  ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
543  ubValues.setDoc( "Upper bounds values." );
544 
545  initialize.addStatement( lbValues == lbBoundValues );
546  initialize.addStatement( ubValues == ubBoundValues );
547  }
548 
549  ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
550 
551  if (performFullCondensing() == true)
552  {
553  // Full condensing case
554  boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
555  boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
556  }
557  else
558  {
559  // Partial condensing case
560  if (initialStateFixed() == true)
561  {
562  // MPC case
563  condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
564  condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
565 
566  boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
567  boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
568  }
569  else
570  {
571  // MHE case
572  boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
573  boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
574  boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
575  boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
576  }
577  }
578  boundSetFcn->addLinebreak( );
579 
581  //
582  // Determine number of affine constraints and set up structures that
583  // holds constraint values
584  //
586 
587  unsigned sizeA = numStateBounds + getNumComplexConstraints();
588 
589  if ( sizeA )
590  {
591  DVector lbTmp, ubTmp;
592 
593  if ( numStateBounds )
594  {
595  DVector lbStateBoundValues( numStateBounds );
596  DVector ubStateBoundValues( numStateBounds );
597  for (unsigned i = 0; i < numStateBounds; ++i)
598  {
599  lbStateBoundValues( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
600  ubStateBoundValues( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
601  }
602 
603  lbTmp.append( lbStateBoundValues );
604  ubTmp.append( ubStateBoundValues );
605  }
606 
607  lbTmp.append( lbPathConValues );
608  ubTmp.append( ubPathConValues );
609 
610  lbTmp.append( lbPointConValues );
611  ubTmp.append( ubPointConValues );
612 
613  if (hardcodeConstraintValues == true)
614  {
615  lbAValues.setup("lbAValues", lbTmp, REAL, ACADO_VARIABLES);
616  ubAValues.setup("ubAValues", ubTmp, REAL, ACADO_VARIABLES);
617  }
618  else
619  {
620  lbAValues.setup("lbAValues", sizeA, 1, REAL, ACADO_VARIABLES);
621  lbAValues.setDoc( "Lower bounds values for affine constraints." );
622  ubAValues.setup("ubAValues", sizeA, 1, REAL, ACADO_VARIABLES);
623  ubAValues.setDoc( "Upper bounds values for affine constraints." );
624 
625  initialize.addStatement( lbAValues == lbTmp );
626  initialize.addStatement( ubAValues == ubTmp );
627  }
628  }
629 
631  //
632  // Setup the bounds on state variables
633  //
635 
636  if ( numStateBounds )
637  {
638  condenseFdb.addVariable( tmp );
639 
640  unsigned offset = (performFullCondensing() == true) ? 0 : NX;
641  unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
642 
643  if (numOps < 1024)
644  {
645  for(unsigned row = 0; row < numStateBounds; ++row)
646  {
647  unsigned conIdx = xBoundsIdx[ row ] - NX;
648 
649  if (performFullCondensing() == false)
650  condensePrep.addStatement( A.getSubMatrix(row, row + 1, 0, NX) == evGx.getRow( conIdx ) );
651 
652  unsigned blk = conIdx / NX + 1;
653  for (unsigned col = 0; col < blk; ++col)
654  {
655  unsigned blkRow = (blk * (blk - 1) / 2 + col) * NX + conIdx % NX;
656 
657  condensePrep.addStatement(
658  A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
659  }
660 
661  condensePrep.addLinebreak();
662  }
663  }
664  else
665  {
666  DMatrix vXBoundIndices(1, numStateBounds);
667  for (unsigned i = 0; i < numStateBounds; ++i)
668  vXBoundIndices(0, i) = xBoundsIdx[ i ];
669  ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
670 
671  condensePrep.addVariable( evXBounds );
672 
673  ExportIndex row, col, conIdx, blk, blkRow;
674 
675  condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );
676 
677  ExportForLoop lRow(row, 0, numStateBounds);
678 
679  lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
680  lRow.addStatement( blk == conIdx / NX + 1);
681 
682  if (performFullCondensing() == false)
683  lRow.addStatement( A.getSubMatrix(row, row + 1, 0, NX) == evGx.getRow( conIdx ) );
684 
685  ExportForLoop lCol(col, 0, blk);
686 
687  lCol.addStatement( blkRow == (blk * (blk - 1) / 2 + col ) * NX + conIdx % NX );
688  lCol.addStatement(
689  A.getSubMatrix(row, row + 1, offset + col * NU, offset + (col + 1) * NU ) == E.getRow( blkRow ) );
690 
691  lRow.addStatement( lCol );
692  condensePrep.addStatement( lRow );
693 
694  condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
695  }
696  condensePrep.addLinebreak( );
697 
698  // shift constraint bounds by first interval
699  for(unsigned run1 = 0; run1 < numStateBounds; ++run1)
700  {
701  unsigned row = xBoundsIdx[ run1 ];
702 
703  if (performFullCondensing() == true)
704  {
705  if (performsSingleShooting() == true)
706  {
707  condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + evGx.getRow(row - NX) * Dx0 );
708  }
709  else
710  {
711  condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + evGx.getRow(row - NX) * Dx0 );
712  condenseFdb.addStatement( tmp += d.getRow(row - NX) );
713  }
714  condenseFdb.addStatement( lbA.getRow( run1 ) == lbAValues.getRow( run1 ) - tmp );
715  condenseFdb.addStatement( ubA.getRow( run1 ) == ubAValues.getRow( run1 ) - tmp );
716  }
717  else
718  {
719  if (performsSingleShooting() == true)
720  condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) );
721  else
722  condenseFdb.addStatement( tmp == x.makeRowVector().getCol( row ) + d.getRow(row - NX) );
723 
724  condenseFdb.addStatement( lbA.getRow( run1 ) == lbAValues.getRow( run1 ) - tmp );
725  condenseFdb.addStatement( ubA.getRow( run1 ) == ubAValues.getRow( run1 ) - tmp );
726  }
727  }
728  condenseFdb.addLinebreak( );
729  }
730 
732  //
733  // Setup the evaluation of the path constraints
734  //
736 
737  if (getNumComplexConstraints() == 0)
738  return SUCCESSFUL_RETURN;
739 
740  if ( numPathCon )
741  {
742  unsigned rowOffset = numStateBounds;
743  unsigned colOffset = performFullCondensing() == true ? 0 : NX;
744 
745  //
746  // Setup evaluation
747  //
748  ExportIndex runPac;
749  condensePrep.acquire( runPac );
750  ExportForLoop loopPac(runPac, 0, N);
751 
752  loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
753  loopPac.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( runPac ) );
754  loopPac.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runPac ) );
755  loopPac.addFunctionCall( evaluatePathConstraints.getName(), conValueIn, conValueOut );
756 
757  loopPac.addStatement( pacEvH.getRows( runPac * dimPacH, (runPac + 1) * dimPacH) ==
758  conValueOut.getTranspose().getRows(0, dimPacH) );
759  loopPac.addLinebreak( );
760 
761  unsigned derOffset = dimPacH;
762 
763  // Optionally store derivatives
764  if ( pacEvHx.isGiven() == false )
765  {
766  loopPac.addStatement(
767  pacEvHx.makeRowVector().
768  getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX)
769  == conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
770  );
771 
772  derOffset = derOffset + dimPacH * NX;
773  }
774  if (pacEvHu.isGiven() == false )
775  {
776  loopPac.addStatement(
777  pacEvHu.makeRowVector().
778  getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU)
779  == conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
780  );
781  }
782 
783  // Add loop to the function.
784  condensePrep.addStatement( loopPac );
785  condensePrep.addLinebreak( );
786 
787  // Define the multHxC multiplication routine
788  ExportVariable tmpA01, tmpHx, tmpGx;
789 
790  if (pacEvHx.isGiven() == true)
791  tmpHx = pacEvHx;
792  else
793  tmpHx.setup("Hx", dimPacH, NX, REAL, ACADO_LOCAL);
794 
795  tmpGx.setup("Gx", NX, NX, REAL, ACADO_LOCAL);
796 
797  if (performFullCondensing() == true)
798  {
799  tmpA01.setup("A01", dimPacH, NX, REAL, ACADO_LOCAL);
800 
801  multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
802  multHxC.addStatement( tmpA01 == tmpHx * tmpGx );
803 
804  A10.setup("A01", numPathCon, NX, REAL, ACADO_WORKSPACE);
805  }
806  else
807  {
808  tmpA01.setup("A01", dimPacH, getNumQPvars(), REAL, ACADO_LOCAL);
809  multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
810  multHxC.addStatement( tmpA01.getSubMatrix(0, dimPacH, 0, NX) == tmpHx * tmpGx );
811 
812  A10 = A;
813  }
814 
815  unsigned offsetA01 = (performFullCondensing() == true) ? 0 : rowOffset;
816 
817  // Define the block A_{10}(0: dimPacH, 0: NX) = H_{x}(0: dimPacH, 0: NX)
818  if (pacEvHx.isGiven() == true)
819  {
820  condensePrep.addStatement(
821  A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
822  == pacEvHx);
823  }
824  else
825  {
826  condensePrep.addStatement(
827  A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
828  == pacEvHx.getSubMatrix(0, dimPacH, 0, NX));
829  }
830  condensePrep.addLinebreak();
831 
832  // Evaluate Hx * C
833  for (unsigned row = 0; row < N - 1; ++row)
834  {
835  if (pacEvHx.isGiven() == true)
836  {
837  condensePrep.addFunctionCall(
838  multHxC,
839  pacEvHx,
840  evGx.getAddress(row * NX, 0),
841  A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
842  }
843  else
844  {
845  condensePrep.addFunctionCall(
846  multHxC,
847  pacEvHx.getAddress((row + 1) * dimPacH, 0),
848  evGx.getAddress(row * NX, 0),
849  A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
850  }
851  }
852  condensePrep.addLinebreak();
853 
854  //
855  // Evaluate Hx * E
856  //
857  ExportVariable tmpE;
858  tmpE.setup("E", NX, NU, REAL, ACADO_LOCAL);
859  ExportIndex iRow( "row" ), iCol( "col" );
860 
861  multHxE.setup("multHxE", tmpHx, tmpE, iRow, iCol);
862  multHxE.addStatement(
863  A.getSubMatrix( rowOffset + iRow * dimPacH,
864  rowOffset + (iRow + 1) * dimPacH,
865  colOffset + iCol * NU,
866  colOffset + (iCol + 1) * NU)
867  == tmpHx * tmpE );
868 
869  if ( N <= 20 )
870  {
871  for (unsigned row = 0; row < N - 1; ++row)
872  {
873  for (unsigned col = 0; col <= row; ++col)
874  {
875  unsigned blk = (row + 1) * row / 2 + col;
876  unsigned row2 = row + 1;
877 
878  if (pacEvHx.isGiven() == true)
879  condensePrep.addFunctionCall(
880  multHxE,
881  pacEvHx,
882  E.getAddress(blk * NX, 0),
883  ExportIndex( row2 ),
884  ExportIndex( col )
885  );
886  else
887  condensePrep.addFunctionCall(
888  multHxE,
889  pacEvHx.getAddress((row + 1) * dimPacH, 0),
890  E.getAddress(blk * NX, 0),
891  ExportIndex( row2 ),
892  ExportIndex( col )
893  );
894  }
895  }
896  }
897  else
898  {
899  ExportIndex row, col, blk, row2;
900  condensePrep.acquire( row );
901  condensePrep.acquire( col );
902  condensePrep.acquire( blk );
903  condensePrep.acquire( row2 );
904 
905  ExportForLoop eLoopI(row, 0, N - 1);
906  ExportForLoop eLoopJ(col, 0, row + 1);
907 
908  eLoopJ.addStatement( blk == (row + 1) * row / 2 + col );
909  eLoopJ.addStatement( row2 == row + 1 );
910 
911  if (pacEvHx.isGiven() == true)
912  {
913  eLoopJ.addFunctionCall(
914  multHxE,
915  pacEvHx,
916  E.getAddress(blk * NX, 0),
917  row2,
918  col
919  );
920  }
921  else
922  {
923  eLoopJ.addFunctionCall(
924  multHxE,
925  pacEvHx.getAddress((row + 1) * dimPacH, 0),
926  E.getAddress(blk * NX, 0),
927  row2,
928  col
929  );
930  }
931 
932  eLoopI.addStatement( eLoopJ );
933  condensePrep.addStatement( eLoopI );
934 
935  condensePrep.release( row );
936  condensePrep.release( col );
937  condensePrep.release( blk );
938  condensePrep.release( row2 );
939  }
940  condensePrep.addLinebreak();
941 
942  if (pacEvHu.getDim() > 0)
943  {
944  for (unsigned i = 0; i < N; ++i)
945  {
946  if (pacEvHu.isGiven() == true)
947  initialize.addStatement(
948  A.getSubMatrix(
949  rowOffset + i * dimPacH,
950  rowOffset + (i + 1) * dimPacH,
951  colOffset + i * NU,
952  colOffset + (i + 1) * NU)
953  == pacEvHu
954  );
955  else
956  condensePrep.addStatement(
957  A.getSubMatrix(
958  rowOffset + i * dimPacH,
959  rowOffset + (i + 1) * dimPacH,
960  colOffset + i * NU,
961  colOffset + (i + 1) * NU)
962  == pacEvHu.getSubMatrix(
963  i * dimPacH,(i + 1) * dimPacH, 0, NU)
964  );
965  }
966  }
967 
968  //
969  // Set upper and lower bounds
970  //
971  condensePrep.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) ==
972  lbAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
973  condensePrep.addLinebreak();
974  condensePrep.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) ==
975  ubAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
976  condensePrep.addLinebreak();
977 
978  if (performFullCondensing() == true)
979  {
980  pacA01Dx0.setup("pacA01Dx0", numPathCon, 1, REAL, ACADO_WORKSPACE);
981 
982  condenseFdb.addStatement( pacA01Dx0 == A10 * Dx0 );
983  condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
984  condenseFdb.addLinebreak();
985  condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
986  condenseFdb.addLinebreak();
987  }
988 
989  // Evaluate Hx * d
990  if ( performsSingleShooting() == false )
991  {
992  ExportVariable tmpd("tmpd", NX, 1, REAL, ACADO_LOCAL);
993  ExportVariable tmpLb("lbA", dimPacH, 1, REAL, ACADO_LOCAL);
994  ExportVariable tmpUb("ubA", dimPacH, 1, REAL, ACADO_LOCAL);
995 
996  macHxd.setup("macHxd", tmpHx, tmpd, tmpLb, tmpUb);
997  macHxd.addStatement( pacEvHxd == tmpHx * tmpd );
998  macHxd.addStatement( tmpLb -= pacEvHxd );
999  macHxd.addStatement( tmpUb -= pacEvHxd );
1000 
1001  for (unsigned i = 0; i < N - 1; ++i)
1002  {
1003  if (pacEvHx.isGiven() == true)
1004  {
1005  condensePrep.addFunctionCall(
1006  macHxd,
1007  pacEvHx,
1008  d.getAddress(i * NX),
1009  lbA.getAddress(rowOffset + (i + 1) * dimPacH),
1010  ubA.getAddress(rowOffset + (i + 1) * dimPacH)
1011  );
1012  }
1013  else
1014  {
1015  condensePrep.addFunctionCall(
1016  macHxd,
1017  pacEvHx.getAddress((i + 1) * dimPacH, 0),
1018  d.getAddress(i * NX),
1019  lbA.getAddress(rowOffset + (i + 1) * dimPacH),
1020  ubA.getAddress(rowOffset + (i + 1) * dimPacH)
1021  );
1022  }
1023  }
1024  condensePrep.addLinebreak();
1025  }
1026  }
1027 
1029  //
1030  // Setup the evaluation of the point constraints
1031  //
1033 
1034  if ( numPointCon )
1035  {
1036  unsigned rowOffset = getNumStateBounds() + N * dimPacH;
1037  unsigned colOffset = performFullCondensing() == true ? 0 : NX;
1038  unsigned dim;
1039 
1040  if (performFullCondensing() == true)
1041  A20.setup("A02", dimPocH, NX, REAL, ACADO_WORKSPACE);
1042 
1043  //
1044  // Evaluate the point constraints
1045  //
1046  for (unsigned i = 0, intRowOffset = 0; i < N + 1; ++i)
1047  {
1048  if (evaluatePointConstraints[ i ] == 0)
1049  continue;
1050 
1051  condensePrep.addComment( string( "Evaluating constraint on node: #" ) + toString( i ) );
1052  condensePrep.addLinebreak();
1053 
1054  condensePrep.addStatement(conValueIn.getCols(0, getNX()) == x.getRow( i ) );
1055  if (i < N)
1056  {
1057  condensePrep.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
1058  condensePrep.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
1059  }
1060  else
1061  condensePrep.addStatement( conValueIn.getCols(NX, NX + NOD) == od.getRow( i ) );
1062 
1063  condensePrep.addFunctionCall( evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
1064  condensePrep.addLinebreak();
1065 
1066  if (i < N)
1067  dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
1068  else
1069  dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
1070 
1071  // Fill pocEvH, pocEvHx, pocEvHu
1072  condensePrep.addStatement(
1073  pocEvH.getRows(intRowOffset, intRowOffset + dim)
1074  == conValueOut.getTranspose().getRows(0, dim));
1075  condensePrep.addLinebreak();
1076 
1077  condensePrep.addStatement(
1078  pocEvHx.makeRowVector().getCols(intRowOffset * NX,
1079  (intRowOffset + dim) * NX)
1080  == conValueOut.getCols(dim, dim + dim * NX));
1081  condensePrep.addLinebreak();
1082 
1083  if (i < N)
1084  {
1085  condensePrep.addStatement(
1086  pocEvHu.makeRowVector().getCols(intRowOffset * NU,
1087  (intRowOffset + dim) * NU)
1088  == conValueOut.getCols(dim + dim * NX,
1089  dim + dim * NX + dim * NU));
1090  condensePrep.addLinebreak();
1091  }
1092 
1093  intRowOffset += dim;
1094  }
1095 
1096  //
1097  // Include point constraint data in the QP problem
1098  //
1099  for (unsigned row = 0, intRowOffset = 0; row < N + 1; ++row)
1100  {
1101  if (evaluatePointConstraints[ row ] == 0)
1102  continue;
1103 
1104  condensePrep.addComment(
1105  string( "Evaluating multiplications of constraint functions on node: #" ) + toString( row ) );
1106  condensePrep.addLinebreak();
1107 
1108  if (row < N)
1109  dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX + NU);
1110  else
1111  dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX);
1112 
1113  if (row == 0)
1114  {
1115  if (performFullCondensing() == true)
1116  condensePrep.addStatement(
1117  A20.getSubMatrix(0, dim, 0, NX)
1118  == pocEvHx.getSubMatrix(0, dim, 0, NX));
1119  else
1120  condensePrep.addStatement(
1121  A.getSubMatrix(rowOffset, rowOffset + dim, 0, NX)
1122  == pocEvHx.getSubMatrix(0, dim, 0, NX));
1123  condensePrep.addLinebreak();
1124 
1125  condensePrep.addStatement(
1126  A.getSubMatrix(rowOffset, rowOffset + dim, colOffset,
1127  colOffset + NU)
1128  == pocEvHu.getSubMatrix(0, dim, 0, NU));
1129  condensePrep.addLinebreak();
1130  }
1131  else
1132  {
1133  // Hx * C
1134  if (performFullCondensing() == true)
1135  condensePrep.addStatement(
1136  A20.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) ==
1137  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
1138  evGx.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
1139  else
1140  condensePrep.addStatement(
1141  A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim, 0, NX) ==
1142  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
1143  evGx.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
1144  condensePrep.addLinebreak();
1145 
1146  // Hx * E
1147  ExportIndex iCol, iBlk;
1148  condensePrep.acquire( iCol );
1149  condensePrep.acquire( iBlk );
1150  ExportForLoop eLoop(iCol, 0, row);
1151 
1152  // row - 1, col -> blk
1153  eLoop.addStatement( iBlk == row * (row - 1) / 2 + iCol );
1154  eLoop.addStatement(
1155  A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
1156  colOffset + iCol * NU, colOffset + (iCol + 1) * NU) ==
1157  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
1158  E.getSubMatrix(iBlk * NX, (iBlk + 1) * NX, 0, NU)
1159  );
1160 
1161  condensePrep.addStatement( eLoop );
1162  condensePrep.release( iCol );
1163  condensePrep.release( iBlk );
1164 
1165  // Hx * d, MS only
1166  if (performsSingleShooting() == false)
1167  {
1168  condensePrep.addStatement(
1169  pocEvHxd.getRows(intRowOffset, intRowOffset + dim) ==
1170  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
1171  d.getRows((row - 1) * NX, row * NX) );
1172  condensePrep.addLinebreak();
1173  }
1174 
1175  // Add Hu block to the A21 block
1176  if (row < N)
1177  {
1178  condensePrep.addStatement(
1179  A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
1180  colOffset + row * NU, colOffset + (row + 1) * NU) ==
1181  pocEvHu.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NU));
1182  condensePrep.addLinebreak();
1183  }
1184  }
1185 
1186  intRowOffset += dim;
1187  }
1188 
1189  //
1190  // And now setup the lbA and ubA
1191  //
1192  condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) ==
1193  lbAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
1194  condensePrep.addLinebreak();
1195  condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) ==
1196  ubAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
1197  condensePrep.addLinebreak();
1198 
1199  if (performFullCondensing() == true)
1200  {
1201  pocA02Dx0.setup("pacA02Dx0", dimPocH, 1, REAL, ACADO_WORKSPACE);
1202 
1203  condenseFdb.addStatement( pocA02Dx0 == A20 * Dx0 );
1204  condenseFdb.addLinebreak();
1205  condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
1206  condenseFdb.addLinebreak();
1207  condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
1208  condenseFdb.addLinebreak();
1209  }
1210 
1211  if (performsSingleShooting() == false)
1212  {
1213  condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
1214  condensePrep.addLinebreak();
1215  condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
1216  condensePrep.addLinebreak();
1217  }
1218  }
1219 
1220  return SUCCESSFUL_RETURN;
1221 }
1222 
1223 returnValue ExportGaussNewtonCondensed::setupCondensing( void )
1224 {
1225  //
1226  // Define LM regularization terms
1227  //
1228  DMatrix mRegH00 = eye<double>( getNX() );
1229  mRegH00 *= levenbergMarquardt;
1230 
1231  condensePrep.setup("condensePrep");
1232  condenseFdb.setup( "condenseFdb" );
1233 
1235  //
1236  // Create block matrices C (alias evGx) and E
1237  //
1239 
1240  LOG( LVL_DEBUG ) << "Setup condensing: create C & E matrices" << endl;
1241 
1242  // Special case, row = col = 0
1243  condensePrep.addFunctionCall(moveGuE, evGu.getAddress(0, 0), E.getAddress(0, 0) );
1244 
1245  if (N <= 20)
1246  {
1247  unsigned row, col, prev, curr;
1248  for (row = 1; row < N; ++row)
1249  {
1250  condensePrep.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T);
1251 
1252  if (performsSingleShooting() == false)
1253  condensePrep.addFunctionCall(multGxd, d.getAddress((row - 1) * NX), evGx.getAddress(row * NX), d.getAddress(row * NX));
1254 
1255  condensePrep.addFunctionCall(multGxGx, T, evGx.getAddress((row - 1) * NX, 0), evGx.getAddress(row * NX, 0));
1256  condensePrep.addLinebreak();
1257 
1258  for(col = 0; col < row; ++col)
1259  {
1260  prev = row * (row - 1) / 2 + col;
1261  curr = (row + 1) * row / 2 + col;
1262 
1263  condensePrep.addFunctionCall(multGxGu, T, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0));
1264  }
1265  condensePrep.addLinebreak();
1266 
1267  curr = (row + 1) * row / 2 + col;
1268  condensePrep.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
1269 
1270  condensePrep.addLinebreak();
1271  }
1272  }
1273  else
1274  {
1275  ExportIndex row, col, curr, prev;
1276 
1277  condensePrep.acquire( row );
1278  condensePrep.acquire( col );
1279  condensePrep.acquire( curr );
1280  condensePrep.acquire( prev );
1281 
1282  ExportForLoop eLoopI(row, 1, N);
1283  ExportForLoop eLoopJ(col, 0, row);
1284 
1285  eLoopI.addFunctionCall(moveGxT, evGx.getAddress(row* NX, 0), T);
1286 
1287  if (performsSingleShooting() == false)
1288  eLoopI.addFunctionCall(multGxd, d.getAddress((row - 1) * NX), evGx.getAddress(row * NX), d.getAddress(row * NX));
1289 
1290  eLoopI.addFunctionCall(multGxGx, T, evGx.getAddress((row - 1) * NX, 0), evGx.getAddress(row * NX, 0));
1291 
1292  eLoopJ.addStatement( prev == row * (row - 1) / 2 + col );
1293  eLoopJ.addStatement( curr == (row + 1) * row / 2 + col );
1294  eLoopJ.addFunctionCall( multGxGu, T, E.getAddress(prev * NX, 0), E.getAddress(curr * NX, 0) );
1295 
1296  eLoopI.addStatement( eLoopJ );
1297  eLoopI.addStatement( curr == (row + 1) * row / 2 + col );
1298  eLoopI.addFunctionCall(moveGuE, evGu.getAddress(row * NX, 0), E.getAddress(curr * NX, 0) );
1299 
1300  condensePrep.addStatement( eLoopI );
1301  condensePrep.addLinebreak();
1302 
1303  condensePrep.release( row );
1304  condensePrep.release( col );
1305  condensePrep.release( curr );
1306  condensePrep.release( prev );
1307  }
1308 
1309  //
1310  // Multiply Gx and E blocks with Q1.
1311  //
1312  LOG( LVL_DEBUG ) << "Setup condensing: multiply with Q1" << endl;
1313 
1314  if (performFullCondensing() == false)
1315  {
1316  for (unsigned i = 0; i < N - 1; ++i)
1317  if (Q1.isGiven() == true)
1318  condensePrep.addFunctionCall(multQ1Gx, evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
1319  else
1320  condensePrep.addFunctionCall(multGxGx, Q1.getAddress((i + 1) * NX, 0), evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
1321 
1322  if (QN1.isGiven() == true)
1323  condensePrep.addFunctionCall(multQN1Gx, evGx.getAddress((N - 1) * NX, 0), QGx.getAddress((N - 1) * NX, 0));
1324  else
1325  condensePrep.addFunctionCall(multGxGx, QN1, evGx.getAddress((N - 1) * NX, 0), QGx.getAddress((N - 1) * NX, 0));
1326  condensePrep.addLinebreak();
1327  }
1328 
1329  if (N <= 20)
1330  {
1331  for (unsigned i = 0; i < N; ++i)
1332  for (unsigned j = 0; j <= i; ++j)
1333  {
1334  unsigned k = (i + 1) * i / 2 + j;
1335 
1336  if (i < N - 1)
1337  {
1338  if (Q1.isGiven() == true)
1339  condensePrep.addFunctionCall(multQ1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1340  else
1341  condensePrep.addFunctionCall(multGxGu, Q1.getAddress((i + 1) * NX, 0), E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1342  }
1343  else
1344  {
1345  if (QN1.isGiven() == true)
1346  condensePrep.addFunctionCall(multQN1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1347  else
1348  condensePrep.addFunctionCall(multGxGu, QN1, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1349  }
1350  }
1351  condensePrep.addLinebreak();
1352  }
1353  else
1354  {
1355  ExportIndex i, j, k;
1356 
1357  condensePrep.acquire( i );
1358  condensePrep.acquire( j );
1359  condensePrep.acquire( k );
1360 
1361  ExportForLoop eLoopI(i, 0, N - 1);
1362  ExportForLoop eLoopJ1(j, 0, i + 1);
1363  ExportForLoop eLoopJ2(j, 0, i + 1);
1364 
1365  eLoopJ1.addStatement( k == (i + 1) * i / 2 + j );
1366 
1367  if (Q1.isGiven() == true)
1368  eLoopJ1.addFunctionCall(multQ1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1369  else
1370  eLoopJ1.addFunctionCall(multGxGu, Q1.getAddress((i + 1) * NX, 0), E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1371 
1372  eLoopI.addStatement( eLoopJ1 );
1373 
1374  eLoopJ2.addStatement( k == (i + 1) * i / 2 + j );
1375 
1376  if (QN1.isGiven() == true)
1377  eLoopJ2.addFunctionCall(multQN1Gu, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1378  else
1379  eLoopJ2.addFunctionCall(multGxGu, QN1, E.getAddress(k * NX, 0), QE.getAddress(k * NX, 0));
1380 
1381  condensePrep.addStatement( eLoopI );
1382  condensePrep.addLinebreak();
1383  condensePrep.addStatement( eLoopJ2 );
1384  condensePrep.addLinebreak();
1385 
1386  condensePrep.release( i );
1387  condensePrep.release( j );
1388  condensePrep.release( k );
1389  }
1390 
1392  //
1393  // Compute Hessian blocks H00, H10, H11
1394  //
1396 
1397  LOG( LVL_DEBUG ) << "Setup condensing: create H00, H10 & H11" << endl;
1398 
1399  LOG( LVL_DEBUG ) << "---> Create H00" << endl;
1400 
1401  //
1402  // Create H00, in case of partial condensing only
1403  //
1404  if (performFullCondensing() == false)
1405  {
1406  condensePrep.addFunctionCall( zeroBlockH00 );
1407 
1408  for (unsigned i = 0; i < N; ++i)
1409  condensePrep.addFunctionCall(multCTQC, evGx.getAddress(i * NX, 0), QGx.getAddress(i * NX, 0));
1410 
1411  condensePrep.addStatement( H00 += mRegH00 );
1412  condensePrep.addLinebreak();
1413 
1414  // TODO: to be checked
1415  if (initialStateFixed() == false)
1416  {
1417  if (Q1.isGiven() == true)
1418  {
1419  condensePrep.addStatement( H00 += Q1 );
1420  }
1421  else
1422  {
1423  condensePrep.addStatement( H00 += Q1.getSubMatrix(0, NX, 0, NX) );
1424  }
1425  }
1426 
1427  if (SAC.getDim() > 0)
1428  condensePrep.addStatement( H00 += SAC );
1429  }
1430 
1431  LOG( LVL_DEBUG ) << "---> Create H10" << endl;
1432 
1433  //
1434  // Create H10 block
1435  //
1436  if (N <= 20)
1437  {
1438  for (unsigned i = 0; i < N; ++i)
1439  {
1440  condensePrep.addFunctionCall(zeroBlockH10, H10.getAddress(i * NU));
1441 
1442  for (unsigned j = i; j < N; ++j)
1443  {
1444  unsigned k = (j + 1) * j / 2 + i;
1445 
1446  condensePrep.addFunctionCall(multQETGx, QE.getAddress(k * NX), evGx.getAddress(j * NX), H10.getAddress(i * NU));
1447  }
1448  }
1449  }
1450  else
1451  {
1452  ExportIndex ii, jj, kk;
1453  condensePrep.acquire( ii );
1454  condensePrep.acquire( jj );
1455  condensePrep.acquire( kk );
1456 
1457  ExportForLoop eLoopI(ii, 0, N);
1458  ExportForLoop eLoopJ(jj, ii, N);
1459 
1460  eLoopI.addFunctionCall(zeroBlockH10, H10.getAddress(ii * NU));
1461 
1462  eLoopJ.addStatement( kk == (jj + 1) * jj / 2 + ii );
1463  eLoopJ.addFunctionCall( multQETGx, QE.getAddress(kk * NX), evGx.getAddress(jj * NX), H10.getAddress(ii * NU) );
1464 
1465  eLoopI.addStatement( eLoopJ );
1466  condensePrep.addStatement( eLoopI );
1467 
1468  condensePrep.release( ii );
1469  condensePrep.release( jj );
1470  condensePrep.release( kk );
1471  }
1472  condensePrep.addLinebreak();
1473 
1474  LOG( LVL_DEBUG ) << "---> Setup H01" << endl;
1475 
1476  //
1477  // Copy H01 = H10^T in case of partial condensing
1478  //
1479  if (performFullCondensing() == false)
1480  {
1481  condensePrep.addStatement( H.getSubMatrix(0, NX, NX, getNumQPvars()) == H10.getTranspose() );
1482  condensePrep.addLinebreak();
1483  }
1484 
1485  LOG( LVL_DEBUG ) << "---> Create H11" << endl;
1486 
1487  //
1488  // Create H11 block
1489  //
1490  if (N <= 20)
1491  {
1492  unsigned row, col;
1493 
1494  for (row = 0; row < N; ++row)
1495  {
1496  // Diagonal block
1497  if (R1.isGiven() == true)
1498  condensePrep.addFunctionCall(setBlockH11_R1, ExportIndex(row), ExportIndex(row), R1);
1499  else
1500  condensePrep.addFunctionCall(setBlockH11_R1, ExportIndex(row), ExportIndex(row), R1.getAddress(row * NU));
1501 
1502  col = row;
1503  for(unsigned blk = col; blk < N; ++blk)
1504  {
1505  unsigned indl = (blk + 1) * blk / 2 + row;
1506  unsigned indr = (blk + 1) * blk / 2 + col;
1507 
1508  condensePrep.addFunctionCall(
1509  setBlockH11, ExportIndex(row), ExportIndex(col),
1510  E.getAddress(indl * NX), QE.getAddress(indr * NX) );
1511  }
1512  condensePrep.addLinebreak();
1513 
1514  // The rest of the blocks in the row
1515  for(col = row + 1; col < N; ++col)
1516  {
1517  condensePrep.addFunctionCall(
1518  zeroBlockH11, ExportIndex(row), ExportIndex(col)
1519  );
1520 
1521  for(unsigned blk = col; blk < N; ++blk)
1522  {
1523  unsigned indl = (blk + 1) * blk / 2 + row;
1524  unsigned indr = (blk + 1) * blk / 2 + col;
1525 
1526  condensePrep.addFunctionCall(
1527  setBlockH11, ExportIndex(row), ExportIndex(col),
1528  E.getAddress(indl * NX), QE.getAddress(indr * NX) );
1529  }
1530  condensePrep.addLinebreak();
1531  }
1532  }
1533  }
1534  else
1535  {
1536  ExportIndex row, col, blk, indl, indr;
1537  condensePrep.acquire( row ).acquire( col ).acquire( blk ).acquire( indl ).acquire( indr );
1538 
1539  ExportForLoop eLoopI(row, 0, N);
1540  ExportForLoop eLoopK(blk, row, N);
1541  ExportForLoop eLoopJ(col, row + 1, N);
1542  ExportForLoop eLoopK2(blk, col, N);
1543 
1544  // The diagonal block
1545  // Diagonal block
1546  if (R1.isGiven() == true)
1547  eLoopI.addFunctionCall(setBlockH11_R1, row, row, R1);
1548  else
1549  eLoopI.addFunctionCall(setBlockH11_R1, row, row, R1.getAddress(row * NU));
1550 
1551  eLoopI.addStatement( col == row );
1552  eLoopK.addStatement( indl == (blk + 1) * blk / 2 + row );
1553  eLoopK.addStatement( indr == (blk + 1) * blk / 2 + col );
1554  eLoopK.addFunctionCall( setBlockH11, row, col, E.getAddress(indl * NX), QE.getAddress(indr * NX) );
1555  eLoopI.addStatement( eLoopK );
1556 
1557  // The rest of the blocks in the row
1558  eLoopJ.addFunctionCall( zeroBlockH11, row, col );
1559 
1560  eLoopK2.addStatement( indl == (blk + 1) * blk / 2 + row );
1561  eLoopK2.addStatement( indr == (blk + 1) * blk / 2 + col );
1562  eLoopK2.addFunctionCall( setBlockH11, row, col, E.getAddress(indl * NX), QE.getAddress(indr * NX) );
1563  eLoopJ.addStatement( eLoopK2 );
1564 
1565  eLoopI.addStatement( eLoopJ );
1566  condensePrep.addStatement( eLoopI );
1567 
1568  condensePrep.release( row ).release( col ).release( blk ).release( indl ).release( indr );
1569  }
1570  condensePrep.addLinebreak();
1571 
1572  LOG( LVL_DEBUG ) << "---> Copy H11 lower part" << endl;
1573 
1574  unsigned offset = (performFullCondensing() == true) ? 0 : NX;
1575 
1576  // Copy to H11 upper triangular part to lower triangular part
1577  if (N <= 20)
1578  {
1579  for (unsigned ii = 0; ii < N; ++ii)
1580  for(unsigned jj = 0; jj < ii; ++jj)
1581  condensePrep.addFunctionCall(
1582  copyHTH, ExportIndex( ii ), ExportIndex( jj )
1583  );
1584  }
1585  else
1586  {
1587  ExportIndex ii, jj;
1588 
1589  condensePrep.acquire( ii );
1590  condensePrep.acquire( jj );
1591 
1592  ExportForLoop eLoopI(ii, 0, N);
1593  ExportForLoop eLoopJ(jj, 0, ii);
1594 
1595  eLoopJ.addFunctionCall(copyHTH, ii, jj);
1596  eLoopI.addStatement( eLoopJ );
1597  condensePrep.addStatement( eLoopI );
1598 
1599  condensePrep.release( ii );
1600  condensePrep.release( jj );
1601  }
1602  condensePrep.addLinebreak();
1603 
1604  LOG( LVL_DEBUG ) << "---> Copy H10" << endl;
1605 
1606  //
1607  // Set block H10 in case of partial condensing
1608  //
1609  if (performFullCondensing() == false)
1610  {
1611  condensePrep.addStatement( H.getSubMatrix(NX, getNumQPvars(), 0, NX) == H10 );
1612  condensePrep.addLinebreak();
1613  }
1614 
1615  int externalCholesky;
1616  get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
1617  ASSERT((CondensedHessianCholeskyDecomposition)externalCholesky == INTERNAL_N3 ||
1618  (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL)
1619 
1620  if ((CondensedHessianCholeskyDecomposition)externalCholesky == INTERNAL_N3)
1621  {
1622  R.setup("R", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1623 
1624  cholSolver.init(getNumQPvars(), NX, "condensing");
1625  cholSolver.setup();
1626 
1627  condensePrep.addStatement( R == H );
1628  condensePrep.addFunctionCall(cholSolver.getCholeskyFunction(), R);
1629  }
1630 
1632  //
1633  // Compute gradient components g0 and g1
1634  //
1636 
1637  //
1638  // Compute d and Qd = Q1 * d
1639  //
1640  LOG( LVL_DEBUG ) << "Setup condensing: compute Qd" << endl;
1641 
1642  if (performsSingleShooting() == false)
1643  {
1644  Qd.setup("Qd", N * NX, 1, REAL, ACADO_WORKSPACE);
1645 
1646  for(unsigned i = 0; i < N - 1; ++i)
1647  {
1648  if (Q1.isGiven() == true)
1649  condensePrep.addFunctionCall(
1650  multQ1d, Q1, d.getAddress(i * NX), Qd.getAddress(i * NX) );
1651  else
1652  condensePrep.addFunctionCall(
1653  multQ1d, Q1.getAddress((i + 1) * NX, 0), d.getAddress(i * NX), Qd.getAddress(i * NX) );
1654  }
1655 
1656  condensePrep.addFunctionCall(
1657  multQN1d, QN1, d.getAddress((N - 1) * NX), Qd.getAddress((N - 1) * NX) );
1658 
1659  condensePrep.addLinebreak();
1660  }
1661 
1662  LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
1663 
1664  if (initialStateFixed() == true)
1665  {
1666  condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
1667  condenseFdb.addLinebreak();
1668  }
1669 
1670  condenseFdb.addStatement( Dy -= y );
1671  condenseFdb.addStatement( DyN -= yN );
1672  condenseFdb.addLinebreak();
1673 
1674  // Compute RDy
1675  for(unsigned run1 = 0; run1 < N; ++run1)
1676  {
1677  if (R2.isGiven() == true)
1678  condenseFdb.addFunctionCall(
1679  multRDy, R2,
1680  Dy.getAddress(run1 * NY, 0),
1681  g.getAddress(offset + run1 * NU, 0) );
1682  else
1683  condenseFdb.addFunctionCall(
1684  multRDy, R2.getAddress(run1 * NU, 0),
1685  Dy.getAddress(run1 * NY, 0),
1686  g.getAddress(offset + run1 * NU, 0) );
1687  }
1688  condenseFdb.addLinebreak();
1689 
1690  // Compute QDy
1691  // NOTE: This is just for the MHE case :: run1 starts from 0; in MPC :: from 1 ;)
1692  for(unsigned run1 = 0; run1 < N; run1++ )
1693  {
1694  if (Q2.isGiven() == true)
1695  condenseFdb.addFunctionCall(
1696  multQDy, Q2,
1697  Dy.getAddress(run1 * NY),
1698  QDy.getAddress(run1 * NX) );
1699  else
1700  condenseFdb.addFunctionCall(
1701  multQDy, Q2.getAddress(run1 * NX),
1702  Dy.getAddress(run1 * NY),
1703  QDy.getAddress(run1 * NX) );
1704  }
1705  condenseFdb.addLinebreak();
1706  condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == QN2 * DyN );
1707  condenseFdb.addLinebreak();
1708 
1709  LOG( LVL_DEBUG ) << "Setup condensing: QDy.getRows(NX, (N + 1) * NX) += Qd" << endl;
1710 
1711  if (performsSingleShooting() == false)
1712  {
1713  condenseFdb.addStatement( QDy.getRows(NX, (N + 1) * NX) += Qd );
1714  condenseFdb.addLinebreak();
1715  }
1716 
1717  if (performFullCondensing() == false)
1718  {
1719  // g0 == C^T * QDy(1: N + 1, :)
1720  condenseFdb.addStatement( g0 == (evGx ^ QDy.getRows(NX, (N + 1) * NX)) );
1721  condenseFdb.addLinebreak();
1722 
1723  if (initialStateFixed() == false)
1724  condenseFdb.addStatement( g0 += QDy.getRows(0, NX) );
1725 
1726  if (SAC.getDim() > 0)
1727  {
1728  // Include arrival cost
1729  condenseFdb.addStatement( DxAC == x.getRow( 0 ).getTranspose() - xAC );
1730  condenseFdb.addStatement( g0 += SAC * DxAC );
1731  }
1732 
1733  condenseFdb.addLinebreak();
1734  }
1735 
1736  if (N <= 20)
1737  {
1738  for (unsigned i = 0; i < N; ++i)
1739  for (unsigned j = i; j < N; ++j)
1740  {
1741  unsigned k = (j + 1) * j / 2 + i;
1742 
1743  condenseFdb.addFunctionCall(
1744  multEQDy, E.getAddress(k * NX, 0), QDy.getAddress((j + 1) * NX), g.getAddress(offset + i * NU) );
1745  }
1746  }
1747  else
1748  {
1749  ExportIndex i, j, k;
1750  condenseFdb.acquire( i );
1751  condenseFdb.acquire( j );
1752  condenseFdb.acquire( k );
1753 
1754  ExportForLoop eLoopI(i, 0, N);
1755  ExportForLoop eLoopJ(j, i, N);
1756 
1757  eLoopJ.addStatement( k == (j + 1) * j / 2 + i );
1758  eLoopJ.addFunctionCall(
1759  multEQDy, E.getAddress(k * NX, 0), QDy.getAddress((j + 1) * NX), g.getAddress(offset + i * NU) );
1760 
1761  eLoopI.addStatement( eLoopJ );
1762  condenseFdb.addStatement( eLoopI );
1763 
1764  condenseFdb.release( i );
1765  condenseFdb.release( j );
1766  condenseFdb.acquire( k );
1767  }
1768  condenseFdb.addLinebreak();
1769 
1770  if (performFullCondensing() == true)
1771  {
1772  condenseFdb.addStatement( g1 += H10 * Dx0 );
1773  condenseFdb.addLinebreak();
1774  }
1775 
1776  //
1777  // Add condensed linear terms from the objective to the gradient
1778  //
1779  if (performFullCondensing() == false && objSlx.getDim() > 0)
1780  {
1781  condensePrep.addStatement( g0 += objSlx );
1782 
1783  ExportVariable g00, C0, Slx0;
1784  g00.setup("g0", NX, 1, REAL, ACADO_LOCAL);
1785  C0.setup("C0", NX, NX, REAL, ACADO_LOCAL);
1786 
1787  if (objSlx.isGiven() == false)
1788  Slx0.setup("Slx0", NX, 1, REAL, ACADO_LOCAL);
1789  else
1790  Slx0 = objSlx;
1791 
1792  macCTSlx.setup("macCTSlx", C0, Slx0, g00);
1793  macCTSlx.addStatement( g00 += C0.getTranspose() * Slx0);
1794 
1795  for (unsigned i = 0; i < N; ++i)
1796  condensePrep.addFunctionCall(macCTSlx, evGx.getAddress(i * NX, 0), objSlx, g);
1797  }
1798 
1799  if (objSlx.getDim() > 0 || objSlu.getDim() > 0)
1800  {
1801  offset = performFullCondensing() == true ? 0 : NX;
1802 
1803  if (objSlu.getDim() > 0 && objSlx.getDim() == 0)
1804  {
1805  for (unsigned i = 0; i < N; ++i)
1806  {
1807  condensePrep.addStatement(
1808  g.getRows(offset + i * NU, offset + (i + 1) * NU) += objSlu
1809  );
1810  }
1811  }
1812  else
1813  {
1814  ExportVariable g10, E0, Slx0, Slu0;
1815 
1816  g10.setup("g1", NU, 1, REAL, ACADO_LOCAL);
1817  E0.setup("E0", NX, NU, REAL, ACADO_LOCAL);
1818 
1819  if (objSlx.isGiven() == false && objSlx.getDim() > 0)
1820  Slx0.setup("Slx0", NX, 1, REAL, ACADO_LOCAL);
1821  else
1822  Slx0 = objSlx;
1823 
1824  macETSlu.setup("macETSlu", E0, Slx0, g10);
1825  macETSlu.addStatement( g10 += E0.getTranspose() * Slx0 );
1826 
1827  if (N <= 20)
1828  {
1829  for (unsigned i = 0; i < N; ++i)
1830  for (unsigned j = i; j < N; ++j)
1831  {
1832  unsigned k = (j + 1) * j / 2 + i;
1833 
1834  condensePrep.addFunctionCall(macETSlu, QE.getAddress(k * NX), objSlx, g.getAddress(offset + i * NU));
1835  }
1836  }
1837  else
1838  {
1839  ExportIndex ii, jj, kk;
1840  condensePrep.acquire( ii );
1841  condensePrep.acquire( jj );
1842  condensePrep.acquire( kk );
1843 
1844  ExportForLoop iLoop(ii, 0, N);
1845  ExportForLoop jLoop(jj, ii, N);
1846 
1847  jLoop.addStatement( kk == (jj + 1) * jj / 2 + ii );
1848  jLoop.addFunctionCall(macETSlu, QE.getAddress(kk * NX), objSlx, g.getAddress(offset + ii * NU));
1849 
1850  iLoop.addStatement( jLoop );
1851  condensePrep.addStatement( iLoop );
1852 
1853  condensePrep.release( ii );
1854  condensePrep.release( jj );
1855  condensePrep.release( kk );
1856  }
1857 
1858  if (objSlu.getDim() > 0)
1859  {
1860  for (unsigned i = 0; i < N; ++i)
1861  condensePrep.addStatement(
1862  g.getRows(offset + i * NU, offset + (i + 1) * NU) += objSlu);
1863  }
1864  }
1865  }
1866 
1868  //
1869  // Expansion routine
1870  //
1872 
1873  LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
1874 
1875  expand.setup( "expand" );
1876 
1877  if (performFullCondensing() == true)
1878  {
1879  expand.addStatement( u.makeRowVector() += xVars.getTranspose() );
1880  expand.addLinebreak();
1881  expand.addStatement( x.getRow( 0 ) += Dx0.getTranspose() );
1882  }
1883  else
1884  {
1885  expand.addStatement( x.makeColVector().getRows(0, NX) += xVars.getRows(0, NX) );
1886  expand.addLinebreak();
1887  expand.addStatement( u.makeRowVector() += xVars.getTranspose().getCols(NX, getNumQPvars() ) );
1888  }
1889  expand.addLinebreak();
1890 
1891  // x += C * deltaX0
1892 
1893  if (performsSingleShooting() == false)
1894  {
1895 
1896  if (performFullCondensing() == true)
1897  expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += d + evGx * Dx0);
1898  else
1899  expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += d + evGx * xVars.getRows(0, NX) );
1900  }
1901  else
1902  {
1903  if (performFullCondensing() == true)
1904  expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += evGx * Dx0);
1905  else
1906  expand.addStatement( x.makeColVector().getRows(NX, (N + 1) * NX) += evGx * xVars.getRows(0, NX) );
1907  }
1908 
1909  expand.addLinebreak();
1910 
1911  // x += E * deltaU
1912 
1913  offset = (performFullCondensing() == true) ? 0 : NX;
1914 
1915  if (N <= 20)
1916  {
1917  for (unsigned i = 0; i < N; ++i)
1918  for (unsigned j = 0; j <= i; ++j)
1919  {
1920  unsigned k = (i + 1) * i / 2 + j;
1921 
1922  expand.addFunctionCall(
1923  multEDu, E.getAddress(k * NX, 0), xVars.getAddress(offset + j * NU), x.getAddress(i + 1) );
1924  }
1925  }
1926  else
1927  {
1928  ExportIndex ii, jj, kk;
1929  expand.acquire( ii );
1930  expand.acquire( jj );
1931  expand.acquire( kk );
1932 
1933  ExportForLoop eLoopI(ii, 0, N);
1934  ExportForLoop eLoopJ(jj, 0, ii + 1);
1935 
1936  eLoopJ.addStatement( kk == (ii + 1) * ii / 2 + jj );
1937  eLoopJ.addFunctionCall(
1938  multEDu, E.getAddress(kk * NX, 0), xVars.getAddress(offset + jj * NU), x.getAddress(ii + 1) );
1939 
1940  eLoopI.addStatement( eLoopJ );
1941  expand.addStatement( eLoopI );
1942 
1943  expand.release( ii );
1944  expand.release( jj );
1945  expand.release( kk );
1946  }
1947 
1949  //
1950  // Calculation of the covariance matrix for the last state estimate
1951  //
1953 
1954  int covCalc;
1955  get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
1956 
1957  if (covCalc == NO)
1958  return SUCCESSFUL_RETURN;
1959 
1960  if (performFullCondensing() == true)
1961  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, \
1962  "Calculation of covariance matrix works for partial condensing only atm.");
1963 
1964  LOG( LVL_DEBUG ) << "Setup condensing: covariance matrix calculation" << endl;
1965 
1966  CEN.setup("CEN", NX, getNumQPvars(), REAL, ACADO_WORKSPACE);
1967  sigmaTmp.setup("sigmaTemp", getNumQPvars(), NX, REAL, ACADO_WORKSPACE);
1968  sigma.setup("sigma", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1969 
1970  sigmaN.setup("sigmaN", NX, NX, REAL, ACADO_VARIABLES);
1971  sigmaN.setDoc("Covariance matrix of the last state estimate.");
1972 
1973  calculateCovariance.setup("calculateCovariance");
1974 
1975  calculateCovariance.addStatement(
1976  CEN.getSubMatrix(0, NX, 0, NX) == evGx.getSubMatrix((N - 1) * NX, N * NX, 0, NX) );
1977  calculateCovariance.addLinebreak();
1978 
1979  ExportIndex cIndex("cIndex");
1980  ExportForLoop cLoop(cIndex, 0, N);
1981  calculateCovariance.addIndex( cIndex );
1982 
1983  unsigned blk = (N - 1 + 1) * (N - 1) / 2;
1984  cLoop.addStatement(
1985  CEN.getSubMatrix(0, NX, NX + cIndex * NU, NX + NU + cIndex * NU) ==
1986  E.getSubMatrix(blk * NX + cIndex * NX, blk * NX + NX + cIndex * NX, 0, NU)
1987  );
1988  calculateCovariance.addStatement( cLoop );
1989  calculateCovariance.addLinebreak();
1990 
1991  // XXX Optimize for long horizons.
1992  calculateCovariance.addStatement(
1993  sigmaTmp == sigma * CEN.getTranspose()
1994  );
1995  calculateCovariance.addLinebreak();
1996 
1997  calculateCovariance.addStatement(
1998  sigmaN == CEN * sigmaTmp
1999  );
2000 
2001  return SUCCESSFUL_RETURN;
2002 }
2003 
2004 returnValue ExportGaussNewtonCondensed::setupVariables( )
2005 {
2007  //
2008  // Make index vector for state constraints
2009  //
2011 
2012  bool boxConIsFinite = false;
2013  xBoundsIdx.clear();
2014 
2015  DVector lbBox, ubBox;
2016  for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
2017  {
2018  lbBox = xBounds.getLowerBounds( i );
2019  ubBox = xBounds.getUpperBounds( i );
2020 
2021  if (isFinite( lbBox ) || isFinite( ubBox ))
2022  boxConIsFinite = true;
2023 
2024  // This is maybe not necessary
2025  if (boxConIsFinite == false || i == 0)
2026  continue;
2027 
2028  for (unsigned j = 0; j < lbBox.getDim(); ++j)
2029  {
2030  if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
2031  {
2032  xBoundsIdx.push_back(i * lbBox.getDim() + j);
2033  }
2034  }
2035  }
2036 
2037  if (initialStateFixed() == true)
2038  {
2039  x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
2040  x0.setDoc( (std::string)"Current state feedback vector." );
2041  Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
2042  }
2043 
2044  T.setup("T", NX, NX, REAL, ACADO_WORKSPACE);
2045  E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
2046  QE.setup("QE", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
2047 
2048  if (performFullCondensing() == false)
2049  QGx.setup("QGx", N * NX, NX, REAL, ACADO_WORKSPACE);
2050 
2051  QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
2052 
2053  // Setup all QP stuff
2054 
2055  H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
2056 
2057  // Stupid aliasing to avoid copying of data
2058  if (performFullCondensing() == true)
2059  {
2060  H11 = H;
2061  }
2062  else
2063  {
2064  H00 = H.getSubMatrix(0, NX, 0, NX);
2065  H11 = H.getSubMatrix(NX, getNumQPvars(), NX, getNumQPvars());
2066  }
2067 
2068  H10.setup("H10", N * NU, NX, REAL, ACADO_WORKSPACE);
2069 
2070  A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
2071 
2072  g.setup("g", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
2073 
2074  if (performFullCondensing() == true)
2075  {
2076  g1 = g;
2077  }
2078  else
2079  {
2080  g0 = g.getRows(0, NX);
2081  g1 = g.getRows(NX, getNumQPvars());
2082  }
2083 
2084  lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
2085  ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
2086 
2087  lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
2088  ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
2089 
2090  xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
2091  yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
2092 
2093  return SUCCESSFUL_RETURN;
2094 }
2095 
2096 returnValue ExportGaussNewtonCondensed::setupMultiplicationRoutines( )
2097 {
2098  ExportIndex iCol( "iCol" );
2099  ExportIndex iRow( "iRow" );
2100 
2101  ExportVariable dp, dn, Gx1, Gx2, Gx3, Gu1, Gu2;
2102  ExportVariable R22, R11, Dy1, RDy1, Q22, QDy1, E1, U1, H101;
2103  dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
2104  dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
2105  Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
2106  Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
2107  Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
2108  Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
2109  Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
2110  R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
2111  R11.setup("R11", NU, NU, REAL, ACADO_LOCAL);
2112  Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
2113  RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
2114  Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
2115  QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
2116  E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
2117  U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
2118  H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
2119 
2120  if ( Q2.isGiven() )
2121  Q22 = Q2;
2122  if ( R2.isGiven() )
2123  R22 = R2;
2124  if ( R1.isGiven() )
2125  R11 = R1;
2126 
2127  // multGxd; // d_k += Gx_k * d_{k-1}
2128  multGxd.setup("multGxd", dp, Gx1, dn);
2129  multGxd.addStatement( dn += Gx1 * dp );
2130  // moveGxT
2131  moveGxT.setup("moveGxT", Gx1, Gx2);
2132  moveGxT.addStatement( Gx2 == Gx1 );
2133  // multGxGx
2134  multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
2135  multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
2136  // multGxGu
2137  multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
2138  multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
2139  // moveGuE
2140  moveGuE.setup("moveGuE", Gu1, Gu2);
2141  moveGuE.addStatement( Gu2 == Gu1 );
2142 
2143  unsigned offset = (performFullCondensing() == true) ? 0 : NX;
2144 
2145  // setBlockH11
2146  setBlockH11.setup("setBlockH11", iRow, iCol, Gu1, Gu2);
2147  setBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) += (Gu1 ^ Gu2) );
2148  // setBlockH11_R1
2149  DMatrix mRegH11 = eye<double>( getNU() );
2150  mRegH11 *= levenbergMarquardt;
2151 
2152  setBlockH11_R1.setup("setBlockH11_R1", iRow, iCol, R11);
2153  setBlockH11_R1.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == R11 + mRegH11 );
2154  // zeroBlockH11
2155  zeroBlockH11.setup("zeroBlockH11", iRow, iCol);
2156  zeroBlockH11.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) == zeros<double>(NU, NU) );
2157  // copyHTH
2158  copyHTH.setup("copyHTH", iRow, iCol);
2159  copyHTH.addStatement(
2160  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
2161  H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
2162  );
2163  // multRDy
2164  multRDy.setup("multRDy", R22, Dy1, RDy1);
2165  multRDy.addStatement( RDy1 == R22 * Dy1 );
2166  // mult QDy1
2167  multQDy.setup("multQDy", Q22, Dy1, QDy1);
2168  multQDy.addStatement( QDy1 == Q22 * Dy1 );
2169  // multEQDy;
2170  multEQDy.setup("multEQDy", E1, QDy1, U1);
2171  multEQDy.addStatement( U1 += (E1 ^ QDy1) );
2172  // multQETGx
2173  multQETGx.setup("multQETGx", E1, Gx1, H101);
2174  multQETGx.addStatement( H101 += (E1 ^ Gx1) );
2175  // zerBlockH10
2176  zeroBlockH10.setup("zeroBlockH10", H101);
2177  zeroBlockH10.addStatement( H101 == zeros<double>(NU, NX) );
2178 
2179 // if (performsSingleShooting() == false)
2180 // {
2181  // multEDu
2182  multEDu.setup("multEDu", E1, U1, dn);
2183  multEDu.addStatement( dn += E1 * U1 );
2184 // }
2185 
2186  if (Q1.isGiven() == true)
2187  {
2188  // multQ1Gx
2189  multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
2190  multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
2191 
2192  // multQ1Gu
2193  multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
2194  multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
2195 
2196  // multQ1d
2197  multQ1d.setup("multQ1d", Q1, dp, dn);
2198  multQ1d.addStatement( dn == Q1 * dp );
2199  }
2200  else
2201  {
2202  // multQ1d
2203  multQ1d.setup("multQ1d", Gx1, dp, dn);
2204  multQ1d.addStatement( dn == Gx1 * dp );
2205  }
2206 
2207  if (QN1.isGiven() == BT_TRUE)
2208  {
2209  // multQN1Gu
2210  multQN1Gu.setup("multQN1Gu", Gu1, Gu2);
2211  multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
2212 
2213  // multQN1Gx
2214  multQN1Gx.setup("multQN1Gx", Gx1, Gx2);
2215  multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
2216  }
2217 
2218  if (performsSingleShooting() == BT_FALSE)
2219  {
2220  // multQN1d
2221  multQN1d.setup("multQN1d", QN1, dp, dn);
2222  multQN1d.addStatement( dn == QN1 * dp );
2223  }
2224 
2225  if (performFullCondensing() == BT_FALSE)
2226  {
2227  // zeroBlockH00
2228  zeroBlockH00.setup( "zeroBlockH00" );
2229  zeroBlockH00.addStatement( H00 == zeros<double>(NX, NX) );
2230 
2231  // multCTQC
2232  multCTQC.setup("multCTQC", Gx1, Gx2);
2233  multCTQC.addStatement( H00 += (Gx1 ^ Gx2) );
2234  }
2235 
2236  return SUCCESSFUL_RETURN;
2237 }
2238 
2239 returnValue ExportGaussNewtonCondensed::setupEvaluation( )
2240 {
2242  //
2243  // Preparation phase
2244  //
2246 
2247  preparation.setup( "preparationStep" );
2248  preparation.doc( "Preparation step of the RTI scheme." );
2249  ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
2250  retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
2251  preparation.setReturnValue(retSim, false);
2252 
2253  preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
2254 
2255  preparation.addFunctionCall( evaluateObjective );
2256  preparation.addFunctionCall( condensePrep );
2257 
2259  //
2260  // Feedback phase
2261  //
2263 
2264  ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
2265  tmp.setDoc( "Status code of the qpOASES QP solver." );
2266 
2267  ExportFunction solve("solve");
2268  solve.setReturnValue( tmp );
2269 
2270  feedback.setup("feedbackStep");
2271  feedback.doc( "Feedback/estimation step of the RTI scheme." );
2272  feedback.setReturnValue( tmp );
2273 
2274  feedback.addFunctionCall( condenseFdb );
2275  feedback.addLinebreak();
2276 
2277  feedback << tmp.getName() << " = " << solve.getName() << "( );\n";
2278  feedback.addLinebreak();
2279 
2280  feedback.addFunctionCall( expand );
2281 
2282  int covCalc;
2283  get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
2284  if (covCalc)
2285  feedback.addFunctionCall( calculateCovariance );
2286 
2288  //
2289  // Setup evaluation of the KKT tolerance
2290  //
2292 
2293  ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
2294  ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
2295  ExportIndex index( "index" );
2296 
2297  getKKT.setup( "getKKT" );
2298  getKKT.doc( "Get the KKT tolerance of the current iterate." );
2299  kkt.setDoc( "The KKT tolerance value." );
2300  getKKT.setReturnValue( kkt );
2301  getKKT.addVariable( prd );
2302  getKKT.addIndex( index );
2303 
2304  // ACC = |\nabla F^T * xVars|
2305  getKKT.addStatement( kkt == (g ^ xVars) );
2306  getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
2307 
2308  ExportForLoop bLoop(index, 0, getNumQPvars());
2309 
2310  bLoop.addStatement( prd == yVars.getRow( index ) );
2311  bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
2312  bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
2313  bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
2314  bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
2315  getKKT.addStatement( bLoop );
2316 
2317  if ((getNumStateBounds() + getNumComplexConstraints())> 0)
2318  {
2319  ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
2320 
2321  cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ) );
2322  cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
2323  cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
2324  cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
2325  cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
2326 
2327  getKKT.addStatement( cLoop );
2328  }
2329 
2330  return SUCCESSFUL_RETURN;
2331 }
2332 
2333 returnValue ExportGaussNewtonCondensed::setupQPInterface( )
2334 {
2335  string folderName;
2336  get(CG_EXPORT_FOLDER_NAME, folderName);
2337 
2338  string moduleName;
2339  get(CG_MODULE_NAME, moduleName);
2340 
2341  int qpSolver;
2342  get(QP_SOLVER, qpSolver);
2343 
2344  std::string sourceFile, headerFile, solverDefine;
2345  ExportQpOasesInterface* qpInterface = 0;
2346 
2347  switch ( (QPSolverName)qpSolver )
2348  {
2349  case QP_QPOASES:
2350  sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
2351  headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
2352  solverDefine = "QPOASES_HEADER";
2353  qpInterface = new ExportQpOasesInterface(headerFile, sourceFile, "");
2354  break;
2355 
2356  case QP_QPOASES3:
2357  sourceFile = folderName + "/" + moduleName + "_qpoases3_interface.c";
2358  headerFile = folderName + "/" + moduleName + "_qpoases3_interface.h";
2359  solverDefine = "QPOASES3_HEADER";
2360  qpInterface = new ExportQpOases3Interface(headerFile, sourceFile, "");
2361  break;
2362 
2363  default:
2364  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
2365  "For condensed solution only qpOASES and qpOASES3 QP solver are supported");
2366  }
2367 
2368  int useSinglePrecision;
2369  get(USE_SINGLE_PRECISION, useSinglePrecision);
2370 
2371  int hotstartQP;
2372  get(HOTSTART_QP, hotstartQP);
2373 
2374  int covCalc;
2375  get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
2376 
2377  int maxNumQPiterations;
2378  get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
2379 
2380  int externalCholesky;
2381  get(CG_CONDENSED_HESSIAN_CHOLESKY, externalCholesky);
2382 
2383  //
2384  // Set up export of the source file
2385  //
2386 
2387  if ( qpInterface == 0 )
2388  return RET_UNKNOWN_BUG;
2389 
2390  qpInterface->configure(
2391  "",
2392  solverDefine,
2393  getNumQPvars(),
2394  getNumStateBounds() + getNumComplexConstraints(),
2395  maxNumQPiterations,
2396  "PL_NONE",
2397  useSinglePrecision,
2398 
2399  commonHeaderName,
2400  "",
2401  xVars.getFullName(),
2402  yVars.getFullName(),
2403  sigma.getFullName(),
2404  hotstartQP,
2405  (CondensedHessianCholeskyDecomposition)externalCholesky == EXTERNAL,
2406  H.getFullName(),
2407  R.getFullName(),
2408  g.getFullName(),
2409  A.getFullName(),
2410  lb.getFullName(),
2411  ub.getFullName(),
2412  lbA.getFullName(),
2413  ubA.getFullName()
2414  );
2415 
2416  returnValue returnvalue = qpInterface->exportCode();
2417 
2418  if ( qpInterface != 0 )
2419  delete qpInterface;
2420 
2421  return returnvalue;
2422 }
2423 
2424 bool ExportGaussNewtonCondensed::performFullCondensing() const
2425 {
2426  int sparseQPsolution;
2427  get(SPARSE_QP_SOLUTION, sparseQPsolution);
2428 
2429  return (SparseQPsolutionMethods)sparseQPsolution == CONDENSING ? false : true;
2430 }
2431 
2432 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
ExportGaussNewtonCondensed::setupObjectiveEvaluation
virtual returnValue setupObjectiveEvaluation(void)
Definition: export_gauss_newton_condensed.cpp:282
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
ExportNLPSolver::conValueOut
ExportVariable conValueOut
Definition: export_nlp_solver.hpp:296
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::objSlu
ExportVariable objSlu
Definition: export_nlp_solver.hpp:278
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
ExportGaussNewtonCondensed::getFunctionDeclarations
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: export_gauss_newton_condensed.cpp:143
SparseQPsolutionMethods
SparseQPsolutionMethods
Definition: acado_types.hpp:755
QP_SOLVER
Definition: acado_types.hpp:386
export_module.hpp
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
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::pocEvH
ExportVariable pocEvH
Definition: export_nlp_solver.hpp:313
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
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
CG_COMPUTE_COVARIANCE_MATRIX
Definition: acado_types.hpp:342
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
ExportNLPSolver::conValueIn
ExportVariable conValueIn
Definition: export_nlp_solver.hpp:295
MatrixVariablesGrid::getUpperBounds
DVector getUpperBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1393
ExportNLPSolver::pacEvHu
ExportVariable pacEvHu
Definition: export_nlp_solver.hpp:301
ExportStatementBlock::addComment
returnValue addComment(const std::string &_comment)
Definition: export_statement_block.cpp:228
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
ExportGaussNewtonCondensed::getNumStateBounds
virtual unsigned getNumStateBounds() const
Definition: export_gauss_newton_condensed.cpp:273
MAX_NUM_QP_ITERATIONS
Definition: acado_types.hpp:387
ExportNLPSolver::objEvFxEnd
ExportVariable objEvFxEnd
Definition: export_nlp_solver.hpp:266
ExportGaussNewtonCondensed::setupConstraintsEvaluation
virtual returnValue setupConstraintsEvaluation(void)
Definition: export_gauss_newton_condensed.cpp:491
CONDENSING
Definition: acado_types.hpp:758
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
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
ExportNLPSolver::objSlx
ExportVariable objSlx
Definition: export_nlp_solver.hpp:278
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
INTERNAL_N3
Definition: acado_types.hpp:818
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
ExportGaussNewtonCondensed::ExportGaussNewtonCondensed
ExportGaussNewtonCondensed(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: export_gauss_newton_condensed.cpp:41
ExportGaussNewtonCondensed::getDataDeclarations
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: export_gauss_newton_condensed.cpp:91
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
ExportGaussNewtonCondensed::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: export_gauss_newton_condensed.cpp:165
ExportNLPSolver::evaluateTerminalCost
ExportAcadoFunction evaluateTerminalCost
Definition: export_nlp_solver.hpp:271
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
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
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
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::cholSAC
ExportCholeskyDecomposition cholSAC
Definition: export_nlp_solver.hpp:334
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
GenericVector::getDim
unsigned getDim() const
Definition: vector.hpp:172
export_gauss_newton_condensed.hpp
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
ExportNLPSolver::pacEvHxd
ExportVariable pacEvHxd
Definition: export_nlp_solver.hpp:301
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
NO
#define NO
Definition: acado_types.hpp:53
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
ExportNLPSolver::SAC
ExportVariable SAC
Definition: export_nlp_solver.hpp:343
CG_USE_OPENMP
Definition: acado_types.hpp:339
ExportHouseholderQR::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: householder_qr_export.cpp:77
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
ASSERT
#define ASSERT(x)
Definition: acado_debugging.hpp:51
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
ACADOWARNINGTEXT
#define ACADOWARNINGTEXT(retval, text)
Definition: acado_message_handling.hpp:126
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::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
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
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
ExportCholeskyDecomposition::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: export_cholesky_decomposition.cpp:119
ExportStatementBlock::addFunction
returnValue addFunction(const ExportFunction &_function)
Definition: export_statement_block.cpp:108
export_qpoases_interface.hpp
CondensedHessianCholeskyDecomposition
CondensedHessianCholeskyDecomposition
Definition: acado_types.hpp:815
ExportNLPSolver::pocEvHxd
ExportVariable pocEvHxd
Definition: export_nlp_solver.hpp:314
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
ExportNLPSolver::pacEvH
ExportVariable pacEvH
Definition: export_nlp_solver.hpp:300
ExportNLPSolver::cholObjS
ExportCholeskyDecomposition cholObjS
Definition: export_nlp_solver.hpp:333
ExportNLPSolver::pocEvHu
ExportVariable pocEvHu
Definition: export_nlp_solver.hpp:314
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
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
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