Finite Elements with Switch Detection for Direct Optimal Control of Nonsmooth Systems

This paper introduces Finite Elements with Switch Detection (FESD), a numerical discretization method for nonsmooth diﬀerential equations. We consider the Filippov convexiﬁcation of these systems and a transformation into dynamic complementarity systems introduced by Stewart [46]. FESD is based on solving nonlinear complementarity problems and can automatically detect nonsmooth events in time. If standard time-stepping Runge-Kutta (RK) methods are naively applied to a nonsmooth ODE, the accuracy is at best of order one. In FESD, we let the integrator step size be a degree of freedom. Additional complementarity conditions, which we call cross comple-mentarities , enable exact switch detection, hence FESD can recover the high order accuracy that the RK methods enjoy for smooth ODE. Additional conditions called step equilibration allow the step size to change only when switches occur and thus avoid spurious degrees of freedom. Convergence results for the FESD method are derived, local uniqueness of the solution and convergence of numerical sensitivities are proven. The eﬃcacy of FESD is demonstrated in several simulation and optimal control examples. In an optimal control problem benchmark with FESD, we achieve up to ﬁve orders of magnitude


Introduction
The goal of this paper is to develop high-accuracy numerical simulation and optimal control methods for Ordinary Differential Equations (ODE) with a discontinuous vector field.We assume the following structure where R i are disjoint open sets and f i (•) are smooth functions on an open neighborhood of R i , n f is a positive integer and u(t) is an externally chosen control function.This formulation of piecewise smooth ODE falls into the class of hybrid systems [14,50].Many practical problems give rise to such ODE with a discontinuous right hand side (r.h.s.), e.g., in sliding mode control [4], mechanics problems with Coulomb friction [49], state constrained ODE derived from Pontryagin's maximum principle [41], electronic circuits [2], biological systems [5], vaccination strategies [15], transportation systems and traffic flow networks [9], constrained optimization algorithms viewed as dynamic systems [28] and many more.Systems with state jumps, including impact mechanics, robotics and hybrid systems with hysteresis can be transformed into systems matching the form of (1) via the time-freezing reformulation [35,37,40].Consequently, efficient and accurate numerical optimal control algorithms for this class of systems are of great interest.
Related work: High-accuracy simulation of ODE with a discontinuous r.h.s. is numerically difficult since an accurate location of the nonsmooth events in time is needed.Standard time-stepping methods with a fixed step size applied to this class of ODE have at best first-order accuracy as they do not detect the switches [3].Additionally, in the case of sliding modes the numerical solution obtained by explicit time-stepping methods tends to chatter around the discontinuity.Convergence of standard time-stepping discretization methods with order one is studied in [18,29,52].Most high-accuracy methods include a root-finding procedure for accurate switch location and they usually assume that the trajectory crosses the discontinuity or only passes through two regions at a time [3].An exception is Stewart's high accuracy method which can deal with almost all switching cases [46,48].However, it also has an external switch location routine and is thus difficult to apply in direct optimal control, i.e., in first discretize, then optimize approaches to optimal control [44].
The parametric sensitivities of discontinuous ODE (1) have jump discontinuities [23,51] when the trajectory passes through or enters a surface of discontinuity, cf.Section 2.4.Therefore, the application of high-accuracy integrators even in a direct multiple shooting setting [12], which hides the external switch detection procedure from the optimizer, will be notoriously difficult, since derivative-based optimization algorithms will likely fail due to the non-Lipschitz sensitivities.Other fundamental difficulties within direct optimal control of non-smooth ODE are illuminated in the seminal paper by Stewart and Anitescu [51].They show that direct methods based on time-stepping integration schemes with fixed step sizes are doomed to fail since the numerical sensitivities obtained by differentiating the results of a simulation are wrong no matter how small the step size is.We refer to this class of methods as standard methods because they are commonly used in optimal control of smooth dynamical systems.It is also shown that the numerical sensitivities of a smooth approximation of an ODE with a discontinuous r.h.s. are only correct if the step size approaches zero faster than the smoothing parameter, which makes accurate approximations computationally expensive.The same effects carry over to many Dynamic Complementarity Systems (DCS) [36].
On the theoretical side, necessary and/or sufficient conditions for optimality are provided in [16,24,45,53].Guo and Ye [24] and Vieira et.al [53] study optimal control of a DCS with absolutely continuous solutions.The problems from this paper fall into this class.A broader overview can be found in [14].
On the practical side, many authors have developed methods to numerically treat discontinuous ODE in optimal control [10,11,13,30,31,36,42,51]. Kirches [31] develops a direct multiple shooting-based approach with a switch detecting integrator and sensitivity update formulae.Similarly, in [42] a method is developed that can treat sliding modes on a single switching surface.Katayama et al. [30] fix the switching sequence and optimize the lengths of the phases in a model predictive control loop, where at every sample the switching sequence is updated.Bemporad et al. [11] assign integer variables to every mode and solve mixed-integer optimization problems.In [10,13,36] the ODE is transformed into a DCS, resulting in a Mathematical Program with Complementarity Constraints (MPCC) to be solved after discretization.In [36] the authors use a standard discretization approach and suggest to use a homotopy to avoid spurious local minima due to wrong sensitivities [51].Baumrucker and Biegler [10] consider systems with a single switching surface (or with multiple independent switching surfaces, cf.Section 2.3) and allow variable step sizes.This method yields exact switch detection, higher order integration accuracy, and correct numerical sensitivities.The step sizes are left to the optimizer as a degree of freedom, hence it can play with the discretization accuracy, possibly in an undesired way.Unfortunately, a formal proof of the appealing properties of the method is not provided in [10].
As it can be seen from the discussion above, most of the practical methods use only first-order accuracy methods with possibly incorrect sensitivities or treat the discontinuity in the integrator which complicates the use of derivative-based optimization algorithms (except [10]).They do not treat sliding modes appropriately or handle only systems with a single switching surface, i.e., only two regions.The goal of this paper is to develop a method that resolves these issues and to provide a proper convergence theory.Note that the (easier) case of ODE with a continuous but nonsmooth r.h.s.fits into the structure of (1).Conversely, the time-freezing reformulation [35,37,40] transforms many systems from the (more difficult) case of systems with state jumps into the form of (1).This enables us to treat many classes of nonsmooth systems with the same numerical method in a unified way.FESD uses the DCS representation introduced by Stewart [46,48] and is motivated by the variable step size ideas of Baumrucker and Biegler [10].
Contributions: In this paper, we develop the FESD method, which can be used both in simulation and optimal control problems.We start with a reformulation of (1) into dynamic complementary systems introduced by Stewart [46,48] and provide a constructive way to pass from more natural definitions of discontinuous ODE to Stewart's form.Discretization of the DCS results in a nonlinear complementary problem.We build on the ideas of varying the step size and allowing switches only to take place at the boundaries of the finite elements introduced in [10].The FESD method can efficiently deal with multiple and simultaneous switches including sliding modes on higher co-dimension surfaces, and thus is more general than [10].Moreover, in contrast to [10], where only Radau-IIA Implicit Runge-Kutta (IRK) methods were considered, in FESD one can use any Runge-Kutta method.
Additionally, we prove that FESD detects the switches exactly in time, recovers the high-order accuracy that RK methods enjoy for smooth ODE, and obtains the correct numerical sensitivities even when the solution crosses or stays on a discontinuity.To allow switching on the boundaries of the finite elements we introduce the cross complementarity formulation.Using FESD to discretize OCP with DCS results in mathematical programs with complementarity constraints which can be solved efficiently with smooth optimization techniques by solving few several Nonlinear Programming (NLP) problems.[8,27,32,33,43].Thus, we avoid the nonsmooth difficulty encountered within direct multiple shooting with switch-detecting integrators.
Since the step sizes h n are allowed to vary, if no switches occur, we would encounter spurious degrees of freedom which let the optimizer play with the integrator accuracy in a possibly undesired way.To avoid this problem we propose additional conditions called step equilibration which are based on an indicator function.This decouples the integrator accuracy from the optimizer and results in piecewise equidistant discretization grids between the switches.We illustrate the practicability of FESD on several simulation and optimal control examples and verify the theoretical findings.
The FESD method with its many variations and the remainder of the tool-chain for numerically solving optimal control problems with nonsmooth systems are implemented in the open source MATLAB toolbox NOSNOC [1,38].
Organization of the paper: Section 2 gives an introduction to Filippov systems and Stewart's reformulation.It provides a practical procedure to construct Stewart's indicator functions and discusses issues with discontinuous sensitivities.Section 3 introduces the FESD method and discusses the main ideas that lead to it.In Section 4 several relevant theoretical properties of the FESD are studied.The sections contain numerical examples that illustrate the theoretical and algorithmic developments.Section 5 shows how to use FESD in numerical optimal control.The section finishes with an optimal control example and a benchmark compassion of FESD to the standard approach.Finally, Section 6 summarizes the paper and outlines future research.

Notation
the concatenation of several column vectors is defined analogously.The identity matrix is denoted by I ∈ R n×n and a column vector with all ones is denoted by e = (1, 1, . . ., 1) ∈ R n , their dimension is clear from the context.The closure of a set C is denoted by C, its boundary as ∂C and conv(C) is its convex hull.Given a matrix M ∈ R n×m , its i-th row is denoted by M i,• and its j-th column is denoted by M •,j .For a function f : R n → R m we denote by Df (x) = ∂f ∂x (x) ∈ R m×n the Jacobian matrix and by ∇f (x) := ∂f ∂x (x) ⊤ its transpose.For the left and the right limits, we use the notation x(t s + ) = lim t→ts, t>ts x(t) and x(t s − ) = lim t→ts, t<ts x(t), respectively.When clear from context, we often drop the dependency on time t.

