This paper extends the Finite Elements with Switch Detection and Jumps (FESD-J) [1] method to problems of rigid body dynamics involving patch contacts. The  method is a high accuracy discretization scheme suitable for use in direct optimal control of nonsmooth mechanical systems. It detects dynamic switches exactly in time and, thereby, maintains the integration order of the underlying Runge-Kutta (RK) method. This is in contrast to commonly used time-stepping methods which only achieve first-order accuracy. Considering rigid bodies with possible patch contacts results in nondifferentiable signed distance functions (SDF), which introduces additional nonsmoothness into the dynamical system. In this work, we utilize so-called equivalent contact points (ECP), which parameterize force and impulse distributions on contact patches by evaluation at single points. We embed a nondifferentiable SDF into a complementarity Lagrangian system (CLS) and show that the determined ECP are well-defined. We then extend the FESD-J discretization to the considered CLS such that its integration accuracy is maintained. The functionality of the method is illustrated for both a simulation and an optimal control example.

§ INTRODUCTION

The use of optimal control methods is advantageous for many robotic tasks as system dynamics and constraints can be easily integrated and resulting control inputs fulfill predefined optimality criteria. Applications such as locomotion and manipulation can be captured by complementarity  systems (CLS) with state jumps [2], which consider standard Newton-Euler equations of rigid body dynamics together with complementarity conditions for force interactions and impact laws. Since impacts and contact forces result in jumps and kinks of system velocities, these CLS fall into the class of nonsmooth dynamical systems. This makes using standard methods applied in direct optimal control difficult, as integration of dynamical systems is usually accomplished by utilizing high-order integration methods such as Runge-Kutta (RK) schemes across fixed time intervals, which for nonsmooth systems only achieves first-order accuracy [3]. A large part of the existing work on direct optimal control for nonsmooth systems relies on  Euler time-stepping methods [4, 5]. These methods do not aim to detect switches in the system exactly but rather integrate forces and impulses simultaneously over fixed time intervals. Thereby, they produce wrong numerical sensitivities and introduce artificial minima in which the optimizer can get stuck [6, 7].

Recently, the Finite Elements with Switch Detection and Jumps (FESD-J) [1] method was introduced, which is an event-based discretization scheme for nonsmooth systems suitable for use in direct optimal control. FESD-J allows to detect switches in the dynamical system and state jumps exactly in time. Thereby, the integration order of an underlying RK method is recovered. The method was introduced, however, for the case where signed distance functions (SDF) are sufficiently smooth.

When considering problems of rigid body dynamics originating from real-world applications, in many cases objects interacting with each other admit to contact surfaces that are not singletons. A patch contact occurs if there are multiple distinct points that are simultaneously contained in the surfaces of two considered objects. To derive rigid body dynamic models of such systems one has to consider SDF between the involved objects, which are for the case of possible patch contacts nondifferentiable. This introduces additional nonsmoothness into the system and thereby provides further challenges for numerical treatment. In [5] a nondifferentiable SDF was integrated in a time-stepping method that allows to simulate the dynamical evolution of polytopic bodies making contact. The concept of an equivalent contact point (ECP) was introduced, which is a unique point on the contact patch such that force acting on this point is equivalent to a force distribution acting across the full contact patch. It was shown that for given discrete time position and velocity solutions of the time-stepping discretization, the corresponding ECP and force magnitudes are unique.

If one uses an event-based discretization method, impulses across patch contacts are simulated in an isolated manner. This is contrary to time-stepping methods as used , which treat impulses and forces simultaneously. Since impulses are complex microscopic phenomena that heavily depend on material properties, in the field of optimal control one often assumes that involved bodies are perfectly rigid and then uses a phenomenological model for impact . When dealing with patch contacts, one has to resolve multiple simultaneous impacts at once, as the velocity of all points on the contact patch needs to undergo a jump to prevent penetration of bodies in the next time instant. Treating simultaneous impacts requires additional modeling .

§.§ Contribution

In this work, we embed a nondifferentiable SDF modeled as a convex optimization problem into a CLS with state jumps to obtain a continuous time model for rigid body dynamics of objects with possible patch contacts. In particular, we focus on the frictionless case and consider padded polytopes, which are obtained by sweeping standard polytopes with a sphere of arbitrarily small radius. This modeling decision is taken to avoid impacts between sharp corners, which require special treatment [9]. We then propose a multi-impact law based on the considered SDF and show that for polytopic contact patches this law is equivalent to Moreau's impact law [10]. We argue that the CLS is well-defined, i.e., that it produces a unique evolution of state trajectories (Section <ref>).

The FESD-J method is then extended to our model, such that the integration order is maintained and the considered impact law is reflected in the discretization. To this end, we have to introduce additional so-called cross complementarity conditions to detect switches in the nondifferentiable SDF exactly in time (Section <ref>).

The functionality of the model and discretization is illustrated in two numerical examples, namely the simulation of a falling cuboid making contact with its environment and the optimal control of a planar manipulation task (Section <ref>).

§.§ Notation

Left- and right-hand side limits of a function at some evaluation time $t_{\mathrm{s}}$ are denoted by $x(t_{\mathrm{s}}^{+}) = \displaystyle\lim_{t\rightarrow t_{\mathrm{s}}, t> t_{\mathrm{s}}} x(t)$ and $x(t_{\mathrm{s}}^{-}) = \displaystyle\lim_{t\rightarrow t_{\mathrm{s}}, t< t_{\mathrm{s}}} x(t)$, respectively. For ease of notation, we drop time dependencies of differential and algebraic variables when it is clear from the context. The complementarity condition for two vectors $a, b\in \mathbb{R}^{n}$ is denoted by , where $ a  \perp  b$ means $ a ^{\top} b = 0$. We denote the rotation matrix in two dimensions for some $\theta \in \mathbb{R}$ by

\begin{equation*}
R(\theta) = \begin{pmatrix}
\cos(\theta) & -\sin{\theta} \\
\sin(\theta) & \cos{\theta}
\end{pmatrix}.
\end{equation*}

The cross product for two vectors $a, b\in \mathbb{R}^{2}$ is given by
$a \times b = a^{\top} R\left(-\frac{\pi}{2}\right)b = a_1 b_2 - a_2b_1.$ Given a vector $a \in \mathbb{R}^{n}$ and a set of indices $\mathcal{I} \subset \{1,\dots,n\}$, $a_{\mathcal{I}} \in \mathbb{R}^{\vert\mathcal{I} \vert}$ is the vector only containing the components of $a$ with indices in $\mathcal{I}$.

§ RIGID BODY DYNAMICS WITH PATCH CONTACTS

We consider CLS with state jumps that arise in nonsmooth mechanics for rigid bodies with inelastic impacts. In particular, we consider the case where these bodies are modeled by padded polytopes. This is a challenging problem since SDF between such objects are not everywhere differentiable, which introduces additional nonsmoothness into the system.

In the following, we first express a SDF between padded polytopes as an optimization problem and discuss properties of resulting primal and dual variables in Section <ref>. In Section <ref>, we state the considered CLS and discuss how to obtain contact normal vectors that specify the direction along which forces and impulses act on the system. We then utilize the SDF to formulate a simultaneous impact law and relate our formulation to Moreau's impact law [10] in Section <ref>. Finally, in Section <ref>, we show that the considered CLS produces a unique evolution of state trajectories. In particular, if objects make patch contacts, there is a unique ECP chosen to parameterize the contact force interaction.

Visualization of two unscaled padded polytopes in bold color, their downsized interior polytopes (dotted) and exemplary circles used for padding (dashed). Further, scaled versions of the padded polytopes according to the scaling factor $\alpha$ determined by the distance problem (<ref>) are visualized in light color.

§.§ Signed Distance Function for Padded Polytopes

Consider two planar polytopic bodies given in a halfspace representation

\begin{equation*}
\mathcal{P}_{1} = \{p \in \mathbb{R}^{2}\ \vert \  A_{1}x \leq b_{1}\}, \ \mathcal{P}_{2} = \{p \in \mathbb{R}^{2} \ \vert \  A_{2}x\leq b_{2}\},
\end{equation*}

where $A_{1} \in \mathbb{R}^{\nha \times 2}$, $b_{1} \in \mathbb{R}^{\nha}$, $A_{2} \in \mathbb{R}^{\nhb \times 2}$, . Without loss of generality, assume that for a given mass distribution, the halfspace representation of the polytopes is constructed such that their center of mass (CoM) coincides with the origin. Furthermore, let the rows of $A_{1}$ and $A_{2}$ be vectors with norm unity. We also assume that for a scalar $r>0$, it holds $b_{i} - \bm{1}r > 0$, $i=1,2$, where $\bm{1} = (1,\dots,1)^{\top}$ denotes the vector of all ones. This means that the minimum Euclidean distance from the CoM to the boundary of the respective polytopes is larger than $r$.

We now transform the polytopes by several alterations. First we associate degrees of freedom (DoF) with them, namely $c_{i}\in \mathbb{R}^{2}$ for the position of their CoM and $\xi_{i} \in \mathbb{R}$ for their orientation, both with respect to a world coordinate frame. We capture all DoF of the bodies in a system configuration variable $q = (c_1,\xi_1,c_2,\xi_2)$. Next, we replace the right-hand side vectors $b_i$ by $b_i - \bm{1}r$ to obtain downsized polytopes, which are then swept by a sphere of radius $r$. This yields an inner approximation of the original polytopes with round corners. Finally, we introduce the scaling factor $\alpha > 0$, which allows one to obtain scaled versions of the padded polytopes by altering the size of the downsized polytopes, for $\alpha = 1$ the original size is retained. We obtain analytic expressions for the scaled padded polytopes by

ℬ_1(q,α)    = {p ∈ℝ^2 |∃  y_1  ∈ℝ^2:

A_1R( ξ_1)^⊤(y_1-c_1) ≤αb_1 - 1r, ‖p - y_1 ‖_2 ≤r},

ℬ_2(q,α)    = {p ∈ℝ^2 |∃  y_2  ∈ℝ^2:

A_2R( ξ_2)^⊤(y_2-c_2) ≤αb_2 - 1r, ‖p - y_2 ‖_2 ≤r}.

See Fig. <ref> for a visualization of these sets.

A SDF $\Phi(q)$ for the padded polytopes $\mathcal{B}_{1}(q,1)$ and $\mathcal{B}_{2}(q,1)$  can be expressed by determining the smallest scaling factor such that there is a common point in both scaled bodies [11]. Using the analytic representations of the sets derived above, we formulate this optimization problem by

p,α, y_1,y_2α-1aaaaaaaaaaaaaaaΦ(q) =
A_1R( ξ_1)^⊤(y_1-c_1)≤αb_1 - 1r,
A_2R( ξ_2)^⊤(y_2-c_2)≤αb_2 - 1r,

which we write compactly as

\begin{equation} \label{distprob_compact}
\Phi(q) =\min_{z} \ f_{\mathrm{d}}(z) \quad \mathrm{s.t.} \quad g_{\mathrm{d}}(z,q)\leq 0,
\end{equation}

where $z = (p,\alpha, y_{1},y_{2}) \in \mathbb{R}^{7}$ collects all primal variables.

We observe that $\Phi(q) = 0 $ means the unscaled padded polytopes touch in at least one point $p$. Furthermore, $\Phi(q) > 0 $ indicates that the size of the padded polytopes has to be increased to find a common point $p$, and on the contrary, $\Phi(q) < 0$ implies the size has to be decreased to find such a point.

The convex optimization problem (<ref>) fulfills Slater's constraint qualification, as there always exists a feasible point for which the constraints are not active. This can be seen from Fig. <ref> as by choosing a larger $\alpha$ the scaled padded polytopes will overlap and, thus, $p$, $y_1$, $y_2$, can be chosen such that the constraints of (<ref>) are fulfilled with strict inequalities. Consequently, the Karush-Kuhn-Tucker (KKT) conditions are necessary and sufficient for optimality.

