Direkt zum Inhalt | Direkt zur Navigation

UniBwM » LRT » LRT 1 » Prof. Gerdts » Teaching » Praktikum Optimale Steuerung » legorobot sample

legorobot sample

legorobot_sample.f90 — Fortran source code, 33 kB (34.045 bytes)

Dateiinhalt

!------------------------------------------------------------------------------------------------------------
!   
!   OCPID-DAE is able to solve optimal control and/or parameter identification
!   problems of the following structure (see the user's guide for details): 
!
!   Minimize 
!
!                phi(x(t0),x(tf),tf,p) + sum_{i=1}^L H(xi_i,x(xi_i),u(xi_i),p)
!
!                Where to enter the data: 
!                The function phi needs to be entered in the subroutine OBJ.
!                The funtion H needs to be entered in the subroutine HFUNC.
!
!   subject to 
!    (a)         implicit differential equation (DAE) : 
!
!                     0 = F(t,x(t),x'(t),u(t),p)      
!
!                or ordinary differential equation of type 
!
!                     M(t,x(t),u(t),p)*x'(t) = f(t,x(t),u(t),p)      (M non-singular)
!
!                Where to enter the data: 
!                The functions F and f, respectively, need to be entered in the 
!                subroutine DAE. 
!                The matrices F'_{x'} and M, respectively, need to be entered in the 
!                subroutine MASS. 
!
!    (b)         boundary conditions 
! 
!                     psi_l <= psi(t0,tf,x(t0),x(tf),u(t0),u(tf),p) <= psi_u                   
!
!                Where to enter the data: 
!                The function psi needs to be entered in BDCOND. 
!                The lower bound psi_l needs to be entered in the array BC(0,:).
!                The upper bound psi_u needs to be entered in the array BC(1,:). 
!
!    (c)         state constraints: 
!  
!                     g_l <= g(t,x(t),u(t),p) <= g_u
!
!                Where to enter the data: 
!                The function g needs to be entered in NLCSTR.
!                The lower bound g_l needs to be entered in the array G(0,:).
!                The upper bound g_u needs to be entered in the array G(1,:).  
!
!    (d)         box constraints for control u: 
!  
!                     u_l <= u(t) <= u_u
!
!                Where to enter the data: 
!                For t=t0 the lower bound u_l needs to be entered in the array UL(-1,:).
!                For t0<t<tf the lower bound u_l needs to be entered in the array UL(0,:).
!                For t=tf the lower bound u_l needs to be entered in the array UL(1,:).
!                For t=t0 the upper bound u_l needs to be entered in the array UU(-1,:).
!                For t0<t<tf the upper bound u_l needs to be entered in the array UU(0,:).
!                For t=tf the upper bound u_l needs to be entered in the array UU(1,:).
!   
!    (e)         box constraints for parameter p:
!  
!                     p_l <= p <= p
!
!                Where to enter the data: 
!                The lower bound p_l needs to be entered in the array P(0,:).
!                The upper bound p_u needs to be entered in the array P(1,:). 
!                An initial guess for p needs to be entered in the array P(2,:).
!
!   The time interval is given by [t0,tf] with t0 and tf fixed. t0 needs to be 
!   entered in T(0). The length of the time interval tf-t0 needs to be entered in 
!   T(1).
!
!   For further details please refer to the user's guide. 
!
!------------------------------------------------------------------------------------------------------------
PROGRAM TEMPLATE
  IMPLICIT NONE
  !------------------------------------------------------------------------------------------------------------
  ! Don't change the following declarations
  !------------------------------------------------------------------------------------------------------------
  INTEGER                                        :: IRES,IREALTIME,NREALTIME, & 
                                                    IADJOINT,NVAR,            & 
                                                    IINMODE,IOUTMODE,IPRINT, I
  INTEGER, DIMENSION(10)                         :: DIM
  INTEGER, DIMENSION(33)                         :: INFO
  DOUBLEPRECISION, DIMENSION(0:1)                :: T
  DOUBLEPRECISION, DIMENSION(:,:), ALLOCATABLE   :: XL,XU,UL,UU,P,G,BC,STATE, & 
                                                    CONTROL,                  & 
                                                    SCONSTRAINTS,DSOLREALTIME
  DOUBLEPRECISION, DIMENSION(7)                  :: TOL
  DOUBLEPRECISION, DIMENSION(:), ALLOCATABLE     :: TAUU,TAUX,TMEASURE,SOL,TIME,PARAMETERS
  DOUBLEPRECISION                                :: HREALTIME
  !------------------------------------------------------------------------------------------------------------
  ! end of don't change the following declarations
  !------------------------------------------------------------------------------------------------------------

  !------------------------------------------------------------------------------------------------------------
  ! Define user arrays of appropriate length, the integer user array IUSER and the 
  ! double user array USER will be piped through all subroutines of OCPID-DAE1 but  
  ! these arrays are not used by OCPID-DAE1. The purpose of these arrays is to 
  ! allow the user to pipe user specific data to the subroutines, which is then 
  ! available in the subroutines in IUSER and USER, respectively. If the user switches 
  ! on the realtime mode IREALTIME > 0, then the user has to provide then nominal 
  ! parameter values via IUSER and USER. The i-th realtime parameter is addressed 
  ! by USER(IUSER(I)), I=1,...,NREALTIME. 
  !------------------------------------------------------------------------------------------------------------
  INTEGER, DIMENSION(2)                          :: IUSER
  DOUBLEPRECISION, DIMENSION(2)                  :: USER
  ! define local variables
  DOUBLEPRECISION                                :: D1MACH

  !------------------------------------------------------------------------------------------------------------
  ! Enter problem specific dimensions and data here
  !------------------------------------------------------------------------------------------------------------
  ! provide tolerances for the integration routine and the step-size used in 
  ! finite difference approximations of Jacobians
  TOL(4)  = 1.0d-12                       ! Tolerance state
  TOL(5)  = 1.0d+7                        ! Tolerance sensitivities
  TOL(6)  = 1.0d-6                        ! finite difference step size
  TOL(7)  = 1.0D-4                        ! Tolerance algebraic variable  
  ! set output flag for printing details of the optimal control problem
  IPRINT = 1
  ! set dimensions
  DIM(1)  = ...                           ! number NX of states
  DIM(2)  = ...                           ! number NU of controls
  DIM(3)  = ...                           ! number NP of optimization parameters
  DIM(4)  = ...                           ! number NG of state constraints
  DIM(5)  = ...                           ! number NBC of boundary conditions in BDCOND
  DIM(6)  = 21                            ! number NGITU of control grid points
  DIM(7)  = 1                             ! number NGITX of shooting nodes
                                          ! (one for single shooting, >1 for multiple shooting)
  DIM(8)  = 0                             ! number NROOT of switching functions
                                          ! (zero for standard optimal control problems)
  DIM(9)  = 0                             ! number NY of algebraic variables in x
  DIM(10) = 0                             ! number NMEASURE of measure points xi_i in function H
                                          ! (zero for standard optimal control problems)
  ! set parameters that control the algorithm
  INFO(1) = 0                             ! flag equidistant grids (=0) or non-equidistant grids (=1)
  INFO(2) = 11                            ! integrator, see user's guide for a list
  INFO(3) = 1                             ! control approximation by B-splines 
                                          ! (1=piecewise constant, 2=continuous, piecewise 
                                          !  linear, ...)
  INFO(4) = 1                             ! 0  = integration mode (no optimization) 
                                          ! >0 = optimization mode
  INFO(5) = 1                             ! method for gradient calculation 
                                          ! (1 = sensitivity DAE, 
                                          !  0 = finite difference approximation)
  INFO(6) = 0                             ! structure of matrix F'_{x'} and M, respectively
                                          ! (0  = constant and diagonal,
                                          !  1  = non-constant and diagonal,
                                          !  10 = constant and dense,
                                          !  11 = non-constant and dense)
  INFO(7) = 0                             ! flag for iteration matrix 
                                          ! (0 = approximation by finite differences, 
                                          !  1 = provide iteration matrix in subroutine ITMAT by user)
  INFO(8) = 0                             ! flag for jacobian of state constraints
                                          ! (0 = approximation by finite differences, 
                                          !  1 = provide jacobian matrix in subroutine JACNLC by user)
  INFO(9) = 6                             ! output will be printed to output file with this number (6=screen)
  INFO(10)= 1                             ! flag for finite differences
                                          ! (1 = forward differences are used,
                                          !  0 = central finite differences are used, 
                                          ! -1 = backward differences are used)
  INFO(11)= 0                             ! computation of consistent initial values for index-1 DAEs
                                          ! (0 = no computation of consistent initial values,
                                          !  1 = computation of consistent initial values, linear case
                                          !  2 = computation of consistent initial values, nonlinear case)
  ! how will the initial guess be provided and where shall the output be written to?
  IINMODE = 0                             ! 0   = initial guess for states at shooting nodes are provided 
                                          !       in subroutine INESTX, initial guess for controls is provided 
                                          !       in subroutine INESTU
                                          ! <>0 = initial guess is provided in array SOL
  IOUTMODE = 3                            ! 1   = output of solution will be written to files 
                                          ! 2   = output of solution will be written to arrays 
                                          !       TIME,STATE,CONTROL,PARAMETERS,SCONSTRAINTS,DSOLREALTIME
                                          ! 3   = output of solution will be written to both, files and variables
  !------------------------------------------------------------------------------------------------------------
  ! Realtime optimization control parameters (sensitivity analysis of optimal solution):
  ! If the user switches on the realtime mode IREALTIME > 0, 
  ! then the user has to provide then nominal parameter values 
  ! via IUSER and USER. The i-th realtime parameter is addressed 
  ! by USER(IUSER(I)), I=1,...,NREALTIME. 
  !------------------------------------------------------------------------------------------------------------
  IREALTIME = 0                           ! switch for realtime optimization (sensitivity analysis of optimal
                                          ! solution): 
                                          ! 0 = no sensitivity analysis, 
                                          ! 1 = sensitivity analysis using DAE sensitivities for Hessian 
                                          !     approximation
                                          ! 2 = sensitivity analysis using finite differences for Hessian 
                                          !     approximation
  NREALTIME = 2                           ! number of realtime parameters
  HREALTIME = 1.D-3                       ! step-size used in finite difference Hessian approximation
  ! IUSER(1)       = 1                    ! provide realtime parameters in USER(IUSER(I)),I=1,...,NREALTIME, if
  ! USER(IUSER(1)) =                      ! IREALTIME > 0.
  !------------------------------------------------------------------------------------------------------------
  ! switch for adjoint estimation
  !------------------------------------------------------------------------------------------------------------ 
  IADJOINT = 0                            ! switch for adjoint estimation (1 = yes, 0 = no)
  !-------------------------------------------------------------------------------
  ! provide measure points xi_i, i=1,...,L, for parameter identification (if DIM(10)>0)
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(TMEASURE(MAX(1,DIM(10))))
  ! TMEASURE(1) = 
  !------------------------------------------------------------------------------------------------------------
  ! initial time, length of time interval
  !------------------------------------------------------------------------------------------------------------
  T(0)     = 0                             ! provide initial time t0 of optimal control problem
  T(1)     = 1.0D0                         ! provide length tf-t0 of optimal control problem
  !------------------------------------------------------------------------------------------------------------
  ! provide box constraints for state at shooting nodes
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(XL(0:1,MAX(1,DIM(1))))
  ALLOCATE(XU(0:1,MAX(1,DIM(1))))
  ! provide lower and upper bounds for initial state x(t0)
  XL(0,1) = ...
  ...
  XU(0,1) = ...
  ...
  ! provide lower and upper bounds for states at multiple shooting nodes in (t0,tf)
  XL(1,1:DIM(1)) = -1.0D+20
  XU(1,1:DIM(1)) =  1.0D+20
  !------------------------------------------------------------------------------------------------------------
  ! provide box constraints for control
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(UL(-1:1,MAX(1,DIM(2))))
  ALLOCATE(UU(-1:1,MAX(1,DIM(2))))
  ! provide lower and upper bounds for initial control u(t0)
  UL(-1,1:DIM(2)) = ... 
  UU(-1,1:DIM(2)) = ...
  ! provide lower and upper bounds for controls u(t), t0<t<tf
  UL( 0,1:DIM(2)) = ... 
  UU( 0,1:DIM(2)) = ... 
  ! provide lower and upper bounds for terminal control u(tf)
  UL( 1,1:DIM(2)) = ... 
  UU( 1,1:DIM(2)) = ...
  !------------------------------------------------------------------------------------------------------------
  ! provide box constraints and initial guess for optimization parameter p
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(P(0:2,MAX(1,DIM(3))))
  P(0,1) = ...           ! lower bound for optimization parameter                
  P(1,1) = ...           ! upper bound for optimization parameter
  P(2,1) = ...           ! initial guess for optimization parameter
  ...
  !------------------------------------------------------------------------------------------------------------
  ! provide lower and upper bounds for state constraints g_l<=g(t,x(t),u(t),p)<=g_u, 
  ! g needs to be provided in NLCSTR
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(G(0:1,MAX(1,DIM(4))))
  G(0,1) = ...           ! lower bound g_l for state constraint g_l <= g(t,x(t),u(t),p) <= g_u
  G(1,1) = ...           ! upper bound g_u for state constraint g_l <= g(t,x(t),u(t),p) <= g_u
  ...
  !------------------------------------------------------------------------------------------------------------
  ! provide lower and upper bounds for boundary conditions psi_l<=psi(t0,tf,x(t0),x(tf),u(t0),u(tf),p)<=psi_u
  ! psi needs to be provided in BDCOND
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(BC(0:1,MAX(1,DIM(5))))
  BC(0,1) = ...          ! lower bound psi_l for boundary condition 
                         ! psi_l<=psi(t0,tf,x(t0),x(tf),u(t0),u(tf),p)<=psi_u
  BC(1,1) = ...          ! upper bound psi_u for boundary condition 
                         ! psi_l<=psi(t0,tf,x(t0),x(tf),u(t0),u(tf),p)<=psi_u
  !------------------------------------------------------------------------------------------------------------
  ! If nonequidistant grids are chosen (INFO(1) <> 0) provide control grid and shooting nodes in 
  ! TAUU and TAUX (grid points need to be relative grid points with 0<=ti<=1.
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(TAUU(MAX(1,DIM(6))))
  ALLOCATE(TAUX(MAX(1,DIM(7))))
  !------------------------------------------------------------------------------------------------------------
  ! Don't change the following declarations
  !------------------------------------------------------------------------------------------------------------
  ALLOCATE(SOL(MAX(1,DIM(1)*DIM(7)+DIM(2)*(DIM(6)+INFO(3)-2)+DIM(3))))
  ALLOCATE(TIME(MAX(1,DIM(6))))
  ALLOCATE(STATE(MAX0(1,DIM(1)),MAX0(1,DIM(6))))
  ALLOCATE(CONTROL(MAX0(1,DIM(2)),MAX0(1,DIM(6))))
  ALLOCATE(PARAMETERS(MAX0(1,DIM(3))))
  ALLOCATE(SCONSTRAINTS(MAX0(1,DIM(4)),MAX0(1,DIM(6))))
  ALLOCATE(DSOLREALTIME(MAX0(1,NREALTIME),MAX0(1,DIM(7)*DIM(1)+DIM(2)*(DIM(6)+INFO(3)-2)+DIM(3))))
  !------------------------------------------------------------------------------------------------------------
  ! end of don't change the following declarations
  !------------------------------------------------------------------------------------------------------------
  !------------------------------------------------------------------------------------------------------------
  ! call to optimal control solver OCPID-DAE1
  !------------------------------------------------------------------------------------------------------------
  CALL OCPIDDAE1(T, XL, XU, UL, UU, P, G, BC,           &  
        TOL, TAUU, TAUX, TMEASURE, IRES,                & 
        IREALTIME, NREALTIME, HREALTIME,DSOLREALTIME,   & 
        IADJOINT, .FALSE.,IPRINT,                       & 
        DIM,INFO,IINMODE,SOL,NVAR,IUSER,USER,           & 
        IOUTMODE,TIME,STATE,CONTROL,PARAMETERS,SCONSTRAINTS )
  
  !------------------------------------------------------------------------------------------------------------
  ! Prepare data for use in ROBOTC
  !------------------------------------------------------------------------------------------------------------
  !    ! Output file: robotc.dat
  !
  OPEN(UNIT=15,FILE='robotc.dat', ACTION='WRITE', FORM='UNFORMATTED', ACCESS='STREAM', STATUS='REPLACE')
  DO I=1,DIM(6)   
     ! write binary data (necessary for ROBOTC)
     ! WRITE(15) REAL(TODO, KIND=4)        ! TODO has to be replaced by an expression that you would like write to the file
     WRITE(15) REAL(TIME(I)*PARAMETERS(1), KIND=4) ! writes time point t_I * P(1) to file
     WRITE(15) REAL(STATE(1,I), KIND=4)    ! writes the first state component X(1) at time point t_I to file
     ...
     WRITE(15) REAL(CONTROL(1,I), KIND=4)  ! writes the first control component U(1) at time point t_I to file
     ...
  ENDDO
  !
  !------------------------------------------------------------------------------------------------------------
  ! deallocate memory
  !------------------------------------------------------------------------------------------------------------
  DEALLOCATE(XL,XU,UL,UU,P,G,BC,TAUU,TAUX,TMEASURE,SOL,TIME,STATE,CONTROL,PARAMETERS,SCONSTRAINTS,DSOLREALTIME)
END PROGRAM TEMPLATE
!------------------------------------------------------------------------------------------------------------
! provide function phi(x(t0),x(tf),tf,p) in objective function 
!
! Input (don't alter): 
! --------------------
! X0 = initial state x(t0)
! XF = terminal state x(tf)
! TF = final time
! P  = optimization parameter
!
! Return: 
! -------
! V = value of phi(x(t0),x(tf),tf,p)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE OBJ( X0, XF, TF, P, V, IUSER, USER )
  IMPLICIT NONE
  INTEGER, DIMENSION(*), INTENT(INOUT)         :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)    :: X0,XF,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT) :: USER
  DOUBLEPRECISION, INTENT(IN)                  :: TF
  DOUBLEPRECISION, INTENT(OUT)                 :: V
  V = ...
  RETURN
