scp_method.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 
26 
35 #include <iomanip>
36 #include <iostream>
37 
38 
39 // #define SIM_DEBUG
40 
41 using namespace std;
42 
44 
45 
46 //
47 // PUBLIC MEMBER FUNCTIONS:
48 //
49 
51 {
52  setupLogging( );
53 
54  eval = 0;
55  scpStep = 0;
57 
58  bandedCPsolver = 0;
59 
61  isCP = BT_FALSE;
62 
66 }
67 
68 
70  const Objective *objective_ ,
71  const DynamicDiscretization *dynamic_discretization_,
72  const Constraint *constraint_,
73  BooleanType _isCP
74  ) : NLPsolver( _userInteraction )
75 {
76  eval = new SCPevaluation( _userInteraction,objective_,dynamic_discretization_,constraint_,_isCP );
77  scpStep = 0;
79 
80  bandedCPsolver = 0;
81 
83  isCP = _isCP;
84 
88 
89  setupLogging( );
90 }
91 
92 
94 {
95 
97  clock = rhs.clock;
99 
100  iter = rhs.iter;
101  oldIter = rhs.oldIter;
102 
103  if( rhs.eval != 0 ) eval = (rhs.eval)->clone( );
104  else eval = 0;
105 
106  if( rhs.scpStep != 0 ) scpStep = (rhs.scpStep)->clone( );
107  else scpStep = 0;
108 
110  else derivativeApproximation = 0;
111 
112  if( rhs.bandedCPsolver != 0 ) bandedCPsolver = (rhs.bandedCPsolver)->clone( );
113  else bandedCPsolver = 0;
114 
115  status = rhs.status;
116  isCP = rhs.isCP;
117 
121 }
122 
123 
125 {
126  if( eval != 0 )
127  delete eval;
128 
129  if( scpStep != 0 )
130  delete scpStep;
131 
132  if( derivativeApproximation != 0 )
134 
135  if( bandedCPsolver != 0 )
136  delete bandedCPsolver;
137 }
138 
139 
141 
142  if ( this != &rhs )
143  {
144  if( eval != 0 )
145  delete eval;
146 
147  if( scpStep != 0 )
148  delete scpStep;
149 
150  if( derivativeApproximation != 0 )
152 
153  if( bandedCPsolver != 0 )
154  delete bandedCPsolver;
155 
156 
157  NLPsolver::operator=( rhs );
158 
159 
161  clock = rhs.clock;
163 
164  iter = rhs.iter;
165  oldIter = rhs.oldIter;
166 
167  if( rhs.eval != 0 ) eval = (rhs.eval)->clone( );
168  else eval = 0;
169 
170  if( rhs.scpStep != 0 ) scpStep = (rhs.scpStep)->clone( );
171  else scpStep = 0;
172 
174  else derivativeApproximation = 0;
175 
176  if( rhs.bandedCPsolver != 0 ) bandedCPsolver = (rhs.bandedCPsolver)->clone( );
177  else bandedCPsolver = 0;
178 
179  status = rhs.status;
180  isCP = rhs.isCP;
181 
185  }
186 
187  return *this;
188 }
189 
190 
192 {
193  return new SCPmethod( *this );
194 }
195 
196 
197 
199  VariablesGrid* xa_init,
200  VariablesGrid* p_init ,
201  VariablesGrid* u_init ,
202  VariablesGrid* w_init )
203 {
204  int printC;
205  get( PRINT_COPYRIGHT, printC );
206 
207  // PRINT THE HEADER:
208  // -----------------
209  if( printC == BT_TRUE )
210  acadoPrintCopyrightNotice( "SCPmethod -- A Sequential Quadratic Programming Algorithm." );
211 
212  iter.init( x_init, xa_init, p_init, u_init, w_init );
213 
214 // iter.print(); // already here different!!
215 
216  if ( setup( ) != SUCCESSFUL_RETURN )
218 
219 
220  // COMPUTATION OF DERIVATIVES:
221  // ---------------------------
222  int printLevel;
223  get( PRINTLEVEL,printLevel );
224 
225  if ( (PrintLevel)printLevel >= HIGH )
226  cout << "--> Computing initial linearization of NLP system ...\n";
227 
228 // iter.print();
229 
231 
232 // iter.print();
233 
234  if ( (PrintLevel)printLevel >= HIGH )
235  cout << "<-- Computing initial linearization of NLP system done.\n";
236 
237  int useRealtimeIterations;
238  get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
239 
240  if ( (BooleanType)useRealtimeIterations == BT_TRUE )
241  {
244  }
245 
246 
247  // freeze condensing in case OCP is QP -- isCP is a bit misleading...
248  if ( ( isCP == BT_TRUE ) && ( eval->hasLSQobjective( ) == BT_TRUE ) )
249  {
250  // bandedCPsolver->freezeCondensing( );
251  // eval->freezeSensitivities( );
252  }
253 
254  status = BS_READY;
255 
256  return SUCCESSFUL_RETURN;
257 }
258 
259 
261  const DVector &p_
262  )
263 {
264  if ( ( status != BS_READY ) && ( status != BS_RUNNING ) )
266 
267  returnValue returnvalue = SUCCESSFUL_RETURN;
268  numberOfSteps = 0;
269 
270  int maxNumberOfSteps;
271  get( MAX_NUM_ITERATIONS, maxNumberOfSteps );
272 
273  while( numberOfSteps < maxNumberOfSteps )
274  {
275  returnvalue = step( x0_,p_ );
276  // also increases numberOfSteps by one
277 
278  if( returnvalue == CONVERGENCE_ACHIEVED )
279  break;
280 
281  if( returnvalue != CONVERGENCE_NOT_YET_ACHIEVED )
283  }
284 
285  replot( PLOT_AT_END );
286 
287  if( numberOfSteps == maxNumberOfSteps )
289 
290  return returnvalue;
291 }
292 
293 
294 
296  const DVector& p_
297  )
298 {
299  if ( numberOfSteps == 0 )
301 
302  if ( feedbackStep( x0_,p_ ) != SUCCESSFUL_RETURN )
303  return RET_NLP_STEP_FAILED;
304 
306  return CONVERGENCE_ACHIEVED;
307 
309 
310  if ( ( returnValue != CONVERGENCE_ACHIEVED ) && ( returnValue != CONVERGENCE_NOT_YET_ACHIEVED ) )
311  return RET_NLP_STEP_FAILED;
312 
313  return returnValue;
314 }
315 
316 
318  const DVector& p_
319  )
320 {
321 
322  #ifdef SIM_DEBUG
323  printf("START OF THE FEEDBACK STEP \n");
324 
325  x0_.print("x0");
326  #endif
327 
328 
329  returnValue returnvalue;
330 
331  if ( ( status != BS_READY ) && ( status != BS_RUNNING ) )
333 
336 
337  status = BS_RUNNING;
339 
340  if ( checkForRealTimeMode( x0_,p_ ) != SUCCESSFUL_RETURN )
342 
343  int hessianMode;
344  get( HESSIAN_APPROXIMATION,hessianMode );
345 
346  if ( ( isInRealTimeMode == BT_FALSE ) && ( (HessianApproximationMode)hessianMode == EXACT_HESSIAN ) )
347  {
348  returnvalue = initializeHessianProjection();
349  if( returnvalue != SUCCESSFUL_RETURN )
350  return ACADOERROR( returnvalue );
351  }
352 
353  //bandedCP.objectiveGradient.print();
354  //x0_.print("x0");
355 
356  if ( isInRealTimeMode == BT_TRUE )
357  {
358  if ( setupRealTimeParameters( x0_,p_ ) != SUCCESSFUL_RETURN )
360  }
361 
362  int printLevel;
363  get( PRINTLEVEL,printLevel );
364 
365  if ( (PrintLevel)printLevel >= HIGH )
366  cout << "--> Solving banded QP ...\n";
367 
368 // iter.print();
369 
372 
373 // bandedCP.deltaX.print();
374 
375  if ( (PrintLevel)printLevel >= HIGH )
376  cout << "<-- Solving banded QP done.\n";
377 
378  ++numberOfSteps;
379 
380  return SUCCESSFUL_RETURN;
381 }
382 
383 
385 {
386  returnValue returnvalue;
387 
388  if ( isInRealTimeMode == BT_TRUE )
389  {
392 
393 // bandedCP.deltaX.print();
394  }
395 
396 // cout <<"bandedCP.dynResiduum = \n");
397 // bandedCP.dynResiduum.print();
398 // cout <<"bandedCP.lambdaDynamic = \n");
399 // bandedCP.lambdaDynamic.print();
400 
401  oldIter = iter;
402 
403  // Perform a globalized step:
404  // --------------------------
405  int printLevel;
406  get( PRINTLEVEL,printLevel );
407 
408  if ( (PrintLevel)printLevel >= HIGH )
409  cout << "--> Perform globalized SQP step ...\n";
410 
411  clock.reset( );
412  clock.start( );
413 
414  #ifdef SIM_DEBUG
415 /* printf("performing the current step...: old iterate \n");
416  (iter.x->getVector(0)).print("iter.x(0)");
417  (iter.u->getVector(0)).print("iter.u(0)");
418  (iter.x->getVector(1)).print("iter.x(1)");
419  (iter.u->getVector(1)).print("iter.u(1)");*/
420  #endif
421 
422  returnvalue = scpStep->performStep( iter,bandedCP,eval );
423  if( returnvalue != SUCCESSFUL_RETURN )
425 
427 
428  clock.stop( );
430 
431  if ( (PrintLevel)printLevel >= HIGH )
432  cout << "<-- Perform globalized SQP step done.\n";
433 
434  printIteration( );
435 
436  // Check convergence criterion if no real-time iterations are performed
437  int terminateAtConvergence = 0;
438  get( TERMINATE_AT_CONVERGENCE,terminateAtConvergence );
439 
440  if ( (BooleanType)terminateAtConvergence == BT_TRUE )
441  {
443  {
445  return CONVERGENCE_ACHIEVED;
446  }
447  }
448 
449  if ( numberOfSteps >= 0 )
450  set( KKT_TOLERANCE_SAFEGUARD,0.0 );
451 
453 }
454 
455 
457 {
458  returnValue returnvalue;
459  RealClock clockLG;
460 
461  BlockMatrix oldLagrangeGradient;
462  BlockMatrix newLagrangeGradient;
463 
464 // cout <<"bandedCP.dynResiduum (possibly shifted) = \n");
465 // bandedCP.dynResiduum.print();
466 // cout <<"bandedCP.lambdaDynamic (possibly shifted) = \n");
467 // bandedCP.lambdaDynamic.print();
468 
469  // Coumpute the "old" Lagrange Gradient with the latest multipliers:
470  // -----------------------------------------------------------------
471  clockLG.reset( );
472  clockLG.start( );
473 
474  returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),oldIter,bandedCP, oldLagrangeGradient );
475  if( returnvalue != SUCCESSFUL_RETURN )
477 
478  clockLG.stop( );
479 
480 
481  // Linearize the NLP system at the new point:
482  // ------------------------------------------
483  int printLevel;
484  get( PRINTLEVEL,printLevel );
485 
486 
487  #ifdef SIM_DEBUG
488 /* printf("preparation step \n");
489  (iter.x->getVector(0)).print("iter.x(0)");
490  (iter.u->getVector(0)).print("iter.u(0)");
491  (iter.x->getVector(1)).print("iter.x(1)");
492  (iter.u->getVector(1)).print("iter.u(1)");*/
493  #endif
494 
495  if ( (PrintLevel)printLevel >= HIGH )
496  cout << "--> Computing new linearization of NLP system ...\n";
497 
498  clock.reset( );
499  clock.start( );
500 
501  if ( needToReevaluate == BT_TRUE )
502  {
503  // needs to re-evaluate due to eval->evaluate call within merit function evaluation!
508  }
509 
510  returnvalue = eval->evaluateSensitivities( iter,bandedCP );
511  if( returnvalue != SUCCESSFUL_RETURN )
513 
514  clock.stop( );
516 
517  if ( (PrintLevel)printLevel >= HIGH )
518  cout << "<-- Computing new linearization of NLP system done.\n";
519  //bandedCP.objectiveGradient.print();
520 
521 
522  // Coumpute the "new" Lagrange Gradient with the latest multipliers:
523  // -----------------------------------------------------------------
524  clockLG.start( );
525 
526  returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),iter,bandedCP, newLagrangeGradient );
527  if( returnvalue != SUCCESSFUL_RETURN )
529 
530  clockLG.stop( );
532 
533 
534  // Compute the next Hessian:
535  // -------------------------
536  if ( (PrintLevel)printLevel >= HIGH )
537  cout << "--> Computing or approximating Hessian matrix ...\n";
538 
539  clock.reset( );
540  clock.start( );
541 
542  returnvalue = computeHessianMatrix( oldLagrangeGradient,newLagrangeGradient );
543  if( returnvalue != SUCCESSFUL_RETURN )
545 
546  clock.stop( );
548 
549  if ( (PrintLevel)printLevel >= HIGH )
550  cout << "<-- Computing or approximating Hessian matrix done.\n";
551 
552  // CONDENSE THE KKT-SYSTEM:
553  // ------------------------
554  if ( isInRealTimeMode == BT_TRUE )
555  {
558  }
559 
561 
563 }
564 
565 
566 
568 {
570 // printf("new reference!\n");
571  return eval->setReference( ref );
572 }
573 
574 
575 // returnValue SCPmethod::enableNeedToReevaluate( )
576 // {
577 // needToReevaluate = BT_TRUE;
578 // return SUCCESSFUL_RETURN;
579 // }
580 
581 
583  DVector lastX,
584  DVector lastXA,
585  DVector lastP,
586  DVector lastU,
587  DVector lastW
588  )
589 {
590  #ifdef SIM_DEBUG
591  cout << "SCPmethod::shiftVariables\n" );
592  #endif
593 
594  if ( acadoIsNegative( timeShift ) == BT_TRUE )
596 
597 // DMatrix tmp;
598 // for( uint i=1; i<iter.getNumPoints()-1; ++i )
599 // {
600 // bandedCP.lambdaDynamic.getSubBlock( i,0, tmp );
601 // bandedCP.lambdaDynamic.setDense( i-1,0, tmp );
602 // }
603 
604 // printf("shifted!\n");
606  return iter.shift( timeShift, lastX, lastXA, lastP, lastU, lastW );
607 }
608 
609 
610 
612 {
613  if( eval->hasLSQobjective( ) == BT_FALSE )
615 
616  if( bandedCPsolver == 0 )
618 
619  return bandedCPsolver->getVarianceCovariance( var );
620 }
621 
622 
624 {
626 }
627 
628 
629 //
630 // PROTECTED MEMBER FUNCTIONS:
631 //
632 
633 
635 {
637 
638  tmp.addItem( LOG_TIME_SQP_ITERATION, "TIME FOR THE WHOLE SQP ITERATION [sec]" );
639  tmp.addItem( LOG_TIME_CONDENSING, "TIME FOR CONDENSING [sec]" );
640  tmp.addItem( LOG_TIME_QP, "TIME FOR SOLVING THE QP [sec]" );
641 // tmp.addItem( LOG_TIME_RELAXED_QP, "TIME FOR SOLVING RELAXED QP's [sec]" );
642 // tmp.addItem( LOG_TIME_EXPAND, "TIME FOR EXPANSION [sec]" );
643 // tmp.addItem( LOG_TIME_EVALUATION, "TIME FOR FUNCTION EVALUATIONS [sec]" );
644  tmp.addItem( LOG_TIME_GLOBALIZATION, "TIME FOR GLOBALIZATION [sec]" );
645  tmp.addItem( LOG_TIME_SENSITIVITIES, "TIME FOR SENSITIVITY GENERATION [sec]" );
646 // tmp.addItem( LOG_TIME_LAGRANGE_GRADIENT, "TIME FOR COMPUTING LAGRANGE GRADIENT [sec]" );
647 // tmp.addItem( LOG_TIME_HESSIAN_COMPUTATION, "TIME FOR HESSIAN EVALUATION [sec]" );
648 
649  timeLoggingIdx = addLogRecord( tmp );
650 
651  LogRecord iterationOutput(LOG_AT_EACH_ITERATION, PS_PLAIN);
652 
653  iterationOutput.addItem( LOG_NUM_SQP_ITERATIONS,"SQP iteration");
654  iterationOutput.addItem( LOG_KKT_TOLERANCE,"KKT tolerance");
655  iterationOutput.addItem( LOG_LINESEARCH_STEPLENGTH,"line search parameter");
656  iterationOutput.addItem( LOG_OBJECTIVE_VALUE,"objective value");
657  iterationOutput.addItem( LOG_MERIT_FUNCTION_VALUE,"merit function value");
658  iterationOutput.addItem( LOG_IS_QP_RELAXED,"QP relaxation");
659  iterationOutput.addItem( LOG_NUM_QP_ITERATIONS,"No. QP iterations");
660 
661  outputLoggingIdx = addLogRecord( iterationOutput );
662 
663  return SUCCESSFUL_RETURN;
664 }
665 
666 
668 {
669  // CONSISTENCY CHECKS:
670  // -------------------
671  if ( isCP == BT_TRUE )
672  {
673  int hessianApproximation;
674  get( HESSIAN_APPROXIMATION,hessianApproximation );
675 
676  // Gauss-Newton is exact for linear-quadratic systems
677  if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN )
679  }
680 
681 
682  // PREPARE THE SQP ALGORITHM:
683  // --------------------------
684 
685  if ( eval == 0 )
686  return ACADOERROR( RET_UNKNOWN_BUG );
687 
688  if ( eval->init( iter ) != SUCCESSFUL_RETURN )
690 
691 
692  // PREPARE THE DATA FOR THE SQP ALGORITHM:
693  // ---------------------------------------
694 
695  if ( bandedCPsolver != 0 )
696  delete bandedCPsolver;
697 
698  int sparseQPsolution;
699  get( SPARSE_QP_SOLUTION,sparseQPsolution );
700 
701  int printLevel;
702  get( PRINTLEVEL,printLevel );
703 
704  if ( (PrintLevel)printLevel >= HIGH )
705  cout << "--> Initializing banded QP solver ...\n";
706 
707  if ( (SparseQPsolutionMethods)sparseQPsolution == CONDENSING )
708  {
711 
714  }
715  else
716  {
718  }
719 
720  if ( (PrintLevel)printLevel >= HIGH )
721  cout << "<-- Initializing banded QP solver done.\n";
722 
723  // INITIALIZE GLOBALIZATION STRATEGY (SCPstep):
724  // --------------------------------------------
725 
726  if ( scpStep != 0 )
727  delete scpStep;
728 
729  int globalizationStrategy;
730  get( GLOBALIZATION_STRATEGY,globalizationStrategy );
731 
732  switch( (GlobalizationStrategy)globalizationStrategy )
733  {
734  case GS_FULLSTEP:
736  break;
737 
738  case GS_LINESEARCH:
740  break;
741 
742  default:
743  return ACADOERROR( RET_UNKNOWN_BUG );
744  }
745 
746 
747  // EVALUATION OF THE NLP FUNCTIONS:
748  // --------------------------------
749  if ( (PrintLevel)printLevel >= HIGH )
750  cout << "--> Initial integration of dynamic system ...\n";
751 
753 
754  if ( (PrintLevel)printLevel >= HIGH )
755  cout << "<-- Initial integration of dynamic system done.\n";
756 
757  // INITIALIZE HESSIAN MATRIX:
758  // --------------------------
759  int hessianMode;
760  get( HESSIAN_APPROXIMATION,hessianMode );
761 
762  if( ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON ) || ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON_WITH_BLOCK_BFGS ) )
763  {
764  if( eval->hasLSQobjective( ) == BT_FALSE ){
765 // ACADOWARNING( RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED );
766 // set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
767 // get( HESSIAN_APPROXIMATION, hessianMode );
768  }
769  }
770 
771  if ( derivativeApproximation != 0 )
773 
774  switch( (HessianApproximationMode)hessianMode )
775  {
776  case EXACT_HESSIAN:
778  break;
779 
780  case CONSTANT_HESSIAN:
782  break;
783 
784  case FULL_BFGS_UPDATE:
786  break;
787 
788  case BLOCK_BFGS_UPDATE:
790  break;
791 
792  case GAUSS_NEWTON:
794  break;
795 
798  break;
799 
800  default:
801  return ACADOERROR( RET_UNKNOWN_BUG );
802  }
803 
805 
806  if ( (PrintLevel)printLevel >= HIGH )
807  cout << "--> Initializing Hessian computations ...\n";
808 
810 
811  if ( (PrintLevel)printLevel >= HIGH )
812  cout << "<-- Initializing Hessian computations done.\n";
813 
814  // SWITCH BETWEEN SINGLE- AND MULTIPLE SHOOTING:
815  // ---------------------------------------------
816  int discretizationMode;
817  get( DISCRETIZATION_TYPE, discretizationMode );
818 
819  if( (StateDiscretizationType)discretizationMode != SINGLE_SHOOTING ){
820  if( iter.x != 0 ) iter.x ->disableAutoInit();
821  if( iter.xa != 0 ) iter.xa->disableAutoInit();
822  }
823 
824  return SUCCESSFUL_RETURN;
825 }
826 
827 
829 {
830  return iter.print( );
831 }
832 
833 
835 {
836  double KKTmultiplierRegularisation;
837  get( KKT_TOLERANCE_SAFEGUARD,KKTmultiplierRegularisation );
838 
840  setLast( LOG_KKT_TOLERANCE, eval->getKKTtolerance( iter,bandedCP,KKTmultiplierRegularisation ) );
842 
843  int printLevel;
844  get( PRINTLEVEL,printLevel );
845 
846  if ( (PrintLevel)printLevel >= MEDIUM )
847  {
848  if (numberOfSteps == 1 || (numberOfSteps % 10) == 0)
849  cout << "sqp it | "
850  << "qp its | "
851  << " kkt tol | "
852  << " obj val | "
853  << " merit val | "
854  << " ls param | "
855  << endl;
856 
857  DMatrix foo;
858 
859  IoFormatter iof( cout );
860 
862  cout << setw( 6 ) << right << (int) foo(0, 0) << " | ";
864  cout << setw( 6 ) << right << (int) foo(0, 0) << " | ";
866  cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
868  cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
870  cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
872  cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
873  cout << endl;
874 
875  // Restore cout flags
876  iof.reset();
877  }
878 
880 
881  return SUCCESSFUL_RETURN;
882 }
883 
884 
886 {
887  double tol;
888  get( KKT_TOLERANCE,tol );
889 
890  // NEEDS TO BE CHECKED CARFULLY !!!
891  double KKTmultiplierRegularisation;
892  get(KKT_TOLERANCE_SAFEGUARD, KKTmultiplierRegularisation);
893 
894  if( eval->getKKTtolerance( iter,bandedCP,KKTmultiplierRegularisation ) <= tol )
895  {
896  int discretizationMode;
897  get( DISCRETIZATION_TYPE, discretizationMode );
898 
899  if( (StateDiscretizationType)discretizationMode == SINGLE_SHOOTING )
900  {
904  }
905 
906  int printLevel;
907  get( PRINTLEVEL,printLevel );
908 
909  if ( (PrintLevel)printLevel >= MEDIUM )
910  {
911  cout << endl
912  << "Covergence achieved. Demanded KKT tolerance is "
913  << scientific << tol
914  << "." << endl << endl;
915  }
916  return CONVERGENCE_ACHIEVED;
917  }
918 
920 }
921 
922 
924  const BlockMatrix& newLagrangeGradient
925  )
926 {
927  returnValue returnvalue;
928 
929  if ( numberOfSteps == 1 )
930  {
931  returnvalue = derivativeApproximation->initScaling( bandedCP.hessian, bandedCP.deltaX, newLagrangeGradient-oldLagrangeGradient );
932  if( returnvalue != SUCCESSFUL_RETURN )
933  ACADOERROR( returnvalue );
934  }
935 
936  returnvalue = derivativeApproximation->apply( bandedCP.hessian, bandedCP.deltaX, newLagrangeGradient-oldLagrangeGradient );
937  if( returnvalue != SUCCESSFUL_RETURN )
938  ACADOERROR( returnvalue );
939 
940  return SUCCESSFUL_RETURN;
941 }
942 
943 
944 
946 {
947  // COMPUTE A HEURISTIC DAMPING FACTOR:
948  // -----------------------------------
949  double damping = 1.0;
950  int run1 = 0;
951  while( run1 < numberOfSteps ){
952  if( run1 == 5 ) break;
953  damping *= 0.01;
954  ++run1;
955  }
956 
958 }
959 
960 
961 
963  const DVector &p_
964  )
965 {
966  int useRealtimeIterations;
967  get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
968  isInRealTimeMode = (BooleanType)useRealtimeIterations;
969 
970  if ( ( isInRealTimeMode == BT_FALSE ) &&
971  ( ( x0_.isEmpty( ) == BT_FALSE ) || ( p_.isEmpty( ) == BT_FALSE ) ) )
973 
974  return SUCCESSFUL_RETURN;
975 }
976 
977 
979  const DVector &p_
980  )
981 {
982  DVector deltaX;
983  DVector deltaP;
984 
985  if( x0_.isEmpty( ) == BT_FALSE )
986  deltaX = x0_ - iter.x->getVector(0);
987 
988  if( p_ .isEmpty( ) == BT_FALSE )
989  deltaP = p_ - iter.p->getVector(0);
990 
991  return bandedCPsolver->setRealTimeParameters( deltaX, deltaP );
992 }
993 
994 
996 {
997  clockTotalTime.stop( );
999 
1000  int printProfile;
1001  get( PRINT_SCP_METHOD_PROFILE, printProfile );
1002 
1003  if( (BooleanType) printProfile == BT_TRUE )
1005 
1006  return SUCCESSFUL_RETURN;
1007 }
1008 
1009 
1010 
1012 
1013  if( iter.x == 0 )
1015 
1016  xd_ = iter.x[0];
1017  return SUCCESSFUL_RETURN;
1018 }
1019 
1020 
1022 
1023  if( iter.xa == 0 )
1025  xa_ = iter.xa[0];
1026  return SUCCESSFUL_RETURN;
1027 }
1028 
1029 
1031 
1032  if( iter.p == 0 )
1034 // p_.init( iter.p[0].getVector( 0 ),iter.p[0].getTimePoints( ) );
1035  p_ = iter.p[0];
1036  return SUCCESSFUL_RETURN;
1037 }
1038 
1039 
1041 
1042  if( iter.p == 0 )
1044 
1045  p_ = iter.p->getVector( 0 );
1046  return SUCCESSFUL_RETURN;
1047 }
1048 
1049 
1051 
1052  if( iter.u == 0 )
1054 
1055  u_ = iter.u[0];
1056  return SUCCESSFUL_RETURN;
1057 }
1058 
1059 
1061 {
1062  #ifdef SIM_DEBUG
1063  cout << "SCPmethod::getFirstControl\n";
1064  #endif
1065 
1066  if( iter.u == 0 )
1068 
1069  u0_ = iter.u->getVector( 0 );
1070 
1071  if ( hasPerformedStep == BT_FALSE )
1072  {
1073  DVector deltaU0( getNU() );
1074  bandedCPsolver->getFirstControl( deltaU0 );
1075 
1076  u0_ += deltaU0;
1077  }
1078 
1079  return SUCCESSFUL_RETURN;
1080 }
1081 
1082 
1084 
1085  if( iter.w == 0 )
1087 
1088  w_ = iter.w[0];
1089  return SUCCESSFUL_RETURN;
1090 }
1091 
1092 
1093 
1095 {
1096  ASSERT( eval != 0 );
1097  return eval->getObjectiveValue();
1098 }
1099 
1100 
1101 
1103  ) const
1104 {
1105  return getAnySensitivities( _sens,0 );
1106 }
1107 
1108 
1110  ) const
1111 {
1112  return getAnySensitivities( _sens,1 );
1113 }
1114 
1116  ) const
1117 {
1118  return getAnySensitivities( _sens,2 );
1119 }
1120 
1121 
1123  ) const
1124 {
1125  return getAnySensitivities( _sens,3 );
1126 }
1127 
1128 
1130  ) const
1131 {
1132  return getAnySensitivities( _sens,4 );
1133 }
1134 
1135 
1137  uint idx
1138  ) const
1139 {
1140  if ( idx > 4 )
1142 
1144  DMatrix tmp;
1145 
1146  _sens.init( N,1 );
1147 
1148  for( uint i=0; i<N; ++i )
1149  {
1150  bandedCP.dynGradient.getSubBlock( i,idx,tmp );
1151  _sens.setDense( i,0,tmp );
1152  }
1153 
1154  return SUCCESSFUL_RETURN;
1155 }
1156 
1157 
1159 
1160 // end of file.
RealClock clockTotalTime
Definition: scp_method.hpp:258
returnValue setLast(LogName _name, int lastValue, double time=-INFTY)
int numberOfSteps
Definition: nlp_solver.hpp:185
virtual returnValue apply(BlockMatrix &B, const BlockMatrix &x, const BlockMatrix &y)=0
StateDiscretizationType
BooleanType isCP
Definition: scp_method.hpp:271
virtual returnValue getDisturbances(VariablesGrid &w_) const
#define N
HessianApproximationMode
Implements a very rudimentary block sparse matrix class.
returnValue addItem(LogName _name, const char *const _label=DEFAULT_LABEL)
Definition: log_record.cpp:70
virtual returnValue solve(BandedCP &cp)=0
Implements linesearch techniques to perform a globalized step of an SCPmethod for solving NLPs...
DVector getConstraintBlockDims() const
VariablesGrid * x
returnValue setReference(const VariablesGrid &ref)
SCPstep * scpStep
Definition: scp_method.hpp:264
Allows real time measurements based on the system&#39;s clock.
Definition: real_clock.hpp:53
SparseQPsolutionMethods
uint getNU() const
returnValue clearDynamicDiscretization()
virtual returnValue getVarianceCovariance(DMatrix &var)
returnValue setup()
Definition: scp_method.cpp:667
returnValue set(OptionsName name, int value)
virtual returnValue setupLogging()
Definition: scp_method.cpp:634
VariablesGrid * u
virtual returnValue reset()
Definition: clock.cpp:86
returnValue checkForConvergence()
Definition: scp_method.cpp:885
Implements an exact Hessian computation for obtaining second-order derivatives within NLPsolvers...
BlockMatrix deltaX
Definition: banded_cp.hpp:123
UserInteraction * userInteraction
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
BooleanType acadoIsNegative(double x, double TOL)
BlockStatus status
Definition: scp_method.hpp:270
Stores and evaluates the constraints of optimal control problems.
Definition: constraint.hpp:60
Provides a time grid consisting of vector-valued optimization variables at each grid point...
virtual returnValue setRealTimeParameters(const DVector &DeltaX, const DVector &DeltaP=emptyConstVector)
Allows to pass back messages to the calling function.
virtual returnValue getParameters(VariablesGrid &p_) const
virtual ~SCPmethod()
Definition: scp_method.cpp:124
virtual returnValue getFirstControl(DVector &u0_) const
NLPsolver & operator=(const NLPsolver &rhs)
Definition: nlp_solver.cpp:70
virtual returnValue evaluate(OCPiterate &iter, BandedCP &cp)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
BlockMatrix lambdaConstraint
Definition: banded_cp.hpp:126
returnValue init(const VariablesGrid *const _x, const VariablesGrid *const _xa, const VariablesGrid *const _p, const VariablesGrid *const _u, const VariablesGrid *const _w)
BandedCP bandedCP
Definition: scp_method.hpp:267
virtual returnValue evaluateLagrangeGradient(uint N, const OCPiterate &iter, const BandedCP &cp, BlockMatrix &nablaL)
returnValue stopClockAndPrintRuntimeProfile()
Definition: scp_method.cpp:995
returnValue printIterate() const
Definition: scp_method.cpp:828
#define CLOSE_NAMESPACE_ACADO
uint getNumConstraintBlocks() const
returnValue printIteration()
Definition: scp_method.cpp:834
virtual returnValue getSensitivitiesXA(BlockMatrix &_sens) const
bool isEmpty() const
Definition: vector.hpp:176
BlockMatrix hessian
Definition: banded_cp.hpp:103
returnValue initializeHessianProjection()
Definition: scp_method.cpp:945
Base class for discretizing a DifferentialEquation for use in optimal control algorithms.
virtual returnValue initHessian(BlockMatrix &B, uint N, const OCPiterate &iter)=0
virtual double getObjectiveValue() const
virtual returnValue getSensitivitiesP(BlockMatrix &_sens) const
virtual returnValue getSensitivitiesX(BlockMatrix &_sens) const
BlockMatrix lambdaDynamic
Definition: banded_cp.hpp:125
virtual returnValue prepareNextStep()
Definition: scp_method.cpp:456
int timeLoggingIdx
Definition: scp_method.hpp:256
VariablesGrid * xa
#define ACADO_TRY(X)
virtual double getObjectiveValue() const
virtual returnValue performStep(OCPiterate &iter, BandedCP &cp, SCPevaluation *eval)=0
Implements a constant Hessian as approximation of second-order derivatives within NLPsolvers...
virtual double getKKTtolerance(const OCPiterate &iter, const BandedCP &cp, double KKTmultiplierRegularisation=0.0)
returnValue setupRealTimeParameters(const DVector &x0_=emptyConstVector, const DVector &p_=emptyConstVector)
Definition: scp_method.cpp:978
returnValue computeHessianMatrix(const BlockMatrix &oldLagrangeGradient, const BlockMatrix &newLagrangeGradient)
Definition: scp_method.cpp:923
virtual returnValue getVarianceCovariance(DMatrix &var)
Definition: scp_method.cpp:611
Implements a Gauss-Newton approximation as second-order derivatives within NLPsolvers.
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
GlobalizationStrategy
uint getNumPoints() const
virtual returnValue getSensitivitiesW(BlockMatrix &_sens) const
printLevel
Definition: example1.py:55
virtual returnValue finalizeSolve(BandedCP &cp)
Implements a fullstep to perform a step of an SCPmethod for solving NLPs.
virtual returnValue step(const DVector &x0_=emptyConstVector, const DVector &p_=emptyConstVector)
Definition: scp_method.cpp:295
double getHessianScaling() const
returnValue init(uint _nRows, uint _nCols)
BandedCPsolver * bandedCPsolver
Definition: scp_method.hpp:268
SCPmethod & operator=(const SCPmethod &rhs)
Definition: scp_method.cpp:140
returnValue checkForRealTimeMode(const DVector &x0_, const DVector &p_)
Definition: scp_method.cpp:962
virtual returnValue print(std::ostream &stream=std::cout, const std::string &name=DEFAULT_LABEL, const std::string &startString=DEFAULT_START_STRING, const std::string &endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const std::string &colSeparator=DEFAULT_COL_SEPARATOR, const std::string &rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: vector.cpp:97
returnValue printLogRecord(std::ostream &_stream, int idx, LogPrintMode _mode=PRINT_ITEM_BY_ITEM) const
BooleanType isInRealTimeMode
Definition: scp_method.hpp:274
virtual returnValue evaluateSensitivities(const OCPiterate &iter, BandedCP &cp)
NLPderivativeApproximation * derivativeApproximation
Definition: scp_method.hpp:265
virtual returnValue getFirstControl(DVector &u0_) const =0
returnValue acadoPrintCopyrightNotice(const std::string &subpackage)
returnValue replot(PlotFrequency _frequency=PLOT_IN_ANY_CASE)
Encapsulates all user interaction for setting options, logging data and plotting results.
virtual returnValue getDifferentialStates(VariablesGrid &xd_) const
virtual returnValue performCurrentStep()
Definition: scp_method.cpp:384
Implements a Gauss-Newton approximation with block BFGS updates as second-order derivatives within NL...
#define returnValue
void rhs(const real_t *x, real_t *f)
Implements different sequential convex programming methods for solving NLPs.
Definition: scp_method.hpp:67
PrintLevel
Implements BFGS updates for approximating second-order derivatives within NLPsolvers.
Definition: bfgs_update.hpp:67
virtual returnValue initScaling(BlockMatrix &B, const BlockMatrix &x, const BlockMatrix &y)=0
virtual returnValue start()
Definition: real_clock.cpp:78
Base class for different algorithms for solving nonlinear programming (NLP) problems.
Definition: nlp_solver.hpp:59
virtual returnValue getSensitivitiesU(BlockMatrix &_sens) const
#define ASSERT(x)
BooleanType hasLSQobjective() const
VariablesGrid * p
#define BT_TRUE
Definition: acado_types.hpp:47
virtual returnValue shift(double timeShift=-1.0, DVector lastX=emptyVector, DVector lastXA=emptyVector, DVector lastP=emptyVector, DVector lastU=emptyVector, DVector lastW=emptyVector)
virtual returnValue init(const OCPiterate &iter)
virtual returnValue getControls(VariablesGrid &u_) const
RealClock clock
Definition: scp_method.hpp:257
virtual NLPsolver * clone() const
Definition: scp_method.cpp:191
virtual returnValue init(VariablesGrid *x_init, VariablesGrid *xa_init, VariablesGrid *p_init, VariablesGrid *u_init, VariablesGrid *w_init)
Definition: scp_method.cpp:198
virtual returnValue stop()
Definition: real_clock.cpp:107
virtual returnValue printRuntimeProfile() const
Definition: scp_method.cpp:623
DVector getVector(uint pointIdx) const
virtual returnValue feedbackStep(const DVector &x0_, const DVector &p_=emptyConstVector)
Definition: scp_method.cpp:317
virtual returnValue getAnySensitivities(BlockMatrix &_sens, uint idx) const
BlockMatrix dynGradient
Definition: banded_cp.hpp:109
Allows to setup and store user-specified log records of algorithmic information.
Definition: log_record.hpp:72
OCPiterate oldIter
Definition: scp_method.hpp:261
Base class for different ways to evaluate functions and derivatives within an SCPmethod for solving N...
VariablesGrid * w
#define BEGIN_NAMESPACE_ACADO
int addLogRecord(LogRecord &_record)
#define BT_FALSE
Definition: acado_types.hpp:49
uint getNumConstraints() const
virtual returnValue setReference(const VariablesGrid &ref)
Definition: scp_method.cpp:567
returnValue print() const
SCPevaluation * eval
Definition: scp_method.hpp:263
BooleanType needToReevaluate
Definition: scp_method.hpp:275
virtual returnValue getAlgebraicStates(VariablesGrid &xa_) const
virtual returnValue shiftVariables(double timeShift=-1.0, DVector lastX=emptyVector, DVector lastXA=emptyVector, DVector lastP=emptyVector, DVector lastU=emptyVector, DVector lastW=emptyVector)
Definition: scp_method.cpp:582
Solves banded conic programs arising in optimal control using condensing.
virtual returnValue solve(const DVector &x0_=emptyConstVector, const DVector &p_=emptyConstVector)
Definition: scp_method.cpp:260
virtual returnValue prepareSolve(BandedCP &cp)
OCPiterate iter
Definition: scp_method.hpp:260
virtual returnValue init(const OCPiterate &iter_)=0
Stores and evaluates the objective function of optimal control problems.
Definition: objective.hpp:123
#define ACADOERROR(retval)
BooleanType hasPerformedStep
Definition: scp_method.hpp:273
returnValue getLast(LogName _name, DMatrix &lastValue) const
virtual returnValue getTime(double &_elapsedTime)
Definition: clock.cpp:92
uint getNumRows() const


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