Piecewise smooth differential equations
This section will introduce some necessary assumptions on the systems PSS (1), its Filippov convexification [22], and Stewart's reformulation into Dynamic Complementarity Systems (DCS) [46,48] to prepare the ground for the novel method presented in Section 3. We discuss some properties of the DCS for a fixed active set and active-set changes.
To have a meaningful solution concept for this class of ODE, the main idea of Filippov was to replace the r.h.s. of (1) with a convex set and to obtain the following Differential Inclusion (DI): where is the Lebesgue measure on R nx and conv(•) maps a subset of R nx to its closed convex hull.Throughout the paper we assume that the regions R i are disjoint, connected and open.They are assumed to be nonempty and to have piecewise-smooth boundaries ∂R i .We assume that i∈J Due to the special structure of (1) the Filippov DI (2) can be written as This means that in the interior of the regions, R i the Filippov set F F (x, u) is equal to {f i (x, u)} and on the boundary between regions we have a convex combination of the neighboring vector fields.If ẋ exists, functions θ i (•) which serve as convex multipliers can be introduced and the Filippov DI can be written as We call the functions θ i (•) Filippov multipliers.As it will be seen later the functions θ i (•) lack any continuity properties.But it can be shown that they are at least measurable [21,46].Given (3), we will compute piecewise active solutions [46], which are defined as follows.
Definition 1 (Piecewise active solution [46]) For an initial value x(0) = x 0 , a given measurable control function u(t) and a compact interval [0, T ], a function x : [0, T ] → R nx is said to be a solution of (2 Note that this definition assumes a finite number of switches and excludes so-called Zeno solutions, where infinitely many switches can occur in a finite time interval.Zeno solutions cannot be treated with event-detecting methods.For a constant active set I(x) one can derive an ODE or Differential Algebraic Equation (DAE) (for sliding modes) from (3) and apply standard integration methods.The overall ODE solution x(t) is continuous and consists of smooth pieces connected by nondifferentiable points ("kinks") at the switching times t s .

Stewart's reformulation
We regard a specific representation of the sets R i which was introduced by Stewart [46].The main assumption is that the regions R i are given as It is assumed that the indicator functions g i (•), i ∈ J , are smooth functions.Moreover, throughout the paper we assume additionally that g i (•), f i (•) and ∇g i (•) are Lipschitz continuous.
Note that due to the definition of the sets R i in (4), the active set can be defined as We define the vectors θ = (θ 1 , . . ., Using the specific representations (4), from [46] and equation ( 3) one can deduce that the Filippov DI can be written as where the algebraic variables θ(x(t)) are a solution of the parametric Linear Program (LP) LP(x) : θ(x) ∈ arg min Using the Karush-Kuhn-Tucker (KKT) conditions of LP(x) and ( 6), we obtain the dynamic complementarity system ẋ = F (x, u)θ, (8a) where the algebraic variables λ ∈ R n f and µ ∈ R are the Lagrange multipliers of the parametric LP (7).To have an even more compact representation we use a C-function Ψ for the complementarity conditions and rewrite the KKT conditions of the LP (7) as the nonsmooth equation: It provides 2n f +1 conditions for the 2n f +1 algebraic variables θ, λ and µ.The DCS reads in compact form as a nonsmooth differential algebraic equation: Example 1 We illustrate this formulation on the simple example of ẋ ∈ 2 − sign(x).This ODE is characterized by the regions It can be verified that with the functions g 1 (x) = x and g 2 (x) = −x we have a representation of the regions as in (4).Moreover, we have the multipliers θ, λ ∈ R 2 and µ ∈ R. Thus, the corresponding DCS reads as:

Remark on how to treat switching functions
Definition (4) might not be the most intuitive way to represent the sets R i .In many practical examples some smooth scalar functions c i (•), called switching functions, are given.Their zero-level sets define the boundaries of the regions R i .For example, < 0} and so on.Let c(x) = (c 1 (x), . . ., c m (x)) ∈ R m and assume that ∇c(x) ∈ R n×m has rank m.Thus, we can locally define up to n f = 2 m regions and encode them via a sign matrix S ∈ R 2 m ×m defined as .
Note that the matrix S has no repeating rows.Moreover, we assume that this matrix has no zero entries.The sets R ′ i can be compactly represented using the rows S i,• as The next proposition provides a constructive way to find the functions g(•) from the more intuitive representation of the regions via c(•).
Proposition 2 Let the function g : R nx → R n f be defined as then for all x ∈ R ′ i the following statements are true: (i) g i (x) < g j (x), for i = j, (ii) the definitions (4) and (12) define the same set, i.e., x(t s,1 ) x(t s,2 ) x(t s,3 ) Fig. 1 Illustration of active sets at different points.It can be seen that I(x(t 1 )) = I 0 = {1}.At x(t s,1 ) the trajectory crosses the surface of discontinuity between R 1 and R 2 , hence I(x(t s,1 )) = I 0 1 = {1, 2} and later I 1 = {2}.The segment between x(t s,2 ) and x(t s,3 ) is a sliding mode and we have I 0 2 = {2, 3} and I 2 = {2, 3}.Finally we have at x(t s,3 ) that Proof.For (i), note that for x ∈ R ′ i all terms in the sum are strictly positive.On the other hand, for any g j (x) = −S j,• c(x) = − k S j,k c k (x), j = i and x ∈ R ′ i , due to (12), all terms in the sum where S j,k = S i,k are strictly negative.Therefore S i,• c(x) > S j,• c(x), thus (i) holds.
For (ii), first regard the rows S j,• that differ from S i,• only in the k-th column.Then g (4) we recover the definition of ( 12) by looking at the rows where S i,k and S j,k differ by one element.For all rows j that differ from S i,• by more than one column, by similar reasoning, we obtain inequalities that do not tighten (12), since g i (x) − g j (x) consists of a sum of the terms from the inequalities where only one component of c(x) is left.Therefore, statement (ii) holds and this completes the proof.

Fixed active set
For a given solution x(•) let us denote all switching points by 0 = t s,0 < t s,1 < • • • < t s,Nsw = T .The fixed active set between two switches is denoted by I n := I(x(t)), t ∈ (t s,n , t s,n+1 ) =: I n and at a switching point t s,n by I 0 n := I(x(t s,n )).Note that I 0 n = I n ∪ I n−1 .These definitions are illustrated in Figure 1.In this subsection, we regard the DCS (8) for a single fixed I n .For ease of notation, we drop the subscripts in this subsection and denote the fixed active set by I. Depending on the active set, the DCS (8) reduces either to an ODE or to a DAE.
To simplify our exposition we introduce the following notation.For a given vector a ∈ R n and set I ⊆ {1, . . ., n}, we define the projection matrix P I ∈ R |I|×n which has zeros or ones as entries.It selects all component a i , i ∈ I from the vector a, i.e., a In the DAE case, x is on the boundary of one or more regions R i , we speak of sliding modes [23], i.e., |I| > 1 and typically obtain an index 2 differential algebraic equation.In this case, two or more equal entries of g(x) are the smallest components of this vector, and the solution θ of the LP(x) is not unique and lies on a facet of the unit simplex.To compute the values of θ, we must treat the DCS as a DAE.We define F I (x, u) := F (x, u)P ⊤ I , which selects the appropriate columns of F (x, u).For t ∈ I, we have θ i = 0, i / ∈ I and λ i = 0, i ∈ I, thus the DCS (8) reduces to the DAE There are |I| + 1 nontrivial algebraic equations and |I| + 1 unknown algebraic variables, namely µ and θ i for i ∈ I, since we consider θ i (t) = 0, i / ∈ I as fixed.
In the ODE case, x is in the interior of some region R i , we have |I| = 1.The algebraic variables µ and θ i can be computed explicitly from (14) and we have θ i = 1 and µ = g i (x).Thus, the DCS reduces to the ODE ẋ = f i (x).
Next, we provide sufficient conditions for solution uniqueness of the DAE (14) for a given |I| ≥ 1.We define the matrix Note that entries of this matrix arise by taking the total time derivative of (14b).
Assumption 3 Given a fixed active set I(x(t)) = I for t ∈ I, it holds that the matrix M I (x(t)) is invertible and e ⊤ M I (x(t)) −1 e = 0 for all t ∈ I.
Proposition 4 Suppose that Assumption 3 holds.Given the initial value x(t s,n ), then the DAE (14) has a unique solution for all t ∈ I.
Proof.For a given x(•) we can differentiate equation (14b) w.r.t.t and obtain the following index 1 DAE with the algebraic variables θ I and v ∈ R. For a given initial condition x(t s,n ), µ(t s,n ) can be directly computed from any component of (14b).Using the Schur complement and Assumption 3, we conclude that we can find unique θ I and v by solving the linear system (16b).Therefore, the DAE (14) can be reduced to an ODE.Since the functions f i are assumed to be Lipschitz the resulting ODE has a unique solution x(t), t ∈ I. ⊓ ⊔ A similar result, with a more complicated proof but different assumptions can be found in [46,Section 2].Note that even though the DAE has a unique solution for a given active set I, there might be multiple I that give a welldefined ODE, as we discuss in the subsequent sections.We do not know a priori whether we need to treat an ODE or a DAE, but for both cases, we will use Runge-Kutta methods within FESD to provide high-accuracy solutions.The crucial part of FESD is the automatic active set and switching time detection so that sliding modes and crossings of region boundaries can be treated in a unified way.

Active-set changes and continuity of λ and µ
Every active-set change in (8d) corresponds to crossing a discontinuity, entering or leaving a sliding mode, or a spontaneous leaving of a surface of discontinuity.These events in time are called switches.
Lemma 5 The functions λ(t) and µ(t) in (8) are continuous in time.
Proof : The function µ(t) is a minimum of continuous functions and is thus continuous.Therefore, continuity of λ(t) = g(x(t)) − µ(t)e follows from the continuity of x(t) and g(x) and Equation (8b).
⊓ ⊔ Remark 6 Continuity of λ(t) implies that at an active-set change of a component i at t s,n+1 some λ i (t s,n+1 ) must be zero.Moreover, for some i / ∈ I n , in the case of crossing a discontinuity or entering a sliding mode with i ∈ I n+1 , it holds that the left time derivative of λ i is negative, i.e., λi (t − s,n+1 ) < 0. Likewise, in the case of leaving a sliding mode or a spontaneous switch, with i ∈ I n and i / ∈ I n+1 , it follows that the right time derivative of λ i is positive, i.e., λi (t + s,n+1 ) > 0. If some of the first-order one-sided derivatives of λ(•) are zero at a switching point t s,n+1 , then one must look at higher-order derivatives to determine if it stays active or not.
We exploit the continuity of λ(•) and µ(•) later in the derivation of the FESD method.The next example discusses the difference between the possible switching cases.In case (a), for x(0) < 0 the trajectory reaches x = 0 and crosses it.In example (b), for any finite x(0), the trajectory reaches x = 0 and stays there.On the other hand, in example (c), for x(0) = 0 the DI has a unique solution and leaves x = 0 at t = 1.In the last example, for x(0) = 0, the DI has infinitely many solutions, and x(t) can spontaneously leave x = 0 at any t ≥ 0. Note that there is a qualitative difference between leaving a sliding mode (c) and spontaneous switch (d).The arguments of Lemma 5 and Remark 6 are illustrated in Figure 2 for the Example 3 (a).

Predicting the new active set
In this subsection, we restate a more technical result from [46] which is later needed in the convergence proof.The reader not interested in the proofs may skip this part.
As already noted in Remark 6, switches are characterized by the time derivative of λ(•).Note that, e.g., for crossing a discontinuity or entering a sliding mode at a switching point and for the subsequent interval it holds that I n ⊆ I 0 n .Moreover, one can construct a Linear Complementarity Problem (LCP) with the data at x(t s,n ) and predict I n+1 .
We define the vector w I (t) := d dt λ I (t) = M I (x(t))θ I (t) − μ(t)e.One can construct the following mixed LCP between λI 0 n and θ I 0 n at t s,n : For a sufficiently large α > 0 all entries of the matrix M I,α (x) = M I (x)+αee ⊤ are strictly positive.This means the matrix M I,α (x) is strictly copositive, i.e., for any a ≥ 0, a = 0 it holds that a ⊤ M I,α (x)a > 0 [20].One can derive an LCP equivalent to (17) [46, Lemma 3.3]: The motivation for rewriting (17) as ( 18) is twofold.It is both easier to prove solution existence and to compute a solution for an LCP with a strictly copositve matrix than for the initial mixed LCP [46].The solution of the initial LCP (w I 0 n , θ I 0 n ) can be reconstructed via θ I 0 n = θI 0 n /e ⊤ θI 0 n and w I 0 n = wI 0 n /e ⊤ θI 0 n , for further details cf.[46,Lemma 3.3].There is a one-toone correspondence between the active set in a neighborhood of a switching point t s,n and the solutions of the LCP (18).This is summarized in the next theorem proved by Stewart [46].Theorem 7 (Theorem 3.2 [46]) Let x(t) be a solution in the sense of Definition 1 for t ∈ [t a , t b ], with I 0 = I(x(t a )) and I = I(x(t)) for all t ∈ (t a , t b ).Suppose Assumption 3 holds for all t ∈ (t a , t b ).Then for each t ∈ (t a , t b ) there is a solution of the LCP (18) Conversely, let x 0 ∈ R nx and t a be given with and the conditions of Assumptions 3 are satisfied for ∇g i (x) and f i (x, u), i ∈ I, then there is a t b > t a and a solution x(•) in the sense of Definition 1 on [t a , t b ] such that x(t a ) = x 0 and I(x(t)) = I for all t ∈ (t a , t b ).

Regard an LCP
with M ∈ R l×l and q ∈ R l .The given LCP ( 19) is compactly denoted by LCP(M, q) and its set of solutions is denoted by SOL(M, q) ⊆ R l .If a solution satisfies (M θ + q) + θ > 0, we say that strict complementarity holds.
To show convergence we will require the solutions of the LCP to be strongly stable [20,46].A solution (w * , θ * ) ∈ SOL(M, q) of a given LCP(M, q) is said to be strongly stable if there is a neighborhood U of θ * and a neighborhood V of the problem data M ∈ R l×l and q ∈ R l , such that the intersection of U with the solution set of an LCP constructed from the data from one point in V is a singleton.We state a regularity assumption about the LCP (18).Assumption 8 Consider a solution x(t) in the sense of Definition 1 for t ∈ [0, T ], and let S = {t s,0 , . . ., t s,Nsw } be the set of switching points.The solutions of the LCP (18) are strongly stable and satisfy strict complementarity for all The strict complementarity assumption is needed to obtain a tight prediction of the next active set I, cf.first part of Theorem 7. From the proof of [46,Theorem 3.2] it follows that the strict complementarity condition implies that the one-sided time derivatives of λ i (t), i / ∈ I(x(t)) are nonzero, see also Remark 6.Without this assumption, one can obtain only an over-approximation of I.
However, it can be relaxed at the cost of looking at higher-order time derivatives of λ i (t s,n ), i ∈ I 0 n and constructing an appropriate LCP for determining the active sets past some switching point, cf.[47, Section 4.2] for derivations.Note that the strict complementarity is needed only in a neighborhood of the switching points.Strong stability is assumed in order to apply some results on parametric LCPs.In our case, we will use it to draw the same conclusions from LCPs constructed at t and t ′ , where t and t ′ are sufficiently close.

Remark on Cartesian products of Filippov systems
The reformulation from the last subsection given by the DCS (8) fails on some simple examples such as: ẋ1 ∈ −sign(x 1 ), ẋ2 ∈ −sign(x 2 ), x ∈ R 2 .This example satisfies the one-sided Lipschitz condition and has a unique Filippov solution [23,48].However, as shown in [48] at (0, 0) the DAE arising from (8) fails to have a unique solution.One can see that ẋ1 ∈ −sign(x 1 ) and ẋ2 ∈ −sign(x 2 ) are completely independent and thus they should be treated in such a way.
Stewart introduced a generalization of his reformulation for such cases in [48].One should identify the n sys independent subsystems with index k = 1, . . ., n sys , where each subsystem has n k f modes.We equip all variables related to the k−th subsystem with the superscript k.Instead of (3) one can write Finding the functions for every subsystem works the same way as in Section 2.2.1.Thereby, the regions of every subsystem are defined via the matrix S k and the switching functions c k (x) ∈ R n k c .Every mode's convex combination is encoded by its parametric linear program (7), constructed with the k-th modes' switching functions where For ease of notation, in the remainder of the paper we treat the case with n sys = 1, as all extensions are straightforward.
To the best of the authors' knowledge, there are no general conditions known which identify when the r.h.s. of ( 1) is partially separable as in (20) and there might even be multiple ways to write it in this form.However, in practice, it is usually easy to identify the structure of (20) by inspection.For example, this occurs if we have multiple surfaces with friction, or multiple objects touching the same frictional surface [48].

Sensitivities with respect to parameters and initial values
Correct calculation of derivatives of solutions w.r.t.parameters (e.g., discretized control functions) and initial values is crucial for efficient numerical optimal control algorithms and verifying the optimality of a solution.This is not straightforward for ODE with a discontinuous r.h.s., as the sensitivity usually exhibits jumps when switches occur.As any constant parameter p can be modeled via adding the state ṗ = 0 and p(0) = p, we restrict our attention to sensitivities w.r.t.initial values.
Regard the DCS given by Eq. ( 8) on a time interval [0, T ] with the initial condition x(0) = x 0 .Assume that the surface ∂R j is reached at t s (x 0 ) ∈ (0, T ) and that x 0 ∈ R i .We consider the case where the solution crosses a co-dimension one surface of discontinuity ∂R j .Other cases are where the trajectory: (a) slides on the surface of discontinuity after reaching it, (b) starts on a surface of discontinuity and stays on it or leaves it, or (c) goes from one surface to another.They can be analyzed with the same arguments as below, but we omit these cases here for brevity, cf.[23,Section 2.11].
In the case of crossing, we have for t ∈ [0, t s ) that I(x(t)) = {i} and from (8) it follows that ẋ = f i (x).After crossing ∂R j at t s we have I(x(t)) = {j} for t ∈ (t s , T ] and ẋ = f j (x).At t s it holds that ψ i,j (x(t s )) = 0 with Thus, the system can be compactly represented by We are interested in the exact sensitivity matrix X(t, 0; x 0 ) = ∂x(t;x0) ∂x0 ∈ R nx×nx of a solution x(t; x 0 ) of the system (23).The function X(t, 0; x 0 ) obeys smooth linear variational differential equations on both sides of t s , but exhibits a jump at t s [23].The statement of the next proposition is adapted from [51, Section 3.3].
Proposition 9 Regard the system (23) with x(0) = x 0 ∈ R i on an interval [0, T ] with a switch at t s ∈ (0, T ).Assume that the functions f i (x), f j (x), ψ i,j (x) are continuously differentiable along x(t), t ∈ [0, T ].Assume the solution x(t) reaches the surface of discontinuity transversally, i.e., ∇ψ i,j (x(t s )) ⊤ f i (x(t s )) > 0. Then the sensitivity X(T, 0; x 0 ) of a solution x(t; x 0 ) of the system described by the ODE (23) is given by This proposition can also be adapted to the case of sliding modes.We obtain similar expressions for the sensitivity jump formula as in (24).The only change needed to be made is to replace f j (x) with f * (x), where f * (x) defines the sliding vector field [22].Since numerical sensitivities obtained via standard time-stepping methods fail to converge to their correct values (24) [40,51], artificial local minima arbitrarily close to the initialization point may arise in the context of optimization and impair the progress of the optimizer.This is resolved within FESD, where the convergence of the discrete-time sensitivities is recovered, cf.Section 4.5.

Finite Elements with Switch Detection
This section introduces the main algorithmic ingredients of the FESD method.The goal of the method is to: (a) detect exactly the time of reaching or leaving the region boundaries which is necessary for high accuracy of integration methods, (b) exactly compute the sensitivities across regions in order to correctly treat the nonsmoothness and (c) appropriately treat the possible evolution on the boundary that is present in sliding modes.
In this section, we regard a single control interval [0, T ] with a constant externally chosen control input q ∈ R nu , i.e., we set u(t) = q for t ∈ [0, T ].Extensions with more complex smooth parametrizations of the control function are straightforward.

Standard Runge-Kutta discretization
As a starting point in our analysis, we regard a standard Runge-Kutta (RK) discretization of the DCS (8).In the nonsmooth ODE community, these schemes are known as time-stepping methods.Opposed to event-based/switchdetection methods, they assume fixed step sizes h n and do not try to detect the switches.As a consequence, they have in general only first-order accuracy [3].
The theoretical properties of RK methods for DI and DCS have been studied by many authors, e.g., [18,29,50,52].
Suppose the initial value x(0) = s 0 is given.We divide the control interval into N FE finite elements (i.e., integration intervals) [t n , t n+1 ] via the grid points 0 = t 0 < t 1 < . . .< t NFE = T .On each of the finite elements we consider an n s -stage Runge-Kutta method which is characterized by the Butcher tableau entries a i,j , b i and c i with i, j ∈ {1, . . ., n s } [25].The fixed step size reads as h n = t n+1 − t n , n = 0, . . ., N FE − 1.The approximation of the differential state at the grid points t n is denoted by x n ≈ x(t n ).We regard a differential representation of the Runge-Kutta method where the derivatives of states at the stage points t n,i := t n + c i h n , i = 1, . . ., n s , are degrees of freedom.For a single finite element, they are summarized in the vector V n := (v n,1 , . . ., v n,ns ) ∈ R nsnx .The stage values for the algebraic variables are collected in the vectors: we denote the value at t n+1 , which is obtained after a single integration step.Finally, the RK equations for a single finite element for the DCS (8) are given by: . . .
Next, we summarize the equations for all N FE finite elements over the whole interval [0, T ] in a discrete-time system manner.For this purpose, we introduce some additional shorthands.All variables of all finite elements for a single control interval are collected in the vectors x = (x 0 , . . ., x NFE ) ∈ R (NFE+1)nx , V = (V 0 , . . ., V NFE−1 ) ∈ R NFEnsnx and h := (h 0 , . . ., h NFE−1 ) ∈ R NFE .Note that the simple continuity condition x n+1 = x next n holds.We collect all stage values of the Filippov multipliers in the vector Θ = (Θ 0 , . . ., Θ NFE−1 ) ∈ R n θ and n θ = N FE n s n f .The vectors Λ ∈ R n θ , M ∈ R nµ for the stage values of the Lagrange multipliers are defined accordingly, with n µ = n θ n f .The vector All computations over a single control interval which we call here the standard discretization are summarized in the following equations which resemble a discrete-time system: where s 1 ∈ R nx is the approximation of x(T ) and . . . .
Note that h are given parameters, implicitly fixed by the chosen discretization grid.It is usually impossible to obtain high-accuracy solutions with this method, as this can only happen if active-set changes occur coincidentally at t n .Despite the high accuracy in this unlikely case, the numerical sensitivities would still be wrong [36,51].When active-set changes happen within a finite element, the IRK method tries to approximate a nonsmooth trajectory by a smooth polynomial, cf. the left plot in Figure 3, which results in a poor approximation.

Algorithmic ingredients of the FESD method
To ensure high-accuracy solutions of FESD, we allow the optimization routine to vary the lengths h n of the finite elements such that all switching points coincide with grid points t n .Consequently, active-set changes cannot happen in the interior of each finite element, and smooth functions are approximated by smooth polynomials within a finite element, cf. the right plot in Figure 3. Thus, the active set I(x(t)) changes its value only at some grid point t n and is constant in the interior of all intervals (t n , t n+1 ).A key assumption in any event-based method is that there are finitely many switches in finite time.We also assume that there are enough finite elements to capture every switch that occurs in the time interval [0, T ].

The step sizes as degrees of freedom
To capture the switches with the discretization grid points t n , the step sizes h n are left to be degrees of freedom in the RK method in the remainder of this paper.Additionally, the condition

NFE−1 n=0
h n = T ensures that we regard a time interval of unaltered length.

Cross complementarity
We want to prohibit active-set changes on stage points inside a finite element.To achieve this, next to the complementarity conditions for every stage point 0 = Φ(θ n,m , λ n,m ) we include additional conditions on the variables Θ and x * (t) Λ.These conditions ensure that the variable step size h n adapts so that the switching times are indeed captured, as shown below.
For ease of exposition, we assume that the underlying RK scheme satisfies c ns = 1 (e.g., Radau and Lobatto methods [25]).This means that the right boundary point of a finite element is a stage point, since t n+1 = t n + c ns h n for c ns = 1.At the end of the section, we detail how to treat the case with c ns = 1 (e.g., Gauss-Legendre methods).

Continuity of λ(•) and µ(•).
The boundary values of the approximation of λ(•) and µ(•) on an interval [t n , t n+1 ] play a crucial role in FESD.Therefore, we regard their values at t n and t n+1 which are denoted by λ n,0 , µ n,0 and λ n,ns , µ n,ns , respectively.We exploit the continuity of λ(•) and µ(•) (cf.Lemma 5) and impose for their discrete-time counterparts for n = 0, . . ., N FE − 1: Therefore, in the sequel we use only the right boundary points λ n,ns and µ n,ns which are degrees of freedom in the RK equations (26).
Moving the switching points to the boundary.Since λ(•) is continuous, on some interval (t n , t n+1 ) with a fixed active set I n , in the interior of the regarded interval its components are either zero or positive on the whole interval.The stage values λ n,i of the discrete-time counterpart should satisfy this property as well.This is achieved by the cross complementarity conditions, which read for all n ∈ {0, . . ., N FE −1} as Some of the appealing properties of the constraints (28) are given by the next lemma.In our notation θ n,m,i is the i-th component of the vector θ n,m .

⊓ ⊔
A consequence of this lemma is that at the boundary points t n+1 for activeset changes we have λ n,ns,i = λ n+1,ns,i = 0.This is important for the switch detection as we discuss below.The results of the last lemma is illustrated in Figure 4.Note that in contrast to the left plot illustrating the standard complementary conditions, in the right plot, all stage points inside a finite element have the same active set and on the finite element boundary we have λ n,ns,i = 0.
Note that λ 0,0 and µ 0,0 are not defined via Eq.( 27), as we do not have a preceding finite element.However, they are crucial for the statement of the last lemma, especially, if the boundary point is the only stage point, as is the case for the implicit Euler method.This can be resolved by pre-computing λ 0,0 explicitly and using it in (28).Note that λ 0,0 is not a degree of freedom.Since x 0 is known, we obtain µ 0,0 = min i g i (x 0 ) and thus we have λ 0,0 = g(x 0 )−µ 0,0 .
The conditions (28) are given in their sparsest form.Due to the nonnegativity of Λ n and Θ n there are many equivalent formulations of this condition, e.g., all conditions above can be summed up for a single finite element or even for all finite elements on the regarded control interval.Moreover, instead of the component-wise products in θ n,m and λ n,m ′ we can use also inner products of these vectors.Thus, we use a more compact form of (28) where we combine the conditions for two neighboring finite elements.The motivation for this form is that we end up with the same number of new conditions as we have new degrees of freedom by varying h n .The conditions read as: Implicit switch detection.We briefly explain how the switch detection for the solution approximation is realized and formalize it later in Section 4. Note that for x next n = x n+1 we have from the KKT conditions of the LP(x n+1 ) (cf.Eq.( 9)) that µ n,ns = min j g j (x n+1 ).Moreover, if the active-set changes between the n-th and n + 1-st finite element in the i-th component, then from Lemma 10 it follows that λ n,ns,i = 0. Therefore, we obtain from (25) implicitly the condition where ψ i,j (x n+1 ) = 0 defines the switching surface between R i and R j .This condition forces h n to adapt such that the switch is detected exactly.Note that the condition (30) appears only if active-set changes happen, hence the whole switch detection procedure is implicit.

Step equilibration
If no switches occur, i.e., the active sets I n do not change between two neighboring finite elements, then the cross complementarity conditions in (29) are trivially satisfied.This yields spurious degrees of freedom in the step sizes h n and the optimizer can adapt the grid in an undesirable way and harm the discretization accuracy.Also, the path-constraint discretization can be exploited unfavorably, just to decrease the objective value.To resolve this problem we introduce step equilibration conditions.The step size should only change if a switch occurs and otherwise be constant.This results in a piecewise uniform discretization grid for the differential and algebraic states on the regarded control interval.To accomplish this, we derive an indicator function that is zero only if a switch occurs otherwise its value is strictly positive.
If some λ i (t n ) is equal to zero and its left or right time derivative is nonzero, then an active-set change has occurred.Instead of looking at the time derivatives, in the discrete-time case, we exploit the non-negativity of λ n,m and the fact that the active set is fixed for the whole finite element (due to cross complementarity, cf.Lemma 10).For n ∈ {1, . . ., N FE − 1}, we define the following backward and forward sums of the stage values over the neighboring finite elements [t n−1 , t n ] and [t n , t n+1 ]: The components of σ λ,B n and σ λ,F n are zero if the left and right time derivatives of the corresponding components of λ n,m are zero.Likewise, they are positive when the left and right time derivatives are nonzero.Analogously, the sums for θ n,m are defined as: Additionally, we define the following vectors for all n ∈ {1, . . ., N FE − 1}: If there is an active-set change in the i-th complementarity pair, then at most one of the i-th components of σ λ,B n and σ λ,F n is nonzero, hence their product, i.e., the i-th component of π λ n , is zero.Due to complementarity, the same holds for π θ n .For sliding modes the corresponding components of π λ n are zero and of π θ n they are positive (due to complementarity).Thus, the i-th component of is only zero if there is an active-set change in the i-th complementarity pair at t n .A function that has the desired properties is defined as: This scalar function summarizes the effects of all components.It is zero only if an active-set change happens at the boundary point t n , otherwise, it is strictly positive.Finally, the constraints that remove possible spurious degrees of freedom in h n read as: Since many products are involved in η n (Θ, Λ), one can replace it by ηn (Θ, Λ) := tanh(η n (Θ, Λ)) to have a better scaling.An example for step equilibration is studied in Subsection 5.3 numerically.

The FESD discretization
We have now all the ingredients to extend the standard RK discretization (26) to the FESD discretization.We use again the same discrete-time representation where F fesd (x) = x NFE is the state transition map and G fesd (x, h, Z, q, T ) collects all other internal computations including all RK steps within the regarded control interval: For a fixed control function q, horizon length T and initial value s 0 , the formulation (32) can be used as an integrator with exact switch detection for PSS (1).Since Filippov DI does not always have unique solutions, one cannot expect uniqueness of solutions for their discrete-time counterparts (32) in all cases.In simulation methods, a common approach is to either pick one local solution obtained by the solver for the nonlinear complementarity problem (32) or to enumerate all possible solutions at an active-set change [5,46].In this paper, we consider only the first option.Note that in sliding modes, we implicitly obtain differential algebraic equations of index 2, cf.Section 2.2.2.To achieve good accuracy in practice it is usually required to use stiffly accurate methods, e.g., Radau-IIA methods [25].

Remark on RK methods with c ns = 1
We outline how to extend the FESD method when an RK scheme with c ns = 1 is regarded.In contrast to the developments so far, with c ns = 1 the variables λ n,ns , µ n,ns do not correspond the boundary values λ(t n+1 ) and µ(t n+1 ) anymore (since t n + c ns h n < t n+1 ).We denote the boundary points in this case by λ n,ns+1 , µ n,ns+1 .They are computed by solving LP(x n+1 ) for n = 0, . . .N FE − 2: We still exploit the continuity of λ(•) and µ(•) (cf.Lemma 5), by replacing (27) with the following continuity conditions for their discrete-time counterparts for n = 0, . . ., N FE − 1: With slight abuse of notation, we add the new variables θ n,ns+1 , λ n,ns+1 and µ n,ns+1 to the vectors Θ, Λ and M, respectively.The vector Z is redefined accordingly.The cross complementarity conditions are now modified such that next to the stage points we include the boundary points with the index n s + 1: For the whole control time we have in total (N FE − 1)(2n f + 1) new variables.

Convergence theory
In this section we present the main convergence result of the FESD method.First, we prove that even though the FESD system ( 32) is always overdetermined it still has a locally isolated solution.Second, we show that the numerical solution approximation xh (•) generated by FESD converges to a solution x(•) in the sense of Definition 1, with the same order that the underlying RK method has for smooth ODE.Additionally, we prove that the numerical sensitivities converge to their correct values with high accuracy.

Main assumptions
We start by introducing some notation and stating some assumptions related to the FESD formulation (32), which are important for our theoretical study in this section.
Assumption 11 (Runge-Kutta method) A Butcher tableau with the entries a i,j , b i and c i , i, j ∈ {1, . . ., n s } related to an n s -stage Runge-Kutta (RK) method is used in the FESD (32).Moreover, we assume that: (a) If the same RK method is applied to the differential algebraic equation ( 14) on an interval [t a , t b ], it has a global accuracy of O(h p ) for the differential states.(b) The RK equations applied to (14) have a locally isolated solution for a sufficiently small h n > 0.
This assumption aims to consider a broad class of RK methods, and both assumptions are standard assumptions [25].
Assumption 12 (Solution existence) For given parameters s 0 , q and T , there exists a solution to the FESD problem (32), such that for all n ∈ {0, . . ., N FE − 1} it holds that h n ≥ 0.
This assumption means that there exists a solution and that we can compute it.If the FESD method is used in direct optimal control, non-negativity of the step sizes can easily be achieved by adding box constraints on h n .This is the strongest assumption we make in this paper.Ideally, one would prove the existence of solutions.Since the system is over-determined this cannot be done straightforwardly by applying standard existence results [20].As we will show below, in practice numerical solvers have no trouble computing such solutions.We state a technical assumption that ensures regularity of the FESD problem (32).
Once all stage values are computed by solving (32), we can use some interpolation method to construct the solution approximation candidate in continuous time, cf.Assumption 11.For example, if we use a collocation-based IRK method continuous-time approximation xn (t; h n ) on every finite element is easily obtained via Lagrange polynomials [25].We append the approximation for every finite element and write where h = max n∈{0,...NFE−1} h n .Similarly, continuous-time representations can be found for the algebraic variables, and we denote them compactly as λh (t), θh (t) and μh (t).Similar to the definitions in Section 2.2.3, the fixed active set in this case is denoted by I(x h (t)) = În , t ∈ ( ts,n , ts,n+1 ) and the active set at switching point ts,n by I(x h ( ts,n )) = Î0 n .

Solutions of the FESD problem are locally isolated
In this subsection, we analyze some properties of solutions of the FESD problem (32).For the convenience of the reader, we restate the problem but discard the trivial state transition map Recall that Z = (x, V, Θ, Λ, M) ∈ R nZ .Additionally, we have that G std : Again, for ease of exposition, we regard c ns = 1 and give the extensions later with n θ = N FE n s n f and n µ = N FE n s .
The vectors s 0 ∈ R nx , q ∈ R nu and T ∈ R are given parameters, hence we have n Z + N FE unknowns and n Z + 2N FE − 1 equations.Consequently, for N FE > 1, which we always assume in FESD, the system (36) is overdetermined.However, we show in the next theorem that for a given active set N FE − 1 equations in (36) are implicitly satisfied, and we always end up with a square system.As a consequence, Eq. ( 36) has under reasonable assumptions a locally unique solutions.Nevertheless, since we do not know the active set a priori, we can also not know which equations are binding and which are implicitly satisfied.Lemma 14 (Corollary 6.1 in [34]) Theorem 15 Suppose that Assumptions 11, 12 and 13 hold.Let s 0 , q 0 and T > 0 be some fixed parameters such that G fesd (Z * , h * , s 0 , q, T ) = 0. Let P * ⊆ R nx × R nu × R be the set of all parameters (ŝ 0 , q, T ) such that Z ∈ R nZ , which is the solution of G fesd (Z, h, ŝ0 , q, T ) = 0, has the same active set as Z * .Additionally, suppose that G fesd (•) is continuously differentiable in s 0 , q and T for all (s 0 , q, T ) ∈ P * .Then there exists a neighborhood P ⊆ P * of (s 0 , q 0 , T ) such that there exist continuously differentiable single valued functions Z * : P → R nZ and h * : P → R NFE .
We collect the binding n 1 cross complementarity conditions, with 0 ≤ n 1 ≤ N FE − 1, in the equation G * cross (Θ * , Λ * ) = 0, and the N FE − 1 − n 1 implicitly satisfied into G res cross (Θ * , Λ * ) = 0. Likewise, we collect the binding n 2 step equilibration conditions, with 1 h n − T is always binding.We can further simplify our system of equations by eliminating some degrees of freedom using G * eq (h * , Θ * , Λ * ) = 0.All components of this vector are of the form η n (h n − h n+1 ) with η n > 0. Therefore, we have n 2 equations of the form of h n = h n+1 and can remove n 2 degrees of freedom.Furthermore, we can express any h j = T − NFE−1 i=0,i =j h n and remove another degree of freedom.In total we removed n 2 + 1 degrees of freedom and can regard a reduced number of unknown step-sizes, which we denote by h * ∈ R n1 , n 1 = N FE − n 2 − 1.With a slight abuse of notation, we redefine the standard RK equations accordingly and obtain G std (Z * , h * , s 0 , q, T ) = 0 with To summarize, for a fixed active set we can rewrite (36) in a reduced form as with G * fesd (Z * , h * , s 0 , q, T ) ∈ R nZ+n1 .These conditions imply with G res fesd (h * , Θ * , Λ * ) ∈ R NFE−1 .Thus, for a given active set we can discard (38) and regard only the equivalent reduced problem (37), which is a square system of Next, we show that the Jacobian matrix ∇ (Z, h) G * fesd (Z * , h * , s 0 , q, T ) ⊤ has full rank.This enables us to apply the implicit function theorem (cf.[19,Theorem 1B.1]) and establish the result of this theorem.We take a closer look at the matrix: Under Assumption 12, for a fixed active set and a fixed h * n the equation G std (Z * , h * , s 0 , q, T ) = 0 boils down to the RK equations for the differential algebraic equation (14).Due to Assumption 11 the RK system G std (Z * , h * , s 0 , q, T ) = 0 has a locally isolated solution.A necessary and sufficient condition for this property is the invertibility of the Jacobian ∇ Z G std (Z * , h * , s 0 , q, T ) ⊤ [19,Theorem 1B.8].Thus, we have that rank(∇ Z G * fesd (Z * , h * , s 0 , q, T ) ⊤ ) = n Z .Second, due to the block diagonal structure of ∇ hG * std (Z * , h * , s 0 , q, T ) and Assumption 13 we can deduce that rank(∇ hG * fesd (Z * , h * , s 0 , q, T ) ⊤ ) = n 1 .Third, due to the nonnegativity of (Θ, Λ) and Assumption 13 by direct computation it can be verified that rank(∇ Z G * cross (Θ, Λ) ⊤ ) = n 1 and ∇ hG * cross (Θ, Λ) ⊤ = 0. We introduce more compact notation and summarize the results so far with: To show that ∇ (Z, h) G * fesd (Z * , h * , s 0 , q, T ) ⊤ has a rank of n Z + n 1 , we show that the linear system with v ∈ R nZ and w ∈ R n1 has zero as the only solution.
From the first line in this linear system, we have that Next, from the second part of our linear system, we have that −M 3 M −1 1 M 2 w = 0. Again, using Lemma 14, we conclude that rank(M 3 M −1 1 M 2 ) = n 1 .Hence, we have w = 0 and v = 0 to be the only solution of the regarded linear system.This completes the proof.

Remark 16
We note that one cannot apply more general forms of implicit function theorems for generalized and nonsmooth equations [19].They usually require Lipschitz continuity of the solution map to reason about local uniqueness, but the solution map for FESD is not continuous in general, but only piecewise continuous.
Example 5 To illustrate the discontinuity of the solution map, we look at the example of ẋ ∈ 2 − sign(x) + x 2 , with N FE = 2, T = 0.2 and vary x 0 ∈ [−0.7, 0.1].A solution approximation is obtained via FESD based on the Radau-IIA method of order 3. Consider an initial value x 0 such that no switch occurs and a perturbed initial value x 0 + ǫ where a single switch occurs on the time interval of interest.Clearly, in the first case, we have an equidistant grid with h 0 = h 1 , and in the second case h 0 jumps to ts,1 .We conclude that h 0 (x 0 ) is not a Lipschitz function, see Figure 5 for an illustration.

Extension for the case of c ns = 1
In the case of c ns = 1, we need to solve the additional LP (33) to obtain the boundary points.Note that if we have g i (x n+1 ) = min j∈J g j (x n+1 ) for more than one i, the variables θ n,ns+1 are not unique and the LP(x n+1 ) has infinitely many solutions.However, these variables are neither used in the cross complementarities nor step equilibration.Therefore, we can discard θ n,ns+1 and simplify (33) to: λ n,ns+1 ≥ 0, which has n f + 1 unknowns and n f equalities and n f inequalities for a given x n+1 .Now suppose that the first m 1 components of λ n,ns+1 are zero (e.g., implied by cross complementarity) and the remaining m 2 are strictly positive, with m 1 + m 2 = n f .We have that As the first m 1 relations all assign the same value to µ n,ns+1 , we can discard m 1 − 1 of them and thus we end up with a system of m 1 + m 2 + 1 = n f + 1 equations and n f + 1 unknowns.This system has still the important property that µ n,ns+1 = min i g i (x n+1 ).With this simplification for an RK method with c ns = 1 we have The new variables are determined by the square linear system (39).Hence, it is straightforward to extend Theorem 15 for the case of c ns = 1.

Convergence and order of FESD
In this subsection we prove that under reasonable assumptions the sequence of approximations xh (•) generated by the FESD method converges with high order to a solution of (1) in the sense of Definition 1. Recall that h = max n∈{0,...NFE−1} h n .The proof is inspired by the proof of Theorem 4.3 in [46].We consider also t s,0 = 0 as a switching point, since at this time point the active set for the first interval (t s,0 , t s,1 ) is determined.Note that for generating solution approximations with FESD it is sufficient to consider only two finite elements at a time, i.e., N FE = 2 in Eq. (36), and then to append the solutions in order to construct xh (t), t ∈ [0, T ] via Eq.(35).This requires of course to have only one switch in the regarded time interval, which can always be achieved with a sufficiently small h.We define the set of all discretization grid points as G = {t 0 , t 1 , . . ., t NFE }.We treat the cases of crossing a discontinuity or entering a sliding mode, i.e., the case of I n ⊂ I 0 n+1 and I n+1 ⊆ I 0 n+1 .Theorem 17 Suppose that x(t) is a solution of (1) in the sense of Definition 1 for t ∈ [0, T ] with x(0) = x 0 .Suppose the following is true: Then x(•) is a limit point of the sequence of approximations xh (•), defined in Eq. (35) as h ↓ 0.Moreover, for sufficiently small h > 0, the solution of (32) generates a solution approximation xh (t) on [0, T ] such that: Proof.The proof will be carried out by induction, where we consider the switching events n = 0, . . ., N sw and the corresponding time intervals (t s,n , t s,n+1 ), with a slight abuse of notation where t s,Nsw+1 = T is not necessarily a switching point.Regard n = 0, where we have trivially that t 0 = 0, thus Moreover, I(x 0 ) = I(x h (0)) = I 0 0 .Now we suppose ( 40) is true for n, i.e., at t = t s,n .We show that the same statements are true for n + 1.By the induction hypothesis and due to continuity of g i (x), i ∈ J , we have that for sufficiently small h the equality I(x( ts,n )) = I(x(t s,n )) = I 0 n holds.Moreover, by Lipschitz continuity of f i (x) and ∇g i (x), i ∈ J , it follows that (cf.Section 2.2.4) )) as h ↓ 0. According to Theorem 7 the solution of the LCP (18) corresponding to Moreover, by Assumption 8 this LCP is strongly stable and due to Lemma 20 (cf.Appendix A), for sufficiently small h > 0 the solution of the LCP corresponding Thus, we conclude that both the solution approximation and the solution predicted the same active set I n in a neighborhood of ts,n and t s,n , respectively.
It is left to verify that such an active set I n predicted by the solution approximation is indeed feasible for the FESD problem.Note that by the induction hypothesis and the reasoning above the solution approximation and x(•) have the same corresponding active set in a neighborhood of t s,n .For a fixed active set, as a consequence of Proposition 4 the arising DAE ( 14) has a unique solution.Finally, under this setting with the given active sets in a neighborhood of t s,n , according to Theorem 15 there is a locally unique solution to a FESD problem, thus we can construct an appropriate xh (•).
Note that one can make arbitrarily many integration steps with a fixed I n before the next switch in time is reached.Again, due to Theorem 15 the corresponding FESD problem has a locally unique solution.Now we provide an error estimate for the solution approximation until the next switching point.First, we define x(t) to be the exact extended solution of the DAE ( 14) with the fixed active set I n on the interval t ∈ [t s,n , t], with x(t s,n ) = x(t s,n ) and t > t s,n+1 .Obviously, it holds that x(t) = x(t) for all t ∈ [t s,n , t s,n+1 ].Second, from the discussions in Section 3.2.2we know that active-set changes can only happen at boundaries of the finite elements, thus it holds that ts,n ∈ G for all n = 0, . . ., Nsw .Third, by the induction hypothesis we have xh ( ts,n ) − x( ts,n ) = O(h p ).As noted above, for a fixed active set I n and fixed h n the FESD equations boil down to standard RK equations applied to (14).Thus, from Assumption 11 we have the estimate With the help of this estimate, in the next few steps we prove that | ts,n+1 − t s,n+1 | = O(h p ).It is assumed that we regard crossing a discontinuity or entering a sliding mode, i.e., the case of I n ⊂ I(x(t s,n+1 )) and I n+1 ⊆ I(x(t s,n+1 )).We need to distinguish the two scenarios: I. ts,n+1 > t s,n+1 and II.ts,n+1 ≤ t s,n+1 .Case I. Regard the following indices j ∈ I n and i ∈ I(x(t s,n+1 )) \ I n .This means that min k g k (x((t s,n+1 )) = g i (x(t s,n+1 )) = g j (x(t s,n+1 )) = µ(t s,n+1 ) holds and one can locally regard the following switching function Note that this function is Lipschitz continuous.It must by definition become zero when an active-set change happens.
Similarly, for the solution approximation we have ψ i,j (x h (t)) = g i (x h (t)) − g j (x h (t)).Since ts,n+1 > t s,n+1 , due to continuity of ψ i,j (•) and xh (•) it follows that ψ i,j (x h (t s,n+1 )) > 0 and d dt ψ i,j (x h (t s,n+1 )) < 0. Now from Lipschitz continuity of ψ i,j (•) and ( 41) we can establish that Note that in contrast to ψ i,j (x(t)), the function ψ i,j (x(t)) is smooth in a neighborhood of t s,n+1 .Thus, we look at the first-order Taylor approximation of ψ i,j (•) at x(t s,n+1 ).
ψi,j (x( ts,n+1)) − aII (t − ts,n+1) Fig. 6 The left plots shows an illustration of the argument of Case I: ts,n+1 > t s,n+1 and the right plot shows an illustration of the argument of Case II: ts,n+1 Due to continuity, ψ i,j (x(t)) is decreasing for t ∈ [t s,n+1 , ts,n+1 ], there exists some positive constant a I with 0 < a I < | d dt ψ i,j (x(t s,n+1 )| such that for sufficiently small h and t ∈ [t s,n+1 , ts,n+1 ] it holds that The arguments above are illustrated in the left plot of Figure 6.From the last inequality and (43) at t = ts,n+1 we have that ψ i,j (x( ts,n+1 )) < 0, ψ i,j (x(t s,n+1 )) = 0, and conclude that ts,n+1 This completes the consideration of case I.
Case II.We apply similar arguments as for I.Under the assumption of ts,n+1 < t s,n+1 , following similar lines as in the proof of [46,Theorem 4.3], we first prove ts,n+1 → t s,n+1 and establish subsequently the convergence rate.
Regard the set H = {h > 0 | ts,n+1 < t s,n+1 }.By the assumption of case II and the induction hypothesis, it holds that ts,n+1 ∈ [ ts,n , t s,n+1 ].Since this is a bounded set, there must be a subsequence H ′ ⊆ H with h ↓ 0 such that ts,n+1 → t.We show now that t = ts,n+1 .We consider again the function ψ i,j (•) for some j ∈ I n and i ∈ I(x(t s,n+1 )) \ I n which becomes zero at an active-set change and is positive before.Similarly, active-set changes for the solution approximation happen when ψ i,j (x h ( ts,n+1 )) = 0. We remind the reader that earlier it was shown that În = I n .
By taking h ↓ 0 and h ∈ H ′ from (41) it follows that ψ i,j (x( t)) = 0.By the definition of a switching point, there must be a i / ∈ I n , but i ∈ I(x( t)).However, this contradicts the assumption that I(x(t)) = I n for t ∈ (t s,n , t s,n+1 ) and we conclude that t / ∈ (t s,n , t s,n+1 ).
On the other hand at t s,n , due to strict complementarity in the active-set determining LCP (cf.Theorem7 and Assumption 8), if some i ∈ I(x(t s,n ))\ I n and j ∈ I n , we know that Due to continuity of the functions g i (•), i ∈ J , and the induction hypothesis, there exists some ǫ > 0 such that However, when a switch happens the derivative in the last line must be negative at t (cf.Remark 6), thus ts,n+1 > ts,n + ǫ, i.e., with h ↓ 0, h ∈ H ′ , t > t s,n + ǫ.This means that t = t s,n and the only option that is left is ts,n+1 → t = ts,n+1 as h ↓ 0, h ∈ H ′ .Now we continue with establishing the convergence rate for ts,n+1 → t s,n+1 .From ts,n+1 ≤ t s,n+1 we have from the definition of ψ i,j (•) that ψ i,j (x( ts,n+1 )) > 0 and ψ i,j (x h ( ts,n+1 )) = 0. Using the fact that x( ts,n+1 ) = x( ts,n+1 ) and ( 41) we have We again use a first-order expansion: Once again, due to assumption 8, we have that d dt ψ i,j (x(t s,n+1 )) < 0. Note that d dt ψ i,j (x(t − s,n+1 )) < 0. From ts,n+1 → t s,n+1 and continuity of ψ i,j (•) it follows that for sufficiently small h > 0: d dt ψ i,j (x( ts,n+1 )) < 0.
Using similar reasoning as in case I (see right plot of Figure 6 for an illustration of the argument), there exists some a II > 0 such that from the last equation at t = t s,n+1 it follows 0 ≤ O(h p ) − a II (t s,n+1 − ts,n+1 ), i.e., Putting ( 44) and ( 45) together, we obtain the first part of the induction statement, i.e., To complete the induction step we must prove that (40b) holds for t = ts,n+1 .For ts,n+1 ≤ t s,n+1 we have x( ts,n+1 ) = x( ts,n+1 ) and the assertion follows directly from (41).Note that for any other t n ∈ G that is not a switching point (40b) follows immediately from Assumption 11.
It is left to investigate the case of ts,n+1 > t s,n+1 .Using Lipschitz continuity of x(•), x(•), the fact that x(t s,n+1 ) = x(t s,n+1 ) and ( 46) one obtains xh ( ts,n+1 ) − x( ts,n+1 ) ≤ xh ( ts,n+1 ) − x( ts,n+1 ) + x(t s,n+1 ) − x( ts,n+1 ) Moreover, from Lipschitz continuity of g i (•), i ∈ J and the last inequality for sufficiently small h > 0 we have that I(x(t s,n+1 )) = I(x( ts,n+1 )), which completes the induction step for n+1.With the use of an interpolation scheme, from (40) it follows that we can make a continuous-time approximation xh (t) for t ∈ [0, T ] with the accuracy O(h p), 1 ≤ p ≤ p for t / ∈ G. Therefore, it follows that the sequence of approximations xh (t) generated by the FESD method converges to a solution x(t) in the sense of Definition 1 for t ∈ [0, T ].The proof is completed.
⊓ ⊔ In the next subsection, we illustrate the results of this theorem for several RK schemes.We compare the results obtained via FESD to the ones obtained with the standard RK discretization from Section 3.1.

Illustrating the integration order
Consider the nonsmooth IVP with 2 − 1, ω = 2π and x(0) = (e −1 , 0) for t ∈ [0, T ].The example trajectory is given in Figure 7.Following Section 2.2, we can write (47) in the form of the DCS (8), where: where g(x) was obtained and via Eq.( 13).It can be shown that the switch happens at t s = 1 and that x(T ) = (e (T −1) cos(2π(T − 1)), −e (T −1) sin(2π(T − 1))) for T > t s .Hence, we can determine the global integration error E(T ) = x(T ) − xh (T ) .We regard solution approximations to this IVP obtained by standard explicit and implicit RK methods (26) and FESD (32) with N FE = 2 and different step sizes.Both integration methods are available in NOSNOC via the function integrator fesd().The regarded RK methods are listed in Table 1 together with their global integration error estimate.For explicit methods, we consider in this paper n s ≤ 4. Several other IRK methods are available in NOSNOC, but we omit the full comparison for brevity.The nominal step h size is obtained by dividing T by the number of simulation steps N sim .We take an irrational number for T = π 2 so that t s = 1 never coincides with Fig. 7 Illustration of the solution to the nonsmooth IVP given by (47).

Method
Global error estimate Radau-IIA Table 1 List of analyzed RK methods and their accuracy order for ODE [25].Note that for Explicit-RK methods the assertion in the table is true for ns ≤ 4, otherwise p < ns.
a finite element boundary.Therefore, we avoid accidental switch detection via the discretization grid for the standard discretization.
The numerical error as a function of the step sizes for the RK and FESD methods from Table 1 are depicted in Figure 8.We can see that the standard discretization achieves in all cases only first-order accuracy (left plots).In contrast, the FESD method recovers in all cases the high accuracy order that the underlying RK method has for smooth ODE (right plots).This verifies the result of Theorem 17 in practice and demonstrates how FESD can be used as an event-based integrator without an external switch detection procedure.The saturation in the right plots of some high-accuracy methods is due to the round-off errors in floating point arithmetic which limit the possible accuracy on a computer.

Convergence of discrete-time sensitivities
One of the most important tools for computing stationary points and verifying their optimality in direct optimal control is the numerical sensitivities, i.e., the derivatives of the numerical solution approximation xh (t; x 0 ) w.r.t. the initial value x 0 (and parameters) for some t ∈ (0, T ].Here we denote them by Xh (t; 0, x 0 ) or sometimes more compactly by Xh (t, x 0 ).
A fundamental limitation of standard discretization methods for nonsmooth systems (e.g., with the RK discretization from Subsection 3.1) is that Xh (t, x 0 ) → X(t, x 0 ) as h ↓ 0 [51].In direct optimal control, this can cause  Fig. 8 Integration error E(T ) = x(T )−x h (T ) vs. the step size h for different RK methods: standard (left plot) vs. FESD (right plot).The legend provides the name of the used RK method and its order of the global integration error.
convergence to artificial stationary points arbitrarily close to the initialization point [36,51].Fortunately, the sensitivities of the solutions generated by the FESD method converge to their true values (cf.Subsection 2.4).This is shown in the next theorem, but before we state it, we make one more assumption on the time derivatives of the solution approximation.
Assumption 18 (RK derivatives) Regard the RK methods from Assumption 11 applied to the differential algebraic equations (14).Suppose that the derivatives of the numerical approximation for the same RK method converge with order 1 ≤ q ≤ p, i.e., ẋh (t) − ẋ(t) = O(h q ), t ∈ G.
The aim of the assumption about the convergence of the derivatives of the numerical approximation is to cover a broad class of RK methods and the value of q depends on the specific choice of an RK method.For example, for collocation-based implicit RK methods for ODE in general it holds that q = p − 1 [26,Theorem 7.10].More specifically, methods that contain the boundary point of a finite element denoted by t, that is c ns = 1 satisfy p = q.This assertion follows directly from Lipschitz continuity and the fact that the numerical approximations satisfy the ODE at every stage point: Proof.Regard partition of [0, T ]: 0 in the open interval between every two neighboring points there is a single switching point with N k > N sw .Then by the chain rule we have W.l.o.g.assume that on [0, t1 ] a single switch occurs at t s ∈ (0, t1 ).We show convergence of the sensitivities on this interval.Convergence on [0, T ] follows by inductively applying the same arguments on every sub-interval [ tk , tk+1 ].Regard the two smooth pieces of the approximation xh (t): 1) xh,1 (t, x 0 ) for t ≤ ts and, 2) xh,2 (t, y 0 (x 0 )) where y 0 (x 0 ) = xh,2 (0, y 0 (x 0 )) = xh,1 ( ts , x 0 ).With this definition, we have for t ≥ ts xh,2 (t − ts , y 0 (x 0 )) = xh (t, x 0 ).(49) From Theorem 17 we know that | ts − t s | = O(h p ). Obviously, the value of ts depends on x 0 and we know from Theorem 17 that we obtain implicitly at a switching point the condition ψ i,j (x h,1 ( ts (x 0 ), x 0 )) = g i (x h,1 ( ts (x 0 ), x 0 )) − g j (x h,1 ( ts (x 0 ), x 0 )) = 0, (50) where i / ∈ I 0 , i ∈ I 1 and j ∈ I 0 .For computing Xh (•) on [ ts , t 1 ] from Eq. ( 49) we have Next, we compute the expressions for the two unknowns ∇ x0 ts (x 0 ) ⊤ and ∇ x0 y 0 (x 0 ).Denote by Xh,1 (t; 0, x 0 ) = ∂ xh,1 (t,x0) ∂x0 . Using the implicit function theorem for (50), we can compute At t = ts , we exploit the fact that xh,2 (0, y 0 (x 0 )) = y 0 (x 0 ) = xh,1 ( ts , x 0 ), thus Combing the last line with ( 52) we obtain We are interested in → I as t ↓ ts .Thus from ( 51), ( 52) and ( 53) one obtains By the chain rule, we have that for t > ts Xh (t; x 0 ) = Xh,2 (t; t+ s , y 0 ) Ĵh (x h ( ts ; x 0 )) Xh,1 ( t− s ; 0, x 0 ).
First, note that for a fixed active set the FESD equations for xh,1 (t, x 0 ) and xh,2 (t, y 0 ) boil down to RK equations for the DAE (14) with fixed step size h n , cf.Theorem 15.Differentiating the RK equations to obtain Xh (•) results in the same RK method applied to the variational differential equations of the system at hand, thus the numerical sensitivities converge in this setting to the continuous-time sensitivities with the same accuracy O(h p ) as for the solution of the system [6].
Second, as h ↓ 0, due to assumption of this theorem, in Ĵh (x h ( ts ; x 0 )) the functions ẋh (•) converge to f (x(•)) with order q.Due to Theorem 17 all other terms converge with order p.Thus, Ĵh (x h ( ts ; x 0 )) − J(x(t s ; x 0 )) = O(h q ).Summarizing the last two arguments and applying them inductively for every active-set change we conclude that Xh (t; x 0 ) → X(t; x 0 ) as h ↓ 0 with the order q = min(p, q).This completes the proof.
⊓ ⊔ The only restrictive assumption we make is that a single active-set change happens at a time.For multiple simultaneous switches the derivation becomes quite involved even in continuous-time case [23], hence we made this assumption for simplicity.The results of Theorem 19 are illustrated on a numerical example in the next subsection.Fig. 9 Approximations of the objective V (x 0 ) computed with FESD and a standard IRK Gauss-Legendre method compared to the true value are shown in the left plot.The right plot shows the value of the optimal solution x * 0 against different initial values x 0 for which a initial feasible solution guess was computed.The optimal solution is computed with FESD and a standard method with and without a homotopy approach for the underlying optimization problem.