END SUBROUTINE OBJ
!------------------------------------------------------------------------------------------------------------
! provide function H(xi_i,x(xi_i),u(xi_i),p) in objective function 
!
! Input (don't alter): 
! --------------------
! I = grid point number xi_I
! T = value of grid point xi_I
! X = state at time T
! U = control at time T
! P  = optimization parameter
!
! Return: 
! -------
! HVAL = value of H(xi_i,x(xi_i),u(xi_i),p)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE HFUNC( I,T,X,U,P,HVAL,IUSER,USER )
  IMPLICIT NONE
  INTEGER, INTENT(IN)                          :: I
  INTEGER, DIMENSION(*), INTENT(INOUT)         :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)    :: X,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT) :: USER
  DOUBLEPRECISION, INTENT(IN)                  :: T
  DOUBLEPRECISION, INTENT(OUT)                 :: HVAL
  HVAL = 0.0d0
  RETURN 
END SUBROUTINE HFUNC
!------------------------------------------------------------------------------------------------------------
! provide differential equation depending on integrator chosen in  INFO(2). 
! Implicit integrators require F(t,x(t),x'(t),u(t),p), 
! explicit integrators require f(t,x(t),u(t),p).
!
! Input (don't alter): 
! --------------------
! T  = time 
! X  = state at time T
! XP = derivative x' at time T (only implicit integrators)
! U  = control at time T
! P  = optimization parameter
!
! Return: 
! -------
! F     = differential equation either in implicit form or right handside depending on INFO(2)
! IFLAG = not used
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE DAE( T, X, XP, U, P, F, IFLAG, IUSER, USER )
  IMPLICIT NONE
  INTEGER, INTENT(IN)                          :: IFLAG
  INTEGER, DIMENSION(*), INTENT(INOUT)         :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)    :: X,XP,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT) :: USER
  DOUBLEPRECISION, INTENT(IN)                  :: T
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)   :: F

  F(1) = ...
  ...
  RETURN
