export_gauss_newton_cn2.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
32 #include <acado/code_generation/export_gauss_newton_cn2.hpp>
33 #include <acado/code_generation/export_qpoases_interface.hpp>
34 #include <acado/code_generation/export_qpoases3_interface.hpp>
35 
36 using namespace std;
37 
38 BEGIN_NAMESPACE_ACADO
39 
40 ExportGaussNewtonCN2::ExportGaussNewtonCN2( UserInteraction* _userInteraction,
41  const std::string& _commonHeaderName
42  ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
43 {}
44 
45 returnValue ExportGaussNewtonCN2::setup( )
46 {
47  if (performFullCondensing() == true && initialStateFixed() == false)
48  return ACADOERRORTEXT( RET_INVALID_OPTION, "Impossible to perform full condensing, when the initial state is not fixed. You can use regular condensing instead." );
49  if (performFullCondensing() == false && initialStateFixed() == true)
50  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
51  if (performsSingleShooting() == true)
52  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
53 
54  LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
55  setupInitialization();
56  LOG( LVL_DEBUG ) << "done!" << endl;
57 
58  LOG( LVL_DEBUG ) << "Solver: setup variables... " << endl;
59  setupVariables();
60  LOG( LVL_DEBUG ) << "done!" << endl;
61 
62  LOG( LVL_DEBUG ) << "Solver: setup multiplication routines... " << endl;
63  setupMultiplicationRoutines();
64  LOG( LVL_DEBUG ) << "done!" << endl;
65 
66  LOG( LVL_DEBUG ) << "Solver: setup model simulation... " << endl;
67  setupSimulation();
68  LOG( LVL_DEBUG ) << "done!" << endl;
69 
70  LOG( LVL_DEBUG ) << "Solver: setup objective evaluation... " << endl;
71  setupObjectiveEvaluation();
72  LOG( LVL_DEBUG ) << "done!" << endl;
73 
74  LOG( LVL_DEBUG ) << "Solver: setup condensing... " << endl;
75  setupCondensing();
76  LOG( LVL_DEBUG ) << "done!" << endl;
77 
78  LOG( LVL_DEBUG ) << "Solver: setup constraints... " << endl;
79  setupConstraintsEvaluation();
80  LOG( LVL_DEBUG ) << "done!" << endl;
81 
82  LOG( LVL_DEBUG ) << "Solver: setup evaluation... " << endl;
83  setupEvaluation();
84  LOG( LVL_DEBUG ) << "done!" << endl;
85 
86  LOG( LVL_DEBUG ) << "Solver: setup auxiliary functions... " << endl;
87  setupAuxiliaryFunctions();
88  LOG( LVL_DEBUG ) << "done!" << endl;
89 
90  return SUCCESSFUL_RETURN;
91 }
92 
93 returnValue ExportGaussNewtonCN2::getDataDeclarations( ExportStatementBlock& declarations,
94  ExportStruct dataStruct
95  ) const
96 {
97  ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
98 
99  declarations.addDeclaration(sbar, dataStruct);
100  declarations.addDeclaration(x0, dataStruct);
101  declarations.addDeclaration(Dx0, dataStruct);
102 
103  declarations.addDeclaration(T1, dataStruct);
104  declarations.addDeclaration(T2, dataStruct);
105  declarations.addDeclaration(C, dataStruct);
106 
107  declarations.addDeclaration(W1, dataStruct);
108  declarations.addDeclaration(W2, dataStruct);
109  declarations.addDeclaration(E, dataStruct);
110 
111  declarations.addDeclaration(QDy, dataStruct);
112  declarations.addDeclaration(w1, dataStruct);
113  declarations.addDeclaration(w2, dataStruct);
114 
115  declarations.addDeclaration(lbValues, dataStruct);
116  declarations.addDeclaration(ubValues, dataStruct);
117  declarations.addDeclaration(lbAValues, dataStruct);
118  declarations.addDeclaration(ubAValues, dataStruct);
119 
120 // if (performFullCondensing() == true)
121 // declarations.addDeclaration(A10, dataStruct);
122 // declarations.addDeclaration(A20, dataStruct);
123 
124  declarations.addDeclaration(pacA01Dx0, dataStruct);
125  declarations.addDeclaration(pocA02Dx0, dataStruct);
126 
127  declarations.addDeclaration(H, dataStruct);
128  declarations.addDeclaration(A, dataStruct);
129  declarations.addDeclaration(g, dataStruct);
130  declarations.addDeclaration(lb, dataStruct);
131  declarations.addDeclaration(ub, dataStruct);
132  declarations.addDeclaration(lbA, dataStruct);
133  declarations.addDeclaration(ubA, dataStruct);
134  declarations.addDeclaration(xVars, dataStruct);
135  declarations.addDeclaration(yVars, dataStruct);
136 
137  // lagrange multipliers
138  declarations.addDeclaration(mu, dataStruct);
139 
140  return SUCCESSFUL_RETURN;
141 }
142 
143 returnValue ExportGaussNewtonCN2::getFunctionDeclarations( ExportStatementBlock& declarations
144  ) const
145 {
146  declarations.addDeclaration( preparation );
147  declarations.addDeclaration( feedback );
148 
149  declarations.addDeclaration( initialize );
150  declarations.addDeclaration( initializeNodes );
151  declarations.addDeclaration( shiftStates );
152  declarations.addDeclaration( shiftControls );
153  declarations.addDeclaration( getKKT );
154  declarations.addDeclaration( getObjective );
155 
156  declarations.addDeclaration( evaluateStageCost );
157  declarations.addDeclaration( evaluateTerminalCost );
158 
159  return SUCCESSFUL_RETURN;
160 }
161 
162 returnValue ExportGaussNewtonCN2::getCode( ExportStatementBlock& code
163  )
164 {
165  setupQPInterface();
166 
167  code.addLinebreak( 2 );
168  code.addStatement( "/******************************************************************************/\n" );
169  code.addStatement( "/* */\n" );
170  code.addStatement( "/* ACADO code generation */\n" );
171  code.addStatement( "/* */\n" );
172  code.addStatement( "/******************************************************************************/\n" );
173  code.addLinebreak( 2 );
174 
175  int useOMP;
176  get(CG_USE_OPENMP, useOMP);
177  if ( useOMP )
178  {
179  code.addDeclaration( state );
180  }
181 
182  code.addFunction( modelSimulation );
183 
184  code.addFunction( evaluateStageCost );
185  code.addFunction( evaluateTerminalCost );
186 
187  code.addFunction( evaluatePathConstraints );
188 
189  for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
190  {
191  if (evaluatePointConstraints[ i ] == 0)
192  continue;
193  code.addFunction( *evaluatePointConstraints[ i ] );
194  }
195 
196  code.addFunction( setObjQ1Q2 );
197  code.addFunction( setObjR1R2 );
198  code.addFunction( setObjS1 );
199  code.addFunction( setObjQN1QN2 );
200  code.addFunction( evaluateObjective );
201 
202  code.addFunction( regularizeHessian );
203 
204  code.addFunction( moveGxT );
205  code.addFunction( multGxGx );
206  code.addFunction( multGxGu );
207  code.addFunction( moveGuE );
208 
209  code.addFunction( multBTW1 );
210  code.addFunction( mac_S1T_E );
211  code.addFunction( macBTW1_R1 );
212  code.addFunction( multGxTGu );
213  code.addFunction( macQEW2 );
214 
215  code.addFunction( macATw1QDy );
216  code.addFunction( macBTw1 );
217  code.addFunction( macS1TSbar );
218  code.addFunction( macQSbarW2 );
219  code.addFunction( macASbar );
220  code.addFunction( expansionStep );
221 
222  code.addFunction( expansionStep2 );
223 
224  code.addFunction( mult_BT_T1 );
225  code.addFunction( mac_ST_C );
226  code.addFunction( multGxTGx );
227  code.addFunction( macGxTGx );
228 
229  code.addFunction( copyHTH );
230  code.addFunction( copyHTH1 );
231 
232  code.addFunction( multRDy );
233  code.addFunction( multQDy );
234 
235  code.addFunction( multQN1Gx );
236  code.addFunction( multQN1Gu );
237 
238  code.addFunction( multHxC );
239  code.addFunction( multHxE );
240  code.addFunction( macHxd );
241 
242  code.addFunction( condensePrep );
243  code.addFunction( condenseFdb );
244  code.addFunction( expand );
245 
246  code.addFunction( preparation );
247  code.addFunction( feedback );
248 
249  code.addFunction( initialize );
250  code.addFunction( initializeNodes );
251  code.addFunction( shiftStates );
252  code.addFunction( shiftControls );
253  code.addFunction( getKKT );
254  code.addFunction( getObjective );
255 
256  return SUCCESSFUL_RETURN;
257 }
258 
259 
260 unsigned ExportGaussNewtonCN2::getNumQPvars( ) const
261 {
262  if (performFullCondensing() == true)
263  return (N * NU);
264 
265  return (NX + N * NU);
266 }
267 
268 unsigned ExportGaussNewtonCN2::getNumStateBounds() const
269 {
270  return xBoundsIdx.size();
271 }
272 
273 //
274 // PROTECTED FUNCTIONS:
275 //
276 
277 returnValue ExportGaussNewtonCN2::setupObjectiveEvaluation( void )
278 {
279  evaluateObjective.setup("evaluateObjective");
280 
281  int variableObjS;
282  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
283 
284  //
285  // A loop the evaluates objective and corresponding gradients
286  //
287  ExportIndex runObj( "runObj" );
288  ExportForLoop loopObjective(runObj, 0, N);
289 
290  evaluateObjective.addIndex( runObj );
291 
292  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
293  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
294  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
295  loopObjective.addLinebreak( );
296 
297  // Evaluate the objective function
298  loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
299 
300  // Stack the measurement function value
301  loopObjective.addStatement(
302  Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
303  );
304  loopObjective.addLinebreak( );
305 
306  // Optionally compute derivatives
307 
308  ExportVariable tmpObjS, tmpFx, tmpFu;
309  ExportVariable tmpFxEnd, tmpObjSEndTerm;
310  tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
311  if (objS.isGiven() == true)
312  tmpObjS = objS;
313  tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
314  if (objEvFx.isGiven() == true)
315  tmpFx = objEvFx;
316  tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
317  if (objEvFu.isGiven() == true)
318  tmpFu = objEvFu;
319  tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
320  if (objEvFxEnd.isGiven() == true)
321  tmpFxEnd = objEvFxEnd;
322  tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
323  if (objSEndTerm.isGiven() == true)
324  tmpObjSEndTerm = objSEndTerm;
325 
326  unsigned indexX = getNY();
327  ExportArgument tmpFxCall = tmpFx;
328  if (tmpFx.isGiven() == false)
329  {
330  tmpFxCall = objValueOut.getAddress(0, indexX);
331  indexX += objEvFx.getDim();
332  }
333 
334  ExportArgument tmpFuCall = tmpFu;
335  if (tmpFu.isGiven() == false)
336  {
337  tmpFuCall = objValueOut.getAddress(0, indexX);
338  }
339 
340  ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
341 
342  //
343  // Optional computation of Q1, Q2
344  //
345  if (Q1.isGiven() == false)
346  {
347  ExportVariable tmpQ1, tmpQ2;
348  tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
349  tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
350 
351  setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
352  setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
353  setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
354 
355  loopObjective.addFunctionCall(
356  setObjQ1Q2,
357  tmpFxCall, objSCall,
358  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
359  );
360 
361  loopObjective.addLinebreak( );
362  }
363 
364  if (R1.isGiven() == false)
365  {
366  ExportVariable tmpR1, tmpR2;
367  tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
368  tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
369 
370  setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
371  setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
372  setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
373 
374  loopObjective.addFunctionCall(
375  setObjR1R2,
376  tmpFuCall, objSCall,
377  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
378  );
379 
380  loopObjective.addLinebreak( );
381  }
382 
383  if (S1.isGiven() == false)
384  {
385  ExportVariable tmpS1;
386  ExportVariable tmpS2;
387 
388  tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
389  tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
390 
391  setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
392  setObjS1.addVariable( tmpS2 );
393  setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
394  setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
395 
396  loopObjective.addFunctionCall(
397  setObjS1,
398  tmpFxCall, tmpFuCall, objSCall,
399  S1.getAddress(runObj * NX, 0)
400  );
401  }
402 
403  evaluateObjective.addStatement( loopObjective );
404 
405  //
406  // Evaluate the quadratic Mayer term
407  //
408  evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
409  evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
410 
411  // Evaluate the objective function, last node.
412  evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
413  evaluateObjective.addLinebreak( );
414 
415  evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
416  evaluateObjective.addLinebreak();
417 
418  if (QN1.isGiven() == false)
419  {
420  ExportVariable tmpQN1, tmpQN2;
421  tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
422  tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
423 
424  setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
425  setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
426  setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
427 
428  indexX = getNYN();
429  ExportArgument tmpFxEndCall = tmpFxEnd.isGiven() == true ? tmpFxEnd : objValueOut.getAddress(0, indexX);
430 
431  evaluateObjective.addFunctionCall(
432  setObjQN1QN2,
433  tmpFxEndCall, objSEndTerm,
434  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
435  );
436 
437  evaluateObjective.addLinebreak( );
438  }
439 
440  return SUCCESSFUL_RETURN;
441 }
442 
443 returnValue ExportGaussNewtonCN2::setupConstraintsEvaluation( void )
444 {
445  ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
446 
447  int hardcodeConstraintValues;
448  get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
449 
450  int hessianApproximation;
451  get( HESSIAN_APPROXIMATION, hessianApproximation );
452  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
453 
455  //
456  // Setup the bounds on control variables
457  //
459 
460  unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
461  unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;
462 
463  DVector lbValuesMatrix( numBounds );
464  DVector ubValuesMatrix( numBounds );
465 
466  if (initialStateFixed( ) == false)
467  for(unsigned run1 = 0; run1 < NX; ++run1)
468  {
469  lbValuesMatrix( run1 )= xBounds.getLowerBound(0, run1);
470  ubValuesMatrix( run1) = xBounds.getUpperBound(0, run1);
471  }
472 
473  for(unsigned run1 = 0; run1 < N; ++run1)
474  for(unsigned run2 = 0; run2 < NU; ++run2)
475  {
476  lbValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getLowerBound(run1, run2);
477  ubValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getUpperBound(run1, run2);
478  }
479 
480 
481  if (hardcodeConstraintValues == YES)
482  {
483  lbValues.setup("lbValues", lbValuesMatrix, REAL, ACADO_VARIABLES);
484  ubValues.setup("ubValues", ubValuesMatrix, REAL, ACADO_VARIABLES);
485  }
486  else
487 // else if (isFinite( lbValuesMatrix ) || isFinite( ubValuesMatrix ))
488  {
489  lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
490  lbValues.setDoc( "Lower bounds values." );
491  ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
492  ubValues.setDoc( "Upper bounds values." );
493 
494  initialize.addStatement(lbValues == lbValuesMatrix);
495  initialize.addStatement(ubValues == ubValuesMatrix);
496  }
497 
498  ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;
499 
500  if (performFullCondensing() == true)
501  {
502  boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
503  boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
504  }
505  else
506  {
507  if ( initialStateFixed( ) == true )
508  {
509  condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
510  condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );
511 
512  boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
513  boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
514  }
515  else
516  {
517  boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
518  boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
519 
520  boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
521  boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
522  }
523  }
524  condensePrep.addLinebreak( );
525  condenseFdb.addLinebreak( );
526 
528  //
529  // Setup the bounds on state variables
530  //
532 
533  DVector lbTmp; DVector ubTmp;
534  unsigned numStateBounds = getNumStateBounds();
535  unsigned numPathCon = N * dimPacH;
536  unsigned numPointCon = dimPocH;
537  if( getNumStateBounds() )
538  {
539  condenseFdb.addVariable( tmp );
540 
541  lbTmp = DVector( getNumStateBounds( ));
542  ubTmp = DVector( getNumStateBounds( ) );
543  for(unsigned i = 0; i < xBoundsIdx.size(); ++i)
544  {
545  lbTmp( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
546  ubTmp( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
547  }
548 
549  unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;
550 
551  if (numOps < 1024)
552  {
553  for(unsigned row = 0; row < getNumStateBounds( ); ++row)
554  {
555  unsigned conIdx = xBoundsIdx[ row ] - NX;
556  if( initialStateFixed( ) == false ) {
557  condensePrep.addStatement( A.getSubMatrix(row, row + 1, 0, NX ) == C.getRow( conIdx ) );
558  }
559 
560  unsigned blk = conIdx / NX + 1;
561  for (unsigned col = 0; col < blk; ++col)
562  {
563  unsigned blkRow = (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX;
564 
565  condensePrep.addStatement(
566  A.getSubMatrix(row, row + 1, offsetBounds + col * NU, offsetBounds + (col + 1) * NU ) == E.getRow( blkRow ) );
567  }
568 
569  condensePrep.addLinebreak();
570  }
571  }
572  else
573  {
574 
575  DMatrix vXBoundIndices(1, numStateBounds);
576  for (unsigned i = 0; i < numStateBounds; ++i)
577  vXBoundIndices(0, i) = xBoundsIdx[ i ];
578  ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);
579 
580  condensePrep.addVariable( evXBounds );
581 
582  ExportIndex row, col, conIdx, blk, blkRow;
583 
584  condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );
585 
586  ExportForLoop lRow(row, 0, numStateBounds);
587 
588  lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
589 
590  if( initialStateFixed( ) == false ) {
591  lRow.addStatement( A.getSubMatrix(row, row + 1, 0, NX ) == C.getRow( conIdx ) );
592  }
593 
594  lRow.addStatement( blk == conIdx / NX + 1 );
595 
596  ExportForLoop lCol(col, 0, blk);
597 
598  lCol.addStatement( blkRow == (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX );
599  lCol.addStatement(
600  A.getSubMatrix(row, row + 1, offsetBounds + col * NU, offsetBounds + (col + 1) * NU ) == E.getRow( blkRow ) );
601 
602  lRow.addStatement( lCol );
603  condensePrep.addStatement( lRow );
604 
605  condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
606  }
607  condensePrep.addLinebreak( );
608  }
609  if( getNumStateBounds() + getNumComplexConstraints()) {
610 
611  lbTmp.append( lbPathConValues );
612  ubTmp.append( ubPathConValues );
613 
614  lbTmp.append( lbPointConValues );
615  ubTmp.append( ubPointConValues );
616 
617  if (hardcodeConstraintValues == YES)
618  {
619  lbAValues.setup("lbAValues", lbTmp, REAL, ACADO_VARIABLES);
620  ubAValues.setup("ubAValues", ubTmp, REAL, ACADO_VARIABLES);
621  }
622  else
623  {
624  lbAValues.setup("lbAValues", numStateBounds+numPathCon+numPointCon, 1, REAL, ACADO_VARIABLES);
625  lbAValues.setDoc( "Lower bounds values for affine constraints." );
626  ubAValues.setup("ubAValues", numStateBounds+numPathCon+numPointCon, 1, REAL, ACADO_VARIABLES);
627  ubAValues.setDoc( "Upper bounds values for affine constraints." );
628 
629  initialize.addStatement(lbAValues == lbTmp);
630  initialize.addStatement(ubAValues == ubTmp);
631  }
632 
633  // Shift constraint bounds by first interval
634  for(unsigned boundIndex = 0; boundIndex < getNumStateBounds( ); ++boundIndex)
635  {
636  unsigned row = xBoundsIdx[boundIndex];
637 
638  condenseFdb.addStatement( tmp == sbar.getRow( row ) + x.makeRowVector().getCol( row ) );
639  condenseFdb.addStatement( lbA.getRow( boundIndex ) == lbAValues.getRow( boundIndex ) - tmp );
640  condenseFdb.addStatement( ubA.getRow( boundIndex ) == ubAValues.getRow( boundIndex ) - tmp );
641  }
642  condenseFdb.addLinebreak( );
643  }
644 
645 
647  //
648  // Setup the evaluation of the path constraints
649  //
651 
652  if ( numPathCon )
653  {
654  unsigned rowOffset = numStateBounds;
655  unsigned colOffset = performFullCondensing() == true ? 0 : NX;
656 
657  //
658  // Setup evaluation
659  //
660  ExportIndex runPac;
661  if( secondOrder ) {
662  evaluateObjective.acquire( runPac );
663  }
664  else {
665  condensePrep.acquire( runPac );
666  }
667  ExportForLoop loopPac(runPac, 0, N);
668 
669  loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
670 
671  uint dimL = 0;
672  if ( secondOrder && pacEvDDH.isGiven() == false ) {
673  dimL = dimPacH;
674  loopPac.addStatement( conValueIn.getCols(NX, NX+dimPacH) == yVars.getRows(getNumQPvars()+getNumStateBounds()+runPac*dimPacH,getNumQPvars()+getNumStateBounds()+runPac*dimPacH+dimPacH).getTranspose() );
675  }
676 
677  loopPac.addStatement( conValueIn.getCols(NX+dimL, NX+dimL + NU) == u.getRow( runPac ) );
678  loopPac.addStatement( conValueIn.getCols(NX+dimL + NU, NX+dimL + NU + NOD) == od.getRow( runPac ) );
679  loopPac.addFunctionCall( evaluatePathConstraints.getName(), conValueIn, conValueOut );
680 
681  loopPac.addStatement( pacEvH.getRows( runPac * dimPacH, (runPac + 1) * dimPacH) ==
682  conValueOut.getTranspose().getRows(0, dimPacH) );
683  loopPac.addLinebreak( );
684 
685  unsigned derOffset = dimPacH;
686 
687  // Optionally store derivatives
688  if ( pacEvHx.isGiven() == false )
689  {
690  loopPac.addStatement(
691  pacEvHx.makeRowVector().
692  getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX)
693  == conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
694  );
695 
696  derOffset = derOffset + dimPacH * NX;
697  }
698  if (pacEvHu.isGiven() == false )
699  {
700  loopPac.addStatement(
701  pacEvHu.makeRowVector().
702  getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU)
703  == conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
704  );
705 
706  derOffset = derOffset + dimPacH * NU;
707  }
708  if( secondOrder ) {
709  if (pacEvDDH.isGiven() == false )
710  {
711  loopPac.addStatement( pacEvDDH.makeRowVector() == conValueOut.getCols(derOffset, derOffset + (NX+NU)*(NX+NU) ) );
712  }
713  loopPac.addStatement( objS.getRows( runPac*(NX+NU), runPac*(NX+NU)+NX+NU ) += pacEvDDH );
714  }
715 
716  // Add loop to the function.
717  if( secondOrder ) {
718  evaluateObjective.addStatement( loopPac );
719  evaluateObjective.addLinebreak( );
720  }
721  else {
722  condensePrep.addStatement( loopPac );
723  condensePrep.addLinebreak( );
724  }
725 
726  // Define the multHxC multiplication routine
727  ExportVariable tmpA01, tmpHx, tmpGx;
728 
729  if (pacEvHx.isGiven() == true)
730  tmpHx = pacEvHx;
731  else
732  tmpHx.setup("Hx", dimPacH, NX, REAL, ACADO_LOCAL);
733 
734  tmpGx.setup("Gx", NX, NX, REAL, ACADO_LOCAL);
735 
736  if (performFullCondensing() == false)
737  {
738 // tmpA01.setup("A01", dimPacH, NX, REAL, ACADO_LOCAL);
739 //
740 // multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
741 // multHxC.addStatement( tmpA01 == tmpHx * tmpGx );
742 //
743 // A10.setup("A01", numPathCon, NX, REAL, ACADO_WORKSPACE);
744 // }
745 // else
746 // {
747  tmpA01.setup("A01", dimPacH, getNumQPvars(), REAL, ACADO_LOCAL);
748  multHxC.setup("multHxC", tmpHx, tmpGx, tmpA01);
749  multHxC.addStatement( tmpA01.getSubMatrix(0, dimPacH, 0, NX) == tmpHx * tmpGx );
750 
751  A10 = A;
752  }
753 
754  unsigned offsetA01 = (performFullCondensing() == true) ? 0 : rowOffset;
755 
756  if (performFullCondensing() == false) {
757  // Define the block A_{10}(0: dimPacH, 0: NX) = H_{x}(0: dimPacH, 0: NX)
758  if (pacEvHx.isGiven() == true)
759  {
760  condensePrep.addStatement(
761  A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
762  == pacEvHx);
763  }
764  else
765  {
766  condensePrep.addStatement(
767  A10.getSubMatrix(offsetA01, offsetA01 + dimPacH, 0, NX)
768  == pacEvHx.getSubMatrix(0, dimPacH, 0, NX));
769  }
770  }
771  condensePrep.addLinebreak();
772 
773  if (performFullCondensing() == false) {
774  // Evaluate Hx * C
775  for (unsigned row = 0; row < N - 1; ++row)
776  {
777  if (pacEvHx.isGiven() == true)
778  {
779  condensePrep.addFunctionCall(
780  multHxC,
781  pacEvHx,
782  C.getAddress(row * NX, 0),
783  A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
784  }
785  else
786  {
787  condensePrep.addFunctionCall(
788  multHxC,
789  pacEvHx.getAddress((row + 1) * dimPacH, 0),
790  C.getAddress(row * NX, 0),
791  A10.getAddress(offsetA01 + (row + 1) * dimPacH, 0) );
792  }
793  }
794  }
795  condensePrep.addLinebreak();
796 
797  //
798  // Evaluate Hx * E
799  //
800  ExportVariable tmpE;
801  tmpE.setup("E", NX, NU, REAL, ACADO_LOCAL);
802  ExportIndex iRow( "row" ), iCol( "col" );
803 
804  multHxE.setup("multHxE", tmpHx, tmpE, iRow, iCol);
805  multHxE.addStatement(
806  A.getSubMatrix( rowOffset + iRow * dimPacH,
807  rowOffset + (iRow + 1) * dimPacH,
808  colOffset + iCol * NU,
809  colOffset + (iCol + 1) * NU)
810  == tmpHx * tmpE );
811 
812  if ( N <= 20 )
813  {
814  for (unsigned row = 0; row < N - 1; ++row)
815  {
816  for (unsigned col = 0; col <= row; ++col)
817  {
818  unsigned blk = (col * (2 * N - col - 1) / 2 + row);
819  unsigned row2 = row + 1;
820 
821  if (pacEvHx.isGiven() == true)
822  condensePrep.addFunctionCall(
823  multHxE,
824  pacEvHx,
825  E.getAddress(blk * NX, 0),
826  ExportIndex( row2 ),
827  ExportIndex( col )
828  );
829  else
830  condensePrep.addFunctionCall(
831  multHxE,
832  pacEvHx.getAddress((row + 1) * dimPacH, 0),
833  E.getAddress(blk * NX, 0),
834  ExportIndex( row2 ),
835  ExportIndex( col )
836  );
837  }
838  }
839  }
840  else
841  {
842  ExportIndex row, col, blk, row2;
843  condensePrep.acquire( row );
844  condensePrep.acquire( col );
845  condensePrep.acquire( blk );
846  condensePrep.acquire( row2 );
847 
848  ExportForLoop eLoopI(row, 0, N - 1);
849  ExportForLoop eLoopJ(col, 0, row + 1);
850 
851  eLoopJ.addStatement( blk == (col * (2 * N - col - 1) / 2 + row) );
852  eLoopJ.addStatement( row2 == row + 1 );
853 
854  if (pacEvHx.isGiven() == true)
855  {
856  eLoopJ.addFunctionCall(
857  multHxE,
858  pacEvHx,
859  E.getAddress(blk * NX, 0),
860  row2,
861  col
862  );
863  }
864  else
865  {
866  eLoopJ.addFunctionCall(
867  multHxE,
868  pacEvHx.getAddress((row + 1) * dimPacH, 0),
869  E.getAddress(blk * NX, 0),
870  row2,
871  col
872  );
873  }
874 
875  eLoopI.addStatement( eLoopJ );
876  condensePrep.addStatement( eLoopI );
877 
878  condensePrep.release( row );
879  condensePrep.release( col );
880  condensePrep.release( blk );
881  condensePrep.release( row2 );
882  }
883  condensePrep.addLinebreak();
884 
885  if (pacEvHu.getDim() > 0)
886  {
887  for (unsigned i = 0; i < N; ++i)
888  {
889  if (pacEvHu.isGiven() == true)
890  initialize.addStatement(
891  A.getSubMatrix(
892  rowOffset + i * dimPacH,
893  rowOffset + (i + 1) * dimPacH,
894  colOffset + i * NU,
895  colOffset + (i + 1) * NU)
896  == pacEvHu
897  );
898  else
899  condensePrep.addStatement(
900  A.getSubMatrix(
901  rowOffset + i * dimPacH,
902  rowOffset + (i + 1) * dimPacH,
903  colOffset + i * NU,
904  colOffset + (i + 1) * NU)
905  == pacEvHu.getSubMatrix(
906  i * dimPacH,(i + 1) * dimPacH, 0, NU)
907  );
908  }
909  }
910 
911  //
912  // Set upper and lower bounds
913  //
914  condensePrep.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) ==
915  lbAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
916  condensePrep.addLinebreak();
917  condensePrep.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) ==
918  ubAValues.getRows(rowOffset, rowOffset + numPathCon) - pacEvH);
919  condensePrep.addLinebreak();
920 
921 // if (performFullCondensing() == true)
922 // {
923 // pacA01Dx0.setup("pacA01Dx0", numPathCon, 1, REAL, ACADO_WORKSPACE);
924 //
925 // condenseFdb.addStatement( pacA01Dx0 == A10 * Dx0 );
926 // condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
927 // condenseFdb.addLinebreak();
928 // condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + numPathCon) -= pacA01Dx0);
929 // condenseFdb.addLinebreak();
930 // }
931 
932  // Evaluate Hx * d
933  if ( performsSingleShooting() == false )
934  {
935  ExportVariable tmpd("tmpd", NX, 1, REAL, ACADO_LOCAL);
936  ExportVariable tmpLb("lbA", dimPacH, 1, REAL, ACADO_LOCAL);
937  ExportVariable tmpUb("ubA", dimPacH, 1, REAL, ACADO_LOCAL);
938 
939  macHxd.setup("macHxd", tmpHx, tmpd, tmpLb, tmpUb);
940  macHxd.addStatement( pacEvHxd == tmpHx * tmpd );
941  macHxd.addStatement( tmpLb -= pacEvHxd );
942  macHxd.addStatement( tmpUb -= pacEvHxd );
943 
944  for (unsigned i = 0; i < N; ++i)
945  {
946  if (pacEvHx.isGiven() == true)
947  {
948  condenseFdb.addFunctionCall(
949  macHxd,
950  pacEvHx,
951  sbar.getAddress(i * NX),
952  lbA.getAddress(rowOffset + i * dimPacH),
953  ubA.getAddress(rowOffset + i * dimPacH)
954  );
955  }
956  else
957  {
958  condenseFdb.addFunctionCall(
959  macHxd,
960  pacEvHx.getAddress(i * dimPacH, 0),
961  sbar.getAddress(i * NX),
962  lbA.getAddress(rowOffset + i * dimPacH),
963  ubA.getAddress(rowOffset + i * dimPacH)
964  );
965  }
966  }
967  condenseFdb.addLinebreak();
968  }
969  }
970 
972  //
973  // Setup the evaluation of the point constraints
974  //
976 
977  if ( numPointCon )
978  {
979  unsigned rowOffset = getNumStateBounds() + N * dimPacH;
980  unsigned colOffset = performFullCondensing() == true ? 0 : NX;
981  unsigned dim;
982 
983  if( secondOrder ) {
984  return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
985  }
986 
987 // if (performFullCondensing() == true)
988 // A20.setup("A02", dimPocH, NX, REAL, ACADO_WORKSPACE);
989 
990  //
991  // Evaluate the point constraints
992  //
993  for (unsigned i = 0, intRowOffset = 0; i < N + 1; ++i)
994  {
995  if (evaluatePointConstraints[ i ] == 0)
996  continue;
997 
998  condensePrep.addComment( string( "Evaluating constraint on node: #" ) + toString( i ) );
999  condensePrep.addLinebreak();
1000 
1001  condensePrep.addStatement(conValueIn.getCols(0, getNX()) == x.getRow( i ) );
1002  if (i < N)
1003  {
1004  condensePrep.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
1005  condensePrep.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
1006  }
1007  else
1008  condensePrep.addStatement( conValueIn.getCols(NX, NX + NOD) == od.getRow( i ) );
1009 
1010  condensePrep.addFunctionCall( evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
1011  condensePrep.addLinebreak();
1012 
1013  if (i < N)
1014  dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
1015  else
1016  dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
1017 
1018  // Fill pocEvH, pocEvHx, pocEvHu
1019  condensePrep.addStatement(
1020  pocEvH.getRows(intRowOffset, intRowOffset + dim)
1021  == conValueOut.getTranspose().getRows(0, dim));
1022  condensePrep.addLinebreak();
1023 
1024  condensePrep.addStatement(
1025  pocEvHx.makeRowVector().getCols(intRowOffset * NX,
1026  (intRowOffset + dim) * NX)
1027  == conValueOut.getCols(dim, dim + dim * NX));
1028  condensePrep.addLinebreak();
1029 
1030  if (i < N)
1031  {
1032  condensePrep.addStatement(
1033  pocEvHu.makeRowVector().getCols(intRowOffset * NU,
1034  (intRowOffset + dim) * NU)
1035  == conValueOut.getCols(dim + dim * NX,
1036  dim + dim * NX + dim * NU));
1037  condensePrep.addLinebreak();
1038  }
1039 
1040  intRowOffset += dim;
1041  }
1042 
1043  //
1044  // Include point constraint data in the QP problem
1045  //
1046  for (unsigned row = 0, intRowOffset = 0; row < N + 1; ++row)
1047  {
1048  if (evaluatePointConstraints[ row ] == 0)
1049  continue;
1050 
1051  condensePrep.addComment(
1052  string( "Evaluating multiplications of constraint functions on node: #" ) + toString( row ) );
1053  condensePrep.addLinebreak();
1054 
1055  if (row < N)
1056  dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX + NU);
1057  else
1058  dim = evaluatePointConstraints[ row ]->getFunctionDim() / (1 + NX);
1059 
1060  if (row == 0)
1061  {
1062  if (performFullCondensing() == false) {
1063 // condensePrep.addStatement(
1064 // A20.getSubMatrix(0, dim, 0, NX)
1065 // == pocEvHx.getSubMatrix(0, dim, 0, NX));
1066 // else
1067  condensePrep.addStatement(
1068  A.getSubMatrix(rowOffset, rowOffset + dim, 0, NX)
1069  == pocEvHx.getSubMatrix(0, dim, 0, NX));
1070  }
1071  condensePrep.addLinebreak();
1072 
1073  condensePrep.addStatement(
1074  A.getSubMatrix(rowOffset, rowOffset + dim, colOffset,
1075  colOffset + NU)
1076  == pocEvHu.getSubMatrix(0, dim, 0, NU));
1077  condensePrep.addLinebreak();
1078  }
1079  else
1080  {
1081  // Hx * C
1082  if (performFullCondensing() == false) {
1083 // condensePrep.addStatement(
1084 // A20.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) ==
1085 // pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
1086 // C.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
1087 // else
1088  condensePrep.addStatement(
1089  A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim, 0, NX) ==
1090  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NX) *
1091  C.getSubMatrix((row - 1) * NX, row * NX, 0, NX) );
1092  }
1093  condensePrep.addLinebreak();
1094 
1095  // Hx * E
1096  ExportIndex iCol, iBlk;
1097  condensePrep.acquire( iCol );
1098  condensePrep.acquire( iBlk );
1099  ExportForLoop eLoop(iCol, 0, row);
1100 
1101  // row - 1, col -> blk
1102  eLoop.addStatement( iBlk == (iCol * (2 * N - iCol - 1) / 2 + row-1) );
1103  eLoop.addStatement(
1104  A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
1105  colOffset + iCol * NU, colOffset + (iCol + 1) * NU) ==
1106  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
1107  E.getSubMatrix(iBlk * NX, (iBlk + 1) * NX, 0, NU)
1108  );
1109 
1110  condensePrep.addStatement( eLoop );
1111  condensePrep.release( iCol );
1112  condensePrep.release( iBlk );
1113 
1114  // Add Hu block to the A21 block
1115  if (row < N)
1116  {
1117  condensePrep.addStatement(
1118  A.getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
1119  colOffset + row * NU, colOffset + (row + 1) * NU) ==
1120  pocEvHu.getSubMatrix(intRowOffset, intRowOffset + dim, 0, NU));
1121  condensePrep.addLinebreak();
1122  }
1123  }
1124 
1125  // Hx * d, MS only
1126  if (performsSingleShooting() == false)
1127  {
1128  condenseFdb.addStatement(
1129  pocEvHxd.getRows(intRowOffset, intRowOffset + dim) ==
1130  pocEvHx.getSubMatrix(intRowOffset, intRowOffset + dim, 0 , NX) *
1131  sbar.getRows(row * NX, row * NX + NX) );
1132  condenseFdb.addLinebreak();
1133  }
1134 
1135  intRowOffset += dim;
1136  }
1137 
1138  //
1139  // And now setup the lbA and ubA
1140  //
1141  condensePrep.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) ==
1142  lbAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
1143  condensePrep.addLinebreak();
1144  condensePrep.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) ==
1145  ubAValues.getRows(rowOffset, rowOffset + dimPocH) - pocEvH);
1146  condensePrep.addLinebreak();
1147 
1148 // if (performFullCondensing() == true)
1149 // {
1150 // pocA02Dx0.setup("pacA02Dx0", dimPocH, 1, REAL, ACADO_WORKSPACE);
1151 //
1152 // condenseFdb.addStatement( pocA02Dx0 == A20 * Dx0 );
1153 // condenseFdb.addLinebreak();
1154 // condenseFdb.addStatement(lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
1155 // condenseFdb.addLinebreak();
1156 // condenseFdb.addStatement(ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocA02Dx0);
1157 // condenseFdb.addLinebreak();
1158 // }
1159 
1160  if (performsSingleShooting() == false)
1161  {
1162  condenseFdb.addStatement( lbA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
1163  condenseFdb.addLinebreak();
1164  condenseFdb.addStatement( ubA.getRows(rowOffset, rowOffset + dimPocH) -= pocEvHxd );
1165  condenseFdb.addLinebreak();
1166  }
1167  }
1168 
1169  return SUCCESSFUL_RETURN;
1170 }
1171 
1172 returnValue ExportGaussNewtonCN2::setupCondensing( void )
1173 {
1174  condensePrep.setup("condensePrep");
1175  condenseFdb.setup( "condenseFdb" );
1176 
1178  //
1179  // Setup local memory for preparation phase: T1, T2, W1, W2
1180  //
1182 
1183  // TODO
1184 
1186  //
1187  // Compute Hessian block H00, H10 and C
1188  //
1190 
1191  if (performFullCondensing() == false || getNumComplexConstraints() > 0) {
1192  LOG( LVL_DEBUG ) << "Setup condensing: C" << endl;
1193  condensePrep.addFunctionCall(moveGxT, evGx.getAddress(0, 0), C.getAddress(0, 0));
1194  for (unsigned row = 1; row < N; ++row)
1195  condensePrep.addFunctionCall(
1196  multGxGx, evGx.getAddress(row * NX), C.getAddress((row - 1) * NX), C.getAddress(row * NX));
1197 
1198  }
1199 
1200  if (performFullCondensing() == false)
1201  {
1202  LOG( LVL_DEBUG ) << "Setup condensing: H00, H10" << endl;
1203 
1204  T1.setup("T1", NX, NX, REAL, ACADO_WORKSPACE);
1205  T2.setup("T2", NX, NX, REAL, ACADO_WORKSPACE);
1206 
1207  /* Algorithm for computation of H10 and H00
1208 
1209  T1 = Q_N * C_{N - 1}
1210  for i = N - 1: 1
1211  H_{i + 1, 0} = B_i^T * T1
1212  H_{i + 1, 0} += S_i^T * C_{i - 1}
1213 
1214  T2 = A_i^T * T1
1215  T1 = T2 + Q_i * C_{i - 1}
1216 
1217  H_{1, 0} = B_0^T * T1
1218  H_{1, 0} += S_0^T
1219  H_{0, 0} = Q_0 + A_0^T * T1
1220 
1221  */
1222 
1223  // H10 Block
1224  ExportFunction* QN1Call = QN1.isGiven() == true ? &multQN1Gx : &multGxGx;
1225  condensePrep.addFunctionCall(*QN1Call, QN1, C.getAddress((N - 1) * NX), T1);
1226  for (unsigned row = N - 1; row > 0; --row)
1227  {
1228  condensePrep.addFunctionCall(mult_BT_T1, evGu.getAddress(row * NX), T1, ExportIndex( row ));
1229 
1230  if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
1231  {
1232  ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(row * NX) : S1;
1233  condensePrep.addFunctionCall(mac_ST_C, S1Call, C.getAddress((row - 1) * NX), ExportIndex( row ));
1234  }
1235 
1236  condensePrep.addFunctionCall(multGxTGx, evGx.getAddress(row * NX), T1, T2);
1237 
1238  ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(row * NX);
1239  condensePrep.addFunctionCall(macGxTGx, T2, Q1Call, C.getAddress((row - 1) * NX), T1);
1240  }
1241 
1242  condensePrep.addFunctionCall(mult_BT_T1, evGu.getAddress( 0 ), T1, ExportIndex( 0 ));
1243  if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
1244  {
1245  condensePrep.addStatement(
1246  H.getSubMatrix(NX, NX + NU, 0, NX) += S1.getSubMatrix(0, NX, 0, NU).getTranspose()
1247  );
1248  }
1249 
1250  // H00 Block
1251  DMatrix mRegH00 = eye<double>( getNX() );
1252  mRegH00 *= levenbergMarquardt;
1253  condensePrep.addStatement(
1254  H.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(0, NX, 0, NX) + (evGx.getSubMatrix(0, NX, 0, NX).getTranspose() * T1)
1255  );
1256  condensePrep.addStatement( H.getSubMatrix(0, NX, 0, NX) += mRegH00 );
1257  }
1258 
1260  //
1261  // Compute Hessian block H11
1262  //
1264 
1265  LOG( LVL_DEBUG ) << "Setup condensing: H11 and E" << endl;
1266 
1267  /*
1268 
1269  This one is only for the case where we have input constraints only
1270 
1271  // Create E and H
1272 
1273  for i = 0: N - 1
1274  {
1275  // Storage for E: (N x nx) x nu
1276 
1277  E_0 = B_i;
1278  for k = 1: N - i - 1
1279  E_k = A_{i + k} * E_{k - 1};
1280 
1281  // Two temporary matrices W1, W2 of size nx x nu
1282 
1283  W1 = Q_N^T * E_{N - i - 1};
1284 
1285  for k = N - 1: i + 1
1286  {
1287  H_{k, i} = B_k^T * W1;
1288 
1289  W2 = A_k^T * W1;
1290  W1 = Q_k^T * E_{k - i - 1} + W2;
1291  }
1292  H_{i, i} = B_i^T * W1 + R_i^T
1293  }
1294 
1295  Else, in general case:
1296 
1297  for i = 0: N - 1
1298  {
1299  // Storage for E: (N * (N + 1) / 2 x nx) x nu
1300 
1301  j = 1 / 2 * i * (2 * N - i + 1);
1302 
1303  E_j = B_i;
1304  for k = 1: N - i - 1
1305  E_{j + k} = A_{i + k} * E_{j + k - 1};
1306 
1307  // Two temporary matrices W1, W2 of size nx x nu
1308 
1309  W1 = Q_N^T * E_{j + N - i - 1};
1310 
1311  for k = N - 1: i + 1
1312  {
1313  H_{k, i} = B_k^T * W1;
1314  H_{k, i} += S_k^T * E_{j + k - i - 1};
1315 
1316  W2 = A_k^T * W1;
1317  W1 = Q_k^T * E_{j + k - i - 1} + W2;
1318  }
1319  H_{i, i} = B_i^T * W1 + R_i^T
1320  }
1321 
1322  */
1323 
1324  W1.setup("W1", NX, NU, REAL, ACADO_WORKSPACE);
1325  W2.setup("W2", NX, NU, REAL, ACADO_WORKSPACE);
1326 
1327  if (N <= 15)
1328  {
1329  for (unsigned col = 0; col < N; ++col)
1330  {
1331  int offset = col * (2 * N - col + 1) / 2;
1332 
1333  condensePrep.addComment( "Column: " + toString( col ) );
1334  condensePrep.addFunctionCall(
1335  moveGuE, evGu.getAddress(col * NX), E.getAddress(offset * NX)
1336  );
1337  for (unsigned row = 1; row < N - col; ++row)
1338  condensePrep.addFunctionCall(
1339  multGxGu,
1340  evGx.getAddress((col + row) * NX),
1341  E.getAddress((offset + row - 1) * NX), E.getAddress((offset + row) * NX)
1342  );
1343  condensePrep.addLinebreak();
1344 
1345  ExportFunction* QN1Call = QN1.isGiven() == true ? &multQN1Gu : &multGxGu;
1346  condensePrep.addFunctionCall(
1347  *QN1Call, QN1, E.getAddress((offset + N - col - 1) * NX), W1
1348  );
1349 
1350  for (unsigned row = N - 1; col < row; --row)
1351  {
1352  condensePrep.addFunctionCall(
1353  multBTW1, evGu.getAddress(row * NX), W1,
1354  ExportIndex( row ), ExportIndex( col )
1355  );
1356 
1357  if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
1358  {
1359  ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(row * NX) : S1;
1360 
1361  condensePrep.addFunctionCall(
1362  mac_S1T_E,
1363  S1Call, E.getAddress((offset + row - col - 1) * NX),
1364  ExportIndex( row ), ExportIndex( col )
1365  );
1366  }
1367 
1368  condensePrep.addFunctionCall(
1369  multGxTGu, evGx.getAddress(row * NX), W1, W2
1370  );
1371 
1372  ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(row * NX);
1373  condensePrep.addFunctionCall(
1374  macQEW2, Q1Call, E.getAddress((offset + row - col - 1) * NX), W2, W1
1375  );
1376  }
1377 
1378  ExportArgument R1Call = R1.isGiven() == true ? R1 : R1.getAddress(col * NU);
1379  condensePrep.addFunctionCall(
1380  macBTW1_R1, R1Call, evGu.getAddress(col * NX), W1,
1381  ExportIndex( col )
1382  );
1383 
1384  condensePrep.addLinebreak();
1385  }
1386  }
1387  else
1388  {
1389  // Long horizons
1390 
1391  ExportIndex row, col, offset;
1392  condensePrep.acquire( row ).acquire( col ).acquire( offset );
1393 
1394  ExportForLoop cLoop(col, 0, N);
1395  ExportForLoop fwdLoop(row, 1, N - col);
1396  ExportForLoop adjLoop(row, N - 1, col, -1);
1397 
1398  cLoop.addStatement( offset == col * (2 * N + 1 - col) / 2 );
1399  cLoop.addFunctionCall(
1400  moveGuE, evGu.getAddress(col * NX), E.getAddress(offset * NX)
1401  );
1402 
1403  fwdLoop.addFunctionCall(
1404  multGxGu,
1405  evGx.getAddress((col + row) * NX),
1406  E.getAddress((offset + row - 1) * NX), E.getAddress((offset + row) * NX)
1407  );
1408  cLoop.addStatement( fwdLoop );
1409  cLoop.addLinebreak();
1410 
1411  ExportFunction* QN1Call = QN1.isGiven() == true ? &multQN1Gu : &multGxGu;
1412  cLoop.addFunctionCall(
1413  *QN1Call, QN1, E.getAddress((offset - col + N - 1) * NX), W1
1414  );
1415 
1416  adjLoop.addFunctionCall(
1417  multBTW1, evGu.getAddress(row * NX), W1,
1418  row, col
1419  );
1420 
1421  if ((S1.isGiven() && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
1422  {
1423  ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(row * NX) : S1;
1424 
1425  adjLoop.addFunctionCall(
1426  mac_S1T_E,
1427  S1Call, E.getAddress((offset + row - col - 1) * NX),
1428  row, col
1429  );
1430  }
1431 
1432  adjLoop.addFunctionCall(
1433  multGxTGu, evGx.getAddress(row * NX), W1, W2
1434  );
1435 
1436  ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(row * NX);
1437  adjLoop.addFunctionCall(
1438  macQEW2, Q1Call, E.getAddress((offset + row - col - 1) * NX), W2, W1
1439  );
1440 
1441  cLoop.addStatement( adjLoop );
1442 
1443  ExportArgument R1Call = R1.isGiven() == true ? R1 : R1.getAddress(col * NU);
1444  cLoop.addFunctionCall(
1445  macBTW1_R1, R1Call, evGu.getAddress(col * NX), W1,
1446  ExportIndex( col )
1447  );
1448 
1449  condensePrep.addStatement( cLoop );
1450  condensePrep.addLinebreak();
1451 
1452  condensePrep.release( row ).release( col ).release( offset );
1453  }
1454 
1456 
1457  LOG( LVL_DEBUG ) << "---> Copy H11 lower part" << endl;
1458 
1459  // Copy to H11 upper lower part to upper triangular part
1460  if (N <= 20)
1461  {
1462  for (unsigned ii = 0; ii < N; ++ii)
1463  for(unsigned jj = 0; jj < ii; ++jj)
1464  condensePrep.addFunctionCall(
1465  copyHTH, ExportIndex( jj ), ExportIndex( ii ));
1466 
1467  // Copy H10 to H01
1468  if( performFullCondensing() == false) {
1469  for (unsigned ii = 0; ii < N; ++ii)
1470  condensePrep.addFunctionCall( copyHTH1, ExportIndex( ii ) );
1471  }
1472  }
1473  else
1474  {
1475  ExportIndex ii, jj;
1476 
1477  condensePrep.acquire( ii );
1478  condensePrep.acquire( jj );
1479 
1480  ExportForLoop eLoopI(ii, 0, N);
1481  ExportForLoop eLoopJ(jj, 0, ii);
1482 
1483  eLoopJ.addFunctionCall(copyHTH, jj, ii);
1484  eLoopI.addStatement( eLoopJ );
1485  condensePrep.addStatement( eLoopI );
1486 
1487  // Copy H10 to H01
1488  if( performFullCondensing() == false) {
1489  ExportForLoop eLoopK(ii, 0, N);
1490  eLoopK.addFunctionCall(copyHTH1, ii);
1491  condensePrep.addStatement( eLoopK );
1492  }
1493 
1494  condensePrep.release( ii );
1495  condensePrep.release( jj );
1496  }
1497  condensePrep.addLinebreak();
1498 
1500  //
1501  // Compute gradient components g0 and g1
1502  //
1504 
1505  LOG( LVL_DEBUG ) << "Setup condensing: create Dx0, Dy and DyN" << endl;
1506 
1507  unsigned offset = performFullCondensing() == true ? 0 : NX;
1508 
1509  if (initialStateFixed() == true)
1510  {
1511  condenseFdb.addStatement( Dx0 == x0 - x.getRow( 0 ).getTranspose() );
1512  }
1513  condenseFdb << (Dy -= y) << (DyN -= yN);
1514  condenseFdb.addLinebreak();
1515 
1516  int gradientUp;
1517  get( LIFTED_GRADIENT_UPDATE, gradientUp );
1518  bool gradientUpdate = (bool) gradientUp;
1519 
1520  int variableObjS;
1521  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
1522  int sensitivityProp;
1523  get( DYNAMIC_SENSITIVITY, sensitivityProp );
1524  bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate));
1525 
1526  // Compute RDy
1527  if( getNY() > 0 || getNYN() ) {
1528  for(unsigned run1 = 0; run1 < N; ++run1)
1529  {
1530  ExportArgument R2Call = R2.isGiven() == true ? R2 : R2.getAddress(run1 * NU, 0);
1531  ExportArgument SluCall =
1532  objSlu.isGiven() == true || (variableObjS == false && !adjoint) ? objSlu : objSlu.getAddress(run1 * NU, 0);
1533  condenseFdb.addFunctionCall(
1534  multRDy, R2Call, Dy.getAddress(run1 * NY, 0), SluCall, g.getAddress(offset + run1 * NU, 0)
1535  );
1536  }
1537  condenseFdb.addLinebreak();
1538 
1539  // Compute QDy
1540  for(unsigned run1 = 0; run1 < N; run1++ )
1541  {
1542  ExportArgument Q2Call = Q2.isGiven() == true ? Q2 : Q2.getAddress(run1 * NX);
1543  ExportArgument SlxCall =
1544  objSlx.isGiven() == true || (variableObjS == false && !adjoint) ? objSlx : objSlx.getAddress(run1 * NX, 0);
1545  condenseFdb.addFunctionCall(
1546  multQDy, Q2Call, Dy.getAddress(run1 * NY), SlxCall, QDy.getAddress(run1 * NX) );
1547  }
1548  condenseFdb.addLinebreak();
1549  ExportVariable SlxCall =
1550  objSlx.isGiven() == true || (variableObjS == false && !adjoint) ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX);
1551  condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == QN2 * DyN );
1552  condenseFdb.addStatement( QDy.getRows(N * NX, (N + 1) * NX) += SlxCall );
1553  condenseFdb.addLinebreak();
1554  }
1555 
1556 
1557  if (performFullCondensing() == false)
1558  condenseFdb.addStatement(g.getRows(0, NX) == QDy.getRows(0, NX));
1559 
1560  /*
1561  if partial condensing:
1562  g0 = q_0
1563 
1564  for k = 0: N - 1
1565  g1_k = r_k
1566 
1567  if partial condensing:
1568  sbar_0 = 0;
1569  else:
1570  sbar_0 = Dx_0;
1571 
1572  sbar(1: N) = d;
1573 
1574  for k = 0: N - 1
1575  sbar_{k + 1} += A_k sbar_k;
1576 
1577  w1 = Q_N^T * sbar_N + q_N;
1578  for k = N - 1: 1
1579  {
1580  g1_k += B_k^T * w1;
1581  g1_k += S_k^T * sbar_k;
1582  w2 = A_k^T * w1 + q_k;
1583  w1 = Q_k^T * sbar_k + w2;
1584  }
1585 
1586  g1_0 += B_0^T * w1;
1587  if partial condensing:
1588  g_0 += A_0^T * w1
1589  else:
1590  g1_0 += S^0^T * x0;
1591 
1592  */
1593 
1594  w1.setup("w1", NX, 1, REAL, ACADO_WORKSPACE);
1595  w2.setup("w2", NX, 1, REAL, ACADO_WORKSPACE);
1596  sbar.setup("sbar", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
1597 
1598  if( performFullCondensing() == true ) {
1599  condenseFdb.addStatement( sbar.getRows(0, NX) == Dx0 );
1600  }
1601  else {
1602  condensePrep.addStatement( sbar.getRows(0, NX) == zeros<double>(NX,1) ); // Dx0 is now a variable as well !!
1603  }
1604  condensePrep.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
1605 
1606  for (unsigned i = 0; i < N; ++i)
1607  condenseFdb.addFunctionCall(
1608  macASbar, evGx.getAddress(i * NX), sbar.getAddress(i * NX), sbar.getAddress((i + 1) * NX)
1609  );
1610  condenseFdb.addLinebreak();
1611 
1612  condenseFdb.addStatement(
1613  w1 == QDy.getRows(N * NX, (N + 1) * NX) + QN1 * sbar.getRows(N * NX, (N + 1) * NX)
1614  );
1615  for (unsigned i = N - 1; 0 < i; --i)
1616  {
1617  condenseFdb.addFunctionCall(
1618  macBTw1, evGu.getAddress(i * NX), w1, g.getAddress(offset + i * NU)
1619  );
1620 
1621  if ((S1.isGiven() == true && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
1622  {
1623  ExportArgument S1Call = S1.isGiven() == false ? S1.getAddress(i * NX) : S1;
1624  condenseFdb.addFunctionCall(macS1TSbar, S1Call, sbar.getAddress(i * NX), g.getAddress(offset + i * NU));
1625  }
1626 
1627  condenseFdb.addFunctionCall(
1628  // TODO Check indexing for QDy
1629  macATw1QDy, evGx.getAddress(i * NX), w1, QDy.getAddress(i * NX), w2
1630  );
1631 
1632  ExportArgument Q1Call = Q1.isGiven() == true ? Q1 : Q1.getAddress(i * NX);
1633  condenseFdb.addFunctionCall(
1634  macQSbarW2, Q1Call, sbar.getAddress(i * NX), w2, w1);
1635  }
1636  condenseFdb.addFunctionCall(
1637  macBTw1, evGu.getAddress( 0 ), w1, g.getAddress( offset + 0 )
1638  );
1639  if( performFullCondensing() == true ) {
1640  if ((S1.isGiven() == true && S1.getGivenMatrix().isZero() == false) || S1.isGiven() == false)
1641  {
1642  condenseFdb.addFunctionCall(macS1TSbar, S1, sbar.getAddress(0), g);
1643  }
1644  }
1645  else {
1646  condenseFdb.addStatement(g.getRows(0, NX) += evGx.getRows(0, NX).getTranspose() * w1);
1647  }
1648  condenseFdb.addLinebreak();
1649 
1650 
1652  //
1653  // Expansion routine
1654  //
1656 
1657  /*
1658 
1659  // Step expansion, assuming that u_k is already updated
1660 
1661  Ds_0 = Dx_0;
1662  Ds_{1: N} = d;
1663 
1664  for i = 0: N - 1
1665  {
1666  Ds_{k + 1} += A_k * Ds_k; // Reuse multABarD
1667  Ds_{k + 1} += B_k * Du_k;
1668  s_{k + 1} += Ds_{k + 1};
1669  }
1670 
1671  */
1672 
1673  LOG( LVL_DEBUG ) << "Setup condensing: create expand routine" << endl;
1674 
1675  expand.setup( "expand" );
1676 
1677  if (performFullCondensing() == true)
1678  {
1679  expand.addStatement( u.makeRowVector() += xVars.getTranspose() );
1680  }
1681  else
1682  {
1683  expand.addStatement( u.makeColVector() += xVars.getRows(NX, getNumQPvars()) );
1684  }
1685 
1686  if( performFullCondensing() == true ) {
1687  expand.addStatement( sbar.getRows(0, NX) == Dx0 );
1688  }
1689  else {
1690  expand.addStatement( sbar.getRows(0, NX) == xVars.getRows(0, NX) );
1691  }
1692  expand.addStatement( sbar.getRows(NX, (N + 1) * NX) == d );
1693 
1694  for (unsigned row = 0; row < N; ++row )
1695  expand.addFunctionCall(
1696  expansionStep, evGx.getAddress(row * NX), evGu.getAddress(row * NX),
1697  xVars.getAddress(offset + row * NU), sbar.getAddress(row * NX),
1698  sbar.getAddress((row + 1) * NX)
1699  );
1700 
1701  expand.addStatement( x.makeColVector() += sbar );
1702 
1703  // !! Calculation of multipliers: !!
1704  int hessianApproximation;
1705  get( HESSIAN_APPROXIMATION, hessianApproximation );
1706  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
1707  if( secondOrder || adjoint ) {
1708  // mu_N = lambda_N + q_N + Q_N^T * Ds_N --> wrong in Joel's paper !!
1709  // for i = N - 1: 1
1710  // mu_k = Q_k^T * Ds_k + A_k^T * mu_{k + 1} + S_k * Du_k + q_k
1711 
1712  for (uint j = 0; j < NX; j++ ) {
1713  uint item = N*NX+j;
1714  uint IdxF = std::find(xBoundsIdx.begin(), xBoundsIdx.end(), item) - xBoundsIdx.begin();
1715  if( IdxF != xBoundsIdx.size() ) { // INDEX FOUND
1716  expand.addStatement( mu.getSubMatrix(N-1,N,j,j+1) == -1.0*yVars.getRow(getNumQPvars()+IdxF) );
1717  }
1718  else { // INDEX NOT FOUND
1719  expand.addStatement( mu.getSubMatrix(N-1,N,j,j+1) == 0.0 );
1720  }
1721  if( getNumComplexConstraints() ) {
1722  if( dimPocH > 0 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
1723  // HERE: POINT CONSTRAINTS ON TERMINAL STATE??
1724  }
1725  }
1726  expand.addStatement( mu.getRow(N-1) += sbar.getRows(N*NX,(N+1)*NX).getTranspose()*QN1 );
1727  expand.addStatement( mu.getRow(N-1) += QDy.getRows(N*NX,(N+1)*NX).getTranspose() );
1728  for (int i = N - 1; i >= 1; i--) {
1729  for (uint j = 0; j < NX; j++ ) {
1730  uint item = i*NX+j;
1731  uint IdxF = std::find(xBoundsIdx.begin(), xBoundsIdx.end(), item) - xBoundsIdx.begin();
1732  if( IdxF != xBoundsIdx.size() ) { // INDEX FOUND
1733  expand.addStatement( mu.getSubMatrix(i-1,i,j,j+1) == -1.0*yVars.getRow(getNumQPvars()+IdxF) );
1734  }
1735  else { // INDEX NOT FOUND
1736  expand.addStatement( mu.getSubMatrix(i-1,i,j,j+1) == 0.0 );
1737  }
1738  if( getNumComplexConstraints() ) {
1739  if( dimPocH > 0 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
1740  if( pacEvHx.isGiven() == false ) {
1741  expand.addStatement( mu.getSubMatrix(i-1,i,j,j+1) -= yVars.getRows(getNumQPvars()+getNumStateBounds()+i*dimPacH,getNumQPvars()+getNumStateBounds()+i*dimPacH+dimPacH).getTranspose()*pacEvHx.getSubMatrix(i*dimPacH,i*dimPacH+dimPacH,j,j+1) );
1742  }
1743  else {
1744  expand.addStatement( mu.getSubMatrix(i-1,i,j,j+1) -= yVars.getRows(getNumQPvars()+getNumStateBounds()+i*dimPacH,getNumQPvars()+getNumStateBounds()+i*dimPacH+dimPacH).getTranspose()*pacEvHx.getCol(j) );
1745  }
1746  }
1747  }
1748  expand.addFunctionCall(
1749  expansionStep2, QDy.getAddress(i*NX), Q1.getAddress(i * NX), sbar.getAddress(i*NX),
1750  S1.getAddress(i * NX), xVars.getAddress(offset + i * NU), evGx.getAddress(i * NX),
1751  mu.getAddress(i-1), mu.getAddress(i) );
1752  }
1753  }
1754 
1755  return SUCCESSFUL_RETURN;
1756 }
1757 
1758 returnValue ExportGaussNewtonCN2::setupVariables( )
1759 {
1761  //
1762  // Make index vector for state constraints
1763  //
1765 
1766  bool boxConIsFinite = false;
1767  xBoundsIdx.clear();
1768 
1769  DVector lbBox, ubBox;
1770  for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
1771  {
1772  lbBox = xBounds.getLowerBounds( i );
1773  ubBox = xBounds.getUpperBounds( i );
1774 
1775  if (isFinite( lbBox ) || isFinite( ubBox ))
1776  boxConIsFinite = true;
1777 
1778  // This is maybe not necessary
1779  if (boxConIsFinite == false || i == 0)
1780  continue;
1781 
1782  for (unsigned j = 0; j < lbBox.getDim(); ++j)
1783  {
1784  if ( ( acadoIsFinite( ubBox( j ) ) == true ) || ( acadoIsFinite( lbBox( j ) ) == true ) )
1785  {
1786  xBoundsIdx.push_back(i * lbBox.getDim() + j);
1787  }
1788  }
1789  }
1790 
1791  if (initialStateFixed() == true)
1792  {
1793  x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
1794  x0.setDoc( (std::string)"Current state feedback vector." );
1795  Dx0.setup("Dx0", NX, 1, REAL, ACADO_WORKSPACE);
1796  }
1797 
1798  if (performFullCondensing() == false || getNumComplexConstraints() > 0)
1799  {
1800  C.setup("C", N*NX, NX, REAL, ACADO_WORKSPACE);
1801  }
1802  E.setup("E", N * (N + 1) / 2 * NX, NU, REAL, ACADO_WORKSPACE);
1803 
1804  QDy.setup ("QDy", (N + 1) * NX, 1, REAL, ACADO_WORKSPACE);
1805 
1806  H.setup("H", getNumQPvars(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1807  g.setup("g", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1808  A.setup("A", getNumStateBounds( ) + getNumComplexConstraints(), getNumQPvars(), REAL, ACADO_WORKSPACE);
1809 
1810  lb.setup("lb", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1811  ub.setup("ub", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1812 
1813  lbA.setup("lbA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
1814  ubA.setup("ubA", getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
1815 
1816  xVars.setup("x", getNumQPvars(), 1, REAL, ACADO_WORKSPACE);
1817  yVars.setup("y", getNumQPvars() + getNumStateBounds() + getNumComplexConstraints(), 1, REAL, ACADO_WORKSPACE);
1818 
1819  return SUCCESSFUL_RETURN;
1820 }
1821 
1822 returnValue ExportGaussNewtonCN2::setupMultiplicationRoutines( )
1823 {
1824  ExportIndex iCol( "iCol" );
1825  ExportIndex iRow( "iRow" );
1826 
1827  ExportVariable dp, dn, Gx1, Gx2, Gx3, Gx4, Gu1, Gu2, Gu3;
1828  ExportVariable R22, Dy1, RDy1, Q22, QDy1, E1, U1, U2, H101, w11, w12, w13;
1829  dp.setup("dOld", NX, 1, REAL, ACADO_LOCAL);
1830  dn.setup("dNew", NX, 1, REAL, ACADO_LOCAL);
1831  Gx1.setup("Gx1", NX, NX, REAL, ACADO_LOCAL);
1832  Gx2.setup("Gx2", NX, NX, REAL, ACADO_LOCAL);
1833  Gx3.setup("Gx3", NX, NX, REAL, ACADO_LOCAL);
1834  Gx4.setup("Gx4", NX, NX, REAL, ACADO_LOCAL);
1835  Gu1.setup("Gu1", NX, NU, REAL, ACADO_LOCAL);
1836  Gu2.setup("Gu2", NX, NU, REAL, ACADO_LOCAL);
1837  Gu3.setup("Gu3", NX, NU, REAL, ACADO_LOCAL);
1838  R22.setup("R2", NU, NY, REAL, ACADO_LOCAL);
1839  Dy1.setup("Dy1", NY, 1, REAL, ACADO_LOCAL);
1840  RDy1.setup("RDy1", NU, 1, REAL, ACADO_LOCAL);
1841  Q22.setup("Q2", NX, NY, REAL, ACADO_LOCAL);
1842  QDy1.setup("QDy1", NX, 1, REAL, ACADO_LOCAL);
1843  E1.setup("E1", NX, NU, REAL, ACADO_LOCAL);
1844  U1.setup("U1", NU, 1, REAL, ACADO_LOCAL);
1845  U2.setup("U2", NU, 1, REAL, ACADO_LOCAL);
1846  H101.setup("H101", NU, NX, REAL, ACADO_LOCAL);
1847 
1848  w11.setup("w11", NX, 1, REAL, ACADO_LOCAL);
1849  w12.setup("w12", NX, 1, REAL, ACADO_LOCAL);
1850  w13.setup("w13", NX, 1, REAL, ACADO_LOCAL);
1851 
1852  if ( Q2.isGiven() )
1853  Q22 = Q2;
1854  if ( R2.isGiven() )
1855  R22 = R2;
1856 
1857  // multGxd; // d_k += Gx_k * d_{k-1}
1858  multGxd.setup("multGxd", dp, Gx1, dn);
1859  multGxd.addStatement( dn += Gx1 * dp );
1860 
1861  if( performFullCondensing() == false || getNumComplexConstraints() > 0 ) {
1862  // moveGxT
1863  moveGxT.setup("moveGxT", Gx1, Gx2);
1864  moveGxT.addStatement( Gx2 == Gx1 );
1865  // multGxGx
1866  multGxGx.setup("multGxGx", Gx1, Gx2, Gx3);
1867  multGxGx.addStatement( Gx3 == Gx1 * Gx2 );
1868  }
1869 
1870  // multGxGu
1871  multGxGu.setup("multGxGu", Gx1, Gu1, Gu2);
1872  multGxGu.addStatement( Gu2 == Gx1 * Gu1 );
1873  // moveGuE
1874  moveGuE.setup("moveGuE", Gu1, Gu2);
1875  moveGuE.addStatement( Gu2 == Gu1 );
1876 
1877  unsigned offset = (performFullCondensing() == true) ? 0 : NX;
1878 
1879  // copyHTH
1880  copyHTH.setup("copyHTH", iRow, iCol);
1881  copyHTH.addStatement(
1882  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1883  H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).getTranspose()
1884  );
1885 
1886  if( performFullCondensing() == false ) {
1887  // copyHTH1
1888  copyHTH1.setup("copyHTH1", iCol);
1889  copyHTH1.addStatement(
1890  H.getSubMatrix(0, NX, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1891  H.getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, 0, NX).getTranspose()
1892  );
1893  }
1894  // multRDy
1895  ExportVariable SluCall = objSlu.isGiven() == true ? objSlu : ExportVariable("Slu", NU, 1, REAL, ACADO_LOCAL);
1896  multRDy.setup("multRDy", R22, Dy1, SluCall, RDy1);
1897  multRDy.addStatement( RDy1 == R22 * Dy1 );
1898  multRDy.addStatement( RDy1 += SluCall );
1899 
1900  // mult QDy1
1901  ExportVariable SlxCall = objSlx.isGiven() == true ? objSlx : ExportVariable("Slx", NX, 1, REAL, ACADO_LOCAL);
1902  multQDy.setup("multQDy", Q22, Dy1, SlxCall, QDy1);
1903  multQDy.addStatement( QDy1 == Q22 * Dy1 );
1904  multQDy.addStatement( QDy1 += SlxCall );
1905  // multEQDy;
1906  multEQDy.setup("multEQDy", E1, QDy1, U1);
1907  multEQDy.addStatement( U1 += (E1 ^ QDy1) );
1908  // multQETGx
1909  multQETGx.setup("multQETGx", E1, Gx1, H101);
1910  multQETGx.addStatement( H101 += (E1 ^ Gx1) );
1911 
1912  if (performsSingleShooting() == false)
1913  {
1914  // multEDu
1915  multEDu.setup("multEDu", E1, U1, dn);
1916  multEDu.addStatement( dn += E1 * U1 );
1917  }
1918 
1919  if (Q1.isGiven() == true)
1920  {
1921  // multQ1Gx
1922  multQ1Gx.setup("multQ1Gx", Gx1, Gx2);
1923  multQ1Gx.addStatement( Gx2 == Q1 * Gx1 );
1924 
1925  // multQ1Gu
1926  multQ1Gu.setup("multQ1Gu", Gu1, Gu2);
1927  multQ1Gu.addStatement( Gu2 == Q1 * Gu1 );
1928 
1929  // multQ1d
1930  multQ1d.setup("multQ1d", Q1, dp, dn);
1931  multQ1d.addStatement( dn == Q1 * dp );
1932  }
1933  else
1934  {
1935  // multQ1d
1936  multQ1d.setup("multQ1d", Gx1, dp, dn);
1937  multQ1d.addStatement( dn == Gx1 * dp );
1938  }
1939 
1940  if (QN1.isGiven() == true)
1941  {
1942  // multQN1Gx
1943  multQN1Gx.setup("multQN1Gx", QN1, Gx1, Gx2);
1944  multQN1Gx.addStatement( Gx2 == QN1 * Gx1 );
1945 
1946  // multQN1Gu
1947  multQN1Gu.setup("multQN1Gu", QN1, Gu1, Gu2);
1948  multQN1Gu.addStatement( Gu2 == QN1 * Gu1 );
1949 
1950  // multQN1d
1951  multQN1d.setup("multQN1d", QN1, dp, dn);
1952  multQN1d.addStatement( dn == QN1 * dp );
1953  }
1954 
1955  //
1956  // N2 condensing related
1957  //
1958 
1959  multBTW1.setup("multBTW1", Gu1, Gu2, iRow, iCol);
1960  multBTW1.addStatement(
1961  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1962  (Gu1 ^ Gu2)
1963  );
1964 
1965  multGxTGu.setup("multGxTGu", Gx1, Gu1, Gu2);
1966  multGxTGu.addStatement( Gu2 == (Gx1 ^ Gu1) );
1967 
1968  ExportVariable Q11 = Q1.isGiven() ? Q1 : ExportVariable("Q11", NX, NX, REAL, ACADO_LOCAL);
1969 
1970  macQEW2.setup("multQEW2", Q11, Gu1, Gu2, Gu3);
1971  macQEW2.addStatement( Gu3 == Gu2 + Q11 * Gu1 );
1972 
1973  macASbar.setup("macASbar", Gx1, w11, w12);
1974  macASbar.addStatement( w12 += Gx1 * w11 );
1975 
1976  macBTw1.setup("macBTw1", Gu1, w11, U1);
1977  macBTw1.addStatement( U1 += Gu1 ^ w11 );
1978 
1979  macATw1QDy.setup("macATw1QDy", Gx1, w11, w12, w13);
1980  macATw1QDy.addStatement( w13 == w12 + (Gx1 ^ w11) );
1981 
1982  macQSbarW2.setup("macQSbarW2", Q11, w11, w12, w13);
1983  macQSbarW2.addStatement( w13 == w12 + Q11 * w11 );
1984 
1985  expansionStep.setup("expansionStep", Gx1, Gu1, U1, w11, w12);
1986  expansionStep.addStatement( w12 += Gx1 * w11 );
1987  expansionStep.addStatement( w12 += Gu1 * U1 );
1988 
1989  //
1990  // Define LM regularization terms
1991  //
1992  DMatrix mRegH11 = eye<double>( getNU() );
1993  mRegH11 *= levenbergMarquardt;
1994 
1995  ExportVariable R11 = R1.isGiven() == true ? R1 : ExportVariable("R11", NU, NU, REAL, ACADO_LOCAL);
1996 
1997  macBTW1_R1.setup("multBTW1_R1", R11, Gu1, Gu2, iRow);
1998  macBTW1_R1.addStatement(
1999  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU) ==
2000  R11 + (Gu1 ^ Gu2)
2001  );
2002  macBTW1_R1.addStatement(
2003  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU) += mRegH11
2004  );
2005 
2006  ExportVariable S11 = zeros<double>(NX, NU);
2007  if (S1.isGiven() == true && S1.getGivenMatrix().isZero() == false)
2008  S11 = S1;
2009  else if (S1.isGiven() == false)
2010  S11 = Gu1;
2011 
2012  if ((S11.isGiven() == true && S11.getGivenMatrix().isZero() == false) || S11.isGiven() == false)
2013  {
2014  mac_S1T_E.setup("mac_S1T_E", S11, Gu2, iRow, iCol);
2015  mac_S1T_E.addStatement(
2016  H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) +=
2017  (S11 ^ Gu2)
2018  );
2019 
2020  macS1TSbar.setup("macS1TSbar", S11, w11, U1);
2021  macS1TSbar.addStatement( U1 += (S11 ^ w11) );
2022  }
2023 
2024  int gradientUp;
2025  get( LIFTED_GRADIENT_UPDATE, gradientUp );
2026  bool gradientUpdate = (bool) gradientUp;
2027 
2028  int hessianApproximation;
2029  get( HESSIAN_APPROXIMATION, hessianApproximation );
2030  bool secondOrder = ((HessianApproximationMode)hessianApproximation == EXACT_HESSIAN);
2031  int sensitivityProp;
2032  get( DYNAMIC_SENSITIVITY, sensitivityProp );
2033  bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate));
2034  if( secondOrder || adjoint ) {
2035  ExportVariable mu1; mu1.setup("mu1", 1, NX, REAL, ACADO_LOCAL);
2036  ExportVariable mu2; mu2.setup("mu2", 1, NX, REAL, ACADO_LOCAL);
2037 
2038  expansionStep2.setup("expansionStep2", QDy1, Q11, w11, S11, U1, Gx1, mu1, mu2);
2039  expansionStep2.addStatement( mu1 += QDy1.getTranspose() );
2040  expansionStep2.addStatement( mu1 += w11 ^ Q11.getTranspose() );
2041  expansionStep2.addStatement( mu1 += U1 ^ S11.getTranspose() );
2042  expansionStep2.addStatement( mu1 += mu2*Gx1 );
2043  }
2044 
2045  if (performFullCondensing() == false)
2046  {
2047  // ExportFunction mult_BT_T1, mac_ST_C, multGxTGx, macGxTGx;
2048 
2049  mult_BT_T1.setup("mult_BT_T1", Gu1, Gx1, iRow);
2050  mult_BT_T1.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, 0, NX) == (Gu1 ^ Gx1) );
2051 
2052  if ((S11.isGiven() == true && S11.getGivenMatrix().isZero() == false) || S11.isGiven() == false)
2053  {
2054  mac_ST_C.setup("mac_ST_C", S11, Gx1, iRow);
2055  mac_ST_C.addStatement( H.getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, 0, NX) += (S11 ^ Gx1) );
2056  }
2057 
2058  multGxTGx.setup("multGxTGx", Gx1, Gx2, Gx3);
2059  multGxTGx.addStatement( Gx3 == (Gx1 ^ Gx2) );
2060 
2061  macGxTGx.setup("macGxTGx", Gx1, Q11, Gx3, Gx4);
2062  macGxTGx.addStatement( Gx4 == Gx1 + (Q11 * Gx3) );
2063  }
2064 
2065  return SUCCESSFUL_RETURN;
2066 }
2067 
2068 returnValue ExportGaussNewtonCN2::setupEvaluation( )
2069 {
2070  int hessianRegularization;
2071  get( HESSIAN_REGULARIZATION, hessianRegularization );
2073  //
2074  // Preparation phase
2075  //
2077 
2078  preparation.setup( "preparationStep" );
2079  preparation.doc( "Preparation step of the RTI scheme." );
2080  ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
2081  retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
2082  preparation.setReturnValue(retSim, false);
2083 
2084  preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
2085 
2086  preparation.addFunctionCall( evaluateObjective );
2087  if( regularizeHessian.isDefined() ) { // ALSO IN THE CASE OF CONDENSED REGULARIZATION, THIS IS CURRENTLY NECESSARY:
2088  preparation.addFunctionCall( regularizeHessian );
2089  }
2090  preparation.addFunctionCall( condensePrep );
2091  if( regularizeHessian.isDefined() && (HessianRegularizationMode)hessianRegularization == CONDENSED_REG ) {
2092  preparation.addFunctionCall( regularization, H );
2093  }
2094 
2096  //
2097  // Feedback phase
2098  //
2100 
2101  ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
2102  tmp.setDoc( "Status code of the qpOASES QP solver." );
2103 
2104  ExportFunction solve("solve");
2105  solve.setReturnValue( tmp );
2106 
2107  feedback.setup("feedbackStep");
2108  feedback.doc( "Feedback/estimation step of the RTI scheme." );
2109  feedback.setReturnValue( tmp );
2110 
2111  feedback.addFunctionCall( condenseFdb );
2112  feedback.addLinebreak();
2113 
2114  stringstream s;
2115  s << tmp.getName() << " = " << solve.getName() << "( );" << endl;
2116  feedback << s.str();
2117  feedback.addLinebreak();
2118 
2119  feedback.addFunctionCall( expand );
2120 
2122  //
2123  // Setup evaluation of the KKT tolerance
2124  //
2126 
2127  ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
2128  ExportVariable prd("prd", 1, 1, REAL, ACADO_LOCAL, true);
2129  ExportIndex index( "index" );
2130 
2131  getKKT.setup( "getKKT" );
2132  getKKT.doc( "Get the KKT tolerance of the current iterate." );
2133  kkt.setDoc( "The KKT tolerance value." );
2134  getKKT.setReturnValue( kkt );
2135  getKKT.addVariable( prd );
2136  getKKT.addIndex( index );
2137 
2138  // ACC = |\nabla F^T * xVars|
2139  getKKT.addStatement( kkt == (g ^ xVars) );
2140  getKKT << kkt.getFullName() << " = fabs( " << kkt.getFullName() << " );\n";
2141 
2142  ExportForLoop bLoop(index, 0, getNumQPvars());
2143 
2144  bLoop.addStatement( prd == yVars.getRow( index ) );
2145  bLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
2146  bLoop << kkt.getFullName() << " += fabs(" << lb.get(index, 0) << " * " << prd.getFullName() << ");\n";
2147  bLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
2148  bLoop << kkt.getFullName() << " += fabs(" << ub.get(index, 0) << " * " << prd.getFullName() << ");\n";
2149  getKKT.addStatement( bLoop );
2150 
2151  if ((getNumStateBounds() + getNumComplexConstraints())> 0)
2152  {
2153  ExportForLoop cLoop(index, 0, getNumStateBounds() + getNumComplexConstraints());
2154 
2155  cLoop.addStatement( prd == yVars.getRow( getNumQPvars() + index ));
2156  cLoop << "if (" << prd.getFullName() << " > " << toString(1.0 / INFTY) << ")\n";
2157  cLoop << kkt.getFullName() << " += fabs(" << lbA.get(index, 0) << " * " << prd.getFullName() << ");\n";
2158  cLoop << "else if (" << prd.getFullName() << " < " << toString(-1.0 / INFTY) << ")\n";
2159  cLoop << kkt.getFullName() << " += fabs(" << ubA.get(index, 0) << " * " << prd.getFullName() << ");\n";
2160 
2161  getKKT.addStatement( cLoop );
2162  }
2163 
2164  return SUCCESSFUL_RETURN;
2165 }
2166 
2167 returnValue ExportGaussNewtonCN2::setupQPInterface( )
2168 {
2169  string folderName;
2170  get(CG_EXPORT_FOLDER_NAME, folderName);
2171 
2172  string moduleName;
2173  get(CG_MODULE_NAME, moduleName);
2174 
2175  int qpSolver;
2176  get(QP_SOLVER, qpSolver);
2177 
2178  std::string sourceFile, headerFile, solverDefine;
2179  ExportQpOasesInterface* qpInterface = 0;
2180 
2181  switch ( (QPSolverName)qpSolver )
2182  {
2183  case QP_QPOASES:
2184  sourceFile = folderName + "/" + moduleName + "_qpoases_interface.cpp";
2185  headerFile = folderName + "/" + moduleName + "_qpoases_interface.hpp";
2186  solverDefine = "QPOASES_HEADER";
2187  qpInterface = new ExportQpOasesInterface(headerFile, sourceFile, "");
2188  break;
2189 
2190  case QP_QPOASES3:
2191  sourceFile = folderName + "/" + moduleName + "_qpoases3_interface.c";
2192  headerFile = folderName + "/" + moduleName + "_qpoases3_interface.h";
2193  solverDefine = "QPOASES3_HEADER";
2194  qpInterface = new ExportQpOases3Interface(headerFile, sourceFile, "");
2195  break;
2196 
2197  default:
2198  return ACADOERRORTEXT(RET_INVALID_ARGUMENTS,
2199  "For condensed solution only qpOASES and qpOASES3 QP solver are supported");
2200  }
2201 
2202  int useSinglePrecision;
2203  get(USE_SINGLE_PRECISION, useSinglePrecision);
2204 
2205  int hotstartQP;
2206  get(HOTSTART_QP, hotstartQP);
2207 
2208  int covCalc;
2209  get(CG_COMPUTE_COVARIANCE_MATRIX, covCalc);
2210 
2211  int maxNumQPiterations;
2212  get(MAX_NUM_QP_ITERATIONS, maxNumQPiterations);
2213 
2214  //
2215  // Set up export of the source file
2216  //
2217 
2218  if ( qpInterface == 0 )
2219  return RET_UNKNOWN_BUG;
2220 
2221  qpInterface->configure(
2222  "",
2223  solverDefine,
2224  getNumQPvars(),
2225  getNumStateBounds() + getNumComplexConstraints(),
2226  maxNumQPiterations,
2227  "PL_NONE",
2228  useSinglePrecision,
2229 
2230  commonHeaderName,
2231  "",
2232  xVars.getFullName(),
2233  yVars.getFullName(),
2234  "", // TODO
2235 // sigma.getFullName(),
2236  hotstartQP,
2237  true,
2238  H.getFullName(),
2239  string(),
2240  g.getFullName(),
2241  A.getFullName(),
2242  lb.getFullName(),
2243  ub.getFullName(),
2244  lbA.getFullName(),
2245  ubA.getFullName()
2246  );
2247 
2248  returnValue returnvalue = qpInterface->exportCode();
2249 
2250  if ( qpInterface != 0 )
2251  delete qpInterface;
2252 
2253  return returnvalue;
2254 }
2255 
2256 bool ExportGaussNewtonCN2::performFullCondensing() const
2257 {
2258  int sparseQPsolution;
2259  get(SPARSE_QP_SOLUTION, sparseQPsolution);
2260 
2261  if ((SparseQPsolutionMethods)sparseQPsolution == CONDENSING || (SparseQPsolutionMethods)sparseQPsolution == CONDENSING_N2 || (SparseQPsolutionMethods)sparseQPsolution == BLOCK_CONDENSING_N2)
2262  return false;
2263 
2264  return (initialStateFixed() == true);
2265 }
2266 
2267 CLOSE_NAMESPACE_ACADO
ExportGaussNewtonCN2::setupCondensing
virtual returnValue setupCondensing()
Definition: export_gauss_newton_cn2.cpp:1172
Grid::getNumPoints
uint getNumPoints() const
ExportGaussNewtonCN2::setupMultiplicationRoutines
virtual returnValue setupMultiplicationRoutines()
Definition: export_gauss_newton_cn2.cpp:1822
LOG
#define LOG(level)
Just define a handy macro for getting the logger.
Definition: acado_message_handling.hpp:193
R22
#define R22
Definition: hydroscal_model.hpp:101
LVL_DEBUG
Lowest level, the debug level.
Definition: acado_types.hpp:1122
ExportNLPSolver::objEvFx
ExportVariable objEvFx
Definition: export_nlp_solver.hpp:266
HessianApproximationMode
HessianApproximationMode
Definition: acado_types.hpp:611
ExportData::getFullName
std::string getFullName() const
Definition: export_data.cpp:121
E1
const double E1
Definition: examples/code_generation/mpc_mhe/cstr.cpp:37
ExportFunction::isDefined
virtual bool isDefined() const
Definition: export_function.cpp:321
ExportNLPSolver::conValueOut
ExportVariable conValueOut
Definition: export_nlp_solver.hpp:296
RET_UNKNOWN_BUG
Definition: acado_types.hpp:845
ExportNLPSolver::objS
ExportVariable objS
Definition: export_nlp_solver.hpp:265
ExportNLPSolver::objSlu
ExportVariable objSlu
Definition: export_nlp_solver.hpp:278
ExportVariable::getGivenMatrix
const DMatrix & getGivenMatrix() const
Definition: export_variable.cpp:460
ExportNLPSolver::shiftStates
ExportFunction shiftStates
Definition: export_nlp_solver.hpp:323
ExportNLPSolver::pacEvHx
ExportVariable pacEvHx
Definition: export_nlp_solver.hpp:301
HESSIAN_REGULARIZATION
Definition: acado_types.hpp:377
SparseQPsolutionMethods
SparseQPsolutionMethods
Definition: acado_types.hpp:755
QP_SOLVER
Definition: acado_types.hpp:386
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
CG_MODULE_NAME
Definition: acado_types.hpp:335
INFTY
const double INFTY
Definition: acado_constants.hpp:65
ExportGaussNewtonCN2::setupObjectiveEvaluation
virtual returnValue setupObjectiveEvaluation(void)
Definition: export_gauss_newton_cn2.cpp:277
ACADO_WORKSPACE
Definition: acado_types.hpp:805
ExportNLPSolver::pocEvH
ExportVariable pocEvH
Definition: export_nlp_solver.hpp:313
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
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
HESSIAN_APPROXIMATION
Definition: acado_types.hpp:376
CG_COMPUTE_COVARIANCE_MATRIX
Definition: acado_types.hpp:342
ExportArgument::isGiven
virtual bool isGiven() const
Definition: export_argument.cpp:136
MatrixVariablesGrid::getLowerBounds
DVector getLowerBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1344
ExportNLPSolver::performsSingleShooting
bool performsSingleShooting() const
Definition: export_nlp_solver.cpp:78
RET_INVALID_OPTION
Definition: acado_types.hpp:889
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
ExportNLPSolver::pacEvHu
ExportVariable pacEvHu
Definition: export_nlp_solver.hpp:301
ExportVariable::getDim
virtual uint getDim() const
Definition: export_variable.cpp:245
ExportVariable::getRow
ExportVariable getRow(const ExportIndex &idx) const
Definition: export_variable.cpp:395
ExportStatementBlock::addComment
returnValue addComment(const std::string &_comment)
Definition: export_statement_block.cpp:228
HessianRegularizationMode
HessianRegularizationMode
Definition: acado_types.hpp:626
ExportQpOasesInterface::configure
virtual returnValue configure(const std::string &_prefix, const std::string &_solverDefine, const int nvmax, const int ncmax, const int nwsrmax, const std::string &_printLevel, bool _useSinglePrecision, const std::string &_commonHeader, const std::string &_namespace, const std::string &_primalSolution, const std::string &_dualSolution, const std::string &_sigma, bool _hotstartQP, bool _externalCholesky, const std::string &_qpH, const std::string &_qpR, const std::string &_qpg, const std::string &_qpA, const std::string &_qplb, const std::string &_qpub, const std::string &_qplbA, const std::string &_qpubA)
Definition: export_qpoases_interface.cpp:53
MAX_NUM_QP_ITERATIONS
Definition: acado_types.hpp:387
ExportNLPSolver::objEvFxEnd
ExportVariable objEvFxEnd
Definition: export_nlp_solver.hpp:266
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
CONDENSING
Definition: acado_types.hpp:758
ExportGaussNewtonCN2::setupConstraintsEvaluation
virtual returnValue setupConstraintsEvaluation(void)
Definition: export_gauss_newton_cn2.cpp:443
ACADO_VARIABLES
Definition: acado_types.hpp:804
ExportForLoop
Allows to export code of a for-loop.
Definition: export_for_loop.hpp:56
std::toString
string toString(T const &value)
Definition: acado_io_utils.hpp:208
ExportSensitivityType
ExportSensitivityType
Definition: integrator_export_types.hpp:64
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::getUpperBounds
DVector getUpperBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1393
ExportNLPSolver::DyN
ExportVariable DyN
Definition: export_nlp_solver.hpp:260
ExportNLPSolver::state
ExportVariable state
Definition: export_nlp_solver.hpp:242
ExportNLPSolver::evGx
ExportVariable evGx
Definition: export_nlp_solver.hpp:249
ExportIndex
Defines a scalar-valued index variable to be used for exporting code.
Definition: export_index.hpp:54
ExportVariable::getSubMatrix
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
Definition: export_variable.cpp:425
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
INEXACT
Definition: integrator_export_types.hpp:72
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
ExportVariable::getRows
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:409
ExportGaussNewtonCN2::setup
virtual returnValue setup()
Definition: export_gauss_newton_cn2.cpp:45
ExportArgument
Defines a matrix-valued variable that can be passed as argument to exported functions.
Definition: export_argument.hpp:61
ExportNLPSolver::getDataDeclarations
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: export_nlp_solver.cpp:89
ExportNLPSolver::evaluateTerminalCost
ExportAcadoFunction evaluateTerminalCost
Definition: export_nlp_solver.hpp:271
ExportNLPSolver::initialStateFixed
bool initialStateFixed() const
Definition: export_nlp_solver.cpp:1589
DYNAMIC_SENSITIVITY
Definition: acado_types.hpp:380
ExportNLPSolver::regularization
ExportFunction regularization
Definition: export_nlp_solver.hpp:346
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
ExportGaussNewtonCN2::setupEvaluation
virtual returnValue setupEvaluation()
Definition: export_gauss_newton_cn2.cpp:2068
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
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
CG_HARDCODE_CONSTRAINT_VALUES
Definition: acado_types.hpp:343
INT
Definition: acado_types.hpp:795
ExportNLPSolver::getObjective
ExportFunction getObjective
Definition: export_nlp_solver.hpp:325
ExportGaussNewtonCN2::setupQPInterface
virtual returnValue setupQPInterface()
Definition: export_gauss_newton_cn2.cpp:2167
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
export_gauss_newton_cn2.hpp
EXACT_HESSIAN
Definition: acado_types.hpp:619
CONDENSED_REG
Definition: acado_types.hpp:629
ExportGaussNewtonCN2::getFunctionDeclarations
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: export_gauss_newton_cn2.cpp:143
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::isGiven
bool isGiven(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
Definition: export_variable.cpp:213
ExportNLPSolver::pacEvHxd
ExportVariable pacEvHxd
Definition: export_nlp_solver.hpp:301
ExportQpOasesInterface::exportCode
virtual returnValue exportCode()
Definition: export_qpoases_interface.cpp:169
ExportVariable::getTranspose
ExportVariable getTranspose() const
Definition: export_variable.cpp:390
ExportNLPSolver::QN2
ExportVariable QN2
Definition: export_nlp_solver.hpp:276
ExportNLPSolver::setupSimulation
virtual returnValue setupSimulation(void)
Definition: export_nlp_solver.cpp:193
ExportNLPSolver::QN1
ExportVariable QN1
Definition: export_nlp_solver.hpp:276
ExportNLPSolver::evaluatePointConstraints
std::vector< std::shared_ptr< ExportAcadoFunction > > evaluatePointConstraints
Definition: export_nlp_solver.hpp:308
UserInteraction
Encapsulates all user interaction for setting options, logging data and plotting results.
Definition: user_interaction.hpp:57
ExportFunction
Allows to export code of an arbitrary function.
Definition: export_function.hpp:58
ExportNLPSolver::shiftControls
ExportFunction shiftControls
Definition: export_nlp_solver.hpp:324
ExportStatementBlock::addStatement
returnValue addStatement(const ExportStatement &_statement)
Definition: export_statement_block.cpp:91
ExportData::getName
std::string getName() const
Definition: export_data.cpp:86
CG_USE_OPENMP
Definition: acado_types.hpp:339
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
GenericVector::append
GenericVector & append(const GenericVector &_arg)
Definition: vector.cpp:42
ExportFunction::setReturnValue
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
Definition: export_function.cpp:141
ExportFunction::acquire
virtual ExportFunction & acquire(ExportIndex &obj)
Definition: export_function.cpp:354
row
RowXpr row(Index i)
Definition: BlockMethods.h:725
ExportFunction::addVariable
ExportFunction & addVariable(const ExportVariable &_var)
Definition: export_function.cpp:368
DVector
GenericVector< double > DVector
Definition: vector.hpp:329
acadoIsFinite
BooleanType acadoIsFinite(double x, double TOL)
Definition: acado_utils.cpp:164
BLOCK_CONDENSING_N2
Definition: acado_types.hpp:762
ACADO_LOCAL
Definition: acado_types.hpp:808
CONDENSING_N2
Definition: acado_types.hpp:761
ExportNLPSolver::evGu
ExportVariable evGu
Definition: export_nlp_solver.hpp:250
ExportNLPSolver::objSEndTerm
ExportVariable objSEndTerm
Definition: export_nlp_solver.hpp:265
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
ExportArgument::getAddress
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
Definition: export_argument.cpp:103
ExportFunction::release
virtual ExportFunction & release(const ExportIndex &obj)
Definition: export_function.cpp:361
ExportGaussNewtonCN2::getNumStateBounds
virtual unsigned getNumStateBounds() const
Definition: export_gauss_newton_cn2.cpp:268
ExportNLPSolver::getNumComplexConstraints
unsigned getNumComplexConstraints(void)
Definition: export_nlp_solver.cpp:1579
HOTSTART_QP
Definition: acado_types.hpp:388
GenericVector::getDim
unsigned getDim() const
Definition: vector.hpp:172
ExportGaussNewtonCN2::ExportGaussNewtonCN2
ExportGaussNewtonCN2(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: export_gauss_newton_cn2.cpp:40
ExportFunction::getName
std::string getName() const
Definition: export_function.cpp:164
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
isFinite
BooleanType isFinite(const T &_value)
Definition: acado_utils.hpp:119
LIFTED_GRADIENT_UPDATE
Definition: acado_types.hpp:346
CG_EXPORT_FOLDER_NAME
Definition: acado_types.hpp:337
ExportVariable::get
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
Definition: export_variable.cpp:227
ExportStatementBlock::addFunction
returnValue addFunction(const ExportFunction &_function)
Definition: export_statement_block.cpp:108
export_qpoases_interface.hpp
ExportNLPSolver::pocEvHxd
ExportVariable pocEvHxd
Definition: export_nlp_solver.hpp:314
col
ColXpr col(Index i)
Definition: BlockMethods.h:708
ExportGaussNewtonCN2::setupVariables
virtual returnValue setupVariables()
Definition: export_gauss_newton_cn2.cpp:1758
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
ExportGaussNewtonCN2::xBoundsIdx
std::vector< unsigned > xBoundsIdx
Definition: export_gauss_newton_cn2.hpp:194
MatrixVariablesGrid::getLowerBound
double getLowerBound(uint pointIdx, uint valueIdx) const
Definition: matrix_variables_grid.cpp:1365
ExportGaussNewtonCN2::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: export_gauss_newton_cn2.cpp:162
ExportNLPSolver::pacEvH
ExportVariable pacEvH
Definition: export_nlp_solver.hpp:300
ExportNLPSolver::pocEvHu
ExportVariable pocEvHu
Definition: export_nlp_solver.hpp:314
ExportNLPSolver::pacEvDDH
ExportVariable pacEvDDH
Definition: export_nlp_solver.hpp:302
ExportAlgorithm::getNX
uint getNX() const
Definition: export_algorithm.cpp:99
ExportVariable::getCols
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:417
ExportVariable::makeRowVector
ExportVariable makeRowVector() const
Definition: export_variable.cpp:442
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
QP_QPOASES3
Definition: acado_types.hpp:636
ExportNLPSolver::objValueOut
ExportVariable objValueOut
Definition: export_nlp_solver.hpp:269
ExportGaussNewtonCN2::getDataDeclarations
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: export_gauss_newton_cn2.cpp:93
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
ExportNLPSolver::regularizeHessian
ExportFunction regularizeHessian
Definition: export_nlp_solver.hpp:345
MatrixVariablesGrid::getUpperBound
double getUpperBound(uint pointIdx, uint valueIdx) const
Definition: matrix_variables_grid.cpp:1414
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106
ExportVariable::makeColVector
ExportVariable makeColVector() const
Definition: export_variable.cpp:448
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
BACKWARD
Definition: integrator_export_types.hpp:68

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Feb 28 2022 21:31:53