Illustration of numerical sensitivity convergence
To demonstrate the improvements in FESD compared to standard methods we repeat the experiments from [36].For this purpose, we look at the optimal control problem from [51]: Note that the initial value x 0 is the only effective degree of freedom.Let V * (x 0 ) be the objective value for the unique feasible trajectory starting at x(0) = x 0 .
The equivalent reduced problem is given by min In the first experiment, we evaluate V * (x 0 ) by simulating the trajectory of (56c) for various x 0 with a standard RK method (26) and FESD (32).We pick the Gauss-Legendre method of fourth order with n s = 2 and N FE = 25.The results are depicted in the left plot in Figure 9.It can be seen that for standard RK methods, the trajectories would converge to the analytic solution, but the derivatives w.r.t.x 0 do not.In fact, many artificial minima arise [36,51].On the other hand, the FESD solution matches the analytic solution.
In the second experiment, we solve the OCP (56) for different initial guesses x 0 .The initial guess for the arising MPCC is obtained from a forward simulation of (56c) with the same method and same grid as in the discretized OCP.The arising MPCC are solved by a relaxation approach, where the complementarity constraints (60c) are replaced by w ⊤ 1 w 2 ≤ σ, w 1 ≥ 0, w 2 ≥ 0.
In the first experiment, the MPCC are solved for a fixed σ = 10 −15 .In the second case, we solve a sequence of NLP in a homotopy procedure where σ is decreased from σ 0 = 1 to σ N = 10 −15 , via the rule σ k+1 = 0.1σ k .It was demonstrated in [36] that a homotopy approach with standard discretizations can lead to convergence to better local minima.The results are depicted in the right plot of Figure 9.
Remarkably, the FESD solution matches the analytic solution for all x 0 regardless of the MPCC strategy and initialization point (FESD -fixed and FESD -homotopy).This highlights that the numerical sensitivities converge to the true ones, cf.Theorem 19.On the other hand, as in [36], with the standard IRK method, the fixed parameter MPCC strategy leads to convergence to the artificial local minima close to the initialization.This results in a stairlike curve in right plot in Figure 9 (Standard -fixed).This is a consequence of the fact that the numerical derivatives are always wrong, i.e., the numerical approximation is "trapped" in one of the local minima of the standard approach, which are visible in the left plot (blue).
The homotopy strategy yields better local minima since in the earlier homotopy iterations the derivatives are still correct [40] (Standard -homotopy).However, the error of O(h) to the analytic solution is still present since a standard method has at best accuracy of order one.This highlights the result of Theorems 17 and 19 and demonstrates the superiority of FESD to the naïve approach even on a very simple example.