END SUBROUTINE DAE
!------------------------------------------------------------------------------------------------------------
! provide function g in state constraints g_l <= g(t,x(t),u(t),p) <= g_u
!
! Input (don't alter): 
! --------------------
! T  = time 
! X  = state at time T
! U  = control at time T
! P  = optimization parameter
!
! Return: 
! -------
! G  = value of g(t,x(t),u(t),p)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE NLCSTR( T, X, U, P, G, IUSER, USER )
  IMPLICIT NONE 
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)         :: X,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)        :: G

  G(1) = ...
  ...
  RETURN
END SUBROUTINE NLCSTR
!------------------------------------------------------------------------------------------------------------
! provide jacobian of function g in state constraints g_l <= g(t,x(t),u(t),p) <= g_u 
! if INFO(8)=1.
!
! Input (don't alter): 
! --------------------
! T  = time 
! X  = state at time T
! U  = control at time T
! P  = optimization parameter
! NG = dimension of g
!
! Return: 
! -------
! GJAC = jacobian matrix of g with respect to x,u,p
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE JACNLC( T, X, U, P, NG, GJAC, IUSER, USER )
  IMPLICIT NONE 
  INTEGER, INTENT(IN)                               :: NG
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)         :: X,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T
  DOUBLEPRECISION, DIMENSION(NG,*), INTENT(OUT)     :: GJAC
  RETURN
