export_gauss_newton_forces.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 
34 
37 
38 #include <acado/code_generation/templates/templates.hpp>
39 
41 
42 using namespace std;
43 
45  const std::string& _commonHeaderName
46  ) : ExportNLPSolver( _userInteraction,_commonHeaderName )
47 {
48  qpObjPrefix = "acadoForces";
49  qpModuleName = "forces";
50  diagH = diagHN = false;
51 
52  numLB = 0;
53  numUB = 0;
54 }
55 
57 {
58  LOG( LVL_DEBUG ) << "Solver: setup initialization... " << endl;
60  // Add QP initialization call to the initialization
61  ExportFunction initializeForces( "initializeForces" );
62  initializeForces.setName( "initializeForces" );
63  initialize.addFunctionCall( initializeForces );
64 
65  LOG( LVL_DEBUG ) << "done!" << endl;
66 
68 
70 
72 
74 
76 
78 
79  return SUCCESSFUL_RETURN;
80 }
81 
83  ExportStruct dataStruct
84  ) const
85 {
86  returnValue status;
87  status = ExportNLPSolver::getDataDeclarations(declarations, dataStruct);
88  if (status != SUCCESSFUL_RETURN)
89  return status;
90 
91  declarations.addDeclaration(x0, dataStruct);
92 
93  declarations.addDeclaration(lbValues, dataStruct);
94  declarations.addDeclaration(ubValues, dataStruct);
95 
96  return SUCCESSFUL_RETURN;
97 }
98 
100  ) const
101 {
102  declarations.addDeclaration( preparation );
103  declarations.addDeclaration( feedback );
104 
105  declarations.addDeclaration( initialize );
106  declarations.addDeclaration( initializeNodes );
107  declarations.addDeclaration( shiftStates );
108  declarations.addDeclaration( shiftControls );
109  declarations.addDeclaration( getKKT );
110  declarations.addDeclaration( getObjective );
111 
112  declarations.addDeclaration( evaluateStageCost );
113  declarations.addDeclaration( evaluateTerminalCost );
114 
115  return SUCCESSFUL_RETURN;
116 }
117 
119  )
120 {
122  code.addStatement( *qpInterface );
123 
124  code.addLinebreak( 2 );
125  code.addStatement( "/******************************************************************************/\n" );
126  code.addStatement( "/* */\n" );
127  code.addStatement( "/* ACADO code generation */\n" );
128  code.addStatement( "/* */\n" );
129  code.addStatement( "/******************************************************************************/\n" );
130  code.addLinebreak( 2 );
131 
132  int useOMP;
133  get(CG_USE_OPENMP, useOMP);
134  if ( useOMP )
135  {
136  code.addDeclaration( state );
137  }
138 
140 
143  code.addFunction( setObjQ1Q2 );
144  code.addFunction( setObjR1R2 );
145  code.addFunction( setObjS1 );
146  code.addFunction( setObjQN1QN2 );
147  code.addFunction( setStageH );
148  code.addFunction( setStagef );
150 
151  code.addFunction( conSetGxGu );
152  code.addFunction( conSetd );
154 
155  code.addFunction( acc );
156 
157  code.addFunction( preparation );
158  code.addFunction( feedback );
159 
160  code.addFunction( initialize );
162  code.addFunction( shiftStates );
163  code.addFunction( shiftControls );
164  code.addFunction( getKKT );
165  code.addFunction( getObjective );
166 
167  return SUCCESSFUL_RETURN;
168 }
169 
170 
172 {
173  return (N + 1) * NX + N * NU;
174 }
175 
177 {
178  return numLB;
179 }
180 
182 {
183  return numUB;
184 }
185 
186 //
187 // PROTECTED FUNCTIONS:
188 //
189 
191 {
192  evaluateObjective.setup("evaluateObjective");
193 
194  int variableObjS;
195  get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
196  int forceDiagHessian;
197  get(CG_FORCE_DIAGONAL_HESSIAN, forceDiagHessian);
198 
199  diagH = false;
200  diagHN = false;
201  unsigned dimHRows = NX + NU;
202  unsigned dimHCols = NX + NU;
203  unsigned dimHNRows = NX;
204  unsigned dimHNCols = NX;
205  if (objS.isGiven() == true || forceDiagHessian == true)
206  if (objS.getGivenMatrix().isDiagonal())
207  {
208  diagH = true;
209  dimHCols = 1;
210  }
211 
212  if (objSEndTerm.isGiven() == true || forceDiagHessian == true)
213  if (objSEndTerm.getGivenMatrix().isDiagonal() == true)
214  {
215  diagHN = true;
216  dimHNCols = 1;
217  }
218 
219  objHessians.resize(N + 1);
220  for (unsigned i = 0; i < N; ++i)
221  {
222  objHessians[ i ].setup(string("H") + toString(i + 1), dimHRows, dimHCols, REAL, FORCES_PARAMS, false, qpObjPrefix);
223  }
224  objHessians[ N ].setup(string("H") + toString(N + 1), dimHNRows, dimHNCols, REAL, FORCES_PARAMS, false, qpObjPrefix);
225 
226  objGradients.resize(N + 1);
227  for (unsigned i = 0; i < N; ++i)
228  {
229  objGradients[ i ].setup(string("f") + toString(i + 1), NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
230  }
231  objGradients[ N ].setup(string("f") + toString(N + 1), NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
232 
233  //
234  // LM regularization preparation
235  //
236 
237  ExportVariable evLmX = zeros<double>(NX, NX);
238  ExportVariable evLmU = zeros<double>(NU, NU);
239 
240  if (levenbergMarquardt > 0.0)
241  {
242  DMatrix lmX = eye<double>( NX );
243  lmX *= levenbergMarquardt;
244 
245  DMatrix lmU = eye<double>( NU );
246  lmU *= levenbergMarquardt;
247 
248  evLmX = lmX;
249  evLmU = lmU;
250  }
251 
252  //
253  // Main loop that calculates Hessian and gradients
254  //
255 
256  ExportIndex runObj( "runObj" );
257  ExportForLoop loopObjective( runObj, 0, N );
258 
259  evaluateObjective.addIndex( runObj );
260 
261  loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
262  loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
263  loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
264  loopObjective.addLinebreak( );
265 
266  // Evaluate the objective function
268 
269  // Stack the measurement function value
270  loopObjective.addStatement(
271  Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
272  );
273  loopObjective.addLinebreak( );
274 
275  // Optionally compute derivatives
276  unsigned indexX = getNY();
277  // unsigned indexG = indexX;
278 
279  ExportVariable tmpObjS, tmpFx, tmpFu;
280  ExportVariable tmpFxEnd, tmpObjSEndTerm;
281  tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
282  if (objS.isGiven() == true)
283  tmpObjS = objS;
284  tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
285  if (objEvFx.isGiven() == true)
286  tmpFx = objEvFx;
287  tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
288  if (objEvFu.isGiven() == true)
289  tmpFu = objEvFu;
290  tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
291  if (objEvFxEnd.isGiven() == true)
292  tmpFxEnd = objEvFxEnd;
293  tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
294  if (objSEndTerm.isGiven() == true)
295  tmpObjSEndTerm = objSEndTerm;
296 
297  //
298  // Optional computation of Q1, Q2
299  //
300  if (Q1.isGiven() == false)
301  {
302  ExportVariable tmpQ1, tmpQ2;
303  tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
304  tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
305 
306  setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
307  setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
308  setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
309 
310  if (tmpFx.isGiven() == true)
311  {
312  if (variableObjS == YES)
313  {
314  loopObjective.addFunctionCall(
315  setObjQ1Q2,
316  tmpFx, objS.getAddress(runObj * NY, 0),
317  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
318  );
319  }
320  else
321  {
322  loopObjective.addFunctionCall(
323  setObjQ1Q2,
324  tmpFx, objS,
325  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
326  );
327  }
328  }
329  else
330  {
331  if (variableObjS == YES)
332  {
333  if (objEvFx.isGiven() == true)
334 
335  loopObjective.addFunctionCall(
336  setObjQ1Q2,
337  objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
338  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
339  );
340  }
341  else
342  {
343  loopObjective.addFunctionCall(
344  setObjQ1Q2,
345  objValueOut.getAddress(0, indexX), objS,
346  Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
347  );
348  }
349  indexX += objEvFx.getDim();
350  }
351 
352  loopObjective.addLinebreak( );
353  }
354 
355  if (R1.isGiven() == false)
356  {
357  ExportVariable tmpR1, tmpR2;
358  tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
359  tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
360 
361  setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
362  setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
363  setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
364 
365  if (tmpFu.isGiven() == true)
366  {
367  if (variableObjS == YES)
368  {
369  loopObjective.addFunctionCall(
370  setObjR1R2,
371  tmpFu, objS.getAddress(runObj * NY, 0),
372  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
373  );
374  }
375  else
376  {
377  loopObjective.addFunctionCall(
378  setObjR1R2,
379  tmpFu, objS,
380  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
381  );
382  }
383  }
384  else
385  {
386  if (variableObjS == YES)
387  {
388  loopObjective.addFunctionCall(
389  setObjR1R2,
390  objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
391  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
392  );
393  }
394  else
395  {
396  loopObjective.addFunctionCall(
397  setObjR1R2,
398  objValueOut.getAddress(0, indexX), objS,
399  R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
400  );
401  }
402  }
403 
404  loopObjective.addLinebreak( );
405  }
406 
407  indexX = getNY();
408  ExportArgument tmpFxCall = tmpFx;
409  if (tmpFx.isGiven() == false)
410  {
411  tmpFxCall = objValueOut.getAddress(0, indexX);
412  indexX += objEvFx.getDim();
413  }
414 
415  ExportArgument tmpFuCall = tmpFu;
416  if (tmpFu.isGiven() == false)
417  {
418  tmpFuCall = objValueOut.getAddress(0, indexX);
419  }
420 
421  ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
422  if (S1.isGiven() == false)
423  {
424  ExportVariable tmpS1;
425  ExportVariable tmpS2;
426 
427  tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
428  tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
429 
430  setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
431  setObjS1.addVariable( tmpS2 );
432  setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
433  setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
434 
435  loopObjective.addFunctionCall(
436  setObjS1,
437  tmpFxCall, tmpFuCall, objSCall,
438  S1.getAddress(runObj * NX, 0)
439  );
440  }
441 
442  evaluateObjective.addStatement( loopObjective );
443 
444  //
445  // Evaluate the quadratic Mayer term
446  //
449 
450  // Evaluate the objective function
453 
456 
457  if (QN1.isGiven() == false)
458  {
459  indexX = getNYN();
460 
461  ExportVariable tmpQN1, tmpQN2;
462  tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
463  tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
464 
465  setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
466  setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
467  setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
468 
469  if (tmpFxEnd.isGiven() == true)
471  setObjQN1QN2,
472  tmpFxEnd, objSEndTerm,
473  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
474  );
475  else
477  setObjQN1QN2,
478  objValueOut.getAddress(0, indexX), objSEndTerm,
479  QN1.getAddress(0, 0), QN2.getAddress(0, 0)
480  );
481 
483  }
484 
485  //
486  // Hessian setup
487  //
488 
489  ExportVariable stageH;
490  ExportIndex index( "index" );
491  stageH.setup("stageH", dimHRows, dimHCols, REAL, ACADO_LOCAL);
492  setStageH.setup("setStageH", stageH, index);
493 
494  if (Q1.isGiven() == false)
495  {
496  if (diagH == false)
498  stageH.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(index * NX, (index + 1) * NX, 0, NX) + evLmX
499  );
500  else
501  for (unsigned el = 0; el < NX; ++el)
503  stageH.getElement(el, 0) == Q1.getElement(index * NX + el, el)
504  );
505  }
506  else
507  {
508  setStageH << index.getFullName() << " = " << index.getFullName() << ";\n";
509  if (diagH == false)
510  {
512  stageH.getSubMatrix(0, NX, 0, NX) == Q1 + evLmX
513  );
514  }
515  else
516  {
518  stageH.getRows(0, NX) == Q1.getGivenMatrix().getDiag() + evLmX.getGivenMatrix().getDiag()
519  );
520  }
521  }
523 
524  if (R1.isGiven() == false)
525  {
526  if (diagH == false)
528  stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1.getSubMatrix(index * NU, (index + 1) * NU, 0, NU) + evLmU
529  );
530  else
531  for (unsigned el = 0; el < NU; ++el)
533  stageH.getElement(NX + el, 0) == R1.getElement(index * NU + el, el)
534  );
535  }
536  else
537  {
538  if (diagH == false)
539  {
541  stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1 + evLmU
542  );
543  }
544  else
545  {
547  stageH.getRows(NX, NX + NU) == R1.getGivenMatrix().getDiag() + evLmU.getGivenMatrix().getDiag()
548  );
549  }
550  }
552 
553  if (diagH == false) {
554  if (S1.isGiven() == false)
555  {
557  stageH.getSubMatrix(0, NX, NX, NX + NU) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU)
558  );
560  stageH.getSubMatrix(NX, NX + NU, 0, NX) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU).getTranspose()
561  );
562  }
563  else if(S1.getGivenMatrix().isZero() == false)
564  {
566  stageH.getSubMatrix(0, NX, NX, NX + NU) == S1
567  );
569  stageH.getSubMatrix(NX, NX + NU, 0, NX) == S1.getTranspose()
570  );
571  }
572  }
573 
574  if (Q1.isGiven() == true && R1.isGiven() == true && S1.isGiven() == true)
575  {
576  initialize <<
577  setStageH.getName() << "( " << objHessians[ 0 ].getFullName() << ", " << "0" << " );\n";
579  if (diagHN == false)
580  {
582  objHessians[ N ] == QN1 + evLmX
583  );
584  }
585  else
586  {
589  );
590  }
591  }
592  else
593  {
594  for (unsigned i = 0; i < N; ++i)
597  if (diagHN == false)
599  objHessians[ N ] == QN1 + evLmX
600  );
601  else
602  for (unsigned el = 0; el < NX; ++el)
604  objHessians[ N ].getElement(el, 0) == QN1.getElement(el, el) + evLmX.getElement(el, el)
605  );
606  }
607 
608  //
609  // Gradient setup
610  //
611 
612  ExportVariable stagef;
613  stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
614  setStagef.setup("setStagef", stagef, index);
615 
616  if (Q2.isGiven() == false)
618  stagef.getRows(0, NX) == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) *
619  Dy.getRows(index * NY, (index + 1) * NY)
620  );
621  else
622  {
623  setStagef << index.getFullName() << " = " << index.getFullName() << ";\n";
625  stagef.getRows(0, NX) == Q2 *
626  Dy.getRows(index * NY, (index + 1) * NY)
627  );
628  }
630 
631  if (R2.isGiven() == false)
633  stagef.getRows(NX, NX + NU) == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) *
634  Dy.getRows(index * NY, (index + 1) * NY)
635  );
636  else
637  {
639  stagef.getRows(NX, NX + NU) == R2 *
640  Dy.getRows(index * NY, (index + 1) * NY)
641  );
642  }
643 
644  return SUCCESSFUL_RETURN;
645 }
646 
648 {
650  //
651  // Setup evaluation of box constraints on states and controls
652  //
654 
655  conLB.clear();
656  conLB.resize(N + 1);
657 
658  conUB.clear();
659  conUB.resize(N + 1);
660 
661  conLBIndices.clear();
662  conLBIndices.resize(N + 1);
663 
664  conUBIndices.clear();
665  conUBIndices.resize(N + 1);
666 
667  conABDimensions.clear();
668  conABDimensions.resize(N + 1);
669 
670  conLBValues.clear();
671  conLBValues.resize(N + 1);
672 
673  conUBValues.clear();
674  conUBValues.resize(N + 1);
675 
676  DVector lbTmp, ubTmp;
677 
678  //
679  // Stack state constraints
680  //
681  for (unsigned i = 0; i < xBounds.getNumPoints(); ++i)
682  {
683  lbTmp = xBounds.getLowerBounds( i );
684  ubTmp = xBounds.getUpperBounds( i );
685 
686  if (isFinite( lbTmp ) == false && isFinite( ubTmp ) == false)
687  continue;
688 
689  for (unsigned j = 0; j < lbTmp.getDim(); ++j)
690  {
691  if (acadoIsFinite( lbTmp( j ) ) == true)
692  {
693  conLBIndices[ i ].push_back( j );
694  conLBValues[ i ].push_back( lbTmp( j ) );
695  numLB++;
696  }
697 
698  if (acadoIsFinite( ubTmp( j ) ) == true)
699  {
700  conUBIndices[ i ].push_back( j );
701  conUBValues[ i ].push_back( ubTmp( j ) );
702  numUB++;
703  }
704  }
705  }
706 
707  //
708  // Stack control constraints
709  //
710  for (unsigned i = 0; i < uBounds.getNumPoints() && i < N; ++i)
711  {
712  lbTmp = uBounds.getLowerBounds( i );
713  ubTmp = uBounds.getUpperBounds( i );
714 
715  if (isFinite( lbTmp ) == false && isFinite( ubTmp ) == false)
716  continue;
717 
718  for (unsigned j = 0; j < lbTmp.getDim(); ++j)
719  {
720  if (acadoIsFinite( lbTmp( j ) ) == true)
721  {
722  conLBIndices[ i ].push_back(NX + j);
723  conLBValues[ i ].push_back( lbTmp( j ) );
724  numLB++;
725  }
726 
727  if (acadoIsFinite( ubTmp( j ) ) == true)
728  {
729  conUBIndices[ i ].push_back(NX + j);
730  conUBValues[ i ].push_back( ubTmp( j ) );
731  numUB++;
732  }
733  }
734  }
735 
736  //
737  // Setup variables
738  //
739  for (unsigned i = 0; i < N + 1; ++i)
740  {
741  conLB[ i ].setup(string("lb") + toString(i + 1), conLBIndices[ i ].size(), 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
742  conUB[ i ].setup(string("ub") + toString(i + 1), conUBIndices[ i ].size(), 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
743  }
744 
745  int hardcodeConstraintValues;
746  get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
747  uint numBounds = numLB+numUB;
748  if (!hardcodeConstraintValues && numBounds > 0)
749  {
750  lbValues.setup("lbValues", numLB, 1, REAL, ACADO_VARIABLES);
751  lbValues.setDoc( "Lower bounds values." );
752  ubValues.setup("ubValues", numUB, 1, REAL, ACADO_VARIABLES);
753  ubValues.setDoc( "Upper bounds values." );
754  }
755 
756  evaluateConstraints.setup("evaluateConstraints");
757 
758  //
759  // Export evaluation of simple box constraints
760  //
761  uint indexB = 0;
762  for (unsigned i = 0; i < N + 1; ++i) {
763  for (unsigned j = 0; j < conLBIndices[ i ].size(); ++j)
764  {
765  if( hardcodeConstraintValues ) {
766  evaluateConstraints << conLB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << toString(conLBValues[ i ][ j ]) << " - ";
767  }
768  else {
769  evaluateConstraints << conLB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << lbValues.get( indexB,0 ) << " - ";
770  }
771  indexB++;
772 
773  if (conLBIndices[ i ][ j ] < NX)
774  evaluateConstraints << x.getFullName() << "[ " << toString(i * NX + conLBIndices[ i ][ j ]) << " ];\n";
775  else
776  evaluateConstraints << u.getFullName() << "[ " << toString(i * NU + conLBIndices[ i ][ j ] - NX) << " ];\n";
777  }
778  }
780 
781  indexB = 0;
782  for (unsigned i = 0; i < N + 1; ++i)
783  for (unsigned j = 0; j < conUBIndices[ i ].size(); ++j)
784  {
785  if( hardcodeConstraintValues ) {
786  evaluateConstraints << conUB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << toString(conUBValues[ i ][ j ]) << " - ";
787  }
788  else {
789  evaluateConstraints << conUB[ i ].getFullName() << "[ " << toString(j) << " ]" << " = " << ubValues.get( indexB,0 ) << " - ";
790  }
791  indexB++;
792 
793  if (conUBIndices[ i ][ j ] < NX)
794  evaluateConstraints << x.getFullName() << "[ " << toString(i * NX + conUBIndices[ i ][ j ]) << " ];\n";
795  else
796  evaluateConstraints << u.getFullName() << "[ " << toString(i * NU + conUBIndices[ i ][ j ] - NX) << " ];\n";
797  }
799 
801  //
802  // Evaluation of the equality constraints
803  // - system dynamics only
804  //
806 
807  conC.clear();
808  conC.resize( N );
809 
810  // XXX FORCES works with column major format
811  // if (initialStateFixed() == true)
812  // conC[ 0 ].setup("C1", NX + NU, 2 * NX, REAL, FORCES_PARAMS, false, qpObjPrefix);
813  // else
814  // conC[ 0 ].setup("C1", NX + NU, NX, REAL, FORCES_PARAMS, false, qpObjPrefix);
815 
816  // for (unsigned i = 1; i < N; ++i)
817  for (unsigned i = 0; i < N; ++i)
818  conC[ i ].setup(string("C") + toString(i + 1), NX + NU, NX, REAL, FORCES_PARAMS, false, qpObjPrefix);
819 
820  ExportIndex index( "index" );
821  conStageC.setup("conStageC", NX + NU, NX, REAL);
822  conSetGxGu.setup("conSetGxGu", conStageC, index);
823 
825  conStageC.getSubMatrix(0, NX, 0, NX) ==
826  evGx.getSubMatrix(index * NX, (index + 1) * NX, 0, NX).getTranspose()
827  );
830  conStageC.getSubMatrix(NX, NX + NU, 0, NX) ==
831  evGu.getSubMatrix(index * NX, (index + 1) * NX, 0, NU).getTranspose()
832  );
833 
834  // if (initialStateFixed() == true)
835  // {
836  // initialize.addStatement(
837  // conC[ 0 ].getSubMatrix(0, NX, 0, NX) == eye( NX )
838  // );
839  // evaluateConstraints.addLinebreak();
840  // evaluateConstraints.addStatement(
841  // conC[ 0 ].getSubMatrix(0, NX, NX, 2 * NX) == evGx.getSubMatrix(0, NX, 0, NX).getTranspose()
842  // );
843  // evaluateConstraints.addLinebreak();
844  // evaluateConstraints.addStatement(
845  // conC[ 0 ].getSubMatrix(NX, NX + NU, NX, 2 * NX) == evGu.getSubMatrix(0, NX, 0, NU).getTranspose()
846  // );
847  // evaluateConstraints.addLinebreak();
848  // }
849 
850  unsigned start = 0; //initialStateFixed() == true ? 1 : 0;
851  for (unsigned i = start; i < N; ++i)
854 
855  cond.clear();
856 
857  unsigned dNum = initialStateFixed() == true ? N + 1 : N;
858  cond.resize(dNum);
859 
860  // if (initialStateFixed() == true)
861  // cond[ 0 ].setup("d1", 2 * NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
862  // else
863  // cond[ 0 ].setup("d1", NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
864 
865  for (unsigned i = 0; i < dNum; ++i)
866  cond[ i ].setup(string("d") + toString(i + 1), NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
867 
868  ExportVariable staged, stagedNew;
869 
870  staged.setup("staged", NX, 1, REAL, ACADO_LOCAL);
871  stagedNew.setup("stagedNew", NX, 1, REAL, ACADO_LOCAL);
872  conSetd.setup("conSetd", stagedNew, index);
874  stagedNew == zeros<double>(NX, 1) - d.getRows(index * NX, (index + 1) * NX)
875  );
876 
877  // evaluateConstraints.addStatement(
878  // cond[ 0 ].getRows(NX, 2 * NX) == dummyZero - d.getRows(0, NX)
879  // );
880  // evaluateConstraints.addLinebreak();
881 
882  if( initialStateFixed() ) {
883  for (unsigned i = 1; i < dNum; ++i)
885  conSetd, cond[ i ], ExportIndex(i - 1)
886  );
887  }
888  else {
889  for (unsigned i = 0; i < dNum; ++i)
891  conSetd, cond[ i ], ExportIndex(i)
892  );
893  }
894 
895  return SUCCESSFUL_RETURN;
896 }
897 
899 {
900  if (initialStateFixed() == true)
901  {
902  x0.setup("x0", NX, 1, REAL, ACADO_VARIABLES);
903  x0.setDoc( "Current state feedback vector." );
904  }
905 
906  return SUCCESSFUL_RETURN;
907 }
908 
910 {
911  return SUCCESSFUL_RETURN;
912 }
913 
915 {
917  //
918  // Setup preparation phase
919  //
921  preparation.setup("preparationStep");
922  preparation.doc( "Preparation step of the RTI scheme." );
923 
924  ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
925  retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
926  preparation.setReturnValue(retSim, false);
927 
928  preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
929 
932 
934  //
935  // Setup feedback phase
936  //
938  ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
939  ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
940  returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
941  feedback.setup("feedbackStep" );
942  feedback.doc( "Feedback/estimation step of the RTI scheme." );
943  feedback.setReturnValue( returnValueFeedbackPhase );
944 
946  // cond[ 0 ].getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
947  cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose()
948  );
950 
951  // Calculate objective residuals
952  feedback << (Dy -= y) << (DyN -= yN);
954 
955  for (unsigned i = 0; i < N; ++i)
959 
960  //
961  // Configure output variables
962  //
963  std::vector< ExportVariable > vecQPVars;
964 
965  vecQPVars.clear();
966  vecQPVars.resize(N + 1);
967  for (unsigned i = 0; i < N; ++i)
968  vecQPVars[ i ].setup(string("out") + toString(i + 1), NX + NU, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
969  vecQPVars[ N ].setup(string("out") + toString(N + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
970 
971  //
972  // In case warm starting is enabled, give an initial guess, based on the old solution
973  //
974  int hotstartQP;
975  get(HOTSTART_QP, hotstartQP);
976 
977  if ( hotstartQP )
978  {
979  std::vector< ExportVariable > zInit;
980 
981  zInit.clear();
982  zInit.resize(N + 1);
983  for (unsigned i = 0; i < N; ++i)
984  {
985  string name = "z_init_";
986  name = name + (i < 10 ? "0" : "") + toString( i );
987  zInit[ i ].setup(name, NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
988  }
989  string name = "z_init_";
990  name = name + (N < 10 ? "0" : "") + toString( N );
991  zInit[ N ].setup(name, NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
992 
993  // TODO This should be further investigated.
994 
995  //
996  // 1) Just use the old solution
997  //
998  // for (unsigned blk = 0; blk < N + 1; blk++)
999  // feedback.addStatement(zInit[ blk ] == vecQPVars[ blk ] );
1000 
1001  //
1002  // 2) Initialization by shifting
1003  //
1004 
1005  // for (unsigned blk = 0; blk < N - 1; blk++)
1006  // feedback.addStatement( zInit[ blk ] == vecQPVars[blk + 1] );
1007  // for (unsigned el = 0; el < NX; el++)
1008  // feedback.addStatement( zInit[N - 1].getElement(el, 0) == vecQPVars[ N ].getElement(el, 0) );
1009  }
1010 
1011  //
1012  // Call a QP solver
1013  // NOTE: we need two prefixes:
1014  // 1. module prefix
1015  // 2. structure instance prefix
1016  //
1017  ExportFunction solveQP;
1018  solveQP.setup("solve");
1019  solveQP.setName( "solve" );
1020 
1021  feedback
1022  << returnValueFeedbackPhase.getFullName() << " = "
1023  << qpModuleName << "_" << solveQP.getName() << "( "
1024  << "&" << qpObjPrefix << "_" << "params" << ", "
1025  << "&" << qpObjPrefix << "_" << "output" << ", "
1026  << "&" << qpObjPrefix << "_" << "info" << " , NULL);\n";
1028 
1029  //
1030  // Here we have to add the differences....
1031  //
1032 
1033  ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
1034  ExportIndex index( "index" );
1035  acc.setup("accumulate", stageOut, index);
1036 
1037  acc.addStatement( x.getRow( index ) += stageOut.getCols(0, NX) );
1038  acc.addLinebreak();
1039  acc.addStatement( u.getRow( index ) += stageOut.getCols(NX, NX + NU) );
1040  acc.addLinebreak();
1041 
1042  for (unsigned i = 0; i < N; ++i)
1043  feedback.addFunctionCall(acc, vecQPVars[ i ], ExportIndex( i ));
1045 
1046  feedback.addStatement( x.getRow( N ) += vecQPVars[ N ].getTranspose() );
1048 
1050  //
1051  // TODO Setup evaluation of KKT
1052  //
1054  ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
1055 
1056  getKKT.setup( "getKKT" );
1057  getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
1058  // kkt.setDoc( "The KKT tolerance value." );
1059  kkt.setDoc( "1e-15." );
1060  getKKT.setReturnValue( kkt );
1061 
1062  getKKT.addStatement( kkt == 1e-15 );
1063 
1064  return SUCCESSFUL_RETURN;
1065 }
1066 
1068 {
1069  //
1070  // Configure and export QP interface
1071  //
1072 
1073  qpInterface = std::shared_ptr< ExportForcesInterface >(new ExportForcesInterface(FORCES_TEMPLATE, "", commonHeaderName));
1074 
1075  ExportVariable tmp1("tmp", 1, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
1076  ExportVariable tmp2("tmp", 1, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
1077  ExportVariable tmp3("tmp", 1, 1, REAL, FORCES_INFO, false, qpObjPrefix);
1078 
1079  string params = qpModuleName + "_params";
1080 
1081  string output = qpModuleName + "_output";
1082 
1083  string info = qpModuleName + "_info";
1084 
1085  string header = qpModuleName + ".h";
1086 
1087  qpInterface->configure(
1088  header,
1089 
1090  params,
1091  tmp1.getDataStructString(),
1092 
1093  output,
1094  tmp2.getDataStructString(),
1095 
1096  info,
1097  tmp3.getDataStructString()
1098  );
1099 
1100  //
1101  // Configure and export MATLAB QP generator
1102  //
1103 
1104  string folderName;
1105  get(CG_EXPORT_FOLDER_NAME, folderName);
1106  string outFile = folderName + "/acado_forces_generator.m";
1107 
1108  qpGenerator = std::shared_ptr< ExportForcesGenerator >(new ExportForcesGenerator(FORCES_GENERATOR, outFile, "", "real_t", "int", 16, "%"));
1109 
1110  int maxNumQPiterations;
1111  get( MAX_NUM_QP_ITERATIONS,maxNumQPiterations );
1112 
1113  int printLevel;
1114  get(PRINTLEVEL, printLevel);
1115 
1116  // if not specified, use default value
1117  if ( maxNumQPiterations <= 0 )
1118  maxNumQPiterations = 3 * getNumQPvars();
1119 
1120  int useOMP;
1121  get(CG_USE_OPENMP, useOMP);
1122 
1123  int hotstartQP;
1124  get(HOTSTART_QP, hotstartQP);
1125 
1126  qpGenerator->configure(
1127  NX,
1128  NU,
1129  N,
1130  conLBIndices,
1131  conUBIndices,
1133  (Q1.isGiven() == true && R1.isGiven() == true) ? 1 : 0,
1134  diagH,
1135  diagHN,
1137  qpModuleName,
1138  (PrintLevel)printLevel == HIGH ? 2 : 0,
1139  maxNumQPiterations,
1140  useOMP,
1141  true,
1142  hotstartQP
1143  );
1144 
1145  qpGenerator->exportCode();
1146 
1147  //
1148  // Export Python generator
1149  //
1150 
1151  outFile = folderName + "/acado_forces_generator.py";
1152 
1153  qpGenerator = std::shared_ptr< ExportForcesGenerator >(new ExportForcesGenerator(FORCES_GENERATOR_PYTHON, outFile, "", "real_t", "int", 16, "#"));
1154 
1155  qpGenerator->configure(
1156  NX,
1157  NU,
1158  N,
1159  conLBIndices,
1160  conUBIndices,
1162  (Q1.isGiven() == true && R1.isGiven() == true) ? 1 : 0, // TODO Remove this one
1163  diagH,
1164  diagHN,
1166  qpModuleName,
1167  (PrintLevel)printLevel == HIGH ? 2 : 0,
1168  maxNumQPiterations,
1169  useOMP,
1170  false,
1171  hotstartQP
1172  );
1173 
1174  qpGenerator->exportCode();
1175 
1176  return SUCCESSFUL_RETURN;
1177 }
1178 
#define LOG(level)
Just define a handy macro for getting the logger.
Lowest level, the debug level.
ExportVariable objEvFx
std::vector< ExportVariable > cond
ExportVariable getRow(const ExportIndex &idx) const
ExportVariable objS
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportFunction shiftStates
ExportFunction & setName(const std::string &_name)
ExportVariable getTranspose() const
bool initialStateFixed() const
virtual returnValue setupObjectiveEvaluation(void)
std::vector< std::vector< unsigned > > conLBIndices
VariablesGrid xBounds
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
std::vector< ExportVariable > conUB
uint getNX() const
Allows to pass back messages to the calling function.
Generator of the FORCES interface, the to, be called from MATLAB.
DVector getUpperBounds(uint pointIdx) const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportVariable objEvFxEnd
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
const DMatrix & getGivenMatrix() const
Allows to export code of a for-loop.
string toString(T const &value)
std::vector< unsigned > conABDimensions
VariablesGrid uBounds
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
#define CLOSE_NAMESPACE_ACADO
ExportVariable DyN
ExportVariable state
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
ExportVariable evGx
Defines a scalar-valued index variable to be used for exporting code.
std::vector< std::vector< double > > conLBValues
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()
std::vector< ExportVariable > objGradients
std::string commonHeaderName
ExportStruct
ExportFunction modelSimulation
virtual ExportFunction & doc(const std::string &_doc)
uint getNY() const
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
ExportFunction getObjective
printLevel
Definition: example1.py:55
ExportGaussNewtonForces(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
std::vector< ExportVariable > conLB
virtual returnValue getCode(ExportStatementBlock &code)
unsigned getDim() const
Definition: vector.hpp:172
ExportVariable objValueIn
ExportFunction initializeNodes
A class for configuration and export for interface to the FORCES QP solver.
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable QN2
virtual returnValue setupMultiplicationRoutines()
virtual returnValue setupSimulation(void)
ExportVariable QN1
Encapsulates all user interaction for setting options, logging data and plotting results.
Allows to export code of an arbitrary function.
virtual uint getDim() const
ExportFunction shiftControls
returnValue addStatement(const ExportStatement &_statement)
PrintLevel
std::string getFullName() const
returnValue addLinebreak(uint num=1)
uint getNYN() const
std::vector< ExportVariable > conC
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
std::shared_ptr< ExportForcesInterface > qpInterface
ExportFunction & addVariable(const ExportVariable &_var)
BooleanType acadoIsFinite(double x, double TOL)
uint getNumPoints() const
ExportVariable evGu
std::vector< std::vector< unsigned > > conUBIndices
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()
DVector getLowerBounds(uint pointIdx) const
std::vector< ExportVariable > objHessians
#define BEGIN_NAMESPACE_ACADO
BooleanType isFinite(const T &_value)
USING_NAMESPACE_ACADO void output(const char *name, const Expression &e)
returnValue addFunction(const ExportFunction &_function)
virtual returnValue setupConstraintsEvaluation(void)
ExportVariable objEvFu
GenericVector< T > getDiag() const
Definition: matrix.cpp:131
std::shared_ptr< ExportForcesGenerator > qpGenerator
Allows to export code for a block of statements.
ExportFunction initialize
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
ExportVariable objValueOut
ExportFunction & addIndex(const ExportIndex &_index)
std::vector< std::vector< double > > conUBValues
std::string getDataStructString() 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)


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