In the following, we will investigate how optimal primal and dual solutions to (<ref>) depend on the system . To this end, we associate nonnegative Lagrange multipliers $\mu_{1, \mathrm{p}} \in \mathbb{R}^{\nha}$, $\mu_{2, \mathrm{p}}\in \mathbb{R}^{\nhb}$, , $\mu_{2, \mathrm{c}}\in \mathbb{R}$, with the constraints (<ref>)-(<ref>) and collect them in the vector $\mu = (\mu_{1, \mathrm{p}},\mu_{2, \mathrm{p}},\mu_{1, \mathrm{c}},\mu_{2, \mathrm{c}})$. The stationary KKT condition of (<ref>), i.e., $\nabla_{z} f_{\mathrm{d}}(z) + \nabla_{z} g_{\mathrm{d}}(z,q)\mu = 0$, evaluates as the system of equations given by

\begin{equation} \label{statKKTcond}
\begin{split}
1 -b_{1}^{\top} \mu_{1, \mathrm{p}} -b_{2}^{\top} \mu_{2, \mathrm{p}} &= 0, \\
2(p-y_{1})\mu_{1, \mathrm{c}} &= - 2(p-y_{2})\mu_{2, \mathrm{c}},  \\
R( \xi_{1})A_{1}^{\top}\mu_{1, \mathrm{p}} &=  2(p-y_{1}) \mu_{1, \mathrm{c}}, \\
R( \xi_{2})A_{2}^{\top}\mu_{2, \mathrm{p}} &=  2(p-y_{2}) \mu_{2, \mathrm{c}}.
\end{split}
\end{equation}

\begin{align*}
\mathcal{A}_{1,\mathrm{p}} = \{i \in \{1,\dots,\nha \} \mid  \mu_{1, \mathrm{p},i} > 0\}, \\
\mathcal{A}_{2,\mathrm{p}} = \{i \in \{1,\dots,\nhb \} \mid  \mu_{2, \mathrm{p},i} > 0\},
\end{align*}

are the active sets for the constraints (<ref>) and (<ref>), respectively.

The following proposition holds true for problem (<ref>).

We denote by

\begin{align*}
&\mathcal{A}_{1,\mathrm{p}} = \{i \ \vert \ \mu_{1,\mathrm{p},i} > 0\}, \\
&\mathcal{A}_{2,\mathrm{p}} = \{i \ \vert \ \mu_{2,\mathrm{p},i} > 0\},
\end{align*}

the active sets for the constraints (<ref>)-(<ref>). By investigating the system of equations given by the stationary KKT condition (<ref>) and the equality conditions imposed by active constraints of (<ref>) given by

\begin{align*}
A_{1,\mathcal{A}_{1,\mathrm{p}}}R( \xi_{1})^{\top}(y_{1}-c_{1}) &= \alpha b_{1,\mathcal{A}_{1,\mathrm{p}}} - \bm{1}r, \\
A_{2,\mathcal{A}_{2,\mathrm{p}}}R( \xi_{2})^{\top}(y_{2}-c_{2}) &=\alpha b_{2,\mathcal{A}_{2,\mathrm{p}}} - \bm{1}r, \\
(p-y_{1})^{\top}(p-y_{1}) &=  r^2, \\
(p-y_{2})^{\top}(p-y_{2}) &= r^2,
\end{align*}

one can derive the following proposition.

For any  $q \in \mathbb{R}^{n_{q}}$, apart from the degenerate case $c_1 = c_2$, optimal dual variables for (<ref>) are unique and depend continuously on $q$. Optimal primal variables are unique if there is no patch contact between the scaled padded polytopes.

A full proof can be found in Appendix <ref>. In the following, we briefly give some intuition on why this holds true with the help of the visualizations in Fig. <ref>. First, we observe that the optimal scaling factor $\alpha$ is uniquely determined as increasing it violates optimality and if it is decreased, no common point $p$ is included in both scaled bodies. In the case of Fig. <ref> or Fig. <ref>, we observe that there is a unique point $p$ that is element of both scaled shapes, as well as unique points $y_1$ and $y_2$ that are elements of the downscaled polytopes. In the case of Fig. <ref>, we observe that there are multiple viable solutions for $p$, $y_1$ and $y_2$. However, we note that the vectors $p-y_{1}$ and $p-y_{2}$ are unique in any case of Fig. <ref>. Consequently, one can show that system (<ref>) together with the complementarity condition of the KKT system is uniquely solvable for the Lagrange multipliers.

§.§ Complementarity Lagrangian System

Dynamic interactions between rigid bodies are commonly modeled by CLS, which combine Newton-Euler equations with the complementarity constraint $0 \leq \Phi(q) \perp \lambda_{\mathrm{n}} \geq 0$, where $\lambda_{\mathrm{n}}$ is the contact force magnitude. If $\Phi(q)$ is differentiable, forces and impulses act along the SDF gradient, which is referred to as the contact normal vector. Since the SDF considered here is not differentiable for configurations as in Fig. <ref>, standard theory does not directly imply. However, we can determine directional derivatives by

\begin{equation} \label{dirderv}
\partial_{d}\Phi(q) = \min_{z, \mu \text{ solve } (\ref{distprob})} d^{\top} \nabla_q  g_{\mathrm{d}}(z,q) \mu ,
\end{equation}

for the direction $d\in \mathbb{R}^{n_{q}}$, cf. [12]. Thus, we observe that $\Phi(q)$ is differentiable whenever there exists a unique primal solution to the distance problem (<ref>). Equation (<ref>) motivates us to consider contact normals given by

\begin{align}
n(z,\mu,q) = \nabla_q  g_{\mathrm{d}}(z,q) \mu &= \begin{pmatrix}
n_{\mathrm{tr}}(\mu,q) \\
(p - c_{1}) \times 	n_{\mathrm{tr}}(\mu,q) \\
-n_{\mathrm{tr}}(\mu,q) \\
- (p - c_{2}) \times 	n_{\mathrm{tr}}(\mu,q)
\end{pmatrix}, \label{normalform} \\
n_{\mathrm{tr}}(\mu,q) &= - R(\xi_{1})A_{1}^{\top}\mu_{1,\mathrm{p}}. \nonumber
\end{align}

The right-hand side of (<ref>) is obtained by evaluating $\nabla_q  g_{\mathrm{d}}(z,q)$ and by using equality conditions . We observe that $n(z,\mu,q)$ is the gradient of $ \Phi(q)$ for configurations where the SDF is differentiable. We show that if $n(z,\mu,q)$ is used to simulate forces or impulses, the ambiguity for nondifferentiable configurations is resolved due to the combined consideration with the Newton-Euler equations.

We formulate a CLS by combining the KKT conditions of (<ref>) together with the Newton-Euler equations for rigid body dynamics and obtain

\begin{align}
&\dot{q} = \nu, \label{cls:a}\\
&M \dot{\nu} = f_{\mathrm{v}}(q,\nu,u) + n(z,\mu,q)\lambda_{\mathrm{n}}, \label{cls:b}\\
&0 \leq  f_{\mathrm{d}}(z)  \ \perp \ \lambda_{\mathrm{n}} \geq 0, \label{cls:c} \\
&\nabla_{z} f_{\mathrm{d}}(z) + \nabla_{z} g_{\mathrm{d}}(z,q) \mu = 0, \label{cls:d} \\
&0 \leq \mu\ \perp \ -g_{\mathrm{d}}(z,q) \geq 0, \label{cls:e}\\
&\text{State jump law (Section \ref{section1imp})} \label{cls:f},
\end{align}

where $\nu \in \mathbb{R}^{n_\mathrm{q}}$ is the system velocity and $M \in \mathbb{R}^{n_\mathrm{q} \times n_\mathrm{q}}$ is the inertia matrix, which is positive definite and diagonal. Furthermore, the function $f_{\mathrm{v}}: \mathbb{R}^{n_\mathrm{q}}\times \mathbb{R}^{n_\mathrm{q}} \times \mathbb{R}^{n_\mathrm{u}} \rightarrow \mathbb{R}^{n_\mathrm{q}}$ captures external forces acting on the system, e.g., such as gravity or forces imposed by a control input $u \in \mathbb{R}^{n_\mathrm{u}}$.

Since the system (<ref>)-(<ref>) can enter a state where violation of $0 \leq  f_{\mathrm{d}}(z)$ becomes unavoidable, an impact has to occur, that is an instantaneous jump of velocities which keeps the system in the feasible domain.

§.§ Simultaneous Impact Law

In the following, an impact law is derived that determines how the CLS (<ref>) evolves if it enters a state $q(t_{\mathrm{s}})$ with $\Phi(q(t_\mathrm{s})) = 0$ and there exists a solution $(z, \mu)$ to (<ref>) such that $n(z,\mu,q(t_\mathrm{s}))^{\top} \nu(t_\mathrm{s}^{-})  < 0$. In this case, the system velocities need to undergo an instantaneous jump to avoid penetration of the objects. Here we have to deal with simultaneous impacts, as in the case of a patch contact, it has to be ensured that after an impact all points on the contact patch have a positive relative velocity with respect to the halfspace separating the objects, cf. Fig. <ref>.

Moreau's impact law is commonly stated for the case where one has $n_{\mathrm{v}} \in \mathbb{N}$ continuously differentiable SDF $\tilde{\Phi}_{i}(q), \ i = 1,\dots, n_{\mathrm{v}}$, that all become active at , i.e., $\tilde{\Phi}_{i}(q(t_\mathrm{s})) = 0$. Given the corresponding contact normals $\tilde{n}_{i}(q) = \nabla_{q}\tilde{\Phi}_{i}(q)$, Moreau's impact law can be stated as a mixed linear complementarity problem (MLCP), . In this formulation, one aims to find $\tilde{\Lambda}_{\mathrm{n},i} \in \mathbb{R}, \ i = 1,\dots, n_{\mathrm{v}}$, such that

\begin{align}
&\nu(t_\mathrm{s}^{+}) = \nu(t_\mathrm{s}^{-}) + M^{-1} \sum_{i = 1}^{n_{\mathrm{v}}}\tilde{n}_{i}(q(t_\mathrm{s})) \tilde{\Lambda}_{\mathrm{n},i}, \label{moreau:a}\\
&0 \leq \tilde{\Lambda}_{\mathrm{n},i}, \quad  0 \leq \tilde{n}_{i}(q(t_\mathrm{s}))^{\top}\nu(t_\mathrm{s}^{+}), \quad i = 1,\dots,n_{\mathrm{v}}, \\
& 0= \left(\sum_{i = 1}^{n_{\mathrm{v}}} \tilde{\Lambda}_{\mathrm{n},i} \tilde{n}_{i}(q(t_\mathrm{s})) \right)^{\top}\nu(t_\mathrm{s}^{+}).
\end{align}

Now, for the case of impacts between padded polytopes, one obtains, generally both in two and three dimensions, polytopic contact patches. Therefore, one can locally resolve an impact, by considering the $n_{\mathrm{v}}$ vertices, denoted by $w_{i}$, of the polytopic contact patch in terms of the SDF derived in Section <ref>. We obtain

\begin{equation*}
\left\{\sum_{i=1}^{n_{\mathrm{v}}}\theta_{i}w_{i}  \mid  \bm{1}^{\top}\theta = 1, \ \theta_{i} \geq 0 \right\} = \{p \mid (z, \mu) \text{ solve }(\ref{distprob}) \},
\end{equation*}

where we remind the reader that $p$ is included in $z$.

The contact patch vertices $w_{i}$ are at the moment of impact elements of both considered bodies. By virtually fixing them to one body and defining the SDF $\tilde{\Phi}_{i}(q)$ such that they encode the point-object distance between $w_{i}$ fixed to one body and the other body, one can apply Moreau's law to solve for the resulting impulse. For general numerical simulation this is impractical as one does not know beforehand which contacts form and has generally no known expression for the vertices $w_{i}$ and the SDF $\tilde{\Phi}_{i}(q)$.