END SUBROUTINE JACNLC
!------------------------------------------------------------------------------------------------------------
! provide function psi in boundary conditions psi_l <= psi(t0,tf,x(t0),x(tf),u(t0),u(tf),p) <= psi_u
!
! Input (don't alter): 
! --------------------
! T0 = intial time t0
! TF = terminal time tf
! X0 = intial state x(t0)
! XF = terminal state x(tf)
! U0 = intial control u(t0)
! UF = terminal control u(tf)
! P  = optimization parameter
!
! Return: 
! -------
! PSI = value of psi(t0,tf,x(t0),x(tf),u(t0),u(tf),p)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE BDCOND( T0,TF,X0,XF,U0,UF,P,PSI,IUSER,USER ) 
  IMPLICIT NONE
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)         :: X0,XF,U0,UF,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T0,TF
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)        :: PSI

  PSI(1) = ...
  ...
  RETURN
END SUBROUTINE BDCOND
!------------------------------------------------------------------------------------------------------------
! provide initial guess for states at shooting nodes
!
! Input (don't alter): 
! --------------------
! T = shooting node
!
! Return: 
! -------
! X = value of state at shooting node T
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE INESTX( T, X, IUSER, USER )
  IMPLICIT NONE
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)        :: X

  X(1) = ...
  ...
  RETURN