FESD in direct optimal control
This section regards the use of FESD in direct optimal control, i.e., a first discretize then optimize approach.We consider the following continuous-time optimal control problem on a control horizon [0, T ctrl ]: where x0 is a given initial value, u(t) ∈ R nu is the control function, z(t) = (λ(t), θ(t), µ(t)) ∈ R 2n f +1 collects the algebraic variables.The function L int : R nx × R nu → R is the Lagrange objective term to be integrated and L end : R nx → R is the terminal cost also called Mayer term.The path and terminal where w = (w 0 , w 1 , w 2 ) ∈ R nw is a decomposition of the problem variables.MPCC are difficult nonsmooth optimization problems that violate the Mangasarian-Fromovitz constraint qualification at all feasible points [8], which makes standard NLP algorithms fail to converge.Fortunately, MPCC can often be solved efficiently via reformulations and homotopy approaches [8,27,33,43].In a homotopy procedure a sequence of more regular, smooth, and relaxed NLPs related to (60) are solved.Several relaxation, smoothing, and penalty homotopy approaches are implemented in MATLAB package NOSNOC [38].They differ in how the complementarity constraints (60c) are treated.Under some regularity assumptions the complementarity constraints are satisfied exactly even after solving a finite sequence of NLP [8,43].In NOSNOC the NLP is solved with IPOPT [54] via its CasADi [7] interface.In our numerical experiments, we do not provide an elaborate initial guess.For the states, we simply use the initial value for all stages and set the controls to zero.The homotopy approaches implemented in NOSNOC are quite robust and usually succeed in finding a good solution, as shown in the benchmark in [39].
Fig. 11 The left plot shows the solution trajectories for v(t).The right plot shows optimal controls obtained via the FESD discretization of the OCP (61).The vertical dashed lines highlight the control discretization grid.