We now consider the SDF defined in Section <ref> and show that it allows us to encode the above impact law, without the need to locally enumerate vertices of contact patches. To this end, let $(z, \mu)$ solve (<ref>) at $q(t_\mathrm{s})$, then the corresponding $p$ is an element of the contact patch. We observe that, $n(z,\mu,q)$ is a linear function in $p$ and does not depend on the other components of $z$. Due to the definition of the functions $\tilde{\Phi}_{i}(q)$ it holds for $\tilde{z}_{i} = (w_{i},\dots)$, $n(\tilde{z}_{i},\mu,q(t_\mathrm{s})) = \tilde{n}_{i}(q(t_\mathrm{s}))$. Consequently, for any $(z, \mu)$ solving (<ref>) at $q(t_\mathrm{s})$ and any $\Lambda_{\mathrm{n}} \geq 0$, there exist , $ \ i = 1,\dots,n_{\mathrm{v}}$, such that

\begin{equation*}
n(z,\mu,q(t_\mathrm{s})) \Lambda_{\mathrm{n}} = \sum_{i = 1}^{n_{\mathrm{v}}}\tilde{n}_{i}(q(t_\mathrm{s})) \tilde{\Lambda}_{\mathrm{n},i}.
\end{equation*}

Now, if it holds

\begin{align*}
0 \leq \tilde{n}_{i}(q(t_\mathrm{s}))^{\top}\nu(t_\mathrm{s}^{+}), \quad i = 1,\dots,n_{\mathrm{v}},
\end{align*}

then for all $\hat{\Lambda}_{\mathrm{n},i}\geq 0, \ i = 1,\dots, n_{\mathrm{v}}$, it follows

\begin{align*}
0 \leq \left(\sum_{i = 1}^{n_{\mathrm{v}}} \hat{\Lambda}_{\mathrm{n},i} \tilde{n}_{i}(q(t_\mathrm{s})) \right)^{\top}\nu(t_\mathrm{s}^{+}),
\end{align*}

and vice versa. Consequently, solving (<ref>) is equivalent to determining $\Lambda_{\mathrm{n}} \geq 0$ and $(z_{\mathrm{I}},\mu_{\mathrm{I}})$ solving (<ref>) at $q(t_\mathrm{s})$ such that

\begin{align}
&\nu(t_{\mathrm{s}}^{+}) = \nu(t_{\mathrm{s}}^{-}) + M^{-1} n(z_{\mathrm{I}},\mu_{\mathrm{I}},q(t_{\mathrm{s}})) \Lambda_{\mathrm{n}}, \label{impactres:a} \\
&0 \leq n(\hat{z}_{\mathrm{I}},\hat{\mu}_{\mathrm{I}},q(t_{\mathrm{s}}))^{\top} v(t_{\mathrm{s}}^{+}),  \ \forall \, (\hat{z}_{\mathrm{I}},\hat{\mu}_{\mathrm{I}}) \text{ solving } (\ref{distprob}), \label{impactres:b}\\
& 0 = n(z_{\mathrm{I}},\mu_{\mathrm{I}},q(t_{\mathrm{s}}))^{\top} v(t_{\mathrm{s}}^{+}). \label{impactres:c}
\end{align}

Since Moreau's impact law provides unique post-impact velocities $\nu(t_{\mathrm{s}}^{+})$ <cit.>, (<ref>) implies that $z_{\mathrm{I}}$ and in particular $p_{\mathrm{I}}$ are also unique. Thus, $p_{\mathrm{I}}$ can be viewed as a unique ECP for impact resolution.

Now, numerical evaluation of (<ref>) is generally not possible as we do not have a representation of the solution set of (<ref>), but only have access to single evaluations. Since this condition encodes that all points on the contact surface have a post-impact velocity such that no penetration occurs, we propose to substitute (<ref>) by the condition

\begin{equation} \label{expeulerstep}
0 \leq \Phi(q(t_{\mathrm{s}}) + h_{\mathrm{E}} \nu(t_{\mathrm{s}}^{+})),
\end{equation}

where $0 < h_{\mathrm{E}}  << 1$ is a small positive constant. Thus, we consider one additional evaluation of the SDF by using an explicit Euler step over a small time interval. If the objects do not intersect an arbitrarily small amount of time after the impulse has acted, (<ref>) was fulfilled.

There are, however, some corner cases where (<ref>) can become infeasible. For example, if two objects with straight surfaces make contact such that the angle between the surfaces is arbitrarily small, then simulating the post-impact velocity forward in time will likely result in a configuration with penetration. On the other hand, if bounds on impact angles and system velocities are known beforehand, $h_{\mathrm{E}}$ can be chosen sufficiently small to prevent this issue. Since we are mainly interested in optimal control applications, this issue has lesser significance as the optimizer can easily avoid extremely small impact angles.

We note that by using the proposed impact law with the SDF (<ref>), we do not necessarily require that we have a polytopic contact patch. For example, in three dimensions one could consider a (padded) cylinder impacting a flat surface such that a circular contact patch forms. Treating such a case with the form (<ref>) of Moreau's impact law will not be possible, as infinitely many differentiable SDF would have to be considered. On the other hand, numerical evaluation of (<ref>) with the approximation (<ref>) is feasible.

Consider the system configuration in Fig. <ref>, we have one rigid body with DoF colliding with another fixed body at time $t_{\mathrm{s}}$. Let the pre-impact velocity be given by $\nu^{(1)}(t_{\mathrm{s}}^{-}) = (0,-1,-0.4)$ or $\nu^{(2)}(t_{\mathrm{s}}^{-}) = (0,-1,-0.4)$. We further have $c_{1} = (0,1), \ p(s) = (s,0),  \ n(s) = (0 , 1, s)$, i.e., we parametrize the contact patch and, thus, the contact point and normal by the scalar $s \in [-1,1]$. In the following, we will then examine, which values of $s$ fulfill the impact law (<ref>)-(<ref>). First, we observe that to fulfill (<ref>) for a certain $s$, it has to hold

$$\Lambda_{\mathrm{n}}(s) = \frac{-n(s)^{\top} \nu(t_{\mathrm{s}}^{-})}{n(s)^{\top}M^{-1}n(s)}.$$

Next we define $C(r, s) = n(r)^{\top}(\nu(t_{\mathrm{s}}^{-}) + M^{-1}  n(s) \Lambda_{\mathrm{n}}(s))$, condition (<ref>) becomes equivalent to

\begin{equation} \label{feascond}
C(r,s) \geq 0, \forall r \in [-1,1].
\end{equation}

Since $C(r,s)$ is linear in $r$, this is equivalent to $C(-1,s) \geq 0, C(1,s) \geq 0$. Fig. <ref> visualizes the functions $C(-1,s), C(1,s)$ for two different values of $\nu(t_{\mathrm{s}}^{-})$. We observe that in both cases there is a unique $s$, and consequentially a unique contact point and normal, such that (<ref>) is fulfilled. This illustrates that the impact law (<ref>) produces unique impulses.

§.§ Well-Posedness of CLS and Uniqueness of ECP

In the following, we show that the CLS (<ref>) is well-posed and admits unique state trajectories $(q,\nu)$. This is not directly clear as the here considered SDF may yield many possible candidates for the ECP $p$, which in turn could result in ambiguous contact forces that are applied to the system.

If we have $f_{\mathrm{d}}(z) > 0$, then then it follows $\lamn = 0$. Therefore, the primal-dual variables of the embedded distance problem are decoupled from the dynamics and there may be multiple primal variables solving the system.

Now assume $\lambda_{\mathrm{n}} > 0$ for some time interval $(t_1,t_2)$, then we have $f_{\mathrm{d}}(z) = 0$ and contact forces are active to prevent penetration of the objects. The ECP $p$ is by definition an element of the contact patch and $\ntr$ induces a separating hyperplane between the objects. If we virtually fix $p$ onto either object, its velocity in world coordinates is given by

\begin{align*}
V_{1} &= v_{1} + \omega_{1} \times (p - c_{1}), \\
V_{2} &= v_{2} + \omega_{2} \times (p - c_{2}).
\end{align*}

I.e., $V_{1}$ and $V_{2}$ describes how $p$ would move along with the first or second object, respectively. If contact persists during $(t_1,t_2)$, we conclude that the condition

\begin{equation} \label{contactzerovel}
n(z, \mu, q)^{\top}\nu = n_{\mathrm{tr}}^{\top} (V_{1}(p) - V_{2}(p)) = 0
\end{equation}

has to hold, as the ECP $p$ can only have relative velocity along the separating halfspace between the objects.

The following proposition is the main result that ensures uniqueness of state trajectories.

On every time interval $(t_{1},t_{2})$ during which the CLS (<ref>) evolves with $\lamn > 0$, no impact occurs and the active set of the SDF does not change, it has a unique solution.

A full proof can be found in Appendix <ref>. We briefly outline the key steps of the proof in the following.

Under the assumptions of Proposition <ref>, condition (<ref>) applies. Thus, the CLS (<ref>) evolves according to the DAE

\begin{align}
&\dot{q} = \nu, \label{contactDAE:a} \\
&M \dot{\nu} = f_{\mathrm{v}}(q,\nu,u) + n(z, \mu, q)\lambda_{\mathrm{n}}, \label{contactDAE:b} \\
&f_{\mathrm{d}}(z) = 0, \\
&n(z, \mu, q)^{\top}\nu = 0, \label{contactDAE:d}\\
&\nabla_{z} f_{\mathrm{d}}(z) + \nabla_{z} g_{\mathrm{d}}(z,q) \mu = 0, \label{contactDAE:e}\\
&0 = \mu_{\mathcal{I}(q)}, \label{contactDAE:f}\\
&0 = g_{\mathrm{d}}(z,q)_{\mathcal{A}(q)}, \label{contactDAE:g}
\end{align}

where $\mathcal{A}(q) \subset \{1,\dots,\nha+\nhb + 2\}$ and $\mathcal{I}(q) = \{1,\dots,\nha+\nhb + 2\} \setminus \mathcal{A}(q)$ denote the index sets for the active and inactive constraints of (<ref>).

For those active sets of the SDF without patch contacts, primal and dual variables of the distance problem are unique. The KKT system (<ref>)-(<ref>) then is a smooth system of equations with an invertible Jacobian. The implicit function theorem yields continuously differentiable functions for $z$ and $\mu$ in terms of the differential states and the control input. Differentiating (<ref>) with respect to time yields a similar differentiable function for $\lamn$, c.f., [13]. Therefore, the DAE (<ref>) can be equivalently stated as an ODE with a differentiable right-hand side, and the Picard-Lindelöf theorem implies uniqueness of solutions.

The more challenging case is the one, where the sets $\mathcal{A}(q)$ and $\mathcal{I}(q)$ are such that patch contact is maintained. Then the distance problem has multiple viable primal solutions. The stationary KKT condition (<ref>) implies in this case

\begin{equation} \label{uniquecondrot}
R(\xi_{1})A^{\top}_{1,k_{1}} + R(\xi_{2})A^{\top}_{2,k_{2}}  = 0,
\end{equation}

where $A^{\top}_{i,j}$ is the $j$-th row of the matrix from the halfspace representation of the $i$-th object and $\{k_{1}\}$,$\{k_{2}\}$, are the singleton active sets of the constraints (<ref>), (<ref>), respectively. By differentiating (<ref>) twice with respect to time and by using equality conditions given by (<ref>), one obtains

\begin{equation} \label{rotacceq}
\dot{\omega}_{1} = \dot{\omega}_{2}.
\end{equation}

Equation (<ref>) then implies that the force components in (<ref>) acting on the rotational DoF have to be equal. Using this, and again differentiating (<ref>) with respect to time yields differentiable expressions for $z$, $\mu$ and $\lamn$ in terms of the differential states and the control input, which again allows use of the Picard-Lindelöf theorem.