END SUBROUTINE INESTX
!------------------------------------------------------------------------------------------------------------
! provide initial guess for controls (deBoor points)
!
! Input (don't alter): 
! --------------------
! T     = time 
! IBOOR = deBoor point
!
! Return: 
! -------
! U = value of deBoor point (=control value for B-splines of order 1 or 2)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE INESTU( T, U, IBOOR, IUSER, USER )
  IMPLICIT NONE 
  INTEGER, INTENT(IN)                              :: IBOOR
  INTEGER, DIMENSION(*), INTENT(INOUT)             :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)     :: USER
  DOUBLEPRECISION, INTENT(IN)                      :: T
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)       :: U

  U(1)= ...
  ...
  RETURN
END SUBROUTINE INESTU
!------------------------------------------------------------------------------------------------------------
! provide mass matrix F'_{x'} or M depending on integrator chosen in INFO(2)
!
! Input (don't alter): 
! --------------------
! NX = state dimension
! T  = time 
! X  = state at time T
! XP = state derivative at time T
! U  = control at time T
! P  = optimization parameter
!
! Return: 
! -------
! MMASS = mass matrix F'_{x'} or M depending on INFO(2) and INFO(6)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE MASS( NX,T,X,XP,U,P,MMASS,IUSER,USER )
  IMPLICIT NONE
  INTEGER, INTENT(IN)                              :: NX
  INTEGER, DIMENSION(*), INTENT(INOUT)             :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)        :: X,XP,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)     :: USER
  DOUBLEPRECISION, INTENT(IN)                      :: T
  DOUBLEPRECISION, DIMENSION(NX,*), INTENT(OUT)    :: MMASS
  MMASS(1:NX,1)= 1.0d0
  RETURN