A numerical optimal control example
The following optimal control problem demonstrates several features developed in this paper: FESD for Cartesian Products of Filippov systems (Sec.2.3), handling multiple sliding modes, step equilibration (Sec.3.2.3)and equidistant control discretization grids (Sec.5.1).We also include a benchmark where we compare the accuracy and computational time of the standard and FESD methods.Regard the following OCP with q ∈ R 2 , v ∈ R 2 , u ∈ R 2 and x = (q, v): where ρ = 10 3 , ϕ 1 (x) = q 1 + 0.15q 2 2 , ϕ 2 (x) = −0.05q 3 1 + q 2 and the function c(x) = (ϕ 1 (x), ϕ 2 (x)) defines the region boundaries.It can be seen that for v(t) = 0 the vector fields of q point in all regions towards the origin in the (q 1 , q 2 ) plane.Settings the control functions to zero, this results in trajectories going to the origin with sliding modes on the surfaces of discontinuity defined by c(x) = 0. On the other hand, by increasing the value of v(t) via the control functions u(t), the vector fields can change their direction and sliding modes can be left or not achieved at all.
The goal in the OCP is to reach q final = (− π 6 , − π 4 ) with a minimum control effort.The trajectory has to: (1) first reach ϕ 1 (x) = 0; (2) slide towards M = {q ∈ R 2 | c(x) = 0}; (3) stay there for some time; (4) exit M and slide on ϕ 2 (x) = 0; (5) and then leave the sliding mode as late as possible to reach q final .The seemingly simple example comprises several difficult switching cases in its solution.The described solution is illustrated in Figure 10 and the optimal controls u(t) and state v(t) are depicted in Figure 11.The OCP is discretized with the FESD Radau-IIA method of order 3 with n s = 2 and N ctrl = 6 control intervals with N FE = 6 finite elements on every control interval.This system is transformed as described in Section 2.3 with n sys = 2, where c i,1 (x) = ϕ i (x), i = 1, 2. The functions g i,j (x) ∈ R 2 , i, j = 1, 2 are computed via Eq.( 13).
This solution comprises four switches and different sliding modes on nonlinear manifolds including: twice sliding on co-dimension 1 manifolds and once on the co-dimension 2 manifold M, and additionally leaving the sliding mode twice.One can see in Figure 11 that the control is discretized on an equidistant grid despite the variable length of the finite elements.This illustrates the multiple shooting-type discretizations described in Section 5.1.In the first experiment, we demonstrate the effects of step equilibration for a solution of the OCP (61).The left plot in Figure 12 depicts the indicator function η(•).Clearly, the function is only zero if a switch occurs, cf. the right plot of Figure 10.The resulting step sizes h k,n with and without step equilibration are depicted in the middle and right plots, respectively.For the right plot we discard the step equilibration conditions G eq (h, Θ, Λ, T ) = 0. Obviously, without them, the optimizer varies the step size in a somewhat random way.On the other hand, with step equilibration, we obtain a piecewise equidistant grid, where the step size changes only when a switch occurs.
In the second experiment, we compare the accuracy of an OCP solution obtained with the standard and FESD method as a function of the CPU time.We take the optimal controls and perform a high-accuracy simulation of the system dynamics in (61), which we denote by x int (t).As a metric, we take the terminal constraint satisfaction of the high accuracy solution, i.e., E(T ) = x int (T ) − x final .We set N ctrl = 6, and vary the number of finite elements per stage N FE from 1 to 7 and the number of stage points n s from 1 to 4. The experiment is performed for the following RK methods: Radau-IIA, Gauss-Legendre, Lobatto-IIIC, and Explicit-RK.
For Radau-IIA-FESD and Lobatto-IIIC-FESD we solved the arising MPCC with an elastic mode homotopy approach [8].In the other scenarios, we were not able to solve all problems to convergence with the elastic mode approach.Therefore, the MPCC was solved with a relaxation homotopy approach, cf.[38] for implementation details.The second approach is slightly slower than elastic mode, but more robust, and all problems were solved successfully.The terminal error as a function of the total CPU time is given in Figure 13.
We can draw several conclusions from the experiments.Clearly, the FESD method outperforms the standard approach in all experiments.For example, for a CPU time of ≈ 1 second FESD achieves five orders of magnitude more accurate solutions than the standard time-stepping approach.A better solution than the most accurate one of the standard approaches can be achieved with FESD by an order of magnitude faster CPU time.The Radau-IIA and Lobatto-IIIC methods are the most efficient ones in this benchmark, whereas the Gauss-Legendre and Explicit-RK methods perform poorly.This is no sur- prise, since the solution trajectories contain sliding mode arcs which require solving nonlinear DAE of index 2. Radau-IIA and Lobatto-IIIC usually perform well and have good theoretical properties for higher index DAE, whereas Gauss-Legendre and Explicit RK even lose the high accuracy orders that they have for ODE [25].The source code of all examples is available in the repository of the open source tool NOSNOC [1].The same repository contains a few additional examples where FESD is used, including systems with state jumps that are transformed via time-freezing into PSS [37,38] and a nonsmooth mechanics simulation problem from [48], among others.