We note that since the system velocity is obtained from integration of the right-hand side of (<ref>), switches of the SDF active set do not hinder uniqueness of state trajectories. Overall, one can conclude that state trajectories for the CLS (<ref>) evolve uniquely, as the considered impact law provides unique post-impact velocities and for every fixed set of the complementarities (<ref>) and (<ref>), unique evolution is ensured by the Picard-Lindelöf theorem. In particular, ECP which parameterize impulse and force interactions are well-defined.

§ FINITE ELEMENTS WITH SWITCH DETECTION AND STATE JUMPS

In this section, we adapt the FESD-J [1] method to the considered CLS (<ref>). To this end, in Section <ref> a standard RK discretization is stated. The discretized impact law is discussed in Section <ref>. In Section <ref>, so-called cross complementarity conditions are formulated, which ensure that dynamic switches are detected exactly in time. Finally, in Section <ref> the derived discretization scheme is integrated in a discrete time optimal control problem (OCP).

§.§ Runge-Kutta Integration

We consider a control interval $[0,T]$, with a constant control input $\hat{u} \in \mathbb{R}^{n_{\mathrm{u}}}$, that is divided into $N_{\mathrm{FE}}$ finite elements (FE) $[t_{n}, t_{n+1}]$, with $t_{n} < t_{n+1}$ and $\cup_{n = 0}^{N_{\mathrm{FE}}} [t_{n}, t_{n+1}] = [0,T]$. On each FE, we consider a $n_{\mathrm{s}}$-stage RK method that is parameterized by its Butcher tableau entries $\hat{a}_{i,j}, \hat{b}_{i}, \hat{c}_{i}$, with $i,j \in \{1,\dots,n_{\mathrm{s}} \}$ [14]. We denote the FE lengths by $h_{n} = t_{n+1} - t_{n}$. Approximations of the differential and algebraic states for the CLS (<ref>) at time $t_{n,i} = t_{n} + \hat{c}_{i} h_{n}$ are denoted by $q_{n,i}$, $\nu_{n,i}$, $\lambda_{\mathrm{n},n,i}$, $z_{n,i}$ and $\mu_{n,i}$, respectively. For the differential states, we additionally introduce the values $q_{n,0}$, $\nu_{n,0}$, which approximate the states at $q(t_{\mathrm{n}}^{+})$, $\nu(t_{\mathrm{n}}^{+})$, while $q_{n-1,n_{\mathrm{s}}}$, $\nu_{n-1,n_{\mathrm{s}}}$, approximate $q(t_{\mathrm{n}}^{-})$, $\nu(t_{\mathrm{n}}^{-})$. For ease of exposition, we assumed that it holds $\hat{c}_{n_{\mathrm{s}}} = 1$, c.f., [1] for the case $\hat{c}_{n_{\mathrm{s}}} < 1$. An example for RK methods that fulfill $\hat{c}_{n_{\mathrm{s}}} = 1$ is given by Radau IIA schemes. Introducing additional variables for the differential states is required as due to possible state jumps in velocity for the CLS (<ref>), we may have $\nu(t_{\mathrm{s}}^{-}) \neq \nu(t_{\mathrm{s}}^{+})$ for some $t_{\mathrm{s}}$. Now let

\begin{equation*}
F_{\mathrm{v}}(q,\nu,z,\mu,\lambda_{\mathrm{n}},u) = M^{-1}( f_{\mathrm{v}}(q,\nu,u) + n(z,\mu,q)\lambda_{\mathrm{n}}),
\end{equation*}

then the discretized differential equations of (<ref>)-(<ref>) read

\begin{equation} \label{rkdiff}
\begin{split}
q_{n,i} &= q_{n,0} + h_{n} \sum _{j = 1}^{n_{\mathrm{s}}} \hat{a}_{i,j} \nu_{n,j}, \\
\nu_{n,i} &= \nu_{n,0} + h_{n} \sum _{j = 1}^{n_{\mathrm{s}}} \hat{a}_{i,j} F_{\mathrm{v}}(q_{n,j},\nu_{n,j},z_{n,j},\mu_{n,j},\lambda_{\mathrm{n},n,j},\hat{u}),
\end{split}
\end{equation}

where an initial state $s_{0} = (q_{0,0},\nu_{0,0})$ at time $t_{0}$ has to be provided. The algebraic conditions in (<ref>) are also evaluated at the RK stage points and read

\begin{align}
&0 \leq  f_{\mathrm{d}}(z_{n,i})  \perp \lambda_{\mathrm{n},n,i} \geq 0,  \label{rkalg:a} \\
&\nabla_{z} f_{\mathrm{d}}(z_{n,i}) + \nabla_{z} g_{\mathrm{d}}(z_{n,i},q_{n,i}) \mu_{n,i} = 0, \\
&0 \leq \mu_{n,i} \perp -g_{\mathrm{d}}(z_{n,i},q_{n,i}) \geq 0. \label{rkalg:c}
\end{align}

§.§ Impact Equations

We observe that the configurations are continuous in time as they are obtained by integration of the velocities (<ref>). Therefore, we impose

\begin{equation*}
q_{n-1,n_{\mathrm{s}}} = q_{n,0}.
\end{equation*}

To connect the velocities at FE boundaries, the impact law (<ref>)-(<ref>) is incorporated into the discretization. To this end, two evaluations of the SDF at FE boundaries are required. This is achieved by evaluating the KKT conditions of (<ref>),

\begin{equation} \label{impKKT}
\begin{split}
&\nabla_{z} f_{\mathrm{d}}(z_{\mathrm{I},n}) + \nabla_{z} g_{\mathrm{d}}(z_{\mathrm{I},n}, q_{n,0}) \mu_{\mathrm{I},n} = 0, \\
&0 \leq \mu_{\mathrm{I},n} \perp -g_{\mathrm{d}}(z_{\mathrm{I},n},q_{n,0}) \geq 0, \\
&\nabla_{z} f_{\mathrm{d}}(z_{\mathrm{E},n}) + \nabla_{z} g_{\mathrm{d}}(z_{\mathrm{E},n}, q_{n,0} + h_{\mathrm{E}}\nu_{n,0} ) \mu_{\mathrm{E},n} = 0, \\
&0 \leq \mu_{\mathrm{E},n} \perp -g_{\mathrm{d}}(z_{\mathrm{E},n},q_{n,0} + h_{\mathrm{E}}\nu_{n,0}) \geq 0,
\end{split}
\end{equation}

with a small constant $h_{\mathrm{E}} > 0$. Thus, $(z_{\mathrm{I},n},\mu_{\mathrm{I},n})$ solves (<ref>) for $q_{n,0}$ and $(z_{\mathrm{E},n},\mu_{\mathrm{E},n})$ solves (<ref>) for $q_{n,0} + h_{\mathrm{E}}\nu_{n,0}$.
The impact equations are given by

\begin{align}
\nu_{n,0} &= \nu_{n-1,n_{\mathrm{s}}} + M^{-1}n(z_{\mathrm{I},n},\mu_{\mathrm{I},n},q_{n,0}) \Lambda_{\mathrm{n},n}, \label{discimpeq:a}\\
0 &= \Lambda_{\mathrm{n},n} n(z_{\mathrm{I},n},\mu_{\mathrm{I},n},q_{n,0})^{\top} \nu_{n,0}, \\
0 &\leq f_{\mathrm{d}}(z_{\mathrm{E},n}), \label{discimpeq:c} \\
0 &\leq \Lambda_{\mathrm{n},n} \perp f_{\mathrm{d}}(z_{\mathrm{I},n}) \geq 0. \label{discimpeq:d}
\end{align}

If $f_{\mathrm{d}}(z_{\mathrm{I},n}) = 0$ and there exist $(z_{\mathrm{I},n},\mu_{\mathrm{I},n})$ with $n(z_{\mathrm{I},n},\mu_{\mathrm{I},n},q_{n,0})^{\top} \nu_{n-1,n_{\mathrm{s}}} < 0$, then it holds $\Lambda_{\mathrm{n},n} > 0 $ as otherwise (<ref>) will be violated. In this case, (<ref>)-(<ref>) encode precisely the impact law discussed in Section <ref>. Otherwise, we have $\Lambda_{\mathrm{n},n} = 0$ and (<ref>) encodes continuity of the velocities across the FE boundary.

We collect all discrete time differential variables $q_{n,i}$, $\nu_{n,i}$, in the vector $\bm{\mathrm{x}}$, discrete time algebraic variables $z_{n,j},\mu_{n,j},\lambda_{\mathrm{n},n,j},z_{\mathrm{I},n},\mu_{\mathrm{I},n},z_{\mathrm{E},n},\mu_{\mathrm{E},n}$, in the vector $\bm{\mathrm{z}}$ and FE lengths in the vector $\bm{\mathrm{h}}$ such that the conditions (<ref>), (<ref>), (<ref>), (<ref>), are summarized by $G_{\mathrm{rk}}(\bm{\mathrm{x}},\bm{\mathrm{z}},\bm{\mathrm{h}},s_{0},\hat{u}) = 0.$

Note that complementarity conditions can be equivalently denoted by so-called C-functions $\Psi(\cdot,\cdot)$, which have the property: $0 \leq a \perp b \geq 0 \Leftrightarrow \Psi(a,b) = 0$.

System configurations for the simulation example. For detailed velocity profiles, cf. Fig. <ref>. Orange and blue dots represent the approximation for $p(t^{-})$ and $p(t^{+})$, respectively. Red markers represent the ECP used for impulse resolution.

§.§ Cross Complementarities

To maintain the high-order accuracy of RK methods, we need to ensure that active set changes of (<ref>) and (<ref>) do only occur on FE boundaries. To this end, in [15] the concept of cross complementarity conditions was introduced. To adopt these conditions to the here considered model, we first observe that $z_{n,j}$ contains the discrete time scaling factor $\alpha_{n,j}$ and the ECP $p_{n,j}$. As noted in Section <ref>, the optimal scaling factor and optimal dual variables of the distance problem (<ref>) are uniquely determined by the system configuration. Since the latter is continuous in time, it motivates to define $ \alpha_{n,0} = \alpha_{n-1,n_{\mathrm{s}}}$ and $\mu_{n,0} = \mu_{n-1,n_{\mathrm{s}}}$. On the other hand, for the ECP it can happen that we have $p(t_{\mathrm{n}}^{-}) \neq p(t_{\mathrm{n}}^{+})$ and, consequentially, $z(t_{\mathrm{n}}^{-}) \neq z(t_{\mathrm{n}}^{+})$. Given this observation, we now utilize the continuity of $\alpha(t)$ and $\mu(t)$ to fix the active sets in (<ref>). The cross complementarity conditions for (<ref>) formulate as

\begin{equation} \label{crosscomps}
\begin{split}
0 &= \lambda_{\mathrm{n},n,j} (\alpha_{n,j^{\prime}} - 1), \ j = 1,\dots, n_{\mathrm{s}}, \ j^{\prime} = 0,\dots, n_{\mathrm{s}}, \\
0 &= \mu_{n,j^{\prime}}^{\top}g_{\mathrm{d}}(z_{n,j},q_{n,j}), \ j = 1,\dots, n_{\mathrm{s}}, \ j^{\prime} = 0,\dots, n_{\mathrm{s}}.
\end{split}
\end{equation}

Including $j^{\prime} = 0$ is a crucial component of the discretization scheme, as this forces $h_{n}$ to adapt such that dynamic switches only occur at FE boundaries. The conditions (<ref>) now ensure that active sets are fixed within a FE, cf. [1, 15]. The conditions (<ref>) are denoted by $G_{\mathrm{cross}}(\bm{\mathrm{x}},\bm{\mathrm{z}},s_{0}) = 0.$