END SUBROUTINE MASS
!------------------------------------------------------------------------------------------------------------
! provide iteration matrix (only for implicit integrators and if INFO(7)=1)
!
! Input (don't alter): 
! --------------------
! T     = time 
! X     = state at time T
! XP    = state derivative at time T
! U     = control at time T
! P     = optimization parameter
! NX    = state dimension
! NSTAB = not used, always = 1
!
! Return: 
! -------
! JAC1  = iteration matrix F'_{x} + CJ * F'_{x'}
! JAC2  = not used (do not alter)
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE ITMAT( T,X,XP,U,P,JAC1,JAC2,CJ,NX,NSTAB,IUSER,USER )
  IMPLICIT NONE
  INTEGER, INTENT(IN)                               :: NX,NSTAB
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, INTENT(IN)                       :: CJ
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)         :: X,XP,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T
  DOUBLEPRECISION, DIMENSION(NX,*), INTENT(OUT)     :: JAC1
  DOUBLEPRECISION, DIMENSION(NSTAB,*), INTENT(OUT)  :: JAC2
  RETURN
END SUBROUTINE ITMAT
!------------------------------------------------------------------------------------------------------
! provide switching functions (root functions), if DIM(8)>0.
!
! Input (don't alter): 
! --------------------
! NX    = state dimension
! T     = time 
! X     = state at time T
! U     = control at time T
! P     = optimization parameter
! NROOT = number of switching functions
!
! Return: 
! -------
! R     = value of switching function
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE ROOT( NX,T,X,U,P,NROOT,R,IUSER,USER )
  IMPLICIT NONE
  INTEGER, INTENT(IN)                               :: NX,NROOT
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)         :: X,U,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)        :: R
  RETURN 