Summary
This paper presents a method that enables direct optimal control of a broad class of switched systems with high simulation accuracy, called Finite Elements with Switch Detection (FESD).We build a solid theoretical foundation and prove that FESD has the same accuracy as the underlying RK method has for smooth differential equations and that it delivers exact numerical sensitivities.An implementation of the FESD method described in this paper is available in the recently introduced open-source package NOSNOC [1].Compared to standard discretizations, FESD achieves for a similar CPU time usually several orders of magnitude more accurate solutions.
A key advantage of the new approach for direct optimal control is that no guessing of the number or order of switches needs to be done.FESD can treat multiple or simultaneous switches and sliding modes.With time-freezing [40,37,35] many nonsmooth systems with state jumps can be recast into a piecewise smooth system.This allows us to treat many classes of nonsmooth systems in direct optimal control in a unified way.
In future work, one should relax some of the possibly restrictive assumptions in our theoretical analysis.We aim to extend FESD to other transformations of piecewise smooth systems into dynamic complementarity systems, e.g., the via the step function approach [5,17].Some further open questions to be answered are: Are all limit points of the solution approximations candidates xh (t) indeed solutions to the Filippov DI (2)? Do unique Filippov solutions to a given problem also imply a unique solution to the corresponding FESD problem (32)?

A Auxiliary results needed for Theorem 17
This lemma is about the active sets of perturbed LCP, where strict complementarity holds at a solution.

