shooting_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 
27 
36 
37 
38 
40 
41 
42 
43 //
44 // PUBLIC MEMBER FUNCTIONS:
45 //
46 
47 
49 
50  integrator = 0;
51 }
52 
53 
55  :DynamicDiscretization( _userInteraction ){
56 
57  integrator = 0;
58 }
59 
61 
62  ShootingMethod::copy( arg );
63 }
64 
65 
67 
69 }
70 
71 
73 
74  if ( this != &arg ){
77  ShootingMethod::copy( arg );
78  }
79 
80  return *this;
81 }
82 
83 
85 
86  int run1;
87  if( arg.integrator != 0 ){
88  integrator = (Integrator**)calloc(N,sizeof(Integrator*));
89  for( run1 = 0; run1 < N; run1++ ){
90  if( arg.integrator[run1] != 0 ) integrator[run1] = (arg.integrator[run1])->clone();
91  else integrator[run1] = 0 ;
92  }
93  }
94  else integrator = 0;
95 
97 }
98 
100 
101  return new ShootingMethod(*this);
102 }
103 
104 
106  const Grid &stageIntervals,
107  const IntegratorType &integratorType_ ){
108 
109 
110  // LOAD THE DIFFERENTIAL EQUATION FROM THE DYNAMIC SYSTEM:
111  // -------------------------------------------------------
112  DifferentialEquation differentialEquation_ = dynamicSystem_.getDifferentialEquation( );
113  int integratorTypeTmp = integratorType_;
114 
115 
116  // AUTOMATICALLY SWITCH TO DISCRETE TIME INTEGRATOR IF NECESSARY:
117  // --------------------------------------------------------------
118  if( differentialEquation_.isDiscretized() == BT_TRUE ){
119  if( integratorTypeTmp != INT_DISCRETE )
120  integratorTypeTmp = INT_DISCRETE;
121  }
122  else{
123  if( integratorTypeTmp == INT_DISCRETE )
125  }
126 
127 
128  // CONSTRUCT THE APPROPRIATE INTEGRATOR BASED ON THE OPTIONS:
129  // ----------------------------------------------------------
130  int run1 = N;
131  unionGrid = unionGrid & stageIntervals;
133 
134  integrator = (Integrator**)realloc(integrator,N*sizeof(Integrator*));
135 
136  while( run1 < N ){
137  allocateIntegrator( run1, (IntegratorType) integratorTypeTmp );
138  integrator[run1]->init( differentialEquation_ );
139  run1++;
140  }
141 
142  // STORE THE INFORMATION ABOUT STAGE-BREAK POINTS AND START/END TIMES:
143  // -------------------------------------------------------------------
144  int tmp = 0;
145  if( breakPoints.getNumRows() > 0 ){
146  addOptionsList( );
147  tmp = (int) breakPoints( breakPoints.getNumRows()-1, 0 );
148  }
149 
150  DMatrix stageIndices(1,5);
151 
152  stageIndices(0,0) = stageIntervals.getNumIntervals() + tmp;
153  stageIndices(0,1) = differentialEquation_.getStartTimeIdx();
154  stageIndices(0,2) = differentialEquation_.getEndTimeIdx();
155  stageIndices(0,3) = differentialEquation_.getStartTime();
156  stageIndices(0,4) = differentialEquation_.getEndTime();
157 
158  breakPoints.appendRows(stageIndices);
159 
160  return SUCCESSFUL_RETURN;
161 }
162 
163 
165 
166  switch( type_ ){
167 
168  case INT_DISCRETE: integrator[idx] = new IntegratorDiscretizedODE(); break;
169  case INT_RK12 : integrator[idx] = new IntegratorRK12 (); break;
170  case INT_RK23 : integrator[idx] = new IntegratorRK23 (); break;
171  case INT_RK45 : integrator[idx] = new IntegratorRK45 (); break;
172  case INT_RK78 : integrator[idx] = new IntegratorRK78 (); break;
173  case INT_BDF : integrator[idx] = new IntegratorBDF (); break;
174  case INT_UNKNOWN : integrator[idx] = new IntegratorBDF (); break;
175  case INT_LYAPUNOV45 : integrator[idx] = new IntegratorLYAPUNOV45 (); break;
176 
177  default: return ACADOERROR( RET_UNKNOWN_BUG ); break;
178  }
179  return SUCCESSFUL_RETURN;
180 }
181 
182 
183 
185 
186  if( transition_.getNXA() != 0 ) return ACADOERROR( RET_TRANSITION_DEPENDS_ON_ALGEBRAIC_STATES );
187  integrator[N-1]->setTransition( transition_ );
188 
189  return SUCCESSFUL_RETURN;
190 }
191 
192 
194 
195  deleteAllSeeds();
197  integrator = 0;
198  breakPoints.init(0,0);
199 
200  return SUCCESSFUL_RETURN;
201 }
202 
203 
204 
206  )
207 {
208 // iter.print(); ASSERT( 1==0 );
209 
210  // INTRODUCE SOME AUXILIARY VARIABLES:
211  // -----------------------------------
212  ASSERT( iter.x != 0 );
213 
214  uint run1;
215  double tStart, tEnd;
216 
217  DVector x ; nx = iter.getNX ();
218  DVector xa; na = iter.getNXA();
219  DVector p ; np = iter.getNP ();
220  DVector u ; nu = iter.getNU ();
221  DVector w ; nw = iter.getNW ();
222 
223  VariablesGrid xAll;
224  VariablesGrid xaAll;
225 
226  residuum = *(iter.x);
227  residuum.setAll( 0.0 );
228 
229  iter.getInitialData( x, xa, p, u, w );
230 // iter.x->print( "x" );
231 // iter.u->print( "u" );
232 
233  // RUN A LOOP OVER ALL INTERVALS OF THE UNION GRID:
234  // ------------------------------------------------
235 
236 // printf("unionGrid:\n");
237 // unionGrid.print();
238 
239  for( run1 = 0; run1 < unionGrid.getNumIntervals(); run1++ ){
240 
241  integrator[run1]->setOptions( getOptions( 0 ) ); // ??
242 
243  int freezeIntegrator;
244  get( FREEZE_INTEGRATOR, freezeIntegrator );
245 
246  if ( (BooleanType)freezeIntegrator == BT_TRUE )
247  integrator[run1]->freezeAll();
248 
249  tStart = unionGrid.getTime( run1 );
250  tEnd = unionGrid.getTime( run1+1 );
251 
252 // printf("tStart = %e, tEnd = %e\n",tStart,tEnd );
253 // x.print("x");
254 // u.print("u");
255 // p.print("p");
256 
257 // integrator[run1]->set( INTEGRATOR_PRINTLEVEL, MEDIUM );
258 
259  Grid evaluationGrid;
260  iter.x->getSubGrid( tStart,tEnd,evaluationGrid );
261 
262  Grid outputGrid;
263  if ( acadoIsNegative( integrator[run1]->getDifferentialEquationSampleTime( ) ) == BT_TRUE )
264  outputGrid.init( tStart,tEnd,getNumEvaluationPoints() );
265  else
266  outputGrid.init( tStart,tEnd, 1+acadoRound( (tEnd-tStart)/integrator[run1]->getDifferentialEquationSampleTime() ) );
267 
268 // printf("evaluationGrid:\n");
269 // evaluationGrid.print();
270 // printf("outputGrid:\n");
271 // outputGrid.print();
272 // printf("evaluationGrid+outputGrid:\n");
273 // (outputGrid&evaluationGrid).print();
274 // printf("\n");
275 // u.print("u before");
276 // x.print("x before");
277 // w.print("w");
278  if ( integrator[run1]->integrate( outputGrid&evaluationGrid, x, xa, p, u, w ) != SUCCESSFUL_RETURN )
280 
281 
282  DVector xOld;
283  DVector pOld = p;
284 
285  if ( evaluationGrid.getNumPoints( ) <= 2 )
286  {
287  integrator[run1]->getX ( x );
288  integrator[run1]->getXA( xa );
289 // x.print("x after2");
290  xOld = x;
291 
292  iter.updateData( tEnd, x, xa, p, u, w );
293  }
294  else
295  {
296  integrator[run1]->getX ( xAll );
297  integrator[run1]->getXA( xaAll );
298 
299  xOld = xAll.getLastVector( );
300 // xOld.print("x after");
301 
302  for( uint run2=1; run2<outputGrid.getNumPoints(); ++run2 )
303  {
304  if ( evaluationGrid.hasTime( outputGrid.getTime(run2) ) == BT_TRUE )
305  {
306  x = xAll.getVector(run2);
307  xa = xaAll.getVector(run2);
308  iter.updateData( outputGrid.getTime(run2), x, xa, p, u, w );
309  }
310  }
311  }
312 
313  if ( iter.isInSimulationMode( ) == BT_FALSE )
314  p = pOld; // should be changed later...
315 
316  residuum.setVector( run1, xOld - x );
317 // (xOld - x).print("residuum");
318  }
319 
320  // LOG THE RESULTS:
321  // ----------------
322  return logTrajectory( iter );
323 }
324 
325 
326 
328  const DMatrix &seed,
329  DMatrix &Gx ,
330  DMatrix &Gp ,
331  DMatrix &Gu ,
332  DMatrix &Gw ){
333 
334  uint run1;
335 
336  Gx.init( seed.getNumRows(), nx );
337  Gp.init( seed.getNumRows(), np );
338  Gu.init( seed.getNumRows(), nu );
339  Gw.init( seed.getNumRows(), nw );
340 
341  for( run1 = 0; run1 < seed.getNumRows(); run1++ ){
342 
343  DVector tmp = seed.getRow( run1 );
344  DVector tmpX( nx );
345  DVector tmpP( np );
346  DVector tmpU( nu );
347  DVector tmpW( nw );
348 
349  ACADO_TRY( integrator[idx]->setBackwardSeed( 1, tmp ) );
350  ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
351  ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX, tmpP, tmpU, tmpW , 1 ) );
352 
353  Gx.setRow( run1, tmpX );
354  Gp.setRow( run1, tmpP );
355  Gu.setRow( run1, tmpU );
356  Gw.setRow( run1, tmpW );
357  }
358 
359  return SUCCESSFUL_RETURN;
360 }
361 
362 
363 
365  const DMatrix &dX ,
366  const DMatrix &dP ,
367  const DMatrix &dU ,
368  const DMatrix &dW ,
369  DMatrix &D ){
370 
371  int run1;
372  int n = 0;
373 
374  n = acadoMax( n, dX.getNumCols() );
375  n = acadoMax( n, dP.getNumCols() );
376  n = acadoMax( n, dU.getNumCols() );
377  n = acadoMax( n, dW.getNumCols() );
378 
379  D.init( nx, n );
380 
381  for( run1 = 0; run1 < n; run1++ ){
382 
383  DVector tmp;
384 
385  DVector tmpX; if( dX.isEmpty() == BT_FALSE ) tmpX = dX.getCol( run1 );
386  DVector tmpP; if( dP.isEmpty() == BT_FALSE ) tmpP = dP.getCol( run1 );
387  DVector tmpU; if( dU.isEmpty() == BT_FALSE ) tmpU = dU.getCol( run1 );
388  DVector tmpW; if( dW.isEmpty() == BT_FALSE ) tmpW = dW.getCol( run1 );
389 
390  ACADO_TRY( integrator[idx]->setForwardSeed( 1, tmpX, tmpP, tmpU, tmpW ) );
391  ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
392  ACADO_TRY( integrator[idx]->getForwardSensitivities( tmp, 1 ) );
393 
394  D.setCol( run1, tmp );
395  }
396 
397  return SUCCESSFUL_RETURN;
398 }
399 
400 
402  const DMatrix &dX ,
403  const DMatrix &dP ,
404  const DMatrix &dU ,
405  const DMatrix &dW ,
406  const DMatrix &seed,
407  DMatrix &D ,
408  DMatrix &ddX ,
409  DMatrix &ddP ,
410  DMatrix &ddU ,
411  DMatrix &ddW ){
412 
413  int run1;
414  int n = 0;
415 
416  n = acadoMax( n, dX.getNumCols() );
417  n = acadoMax( n, dP.getNumCols() );
418  n = acadoMax( n, dU.getNumCols() );
419  n = acadoMax( n, dW.getNumCols() );
420 
421  D.init( nx, n );
422 
423  ddX.init( n, nx );
424  ddP.init( n, np );
425  ddU.init( n, nu );
426  ddW.init( n, nw );
427 
428  for( run1 = 0; run1 < n; run1++ ){
429 
430  DVector tmp;
431 
432  DVector tmpX; if( dX.isEmpty() == BT_FALSE ) tmpX = dX.getCol( run1 );
433  DVector tmpP; if( dP.isEmpty() == BT_FALSE ) tmpP = dP.getCol( run1 );
434  DVector tmpU; if( dU.isEmpty() == BT_FALSE ) tmpU = dU.getCol( run1 );
435  DVector tmpW; if( dW.isEmpty() == BT_FALSE ) tmpW = dW.getCol( run1 );
436 
437  ACADO_TRY( integrator[idx]->setForwardSeed( 1, tmpX, tmpP, tmpU, tmpW ) );
438  ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
439  ACADO_TRY( integrator[idx]->getForwardSensitivities( tmp, 1 ) );
440 
441  D.setCol( run1, tmp );
442 
443  DVector tmp2 = seed.getCol(0);
444 
445  DVector tmpX2( nx );
446  DVector tmpP2( np );
447  DVector tmpU2( nu );
448  DVector tmpW2( nw );
449 
450  ACADO_TRY( integrator[idx]->setBackwardSeed( 2, tmp2 ) );
451  ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
452  ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX2, tmpP2, tmpU2, tmpW2 , 2 ) );
453 
454  ddX.setRow( run1, tmpX2 );
455  ddP.setRow( run1, tmpP2 );
456  ddU.setRow( run1, tmpU2 );
457  ddW.setRow( run1, tmpW2 );
458 
460  }
461 
462  return SUCCESSFUL_RETURN;
463 }
464 
465 
466 
468 
469  int i;
470 
471  // COMPUTATION OF BACKWARD SENSITIVITIES:
472  // --------------------------------------
473 
474  if( bSeed.isEmpty() == BT_FALSE ){
475 
476  dBackward.init( N, 5 );
477 
478  for( i = 0; i < N; i++ ){
479 
480  DMatrix seed, X, P, U, W;
481  bSeed.getSubBlock( 0, i, seed );
482 
483  ACADO_TRY( differentiateBackward( i, seed, X, P, U, W ) );
484 
485  if( nx > 0 ) dBackward.setDense( i, 0, X );
486  if( np > 0 ) dBackward.setDense( i, 2, P );
487  if( nu > 0 ) dBackward.setDense( i, 3, U );
488  if( nw > 0 ) dBackward.setDense( i, 4, W );
489  }
490  return SUCCESSFUL_RETURN;
491  }
492 
493 
494  // COMPUTATION OF FORWARD SENSITIVITIES:
495  // -------------------------------------
496 
497  dForward.init( N, 5 );
498 
499  for( i = 0; i < N; i++ ){
500 
501  DMatrix X, P, U, W, D, E;
502 
503  if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
504  if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
505  if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
506  if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
507 
508  if( nx > 0 ){ ACADO_TRY( differentiateForward( i, X, E, E, E, D )); dForward.setDense( i, 0, D ); }
509  if( np > 0 ){ ACADO_TRY( differentiateForward( i, E, P, E, E, D )); dForward.setDense( i, 2, D ); }
510  if( nu > 0 ){ ACADO_TRY( differentiateForward( i, E, E, U, E, D )); dForward.setDense( i, 3, D ); }
511  if( nw > 0 ){ ACADO_TRY( differentiateForward( i, E, E, E, W, D )); dForward.setDense( i, 4, D ); }
512  }
513  return SUCCESSFUL_RETURN;
514 }
515 
516 
517 
518 
520 
521  if( B.getNumCols() == 0 ) return SUCCESSFUL_RETURN;
522 
523  DMatrix E = eye<double>(B.getNumCols());
524  E *= 1e-10;
525 
526  DMatrix S = ((A.transpose().eval() * A)+E).inverse();
527  G += (B-G*A)*(S*A.transpose());
528  return SUCCESSFUL_RETURN;
529 }
530 
531 
532 
534 
535  int i,j;
536 
537  dForward.init( N, 5 );
538  DMatrix Gx, *Gu, b, d, D, E, X, P, U, W, A, B;
539  Gu = new DMatrix[N];
540 
541  for( i = 0; i < N; i++ ){
542 
543  if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
544  if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
545  if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
546  if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
547 
548  if( np > 0 ){ D.init( nx, np ); D.setZero(); dForward.setDense( i, 2, D ); }
549  if( nw > 0 ){ D.init( nx, nw ); D.setZero(); dForward.setDense( i, 4, D ); }
550 
551  if( nu > 0 ){ ACADO_TRY( differentiateForward( i, E, E, U, E, D )); dForward.setDense( i, 3, D ); Gu[i] = D; }
552  }
553 
554  if( nu > 0 ){
555 
556  d.init(nx,1);
557  d.setZero();
558 
559  for( i = 0; i < N; i++ ){
560  A.init(0,0);
561  B.init(0,0);
562  for( j = 0; j < i; j++ ){
563  differentiateForward( i, Gu[j], E, E, E, D );
564  A.appendCols( Gu[j] );
565  B.appendCols( D );
566  Gu[j] = D;
567  }
568 
569  if( i > 0 ){
570  differentiateForward( i, d, E, E, E, b );
571  A.appendCols( d );
572  B.appendCols( b );
573  d = b;
574  }
575  d += residuum.getMatrix(i);
576  Gx = eye<double>(nx);
577  Gx *= 0.001;
578  update( Gx, A, B );
579  dForward.setDense( i, 0, Gx );
580  }
581  }
582  delete[] Gu;
583 
584  return SUCCESSFUL_RETURN;
585 }
586 
587 
588 
590 
591  const int NN = N+1;
592  dForward.init( N, 5 );
593  int i;
594 
595  for( i = 0; i < N; i++ ){
596 
597  DMatrix X, P, U, W, D, E, HX, HP, HU, HW, S;
598 
599  if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
600  if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
601  if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
602  if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
603 
604  seed.getSubBlock( i, 0, S, nx, 1 );
605 
606  if( nx > 0 ){
607 
608  ACADO_TRY( differentiateForwardBackward( i, X, E, E, E, S, D, HX, HP, HU, HW ));
609  dForward.setDense( i, 0, D );
610 
611  if( nx > 0 ) hessian.addDense( i, i, HX );
612  if( np > 0 ) hessian.addDense( i, 2*NN+i, HP );
613  if( nu > 0 ) hessian.addDense( i, 3*NN+i, HU );
614  if( nw > 0 ) hessian.addDense( i, 4*NN+i, HW );
615  }
616 
617  if( np > 0 ){
618 
619  ACADO_TRY( differentiateForwardBackward( i, E, P, E, E, S, D, HX, HP, HU, HW ));
620  dForward.setDense( i, 2, D );
621 
622  if( nx > 0 ) hessian.addDense( 2*NN+i, i, HX );
623  if( np > 0 ) hessian.addDense( 2*NN+i, 2*NN+i, HP );
624  if( nu > 0 ) hessian.addDense( 2*NN+i, 3*NN+i, HU );
625  if( nw > 0 ) hessian.addDense( 2*NN+i, 4*NN+i, HW );
626  }
627 
628  if( nu > 0 ){
629 
630  ACADO_TRY( differentiateForwardBackward( i, E, E, U, E, S, D, HX, HP, HU, HW ));
631  dForward.setDense( i, 3, D );
632 
633  if( nx > 0 ) hessian.addDense( 3*NN+i, i, HX );
634  if( np > 0 ) hessian.addDense( 3*NN+i, 2*NN+i, HP );
635  if( nu > 0 ) hessian.addDense( 3*NN+i, 3*NN+i, HU );
636  if( nw > 0 ) hessian.addDense( 3*NN+i, 4*NN+i, HW );
637  }
638 
639  if( nw > 0 ){
640 
641  ACADO_TRY( differentiateForwardBackward( i, E, E, E, W, S, D, HX, HP, HU, HW ));
642  dForward.setDense( i, 4, D );
643 
644  if( nx > 0 ) hessian.addDense( 4*NN+i, i, HX );
645  if( np > 0 ) hessian.addDense( 4*NN+i, 2*NN+i, HP );
646  if( nu > 0 ) hessian.addDense( 4*NN+i, 3*NN+i, HU );
647  if( nw > 0 ) hessian.addDense( 4*NN+i, 4*NN+i, HW );
648  }
649  }
650  return SUCCESSFUL_RETURN;
651 }
652 
653 
655 
656  int run1;
657  for( run1 = 0; run1 < (int) unionGrid.getNumIntervals(); run1++ )
658  integrator[run1]->unfreeze();
659 
660  return SUCCESSFUL_RETURN;
661 }
662 
663 
665 
667 
668  uint i;
669  for( i = 0; i < unionGrid.getNumIntervals(); i++ )
671 
672  return SUCCESSFUL_RETURN;
673 }
674 
675 
677 
678  for( int run1 = 0; run1 < N; ++run1 )
679  if ( integrator[run1]->isDifferentialEquationAffine( ) == BT_FALSE )
680  return BT_FALSE;
681 
682  return BT_TRUE;
683 }
684 
685 
687 
688  int run1;
689  if( integrator != 0 ){
690  for( run1 = 0; run1 < N; run1++ )
691  if( integrator[run1] != 0 )
692  delete integrator[run1];
693  free(integrator);
694  integrator = 0;
695  }
696 
697  unionGrid.init();
699 
700  return SUCCESSFUL_RETURN;
701 }
702 
703 
705 
706  if( integrator == 0 ) return SUCCESSFUL_RETURN;
707 
708  int i, j;
709  double T = 0.0;
710  double t1 = 0.0, t2 = 0.0;
711  double h = 0.0;
712  BooleanType needToRescale = BT_FALSE;
713 
714  VariablesGrid logX, logXA, logP, logU, logW, logI, tmp,tmp2;
715 
716  DMatrix intervalPoints(N+1,1);
717  intervalPoints(0,0) = 0.0;
718 
719  j = 0;
720  for( i = 0; i < N; i++ ){
721 
722  if( (int) breakPoints(j,0) <= i ) j++;
723 
724  int i1 = (int) breakPoints(j,1);
725  int i2 = (int) breakPoints(j,2);
726 
727  if( i1 >= 0 ) t1 = iter.p->operator()(0,i1);
728  else t1 = breakPoints(j,3);
729 
730  if( i2 >= 0 ) t2 = iter.p->operator()(0,i2);
731  else t2 = breakPoints(j,4);
732 
733  if( i == 0 ) T = t1;
734 
735  integrator[i]->getX( tmp );
736 
737  intervalPoints(i+1,0) = intervalPoints(i,0) + tmp.getNumPoints();
738 
739  if ( ( i1 >= 0 ) || ( i2 >= 0 ) )
740  {
741  if ( iter.isInSimulationMode() == BT_FALSE )
742  {
743  h = t2-t1;
744  needToRescale = BT_TRUE;
745  }
746  }
747  else
748  {
749  h = 1.0;
750  needToRescale = BT_FALSE;
751  }
752 
753  if( nx > 0 ){ if ( needToRescale == BT_TRUE ) rescale( &tmp, T, h );
754  logX .appendTimes( tmp );
755  }
756  if( na > 0 ){ integrator[i]->getXA( tmp );
757  if ( needToRescale == BT_TRUE ) rescale( &tmp, T, h );
758  logXA.appendTimes( tmp );
759  }
760  if( np > 0 ){ tmp2.init( np, tmp.getFirstTime(),tmp.getLastTime(),2 );
761  if ( iter.isInSimulationMode( ) == BT_FALSE )
762  tmp2.setAllVectors( iter.p->getVector(0) );
763  else
764  tmp2.setAllVectors( iter.p->getVector(i) );
765  logP .appendTimes( tmp2 );
766  }
767  if( nu > 0 ){ tmp2.init( nu, tmp.getFirstTime(),tmp.getLastTime(),2 );
768  tmp2.setAllVectors(iter.u->getVector(i));
769  logU .appendTimes( tmp2 );
770  }
771  if( nw > 0 ){ tmp.init( nw, tmp );
772  tmp.setAllVectors(iter.w->getVector(i));
773  logW .appendTimes( tmp );
774  }
775  integrator[i]->getI( tmp );
776  if ( needToRescale == BT_TRUE ) rescale( &tmp, T, h );
777  logI .appendTimes( tmp );
778  T = tmp.getLastTime();
779  }
780 
781 
782  // WRITE DATA TO THE LOG COLLECTION:
783  // ---------------------------------
784  if( nx > 0 ) setLast( LOG_DIFFERENTIAL_STATES, logX );
785  if( na > 0 ) setLast( LOG_ALGEBRAIC_STATES , logXA );
786  if( np > 0 ) setLast( LOG_PARAMETERS , logP );
787  if( nu > 0 ) setLast( LOG_CONTROLS , logU );
788  if( nw > 0 ) setLast( LOG_DISTURBANCES , logW );
789 
791  setLast( LOG_DISCRETIZATION_INTERVALS, intervalPoints );
792 
793 
794  return SUCCESSFUL_RETURN;
795 }
796 
797 
799  double tEndNew,
800  double newIntervalLength
801  ) const
802 {
803  trajectory->shiftTimes( -trajectory->getTime(0) );
804  trajectory->scaleTimes( newIntervalLength );
805  trajectory->shiftTimes( tEndNew );
806 
807  return SUCCESSFUL_RETURN;
808 }
809 
810 
812 
813 // end of file.
VariablesGrid & shiftTimes(double timeShift)
returnValue setLast(LogName _name, int lastValue, double time=-INFTY)
GenericVector< T > getCol(unsigned _idx) const
Definition: matrix.hpp:205
returnValue setOptions(const Options &arg)
virtual returnValue getForwardSensitivities(BlockMatrix &D) const
Implements the Runge-Kutta-45 scheme for integrating ODEs.
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
returnValue getX(DVector &xEnd) const
Implements a very rudimentary block sparse matrix class.
virtual returnValue clear()
bool isEmpty() const
Definition: matrix.hpp:193
virtual returnValue evaluate(OCPiterate &iter)
double getStartTime() const
USING_NAMESPACE_ACADO typedef TaylorVariable< Interval > T
VariablesGrid * x
virtual returnValue evaluateSensitivitiesLifted()
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
uint getNX() const
returnValue allocateIntegrator(uint idx, IntegratorType type_)
double getTime(uint pointIdx) const
int acadoMax(const int x, const int y)
Definition: acado_utils.cpp:64
double getFirstTime() const
virtual BooleanType isDiscretized() const
Implements the backward-differentiation formula for integrating DAEs.
GenericMatrix & appendCols(const GenericMatrix &_arg)
Definition: matrix.cpp:83
VariablesGrid * u
Stores a DifferentialEquation together with an OutputFcn.
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
BooleanType acadoIsNegative(double x, double TOL)
virtual returnValue deleteAllSeeds()
GenericMatrix & setCol(unsigned _idx, const GenericVector< T > &_arg)
Definition: matrix.hpp:224
returnValue rescale(VariablesGrid *trajectory, double tEndNew, double newIntervalLength) const
Discretizes a DifferentialEquation by means of single or multiple shooting.
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
virtual returnValue evaluateSensitivities()
virtual returnValue setBackwardSeed(const BlockMatrix &seed)
void copy(const ShootingMethod &arg)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
returnValue updateData(double t, DVector &x_=emptyVector, DVector &xa_=emptyVector, DVector &p_=emptyVector, DVector &u_=emptyVector, DVector &w_=emptyVector)
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
returnValue differentiateForwardBackward(const int &idx, const DMatrix &dX, const DMatrix &dP, const DMatrix &dU, const DMatrix &dW, const DMatrix &seed, DMatrix &D, DMatrix &ddX, DMatrix &ddP, DMatrix &ddU, DMatrix &ddW)
DynamicDiscretization & operator=(const DynamicDiscretization &rhs)
const DifferentialEquation & getDifferentialEquation(uint stageIdx=0) const
GenericMatrix & appendRows(const GenericMatrix &_arg)
Definition: matrix.cpp:62
uint getNP() const
Integrator ** integrator
virtual returnValue deleteAllSeeds()
uint getNU() const
#define CLOSE_NAMESPACE_ACADO
returnValue getSubGrid(double tStart, double tEnd, Grid &_subGrid) const
Definition: grid.cpp:655
Base class for discretizing a DifferentialEquation for use in optimal control algorithms.
int getNXA() const
Definition: function.cpp:212
returnValue addOptionsList()
int getStartTimeIdx() const
returnValue setAll(double _value)
Implements the Runge-Kutta-23 scheme for integrating ODEs.
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
Definition: integrator.hpp:61
returnValue differentiateBackward(const int &idx, const DMatrix &seed, DMatrix &Gx, DMatrix &Gp, DMatrix &Gu, DMatrix &Gw)
#define ACADO_TRY(X)
ShootingMethod & operator=(const ShootingMethod &rhs)
uint getNW() const
returnValue logTrajectory(const OCPiterate &iter)
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
returnValue setVector(uint pointIdx, const DVector &_values)
IntegratorType
returnValue getInitialData(DVector &x_=emptyVector, DVector &xa_=emptyVector, DVector &p_=emptyVector, DVector &u_=emptyVector, DVector &w_=emptyVector) const
virtual DynamicDiscretization * clone() const
returnValue getI(VariablesGrid &I) const
returnValue init(uint _nRows, uint _nCols)
int acadoRound(double x)
GenericMatrix & setRow(unsigned _idx, const GenericVector< T > &_values)
Definition: matrix.hpp:213
Implements the Runge-Kutta-78 scheme for integrating ODEs.
returnValue setAllVectors(const DVector &_values)
#define E
DVector getLastVector() const
virtual returnValue addTransition(const Transition &transition_)
returnValue init()
returnValue setTransition(const Transition &trs)
Definition: integrator.cpp:198
returnValue init(uint _nPoints=0, const double *const _times=0)
Definition: grid.cpp:107
Derived & setZero(Index size)
Encapsulates all user interaction for setting options, logging data and plotting results.
Implements the Runge-Kutta-12 scheme for integrating ODEs.
returnValue appendTimes(const VariablesGrid &arg, MergeMethod _mergeMethod=MM_DUPLICATE)
double getEndTime() const
virtual BooleanType isAffine() const
returnValue scaleTimes(double scaling)
Definition: grid.cpp:460
DMatrix getMatrix(uint pointIdx) const
unsigned getNumRows() const
Definition: matrix.hpp:185
#define ASSERT(x)
Implements the Runge-Kutta-45 scheme for integrating ODEs.
VariablesGrid * p
returnValue getXA(DVector &xaEnd) const
#define BT_TRUE
Definition: acado_types.hpp:47
returnValue deleteAll()
virtual returnValue unfreeze()
unsigned getNumCols() const
Definition: matrix.hpp:189
uint getNumIntervals() const
returnValue differentiateForward(const int &idx, const DMatrix &dX, const DMatrix &dP, const DMatrix &dU, const DMatrix &dW, DMatrix &D)
Allows to setup and evaluate transition functions based on SymbolicExpressions.
Definition: transition.hpp:53
uint getNumPoints() const
Options getOptions(uint idx) const
DVector getVector(uint pointIdx) const
virtual returnValue setForwardSeed(const BlockMatrix &xSeed_, const BlockMatrix &pSeed_, const BlockMatrix &uSeed_, const BlockMatrix &wSeed_)
virtual returnValue init(const DifferentialEquation &rhs)=0
double getLastTime() const
bool isEmpty() const
uint getNXA() const
VariablesGrid * w
#define BEGIN_NAMESPACE_ACADO
#define BT_FALSE
Definition: acado_types.hpp:49
virtual ~ShootingMethod()
virtual returnValue getBackwardSensitivities(BlockMatrix &D) const
returnValue update(DMatrix &G, const DMatrix &A, const DMatrix &B)
virtual returnValue addStage(const DynamicSystem &dynamicSystem_, const Grid &stageIntervals, const IntegratorType &integratorType_=INT_UNKNOWN)
returnValue addDense(uint rowIdx, uint colIdx, const DMatrix &value)
int getEndTimeIdx() const
BooleanType isInSimulationMode() const
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
Implements a scheme for evaluating discretized ODEs.
#define ACADOERROR(retval)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.


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