export_gauss_newton_generic.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 
33 
35 
36 using namespace std;
37 
39  const std::string& _commonHeaderName
40  ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
41 {}
42 
44 {
46 
48 
50 
52 
54 
56 
58 
59  return SUCCESSFUL_RETURN;
60 }
61 
63  ExportStruct dataStruct
64  ) const
65 {
66  returnValue status;
67  status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
68  if (status != SUCCESSFUL_RETURN)
69  return status;
70 
71  int hardcodeConstraintValues;
72  get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
73 
74  declarations.addDeclaration(x0, dataStruct);
75 
76  if (Q1.isGiven() == true)
77  declarations.addDeclaration(qpQ, dataStruct);
78  if (QN1.isGiven() == true)
79  declarations.addDeclaration(qpQf, dataStruct);
80  if (S1.isGiven() == true)
81  declarations.addDeclaration(qpS, dataStruct);
82  if (R1.isGiven() == true)
83  declarations.addDeclaration(qpR, dataStruct);
84 
85  declarations.addDeclaration(qpq, dataStruct);
86  declarations.addDeclaration(qpqf, dataStruct);
87  declarations.addDeclaration(qpr, dataStruct);
88 
89  declarations.addDeclaration(qpx, dataStruct);
90  declarations.addDeclaration(qpu, dataStruct);
91 
92  declarations.addDeclaration(qpLb, dataStruct);
93  declarations.addDeclaration(qpUb, dataStruct);
94 
95  declarations.addDeclaration(qpLbA, dataStruct);
96  declarations.addDeclaration(qpUbA, dataStruct);
97 
98  declarations.addDeclaration(sigmaN, dataStruct);
99 
100  declarations.addDeclaration(qpLambda, dataStruct);
101  declarations.addDeclaration(qpMu, dataStruct);
102 
103  declarations.addDeclaration(nIt, dataStruct);
104 
105  if (hardcodeConstraintValues == NO) {
106  declarations.addDeclaration(evLbValues, dataStruct);
107  declarations.addDeclaration(evUbValues, dataStruct);
108 
109  declarations.addDeclaration(evLbAValues, dataStruct);
110  declarations.addDeclaration(evUbAValues, dataStruct);
111  }
112 
113  return SUCCESSFUL_RETURN;
114 }
115 
117  ) const
118 {
119  declarations.addDeclaration( preparation );
120  declarations.addDeclaration( feedback );
121 
122  declarations.addDeclaration( initialize );
123  declarations.addDeclaration( initializeNodes );
124  declarations.addDeclaration( shiftStates );
125  declarations.addDeclaration( shiftControls );
126  declarations.addDeclaration( getKKT );
127  declarations.addDeclaration( getObjective );
128 
129  declarations.addDeclaration( evaluateStageCost );
130  declarations.addDeclaration( evaluateTerminalCost );
131 
132  return SUCCESSFUL_RETURN;
133 }
134 
136  )
137 {
138  string moduleName;
139  get(CG_MODULE_NAME, moduleName);
140 
141  code.addLinebreak( 2 );
142  code.addStatement( "/******************************************************************************/\n" );
143  code.addStatement( "/* */\n" );
144  code.addStatement( "/* ACADO code generation */\n" );
145  code.addStatement( "/* */\n" );
146  code.addStatement( "/******************************************************************************/\n" );
147  code.addLinebreak( 2 );
148 
149  int useOMP;
150  get(CG_USE_OPENMP, useOMP);
151  if ( useOMP )
152  {
153  code.addDeclaration( state );
154  }
155 
157 
160  code.addFunction( setObjQ1Q2 );
161  code.addFunction( setObjR1R2 );
162  code.addFunction( setObjS1 );
163  code.addFunction( setObjQN1QN2 );
164  code.addFunction( setStagef );
166 
168 
169  for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
170  {
171  if (evaluatePointConstraints[ i ] == 0)
172  continue;
174  }
175 
176  code.addFunction( setStagePac );
178 
179  code.addFunction( acc );
180 
181  code.addFunction( preparation );
182  code.addFunction( feedback );
183 
184  code.addFunction( initialize );
186  code.addFunction( shiftStates );
187  code.addFunction( shiftControls );
188  code.addFunction( getKKT );
189  code.addFunction( getObjective );
190 
191  return SUCCESSFUL_RETURN;
192 }
193 
194 
196 {
197  if (initialStateFixed() == true)
198  return N * NX + N * NU;
199 
200  return (N + 1) * NX + N * NU;
201 }
202 
203 //
204 // PROTECTED FUNCTIONS:
205 //
206 
208 {
209  evaluateObjective.setup("evaluateObjective");
210 
211  int variableObjS;
212  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
213 
214  ExportVariable evLmX = zeros<double>(NX, NX);
215  ExportVariable evLmU = zeros<double>(NU, NU);
216 
217  if (levenbergMarquardt > 0.0)
218  {
219  DMatrix lmX = eye<double>( NX );
220  lmX *= levenbergMarquardt;
221 
222  DMatrix lmU = eye<double>( NU );
223  lmU *= levenbergMarquardt;
224 
225  evLmX = lmX;
226  evLmU = lmU;
227  }
228 
229  //
230  // Main loop that calculates Hessian and gradients
231  //
232 
233  ExportIndex runObj( "runObj" );
234  ExportForLoop loopObjective( runObj, 0, N );
235 
236  evaluateObjective.addIndex( runObj );
237 
238  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
239  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
240  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
241  loopObjective.addLinebreak( );
242 
243  // Evaluate the objective function
245 
246  // Stack the measurement function value
247  loopObjective.addStatement(
248  Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
249  );
250  loopObjective.addLinebreak( );
251 
252  // Optionally compute derivatives
253 
254  ExportVariable tmpObjS, tmpFx, tmpFu;
255  ExportVariable tmpFxEnd, tmpObjSEndTerm;
256  tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
257  if (objS.isGiven() == true)
258  tmpObjS = objS;
259  tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
260  if (objEvFx.isGiven() == true)
261  tmpFx = objEvFx;
262  tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
263  if (objEvFu.isGiven() == true)
264  tmpFu = objEvFu;
265  tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
266  if (objEvFxEnd.isGiven() == true)
267  tmpFxEnd = objEvFxEnd;
268  tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
269  if (objSEndTerm.isGiven() == true)
270  tmpObjSEndTerm = objSEndTerm;
271 
272  unsigned indexX = getNY();
273  ExportArgument tmpFxCall = tmpFx;
274  if (tmpFx.isGiven() == false)
275  {
276  tmpFxCall = objValueOut.getAddress(0, indexX);
277  indexX += objEvFx.getDim();
278  }
279 
280  ExportArgument tmpFuCall = tmpFu;
281  if (tmpFu.isGiven() == false)
282  {
283  tmpFuCall = objValueOut.getAddress(0, indexX);
284  }
285 
286  ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
287 
288  //
289  // Optional computation of Q1, Q2
290  //
291  if (Q1.isGiven() == false)
292  {
293  ExportVariable tmpQ1, tmpQ2;
294  tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
295  tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
296 
297  setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
298  setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
299  setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
300  setObjQ1Q2.addStatement( tmpQ1 += evLmX );
301 
302  loopObjective.addFunctionCall(
303  setObjQ1Q2,
304  tmpFxCall, objSCall,
305  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
306  );
307 
308  loopObjective.addLinebreak( );
309  }
310  else if (levenbergMarquardt > 0.0)
311  Q1 = Q1.getGivenMatrix() + evLmX.getGivenMatrix();
312 
313  if (R1.isGiven() == false)
314  {
315  ExportVariable tmpR1, tmpR2;
316  tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
317  tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
318 
319  setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
320  setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
321  setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
322  setObjR1R2.addStatement( tmpR1 += evLmU );
323 
324  loopObjective.addFunctionCall(
325  setObjR1R2,
326  tmpFuCall, objSCall,
327  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
328  );
329 
330  loopObjective.addLinebreak( );
331  }
332  else if (levenbergMarquardt > 0.0)
333  R1 = R1.getGivenMatrix() + evLmU.getGivenMatrix();
334 
335  if (S1.isGiven() == false)
336  {
337  ExportVariable tmpS1;
338  ExportVariable tmpS2;
339 
340  tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
341  tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
342 
343  setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
344  setObjS1.addVariable( tmpS2 );
345  setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
346  setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
347 
348  loopObjective.addFunctionCall(
349  setObjS1,
350  tmpFxCall, tmpFuCall, objSCall,
351  S1.getAddress(runObj * NX, 0)
352  );
353  }
354 
355  evaluateObjective.addStatement( loopObjective );
356 
357  //
358  // Evaluate the quadratic Mayer term
359  //
362 
363  // Evaluate the objective function, last node.
366 
369 
370  if (QN1.isGiven() == false)
371  {
372  ExportVariable tmpQN1, tmpQN2;
373  tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
374  tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
375 
376  setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
377  setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
378  setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
379  setObjQN1QN2.addStatement( tmpQN1 += evLmX );
380 
381  indexX = getNYN();
382  ExportArgument tmpFxEndCall = tmpFxEnd.isGiven() == true ? tmpFxEnd : objValueOut.getAddress(0, indexX);
383 
385  setObjQN1QN2,
386  tmpFxEndCall, objSEndTerm,
387  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
388  );
389 
391  }
392  else if (levenbergMarquardt > 0.0)
393  QN1 = QN1.getGivenMatrix() + evLmX.getGivenMatrix();
394 
395  //
396  // Hessian setup
397  //
398 
399  ExportIndex index( "index" );
400 
401  //
402  // Gradient setup
403  //
404  ExportVariable qq, rr;
405  qq.setup("stageq", NX, 1, REAL, ACADO_LOCAL);
406  rr.setup("stager", NU, 1, REAL, ACADO_LOCAL);
407  setStagef.setup("setStagef", qq, rr, index);
408 
409  if (Q2.isGiven() == false)
411  qq == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) * Dy.getRows(index * NY, (index + 1) * NY)
412  );
413  else
414  {
415  setStagef << "(void)" << index.getFullName() << ";\n";
417  qq == Q2 * Dy.getRows(index * NY, (index + 1) * NY)
418  );
419  }
421 
422  if (R2.isGiven() == false)
424  rr == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) * Dy.getRows(index * NY, (index + 1) * NY)
425  );
426  else
427  {
429  rr == R2 * Dy.getRows(index * NY, (index + 1) * NY)
430  );
431  }
432 
433  //
434  // Setup necessary QP variables
435  //
436 
437  if (Q1.isGiven() == true)
438  {
439  qpQ.setup("qpQ", N * NX, NX, REAL, ACADO_WORKSPACE);
440  for (unsigned blk = 0; blk < N; ++blk)
441  initialize.addStatement( qpQ.getSubMatrix(blk * NX, (blk + 1) * NX, 0, NX) == Q1);
442  }
443  else
444  {
445  qpQ = Q1;
446  }
447 
448  if (R1.isGiven() == true)
449  {
450  qpR.setup("qpR", N * NU, NU, REAL, ACADO_WORKSPACE);
451  for (unsigned blk = 0; blk < N; ++blk)
452  initialize.addStatement( qpR.getSubMatrix(blk * NU, (blk + 1) * NU, 0, NU) == R1);
453  }
454  else
455  {
456  qpR = R1;
457  }
458 
459  if (S1.isGiven() == true)
460  {
461  qpS.setup("qpS", N * NX, NU, REAL, ACADO_WORKSPACE);
462  if (S1.getGivenMatrix().isZero() == true)
463  initialize.addStatement(qpS == zeros<double>(N * NX, NU));
464  else
465  for (unsigned blk = 0; blk < N; ++blk)
466  initialize.addStatement( qpS.getSubMatrix(blk * NX, (blk + 1) * NX, 0, NU) == S1);
467  }
468  else
469  {
470  qpS = S1;
471  }
472 
473  if (QN1.isGiven() == true)
474  {
475  qpQf.setup("qpQf", NX, NX, REAL, ACADO_WORKSPACE);
477  }
478  else
479  {
480  qpQf = QN1;
481  }
482 
483  return SUCCESSFUL_RETURN;
484 }
485 
487 {
489  //
490  // Setup evaluation of box constraints on states and controls
491  //
493 
494 
495 
496  int hardcodeConstraintValues;
497  get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
498 
499  evaluateConstraints.setup("evaluateConstraints");
500 
501  DVector lbTmp, ubTmp;
502 
503  DVector lbXInf( NX );
504  lbXInf.setAll( -INFTY );
505 
506  DVector ubXInf( NX );
507  ubXInf.setAll( INFTY );
508 
509  DVector lbUInf( NU );
510  lbUInf.setAll( -INFTY );
511 
512  DVector ubUInf( NU );
513  ubUInf.setAll( INFTY );
514 
515  DVector lbValues, ubValues;
516 
517  //
518  // Stack input bounds
519  //
520  for (unsigned node = 0; node < N; ++node)
521  {
522  lbTmp = uBounds.getLowerBounds( node );
523  if ( !lbTmp.getDim() )
524  lbValues.append( lbUInf );
525  else
526  lbValues.append( lbTmp );
527 
528  ubTmp = uBounds.getUpperBounds( node );
529  if ( !ubTmp.getDim() )
530  ubValues.append( ubUInf );
531  else
532  ubValues.append( ubTmp );
533  }
534 
535  //
536  // Stack state bounds
537  //
538  for (unsigned node = 1; node < N + 1; ++node)
539  {
540  lbTmp = xBounds.getLowerBounds( node );
541  if ( !lbTmp.getDim() )
542  lbValues.append( lbXInf );
543  else
544  lbValues.append( lbTmp );
545 
546  ubTmp = xBounds.getUpperBounds( node );
547  if ( !ubTmp.getDim() )
548  ubValues.append( ubXInf );
549  else
550  ubValues.append( ubTmp );
551  }
552 
553  qpLb.setup("qpLb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
554  qpUb.setup("qpUb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
555 
556  if( hardcodeConstraintValues == YES ) {
557  evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL);
558  evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL);
559 
562  }
563  else {
564  evLbValues.setup("lbValues", N * NU + N * NX, 1, REAL, ACADO_VARIABLES);
565  evLbValues.setDoc( "Lower bounds values." );
566  evUbValues.setup("ubValues", N * NU + N * NX, 1, REAL, ACADO_VARIABLES);
567  evUbValues.setDoc( "Upper bounds values." );
568 
569  initialize.addStatement( evLbValues == lbValues );
570  initialize.addStatement( evUbValues == ubValues );
571  }
572 
575 
576  evaluateConstraints.addStatement( qpLb.getRows(N * NU, N * NU + N * NX) == evLbValues.getRows(N * NU, N * NU + N * NX) - x.makeColVector().getRows(NX, NX * (N + 1)) );
577  evaluateConstraints.addStatement( qpUb.getRows(N * NU, N * NU + N * NX) == evUbValues.getRows(N * NU, N * NU + N * NX) - x.makeColVector().getRows(NX, NX * (N + 1)) );
578 
579 
580 
582  //
583  // Setup evaluation of path and point constraints
584  //
586 
587  qpDimHtot = N * dimPacH;
588  qpDimH = N * dimPacH;
589  qpDimHN = 0;
590 
591  qpConDim.resize(N + 1, 0);
592  for (unsigned i = 0; i < N; ++i)
593  qpConDim[ i ] += dimPacH;
594 
595  for (unsigned i = 0; i < N; ++i)
596  if (evaluatePointConstraints[ i ])
597  {
598  unsigned dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
599 
600  qpDimHtot += dim;
601  qpDimH += dim;
602 
603  qpConDim[ i ] += dim;
604  }
605 
606  if (evaluatePointConstraints[ N ])
607  {
608  unsigned dim = evaluatePointConstraints[ N ]->getFunctionDim() / (1 + NX);
609  qpDimHtot += dim;
610  qpDimHN += dim;
611 
612  qpConDim[ N ] += dim;
613  }
614 
615 
616  if (qpDimHtot) // this is a bit of a hack...
617  // dummy qpUbA and qpLbA are created if there are no polytopic constraints
618  {
619  qpLbA.setup("qpLbA", qpDimHtot, 1, REAL, ACADO_WORKSPACE);
620  qpUbA.setup("qpUbA", qpDimHtot, 1, REAL, ACADO_WORKSPACE);
621  qpMu.setup("qpMu", 2 * N * (NX + NU) + 2*qpDimHtot, 1, REAL, ACADO_WORKSPACE);
622  }
623  else
624  {
625  qpLbA.setup("qpLbA", 1, 1, REAL, ACADO_WORKSPACE);
626  qpUbA.setup("qpUbA", 1, 1, REAL, ACADO_WORKSPACE);
627  qpMu.setup("qpMu", 2 * N * (NX + NU), 1, REAL, ACADO_WORKSPACE);
628  }
629 
630  //
631  // Setup constraint values for the whole horizon.
632  //
633  DVector lbAValues;
634  DVector ubAValues;
635 
636  for (unsigned i = 0; i < N; ++i)
637  {
638  if ( dimPacH )
639  {
640  lbAValues.append( lbPathConValues.block(i * dimPacH, 0, dimPacH, 1) );
641  ubAValues.append( ubPathConValues.block(i * dimPacH, 0, dimPacH, 1) );
642  }
643  lbAValues.append( pocLbStack[ i ] );
644  ubAValues.append( pocUbStack[ i ] );
645  }
646  lbAValues.append( pocLbStack[ N ] );
647  ubAValues.append( pocUbStack[ N ] );
648 
649  if( hardcodeConstraintValues == YES || qpDimHtot == 0 ) {
650  evLbAValues.setup("lbAValues", lbAValues, STATIC_CONST_REAL);
651  evUbAValues.setup("ubAValues", ubAValues, STATIC_CONST_REAL);
652 
655  }
656  else {
657  evLbAValues.setup("lbAValues", qpDimHtot, 1, REAL, ACADO_VARIABLES);
658  evLbAValues.setDoc( "Lower affine bounds values." );
659  evUbAValues.setup("ubAValues", qpDimHtot, 1, REAL, ACADO_VARIABLES);
660  evUbAValues.setDoc( "Upper affine bounds values." );
661 
662  initialize.addStatement( evLbAValues == lbAValues );
663  initialize.addStatement( evUbAValues == ubAValues );
664  }
665 
666  //
667  // Evaluate path constraints
668  //
669 
670  if ( dimPacH )
671  {
672  ExportIndex runPac;
673  evaluateConstraints.acquire( runPac );
674  ExportForLoop loopPac(runPac, 0, N);
675 
676  loopPac.addStatement( conValueIn.getCols(0, NX) == x.getRow( runPac ) );
677  loopPac.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( runPac ) );
678  loopPac.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runPac ) );
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().getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX) ==
692  conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
693  );
694 
695  derOffset = derOffset + dimPacH * NX;
696  }
697  if (pacEvHu.isGiven() == false )
698  {
699  loopPac.addStatement(
700  pacEvHu.makeRowVector().getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU) ==
701  conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
702  );
703  }
704 
705  // Add loop to the function.
707  evaluateConstraints.release( runPac );
709  }
710 
711  //
712  // Evaluate point constraints
713  //
714 
715  for (unsigned i = 0, intRowOffset = 0, dim = 0; i < N + 1; ++i)
716  {
717  if (evaluatePointConstraints[ i ] == 0)
718  continue;
719 
721  string( "Evaluating constraint on node: #" ) + toString( i )
722  );
723 
725  if (i < N)
726  {
727  evaluateConstraints.addStatement( conValueIn.getCols(NX, NX + NU) == u.getRow( i ) );
728  evaluateConstraints.addStatement( conValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( i ) );
729  }
730  else
732 
734  evaluatePointConstraints[ i ]->getName(), conValueIn, conValueOut );
736 
737  if (i < N)
738  dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
739  else
740  dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX);
741 
742  // Fill pocEvH, pocEvHx, pocEvHu
744  pocEvH.getRows(intRowOffset, intRowOffset + dim) ==
745  conValueOut.getTranspose().getRows(0, dim));
747 
749  pocEvHx.makeRowVector().getCols(intRowOffset * NX, (intRowOffset + dim) * NX)
750  == conValueOut.getCols(dim, dim + dim * NX));
752 
753  if (i < N)
754  {
756  pocEvHu.makeRowVector().getCols(intRowOffset * NU, (intRowOffset + dim) * NU)
757  == conValueOut.getCols(dim + dim * NX, dim + dim * NX + dim * NU));
759  }
760 
761  intRowOffset += dim;
762  }
763 
764  //
765  // Copy data to QP solver structures
766  //
767 
768  ExportVariable tLbAValues, tUbAValues;
769  ExportIndex offsetPac("offset"), indPac( "ind" );
770 
771  tLbAValues.setup("lbAValues", dimPacH, 1, REAL, ACADO_LOCAL);
772  tUbAValues.setup("ubAValues", dimPacH, 1, REAL, ACADO_LOCAL);
773 
774  setStagePac.setup("setStagePac", offsetPac, indPac, tLbAValues, tUbAValues);
775 
777  << (qpLbA.getRows(offsetPac, offsetPac + dimPacH) == tLbAValues - pacEvH.getRows(indPac * dimPacH, indPac * dimPacH + dimPacH))
778  << (qpUbA.getRows(offsetPac, offsetPac + dimPacH) == tUbAValues - pacEvH.getRows(indPac * dimPacH, indPac * dimPacH + dimPacH));
779 
780  ExportVariable tPocA;
781  tPocA.setup("tPocA", conValueOut.getDim(), NX + NU, REAL);
782  if ( dimPocH )
784 
785  unsigned offsetEval = 0;
786  unsigned offsetPoc = 0;
787  for (unsigned i = 0; i < N; ++i)
788  {
789  if ( dimPacH )
790  {
792  setStagePac,
793  ExportIndex( offsetEval ), ExportIndex( i ),
794  evLbAValues.getAddress( offsetEval ), evUbAValues.getAddress( offsetEval )
795  );
796 
797  offsetEval += dimPacH;
798  }
799 
800  if ( evaluatePointConstraints[ i ] )
801  {
802  unsigned dim = evaluatePointConstraints[ i ]->getFunctionDim() / (1 + NX + NU);
803 
805 
807  << (tPocA.getSubMatrix(0, dim, 0, NX) == pocEvHx.getSubMatrix(offsetPoc, offsetPoc + dim, 0, NX))
808  << (tPocA.getSubMatrix(0, dim, NX, NX + NU) == pocEvHu.getSubMatrix(offsetPoc, offsetPoc + dim, 0, NU))
809  << (qpLbA.getRows(offsetEval, offsetEval + dim) ==
810  evLbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim))
811  << (qpUbA.getRows(offsetEval, offsetEval + dim) ==
812  evUbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim));
813 
814  offsetEval += dim;
815  offsetPoc += dim;
816  }
817  }
818 
819  if ( evaluatePointConstraints[ N ] )
820  {
821  unsigned dim = evaluatePointConstraints[ N ]->getFunctionDim() / (1 + NX);
822 
824  << (qpLbA.getRows(offsetEval, offsetEval + dim) ==
825  evLbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim))
826  << (qpUbA.getRows(offsetEval, offsetEval + dim) ==
827  evUbAValues.getRows(offsetEval, offsetEval + dim) - pocEvH.getRows(offsetPoc, offsetPoc + dim));
828  }
829 
830  return SUCCESSFUL_RETURN;
831 }
832 
833 
835 {
836  if (initialStateFixed() == true)
837  {
838  x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
839  x0.setDoc( "Current state feedback vector." );
840  }
841  else
842  {
843  xAC.setup("xAC", NX, 1, REAL, ACADO_VARIABLES);
844  DxAC.setup("DxAC", NX, 1, REAL, ACADO_WORKSPACE);
845  SAC.setup("SAC", NX, NX, REAL, ACADO_VARIABLES);
846  sigmaN.setup("sigmaN", NX, NX, REAL, ACADO_VARIABLES);
847  }
848 
849  return SUCCESSFUL_RETURN;
850 }
851 
853 {
854  return SUCCESSFUL_RETURN;
855 }
856 
858 {
860  //
861  // Setup preparation phase
862  //
864  preparation.setup("preparationStep");
865  preparation.doc( "Preparation step of the RTI scheme." );
866 
867  ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
868  retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
869  preparation.setReturnValue(retSim, false);
870 
871  preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
872 
875 
877  //
878  // Setup feedback phase
879  //
881  ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
882  ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
883  returnValueFeedbackPhase.setDoc( "Status code of the QP solver." );
884  feedback.setup("feedbackStep" );
885  feedback.doc( "Feedback/estimation step of the RTI scheme." );
886  feedback.setReturnValue( returnValueFeedbackPhase );
887 
888  qpx.setup("qpx", NX * (N + 1), 1, REAL, ACADO_WORKSPACE);
889  qpu.setup("qpu", NU * N, 1, REAL, ACADO_WORKSPACE);
890 
891  qpq.setup("qpq", NX * N, 1, REAL, ACADO_WORKSPACE);
892  qpqf.setup("qpqf", NX, 1, REAL, ACADO_WORKSPACE);
893  qpr.setup("qpr", NU * N, 1, REAL, ACADO_WORKSPACE);
894 
895  qpLambda.setup("qpLambda", N * NX, 1, REAL, ACADO_WORKSPACE);
896 
897  nIt.setup("nIt", 1, 1, INT, ACADO_WORKSPACE);
898 
899 
900  if (initialStateFixed() == false)
901  {
902  }
903  else
904  {
905  // State feedback
906  feedback.addStatement( qpx.getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose() );
907  }
908 
909  //
910  // Calculate objective residuals
911  //
912  feedback.addStatement( Dy -= y );
914  feedback.addStatement( DyN -= yN );
916 
917  for (unsigned i = 0; i < N; ++i)
920  feedback.addStatement( qpqf == QN2 * DyN );
922 
923  //
924  // Arrival cost in the MHE case
925  //
926  if (initialStateFixed() == false)
927  {
928  // It is assumed this is the shifted version from the previous time step!
930  }
931 
932  //
933  // Here we have to add the differences....
934  //
935 
936  string moduleName;
937  get(CG_MODULE_NAME, moduleName);
938 
939  // Call the solver
940  feedback << returnValueFeedbackPhase.getFullName() << " = " << moduleName << "_solve( );\n";
941 
942  // Accumulate the solution, i.e. perform full Newton step
945 
946  if (initialStateFixed() == false)
947  {
948  // This is the arrival cost for the next time step!
950  }
951 
953  //
954  // Setup evaluation of the KKT tolerance
955  //
957 
958  ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
959  ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);
960  ExportIndex index( "index" );
961 
962  getKKT.setup( "getKKT" );
963  getKKT.doc( "Get the KKT tolerance of the current iterate." );
964  kkt.setDoc( "The KKT tolerance value." );
965  getKKT.setReturnValue( kkt );
966  getKKT.addVariable( tmp );
967  getKKT.addIndex( index );
968 
969  getKKT.addStatement( kkt == 0.0 );
970 
971  // XXX This is still probably relevant for the NMPC case
972 
973  getKKT.addStatement( tmp == (qpq ^ qpx.getRows(0, N * NX)) );
974  getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
975  getKKT.addStatement( tmp == (qpqf ^ qpx.getRows(N * NX, (N + 1) * NX)) );
976  getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
977  getKKT.addStatement( tmp == (qpr ^ qpu) );
978  getKKT << kkt.getFullName() << " += fabs( " << tmp.getFullName() << " );\n";
979 
980  ExportForLoop lamLoop(index, 0, N * NX);
981  lamLoop << kkt.getFullName() << "+= fabs( " << d.get(index, 0) << " * " << qpLambda.get(index, 0) << ");\n";
982  getKKT.addStatement( lamLoop );
983 
984  if (initialStateFixed() == true)
985  {
986  // XXX This is because the MHE does not support inequality constraints at the moment
987 
988  ExportForLoop lbLoop(index, 0, N * NU + N * NX);
989  lbLoop << kkt.getFullName() << "+= fabs( " << qpLb.get(index, 0) << " * " << qpMu.get(index, 0) << ");\n";
990  ExportForLoop ubLoop(index, 0, N * NU + N * NX);
991  ubLoop << kkt.getFullName() << "+= fabs( " << qpUb.get(index, 0) << " * " << qpMu.get(index + N * NU + N * NX, 0) << ");\n";
992  ExportForLoop lgLoop(index, 0, qpDimHtot);
993  lgLoop << kkt.getFullName() << "+= fabs( " << qpLbA.get(index, 0) << " * " << qpMu.get(index + 2*N*(NU+NX), 0) << ");\n";
994  ExportForLoop ugLoop(index, 0, qpDimHtot);
995  ugLoop << kkt.getFullName() << "+= fabs( " << qpUbA.get(index, 0) << " * " << qpMu.get(index + 2*N*(NU+NX) + qpDimHtot, 0) << ");\n";
996 
997  getKKT.addStatement( lbLoop );
998  getKKT.addStatement( ubLoop );
999  getKKT.addStatement( lgLoop );
1000  getKKT.addStatement( ugLoop );
1001  }
1002 
1003  return SUCCESSFUL_RETURN;
1004 }
1005 
ExportVariable objEvFx
ExportVariable conValueOut
ExportVariable getRow(const ExportIndex &idx) const
ExportVariable objS
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportVariable DxAC
ExportFunction shiftStates
ExportVariable pacEvHx
virtual returnValue setupObjectiveEvaluation(void)
const double INFTY
ExportVariable getTranspose() const
ExportVariable pocEvH
bool initialStateFixed() const
VariablesGrid xBounds
ExportVariable pocEvHx
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())
bool isGiven(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
uint getNX() const
Allows to pass back messages to the calling function.
ExportVariable conValueIn
DVector getUpperBounds(uint pointIdx) const
ExportVariable pacEvHu
std::vector< DVector > pocLbStack
returnValue addComment(const std::string &_comment)
virtual returnValue getCode(ExportStatementBlock &code)
ExportVariable objEvFxEnd
const DMatrix & getGivenMatrix() const
Allows to export code of a for-loop.
ExportVariable xAC
string toString(T const &value)
VariablesGrid uBounds
#define CLOSE_NAMESPACE_ACADO
ExportVariable DyN
ExportVariable state
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
Defines a scalar-valued index variable to be used for exporting code.
Base class for export of NLP/OCP solvers.
ExportAcadoFunction evaluateStageCost
Defines a matrix-valued variable that can be passed as argument to exported functions.
ExportAcadoFunction evaluateTerminalCost
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)
#define YES
Definition: acado_types.hpp:51
virtual returnValue setDoc(const std::string &_doc)
virtual returnValue setupInitialization()
ExportStruct
ExportFunction modelSimulation
virtual ExportFunction & doc(const std::string &_doc)
uint getNY() const
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
ExportFunction getObjective
unsigned getDim() const
Definition: vector.hpp:172
ExportVariable makeRowVector() const
ExportVariable objValueIn
ExportFunction initializeNodes
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable QN2
virtual returnValue setupSimulation(void)
#define NO
Definition: acado_types.hpp:53
ExportVariable QN1
std::vector< std::shared_ptr< ExportAcadoFunction > > evaluatePointConstraints
Encapsulates all user interaction for setting options, logging data and plotting results.
virtual uint getDim() const
ExportFunction shiftControls
returnValue addStatement(const ExportStatement &_statement)
virtual bool isGiven() const
ExportVariable SAC
std::string getFullName() const
returnValue addLinebreak(uint num=1)
ExportAcadoFunction evaluatePathConstraints
uint getNYN() const
GenericVector & append(const GenericVector &_arg)
Definition: vector.cpp:42
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
virtual ExportFunction & acquire(ExportIndex &obj)
ExportFunction & addVariable(const ExportVariable &_var)
void setAll(const T &_value)
Definition: vector.hpp:160
ExportVariable objSEndTerm
std::string getName() const
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
returnValue setupAuxiliaryFunctions()
virtual ExportFunction & release(const ExportIndex &obj)
DVector getLowerBounds(uint pointIdx) const
#define BEGIN_NAMESPACE_ACADO
returnValue addFunction(const ExportFunction &_function)
std::vector< DVector > pocUbStack
virtual returnValue setupMultiplicationRoutines()
ExportVariable objEvFu
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportVariable pacEvH
ExportVariable pocEvHu
Allows to export code for a block of statements.
ExportFunction initialize
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
ExportVariable objValueOut
ExportGaussNewtonGeneric(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
ExportFunction & addIndex(const ExportIndex &_index)
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Defines a matrix-valued variable to be used for exporting code.
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)
ExportVariable makeColVector() const
virtual returnValue setupConstraintsEvaluation(void)


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