:
The complementary conditions for two vectors a, b ∈ R n read as 0 ≤ a ⊥ b ≥ 0, where a ⊥ b means a ⊤ b = 0.For two scalar variables a, b the so-called C-functions [20, Section 1.5.1]have the property φ(a, b) = 0 ⇐⇒ a ≥ 0, b ≥ 0, ab = 0. Examples are the natural residual functions φ NR (a, b) = min(a, b) or the Fischer-Burmeister function φ FB (a, b) = a + b − √ a 2 + b 2 .If a, b ∈ R n , we use φ(•) componentwise and define Φ(a, b) = (φ(a 1 , b 1 ), . . ., φ(a n , b n )).All vector inequalities are to be understood element-wise, diag(x) ∈ R n×n returns a diagonal matrix with x ∈ R n containing the diagonal entries.The concatenation of two column vectors

Fig. 3
Fig.3Illustration of the analytic solution and a polynomial solution approximation to a PSS via an IRK Radau-IIA method of order 7.The left plot shows an approximation with a fixed step size where an active-set change happens on a stage point.The right plot shows an approximation obtained with FESD (based on the same IRK method) where the switch happens on the boundary.The circles represent the stage values, the vertical dotted lines the finite elements boundaries, and the vertical dashed line the switching time ts.

Fig. 4
Fig. 4 An illustration of the standard complementarity conditions Ψ(Θ, Λ) = 0 (left plot) and the standard complementarity conditions augmented by 0 = Gcross(Θ, Λ) (right plot).The dots represent the stage values.The vertical dotted line marks the finite element boundaries, and the vertical dashed line marks the switching time ts.In the standard case (left plot), an active-set change can happen at any complementarity pair.With the cross complementarities (29) (right plot) an active-set change can only happen on the boundaries of a finite element.

Fig. 5
Fig.5Illustration of the discontinuity of the solution map of (36) for the PSS ẋ ∈ 2 − sign(x) + x 2 for T = 0.2 and N FE = 2.

Fig. 12
Fig.12The left plot depicts the switching indicator function η(•) at the solution of the OCP (61).The middle plot show the step size h n,k with step equilibration.The right plot shows the step sizes without step equilibration.The horizontal dashed lines correspond to the minimum, maximum, and nominal step size, the vertical dotted lines correspond to control interval boundaries.

Fig. 13
Fig. 13 Terminal constraint satisfaction vs. CPU time for the Standard and FESD method.The size of the marker indicates the number of stage points, the smallest corresponds to ns = 1 and the largest to ns = 4.