§.§ Step Equilibrium

To ensure that FE sum up to the length of the control interval, we impose

\begin{equation*} \label{heq}
G_{\mathrm{eq}}(\bm{\mathrm{h}}, T) = T - \sum_{n = 0}^{N_{\mathrm{FE}} - 1}h_{n}  = 0.
\end{equation*}

Furthermore, to remove spurious DoF if no switches occur, the cost function

\begin{equation*}
C_{\mathrm{h}}(\bm{\mathrm{h}}) = \Vert h_{n} - \frac{T}{N_{\mathrm{FE}}} \Vert_{2}^{2}
\end{equation*}

is utilized. See [15] for an elaborate discussion on step equilibrium.

§.§ Direct Optimal Control with Exact Switch Detection

We compactly denote evaluation of one control interval by

\begin{align*}
s_{1} &= F_{\mathrm{fesd}}(\bm{\mathrm{x}}), \\
0 &= G_{\mathrm{fesd}}(\bm{\mathrm{x}},\bm{\mathrm{z}},\bm{\mathrm{h}},s_{0},\hat{u}, T) = \begin{pmatrix}
G_{\mathrm{rk}}(\bm{\mathrm{x}},\bm{\mathrm{z}},\bm{\mathrm{h}},s_{0},\hat{u}) \\
T - \sum_{n = 0}^{N_{\mathrm{FE}} - 1}h_{n}
\end{pmatrix}, \\
\end{align*}

where $F_{\mathrm{fesd}}(\bm{\mathrm{x}}) = (q_{N_{\mathrm{FE}} - 1, n_{\mathrm{s}}},\nu_{N_{\mathrm{FE}} - 1, n_{\mathrm{s}}})$ evaluates the system state at time $T$ and a condition was added that ensures that all FE sum up to the length of the control interval.

Evolution of differential and algebraic states for the simulation example. Active set changes of the force complementarity (<ref>) and the SDF complementarity (<ref>) are marked by purple and orange dotted lines, respectively.

A discrete time OCP for a time-horizon $[0,T]$ split into $N$ equidistant control intervals with corresponding constant controls $\bm{\mathrm{u}} = (\hat{u}_{0},\dots, \hat{u}_{N-1})$ is given by

s,u, 𝒳, 𝒵, ℋ∑_k = 0^N -1 L̂(s_k,x_k,û_k) + C_h(h_k) + L_t(s_N),
s_0= x̅_0,
s_k+1= F_fesd(x_k),
0= G_fesd(x_k,z_k,h_k,s_k,û_k, T/N),
0 ≥G_p(s_k, û_k),

where $\bm{\mathrm{s}} = (s_{0},\dots,s_{N-1})$, $\mathcal{X} = (\bm{\mathrm{x}}_{0},\dots,\bm{\mathrm{x}}_{N-1})$, $\mathcal{Z} = (\bm{\mathrm{z}}_{0},\dots,\bm{\mathrm{z}}_{N-1})$ and $\mathcal{H} = (\bm{\mathrm{h}}_{0},\dots,\bm{\mathrm{h}}_{N-1})$. The function $\hat{L}: \mathbb{R}^{n_{x}} \times \mathbb{R}^{N_{\mathrm{FE}}(n_{\mathrm{s}} + 1)n_{x}} \times \mathbb{R}^{n_{u}} \rightarrow \mathbb{R}$ is the running cost, $L_{\mathrm{t}}: \mathbb{R}^{n_{x}} \rightarrow \mathbb{R}$ is the terminal cost, $G_{\mathrm{p}}: \mathbb{R}^{n_{x}}\times \mathbb{R}^{n_{u}} \rightarrow \mathbb{R}$ denotes path constraints and $\bar{x}_{0} \in \mathbb{R}^{n_{x}}$ is a given initial state.  The cost function

\begin{equation*}
C_{\mathrm{h}}(\bm{\mathrm{h}}_{k}) = \sum_{n = 0}^{N_{\mathrm{FE}} - 1}\Vert h_{k,n} - \frac{T}{N \cdot N_{\mathrm{FE}}} \Vert_{2}^{2}
\end{equation*}

restricts spurious DoF corresponding to FE lengths. See [15] for an elaborate discussion on step equilibrium.

System configurations for the optimal control example. For detailed velocity profiles, cf. Fig. <ref>. Orange and blue dots represent the approximation for $p(t^{-})$ and $p(t^{+})$, respectively. Red markers represent the ECP used for impulse resolution.

§ NUMERICAL EXAMPLES

In the following, we consider two numerical examples, which illustrate the different dynamic switches permitted by the CLS (<ref>). To this end, we consider the simulation of a falling cuboid in Section <ref>. In Section <ref> the optimal control of a planar manipulation task is discussed.

Due to the complementarity constraints in the FESD-J discretization, the discrete time OCP (<ref>) is a mathematical problem with complementarity constraints (MPCC). Here we solve MPCC via a relaxation strategy [16] using  [17] as solver, which is called from the  [18] interface.

§.§ Simulation of a Falling Cuboid

We consider a single cuboid with DoF making contact with another fixed cuboid. Thus, there are three DoF for the system configuration $q = (c_{\mathrm{x}},c_{\mathrm{y}},\xi) \in \mathbb{R}^{3}$. The cuboid with DoF is under the influence of a constant external force $f_{\mathrm{v}}(q,\nu,u) = (0,-1,0)^{\top}$ and has an initial horizontal velocity $\hat{\nu}_{0} = (1,0,0)$. The time horizon $[0,T]$, with $T = 3.68$, is divided into $N = 23$ simulation steps, for each $N_{\mathrm{FE}} = 2$ FE are considered. The Radau IIA method with $n_{\mathrm{s}} = 4$ stage points is used, which has seventh-order accuracy [14].

Fig. <ref> shows the configuration of the falling cuboid at time instances where dynamic switches occur. Fig. <ref> visualizes the evolution of the state trajectories as well as force and impulse magnitudes and the length of the FE. We observe that the FE lengths are practically equidistant for the simulation steps where no switches happen, and, on the other hand, adjust to accurately detect switches if one occurs.

At the state in Fig. <ref>, the bodies make contact for the first time and an impact occurs. Consequently, system velocities undergo a jump. At the time instance in Fig. <ref> another impact occurs, as the cuboids form a patch contact. At the same time the active set of the SDF changes and the ECP parameterizing the contact force normal jumps, which illustrates that it is generally discontinuous. We observe in Fig. <ref> that the ECP for the impulse simulation is chosen within the contact surface such that patch contact is maintained. Due to the initial velocity and no involved friction the cuboid then slides horizontally until the active set of the SDF changes at the configuration in Fig. <ref>. Another active set change in the SDF occurs in Fig. <ref>, which simultaneously results in a jump of the .

Evolution of differential and algebraic states for the optimal control example. Active set changes of the force complementarity (<ref>) and the SDF complementarity (<ref>) are marked by purple and orange dotted lines, respectively.

§.§ Optimal Control for a Manipulation Task

We now consider a manipulation task, where a triangular shaped object can be actuated and through contact force can influence another object, which is a nonregular pentagon. Both objects should end up in a goal position, which is transcribed in the time discrete OCP (<ref>) through the terminal cost function. The nominal dynamics of the CLS (<ref>) are given by $f_{\mathrm{v}}(q,v,u) = (u,\bm{0}) + (-0.1\nu_{1},-\nu_{2})$, where $u \in \mathbb{R}^{3}$, and $\nu_{1} = (v_{1,\mathrm{x}},v_{1,\mathrm{y}},\omega_{1})\in \mathbb{R}^{3}$, $\nu_{2} = (v_{2,\mathrm{x}},v_{2,\mathrm{y}},\omega_{2})\in \mathbb{R}^{3}$, $\nu = (\nu_{1},\nu_{2})$, are the translational and angular velocities of the two bodies. Including forces proportional to the negative velocity models simplified friction effects. We consider the time horizon $[0,T]$, with $T = 20$, which is divided into $N=20$ equidistant control intervals. For each control interval $N_{\mathrm{FE}} = 2$ FE are used together with the seventh-order Radau IIA method, i.e., $n_{\mathrm{s}} = 4$.

Fig. <ref> displays five system configurations for a solution trajectory of the discrete time OCP (<ref>), and Fig. <ref> visualizes the corresponding control, velocity, force and impulse magnitudes. We again note that the FE lengths adapt to detect switches if any occur. Furthermore, we can observe that since control forces change discontinuously across control intervals, contact forces are also discontinuous. However, we observe that within an equidistant control interval, contact forces remain continuous if no switch occurs. This is the expected behavior, as if external control forces are smooth and no dynamic switch occurs, CLS (<ref>) evolves smoothly.

§ CONCLUSION AND OUTLOOK

This paper presented a complementarity Lagrangian system with an embedded nondifferentiable signed distance function that allows one to model dynamic interactions of rigid bodies with patch contacts. We discussed that if a suitable impact law is chosen, the evolution of state trajectories for this system is unique. We then showed that the FESD-J method can be extended to the considered system such that the integration order of an underlying Runge-Kutta method is maintained. In further work, the methodology will be extended to incorporate frictional effects and will be discussed in a detailed manner in a journal publication.

§ APPENDIX

§.§ Proof of Proposition <ref>

We investigate the KKT conditions of the distance problem (<ref>) to prove the proposition. To this end, let $(z, \mu)$ be optimal for (<ref>). We define

\begin{align*}
&\mathcal{A}_{1,\mathrm{p}} = \{i \ \vert \ \mu_{1,\mathrm{p},i} > 0\}, \\
&\mathcal{A}_{2,\mathrm{p}} = \{i \ \vert \ \mu_{2,\mathrm{p},i} > 0\},
\end{align*}

as the active sets for the constraints (<ref>) and (<ref>), respectively. We first observe that in two dimensions it holds

\begin{equation*}
1 \leq \vert \mathcal{A}_{1,\mathrm{p}} \vert \leq 2, \qquad 1 \leq \vert \mathcal{A}_{2,\mathrm{p}} \vert \leq 2.
\end{equation*}

The constraints (<ref>) and (<ref>) are necessarily active as well. If any of the aforementioned active constraints would be inactive, this would imply that $\alpha$ can be decreased such that there are primal variables $p$, $y_{1}$, $y_{2}$, fulfilling the constraints (<ref>)-(<ref>), which would contradict optimality. In the following, we distinguish three cases.

Case 1: Let $\vert \mathcal{A}_{1,\mathrm{p}} \vert = 2$ and $\vert \mathcal{A}_{2,\mathrm{p}} \vert = 2$. Then we have $\mathcal{A}_{1,\mathrm{p}} = \{k_{1,1},k_{1,2}\}$, $\mathcal{A}_{2,\mathrm{p}} = \{k_{2,1},k_{2,2}\}$, $k_{1,1},k_{1,2}\in \{1,\dots,\nha\}$, $k_{2,1},k_{2,2}\in \{1,\dots,\nhb\}$. We select active components of the halfspace representation and dual variables as

A̅_1 = A_1,𝒜_1,p,       A̅_2 = A_2,𝒜_2,p,

b̅_1 = b_1,𝒜_1,p,        b̅_2 = b_2,𝒜_2,p,

μ̅_1,p = μ_1, p,𝒜_1,p,          μ̅_2,p = μ_2, p,𝒜_2,p,

where $\bar{A}_{1} \in \mathbb{R}^{2\times2}$ and $\bar{A}_{2} \in \mathbb{R}^{2\times2}$ contain the normal vectors corresponding to active halfspaces as rows.

Then the equality conditions imposed by the active inequality constraints of (<ref>) together with the equality conditions imposed by the stationary KKT condition (<ref>) are given by

