ACADOintegrators.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-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.
6  * Developed within the Optimization in Engineering Center (OPTEC) under
7  * supervision of Moritz Diehl. All rights reserved.
8  *
9  * ACADO Toolkit is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public
11  * License as published by the Free Software Foundation; either
12  * version 3 of the License, or (at your option) any later version.
13  *
14  * ACADO Toolkit is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17  * Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public
20  * License along with ACADO Toolkit; if not, write to the Free Software
21  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
22  *
23  */
24 
25 
32 #include <acado_integrators.hpp>
33 #include <acado/utils/matlab_acado_utils.hpp>
34 
35 
36 USING_NAMESPACE_ACADO
37 
38 
39 #include "model_include.hpp"
40 
41 //#include "stdafx.h"
42 
43 
44 // global pointer to Matlab function handle,
45 // if dynamic model is given like that
46 mxArray* ModelFcn_f = NULL;
47 DifferentialEquation* modelFcn = NULL;
48 mxArray* ModelFcn_jac = NULL;
49 
50 mxArray* ModelFcnT = NULL;
51 mxArray* ModelFcnX = NULL;
52 mxArray* ModelFcnXA = NULL;
53 mxArray* ModelFcnU = NULL;
54 mxArray* ModelFcnP = NULL;
55 mxArray* ModelFcnW = NULL;
56 mxArray* ModelFcnDX = NULL;
57 
58 unsigned int modelFcnNX = 0;
59 unsigned int modelFcnNXA = 0;
60 unsigned int modelFcnNU = 0;
61 unsigned int modelFcnNP = 0;
62 unsigned int modelFcnNW = 0;
63 unsigned int modelFcnNDX = 0;
64 
65 unsigned int jacobianNumber = -1;
66 double* f_store = NULL;
67 double* J_store = NULL;
68 
69 void clearAllGlobals( )
70 {
71  // first, clear all static counters within the toolkit
72  clearAllStaticCounters( );
73 
74  // second, clear all global variables of the interface
75  if ( modelFcn != NULL )
76  {
77  modelFcn = NULL;
78  delete modelFcn;
79  }
80 
81  if ( f_store != NULL )
82  {
83  f_store = NULL;
84  delete f_store;
85  }
86 
87  if ( J_store != NULL )
88  {
89  J_store = NULL;
90  delete J_store;
91  }
92 
93  if ( ModelFcn_f != NULL )
94  {
95  mxDestroyArray( ModelFcn_f );
96  ModelFcn_f = NULL;
97  }
98 
99  if ( ModelFcnT != NULL )
100  {
101  mxDestroyArray( ModelFcnT );
102  ModelFcnT = NULL;
103  }
104 
105  if ( ModelFcnX != NULL )
106  {
107  mxDestroyArray( ModelFcnX );
108  ModelFcnX = NULL;
109  }
110 
111  if ( ModelFcnXA != NULL )
112  {
113  mxDestroyArray( ModelFcnXA );
114  ModelFcnXA = NULL;
115  }
116 
117  if ( ModelFcnU != NULL )
118  {
119  mxDestroyArray( ModelFcnU );
120  ModelFcnU = NULL;
121  }
122 
123  if ( ModelFcnP != NULL )
124  {
125  mxDestroyArray( ModelFcnP );
126  ModelFcnP = NULL;
127  }
128 
129  if ( ModelFcnW != NULL )
130  {
131  mxDestroyArray( ModelFcnW );
132  ModelFcnW = NULL;
133  }
134 
135  if ( ModelFcnDX != NULL )
136  {
137  mxDestroyArray( ModelFcnDX );
138  ModelFcnDX = NULL;
139  }
140 
141  if ( ModelFcn_jac != NULL )
142  {
143  mxDestroyArray( ModelFcn_jac );
144  ModelFcn_jac = NULL;
145  }
146 
147 
148  modelFcnNX = 0;
149  modelFcnNXA = 0;
150  modelFcnNU = 0;
151  modelFcnNP = 0;
152  modelFcnNW = 0;
153  modelFcnNDX = 0;
154  jacobianNumber = -1;
155 }
156 
157 
158 // generic dynamic model function for evaluating
159 // Matlab function handles from C
160 void genericODE( double* x, double* f, void *userData )
161 {
162  unsigned int i;
163 
164  double* tt = mxGetPr( ModelFcnT );
165  tt[0] = x[0];
166 
167  double* xx = mxGetPr( ModelFcnX );
168  for( i=0; i<modelFcnNX; ++i )
169  xx[i] = x[i+1];
170 
171  double* uu = mxGetPr( ModelFcnU );
172  for( i=0; i<modelFcnNU; ++i )
173  uu[i] = x[i+1+modelFcnNX];
174 
175  double* pp = mxGetPr( ModelFcnP );
176  for( i=0; i<modelFcnNP; ++i )
177  pp[i] = x[i+1+modelFcnNX+modelFcnNU];
178 
179  double* ww = mxGetPr( ModelFcnW );
180  for( i=0; i<modelFcnNW; ++i )
181  ww[i] = x[i+1+modelFcnNX+modelFcnNU+modelFcnNP];
182 
183  mxArray* FF = NULL;
184 
185  mxArray* argIn[] = { ModelFcn_f,ModelFcnT,ModelFcnX,ModelFcnU,ModelFcnP,ModelFcnW };
186  mxArray* argOut[] = { FF };
187 
188  mexCallMATLAB( 1,argOut, 6,argIn,"generic_ode" );
189 
190 
191  double* ff = mxGetPr( *argOut );
192 
193  for( i=0; i<modelFcnNX; ++i )
194  f[i] = ff[i];
195 
196  mxDestroyArray( *argOut );
197 
198 }
199 
200 
201 void genericJacobian ( int number, double* x, double* seed, double* f, double* df, void *userData )
202 {
203  unsigned int i, j;
204  double* ff;
205  double* J;
206 
207 
208  if (J_store == NULL){
209 
210  J_store = (double*) calloc ((modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW)*(modelFcnNX+modelFcnNXA),sizeof(double));
211  f_store = (double*) calloc (modelFcnNX,sizeof(double));
212 
213  }
214 
215 
216  if ( (int) jacobianNumber == number){
217 
218  //mexPrintf("\n SAME JACOBIAN --- Number:%d", number);
219  //ff = f_store;
220  J = J_store;
221  ff = f_store;
222 
223  for( i=0; i<modelFcnNX+modelFcnNXA; ++i ) {
224  df[i] = 0;
225  f[i] = 0; //F[i]
226  for (j=0; j < modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW; ++j){
227  df[i] += J[(j*(modelFcnNX+modelFcnNXA))+i]*seed[j+1]; // J+1 since we do not use nt here
228  //mexPrintf ("\n jac %d, %d = %f -- seed: %f -- function : %f", i, j, J[(j*(modelFcnNX+modelFcnNXA))+i], seed[j+1], f[i]);
229  }
230  }
231 
232 
233  for( i=0; i<modelFcnNX; ++i ){
234  f[i] = ff[i];
235  //mexPrintf("\n function EVAL %d --> %f", i, f[i]);
236  }
237 
238 
239  }else{
240  //mexPrintf("\n DIFFERENT JACOBIAN --- Number:%d", number);
241 
242  jacobianNumber = number; // Store current number in global.
243 
244 
245  // Set the values to pass to generic_jacobian
246  double* tt = mxGetPr( ModelFcnT );
247  tt[0] = x[0];
248 
249  double* xx = mxGetPr( ModelFcnX );
250  for( i=0; i<modelFcnNX; ++i )
251  xx[i] = x[i+1];
252 
253  double* uu = mxGetPr( ModelFcnU );
254  for( i=0; i<modelFcnNU; ++i )
255  uu[i] = x[i+1+modelFcnNX];
256 
257  double* pp = mxGetPr( ModelFcnP );
258  for( i=0; i<modelFcnNP; ++i )
259  pp[i] = x[i+1+modelFcnNX+modelFcnNU];
260 
261  double* ww = mxGetPr( ModelFcnW );
262  for( i=0; i<modelFcnNW; ++i )
263  ww[i] = x[i+1+modelFcnNX+modelFcnNU+modelFcnNP];
264 
265  // Execute generic_jacobian
266  mxArray* FF = NULL;
267  mxArray* argIn[] = { ModelFcn_jac,ModelFcnT,ModelFcnX,ModelFcnU,ModelFcnP,ModelFcnW };
268  mxArray* argOut[] = { FF };
269 
270  //mexPrintf("\n EVAL JACOBIAN - JAC");
271  mexCallMATLAB( 1,argOut, 6,argIn,"generic_jacobian" );
272 
273  // Examine output
274  unsigned int rowLen = mxGetM(*argOut);
275  unsigned int colLen = mxGetN(*argOut);
276 
277 
278  if (rowLen != modelFcnNX+modelFcnNXA)
279  {
280  mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian matrix rows do not match (should be modelFcnNX+modelFcnNXA). " );
281  }
282 
283  if (colLen != modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW)
284  {
285  mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian matrix columns do not match (should be modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW). " );
286  }
287 
288  J = mxGetPr( *argOut );
289 
290  memcpy(J_store, J, (modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW)*(modelFcnNX+modelFcnNXA) * sizeof ( double ));
291 
292 
293 
294  for( i=0; i<modelFcnNX+modelFcnNXA; ++i ) {
295  df[i] = 0;
296  f[i] = 0; //F[i]
297  for (j=0; j < modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW; ++j){
298  df[i] += J[(j*(modelFcnNX+modelFcnNXA))+i]*seed[j+1]; // J+1 since we do not use nt here
299  //mexPrintf ("\n jac %d, %d = %f -- seed: %f -- function : %f", i, j, J[(j*(modelFcnNX+modelFcnNXA))+i], seed[j+1], f[i]);
300  }
301  }
302 
303 
304 
305 
306  mxArray* FF2 = NULL;
307  mxArray* argIn2[] = { ModelFcn_f,ModelFcnT,ModelFcnX,ModelFcnU,ModelFcnP,ModelFcnW };
308  mxArray* argOut2[] = { FF2 };
309 
310  mexCallMATLAB( 1,argOut2, 6,argIn2,"generic_ode" );
311 
312 
313  ff = mxGetPr( *argOut2 );
314  memcpy(f_store, ff, (modelFcnNX) * sizeof ( double ));
315 
316 
317  for( i=0; i<modelFcnNX; ++i ){
318  f[i] = ff[i];
319  //mexPrintf("\n function EVAL %d --> %f", i, f[i]);
320  }
321 
322  mxDestroyArray( *argOut );
323  mxDestroyArray( *argOut2 );
324 
325 
326  }
327 
328 
329 
330  //mexPrintf("\n\n");
331 }
332 
333 // generic dynamic model function for evaluating
334 // Matlab function handles from C
335 void genericDAE( double* x, double* f, void *userData )
336 {
337  unsigned int i;
338 
339  double* tt = mxGetPr( ModelFcnT );
340  tt[0] = x[0];
341 
342  double* xx = mxGetPr( ModelFcnX );
343  for( i=0; i<modelFcnNX; ++i )
344  xx[i] = x[i+1];
345 
346  double* xxa = mxGetPr( ModelFcnXA );
347  for( i=0; i<modelFcnNXA; ++i )
348  xxa[i] = x[i+1+modelFcnNX];
349 
350  double* uu = mxGetPr( ModelFcnU );
351  for( i=0; i<modelFcnNU; ++i )
352  uu[i] = x[i+1+modelFcnNX+modelFcnNXA];
353 
354  double* pp = mxGetPr( ModelFcnP );
355  for( i=0; i<modelFcnNP; ++i )
356  pp[i] = x[i+1+modelFcnNX+modelFcnNXA+modelFcnNU];
357 
358  double* ww = mxGetPr( ModelFcnW );
359  for( i=0; i<modelFcnNW; ++i )
360  ww[i] = x[i+1+modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP];
361 
362  // call matlab model via function handle and stack results
363  mxArray* argIn_f[] = { ModelFcn_f,ModelFcnT,ModelFcnX,ModelFcnXA,ModelFcnU,ModelFcnP,ModelFcnW };
364 
365  mxArray* FF = NULL;
366  double* ff = NULL;
367  mxArray* argOut[] = { FF };
368 
370 
371  mexCallMATLAB( 1,argOut, 7,argIn_f,"generic_dae" );
372  ff = mxGetPr( *argOut );
373  for( i=0; i<modelFcnNX+modelFcnNXA; ++i )
374  f[i] = ff[i];
375 
376  //printf("after DAE call\n");
377 
378  mxDestroyArray( *argOut );
379 }
380 
381 
382 unsigned int determineNumberOfAlgebraicStates( )
383 {
384  return 0;
385 }
386 
387 
388 void plotErrorMessage( returnValue returnvalue,
389  double* PrintLevel_
390  )
391 {
392  if ( ( PrintLevel_ != NULL ) && ( PrintLevel_[0] <= 0.0 ) )
393  {
394  switch ( returnvalue )
395  {
396  case RET_MISSING_INPUTS:
397  mexPrintf( "ERROR (ACADOintegrator): The integrator misses some inputs.\n" );
398 
399  case RET_TO_SMALL_OR_NEGATIVE_TIME_INTERVAL:
400  mexPrintf( "ERROR (ACADOintegrator): The integration interval is too small or negative.\n" );
401 
402  case RET_MAX_NUMBER_OF_STEPS_EXCEEDED:
403  mexPrintf( "ERROR (ACADOintegrator): The maximum number of integration steps is exceeded.\n" );
404 
405  case RET_INPUT_HAS_WRONG_DIMENSION:
406  mexPrintf( "ERROR (ACADOintegrator): At least one of the inputs has a wrong dimension.\n" );
407 
408  case SUCCESSFUL_RETURN:
409  return;
410 
411  default:
412  mexPrintf( "ERROR (ACADOintegrator): Unsuccessful return from the integrator.\n");
413  }
414  }
415 }
416 
420 DifferentialEquation* allocateDifferentialEquation( char *modelName, // name of the model
421  char *integratorName // name of the integrator
422  )
423 {
424  DifferentialEquation* f = NULL;
425 
426  if( strcmp(integratorName,"DiscretizedODE") == 0 )
427  f = new DiscretizedDifferentialEquation;
428  else
429  f = new DifferentialEquation;
430 
431  if ( modelName != NULL )
432  {
433  // function name is given as string
434  void (*fcn)( DifferentialEquation* );
435  fcn = allocateFunctionPointer( modelName );
436 
437  if( fcn == 0 )
438  {
439  delete f;
440  clearAllGlobals( );
441  mexErrMsgTxt( "ERROR (ACADOintegrator): A model with the specified name does not exist.\n" );
442  }
443 
444  fcn( f );
445  }
446 
447  return f;
448 }
449 
450 
451 //allocateJacobian
452 
453 
457 Integrator* allocateIntegrator( char *integratorName, // name of the integrator
458  DifferentialEquation* f // allocated differential equation
459  )
460 {
461  Integrator* integrator = NULL;
462 
463  if ( f == NULL )
464  mexErrMsgTxt( "bug!" );
465 
466  if( strcmp(integratorName,"RK12") == 0 )
467  integrator = new IntegratorRK12( *f );
468 
469  if( strcmp(integratorName,"RK23") == 0 )
470  integrator = new IntegratorRK23( *f );
471 
472  if( strcmp(integratorName,"RK45") == 0 )
473  integrator = new IntegratorRK45( *f );
474 
475  if( strcmp(integratorName,"RK78") == 0 )
476  integrator = new IntegratorRK78( *f );
477 
478  if( strcmp(integratorName,"DiscretizedODE") == 0 )
479  integrator = new IntegratorDiscretizedODE( *f );
480 
481  if( strcmp(integratorName,"BDF") == 0 )
482  integrator = new IntegratorBDF( *f );
483 
484  return integrator;
485 }
486 
487 
488 // --------------------------------------------------------------------------
489 
490  // ********************************//
491  // //
492  // m e x F u n c t i o n //
493  // ________________________________//
494 
513 void mexFunction( int nlhs, mxArray *plhs[],
514  int nrhs, const mxArray *prhs[]
515  )
516 {
517  unsigned int i,j;
518 
519 
520  // INPUT ARGUMENTS:
521  // ---------------------------------------------------------------
522 
523  // integrator settings struct
524  const mxArray *IntegratorSettings = NULL;
525 
526  // start point for differential states
527  double *xStart = NULL;
528 
529  // initial value for algebraic states (only for DAEs)
530  double *xaStart = NULL;
531 
532  // start of the integration interval
533  double *tStart = NULL;
534  int tStartIdx = -1;
535 
536  // end of the integration interval
537  double *tEnd = NULL;
538  int tEndIdx = -1;
539 
540  // is dynamic model an ODE (DAE otherwise)?
541  BooleanType isODE = BT_TRUE;
542 
543  // end value of algebraic states is to be returned to plhs[xaEndIdx] (= -1 for none)
544  int xaEndIdx = -1;
545 
546  // an output struct is to be returned to plhs[outputIdx] (= -1 for none)
547  int outputIdx = -1;
548 
549 
550  // SETTINGS:
551  // ---------------------------------------------------------------
552 
553  // name of the model
554  mxArray *ModelName = NULL;
555  char *modelName = NULL; // (if specified as string, otherwise global modelFcn is used)
556 
557  // name of the integrator
558  mxArray *IntegratorName = NULL;
559  char *integratorName = NULL;
560 
561  // tolerance of the integrator
562  mxArray *Tol = NULL;
563  double *tol = NULL;
564 
565  // tolerance of the integrator
566  mxArray *aTol = NULL;
567  double *atol = NULL;
568 
569  // maximum number of allowed steps
570  mxArray *MaxNumStep = NULL;
571  double *maxNumStep = NULL;
572 
573  // min step size
574  mxArray *MinStep = NULL;
575  double *minStep = NULL;
576 
577  // initial step size
578  mxArray *InitialStep = NULL;
579  double *initialStep = NULL;
580 
581  // max step size
582  mxArray *MaxStep = NULL;
583  double *maxStep = NULL;
584 
585  // tuning
586  mxArray *StepTuning = NULL;
587  double *stepTuning = NULL;
588 
589  // linear algebra solver
590  mxArray *LinearAlgebraSolver = NULL;
591  char *linearAlgebraSolver = NULL;
592 
593  // controls
594  mxArray *U = NULL;
595  double *u = NULL;
596 
597  // parameters
598  mxArray *P = NULL;
599  double *p = NULL;
600 
601  // disturbances
602  mxArray *W = NULL;
603  double *w = NULL;
604 
605  // initial value for xdot (only for DAEs)
606  mxArray *DxInit = NULL;
607  double *dxInit = NULL;
608 
609  // sensitivity mode
610  mxArray *SensitivityMode = NULL;
611  char *sensitivityMode = NULL;
612 
613  // backward seed
614  mxArray *Bseed = NULL;
615  double *bseed = NULL;
616 
617  // forward seed (w.r.t. x)
618  mxArray *Dx = NULL;
619  double *dx = NULL;
620 
621  // forward seed (w.r.t. p)
622  mxArray *Dp = NULL;
623  double *dp = NULL;
624 
625  // forward seed (w.r.t. u)
626  mxArray *Du = NULL;
627  double *du = NULL;
628 
629  // forward seed (w.r.t. w)
630  mxArray *Dw = NULL;
631  double *dw = NULL;
632 
633  // backward seed (2nd order)
634  mxArray *Bseed2 = NULL;
635  double *bseed2 = NULL;
636 
637  // forward seed (w.r.t. w) (2nd order)
638  mxArray *Dx2 = NULL;
639  double *dx2 = NULL;
640 
641  // forward seed (w.r.t. w) (2nd order)
642  mxArray *Dp2 = NULL;
643  double *dp2 = NULL;
644 
645  // forward seed (w.r.t. w) (2nd order)
646  mxArray *Du2 = NULL;
647  double *du2 = NULL;
648 
649  // forward seed (w.r.t. w) (2nd order)
650  mxArray *Dw2 = NULL;
651  double *dw2 = NULL;
652 
653  // defines if (error) messages shall be printed or not
654  mxArray *PrintLevel_ = NULL;
655  double *printLevel_ = NULL;
656 
657  // array containing the indices of the states whose trajectory shall be plotted
658  mxArray *PlotXTrajectory = NULL;
659  mxArray *PlotXaTrajectory = NULL;
660 
661  // defines if subplots are to be used
662  mxArray *UseSubplots = NULL;
663 
664  // tolerance of the integrator
665  mxArray *Jacobian = NULL;
666  double *jacobian = NULL;
667 
668  // OUTPUTS:
669  // -----------------------------------------------------------------
670 
671  // end values of differential and algebraic states
672  double *xEnd = NULL;
673  double *xaEnd = NULL;
674 
675  // integrator status
676  mxArray *Status = NULL;
677  double *statusPtr = NULL;
678 
679  // number of integrator steps
680  mxArray *NumberOfSteps = NULL;
681  double *numberOfSteps = NULL;
682 
683  // number of rejected integrator steps
684  mxArray *NumberOfRejectedSteps = NULL;
685  double *numberOfRejectedSteps = NULL;
686 
687  // step size
688  mxArray *GetStepSize = NULL;
689  double *getStepSize = NULL;
690 
691  // corrector tollerance
692  mxArray *CorrectorTolerance = NULL;
693  double *correctorTolerance = NULL;
694 
695  // x trajectory values
696  mxArray *XTrajectory = NULL;
697  double *xTrajectory = NULL;
698 
699  // xa trajectory values
700  mxArray *XaTrajectory = NULL;
701  double *xaTrajectory = NULL;
702 
703  // sensitivity w.r.t everything (in forward mode only)
704  mxArray *JJ = NULL;
705  double *jj = NULL;
706 
707  // sensitivity w.r.t the states (in backward mode only)
708  mxArray *Jx = NULL;
709  double *jx = NULL;
710 
711  // sensitivity w.r.t the parameters (in backward mode only)
712  mxArray *Jp = NULL;
713  double *jp = NULL;
714 
715  // sensitivity w.r.t the controls (in backward mode only)
716  mxArray *Ju = NULL;
717  double *ju = NULL;
718 
719  // sensitivity w.r.t the disturbances (in backward mode only)
720  mxArray *Jw = NULL;
721  double *jw = NULL;
722 
723  // second order sensitivity w.r.t everything (in forward mode 2 only)
724  mxArray *JJ2 = NULL;
725  double *jj2 = NULL;
726 
727  // second order sensitivity w.r.t the states (in backward mode only)
728  mxArray *Jx2 = NULL;
729  double *jx2 = NULL;
730 
731  // second order sensitivity w.r.t the parameters (in backward mode only)
732  mxArray *Jp2 = NULL;
733  double *jp2 = NULL;
734 
735  // second order sensitivity w.r.t the controls (in backward mode only)
736  mxArray *Ju2 = NULL;
737  double *ju2 = NULL;
738 
739  // second order sensitivity w.r.t the disturbances (in backward mode only)
740  mxArray *Jw2 = NULL;
741  double *jw2 = NULL;
742 
743  // second order sensitivity w.r.t the disturbances (in backward mode only)
744  mxArray *StorageResolution = NULL;
745  double *storageResolution = NULL;
746 
747 
748  // DIMENSIONS:
749  // ---------------------------- -------------------------------------
750  unsigned int nx = 0; // number of differential states
751  unsigned int nxa = 0; // number of algebraic states
752  unsigned int nu = 0; // number of controls
753  unsigned int np = 0; // number of parameters
754  unsigned int nw = 0; // number of disturbances
755  unsigned int nbDir = 0; // number of backward directions
756  unsigned int nfDir = 0; // number of forward directions
757  unsigned int nbDir2 = 0; // number of 2nd order backward directions
758  unsigned int nfDir2 = 0; // number of 2nd order forward directions
759 
760 
761  // I) CONSISTENCY CHECKS:
762  // ----------------------
763 
764  if ( ( nrhs < 4 ) || ( nrhs > 5 ) )
765  mexErrMsgTxt( "ERROR (ACADOintegrator): Invalid number of input arguments!\n Type 'help ACADOintegrators' for further information." );
766 
767  if ( ( nlhs < 1 ) || ( nlhs > 3 ) )
768  mexErrMsgTxt( "ERROR (ACADOintegrator): Invalid number of output arguments!\n Type 'help ACADOintegrators' for further information." );
769 
770 
771  // II. READING THE INPUT ARGUMENTS:
772  // --------------------------------
773 
774  if( !mxIsEmpty( prhs[0]) )
775  {
776  IntegratorSettings = prhs[0];
777  if ( !mxIsStruct( IntegratorSettings ) )
778  mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator settings defined!" );
779  }
780  else
781  mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator settings defined!" );
782 
783  if( !mxIsEmpty( prhs[1]) )
784  {
785  xStart = mxGetPr( prhs[1] );
786  nx = mxGetM( prhs[1] );
787 
788  if ( mxGetN( prhs[1] ) != 1 )
789  mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for differential state needs to be given as column vector." );
790  }
791  else
792  mexErrMsgTxt( "ERROR (ACADOintegrator): No initial value specified." );
793 
794  if ( nrhs == 4 )
795  {
796  // ODE system
797  // An ODE system is supposed when ACADOintegrators() is called with 4 arguments
798  isODE = BT_TRUE;
799  tStartIdx = 2;
800  tEndIdx = 3;
801  }
802  else
803  {
804  // DAE system, xaStart optional
805  isODE = BT_FALSE;
806  tStartIdx = 3;
807  tEndIdx = 4;
808 
809  if( !mxIsEmpty( prhs[2]) )
810  {
811  xaStart = mxGetPr( prhs[2] );
812  nxa = mxGetM( prhs[2] );
813 
814  if ( mxGetN( prhs[2] ) != 1 )
815  mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for algebraic state needs to be given as column vector." );
816  }
817  }
818 
819  if( !mxIsEmpty( prhs[tStartIdx]) )
820  {
821  tStart = mxGetPr( prhs[tStartIdx] );
822 
823  if ( isScalar( prhs[tStartIdx] ) == 0 )
824  mexErrMsgTxt( "ERROR (ACADOintegrator): Start time needs to be a scalar." );
825  }
826  else
827  mexErrMsgTxt( "ERROR (ACADOintegrator): No start time defined." );
828 
829 
830  if( !mxIsEmpty( prhs[tEndIdx]) )
831  {
832  tEnd = mxGetPr( prhs[tEndIdx] );
833 
834  if ( isScalar( prhs[tEndIdx] ) == 0 )
835  mexErrMsgTxt( "ERROR (ACADOintegrator): End time needs to be a scalar." );
836  }
837  else
838  mexErrMsgTxt( "ERROR (ACADOintegrator): No end time defined." );
839 
840 
841  // III. READING THE SETTINGS STRUCT:
842  // ---------------------------------
843 
844  ModelName = mxGetField( IntegratorSettings,0,"Model" );
845  IntegratorName = mxGetField( IntegratorSettings,0,"Integrator" );
846  Tol = mxGetField( IntegratorSettings,0,"Tolerance" );
847  aTol = mxGetField( IntegratorSettings,0,"AbsoluteTolerance" );
848  MaxNumStep = mxGetField( IntegratorSettings,0,"MaxNumberOfSteps" );
849  MinStep = mxGetField( IntegratorSettings,0,"MinimumStepSize" );
850  InitialStep = mxGetField( IntegratorSettings,0,"InitialStepSize" );
851  MaxStep = mxGetField( IntegratorSettings,0,"MaximumStepSize" );
852  CorrectorTolerance = mxGetField( IntegratorSettings,0,"CorrectorTolerance" );
853  StepTuning = mxGetField( IntegratorSettings,0,"StepSizeTuning" );
854  LinearAlgebraSolver = mxGetField( IntegratorSettings,0,"LinearAlgebraSolver" );
855  U = mxGetField( IntegratorSettings,0,"u" );
856  P = mxGetField( IntegratorSettings,0,"p" );
857  W = mxGetField( IntegratorSettings,0,"w" );
858  DxInit = mxGetField( IntegratorSettings,0,"dxInit" );
859  SensitivityMode = mxGetField( IntegratorSettings,0,"SensitivityMode" );
860  Bseed = mxGetField( IntegratorSettings,0,"mu" );
861  Dx = mxGetField( IntegratorSettings,0,"lambdaX" );
862  Du = mxGetField( IntegratorSettings,0,"lambdaU" );
863  Dp = mxGetField( IntegratorSettings,0,"lambdaP" );
864  Dw = mxGetField( IntegratorSettings,0,"lambdaW" );
865  Bseed2 = mxGetField( IntegratorSettings,0,"mu2" );
866  Dx2 = mxGetField( IntegratorSettings,0,"lambda2X" );
867  Du2 = mxGetField( IntegratorSettings,0,"lambda2U" );
868  Dp2 = mxGetField( IntegratorSettings,0,"lambda2P" );
869  Dw2 = mxGetField( IntegratorSettings,0,"lambda2W" );
870  PrintLevel_ = mxGetField( IntegratorSettings,0,"PrintLevel" );
871  PlotXTrajectory = mxGetField( IntegratorSettings,0,"PlotXTrajectory" );
872  PlotXaTrajectory = mxGetField( IntegratorSettings,0,"PlotXaTrajectory" );
873  UseSubplots = mxGetField( IntegratorSettings,0,"UseSubplots" );
874  Jacobian = mxGetField( IntegratorSettings,0,"Jacobian" );
875  StorageResolution = mxGetField( IntegratorSettings,0,"StorageResolution" );
876 
877  if( !mxIsEmpty(IntegratorName) )
878  {
879  if ( !mxIsChar(IntegratorName) )
880  mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator specified." );
881 
882  integratorName = mxArrayToString( IntegratorName );
883  }
884  else
885  mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator specified." );
886 
887  if( !mxIsEmpty(MaxNumStep) )
888  {
889  maxNumStep = mxGetPr( MaxNumStep );
890 
891  if ( isScalar( MaxNumStep ) == 0 )
892  mexErrMsgTxt( "ERROR (ACADOintegrator): Maximum number of steps needs to be a scalar." );
893  }
894 
895  if( !mxIsEmpty(MinStep) )
896  {
897  minStep = mxGetPr( MinStep );
898 
899  if ( isScalar( MinStep ) == 0 )
900  mexErrMsgTxt( "ERROR (ACADOintegrator): Minimum step size must be a scalar ." );
901  }
902 
903  if( !mxIsEmpty(InitialStep) )
904  {
905  initialStep = mxGetPr( InitialStep );
906 
907  if ( isScalar( InitialStep ) == 0 )
908  mexErrMsgTxt( "ERROR (ACADOintegrator): Initial step size must be a scalar ." );
909  }
910 
911  if( !mxIsEmpty(CorrectorTolerance) )
912  {
913  correctorTolerance = mxGetPr( CorrectorTolerance );
914 
915  if ( isScalar( CorrectorTolerance ) == 0 )
916  mexErrMsgTxt( "ERROR (ACADOintegrator): Corrector Tolerance must be a scalar ." );
917  }
918 
919  if( !mxIsEmpty(MaxStep) )
920  {
921  maxStep = mxGetPr( MaxStep );
922 
923  if ( isScalar( MaxStep ) == 0 )
924  mexErrMsgTxt( "ERROR (ACADOintegrator): Maximum step size must be a scalar ." );
925  }
926 
927  if( !mxIsEmpty(StepTuning) )
928  {
929  stepTuning = mxGetPr( StepTuning );
930 
931  if ( isScalar( StepTuning ) == 0 )
932  mexErrMsgTxt( "ERROR (ACADOintegrator): StepTuning size must be a scalar ." );
933  }
934 
935  if( !mxIsEmpty(LinearAlgebraSolver) )
936  {
937  if ( !mxIsChar(LinearAlgebraSolver) )
938  mexErrMsgTxt( "ERROR (ACADOintegrator): No linear algebra solver specified." );
939 
940  linearAlgebraSolver = mxArrayToString( LinearAlgebraSolver );
941  }
942  else
943  mexErrMsgTxt( "ERROR (ACADOintegrator): No linear algebra solver specified." );
944 
945  if( !mxIsEmpty(Tol) )
946  {
947  tol = mxGetPr( Tol );
948 
949  if ( isScalar( Tol ) == 0 )
950  mexErrMsgTxt( "ERROR (ACADOintegrator): Tolerance needs to be a scalar." );
951  }
952 
953  if( !mxIsEmpty(aTol) )
954  {
955  atol = mxGetPr( aTol );
956 
957  if ( isScalar( aTol ) == 0 )
958  mexErrMsgTxt( "ERROR (ACADOintegrator): Absolute Tolerance needs to be a scalar." );
959  }
960 
961  if( !mxIsEmpty(U) )
962  {
963  u = mxGetPr( U );
964  nu = mxGetM( U );
965 
966  if ( mxGetN( U ) != 1 )
967  mexErrMsgTxt( "ERROR (ACADOintegrator): Controls need to be given as column vector." );
968  }
969 
970  if( !mxIsEmpty(P) )
971  {
972  p = mxGetPr( P );
973  np = mxGetM( P );
974 
975  if ( mxGetN( P ) != 1 )
976  mexErrMsgTxt( "ERROR (ACADOintegrator): Parameters need to be given as column vector." );
977  }
978 
979  if( !mxIsEmpty(W) )
980  {
981  w = mxGetPr( W );
982  nw = mxGetM( W );
983 
984  if ( mxGetN( W ) != 1 )
985  mexErrMsgTxt( "ERROR (ACADOintegrator): Disturbances need to be given as column vector." );
986  }
987 
988  if( !mxIsEmpty(DxInit) )
989  {
990  dxInit = mxGetPr( DxInit );
991  if ( mxGetM( DxInit ) != nx )
992  {
993  clearAllGlobals( );
994  mexErrMsgTxt( "ERROR (ACADOintegrator): Initial value for differential states derivatives has invalid dimensions." );
995  }
996 
997  if ( mxGetN( DxInit ) != 1 )
998  {
999  clearAllGlobals( );
1000  mexErrMsgTxt( "ERROR (ACADOintegrator): Initial value for differential states derivatives needs to be given as column vector." );
1001  }
1002  }
1003 
1004  if( !mxIsEmpty(Bseed) )
1005  {
1006  bseed = mxGetPr( Bseed );
1007 
1008  if ( mxGetN( Bseed ) != nx )
1009  mexErrMsgTxt( "ERROR (ACADOintegrator): Backward seed has incompatible dimensions." );
1010 
1011  nbDir = mxGetM( Bseed );
1012  }
1013 
1014  if( !mxIsEmpty(Dx) )
1015  {
1016  dx = mxGetPr( Dx );
1017 
1018  if ( mxGetM( Dx ) != nx )
1019  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1020 
1021  nfDir = mxGetN( Dx );
1022  }
1023 
1024  if( !mxIsEmpty(Du) )
1025  {
1026  du = mxGetPr( Du );
1027 
1028  if ( mxGetM( Du ) != nx )
1029  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1030 
1031  if ( ( nfDir != 0 ) && ( nfDir != mxGetN( Du ) ) )
1032  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1033 
1034  nfDir = mxGetN( Du );
1035  }
1036 
1037  if( !mxIsEmpty(Dp) )
1038  {
1039  dp = mxGetPr( Dp );
1040 
1041  if ( mxGetM( Dp ) != nx )
1042  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1043 
1044  if ( ( nfDir != 0 ) && ( nfDir != mxGetN( Dp ) ) )
1045  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1046 
1047  nfDir = mxGetN( Dp );
1048  }
1049 
1050  if( !mxIsEmpty(Dw) )
1051  {
1052  dw = mxGetPr( Dw );
1053 
1054  if ( mxGetM( Dw ) != nx )
1055  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1056 
1057  if ( ( nfDir != 0 ) && ( nfDir != mxGetN( Dw ) ) )
1058  mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
1059 
1060  nfDir = mxGetN( Dw );
1061  }
1062 
1063  if( !mxIsEmpty(Bseed2) )
1064  {
1065  bseed2 = mxGetPr( Bseed2 );
1066 
1067  if ( mxGetN( Bseed2 ) != nx )
1068  mexErrMsgTxt( "ERROR (ACADOintegrator): Second backward seed has incompatible dimensions." );
1069 
1070  nbDir2 = mxGetM( Bseed2 );
1071  }
1072 
1073  if( !mxIsEmpty(Dx2) )
1074  {
1075  dx2 = mxGetPr( Dx2 );
1076 
1077  if ( mxGetM( Dx2 ) != nx )
1078  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1079 
1080  nfDir2 = mxGetN( Dx2 );
1081  }
1082 
1083  if( !mxIsEmpty(Du2) )
1084  {
1085  du2 = mxGetPr( Du2 );
1086 
1087  if ( mxGetM( Du2 ) != nx )
1088  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1089 
1090  if ( ( nfDir2 != 0 ) && ( nfDir2 != mxGetN( Du2 ) ) )
1091  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1092 
1093  nfDir2 = mxGetN( Du2 );
1094  }
1095 
1096  if( !mxIsEmpty(Dp2) )
1097  {
1098  dp2 = mxGetPr( Dp2 );
1099 
1100  if ( mxGetM( Dp2 ) != nx )
1101  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1102 
1103  if ( ( nfDir2 != 0 ) && ( nfDir2 != mxGetN( Dp2 ) ) )
1104  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1105 
1106  nfDir2 = mxGetN( Dp2 );
1107  }
1108 
1109  if( !mxIsEmpty(Dw2) )
1110  {
1111  dw2 = mxGetPr( Dw2 );
1112 
1113  if ( mxGetM( Dw2 ) != nx )
1114  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1115 
1116  if ( ( nfDir2 != 0 ) && ( nfDir2 != mxGetN( Dw2 ) ) )
1117  mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
1118 
1119  nfDir2 = mxGetN( Dw2 );
1120  }
1121 
1122  if( !mxIsEmpty(PrintLevel_) )
1123  printLevel_ = mxGetPr( PrintLevel_ );
1124 
1125 
1126  // Set link to Jacobian
1127  if ( mxIsCell(Jacobian) )
1128  {
1129  if ( ( mxGetN( Jacobian ) == 1 ) && ( mxGetCell( Jacobian,0 ) != NULL ) )
1130  {
1131  ModelFcn_jac = mxDuplicateArray( mxGetCell( Jacobian,0 ) );
1132 
1133  if ( isFunctionHandle(ModelFcn_jac) == 0 )
1134  mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian: No valid dynamic model specified." );
1135  }else{
1136  mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian: Could not set Jacobian" );
1137 
1138  }
1139  }
1140 
1141  if( !mxIsEmpty(StorageResolution) )
1142  {
1143  storageResolution = mxGetPr( StorageResolution );
1144 
1145  if ( isScalar( StorageResolution ) == 0 )
1146  mexErrMsgTxt( "ERROR (ACADOintegrator): Maximum number of steps needs to be a scalar." );
1147  }
1148 
1149 
1150 
1151  DifferentialEquation* f;
1152  f = new DifferentialEquation();
1153 
1154  CFunction *cModel = NULL;
1155 
1156 
1157  if( !mxIsEmpty(ModelName) )
1158  {
1159  if ( mxGetM( ModelName ) != 1 )
1160  mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
1161 
1162  if ( mxIsChar(ModelName) )
1163  {
1164  modelName = mxArrayToString( ModelName );
1165  f = allocateDifferentialEquation( modelName,integratorName );
1166 
1167  if ( f == NULL )
1168  mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
1169 
1170  if ( f->getNumAlgebraicEquations( ) != (int) nxa )
1171  mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for algebraic states has invalid dimension." );
1172 
1173  if ( f->getNumDynamicEquations( ) != (int) nx ){
1174  printf("Differentialstate dimension: %d , input dim: %d \n", f->getNumDynamicEquations( ), (int)nx );
1175  mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for differential state has invalid dimension." );
1176  }
1177  }
1178  else
1179  {
1180  if ( mxIsCell(ModelName) )
1181  {
1182  // get f
1183  if ( ( mxGetN( ModelName ) > 0 ) && ( mxGetCell( ModelName,0 ) != NULL ) )
1184  {
1185  ModelFcn_f = mxDuplicateArray( mxGetCell( ModelName,0 ) );
1186 
1187  if ( isFunctionHandle(ModelFcn_f) == 0 )
1188  mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
1189  }
1190  else
1191  mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
1192 
1193 
1194 
1195  // if dynamic model is given as Matlab function handle, allocate
1196  // dynamic model as C function with corresponding dimensions
1197 
1198  ModelFcnT = mxCreateDoubleMatrix( 1, 1,mxREAL );
1199  ModelFcnX = mxCreateDoubleMatrix( nx, 1,mxREAL );
1200  ModelFcnXA = mxCreateDoubleMatrix( nxa,1,mxREAL );
1201  ModelFcnDX = mxCreateDoubleMatrix( nx, 1,mxREAL );
1202  ModelFcnU = mxCreateDoubleMatrix( nu, 1,mxREAL );
1203  ModelFcnP = mxCreateDoubleMatrix( np, 1,mxREAL );
1204  ModelFcnW = mxCreateDoubleMatrix( nw, 1,mxREAL );
1205 
1206  modelFcnNX = nx;
1207  modelFcnNXA = nxa;
1208  modelFcnNDX = nx;
1209  modelFcnNP = np;
1210  modelFcnNU = nu;
1211  modelFcnNW = nw;
1212 
1213 
1214  IntermediateState is( 1+modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW);
1215  int j = 0;
1216 
1217  TIME t;
1218  is(j) = t; j++;
1219 
1220  if (modelFcnNX > 0){
1221  DifferentialState x("",modelFcnNX,1);
1222  for( i=0; i<modelFcnNX; ++i ){
1223  is(j) = x(i); j++;
1224  }
1225  }
1226 
1227  if (modelFcnNXA > 0){
1228  AlgebraicState ax("",modelFcnNXA,1);
1229  for( i=0; i<modelFcnNXA; ++i ){
1230  is(j) = ax(i); j++;
1231  }
1232  }
1233 
1234  if (modelFcnNU > 0){
1235  Control u("",modelFcnNU,1);
1236  for( i=0; i<modelFcnNU; ++i ){
1237  is(j) = u(i); j++;
1238  }
1239  }
1240 
1241  if (modelFcnNP > 0){
1242  Parameter p("",modelFcnNP,1);
1243  for( i=0; i<modelFcnNP; ++i ){
1244  is(j) = p(i); j++;
1245  }
1246  }
1247 
1248  if (modelFcnNW > 0){
1249  Disturbance w("",modelFcnNW,1);
1250  for( i=0; i<modelFcnNW; ++i ){
1251  is(j) = w(i); j++;
1252  }
1253  }
1254 
1255 
1256  //mexprintf ("nxn:%d nxa:%d np:%d nu:%d nw:%d -- j=%d\n", nx, nxa, np, nu, nw, j);
1257 
1258 
1259  if ( isODE == BT_TRUE )
1260  {
1261  if( !mxIsEmpty(Jacobian) )
1262  {
1263  CFunction cLinkModel(nx, genericODE, genericJacobian, genericJacobian);
1264  *f << cLinkModel(is);
1265  }
1266  else
1267  {
1268  CFunction cLinkModel(nx, genericODE);
1269  *f << cLinkModel(is);
1270  }
1271  }
1272  else
1273  {
1274  CFunction cLinkModel(nx+nxa, genericDAE);
1275  *f << cLinkModel(is);
1276  }
1277 
1278 
1279 
1280  //printf("after link model \n");
1281 
1282  }
1283  else
1284  mexErrMsgTxt( "ERROR (ACADOintegrator): No dynamic model specified." );
1285  }
1286  }
1287  else
1288  mexErrMsgTxt( "ERROR (ACADOintegrator): No dynamic model specified." );
1289 
1290  if( !mxIsEmpty(SensitivityMode) )
1291  {
1292  if ( !mxIsChar(SensitivityMode) )
1293  {
1294  delete f; delete cModel; clearAllGlobals( );
1295  mexErrMsgTxt( "ERROR (ACADOintegrator): No valid sensitivity mode specified." );
1296  }
1297 
1298  sensitivityMode = mxArrayToString( SensitivityMode );
1299 
1300  if ( ( modelName == NULL ) && ( strcmp(sensitivityMode,"AD_BACKWARD") == 0 ) )
1301  {
1302  delete f; delete cModel; clearAllGlobals( );
1303  mexErrMsgTxt( "ERROR (ACADOintegrator): Adjoint sensitivity generation via function handle not possible." );
1304  }
1305  }
1306 
1307 
1308  // set flags indicating of ODE/DAE is given and
1309  // if output struct shall be setup
1310  if ( isODE == BT_TRUE )
1311  {
1312  if ( nlhs == 3 )
1313  {
1314  delete f; delete cModel; clearAllGlobals( );
1315  mexErrMsgTxt( "ERROR (ACADOintegrator): Invalid number of output arguments." );
1316  }
1317 
1318  if ( nlhs == 2 )
1319  outputIdx = 1;
1320  }
1321  else
1322  {
1323  if ( nlhs >= 2 )
1324  xaEndIdx = 1;
1325 
1326  if ( nlhs == 3 )
1327  outputIdx = 2;
1328  }
1329 
1330 
1331  // IV. CREATE OUTPUT VECTORS AND ASSIGN POINTERS:
1332  // ----------------------------------------------
1333 
1334  double status = 0.0;
1335 
1336  plhs[0] = mxCreateDoubleMatrix( nx,1,mxREAL );
1337  xEnd = mxGetPr( plhs[0] );
1338 
1339  if ( xaEndIdx > 0 )
1340  {
1341  plhs[xaEndIdx] = mxCreateDoubleMatrix( nxa,1,mxREAL );
1342  xaEnd = mxGetPr( plhs[xaEndIdx] );
1343  }
1344 
1345  if ( outputIdx > 0 )
1346  {
1347  // create outputs struct
1348  // DO NOT forget to document all changes done here within integratorsOutputs.m
1349  const char* outputFieldNames[] = { "Status",
1350  "NumberOfSteps",
1351  "NumberOfRejectedSteps",
1352  "xTrajectory",
1353  "xaTrajectory",
1354  "J",
1355  "Jx",
1356  "Ju",
1357  "Jp",
1358  "Jw",
1359  "J2",
1360  "J2x",
1361  "J2u",
1362  "J2p",
1363  "J2w",
1364  "GetStepSize"
1365  };
1366 
1367  plhs[outputIdx] = mxCreateStructMatrix( 1,1,16,outputFieldNames );
1368  }
1369 
1370 
1371  // store trajectory either if output struct or if plot is desired
1372  BooleanType freezeTrajectory;
1373 
1374  if ( ( outputIdx > 0 ) || ( !mxIsEmpty( PlotXTrajectory ) ) || ( !mxIsEmpty( PlotXaTrajectory ) ) )
1375  freezeTrajectory = BT_TRUE;
1376  else
1377  freezeTrajectory = BT_FALSE;
1378 
1379 
1380  int *maxNumStepInt = 0;
1381  if ( !mxIsEmpty(MaxNumStep) )
1382  {
1383  maxNumStepInt = new int;
1384  maxNumStepInt[0] = (int) *maxNumStep;
1385  }
1386 
1387 
1388  // V. CALL THE INTEGRATION ROUTINE:
1389  // --------------------------------
1390 
1391  // allocate integrator object
1392  Integrator* integrator = allocateIntegrator( integratorName,f );
1393 
1394  if ( integrator == NULL )
1395  {
1396  delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1397  mexErrMsgTxt( "ERROR (ACADOintegrator): The specified integrator has not been found.\n");
1398  }
1399 
1400  clearAllStaticCounters( );
1401 
1402 
1403  if( maxNumStep != 0 )
1404  integrator->set( MAX_NUM_INTEGRATOR_STEPS,*maxNumStepInt );
1405 
1406  if( minStep != 0 )
1407  integrator->set( MIN_INTEGRATOR_STEPSIZE,*minStep );
1408 
1409  if( initialStep != 0 )
1410  integrator->set( INITIAL_INTEGRATOR_STEPSIZE,*initialStep );
1411 
1412  if( maxStep != 0 )
1413  integrator->set( MAX_INTEGRATOR_STEPSIZE,*maxStep );
1414 
1415  if( tol != 0 )
1416  integrator->set(INTEGRATOR_TOLERANCE,*tol );
1417 
1418  if( atol != 0 )
1419  integrator->set(ABSOLUTE_TOLERANCE,*atol );
1420 
1421  if( stepTuning != 0 )
1422  integrator->set( STEPSIZE_TUNING,*stepTuning );
1423 
1424  if( correctorTolerance != 0 )
1425  integrator->set( CORRECTOR_TOLERANCE,*correctorTolerance );
1426 
1427 
1428 
1429 // integrator->set( LINEAR_ALGEBRA_SOLVER,(int)LAS_HOUSEHOLDER_METHOD );
1430 // if( linearAlgebraSolver != 0 )
1431 // {
1432 // if ( strcmp(linearAlgebraSolver,"sparse") == 0 )
1433 // {
1434 // if ( strcmp(integratorName,"BDF") == 0 )
1435 // {
1436 // integrator->set( LINEAR_ALGEBRA_SOLVER,(int)LAS_SPARSE_CONJUGATE_GRADIENT_METHOD );
1437 // }
1438 // else
1439 // {
1440 // delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1441 // mexErrMsgTxt( "ERROR (ACADOintegrator): The specified integrator has not been found.\n");
1442 // }
1443 // }
1444 // }
1445 
1446 
1447  if( dxInit != NULL )
1448  integrator->setDxInitialization( dxInit );
1449 
1450  if( freezeTrajectory == BT_TRUE )
1451  integrator->freezeAll( );
1452 
1453  //printf("before integrate\n");
1454 
1455  Grid timeInterval( *tStart, *tEnd, (int) *storageResolution );
1456  returnValue returnvalue = integrator->integrate( timeInterval, xStart,xaStart,p,u,w );
1457 
1458  //printf("after integrate \n");
1459 
1460  if ( returnvalue != SUCCESSFUL_RETURN )
1461  {
1462  status = -1.0;
1463  plotErrorMessage( returnvalue,printLevel_ );
1464  }
1465 
1466  // assign end value for states
1467  //double* xEndFull = new double[nx+nxa];
1468  DVector xEnd_, xaEnd_;
1469  integrator->getX ( xEnd_ );
1470  integrator->getXA( xaEnd_ );
1471 
1472  for( i=0; i<nx; ++i )
1473  xEnd[i] = xEnd_(i);
1474 
1475  if ( xaEndIdx > 0 )
1476  {
1477  for( i=0; i<nxa; ++i )
1478  xaEnd[i] = xaEnd_(i);
1479  }
1480 
1481  //printf("after xaEnd, before sensitivities \n");
1482 
1483  // SENSITIVITIES:
1484  // -----------------------------------
1485  if ( ( !mxIsEmpty(SensitivityMode) ) && ( status >= 0.0 ) )
1486  {
1487  //printf("\n !! SensitivityMode is currently disabled !! \n\n");
1488  // forward mode
1489  if( strcmp(sensitivityMode,"AD_FORWARD") == 0 )
1490  {
1491  if ( ( dx == NULL ) && ( du == NULL ) && ( dp == NULL ) && ( dw == NULL ) )
1492  {
1493  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1494  mexErrMsgTxt( "ERROR (ACADOintegrator): The forward seed is not defined." );
1495  }
1496 
1497  // setup seed and sensitvity matrices
1498  DMatrix D_x( nx,nfDir );
1499  DMatrix D_p( np,nfDir );
1500  DMatrix D_u( nu,nfDir );
1501  DMatrix D_w( nw,nfDir );
1502 
1503  if( dx != NULL ) {
1504  D_x = Eigen::Map<DMatrix>(dx,nx,nfDir);
1505  D_x.transposeInPlace();
1506  }
1507  else
1508  D_x.setZero();
1509 
1510  if( du != NULL ) {
1511  D_u = Eigen::Map<DMatrix>(du,nu,nfDir);
1512  D_u.transposeInPlace();
1513  }
1514  else
1515  D_u.setZero();
1516 
1517  if( dp != NULL ) {
1518  D_p = Eigen::Map<DMatrix>(dp,np,nfDir);
1519  D_p.transposeInPlace();
1520  }
1521  else
1522  D_p.setZero();
1523 
1524  if( dw != NULL ) {
1525  D_w = Eigen::Map<DMatrix>(dw,nw,nfDir);
1526  D_w.transposeInPlace();
1527  }
1528  else
1529  D_w.setZero();
1530 
1531  DMatrix J( nx+nxa,nfDir );
1532 
1533  // determine forward sensitivities
1534 
1535  int run1;
1536 
1537  for( run1 = 0; run1 < nfDir; run1++ ){
1538 
1539  DVector DXX = D_x.getCol( run1 );
1540  DVector DPP = D_p.getCol( run1 );
1541  DVector DUU = D_u.getCol( run1 );
1542  DVector DWW = D_w.getCol( run1 );
1543 
1544  integrator->setForwardSeed( 1, DXX, DPP, DUU, DWW );
1545  returnvalue = integrator->integrateSensitivities( );
1546  if( returnvalue != SUCCESSFUL_RETURN )
1547  {
1548  status = -1.0;
1549  plotErrorMessage( returnvalue,printLevel_ );
1550  }
1551  else
1552  {
1553  DVector JJ(nx+nxa);
1554  integrator->getForwardSensitivities( JJ, 1 );
1555  J.setCol( run1, JJ );
1556  }
1557  }
1558 
1559  // write sensitivity matrices into output struct (if given)
1560  if ( returnvalue == SUCCESSFUL_RETURN && outputIdx > 0 )
1561  {
1562  JJ = mxCreateDoubleMatrix( nx+nxa,nfDir,mxREAL );
1563  jj = mxGetPr( JJ );
1564  DMatrix jj_tmp = Eigen::Map<DMatrix>(jj, J.rows(), J.cols()); jj_tmp = J.transpose(); jj = jj_tmp.data();
1565  mxSetField( plhs[outputIdx],0,"J",JJ );
1566  }
1567  }
1568 
1569  // backward mode
1570  if( strcmp(sensitivityMode,"AD_BACKWARD") == 0 )
1571  {
1572  if( bseed == NULL )
1573  {
1574  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1575  mexErrMsgTxt( "ERROR (ACADOintegrator): The backward seed is not defined." );
1576  }
1577 
1578  // setup seed and sensitvity matrices
1579  DMatrix xSeed( nbDir,nx );
1580  xSeed = Eigen::Map<DMatrix>(bseed,nbDir,nx);
1581  xSeed.transposeInPlace();
1582 
1583  DMatrix J_x( nbDir,nx+nxa );
1584  DMatrix J_u( nbDir,nu );
1585  DMatrix J_p( nbDir,np );
1586  DMatrix J_w( nbDir,nw );
1587 
1588 
1589  int run1;
1590  for( run1 = 0; run1 < nbDir; run1++ ){
1591 
1592  DVector XSEED = xSeed.getRow(run1);
1593 
1594  // determine backward sensitivities
1595  integrator->setBackwardSeed( 1, XSEED );
1596  returnvalue = integrator->integrateSensitivities( );
1597  if( returnvalue != SUCCESSFUL_RETURN )
1598  {
1599  status = -1.0;
1600  plotErrorMessage( returnvalue,printLevel_ );
1601  }
1602  else
1603  {
1604  DVector JXX(nx+nxa), JPP(np), JUU(nu), JWW(nw);
1605 
1606  integrator->getBackwardSensitivities( JXX, JPP, JUU, JWW, 1 );
1607 
1608  J_x.setRow( run1, JXX );
1609  J_p.setRow( run1, JPP );
1610  J_u.setRow( run1, JUU );
1611  J_w.setRow( run1, JWW );
1612  }
1613  }
1614 
1615 
1616  if( returnvalue == SUCCESSFUL_RETURN ){
1617  // write sensitivity matrices into output struct
1618  if ( outputIdx > 0 )
1619  {
1620  Jx = mxCreateDoubleMatrix( nbDir,nx+nxa,mxREAL );
1621  jx = mxGetPr( Jx );
1622  DMatrix jx_tmp = Eigen::Map<DMatrix>(jx, J_x.rows(), J_x.cols()); jx_tmp = J_x.transpose(); jx = jx_tmp.data();
1623  mxSetField( plhs[outputIdx],0,"Jx",Jx );
1624 
1625  if ( nu > 0 )
1626  {
1627  Ju = mxCreateDoubleMatrix( nbDir,nu,mxREAL );
1628  ju = mxGetPr( Ju );
1629  DMatrix ju_tmp = Eigen::Map<DMatrix>(ju, J_u.rows(), J_u.cols()); ju_tmp = J_u.transpose(); ju = ju_tmp.data();
1630  mxSetField( plhs[outputIdx],0,"Ju",Ju );
1631  }
1632 
1633  if( np > 0 )
1634  {
1635  Jp = mxCreateDoubleMatrix( nbDir,np,mxREAL );
1636  jp = mxGetPr( Jp );
1637  DMatrix jp_tmp = Eigen::Map<DMatrix>(jp, J_p.rows(), J_p.cols()); jp_tmp = J_p.transpose(); jp = jp_tmp.data();
1638  mxSetField( plhs[outputIdx],0,"Jp",Jp );
1639  }
1640 
1641  if( nw > 0 )
1642  {
1643  Jw = mxCreateDoubleMatrix( nbDir,nw,mxREAL );
1644  jw = mxGetPr( Jw );
1645  DMatrix jw_tmp = Eigen::Map<DMatrix>(jw, J_w.rows(), J_w.cols()); jw_tmp = J_w.transpose(); jw = jw_tmp.data();
1646  mxSetField( plhs[outputIdx],0,"Jw",Jw );
1647  }
1648  }
1649  }
1650  }
1651 
1652 
1653  // forward mode (2nd derivatives)
1654  if ( strcmp(sensitivityMode,"AD_FORWARD2") == 0 )
1655  {
1656  if ( ( dx == NULL ) && ( du == NULL ) && ( dp == NULL ) && ( dw == NULL ) )
1657  {
1658  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1659  mexErrMsgTxt( "ERROR (integrator): The forward seed is not defined." );
1660  }
1661 
1662  if ( ( dx2 == NULL ) && ( du2 == NULL ) && ( dp2 == NULL ) && ( dw2 == NULL ) )
1663  {
1664  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1665  mexErrMsgTxt( "ERROR (integrator): The second order forward seed is not defined." );
1666  }
1667 
1668  if ( nfDir > 1 )
1669  {
1670  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1671  mexErrMsgTxt( "ERROR (integrator): More than one first order seed is not allowed in second order mode - please compute the required directions in a loop." );
1672  }
1673 
1674  // setup seed and sensitvity matrices (first order)
1675  DMatrix D_x( nx,nfDir );
1676  DMatrix D_p( np,nfDir );
1677  DMatrix D_u( nu,nfDir );
1678  DMatrix D_w( nw,nfDir );
1679 
1680  if( dx != NULL ) {
1681  D_x = Eigen::Map<DMatrix>(dx,nx,nfDir);
1682  D_x.transposeInPlace();
1683  }
1684  else
1685  D_x.setZero();
1686 
1687  if( du != NULL ) {
1688  D_u = Eigen::Map<DMatrix>(du,nu,nfDir);
1689  D_u.transposeInPlace();
1690  }
1691  else
1692  D_u.setZero();
1693 
1694  if( dp != NULL ) {
1695  D_p = Eigen::Map<DMatrix>(dp,np,nfDir);
1696  D_p.transposeInPlace();
1697  }
1698  else
1699  D_p.setZero();
1700 
1701  if( dw != NULL ) {
1702  D_w = Eigen::Map<DMatrix>(dw,nw,nfDir);
1703  D_w.transposeInPlace();
1704  }
1705  else
1706  D_w.setZero();
1707 
1708  DMatrix J( nx+nxa,nfDir );
1709 
1710  // setup seed and sensitvity matrices (second order)
1711  DMatrix D_x2( nx,nfDir2 );
1712  DMatrix D_p2( np,nfDir2 );
1713  DMatrix D_u2( nu,nfDir2 );
1714  DMatrix D_w2( nw,nfDir2 );
1715 
1716  if( dx != NULL ) {
1717  D_x2 = Eigen::Map<DMatrix>(dx2,nx,nfDir2);
1718  D_x2.transposeInPlace();
1719  }
1720  else
1721  D_x2.setZero();
1722 
1723  if( du != NULL ) {
1724  D_u2 = Eigen::Map<DMatrix>(du2,nu,nfDir2);
1725  D_u2.transposeInPlace();
1726  }
1727  else
1728  D_u2.setZero();
1729 
1730  if( dp2 != NULL ) {
1731  D_p2 = Eigen::Map<DMatrix>(dp2,np,nfDir2);
1732  D_p2.transposeInPlace();
1733  }
1734  else
1735  D_p2.setZero();
1736 
1737  if( dw2 != NULL ) {
1738  D_w2 = Eigen::Map<DMatrix>(dw2,nw,nfDir2);
1739  D_w2.transposeInPlace();
1740  }
1741  else
1742  D_w2.setZero();
1743 
1744  DMatrix J2( nx+nxa,nfDir2 );
1745 
1746 
1747  int run1, run2;
1748 
1749  for( run1 = 0; run1 < nfDir; run1++ ){
1750 
1751  DVector DXX = D_x.getCol( run1 );
1752  DVector DPP = D_p.getCol( run1 );
1753  DVector DUU = D_u.getCol( run1 );
1754  DVector DWW = D_w.getCol( run1 );
1755 
1756  integrator->setForwardSeed( 1, DXX, DPP, DUU, DWW );
1757  returnvalue = integrator->integrateSensitivities( );
1758 
1759  if( returnvalue != SUCCESSFUL_RETURN )
1760  {
1761  status = -1.0;
1762  plotErrorMessage( returnvalue,printLevel_ );
1763  }
1764  else
1765  {
1766  DVector JJ(nx+nxa);
1767  integrator->getForwardSensitivities( JJ, 1 );
1768  J.setCol( run1, JJ );
1769  }
1770 
1771  for( run2 = 0; run2 < nfDir2; run2++ ){
1772 
1773  DVector DXX2 = D_x2.getCol( run2 );
1774  DVector DPP2 = D_p2.getCol( run2 );
1775  DVector DUU2 = D_u2.getCol( run2 );
1776  DVector DWW2 = D_w2.getCol( run2 );
1777 
1778  // determine forward sensitivities
1779  integrator->setForwardSeed( 2, DXX2, DPP2, DUU2, DWW2 );
1780  returnvalue = integrator->integrateSensitivities( );
1781 
1782  if( returnvalue != SUCCESSFUL_RETURN )
1783  {
1784  status = -1.0;
1785  plotErrorMessage( returnvalue,printLevel_ );
1786  }
1787  else
1788  {
1789  DVector JJ2(nx+nxa);
1790  integrator->getForwardSensitivities( JJ2, 2 );
1791  J2.setCol( run2, JJ2 );
1792  }
1793  }
1794  }
1795 
1796  // write sensitivity matrices into output struct (if given)
1797  if ( returnvalue == SUCCESSFUL_RETURN && outputIdx > 0 )
1798  {
1799  JJ = mxCreateDoubleMatrix( nx+nxa,nfDir,mxREAL );
1800  jj = mxGetPr( JJ );
1801  DMatrix jj_tmp = Eigen::Map<DMatrix>(jj, J.rows(), J.cols()); jj_tmp = J.transpose(); jj = jj_tmp.data();
1802  mxSetField( plhs[outputIdx],0,"J",JJ );
1803 
1804  JJ2 = mxCreateDoubleMatrix( nx+nxa,nfDir2,mxREAL );
1805  jj2 = mxGetPr( JJ2 );
1806  DMatrix jj2_tmp = Eigen::Map<DMatrix>(jj2, J2.rows(), J2.cols()); jj2_tmp = J2.transpose(); jj2 = jj2_tmp.data();
1807  mxSetField( plhs[outputIdx],0,"J2",JJ2 );
1808  }
1809  }
1810 
1811 
1812  // mixed forward-backward mode (2nd derivatives)
1813  if ( strcmp(sensitivityMode,"AD_FORWARD_BACKWARD") == 0 )
1814  {
1815  if ( ( dx == NULL ) && ( du == NULL ) && ( dp == NULL ) && ( dw == NULL ) )
1816  {
1817  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1818  mexErrMsgTxt( "ERROR (integrator): The forward seed is not defined." );
1819  }
1820 
1821  if( bseed2 == NULL )
1822  {
1823  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1824  mexErrMsgTxt( "ERROR (integrator): The second order backward seed is not defined." );
1825  }
1826 
1827  if( nfDir > 1 )
1828  {
1829  delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
1830  mexErrMsgTxt( "ERROR (integrator): More than one first order seed is not allowed in second order mode - please compute the required directions in a loop." );
1831  }
1832 
1833  // setup seed and sensitvity matrices (first order)
1834  DMatrix D_x( nx,nfDir );
1835  DMatrix D_p( np,nfDir );
1836  DMatrix D_u( nu,nfDir );
1837  DMatrix D_w( nw,nfDir );
1838 
1839  if( dx != NULL ) {
1840  D_x = Eigen::Map<DMatrix>(dx,nx,nfDir);
1841  D_x.transposeInPlace();
1842  }
1843  else
1844  D_x.setZero();
1845 
1846  if( du != NULL ) {
1847  D_u = Eigen::Map<DMatrix>(du,nu,nfDir);
1848  D_u.transposeInPlace();
1849  }
1850  else
1851  D_u.setZero();
1852 
1853  if( dp != NULL ) {
1854  D_p = Eigen::Map<DMatrix>(dp,np,nfDir);
1855  D_p.transposeInPlace();
1856  }
1857  else
1858  D_p.setZero();
1859 
1860  if( dw != NULL ) {
1861  D_w = Eigen::Map<DMatrix>(dw,nw,nfDir);
1862  D_w.transposeInPlace();
1863  }
1864  else
1865  D_w.setZero();
1866 
1867  DMatrix J( nx+nxa,nfDir );
1868 
1869  // setup seed and sensitvity matrices
1870  DMatrix bSeed2( nbDir2,nx );
1871  bSeed2 = Eigen::Map<DMatrix>(bseed2,nbDir2,nx);
1872  bSeed2.transposeInPlace();
1873 
1874  DMatrix J_x2( nbDir2,nx+nxa );
1875  DMatrix J_u2( nbDir2,nu );
1876  DMatrix J_p2( nbDir2,np );
1877  DMatrix J_w2( nbDir2,nw );
1878 
1879 
1880  int run1, run2;
1881 
1882  for( run1 = 0; run1 < nfDir; run1++ ){
1883 
1884  DVector DXX = D_x.getCol( run1 );
1885  DVector DPP = D_p.getCol( run1 );
1886  DVector DUU = D_u.getCol( run1 );
1887  DVector DWW = D_w.getCol( run1 );
1888 
1889  integrator->setForwardSeed( 1, DXX, DPP, DUU, DWW );
1890  returnvalue = integrator->integrateSensitivities( );
1891 
1892  if( returnvalue != SUCCESSFUL_RETURN )
1893  {
1894  status = -1.0;
1895  plotErrorMessage( returnvalue,printLevel_ );
1896  }
1897  else
1898  {
1899  DVector JJ(nx+nxa);
1900  integrator->getForwardSensitivities( JJ, 1 );
1901  J.setCol( run1, JJ );
1902  }
1903 
1904  for( run2 = 0; run2 < nbDir2; run2++ ){
1905 
1906  DVector XSEED = bSeed2.getRow(run2);
1907 
1908  // determine backward sensitivities
1909  integrator->setBackwardSeed( 2, XSEED );
1910  returnvalue = integrator->integrateSensitivities( );
1911  if( returnvalue != SUCCESSFUL_RETURN )
1912  {
1913  status = -1.0;
1914  plotErrorMessage( returnvalue,printLevel_ );
1915  }
1916  else
1917  {
1918  DVector JXX2(nx+nxa), JPP2(np), JUU2(nu), JWW2(nw);
1919 
1920  integrator->getBackwardSensitivities( JXX2, JPP2, JUU2, JWW2, 2 );
1921 
1922  J_x2.setRow( run2, JXX2 );
1923  J_p2.setRow( run2, JPP2 );
1924  J_u2.setRow( run2, JUU2 );
1925  J_w2.setRow( run2, JWW2 );
1926  }
1927  }
1928  }
1929 
1930 
1931  // write sensitivity matrices into output struct (if given)
1932  if ( returnvalue == SUCCESSFUL_RETURN && outputIdx > 0 )
1933  {
1934  JJ = mxCreateDoubleMatrix( nx+nxa,nfDir,mxREAL );
1935  jj = mxGetPr( JJ );
1936  DMatrix jj_tmp = Eigen::Map<DMatrix>(jj, J.rows(), J.cols()); jj_tmp = J.transpose(); jj = jj_tmp.data();
1937  mxSetField( plhs[outputIdx],0,"J",JJ );
1938 
1939  Jx2 = mxCreateDoubleMatrix( nbDir2,nx+nxa,mxREAL );
1940  jx2 = mxGetPr( Jx2 );
1941  DMatrix jx2_tmp = Eigen::Map<DMatrix>(jx2, J_x2.rows(), J_x2.cols()); jx2_tmp = J_x2.transpose(); jx2 = jx2_tmp.data();
1942  mxSetField( plhs[outputIdx],0,"J2x",Jx2 );
1943 
1944  if ( nu > 0 )
1945  {
1946  Ju2 = mxCreateDoubleMatrix( nbDir2,nu,mxREAL );
1947  ju2 = mxGetPr( Ju2 );
1948  DMatrix ju2_tmp = Eigen::Map<DMatrix>(ju2, J_u2.rows(), J_u2.cols()); ju2_tmp = J_u2.transpose(); ju2 = ju2_tmp.data();
1949  mxSetField( plhs[outputIdx],0,"J2u",Ju2 );
1950  }
1951 
1952  if( np > 0 )
1953  {
1954  Jp2 = mxCreateDoubleMatrix( nbDir2,np,mxREAL );
1955  jp2 = mxGetPr( Jp2 );
1956  DMatrix jp2_tmp = Eigen::Map<DMatrix>(jp2, J_p2.rows(), J_p2.cols()); jp2_tmp = J_p2.transpose(); jp2 = jp2_tmp.data();
1957  mxSetField( plhs[outputIdx],0,"J2p",Jp2 );
1958  }
1959 
1960  if( nw > 0 )
1961  {
1962  Jw2 = mxCreateDoubleMatrix( nbDir2,nw,mxREAL );
1963  jw2 = mxGetPr( Jw2 );
1964  DMatrix jw2_tmp = Eigen::Map<DMatrix>(jw2, J_w2.rows(), J_w2.cols()); jw2_tmp = J_w2.transpose(); jw2 = jw2_tmp.data();
1965  mxSetField( plhs[outputIdx],0,"J2w",Jw2 );
1966  }
1967  }
1968  }
1969  }
1970 
1971 
1972  // VI. STORE OUTPUT STRUCT:
1973  // ------------------------
1974 
1975  if ( ( outputIdx > 0 ) || ( !mxIsEmpty( PlotXTrajectory ) ) || ( !mxIsEmpty( PlotXaTrajectory ) ) )
1976  {
1977  Status = mxCreateDoubleMatrix( 1,1,mxREAL );
1978  statusPtr = mxGetPr( Status );
1979  statusPtr[0] = status;
1980 
1981  if ( status >= 0.0 )
1982  {
1983 
1984  // Get x
1985  VariablesGrid out_x;
1986  integrator->getX(out_x);
1987 
1988  XTrajectory = mxCreateDoubleMatrix( out_x.getNumPoints(),1+out_x.getNumValues(),mxREAL );
1989  xTrajectory = mxGetPr( XTrajectory );
1990 
1991  for( int i=0; i<out_x.getNumPoints(); ++i ){
1992  xTrajectory[0*out_x.getNumPoints() + i] = out_x.getTime(i);
1993  for( int j=0; j<out_x.getNumValues(); ++j ){
1994  xTrajectory[(1+j)*out_x.getNumPoints() + i] = out_x(i, j);
1995  }
1996  }
1997 
1998 
1999  // Get xa
2000  if ( isODE == BT_FALSE ){
2001 
2002  VariablesGrid out_xa;
2003  integrator->getXA(out_xa);
2004 
2005  XaTrajectory = mxCreateDoubleMatrix( out_xa.getNumPoints(),1+out_xa.getNumValues(),mxREAL );
2006  xaTrajectory = mxGetPr( XaTrajectory );
2007 
2008  for( int i=0; i<out_xa.getNumPoints(); ++i ){
2009  xaTrajectory[0*out_xa.getNumPoints() + i] = out_xa.getTime(i);
2010  for( int j=0; j<out_xa.getNumValues(); ++j ){
2011  xaTrajectory[(1+j)*out_xa.getNumPoints() + i] = out_xa(i, j);
2012  }
2013  }
2014  }
2015 
2016 
2017  NumberOfSteps = mxCreateDoubleMatrix( 1,1,mxREAL );
2018  numberOfSteps = mxGetPr( NumberOfSteps );
2019  *numberOfSteps = (double) integrator->getNumberOfSteps();
2020 
2021  NumberOfRejectedSteps = mxCreateDoubleMatrix( 1,1,mxREAL );
2022  numberOfRejectedSteps = mxGetPr( NumberOfRejectedSteps );
2023  *numberOfRejectedSteps = (double) integrator->getNumberOfRejectedSteps();
2024 
2025  GetStepSize = mxCreateDoubleMatrix( 1,1,mxREAL );
2026  getStepSize = mxGetPr( GetStepSize );
2027  *getStepSize = (double) integrator->getStepSize();
2028 
2029 
2030 
2031  // plot if desired
2032  mxArray *PlotType = mxCreateDoubleMatrix( 1,1,mxREAL );
2033  double *plotType = mxGetPr( PlotType );
2034 
2035  mxArray *IsDiscretized = mxCreateDoubleMatrix( 1,1,mxREAL );
2036  double *isDiscretized = mxGetPr( IsDiscretized );
2037 
2038  if( strcmp(integratorName,"DiscretizedODE") == 0 )
2039  isDiscretized[0] = 1.0;
2040  else
2041  isDiscretized[0] = 0.0;
2042 
2043 
2044  if ( !mxIsEmpty( PlotXTrajectory ) )
2045  {
2046  plotType[0] = 0.0;
2047  mxArray* plotArguments[] = { XTrajectory,PlotXTrajectory,UseSubplots,
2048  PlotType,IsDiscretized };
2049  mexCallMATLAB( 0,0,5,plotArguments,"plot_trajectory" );
2050  }
2051 
2052  if ( ( isODE == BT_FALSE ) && ( !mxIsEmpty( PlotXaTrajectory ) ) )
2053  {
2054  plotType[0] = 1.0;
2055  mxArray* plotArguments[] = { XaTrajectory,PlotXaTrajectory,UseSubplots,
2056  PlotType,IsDiscretized };
2057  mexCallMATLAB( 0,0,5,plotArguments,"plot_trajectory" );
2058  }
2059 
2060  mxDestroyArray( IsDiscretized );
2061  mxDestroyArray( PlotType );
2062  PlotType = NULL;
2063  plotType = NULL;
2064 
2065 
2066  // write output struct if given
2067  if ( outputIdx > 0 )
2068  {
2069  mxSetField( plhs[outputIdx],0,"Status",Status );
2070  mxSetField( plhs[outputIdx],0,"NumberOfSteps",NumberOfSteps );
2071  mxSetField( plhs[outputIdx],0,"NumberOfRejectedSteps",NumberOfRejectedSteps );
2072  mxSetField( plhs[outputIdx],0,"xTrajectory",XTrajectory );
2073  mxSetField( plhs[outputIdx],0,"GetStepSize",GetStepSize );
2074 
2075  if ( isODE == BT_FALSE )
2076  mxSetField( plhs[outputIdx],0,"xaTrajectory",XaTrajectory );
2077  }
2078 
2079  }
2080  else
2081  {
2082  if ( outputIdx > 0 )
2083  mxSetField( plhs[outputIdx],0,"Status",Status );
2084  }
2085  }
2086 
2087 
2088  // VII. FREE THE MEMORY:
2089  // ---------------------
2090 
2091  delete integrator;
2092  delete f;
2093  delete cModel;
2094 
2095  if ( modelName != NULL )
2096  mxFree(modelName);
2097 
2098  if( !mxIsEmpty(IntegratorName) )
2099  mxFree(integratorName);
2100 
2101  if( !mxIsEmpty(SensitivityMode) )
2102  mxFree(sensitivityMode);
2103 
2104  if ( !mxIsEmpty(MaxNumStep) )
2105  delete maxNumStepInt;
2106 
2107  clearAllGlobals( );
2108 }
GenericMatrix::getCol
GenericVector< T > getCol(unsigned _idx) const
Definition: matrix.hpp:205
MAX_INTEGRATOR_STEPSIZE
Definition: acado_types.hpp:361
isFunctionHandle
int isFunctionHandle(const mxArray *const M)
Definition: matlab_acado_utils.hpp:61
Integrator::getX
returnValue getX(DVector &xEnd) const
genericDAE
void genericDAE(double *x, double *f, void *userData)
Definition: ACADOintegrators.cpp:335
Integrator::freezeAll
virtual returnValue freezeAll()=0
ModelFcnDX
mxArray * ModelFcnDX
Definition: ACADOintegrators.cpp:56
Integrator::getStepSize
virtual double getStepSize() const =0
RET_INPUT_HAS_WRONG_DIMENSION
Definition: acado_types.hpp:1011
modelFcnNXA
unsigned int modelFcnNXA
Definition: ACADOintegrators.cpp:59
ModelFcnW
mxArray * ModelFcnW
Definition: ACADOintegrators.cpp:55
Grid::getTime
double getTime(uint pointIdx) const
DifferentialEquation::getNumDynamicEquations
int getNumDynamicEquations() const
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
Integrator::getNumberOfSteps
virtual int getNumberOfSteps() const =0
IntegratorBDF
Implements the backward-differentiation formula for integrating DAEs.
Definition: integrator_bdf.hpp:52
LinearAlgebraSolver
LinearAlgebraSolver
Definition: acado_types.hpp:145
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:104
AlgorithmicBase::set
returnValue set(OptionsName name, int value)
Definition: algorithmic_base.cpp:126
MAX_NUM_INTEGRATOR_STEPS
Definition: acado_types.hpp:353
acado_integrators.hpp
GenericMatrix::setCol
GenericMatrix & setCol(unsigned _idx, const GenericVector< T > &_arg)
Definition: matrix.hpp:224
STEPSIZE_TUNING
Definition: acado_types.hpp:362
USING_NAMESPACE_ACADO
#define USING_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:59
allocateDifferentialEquation
DifferentialEquation * allocateDifferentialEquation(char *modelName, char *integratorName)
Definition: ACADOintegrators.cpp:420
Integrator::getNumberOfRejectedSteps
virtual int getNumberOfRejectedSteps() const =0
VariablesGrid
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Definition: variables_grid.hpp:56
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
ABSOLUTE_TOLERANCE
Definition: acado_types.hpp:358
DifferentialEquation::getNumAlgebraicEquations
int getNumAlgebraicEquations() const
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
isScalar
BEGIN_NAMESPACE_ACADO int isScalar(const mxArray *const M)
Definition: matlab_acado_utils.hpp:52
U
#define U
Definition: rocket_with_templates_ocp.hpp:45
modelFcn
DifferentialEquation * modelFcn
Definition: ACADOintegrators.cpp:47
MIN_INTEGRATOR_STEPSIZE
Definition: acado_types.hpp:360
matlab_acado_utils.hpp
modelFcnNU
unsigned int modelFcnNU
Definition: ACADOintegrators.cpp:60
ModelFcnU
mxArray * ModelFcnU
Definition: ACADOintegrators.cpp:53
clearAllGlobals
void clearAllGlobals()
Definition: ACADOintegrators.cpp:69
determineNumberOfAlgebraicStates
unsigned int determineNumberOfAlgebraicStates()
Definition: ACADOintegrators.cpp:382
mexFunction
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
Definition: ACADOintegrators.cpp:513
IntegratorRK23
Implements the Runge-Kutta-23 scheme for integrating ODEs.
Definition: integrator_runge_kutta23.hpp:54
Integrator
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
Definition: integrator.hpp:61
Eigen::PlainObjectBase::rows
EIGEN_STRONG_INLINE Index rows() const
Definition: PlainObjectBase.h:132
modelFcnNW
unsigned int modelFcnNW
Definition: ACADOintegrators.cpp:62
ModelFcnT
mxArray * ModelFcnT
Definition: ACADOintegrators.cpp:50
DiscretizedDifferentialEquation
Allows to setup and evaluate discretized differential equations based on SymbolicExpressions.
Definition: discretized_differential_equation.hpp:54
jacobianNumber
unsigned int jacobianNumber
Definition: ACADOintegrators.cpp:65
modelFcnNX
unsigned int modelFcnNX
Definition: ACADOintegrators.cpp:58
jacobian
Expression jacobian(const Expression &arg1, const Expression &arg2)
Definition: acado_syntax.cpp:169
INTEGRATOR_TOLERANCE
Definition: acado_types.hpp:355
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
Integrator::setForwardSeed
returnValue setForwardSeed(const int &order, const DVector &xSeed, const DVector &pSeed=emptyVector, const DVector &uSeed=emptyVector, const DVector &wSeed=emptyVector)
Definition: integrator.cpp:308
f_store
double * f_store
Definition: ACADOintegrators.cpp:66
nu
const int nu
Definition: rocket_with_templates_ocp.hpp:38
Integrator::setDxInitialization
virtual returnValue setDxInitialization(double *dx0)=0
GenericMatrix::setRow
GenericMatrix & setRow(unsigned _idx, const GenericVector< T > &_values)
Definition: matrix.hpp:213
ModelFcnX
mxArray * ModelFcnX
Definition: ACADOintegrators.cpp:51
IntegratorRK78
Implements the Runge-Kutta-78 scheme for integrating ODEs.
Definition: integrator_runge_kutta78.hpp:54
modelFcnNDX
unsigned int modelFcnNDX
Definition: ACADOintegrators.cpp:63
ModelFcnP
mxArray * ModelFcnP
Definition: ACADOintegrators.cpp:54
modelFcnNP
unsigned int modelFcnNP
Definition: ACADOintegrators.cpp:61
genericJacobian
void genericJacobian(int number, double *x, double *seed, double *f, double *df, void *userData)
Definition: ACADOintegrators.cpp:201
J_store
double * J_store
Definition: ACADOintegrators.cpp:67
allocateIntegrator
Integrator * allocateIntegrator(char *integratorName, DifferentialEquation *f)
Definition: ACADOintegrators.cpp:457
Eigen::PlainObjectBase::setZero
Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:515
IntegratorRK12
Implements the Runge-Kutta-12 scheme for integrating ODEs.
Definition: integrator_runge_kutta12.hpp:54
genericODE
void genericODE(double *x, double *f, void *userData)
Definition: ACADOintegrators.cpp:160
CORRECTOR_TOLERANCE
Definition: acado_types.hpp:363
Integrator::getBackwardSensitivities
returnValue getBackwardSensitivities(DVector &Dx_x0, DVector &Dx_p, DVector &Dx_u, DVector &Dx_w, int order) const
Definition: integrator.cpp:424
clearAllStaticCounters
returnValue clearAllStaticCounters()
Definition: acado_syntax.cpp:248
IntegratorRK45
Implements the Runge-Kutta-45 scheme for integrating ODEs.
Definition: integrator_runge_kutta45.hpp:54
Integrator::getXA
returnValue getXA(DVector &xaEnd) const
ModelFcnXA
mxArray * ModelFcnXA
Definition: ACADOintegrators.cpp:52
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
RET_MISSING_INPUTS
Definition: acado_types.hpp:1000
Grid::getNumPoints
uint getNumPoints() const
RET_MAX_NUMBER_OF_STEPS_EXCEEDED
Definition: acado_types.hpp:1004
INITIAL_INTEGRATOR_STEPSIZE
Definition: acado_types.hpp:359
ModelFcn_jac
mxArray * ModelFcn_jac
Definition: ACADOintegrators.cpp:48
CFunction
(no description yet)
Definition: c_function.hpp:54
Integrator::integrateSensitivities
returnValue integrateSensitivities()
Definition: integrator.cpp:357
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
Integrator::getForwardSensitivities
returnValue getForwardSensitivities(DVector &Dx, int order) const
Definition: integrator.cpp:406
Eigen::PlainObjectBase::cols
EIGEN_STRONG_INLINE Index cols() const
Definition: PlainObjectBase.h:133
plotErrorMessage
void plotErrorMessage(returnValue returnvalue, double *PrintLevel_)
Definition: ACADOintegrators.cpp:388
Integrator::setBackwardSeed
returnValue setBackwardSeed(const int &order, const DVector &seed)
Definition: integrator.cpp:338
ModelFcn_f
USING_NAMESPACE_ACADO mxArray * ModelFcn_f
Definition: ACADOintegrators.cpp:46
RET_TO_SMALL_OR_NEGATIVE_TIME_INTERVAL
Definition: acado_types.hpp:1001
GenericMatrix::getRow
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
Integrator::integrate
returnValue integrate(double t0, double tend, double *x0, double *xa=0, double *p=0, double *u=0, double *w=0)
Definition: integrator.cpp:207
IntegratorDiscretizedODE
Implements a scheme for evaluating discretized ODEs.
Definition: integrator_discretized_ode.hpp:57
DifferentialEquation
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
Definition: differential_equation.hpp:55

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