END SUBROUTINE ROOT
!------------------------------------------------------------------------------------------------------
! provide state update at root of switchung function, if DIM(8)>0.
!
! Input (don't alter): 
! --------------------
! IC    = number of switching function that detected a switch
! T     = switching time point
! XL    = left-sided limit of state at switching point T
! UL    = left-sided limit of control at switching point T
! P     = optimization parameter
!
! Return: 
! -------
! D     = jump function d(T,XL,UL,P) that updates the state
!         according to XR = XL + d(T,XL,UL,P), 
!         where XR denotes the right-sided state limit at
!         switching point T.
!
! User arrays: 
! ------------
! IUSER = integer user array (can be modified by user)
! USER  = double user array (can be modified by user)
!------------------------------------------------------------------------------------------------------------
SUBROUTINE DJUMP( IC,T,XL,UL,P,D,IUSER,USER )
  IMPLICIT NONE
  INTEGER, INTENT(IN)                               :: IC
  INTEGER, DIMENSION(*), INTENT(INOUT)              :: IUSER
  DOUBLEPRECISION, DIMENSION(*), INTENT(IN)         :: XL,UL,P
  DOUBLEPRECISION, DIMENSION(*), INTENT(INOUT)      :: USER
  DOUBLEPRECISION, INTENT(IN)                       :: T
  DOUBLEPRECISION, DIMENSION(*), INTENT(OUT)        :: D
  RETURN
END SUBROUTINE DJUMP