\begin{align}
\bar{A}_{1}R( \xi_{1})^{\top}(y_{1}-c_{1})&= \alpha \bar{b}_{1} - \bm{1}r, \label{KKTsystem:a}\\
\bar{A}_{2}R( \xi_{2})^{\top}(y_{2}-c_{2})&= \alpha \bar{b}_{2} - \bm{1}r, \label{KKTsystem:b}\\
\Vert p - y_{1} \Vert_{2}^{2} &= r^2, \label{KKTsystem:c}\\
\Vert p - y_{2} \Vert_{2}^{2} &= r^2, \label{KKTsystem:d}\\
1 -\bar{b}_{1}^{\top} \bar{\mu}_{1,\mathrm{p}} -\bar{b}_{2}^{\top} \bar{\mu}_{2,\mathrm{p}} &= 0, \label{KKTsystem:e}\\
2(p-y_{1})\mu_{1, \mathrm{c}} &= - 2(p-y_{2})\mu_{2, \mathrm{c}},  \label{KKTsystem:f}\\
R( \xi_{1})\bar{A}_{1}^{\top}\bar{\mu}_{1,\mathrm{p}} &=  2(p-y_{1}) \mu_{1, \mathrm{c}}, \label{KKTsystem:g}\\
R( \xi_{2})\bar{A}_{2}^{\top}\bar{\mu}_{2,\mathrm{p}} &=  2(p-y_{2}) \mu_{2, \mathrm{c}}.\label{KKTsystem:h}
\end{align}

Resolving (<ref>) and (<ref>) for $y_1$ and $y_2$, respectively, results in

\begin{align}
y_{1} &= c_{1} + R(\xi_{1})\bar{A}_{1}^{-1}(\alpha \bar{b}_{1} - \bm{1}r), \\
y_{2} &= c_{2} + R(\xi_{2})\bar{A}_{2}^{-1}(\alpha \bar{b}_{2} - \bm{1}r).
\end{align}

Due to the assumptions in Section <ref> that the considered polytopes are not degenerate, the matrices $\bar{A}_{1}$ and $\bar{A}_{2}$ are invertible. Condition (<ref>) implies that $p$ is element of the straight line connecting $y_{1}$ and $y_{2}$. Conditions (<ref>) and (<ref>) imply that $p$ has a distance of $r$ to $y_{1}$ and $y_{2}$. Thus, we obtain

\begin{equation}  \label{ydisteq}
\Vert y_{1}-y_{2} \Vert_{2} = 2r,
\end{equation}

\begin{equation} \label{pexpl}
p = y_{1} + \frac{y_{2} - y_{1}}{2}.
\end{equation}

Substituting (<ref>) in (<ref>) results in the condition

\begin{equation} \label{quadalphaeq}
\begin{split}
&\Vert c_{1}-c_{2} + (R(\xi_{2})\bar{A}_{2}^{-1}-R(\xi_{1})\bar{A}_{1}^{-1})\bm{1}r \\
&\quad \quad + (R(\xi_{1})\bar{A}_{1}^{-1}\bar{b}_{1}-R(\xi_{2})\bar{A}_{2}^{-1}\bar{b}_{2}) \alpha \Vert_{2} = 2r.
\end{split}
\end{equation}

This is a quadratic equation for the scalar $\alpha$ that, due to the geometric interpretation of the problem, uniquely fixes $\alpha$. In particular, the solution of (<ref>) with larger value violates the constraints (<ref>)-(<ref>). Consequently, we derived unique expressions for the optimal primal variables. Finally, for the dual variables, we observe that (<ref>), (<ref>) and (<ref>) imply

\begin{equation} \label{eqofmuc}
\mu_{1, \mathrm{c}} = \mu_{2, \mathrm{c}}.
\end{equation}

Solving (<ref>) and (<ref>) for $\bar{\mu}_{1,\mathrm{p}}$ and $\bar{\mu}_{2,\mathrm{p}}$, respectively, yields

\begin{align}
\bar{\mu}_{1,\mathrm{p}} &= 2(\bar{A}_{1}^{\top})^{-1}R( \xi_{1})^{\top}(p-y_{1}) \mu_{1, \mathrm{c}}, \\
\bar{\mu}_{2,\mathrm{p}} &= 2(\bar{A}_{2}^{\top})^{-1}R( \xi_{2})^{\top}(p-y_{2}) \mu_{2, \mathrm{c}}.
\end{align}

Substituting the above in (<ref>) and using (<ref>) yields

\begin{equation*}
1 -2(\bar{b}_{1}^{\top} (\bar{A}_{1}^{\top})^{-1}R( \xi_{1})^{\top}-\bar{b}_{2}^{\top}(\bar{A}_{2}^{\top})^{-1}R( \xi_{2})^{\top})(p-y_{1}) \mu_{1, \mathrm{c}},
\end{equation*}

which lets us solve for $\mu_{1, \mathrm{c}}$ since primal variables already have been uniquely determined. Consequently, due to (<ref>) we can solve for $\mu_{2, \mathrm{c}}$, and due to (<ref>) for $\mu_{1, \mathrm{p}}$ and $\mu_{2, \mathrm{p}}$.

Case 2:  Here we consider $\vert \mathcal{A}_{1,\mathrm{p}} \vert = 2$ and $\vert \mathcal{A}_{2,\mathrm{p}} \vert = 1$. We redefine

\begin{equation*}
\bar{A}_{2} = A_{2,k_{2,1}}, \quad
\bar{b}_{2} = b_{2,k_{2,1}}, \quad
\bar{\mu}_{2,\mathrm{p}} = \mu_{2, \mathrm{p},k_{2,1}},
\end{equation*}

i.e., here $\bar{A}_{2} \in  \mathbb{R}^{1 \times 2}$ is a row vector. Elaborate discussion can then be carried out analogously to Case 1. Here, (<ref>) does not directly determine $y_{2}$ for a given $\alpha$ as it is only one scalar equation. However, $y_{1}$ is again uniquely determined by (<ref>) and this time, (<ref>) fixes the slope of the straight line passing through $y_{1}$, $y_{2}$, $p$. Given unique primal variables, derivation of the dual variables follows similarly as in .
The vice versa case with $\vert \mathcal{A}_{1,\mathrm{p}} \vert = 1$ and $\vert \mathcal{A}_{2,\mathrm{p}} \vert = 2$ follows analogously.

Case 3: This is the case of possible patch contact with $\vert \mathcal{A}_{1,\mathrm{p}} \vert = 1$ and $\vert \mathcal{A}_{2,\mathrm{p}} \vert = 1$. Here we have

A̅_1 = A_1,k_1,1,       A̅_2 = A_2,k_2,1,

b̅_1 = b_1,k_1,1,        b̅_2 = b_2,k_2,1,

μ̅_1,p = μ_1, p,k_1,1,          μ̅_2,p = μ_2, p,k_2,1,

where now $\bar{A}_{1} \in \mathbb{R}^{1\times2}$ and $\bar{A}_{2}\in \mathbb{R}^{1\times2}$ are row vectors. In this case, (<ref>) and (<ref>) imply that the vectors $p-y_{i}$ are positive multiples of the vectors $R( \xi_{i})\bar{A}_{i}^{\top}$, for $i =1,2,$ respectively. Furthermore, (<ref>) and (<ref>) imply that  have an Euclidean norm equal to $r$. Therefore, we can equivalently formulate the system (<ref>)-(<ref>) regrading the dual variables as

\begin{align}
1 -\bar{b}_{1}^{\top} \bar{\mu}_{1,\mathrm{p}} -\bar{b}_{2}^{\top} \bar{\mu}_{2,\mathrm{p}} &= 0, \label{refdualsystem:e}\\
2rR( \xi_{1})\bar{A}_{1}^{\top}\mu_{1, \mathrm{c}} &= - 2rR( \xi_{2})\bar{A}_{2}^{\top}\mu_{2, \mathrm{c}},  \label{refdualsystem:f}\\
R( \xi_{1})\bar{A}_{1}^{\top}\bar{\mu}_{1,\mathrm{p}} &=  2rR( \xi_{1})\bar{A}_{1}^{\top}  \mu_{1, \mathrm{c}}, \label{refdualsystem:g}\\
R( \xi_{2})\bar{A}_{2}^{\top}\bar{\mu}_{2,\mathrm{p}} &=  2rR( \xi_{2})\bar{A}_{2}^{\top}  \mu_{2, \mathrm{c}},\label{refdualsystem:h}
\end{align}

where we used that $A_{1}$ and $A_{2}$ are assumed to be constructed such that their rows are vectors with unitary norm. System (<ref>) can be restated as

\begin{align}
1 -\bar{b}_{1}^{\top} \bar{\mu}_{1,\mathrm{p}} -\bar{b}_{2}^{\top} \bar{\mu}_{2,\mathrm{p}} &= 0, \\
\mu_{1, \mathrm{c}} &=  \mu_{2, \mathrm{c}},  \\
\bar{\mu}_{1,\mathrm{p}} &=  2r \mu_{1, \mathrm{c}}, \\
\bar{\mu}_{2,\mathrm{p}} &=  2r \mu_{2, \mathrm{c}},
\end{align}

which is a linear system of four scalar equations for four scalar variables with a unique solution.

It remains to show that the dual variables $\mu$ always depend continuously on the system configuration $q$. This follows directly from the result in [19] which states that if for a convex parametric program, Slater's constraint qualification applies, the set of primal solutions is bounded for a certain parameter and dual variables are unique in a neighborhood of this parameter, then the Lagrange multipliers behave continuously. Since we already inferred that Lagrange multipliers are unique for any $q$, Slater's constraint qualification applies and primal optimal solutions are naturally bounded, even for the case of patch contacts, we conclude that the Lagrange multipliers depend continuously on $q$.

To show that the dual variables remain continuous across active set changes, probably all different switching scenarios have to be distinguished. The idea would be to show that the vectors $p-y_{1}$ and $p-y_{2}$ are continuous w.r.t. $q$. Then (<ref>)-(<ref>) is a system of equations for the dual variables solely parameterized by $q$. Furthermore, even so the matrices and vectors $\bar{A}_{1}, \ \bar{A}_{2}, \ \bar{b}_{1}, \ \bar{b}_{2}$ change instantaneously at an active set change, solving (<ref>)-(<ref>) at this active set change for both sets of matrices and vectors necessarily results in the same $\mu$.

We illustrate this in an example. Fig. <ref> shows the evolution of dual variables for a system of two cuboids as depicted in Fig. <ref>. The only DoF in the system is given by $\theta \in \mathbb{R}$ which denotes the orientation of the blue cuboid. At the configuration in Fig. <ref> a switch in the distance problem occurs. We observe that in this case, the active set $\mathcal{A}_{2,\mathrm{p}}$ corresponding to the orange cuboid stays constant. Thereby, (<ref>) together with (<ref>) already imply that the vectors $p-y_{1}$ and $p-y_{2}$ do not change. Furthermore, nor $\mu_{1,\mathrm{p},2}$ or $\mu_{1,\mathrm{p},4}$ which correspond to the bottom and top halfspace corresponding to the blue cuboid in Fig. <ref> are active at the switch.

We consider another example in Fig.<ref>, which again shows the evolution of dual variables for a similar system of two cuboids. Here five different switching configurations are depicted in Fig. <ref>. From Fig. <ref> we can again deduce that the dual variables exhibit kinks when switches occur but remain continuous.

Generally, one probably could think of all possible switching scenarios and carefully iterate analytically through them to show continuity of the dual variables. However, this seems quite unwieldy compared to geometric considerations. Is there a nicer way to do an analytic proof?

Possible switches

$\bullet$ $\mathcal{A}_{1,\mathrm{p}} = 2, \mathcal{A}_{2,\mathrm{p}} = 1 \quad \leftrightarrow \quad \mathcal{A}_{1,\mathrm{p}} = 2, \mathcal{A}_{2,\mathrm{p}} = 1$

$\bullet$ $\mathcal{A}_{1,\mathrm{p}} = 1, \mathcal{A}_{2,\mathrm{p}} = 1 \quad \leftrightarrow \quad \mathcal{A}_{1,\mathrm{p}} = 2, \mathcal{A}_{2,\mathrm{p}} = 2$

$\bullet$ $\mathcal{A}_{1,\mathrm{p}} = 2, \mathcal{A}_{2,\mathrm{p}} = 1 \quad \leftrightarrow \quad \mathcal{A}_{1,\mathrm{p}} = 2, \mathcal{A}_{2,\mathrm{p}} = 2$

$\bullet$ $\mathcal{A}_{1,\mathrm{p}} = 2, \mathcal{A}_{2,\mathrm{p}} = 1 \quad \leftrightarrow \quad \mathcal{A}_{1,\mathrm{p}} = 1, \mathcal{A}_{2,\mathrm{p}} = 1$

Evolution of dual variables for the system depicted in Fig. <ref>.

Different configurations of a system with two cuboids, whereby only the blue cuboid has one rotational DoF. A switch in the SDF happens at the configuration (b).

Evolution of dual variables for the system depicted in Figure <ref>.

Different configurations of a system with two cuboids, whereby only the blue cuboid has one rotational DoF. Switches in the active set of the SDF occur at all depicted configurations.

§.§ Proof of Proposition <ref>

As explained in the main part of the paper, the goal is to derive smooth expressions for the algebraic variables of the CLS (<ref>) in terms of the differential variables and the control input. Consequently, an ODE with a smooth right-hand side is obtained that admits to unique solutions due to the Picard-Lindelöf theorem. To this end, we again consider the active sets for the constraints (<ref>) and (<ref>) given by

\begin{align*}
&\mathcal{A}_{1,\mathrm{p}} = \{i \ \vert \ \mu_{1,\mathrm{p},i} > 0\}, \\
&\mathcal{A}_{2,\mathrm{p}} = \{i \ \vert \ \mu_{2,\mathrm{p},i} > 0\},
\end{align*}

We distinguish between two cases.

Case 1: Let $q(t)$ solve (<ref>) on $(t_{1},t_{2})$ such that $\vert \mathcal{A}_{1,\mathrm{p}} \vert = 2$ or $\vert \mathcal{A}_{2,\mathrm{p}} \vert = 2$. Then there exist unique ($z, \mu$) solving (<ref>), i.e., the SDF in classically differentiable. In this case, one can discuss the contact DAE similarly as done in [13], where a CLS with state jumps is discussed for the case of differentiable SDF.

Case 2: Let $q(t)$ solve (<ref>) on $(t_{1},t_{2})$ such that $\vert \mathcal{A}_{1,\mathrm{p}} \vert = 1$ and $\vert \mathcal{A}_{2,\mathrm{p}} \vert = 1$. Let

𝒜_1,p = {k_1},       𝒜_2,p = {k_2},

k_1 ∈{1,…,},         k_2 ∈{1,…,},

a_1 = A^⊤_1,k_1,       a_2 = A^⊤_2,k_2.

Then the KKT conditions (<ref>) imply

\begin{equation*}
R(\theta_{1})a_{1} \mu_{1,\mathrm{p},k_{1}} + R(\theta_{2})a_{2} \mu_{2,\mathrm{p},k_{2}} = 0.
\end{equation*}

Since the the rows of $A_{1}, A_{2}$, are assumed to be unit vectors, the above equation reduces to

\begin{equation} \label{k1}
R(\theta_{1})a_{1} + R(\theta_{2})a_{2}  = 0.
\end{equation}

Differentiating (<ref>) with respect to time yields

\begin{equation}\label{k2}
\begin{split}
0 &= \ddt	(R(\theta_{1})a_{1} + R(\theta_{2})a_{2}) \\
&= R\left(\theta_{1} + \frac{\pi}{2}\right)a_{1} \omega_{1} + R\left(\theta_{2}+ \frac{\pi}{2}\right)a_{2}\omega_{2}.
\end{split}
\end{equation}

By substituting (<ref>) in (<ref>), we obtain

\begin{equation*}
\begin{split}
&0 = R\left(\frac{\pi}{2}\right) (R(\theta_{1})a_{1} \omega_{1} - R(\theta_{1})a_{1} \omega_{2}) \\
\Leftrightarrow \quad&0= \omega_{1}-\omega_{2}
\end{split}
\end{equation*}

and, consequently,

\begin{equation*}
\begin{split}
0 &= \dot{\omega}_{1}-\dot{\omega}_{2}.
\end{split}
\end{equation*}

From the right-hand side of (<ref>), we can then infer that it has to hold

⇔  p ×    0       =M^-1_ω_1(f_v,ω_1(q,v,u) + (p-c_1) ×)

=- M^-1_ω_2(f_v,ω_2(q,v,u) - (p-c_2) ×)

⇔  p ×     0       = (M^-1_ω_1 + M^-1_ω_2)p ×

=- C_n +  R_1(q,v,u)

⇔  p ×      = - R_1(q,v,u)/M^-1_ω_1 + M^-1_ω_2 +C_n /M^-1_ω_1 + M^-1_ω_2,

\begin{equation*}
C_{\mathrm{n}} = M^{-1}_{\omega_{1}} c_{1} \times \ntr + M^{-1}_{\omega_{2}} c_{2} \times \ntr.
\end{equation*}

The system velocities are captured in the vector  and , denote components of the inertia matrix and the nominal dynamics influencing the respective elements of the system velocity. We used that $M$ is assumed to be diagonal. Furthermore, $R_{1}(q,v,u)$ denotes a smooth remainder term given by

\begin{equation*}
R_{1}(q,v,u) = M^{-1}_{\omega_{1}}f_{\mathrm{v},\omega_{1}}(q,v,u) -M^{-1}_{\omega_{2}}f_{\mathrm{v},\omega_{2}}(q,v,u).
\end{equation*}

Under the assumption of lasting patch contact, we have , cf. Case 3 in the proof of Proposition <ref>. It then follows

\begin{equation}
\dntr = R\left( \frac{\pi}{2}\right) R(\theta_{1})a_{1}\omega_{1}.
\end{equation}

We deduce

\begin{align}
\dot{n}^{\top}v &= \begin{pmatrix}
\dntr \\
(\dot{p}-v_{1}) \times \ntr + (p-c_{1}) \times \dntr \\
-\dntr \\
-(\dot{p}-v_{2}) \times \ntr - (p-c_{2}) \times \dntr
\end{pmatrix}^{\top}v \\
&= R_{2}(q,v) + (\dot{p}\times \ntr + p\times \dntr)(\omega_{1}-\omega_{2}) \\
&= R_{2}(q,v),
\end{align}

\begin{equation*}
R_{2}(q,v) = \begin{pmatrix}
\dntr \\
-v_{1} \times \ntr + c_{1} \times \dntr \\
-\dntr \\
v_{2} \times \ntr - c_{2} \times \dntr
\end{pmatrix}^{\top}v.
\end{equation*}

Now let

\begin{equation*}
n = n_{\mathrm{c}} + n_{\mathrm{p}} = \begin{pmatrix}
\ntr \\
-c_{1} \times \ntr \\
-\ntr \\
c_{2} \times \ntr
\end{pmatrix} + \begin{pmatrix}
0 \\
p \times \ntr \\
0 \\
-p \times \ntr
\end{pmatrix}.
\end{equation*}

Differentiating the contact normal with respect to time yields

\begin{align}
0 &=\ddt	n^{\top}v \nonumber \\
&= \dot{n}^{\top}v + n^{\top}\dot{v} \nonumber \\
&=R_{2}(q,v) + n^{\top}(M^{-1}(f_{\mathrm{v}} + n\lamn)) \nonumber \\
& = R_{2}(q,v) + (n_{\mathrm{c}} + n_{\mathrm{p}})^{\top}(M^{-1}(f_{\mathrm{v}} + (n_{\mathrm{c}} + n_{\mathrm{p}})\lamn))\nonumber  \\
& = R_{2}(q,v) + n_{\mathrm{c}}^{\top}(M^{-1}(f_{\mathrm{v}} + (n_{\mathrm{c}} + n_{\mathrm{p}})\lamn))\nonumber \\
& =R_{3}(q,v,u) + n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{c}}\lamn + n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{p}}\lamn\nonumber \\
& =R_{3}(q,v,u) + n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{c}}\lamn
-C_{\mathrm{n}} p \times \ntr \lamn, \label{dnvcond}
\end{align}

\begin{equation*}
R_{3}(q,v,u) = R_{2}(q,v) + n_{\mathrm{c}}^{\top}M^{-1}f_{\mathrm{v}}.
\end{equation*}

By substituting (<ref>) into (<ref>), one obtains

\begin{align}
0 &= R_{3}(q,v,u) + n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{c}}\lamn \nonumber \nonumber\\
&\quad -C_{\mathrm{n}} \left(\frac{C_{\mathrm{n}} \lamn - R_{1}(q,v,u)}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}} \right) \nonumber \\
\Leftrightarrow \, 0 &= R_{4}(q,v,u) + \left(n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{c}} - \frac{C_{\mathrm{n}}^{2}}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}} \right) \lamn,
\end{align}

\begin{equation*}
R_{4}(q,v,u) = R_{3}(q,v,u) + \frac{C_{\mathrm{n}}}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}} R_{1}(q,v,u).
\end{equation*}

If it holds

\begin{equation} \label{ineqproof}
\left(n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{c}} - \frac{C_{\mathrm{n}}^{2}}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}} \right) > 0,
\end{equation}

an analytic expression for $\lamn$ is given by

\begin{equation} \label{lamnexplicit}
\lamn  = -\left(n_{\mathrm{c}}^{\top}M^{-1}n_{\mathrm{c}} - \frac{C_{\mathrm{n}}^{2}}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}} \right)^{-1}  R_{4}(q,v,u),
\end{equation}

which is arbitrarily smooth in the states $q,v$ and the input $u$.

We now additionally use, that $p$ has to be an element of the hyperplane induced by the active halfspace constraints. I.e., it holds

\begin{equation*}
\ntr^{\top}p = b_{1,k_{1}}.
\end{equation*}

Together with (<ref>) we arrive at the explicit representation

\begin{equation} \label{explprep}
p = \begin{pmatrix}
\ntr^{\top} \\
[\ntr ]_{\times}^{\top}
\end{pmatrix}^{-1} \begin{pmatrix}
b_{1,k_{1}} \\
\frac{C_{\mathrm{n}}}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}}  - \frac{ R_{1}(q,v,u)}{(M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}})\lamn}
\end{pmatrix},
\end{equation}

where we define $[\ntr ]_{\times} = (n_{\mathrm{tr},2},-n_{\mathrm{tr},1})^{\top}$ as then it follows $p \times \ntr = [\ntr ]_{\times}^{\top}p$. The right-hand side of (<ref>) is well-defined since we have assumed $\lamn > 0$ and since $\lamn$ admits a smooth expression in the states $q,v$ and the input $u$, so does the right-hand side of the above system for $p$.

Therefore, we can substitute the derived explicit representations for primal and dual optimal variables of the distance problem as well as the force magnitude into the differential equations (<ref>)-(<ref>). Uniqueness then follows as in Case 1 as we derived a pure ODE by index reduction with arbitrarily smooth right-hand side for which such a solution exists by the Picard-Lindelöf theorem.

It remains to show (<ref>). To this end, let

s_1 =  c_1 ×,         s_2 =  c_2 ×,

m_1 =  M^-1_ω_1,       m_2 =  M^-1_ω_2.

By expanding all terms, we deduce

\begin{align*}
n_{\mathrm{c}}^{\top}&M^{-1}n_{\mathrm{c}} - \frac{C_{\mathrm{n}}^{2}}{M^{-1}_{\omega_{1}} + M^{-1}_{\omega_{2}}} \\
&= \ntr^{\top}M^{-1}_{v_{1}}\ntr + \ntr^{\top}M^{-1}_{v_{2}}\ntr + m_{1} s_{1}^{2} + m_{2} s_{2}^{2} \\
&\phantom{{}={}}- \frac{m_{1}^{2}  s_{1}^{2} +m_{2}^{2}  s_{2}^{2} + 2m_{1}m_{2} s_{1}s_{2}  }{m_{1}+ m_{2}} \\
& > m_{1} s_{1}^{2} + m_{2} s_{2}^{2} - \frac{m_{1}^{2}  s_{1}^{2} +m_{2}^{2}  s_{2}^{2} + 2m_{1}m_{2} s_{1}s_{2}  }{m_{1}+ m_{2}} \\
&= \left(1 - \frac{m_{1} + m_{2} - m_{2}}{m_{1} + m_{2}}\right)m_{1}s_{1}^{2} \\
&\phantom{{}={}}+ \left(1 - \frac{m_{2} + m_{1} - m_{1}}{m_{1} + m_{2}}\right)m_{2}s_{2}^{2} - \frac{2m_{1}m_{2} s_{1}s_{2}  }{m_{1}+ m_{2}} \\
&=  \frac{m_{1}m_{2}}{m_{1} + m_{2}}s_{1}^{2} + \frac{m_{1}m_{2}}{m_{1} + m_{2}}s_{2}^{2} - \frac{m_{1}m_{2}}{m_{1} + m_{2}} 2s_{1}s_{2} \\
&= \frac{m_{1}m_{2}}{m_{1} + m_{2}}( s_{1} - s_{2} )^{2} \\
&\geq 0.
\end{align*}

Let $f:\mathbb{R}^{n} \rightarrow \mathbb{R}$ be twice differentiable with positive definite and symmetric Hessian and let $K \subset \mathbb{R}^{n}$ be convex. Then the solving the variational inequality problem:

\begin{equation} \label{vi1}
\text{Find } \bar{x} \in K: \ (x-\bar{x})^{\top} \nabla f(\bar{x}) \geq 0, \ \forall x \in K,
\end{equation}

is equivalent to solving the optimization problem

x ∈K.

Furthemore, if $K$ is a convex cone with $0 \in K$, one can use $x = 0$ and $x = 2\bar{x}$ in (<ref>) to obtain
the equivalence

\begin{equation*}
(\ref{vi1}) \quad \Leftrightarrow \quad  (x-\bar{x})^{\top} \nabla f(\bar{x}) \geq 0, \ \forall x \in K, \quad \bar{x}^{\top} \nabla f(\bar{x}) = 0.
\end{equation*}

(Based on <https://supernet.isenberg.umass.edu/austria_lectures/fvisli.pdf>).

The considered impact law provides unique solutions.

Let $\mathcal{P} \subset \mathbb{R}^{n_{\mathrm{w}}}$ be the contact patch at which an impact occurs at time $t_{\mathrm{s}}$, i.e., $p$ solves $\Phi(q(t_{\mathrm{s}}))$ (<ref>) $\Leftrightarrow p \in \mathcal{P}$. Here $\mathcal{P}$ is a line as we consider the two-dimensional case $n_{\mathrm{w}} = 2$. But the following also transfers directly to three dimenesions with $n_{\mathrm{w}} = 3$. Since we only consider convex objects, $\mathcal{P}$ will also always be convex. We now consider the set

\begin{equation*}
\mathcal{N} = \{\Lambda_{\mathrm{n}} n(p,\mu, q(t_{\mathrm{s}})) \ \vert \ p \in \mathcal{P}, \Lambda_{\mathrm{n}} \geq 0 \},
\end{equation*}

which is a convex cone.

Define $f(n) = \nu(t_{\mathrm{s}}^{-})^{\top}n + \frac{1}{2}n^{\top}M^{-1}n$. Due to Lemma <ref> we have

nν(t_s^-)^⊤n + 1/2n^⊤M^-1nn̅=
n ∈𝒩,

is equivalent to the considered impact law given by

\begin{equation} \label{impactlaw}
\begin{split}
&n^{\top}(\nu(t_{\mathrm{s}}^{-}) + M^{-1}\bar{n}) \geq 0, \quad \forall n \in \mathcal{N}. \\
&\bar{n}^{\top}(\nu(t_{\mathrm{s}}^{-}) + M^{-1}\bar{n}) = 0.
\end{split}
\end{equation}

Since $M$ is positive definite, (<ref>) is an optimization problem with a strictly convex objective function and a convex feasible set. Thus, solutions are unique.

Is solving (<ref>) also equivalent to solving the Gauss' principle?

n ∈𝒩,
ñ^⊤(ν(t_s^-) + M^-1n) ≥0,  ∀ñ ∈𝒩.

Denote by $V(q(t_{\mathrm{s}})) = \{v \in \mathbb{R}^{n_{q}} \ \vert \ v^{\top}n \geq 0, \ \forall n \in \mathcal{N} \}.$ [Nguyen] then states the following representation of the post-impact velocity according to Moreau's law:

\begin{equation*}
v^{+} = \mathrm{proj}_{M}(V(q(t_{\mathrm{s}})), v^{-}),
\end{equation*}

from which is follows

v ∈V(q(t_s) 1/2 ‖v - v^- ‖_M^2v^+ =

By considering that $v^{+} = v^{-} + M^{-1}n$ for some $n \in \mathcal{N}$, solving the above is equivalent to solving

n ∈𝒩 1/2 ‖M^-1n ‖_M^2
ñ^⊤(ν(t_s^-) + M^-1n) ≥0,  ∀ñ ∈𝒩,

which is precisely the Gauss' principle as stated in [Redon].

Equivalence of the Gauss'principle / Moreau's law and (<ref>) / (<ref>) is shown in <cit.>, (<ref>) are the KKT conditions of the Gauss'principle / Moreau's law.

Let $K \subset \mathbb{R}^{n}$ be a convex polyhedral cone, i.e., $K = \{\beta_{i}k_{i} \ \vert \ \beta_{i} \geq 0, \ i = 1\dots,n_{\mathrm{k}}\}$, where $k_{i} \in \mathbb{R}^{n}$. Let $M \in \mathbb{R}^{n\times n}$ be positive definite, let $v \in \mathbb{R}^{n}$. Then

x ∈K1/2 ‖x ‖_ M^-1^2x̅=
x̃^⊤(v + M^-1x) ≥0,   ∀x̃ ∈K.

is equivalent to

x ∈K1/2 ‖v + M^-1x ‖_M^2.x̅=

This in turn is due to Lemma <ref> equivalent to

\begin{align*}
(x- \bar{x})^{\top}(v + M^{-1}\bar{x}) \geq 0, \forall x \in K, \quad \bar{x}^{\top}(v + M^{-1}\bar{x}) = 0.
\end{align*}

"$\Rightarrow$": It is sufficient to show that from (<ref>) it follows $\bar{x}^{\top}(v + M^{-1}\bar{x}) = 0.$

Given that the impact law can be interpreted as an optimization problem with a strictly convex objective function, sharp corner impacts of standard polytopes could also be handled equivalently and unique impulses would be produced. But what is common sense in the literature for this case, i.e., what result would one like to obtain? Also how can (<ref>)-(<ref>) be integrated in the discretization scheme? Is it also equivalent to an Euler step simulation step? Finally, if the impact law is reasonable defined for sharp corner impacts, is the contact DAE also well-defined for forces acting on sharp corners? This seems very likely as varying the normal on corners during force simulation would rotate both objects in the opposite directions which would cause separation of corners. Maybe padding polytopes is not necessary but at the moment it is unclear to me if the system is still well-defined. What makes padding polytopes generally attractive is that the translational normal $n_\mathrm{tr}(\mu,q)$ is a function purely of $q$, since $\mu$ are always unique one can write $\mu(q)$. Therefore, one could evaluate $n_\mathrm{tr}(\mu(q),q)$ and $\Phi(q)$ externally by standard fast distance algorithms, e.g., Gilbert–Johnson–Keerthi etc. and then provide function evaluations and derivatives to the numerical solver. Thereby, one can get rid of all the primal and dual variables of the distance problem which make up a significant part of the optimization variables. Only the ECP $p$ has to be kept in the optimization problem to evaluate the normal $n$.

[1]
A. Nurkanović, J. Frey, A. Pozharskiy, and M. Diehl, “FESD-J: Finite
elements with switch detection for numerical optimal control of rigid bodies
with impacts and Coulomb friction,” Nonlinear Analysis: Hybrid
Systems, vol. 52, p. 101460, 2024.

[2]
B. Brogliato, Nonsmooth Mechanics: Models, Dynamics and Control.
Springer, 2016.

[3]
V. Acary and B. Brogliato, Numerical Methods for Nonsmooth Dynamical
Springer, 2008.

[4]
M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory
optimization of rigid bodies through contact,” The International
Journal of Robotics Research, vol. 33, pp. 69–81, 2014.

[5]
J. Xie and N. Chakraborty, “Rigid body dynamic simulation with line and
surface contact,” in 2016 IEEE Int. Conf. on Simulation, Modeling, and
Programming for Autonomous Robots (SIMPAR), pp. 9–15, 2016.

[6]
D. Stewart and M. Anitescu, “Optimal control of systems with discontinuous
differential equations,” Numerische Mathematik, vol. 114,
pp. 653–695, 2010.

[7]
Y. D. Zhong, J. Han, and G. O. Brikis, “Differentiable physics simulations
with contacts: Do they have correct gradients w.r.t. position, velocity and
control?,” in ICML 2022 2nd AI for Science Workshop, 2022.

[8]
N. S. Nguyen and B. Brogliato, “Comparisons of multiple-impact laws for
multibody systems: Moreau's law, binary impacts, and the LZB approach,” in
Advanced Topics in Nonsmooth Dynamics, pp. 1–45, Springer, 2018.

[9]
C. Glocker, “An introduction to impacts,” in Nonsmooth Mechanics of
Solids, pp. 45–101, Springer, 2006.

[10]
J. J. Moreau, “Unilateral contact and dry friction in finite freedom
dynamics,” in Nonsmooth Mechanics and Applications, pp. 1–82,
Springer, 1988.

[11]
K. Tracy, T. A. Howell, and Z. Manchester, “Differentiable collision detection
for a set of convex primitives,” in 2023 IEEE Int. Conf. on Robotics
and Automation (ICRA), pp. 3663–3670, 2023.

[12]
W. Hogan, “Directional derivatives for extremal-value functions with
applications to the completely convex case,” Operations Research,
vol. 21, pp. 188–209, 1973.

[13]
A. Nurkanović, S. Albrecht, B. Brogliato, and M. Diehl, “The time-freezing
reformulation for numerical optimal control of complementarity Lagrangian
systems with state jumps,” Automatica, vol. 158, p. 111295, 2023.

[14]
E. Hairer and G. Wanner, Solving Ordinary Differential Equations II: Stiff
and Differential-Algebraic Problems.
Springer, 1996.

[15]
A. Nurkanović, M. Sperl, S. Albrecht, and M. Diehl, “Finite elements with
switch detection for direct optimal control of nonsmooth systems.” arXiv
preprint arXiv:2205.05337, 2024.

[16]
S. Scholtes, “Convergence properties of a regularization scheme for
mathematical programs with complementarity constraints,” SIAM Journal
on Optimization, vol. 11, pp. 918–936, 2001.

[17]
A. Wächter and L. T. Biegler, “On the implementation of an interior-point
filter line-search algorithm for large-scale nonlinear programming,” Mathematical Programming, vol. 106, pp. 25–57, 2006.

[18]
J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl,
“CasADi – A software framework for nonlinear optimization and optimal
control,” Mathematical Programming Computation, vol. 11, pp. 1–36,

[19]
J. Semple and S. Zlobec, “On the continuity of a Lagrangian multiplier
function in input optimization,” Mathematical Programming, vol. 34,
pp. 362–369, 1986.