# A Model for Optimal Resilient Planning Subject to Fallible Actuators

Kyle Baldes, Diptanil Chaudhuri, Jason M. O’Kane, and Dylan A. Shell The
authors are all affiliated with Dept. of Computer Science & Engineering, Texas
A&M University, College Station, TX, USA<EMAIL_ADDRESS>
###### Abstract

Robots incurring component failures ought to adapt their behavior to best
realize still-attainable goals under reduced capacity. We formulate the
problem of planning with actuators known _a priori_ to be susceptible to
failure within the Markov Decision Processes (MDP) framework. The model
captures utilization-driven malfunction and state-action dependent likelihoods
of actuator failure in order to enable reasoning about potential impairment
and the long-term implications of impoverished future control. This leads to
behavior differing qualitatively from plans which ignore failure. As actuators
malfunction, there are combinatorially many configurations which can arise. We
identify opportunities to save computation through re-use, exploiting the
observation that differing configurations yield closely related problems. Our
results show how strategic solutions are obtained so robots can respond when
failures do occur—for instance, in prudently scheduling utilization in order
to keep critical actuators in reserve.

##  I Introduction: Motivation and related work

Robot hardware malfunctions and faults aren’t rare events. One means of
mitigation to engineer rugged components; another is redundancy, as in
reconfigurable modular robots [1]. But over-provisioning, the idea underlying
both approaches, drives up weight, energy, and economic costs. An alternative
is to imbue systems with the ability to anticipate and tolerate their own
deterioration. The present paper adopts this latter philosophy. It focuses on
the specific problem of actuator failure—a known issue already identified as
important in prior work (e.g., see [12, 25, 26, 8]). We consider a robot that
plans and then executes a policy, reasoning not only about traditional
uncertainty in the form of imprecise control (i.e., stochasticity in action
outcomes), but also the possibility that each use of an actuator may cause it
to fail.

We treat this problem through the Markov Decision Processes (MDP) framework
[11, 17, 20, 3]. It is standard practice within robotics to solve MDPs under
the expected total discounted reward criterion and, although the discount
factor is often explained as quantifying the preference for immediate versus
future rewards, Puterman points out:

“Discounting may be viewed in another way. The decision maker values policies
according to the expected total reward criterion; however, the horizon
length…is random and independent of the actions of the decision maker. Such
randomness in the horizon length might be due to …death…” [16, §5.3, .p. 125]
Under this interpretation current approaches treat robot systems as failing in
a catastrophic (or all-or-nothing) way. The approach we present, thus, can be
considered a generalization of the standard model where, rather than a failure
debilitating the entire device, it retains some abilities to operate albeit in
a diminished fashion.

A direct way to treat failure of an actuator, once the system has determined
that the associated actions are no longer feasible, is to re-plan using those
capabilities which remain. This approach is _panglossian_ : despite failures
having occurred, the robot plans as if there will be no future failures.

Fundamentally, re-planning adopts a reactive point of view — it lacks
foresight. Instead, one might reason strategically about when and where to
employ actions, taking into account the implications for the future,
considering the structure of the state-space and, critically, the
_reliability_ of the available actuators. Because this involves reasoning
about (probabilistically weighted) impaired future operation, generating such
behavior is anticipatory rather than reactive.

x

Figure 1: A robot equipped with _wheels_ and _tracks_ travels from location A
to B subject to both motion uncertainty and the possibility of actuator
failure.

(a) A panglossian policy.

(b) Failure aware policy.

(c) Panglossian policy execution.

.

Figure 2: Computed policies for the situation in Fig. 1 for specific reward
values. (a) Policy derived from a $6\times 6$ gridworld representation of Fig.
1 not accounting for actuator failures. (b) Failure aware policy. (c)
Execution of the panglossian policy in (a) that, after failure, results in
undesired behavior. At each grid cell, the control with the maximum expected
future reward is displayed by indicating the actuator, direction of travel,
and the cost-to-go for that control. At the goal state, denoted by G, all
controls have the same expected future reward.

###  I-A Motivating scenario

Consider the scenario in Fig. 1: a robot must navigate from A to B. It has
wheels and tracks, through a reconfigurable wheel-track system like that of
[13] that allows the robot to rapidly transition between them. Only one
actuator may be used at a time so the robot must decide when to use each. The
tracks and wheels have complementary capabilities: wheels enable the robot to
move faster on the road at the price of reliability; tracks, in turn, trade
speed on the road for reliability at moderate speed anywhere. The tracks’
slower speed makes them unusable along sections of road with a minimum speed,
such as the one on the bridge designated point C, where a threshold is imposed
to alleviate highway congestion. This intricate actuation arrangement is not
infallible, as there is a nonzero probability of failure for both wheels and
tracks. The rate of failure is depends on usage, and the occurrence of
malfunction is detected when employing the actuator. The system may recover
from a single failure by switching when a failure is detected, however a
subsequent failure will leave the robot immobile.

To travel from A to B, the robot constructs a policy informing it about the
controls it will use at each location. Since the robot is unable to operate in
the water, there are two high-level classes of path along which it can travel
to get from A to B. The first, called route 1, takes the robot along the
access road, and briefly along the highway to cross the bridge at point C,
before taking the top access road to B. The second route requires the robot to
travel off-road through the sand at point D, and then across the grass before
reaching point B. A robot which disregards the potential for actuator failure
produces a policy (which we designate the panglossian policy) seen in Fig.
2(a). This policy has the robot traveling along route 1, utilizing the wheels
along the entire journey as their higher speed garners a higher expected
reward than using the tracks.

Now consider a policy which does account for actuator failure, depicted in
Fig. 2(b). This policy also chooses to travel along route 1, but instead
elects to use both actuators along the way. Most significantly, the wheels are
held in reserve before reaching the critical point C, where they are required
to cross the bridge along the highway. Once close enough to the goal, the
policy favors the wheels for their speed along the access road—as even if they
fail, the robot can then still succeed by falling back to the tracks.

The implications of the policy difference is illustrated in Fig. 2(c). Here a
myopic robot executing the panglossian policy travels along route 1 using the
wheels (highlighted in violet). However, the robot is alerted to the failure
of the wheels as it attempts to use them just before point C. Now, unable to
cross the bridge, the robot must turn-around and travel back to location A
before continuing on route 2 to location B (in magenta). A robot with policy
which anticipates failures will not encounter such a scenario as the wheels
are preserved, ensuring their availability at the critical juncture. Moreover,
the difference in outcomes can be arbitrarily bad, for example, when a robot
crosses a bridge onto an island and, owing to a failure, the stricken robot is
now stranded.

This trivial case exemplifies the necessity to equip robots with information
that characterizes how their capabilities might change with use and time.
Metrics such as component failure rates are already widely adopted, suggesting
that a reliability metric based on utilization is reasonable.

###  I-B Related work

The distinction between an over-provisioned system and one which adjusts its
performance in light of operating challenges has been proposed as the
difference between resilient and robust behavior [15, 27], both having seen
much recent attention.

Fault tolerance and fault detection have a long history in robotics [18, 6];
the early work is summarized in the survey of [23], and was revisited more
recently in [24]. Work in this vein generally involves introspection or a
process of scanning in order to detect faults and diagnosis (i.e. attributing
and tracking faultiness [22], or determining the bounds of one’s own
performance [7]). The literature on diagnosis and fault detection is
considerable, for a survey specifically with a focus on robots see [9]. Some
research also emphasizes specific recovery mechanisms, e.g., [4].

The control theory community has devoted considerable attention to the topic
of fault tolerant control, with wide application in safety critical systems
(e.g., aircraft and nuclear facilities [10]). The recent survey of [2]
evaluates different methodologies for detecting, isolating and handling faults
in both sensors and actuators. Those methods handle low-level fault detection
and tolerance, but neglect to incorporate actuator reliability into strategic
decisions that can affect how high-level goals are attained, e.g., picking
vastly different classes of solution.

For multi-robot systems, recent work on pursuit-evasion computes plans robust
to a subset of robots perishing [14]. Also, work has examined robot
reliability in teams of unmanned aerial vehicles (UAVs) [21], and during
multi-robot task allocation [19]. The latter provides the aphorism “in order
to successfully plan we must ‘plan to fail’ ”— a statement which succinctly
epitomize the methodology behind our work wherein we incorporate knowledge of
component reliability of a single robot into decision making.

##  II Notation, preliminaries, and assumptions

###  II-A Notation

Let $[{n}]$ be the set $\\{1,2,\dots,n\\}$, and ${2}^{X}$ denote the powerset
of $X$. We flatten a collection of sets $\boldsymbol{V}$ as
${\boldsymbol{V}}^{\\!\mskip 1.5mu\flat}=\cup_{V_{i}\in\boldsymbol{V}}V_{i}$.

###  II-B Preliminaries

Recall the following standard definition:

######  Definition 1 ([16])

A _Markov Decision Process (MDP)_

is a 5-tuple $\langle S,U,T,R,\gamma\rangle$ where

  * •

$S=\\{s_{1},s_{2},\dots,s_{|S|}\\}$ is the finite set of states;

  * •

$U=\\{u_{1},u_{2},\dots,u_{|U|}\\}$ is the finite set of controls;

  * •

$T:S\times U\times S\to[0,1]$ is the transition dynamics, describing the
stochastic state transitions of the system, assumed to be Markovian in the
states, where $\forall
t,\Pr(s^{t+1}=s^{\prime}|s^{t}=s,u^{t}=u)=T(s^{\prime},u,s)$;

  * •

$R:S\times U\to\mathbb{R}$ is the reward, specifying that value $R(s,u)$ is
obtained by executing control $u$ in state $s$;

  * •

$\gamma\in[0,1)$ is the discount factor.

One solves an MDP by providing a _policy_ , $\pi:S\to U$, which prescribes
that control $\pi(s)$ be executed in state $s$.

For policy $\pi$, starting from some initial state $s^{0}\in S$, we will
consider the following criterion:

###### Definition 2

For MDP $\scalebox{0.85}{$\mathcal{M}$}=\langle S,U,T,R,\gamma\rangle$ the
_expected cumulative reward_ of policy $\pi$ is

$E[\pi|\scalebox{0.85}{$\mathcal{M}$}]=\mathop{\scalebox{1.35}{$\mathbb{E}$}}\limits_{s^{0},s^{1},\dots}\left(\sum_{t=0}^{\infty}(\gamma^{t})R\big{(}s^{t},\pi(s^{t})\big{)}\right),$
(1)

where the expectation is over sequences $s^{0},s^{1},s^{2},\dots$ arising with
probabilities $T(s^{i},\pi(s^{t-1}),s^{t-1})$, for $t=1,2,\dots$.

We seek $\pi^{\star}(\cdot)$ maximizing (1), an _optimal policy_.

###  II-C Assumptions

The model explicitly assumes three things:—
1. Failure can be detected.
2. Failure is irreparable and considered binary.
3. Efficiency cannot increase after failure of an actuator.

Failure is detectable in that, after attempting to execute an action, there is
some indicator that failure has occurred when one has. We model complete
failure of each component so it remains inoperative thereafter. The treatment
does directly allow, as special cases, a variety of models where discovery of
failure is natural: for instance, as when an actuator’s failure is learned
only after trying it and then discovering a non-transition which only occurs
upon failure.

We specifically rule out the rather pathological cases where after a failure
has occurred, the robot has higher expected reward over future actions. This
bounds the performance of a particular set of actuators. Importantly, we do
not rule out the possibility that in the process of failing the robot
transitions to states with higher expected reward. Still the mere reduction in
the number of available actuators in a state cannot itself improve the state’s
expected value.

##  III Failure model

Consider a decision process where the set of controls is not modeled merely as
a flat set, but that each control is generated by a subsystem —which we will
designate as an ‘actuator’— that is responsible for multiple controls. These
are the atomic units of failure that we model. Let the robot have $m$
actuators so that the controls are partitioned into disjoint sets
$U_{1},U_{2},\dots,U_{m}$. For example, $U_{2}=\\{u_{5},u_{6},u_{7},u_{8}\\}$
gives the $4$ controls that actuator $2$ is involved in generating. Then we
might consider MDP $M=\langle S,\;U_{1}\cup U_{2}\cup\dots\cup
U_{m}\;,T,R,\gamma\rangle.$

Imagine that the robot attempts to execute some control $u_{j}$ but that as it
does so, with some probability, the actuator fails. The state outcome does not
follow the usual $T(\cdot,u_{j},\cdot)$ rule, and the actuator becomes
inoperative so that, from this point forward, none of the controls it
generates can be executed. Per Assumption 1, this failure can be reliably
determined either because the state transition is unusual, or by some other
detector (one might imagine smoke, odd sounds, etc.). We treat actuator
failures as independent statistical events: while greater use of the actuator
is more likely to cause failure, one actuator failing does not cause another
to fail. But one slight subtlety: because an actuator fails, the controls it
generates are no longer available, this may cause other controls to be
executed than would have been, thereby increasing the chance of another
actuator failing. This correlation, through increased use, _is_ captured in
the model.

Here is our definition:

###### Definition 3

A _Fallible Actuator MDP (FA-MDP)_ is an 8-tuple $\langle
S,m,\boldsymbol{U},T,F,R,\rho,\gamma\rangle$ where

  * •

$S=\\{s_{1},s_{2},\dots,s_{|S|}\\}$ is the finite set of states;

  * •

$m\in\mathbb{N}_{>0}$ is the number of actuators;

  * •

$\boldsymbol{U}=\\{U_{1},U_{2},\dots,U_{m}\\}$ is a collection of disjoint
control sets.

  * •

$T:S\times{\boldsymbol{U}}^{\\!\mskip 1.5mu\flat}\times S\to[0,1]$ is the
regular (or nominal) transition model, describing the stochastic state
transitions of the system, assumed to be Markovian in the states, where
$\forall t,\Pr(s^{t+1}=s^{\prime}|s^{t}=s,u^{t}=u)=T(s^{\prime},u,s)$;

  * •

$F:S\times{\boldsymbol{U}}^{\\!\mskip 1.5mu\flat}\times S\to[0,1]$ is the
malfunctioning transition dynamics, describing the stochastic state
transitions of the system as the control fails, Markovian in the states, where
$\forall
t,\Pr(s^{t+1}\\!=\\!s^{\prime}|s^{t}\\!=\\!s,u^{t}\\!=\\!u)=\allowbreak
F(s^{\prime},u,s)$ as the actuator that generates $u$ becomes inoperative;

  * •

$R:S\times{\boldsymbol{U}}^{\\!\mskip 1.5mu\flat}\to\mathbb{R}$ is the reward
function;

  * •

$\rho:S\times{\boldsymbol{U}}^{\\!\mskip 1.5mu\flat}\to[0,1]$ is the
reliability function, modeling that control $u$, when executed from state $s$,
results in a failure of actuator $k$ with probability $1-\rho(s,u)$;

  * •

$\gamma\in[0,1]$ is the discount factor.

Let $\hat{\rho}_{k}=\\!\\!\\!\max\limits_{s\in S,u\in U_{k}}\\!\\!\rho(s,u)$
be the most reliable control for actuator $k\in[{m}]$ and, similarly, define
the least reliable control, $\check{\rho}_{k}=\\!\\!\\!\min\limits_{s\in
S,u\in U_{k}}\\!\\!\rho(s,u)$. Looking across all the actuators, now also
define
$\overline{\rho}=\\!\\!\max\limits_{k\in[{m}]}\\!\hat{\rho}_{k}=\\!\\!\\!\\!\max\limits_{s\in
S,u\in{\boldsymbol{U}}^{\\!\mskip 1.5mu\flat}}\\!\\!\rho(s,u)$. We will
require either that $\gamma\in[0,1)$ or $\overline{\rho}\in[0,1)$.

Note that we do not require $0<\overline{\rho}$ as in those cases the problem
will involve a finite-horizon (there are at most $|{\boldsymbol{U}}^{\\!\mskip
1.5mu\flat}|$ steps). In fact, $\overline{\rho}=0$ is meaningful, being a
model of single-shot actuators (i.e., is the case of disposable/one-time use
gadgets—like thruster rockets or parachutes).

The evolution of FA-MDP $\scalebox{0.85}{$\mathcal{M}$}=\langle
S,m,\boldsymbol{U},T,F,R,\boldsymbol{\alpha}\rangle$ is straightforward: when,
at time $t$, the robot is in some state $s^{t}$, and it attempts to execute
$u^{t}=u_{j}\in U_{k}$, either

  * •

with probability $\rho(s^{t},u_{j})$: actuator $k$ does not fail and the
system transitions to state $s^{t+1}$ via $T(s^{t+1},u_{j},s^{t}$);

  * •

alternatively with probability $1-\rho(s^{t},u_{j})$: actuator $k$ fails and
the system obeys law $F(s^{t+1},u_{j},s^{t})$.

We have assumed here that $u_{j}$ was admissible because actuator $k$ remained
operable at time $t$. Any attempt to execute a control from a previously
failed actuator is undefined.

A FA-MDP can be cast into a traditional MDP as follows:

###### Definition 4

FA-MDP $\scalebox{0.85}{$\mathcal{M}$}=\langle
S,m,\boldsymbol{U},T,F,R,\rho,\gamma\rangle$ has an associated _monolithic
MDP_ $\scalebox{0.85}{$\mathcal{M}$}_{\rm mon}=\langle
S^{\prime},U^{\prime},T^{\prime},R^{\prime},\gamma^{\prime}\rangle$:

  * •

$S^{\prime}=S\times{2}^{[{m}]}$;

  * •

$U^{\prime}=U_{1}\cup U_{2}\cup\dots\cup U_{m}$;

  * •

$T^{\prime}((s^{\prime},I^{\prime}),u,(s,I))=$
$\left\\{\begin{array}[]{ll}\frac{1}{\beta}\rho(s,u)T(s^{\prime},u,s),&{\small\text{if
}u\in U_{k},k\in I,I=I^{\prime}},\\\\[6.0pt]
\hbox{\multirowsetup$\frac{1}{\beta}(1-\rho(s,u))F(s^{\prime},u,s),\\!\\!$}&{\small\text{if
}u\in U_{k},k\in I\setminus I^{\prime}},\\\ &\quad{\small\text{\; and
}I=I^{\prime}\cup\\{k\\}},\\\\[6.0pt]
0,&{\small\text{otherwise},}\end{array}\right.$

  * where it is to be understood that $U_{k}\in\boldsymbol{U}$, and multiplier $\phantom{.}\beta=\left\\{\begin{array}[]{ll}\overline{\rho}&{\small\text{if }\gamma=1},\\\ 1&{\small\text{otherwise};}\end{array}\right.$

  * •

$R^{\prime}\big{(}(s,I),u\big{)}=R(s,u)$;

  * •

$\gamma^{\prime}=(\gamma\cdot\beta)$.

In monolithic MDP form, the state space is augmented by a set indicating which
actuators are operable and solving the problem involves producing a policy,
$u^{t}=\pi^{\star}((s^{t},I^{t}))$, starting from state $(s^{0},[{m}])$. The
first case of $T^{\prime}$ is wherein no actuator failure occurs; the second
when actuator $k$ fails.

As there are many potential ways actuators may fail, the state space is
exponential in $m$. But we note that the state space’s structure is ripe for
exploitation in data-structures (to help reduce memory requirements) and
algorithms.

##  IV Exploiting structure

Instead of $M_{\rm mon}$ where the state-space structure is lost, to expose
the underlying structure we represent the FA-MDP as a directed graph with
associated state data:

###### Definition 5

For FA-MDP $\scalebox{0.85}{$\mathcal{M}$}=\langle
S,m,\boldsymbol{U},T,F,R,\rho,\gamma\rangle$, an associated _value function
lattice_ is a directed graph
$G(\scalebox{0.85}{$\mathcal{M}$})=(\mathcal{X},\mathcal{E})$, with

  * $\bullet\,$ nodes $\mathcal{X}\\!=\\!\left\\{\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle\,|\,\boldsymbol{X}_{r}\subseteq\boldsymbol{U},V_{r}\\!:\\!S\to\mathbb{R}\right\\}$, every $\boldsymbol{X}_{r}$ being a subset of $\boldsymbol{U}$, and each $V_{r}$ a value function; and

  * $\bullet\,$ the set of arcs $\mathcal{E}\\!=\\!\big{\\{}(\langle{\boldsymbol{X}_{q}};{V_{q}}\rangle,\langle{\boldsymbol{X}_{d}};{V_{d}}\rangle)\,\big{|}\,U_{i}\not\in\boldsymbol{X}_{d}\text{ and }\allowbreak\boldsymbol{X}_{q}=\boldsymbol{X}_{d}\cup\\{U_{i}\\}\big{\\}}$.

The unique _top_ of $G(\scalebox{0.85}{$\mathcal{M}$})$ is the node
$\langle{\boldsymbol{U}};{V_{\boldsymbol{U}}}\rangle$, and the unique _bottom_
is the node $\langle{\varnothing};{V_{\varnothing}}\rangle$. For this
degenerate case, we define the value function as

$V_{\varnothing}(s)=\tfrac{1}{1-\gamma}\cdot\min_{u\in{\boldsymbol{U}}^{\\!\mskip
1.5mu\flat}}R(s,u).$

Figure 3: Value function lattice for a simple example with a set
$\boldsymbol{U}$ comprising three elements. The small grids are a cartoon
depiction of a $5\times 5$ state space emphasizing that each element is a
value function; e.g., $V_{\\{2,3\\}}:\\!S\to\mathbb{R}$ assigns values to each
state for the situation when $1$ has failed.

For node $\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle\in\mathcal{X}$ with
$U_{\ell}\in\boldsymbol{X}_{r}$, we introduce the notation
$\boldsymbol{X}_{r\setminus
U_{\ell}}\coloneqq\boldsymbol{X}_{r}\setminus\\{U_{\ell}\\}$, and immediately
extend its use so that $\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle$ is
connected by an arc to $\langle{\boldsymbol{X}_{r\setminus
U_{\ell}}};{V_{r\setminus U_{\ell}}}\rangle$. It is helpful to visualize the
available actuators as being described in this directed graph, arranged

in layers per set cardinality, from $m$ at the top, to $0$ at the bottom as
depicted in Fig. 3. Actuator failures occur in a directed way on this graph,
moving along an arc. We can think of local copies of state space $S$, along
with the associated value functions for each node. As the robot moves, it
occupies a state in $S$ and the transition dynamics, though moving in $S$,
either stay in the node when there is no failure, or move down to a node in
the layer below.

The core idea is to perform a specialized form of value iteration on the value
functions associated at nodes because it allows us to exploit the inter-
relationships between value functions. There are two specific forms. Failure
is monotonic: if actuator $k$ fails, the controls in $U_{k}$ become
unavailable ever after—the propagation of updates during value iteration
respects a partial order on all the states induced by this monotonicity.
Secondly, a value function at one layer yields a lower-bound for value
functions above.

To apply value iteration at some particular node in
$G(\scalebox{0.85}{$\mathcal{M}$})$, we introduce a local operator. For node
$\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle$, a single Bellman update at state
$s\in S$ is as follows:

$\displaystyle V_{r}^{\scalebox{0.8}{$({i+1})$}}(s)\\!=\\!\\!\\!$
$\displaystyle\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\bigg{[}\gamma\\!\sum_{s^{\prime}\in
S}\\!\Big{(}\rho(s,u)T(s^{\prime},u,s)V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})\;+$
$\displaystyle\;\;(1-\rho(s,u))F(s^{\prime},u,s)V^{\scalebox{0.8}{$({i})$}}_{r\setminus
U_{k}}(s^{\prime})\Big{)}+R(s,u)\bigg{]}.$

Let ${\rm\bf b}$ denote the operator defined so that, given
$\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle$, the update is performed across
the vector of states:

$V_{r}^{\scalebox{0.8}{$({i+1})$}}={\rm\bf b}V_{r}^{\scalebox{0.8}{$({i})$}}.$

Here ${\rm\bf b}$ is a _local_ Bellman operator in the sense that only $V_{r}$
and the value functions immediately reached by arcs departing
$\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle$ are involved.

First, we establish that this operator converges to a fixed value function by
showing that it is a contraction. Given two different value functions at some
node, applying ${\rm\bf b}$ brings them closer together, quantified through
the _max norm_ :

$\left\|{V}\right\|=\max_{s\in S}\left|{V(s)}\right|.$ (2)

###### Property 1

Local Bellman operator ${\rm\bf b}$ is a contraction.

###### Proof:

For actuator set $\boldsymbol{X}_{r}$, consider the two value functions
$V_{r}^{\scalebox{0.8}{$({i})$}}$ and
${V^{\prime}}_{r}^{\scalebox{0.8}{$({i})$}}$:

$\displaystyle\big{\|}{\rm\bf b}V_{r}^{\scalebox{0.8}{$({i})$}}$
$\displaystyle-{\rm\bf b}{V^{\prime}}_{r}^{\scalebox{0.8}{$({i})$}}\big{\|}$
$\displaystyle\leq\max_{s\in S}\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\bigg{|}\gamma\rho(s,u)\sum_{s^{\prime}\in
S}T(s^{\prime},u,s)\Big{(}V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-{V^{\prime}}_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})\Big{)}\bigg{|}$
$\displaystyle\leq\gamma\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\hat{\rho}_{k}\cdot\max_{s\in
S}\sum_{s^{\prime}\in
S}T(s^{\prime},u,s)\Big{|}V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-{V^{\prime}}_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})\Big{|}$
$\displaystyle\leq\gamma\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\hat{\rho}_{k}\cdot\max_{s^{\prime}\in
S}\Big{|}V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-{V^{\prime}}_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})\Big{|}$
$\displaystyle=\gamma\cdot\overline{\rho}\cdot\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-{V^{\prime}}_{r}^{\scalebox{0.8}{$({i})$}}}\right\|.$

Finally, the contraction follows as factor $\gamma\cdot\overline{\rho}<1$. ∎

We shall use of the following (based upon [16, p. 150]):

###### Lemma 1

$\big{\|}{V^{\star}-V^{\scalebox{0.8}{$({n})$}}}\big{\|}\leq\lambda\big{\|}{V^{\scalebox{0.8}{$({n})$}}-V^{\scalebox{0.8}{$({n-1})$}}}\big{\|}$
. where
$\lambda=\dfrac{{(\gamma\cdot\overline{\rho})}}{1-{(\gamma\cdot\overline{\rho})}}$.
.

###### Proof:

$\displaystyle\big{\|}{V^{\scalebox{0.8}{$({m+n})$}}-V^{\scalebox{0.8}{$({n})$}}}\big{\|}$
$\displaystyle\leq\sum_{k=0}^{m-1}{\big{\|}{V^{\scalebox{0.8}{$({n+k+1})$}}-V^{\scalebox{0.8}{$({n+k})$}}}\big{\|}}$
$\displaystyle\leq\sum_{k=0}^{m-1}{\big{\|}{{\rm\bf
b}^{k}V^{\scalebox{0.8}{$({n+1})$}}-{\rm\bf
b}^{k}V^{\scalebox{0.8}{$({n})$}}}\big{\|}}$
$\displaystyle\leq\sum_{k=0}^{m-1}{{(\gamma\cdot\overline{\rho})^{k}}\big{\|}{V^{\scalebox{0.8}{$({n+1})$}}-V^{\scalebox{0.8}{$({n})$}}}\big{\|}}$
$\displaystyle\leq{\frac{1-{(\gamma\cdot\overline{\rho})}^{m}}{1-{(\gamma\cdot\overline{\rho})}}}\big{\|}{V^{\scalebox{0.8}{$({n+1})$}}-V^{\scalebox{0.8}{$({n})$}}}\big{\|}.$

Hence, the sequence
$V^{\scalebox{0.8}{$({n})$}},V^{\scalebox{0.8}{$({n+1})$}},V^{\scalebox{0.8}{$({n+2})$}},V^{\scalebox{0.8}{$({n+3})$}},\cdots$
is Cauchy, and we can take the limit as $m\to\infty$. We obtain the final
result unrolling once using Property 1. ∎

Next, we examine the relationship between approximation errors within the
value function lattice. Consider an arbitrary node
$\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle$, where
$\boldsymbol{X}_{r}=\\{U_{1},U_{2},U_{3},\dots,U_{N_{r}}\\}$. It accordingly
has arcs connecting to nodes $\langle{\boldsymbol{X}_{r\setminus
U_{1}}};{V_{r\setminus U_{1}}}\rangle,$ $\langle{\boldsymbol{X}_{r\setminus
U_{2}}};{V_{r\setminus U_{2}}}\rangle,$ $\dots,$
$\langle{\boldsymbol{X}_{r\setminus U_{N_{r}}}};{V_{r\setminus
U_{N_{r}}}}\rangle$. Denote the global fixed point value functions as
$V^{\star}_{r}$ for the node itself, and $V^{\star}_{r\setminus U_{j}}$ with
$j\in[{N_{r}}]$ for those the layer below. (One can imagine this fixed point
either obtained from the monolithic problem as in Definition 4, or as the
fixed point under local Bellman operator, computed upward from the lattice
bottom.)

We analyze an update to $V_{r}^{\scalebox{0.8}{$({i})$}}$, assuming that, for
each $j\in[{N_{r}}]$, we have value functions $V_{r\setminus U_{j}}$ with
error at most $\varepsilon_{j}$. Then, the error in some Bellman iterate is
bounded:

$\displaystyle\big{\|}$ $\displaystyle{\rm\bf
b}V_{r}^{\scalebox{0.8}{$({i})$}}-{V}_{r}^{\star}\big{\|}=\big{\|}V_{r}^{\scalebox{0.8}{$({i+1})$}}-V_{r}^{\star}\big{\|}$
$\displaystyle=\max_{s\in
S}\left|{V_{r}^{\scalebox{0.8}{$({i+1})$}}(s,u)-V_{r}^{\star}(s,u)}\right|$
$\displaystyle=\gamma\max_{s\in S}\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\\!\bigg{|}\sum_{s^{\prime}\in
S}\rho(s,u)T(s^{\prime},u,s)\Big{(}V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-V_{r}^{\star}(s^{\prime})\Big{)}+$
$\displaystyle\quad\qquad\sum_{s^{\prime}\in
S}(1-\rho(s,u))F(s^{\prime},u,s)\Big{(}V_{r\setminus
U_{k}}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-V_{r\setminus
U_{k}}^{\star}(s^{\prime})\Big{)}\bigg{|}$
$\displaystyle\leq\gamma\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\\!\Bigg{(}\max_{s\in
S}\bigg{|}\sum_{s^{\prime}\in
S}\rho(s,u)T(s^{\prime},u,s)\Big{(}V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-V_{r}^{\star}(s^{\prime})\Big{)}\bigg{|}+$
$\displaystyle\qquad\max_{s\in S}\bigg{|}\sum_{s^{\prime}\in
S}(1-\rho(s,u))F(s^{\prime},u,s)\Big{(}V_{r\setminus
U_{k}}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-V_{r\setminus
U_{k}}^{\star}(s^{\prime})\Big{)}\bigg{|}\Bigg{)}$
$\displaystyle\leq\gamma\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\\!\bigg{(}\hat{\rho}_{k}\cdot\max_{s^{\prime}\in
S}\Big{|}\big{(}V_{r}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-V_{r}^{\star}(s^{\prime})\big{)}\Big{|}+$
$\displaystyle\quad\quad\qquad\qquad(1-\check{\rho}_{k})\cdot\max_{s^{\prime}\in
S}\Big{|}\big{(}V_{r\setminus
U_{k}}^{\scalebox{0.8}{$({i})$}}(s^{\prime})-V_{r\setminus
U_{k}}^{\star}(s^{\prime})\big{)}\Big{|}\bigg{)}$
$\displaystyle=\gamma\max_{\begin{subarray}{c}u\in U_{k}\\\
U_{k}\in\boldsymbol{X}_{r}\end{subarray}}\\!\bigg{(}\hat{\rho}_{k}\cdot\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\star}}\right\|+(1-\check{\rho}_{k})\cdot\left\|{V_{r\setminus
U_{k}}^{\scalebox{0.8}{$({i})$}}-V_{r\setminus
U_{k}}^{\star}}\right\|\bigg{)}$
$\displaystyle\leq\gamma\max_{k\in[{{N_{r}}}]}\\!\bigg{(}\hat{\rho}_{k}\cdot\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\star}}\right\|+(1-\check{\rho}_{k})\cdot\varepsilon_{k}\bigg{)}.$

To bound $\left\|{{\rm\bf
b}V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\star}}\right\|$ by
$\varepsilon_{r}$, it is enough to have

$\displaystyle\max_{k\in[{N_{r}}]}\\!\bigg{(}\hat{\rho}_{k}\cdot\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\star}}\right\|+(1-\check{\rho}_{k})\cdot\varepsilon_{k}\bigg{)}$
$\displaystyle\leq\frac{\varepsilon_{r}}{\gamma},$

but we can rewrite it, using Lemma 1,

$\displaystyle\max_{k\in[{N_{r}}]}\\!\bigg{(}\hat{\rho}_{k}\cdot\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\star}}\right\|+(1-\check{\rho}_{k})\cdot\varepsilon_{k}\bigg{)}$
$\displaystyle\leq$
$\displaystyle\max_{k\in[{N_{r}}]}\\!\bigg{(}\hat{\rho}_{k}\cdot\lambda\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\scalebox{0.8}{$({i-1})$}}}\right\|+(1-\check{\rho}_{k})\cdot\varepsilon_{k}\bigg{)}$
$\displaystyle\leq\frac{\varepsilon_{r}}{\gamma},$

and, as a bound on the maximum, we can simply require:

$\displaystyle\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\scalebox{0.8}{$({i-1})$}}}\right\|\leq\min_{k\in[{N_{r}}]}\Bigg{[}\frac{1}{\lambda}\bigg{(}\frac{\varepsilon_{r}}{\gamma\hat{\rho}_{k}}-\frac{(1-\check{\rho}_{k})}{\hat{\rho}_{k}}\cdot\varepsilon_{k}\bigg{)}\Bigg{]}.$

Further, when each node
$\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle\in\mathcal{X}$ is subject to the
same quality constraint $\varepsilon_{r}$ we have:

$\displaystyle\left\|{V_{r}^{\scalebox{0.8}{$({i})$}}-V_{r}^{\scalebox{0.8}{$({i-1})$}}}\right\|\leq\min_{k\in[{N_{r}}]}\Bigg{[}\frac{\varepsilon_{r}}{\lambda\hat{\rho}_{k}}\bigg{(}\frac{1}{\gamma}+\check{\rho}_{k}-1\bigg{)}\Bigg{]}.$

##  V Solving FA-MDP Problems

The simplest approach to solving FA-MDP problems is to construct a monolithic
FA-MDP which can be treated by a general MDP solver. We define a planner
utilizing this approach as a Monolithic Planner. By introducing a value
function lattice we can reduce the time required to converge to an optimal
policy. This requires a specialized solver which can operate on the value
function lattice directly, essentially treating the FA-MDP as a lattice of
smaller MDPs. A planner which utilizes the value function lattice is called a
Lattice Planner.

Since the value function lattice is a directed acyclic graph (DAG), we can use
a method similar to Topological Value Iteration [5] to solve each node to a
given quality requirement $\varepsilon_{\rm{desired}}$. This is done from the
bottom upward, where each node is solved to the quality constraint once those
below it have already been solved. The binary failure property of actuators
guarantees this approach since every node is causally dependent on only the
nodes below it. To solve an FA-MDP subject to the quality requirement
$\varepsilon_{\rm{desired}}$ using a Lattice Planner we first compute the
maximum Bellman error $y_{r}$ required to satisfy $\varepsilon_{\rm{desired}}$
for each node $\langle{\boldsymbol{X}_{r}};{V_{r}}\rangle$. If each node is
held to the same quality constraint we have

$\displaystyle
y_{r}=\min_{j\in[{N_{r}}]}\Bigg{[}\frac{\varepsilon_{\rm{desired}}}{\lambda\hat{\rho}_{j}}\bigg{(}\frac{1}{\gamma}+\check{\rho}_{j}-1\bigg{)}\Bigg{]}.$

We then perform value iteration on each node in the lattice in a bottom-up
fashion. Note that in contrast to Topological Value Iteration [5], we have the
useful properties that the meta-state orderings enable, but without having to
pay the cost of actually computing strongly connected components.

For an additional speedup we can leverage the similarity between actuator
combinations to propagate values up the lattice. The monotonicity of failure
ensures that value functions at every node in the lattice serves as a lower
bound for the nodes above them. As a result, we can _hot-start_ the value
function at each node by taking the maximum value at each state in the nodes
below. A Lattice Planner which employs hot-starting is called a Hot-Start
Lattice Planner.

##  VI Experiments

To measure the performance of each planner, we track the planning time
required via the total number of reads and writes to the value functions. This
metric is a meaningful approximation of a planner’s running time because it is
implementation agnostic and removes noisy runtime clock measurements. Our
implementation (in Python) limits the scale of problems we can evaluate in a
practice—future work might optimize the implementation to investigate FA-MDPs
with larger state spaces and more actuators.

Figure 4: Value Function Operations _vs_ State Backup Ordering:
($\gamma=0.99$, $\varepsilon_{{\rm desired}}=0.001$) Comparison of state
backup orderings during asynchronous value iteration. Ordering distributions
are represented by the box and whisker plots.

###  VI-A Methodology

To explore the properties of FA-MDP problems we will use a 2D gridworld which
provides a simple yet scalable environment to test on. Each cell in the grid
corresponds to a state and is associated with a terrain property. It is
convenient to define the transition dynamics $T$ and $F$, the reward $R$, and
the reliability $\rho$ functions with respect to this property. The terrain
dictates the controls available in a given state, with only a subset of the
actuators working in each terrain. If an actuator is available in a state, it
has a set of controls for moving to directly adjacent cells. The transition
dynamics are dictated by a single _precision_ value $p\in[0,1]$, which is the
probability that a given control will transition to the desired next state,
assuming no failure occurs. For example if the agent is in state $s\in S$,
control $u\in U_{k}$ is __up and the state above $s$ is $s^{\prime}\in S$,
then $T(s^{\prime},u,s)=a$. If state $s$ has $n$ states adjacent to it, then
for every remaining state $s_{i},i\in[{n-1}]$,

$\displaystyle T(s_{i},u,s)=\frac{(1-a)}{(n-1)}.$

The transition dynamics for when an actuator fails $F$ are defined similarly,
but usually with a reduced precision value.

The problem instance includes a single goal state $s_{{\rm goal}}$ where
$T(s_{{\rm goal}},u,s_{{\rm goal}})=1,\rho(s_{{\rm goal}},u)=1$, and
$R(s_{{\rm goal}},u)$ is a positive reward $R_{{\rm goal}}$ for all controls
$u\in{\boldsymbol{U}}^{\\!\mskip 1.5mu\flat}$. The value functions are
initialized such that

$\displaystyle V^{(0)}(s)=\min_{s\in
S,u\in\boldsymbol{U}}\frac{R(s,u)}{1-\gamma}\text{\; for all }s\in
S\setminus\\{s_{{\rm goal}}\\},$

analogous to being stranded for eternity. At $s_{{\rm goal}}$ we have

$\displaystyle V^{(0)}(s_{{\rm goal}})=\frac{R_{{\rm goal}}}{1-\gamma},$

analogous to using any control in the goal state continuously.

###  VI-B Asynchronous Value Iteration State Backup Ordering

For asynchronous value iteration, the sequence in which states undergo value
function backup plays a pivotal role in determining the computation required
for convergence. This study investigates the ramifications of state ordering
on algorithmic performance, so that we might distinguish between different
performance factors. We also lay the groundwork for an evaluation of ordering
heuristics.

Fig. 4 illustrates the impact ordering plays on performance across planners.
For each of the planners, we randomly sample 500 unique backup orderings from
a uniform distribution and perform value iteration. Note that the definition
of a random ordering is different for a Monolithic Planner and a Lattice
Planner. Since the monolithic FA-MDP does not preserve the partial order
imposed by the actuators over the full state space, a random ordering for a
Monolithic Planner is a random order across all $|S|2^{m}$ states. For a
Lattice Planner, a random ordering is defined as a random order over the local
copy of $|S|$ states within each node. As a result, the Monolithic Planner has
the largest performance range.

To better analyze the backup order implications on the Monolithic Planner, we
propose a hypothetical minimum number of operations it requires. We define
this minimum by utilizing an _oracle_ to provide the converged value function
$V^{*}$. The hypothetical best order (via an oracle) is then defined as a
partial order over the states subject to the values provided by the oracle,
with the highest valued states being visited first. We then can define the
hypothetical minimum number of operations as the number of operations required
for a Monolithic Planner to converge to $\varepsilon_{{\rm desired}}$ using
the hypothetical best order. In practice, we obtain the oracle ordering by
running value iteration until convergence to some $\varepsilon_{{\rm
desired}}$ and use that value function as a reasonable approximation of
$V^{*}$. We then create our hypothetical best order informed by this value
function and evaluate the total number of operations required for the
Monolithic Planner to converge with this ordering. The orange line in Fig. 4
represents the hypothetical minimum operations. Note that the Lattice Planner
with the Manhattan distance heuristic ordering outperforms the hypothetical
best-case ordering for the Monolithic Planner. This indicates that the Lattice
Planners benefit not only from the partial ordering imposed by the lattice
structure, but also from the decomposition into smaller MDPs. Despite the
small size of the state space and the limit actuator set, the Lattice Planners
unquestionably outperform the Monolithic Planner.

To effectively scale these planners for more complex MDPs, it’s crucial to
adopt an ordering heuristic tailored to the problem domain. Our evaluation of
the Manhattan distance heuristic demonstrates its superiority over most random
orderings on each of the planners, making it a good choice as the default
ordering heuristic for subsequent tests.

Figure 5: Value Function Operations _vs_ Number of Actuators: Scaling the
number of actuators from 2 to 12 using the gridworld from Fig. 2(b).

###  VI-C Scaling

To characterize the performance of each planner as the size of the lattice
grows, we increase the number of actuators $m$ in the baseline $6\times 6$
gridworld from 2 up to 12. For consistency, we create an additional copy of
each actuator for each increasing scale. Thus an FA-MDP with 12 actuators
contains 6 copies of the 2 original actuators.

Fig. 5 demonstrates why the exponential nature of FA-MDP problems necessitates
planner efficiency. The Monolithic Planner rapidly becomes infeasible beyond 6
actuators, even with a small grid containing 36 states. While all planners
have the same asymptotic complexity, the Lattice Planners are able to scale to
significantly more actuators before becoming infeasible. Here the value of
hot-starting is made apparent as the number of actuators increases. Despite
the small grid, hot-starting enables planning on more complicated robots as
the Hot-Start Lattice Planner with 12 actuators performs a similar number of
operations as the Lattice Planner with only 10.

##  VII Conclusion

When suitably interpreted, discount factors within MDPs express an all-or-
nothing model of breakdown. The present paper refines that model to enable
treatment of incremental, component-wise failure for the case of actuators
that cease proper functioning with a likelihood proportional to their use.
This allows one to model a robot which degrades across time and to plan
operations strategically by choosing actions that anticipate a future in which
the hardware is less capable. In essence, our approach augments the notion of
state with characteristics of robot’s operational hardware. While this means
that standard MDP solvers can treat such problems, being general solvers, they
are necessarily oblivious to the specific structure which arises when
characterizing malfunction. We demonstrate that the lattice structure produces
a more efficient solution through 3 separate means: (1) the partial order
induced by the subset structure arising from set inclusion of actuators; (2)
decomposition into a set of smaller MDPs; (3) the similarity of value
functions for sets of actuators that have minor differences. Our empirical
results show that the advantage afforded by these three ideas increases with
the addition of actuators, i.e., as robots become more complex.

## References

  * [1] H. Ahmadzadeh and E. Masehian, “Modular robotic systems: Methods and algorithms for abstraction, planning, control, and synchronization,” _Artificial Intelligence_ , vol. 223, pp. 27–64, 2015.
  * [2] A. A. Amin and K. M. Hasan, “A review of fault tolerant control systems: Advancements and applications,” _Measurement_ , vol. 143, pp. 58–68, 2019.
  * [3] D. P. Bertsekas, _Reinforcement Learning and Optimal Control_. Belmont, M.A., U.S.A: Athena Scientific, 2019.
  * [4] D. Crestani, K. Godary-Dejean, and L. Lapierre, “Enhancing fault tolerance of autonomous mobile robots,” _Robotics and Autonomous Systems_ , vol. 68, pp. 140–155, 2015.
  * [5] P. Dai, Mausam, D. S. Weld, and J. Goldsmith, “Topological value iteration algorithms,” _Journal of Artificial Intelligence Research_ , vol. 42, pp. 181–209, 2011.
  * [6] B. R. Donald, “A geometric approach to error detection and recovery for robot motion planning with uncertainty,” _Artificial Intelligence_ , vol. 37, no. 1, pp. 223–271, 1988.
  * [7] A. Gautam, T. Whiting, X. Cao, M. A. Goodrich, , and J. W. Crandall, “A Method for Designing Autonomous Robots that Know Their Limits,” in _Proc. of IEEE International Conference on Robotics and Automation (ICRA)_ , 2022, (to appear).
  * [8] T. Hsiao and M.-C. Weng, “A hierarchical multiple-model approach for detection and isolation of robotic actuator faults,” _Robotics and Autonomous Systems_ , vol. 60, no. 2, pp. 154–166, 2012.
  * [9] E. Khalastchi and M. Kalech, “On fault detection and diagnosis in robotic systems,” _ACM Computing Surveys (CSUR)_ , vol. 51, no. 1, pp. 1–24, 2018.
  * [10] V. Kumar, L. Singh, and A. K. Tripathi, “Reliability analysis of safety-critical and control systems: a state-of-the-art review,” _IET Software_ , vol. 12, no. 1, pp. 1–18, 2018.
  * [11] S. M. LaValle, _Planning Algorithms_. Cambridge, U.K.: Cambridge University Press, 2006, available at http://planning.cs.uiuc.edu/.
  * [12] Y. Lu, H. R. Karimi, and N. Zhang, “Joint actuator fault estimation and localization under round-robin protocol for wheeled mobile robot,” _Transactions of the Institute of Measurement and Control_ , 2023.
  * [13] H. Lyness and D. Apostolopoulos, “An evaluation of reconfigurable wheel-track testbeds for military vehicles,” in _Proc. of 15th European-African Regional Conference of the International Society for Terrain Vehicle Systems Conference (ISTVS ’19)_ , September 2019.
  * [14] T. Olsen, N. Stiffler, and J. M. O’Kane, “Robust-by-design plans for multi-robot pursuit-evasion,” in _Proc. IEEE International Conference on Robotics and Automation_ , 2022.
  * [15] A. Prorok, M. Malencia, L. Carlone, G. S. Sukhatme, B. M. Sadler, and V. Kumar, “Beyond robustness: A taxonomy of approaches towards resilient multi-robot systems,” _arXiv preprint arXiv:2109.12343_ , 2021.
  * [16] M. L. Puterman, _Markov Decision Processes: Discrete Stochastic Dynamic Programming_. Hoboken, N.J., U.S.A: Wiley-Interscience, 1994.
  * [17] S. J. Russell and P. Norvig, _Artificial Intelligence: A Modern Approach_ , 3rd ed. Upper Saddle River, NJ, U.S.A.: Prentice-Hall, Inc., 2009.
  * [18] S. Srinivas, “Error Recovery in Robot Systems,” PhD Dissertation, California Institute of Technology, 1977.
  * [19] S. B. Stancliff, J. Dolan, and A. Trebi-Ollennu, “Planning to fail—reliability needs to be considered a priori in multirobot task allocation,” in _2009 IEEE International Conference on Systems, Man and Cybernetics_. IEEE, 2009, pp. 2362–2367.
  * [20] R. S. Sutton and A. G. Barto, _Reinforcement Learning: An Introduction_ , 2nd ed. Cambridge, M.A., U.S.A.: MIT Press, 2018.
  * [21] N. K. Ure, G. Chowdhary, J. P. How, M. A. Vavrina, and J. Vian, “Health aware planning under uncertainty for uav missions with heterogeneous teams,” in _2013 European Control Conference (ECC)_. IEEE, 2013, pp. 3312–3319.
  * [22] V. Verma, G. Gordon, R. Simmons, and S. Thrun, “Real-Time Fault Diagnosis: Tractable Particle Filters for Robot Fault Diagnosis,” _IEEE Robotics & Automation Magazine_, vol. 11, no. 2, pp. 56–66, 2004.
  * [23] M. L. Visinsky, J. R. Cavallaro, and I. D. Walker, “Robotic fault detection and fault tolerance: A survey,” _Reliability Engineering & System Safety_, vol. 46, no. 2, pp. 139–158, 1994.
  * [24] I. D. Walker, “Robot fault detection: At the analog-digital boundary,” in _2022 IEEE Annual Reliability and Maintainability Symposium (RAMS)_ , 2022, pp. 1–6.
  * [25] B. Xiao, L. Cao, S. Xu, and L. Liu, “Robust Tracking Control of Robot Manipulators With Actuator Faults and Joint Velocity Measurement Uncertainty,” _IEEE/ASME Transactions on Mechatronics_ , vol. 25, no. 3, pp. 1354–1365, 2020.
  * [26] S. Yoo, “Actuator fault detection and adaptive accommodation control of flexible-joint robots,” _IET control theory & applications_, vol. 6, no. 10, pp. 1497–1507, 2012.
  * [27] T. Zhang, W. Zhang, and M. M. Gupta, “Resilient Robots: Concept, Review, and Future Directions,” _Robotics—Special Issue: Robust and Resilient Robots_ , vol. 6, no. 4, 2017.


and optimal control problems has been done in [40, Section 5.8]. It turns out
that statistically both formulations have similar performance, but on specific
examples one can outperform the other. In addition, the Heaviside step
reformulation provides more modeling flexibility.

##  7 Numerical examples

In this section, we consider two numerical examples. First, we consider a
numerical simulation example of a gene regulatory network and empirically
demonstrate that the FESD method has higher order integration accuracy.
Second, we consider an optimal control example of a planar two-link monoped
that must reach a certain goal in the horizontal direction. We compare the
Heaviside step reformulation with the Stewart reformulation in terms of total
computational time and cost per iteration.

###  7.1 Gene regulatory networks

In the simulation experiment we consider a gene regulatory network model,
which is described by a DI of the form (14). The model is not a Filippov
system, but a more general DI. More numerical examples with FESD for the step
reformulation can be found in [32]. In this reference, we have generated
integration order plots for a Filippov system and confirmed empirically the
results of Theorem 16.

We regard the IRMA example, a synthetic network composed of five genes,
originally proposed in [14]. This example is inspired by [5], which includes
more examples with Heaviside step functions that are implemented in nosnoc
[2].

##### IRMA model

In Figure 5, we reproduce the state trajectories from [5, Fig. 11].
Additionally, the vertical lines show the switching times of the selection
variables $\alpha_{i}$. The states of this system are the protein
concentrations of Gal4, Swi5, Ash1, Cbf1, and Gal80, which are denoted by
$x_{1},\dots,x_{5}$. There are seven switching functions, which are defined by
the states crossing certain thresholds, which are plotted as horizontal lines
in Figure 5. Specifically, the switching functions are

$\displaystyle\psi(x)=(x_{1}-0.01,x_{2}-0.01,x_{2}-0.06,x_{2}-0.08,x_{3}-0.035,x_{4}-0.04,x_{5}-0.01).$

Note that Swi5 ($x_{2}$) has three threshold values. The continuous-time
dynamics of the system are given by

$\displaystyle\dot{x}_{1}$ $\displaystyle\in-
p_{1}x_{1}+\kappa_{1}^{1}+\kappa_{1}^{2}\gamma(\psi_{6}(x)),$ (50a)
$\displaystyle\dot{x}_{2}$ $\displaystyle\in-
p_{2}x_{2}+\kappa_{2}^{1}+\kappa_{2}^{2}\gamma(\psi_{1}(x))(1-u)\gamma(\psi_{7}(x)),$
(50b) $\displaystyle\dot{x}_{3}$ $\displaystyle\in-
p_{3}x_{3}+\kappa_{3}^{1}+\kappa_{3}^{2}\gamma(\psi_{3}(x)),$ (50c)
$\displaystyle\dot{x}_{4}$ $\displaystyle\in-
p_{4}x_{4}+\kappa_{4}^{1}+\kappa_{4}^{2}\gamma(\psi_{2}(x))(1-\gamma(\psi_{5}(x))),$
(50d) $\displaystyle\dot{x}_{5}$ $\displaystyle\in-
p_{5}x_{5}+\kappa_{5}^{1}+\kappa_{5}^{2}\gamma(\psi_{4}(x)).$ (50e)

Figure 5: State trajectory of the IRMA example with red dotted vertical lines
indicating the switches.

Note that $u\in\\{0,1\\}$ is an external input, which is set to $u=1$ in the
scenario considered here. The initial state is given by
$x_{0}=(0.011,0.09,0.04,0.05,0.015)$. The parameter values are given as

$\displaystyle p$ $\displaystyle=(0.05,0.04,0.05,0.02,0.6),$
$\displaystyle\kappa^{1}$ $\displaystyle=(1.1\cdot 10^{-4},3\cdot
10^{-4},6\cdot 10^{-4},5\cdot 10^{-4},7.5\cdot 10^{-4})$
$\displaystyle\kappa^{2}$ $\displaystyle=(9\cdot
10^{-4},0.15,0.018,0.03,0.015).$

##### Integration order experiment

Figure 6: Accuracy vs. step size: Simulation of first 100 minutes of the
trajectory in Figure 5 with different RK schemes and step sizes.

To showcase that the FESD step reformulation preserves the integration order
of the underlying Runge-Kutta method, we simulate the first 100 minutes of the
trajectory depicted in Figure 5. This time interval contains exactly two
switches.We use $N_{\mathrm{FE}}=3$, to capture those switches even with a
single FESD step. We plot the integration error over the average step size
$\bar{h}={T_{\mathrm{sim}}}/{{N_{\mathrm{sim}}}N_{\mathrm{FE}}}$, where
${N_{\mathrm{sim}}}$ is the total number of simulation steps. The results are
summarized in Figure 6 for FESD with underlying Radau IIA of orders 1, 3, 5,
and 7, and Gauss-Legendre methods of orders 2, 4, 6, and 8. It can be seen
that the FESD step formulation preserves the integration order of the
underlying Runge-Kutta method, while with the standard time-stepping approach,
using methods that typically have higher-order integration accuracy degrade to
order one without FESD. This experiment shows, that if a feasible solution is
found the integration order of the underlying Runge-Kutta method is preserved.
The implementation of the example is publicly
available111https://github.com/FreyJo/nosnoc_py/blob/main/examples/Acary2014/irma_integration_order_experiment.py.

###  7.2 Optimal control example with state jumps

Now we further investigate the use of FESD for the Heaviside step
reformulation by applying it to an optimal control problem. We regard the
problem of synthesizing dynamic motions of the two-link Capler robot with
state jumps and friction [15]. Systems with state jumps do not directly fit
the form of ODEs with set-valued Heaviside step functions (14). However, using
the time-freezing reformulation we can transform the system with state jumps
into a PSS of the form of (8) [37] and use the methods developed in this
paper, see also Remark 2. The monoped’s configuration is described by four
degrees of freedom $q=(q_{x},q_{z},\phi_{\mathrm{knee}},\phi_{\mathrm{hip}})$,
where $(q_{x},q_{z})$ are the coordinates of the monoped’s base at the hip and
$\phi_{\mathrm{knee}}$, $\phi_{\mathrm{hip}}$ are the angles of the hip and
knee. The robot is actuated by two direct-drive motors at the hip and knee
joints. The control variables are the torques of these motors
$u(t)=(u_{\mathrm{knee}}(t),u_{\mathrm{hip}}(t))$. Denote by
$p_{\mathrm{foot}}(q)=(p_{\mathrm{foot},x}(q),p_{\mathrm{foot},z}(q))$ and
$p_{\mathrm{knee}}(q)=(p_{\mathrm{knee},x}(q),p_{\mathrm{knee},z}(q))$ the
kinematic position of the robot’s foot and knee, respectively. We model a
single contact point, the tip of the robot’s foot touching the ground, which
is expressed via the unilateral constraint

$f_{c}(q)=p_{\mathrm{foot},z}(q)\geq 0.$

Moreover, we denote the expression summarizing the Coriolis, control, and
gravitational forces by $f_{v}(q,u)$, the inertia matrix by $M(q)$, the normal
contact Jacobian by $J_{\mathrm{n}}(q)\in{\mathbb{R}}^{n_{q}}$, and the
contact tangent by $J_{\mathrm{t}}(q)\in{\mathbb{R}}^{n_{q}}$. The detailed
derivation of all mentioned functions, i.e., the model equations, kinematic
expressions, and all parameters for the robot can be found in [23, Appendix
A].

After the time-freezing reformulation, the PSS state consists of
$x(\tau)=(q(\tau),v(\tau),t(\tau))\in{\mathbb{R}}^{9}$, with $q(\tau)$ being
the position, $v(\tau)$ the velocity, $t(\tau)$ is a clock state needed for
the time-freezing reformulation and $\tau$ is the time of the ODE, cf. [37].
For the time-freezing PSS, we have in total three switching functions: the gap
function, as well as the normal and tangential contact velocities [37]:

$\displaystyle\psi(x)=(f_{c}(q),J_{\mathrm{n}}(q)^{\top}v,J_{\mathrm{t}}(q)^{\top}v).$

This allows a definition of eight base sets via the sign matrix (cf.
Definition 1):

$\displaystyle{S}$ $\displaystyle=\begin{bmatrix}1&1&1\\\ 1&1&-1\\\ 1&-1&1\\\
1&-1&-1\\\ -1&1&1\\\ -1&1&-1\\\ -1&-1&1\\\ -1&-1&-1\\\ \end{bmatrix}$

However, the time-freezing PSS has only three regions, where the first one
consists of the union of the first six base sets, and the other two match the
two remaining base sets, i.e.:

$\displaystyle R_{1}$
$\displaystyle=\cup_{i=1}^{6}\tilde{R}_{i}=\\{x\in{\mathbb{R}}^{n_{x}}\mid
f_{c}(q)>0\\}\cup\\{x\in{\mathbb{R}}^{n_{x}}\mid
f_{c}(q)<0,J_{\mathrm{n}}(q)^{\top}v>0\\},$ $\displaystyle R_{2}$
$\displaystyle=\tilde{R}_{7}=\\{x\in{\mathbb{R}}^{n_{x}}\mid
f_{c}(q)<0,J_{\mathrm{n}}(q)^{\top}v<0,J_{\mathrm{t}}(q)^{\top}v>0\\},$
$\displaystyle R_{3}$
$\displaystyle=\tilde{R}_{8}=\\{x\in{\mathbb{R}}^{n_{x}}\mid
f_{c}(q)<0,J_{\mathrm{n}}(q)^{\top}v<0,J_{\mathrm{t}}(q)^{\top}v<0\\}.$

In region $R_{1}$, we define the unconstrained (free flight) dynamics of the
monoped, and in regions, $R_{2}$ and $R_{3}$, auxiliary ODEs that mimic state
jumps in normal and tangential directions due to frictional impacts:

$\displaystyle f_{1}(x,u)=(q,M(q)^{-1}f_{v}(q,u),1),$ $\displaystyle
f_{2}(x)=(\mathbf{0}_{4,1},M(q)^{-1}(J_{\mathrm{n}}(q)-J_{\mathrm{t}}(q)\mu)a_{\mathrm{n}},0),$
$\displaystyle
f_{3}(x)=(\mathbf{0}_{4,1},M(q)^{-1}(J_{\mathrm{n}}(q)+J_{\mathrm{t}}(q)\mu)a_{\mathrm{n}},0).$

Figure 7: Illustration of several frames of the solution of the discretized
OCP. Figure 8: The monoped’s vertical and horizontal positions (top plots),
and velocities (bottom plots).

Here, $a_{\mathrm{n}}=200$ is the auxiliary ODE’s constant [37] and the
coefficient of friction is $\mu=0.8$. Observe that the clock state dynamics
are $\frac{{\mathrm{d}}t}{{\mathrm{d}}\tau}=1$ in $R_{1}$, and
$\frac{{\mathrm{d}}t}{{\mathrm{d}}\tau}=0$ in $R_{2}$ and $R_{3}$. A solution
trajectory of a PSS is continuous in time. By taking the pieces of the
trajectory where $\frac{{\mathrm{d}}t}{{\mathrm{d}}\tau}>0$, we recover the
solution of the original system with state jumps [37].

It follows from the discussion in Section 6, that with the Heaviside step
reformulation, we have $\theta\in{\mathbb{R}}^{3}$. Since we have
$n_{\psi}=3$, the step DCS (22) in this example has three complementarity
constraints and six equality constraints. For Stewart’s reformulation, we
cannot exploit the union of basis sets but must define eight regions, which
are equal to the basis sets. The first six regions are equipped with
$f_{1}(x,u)$ and the remaining two with $f_{2}(x)$ and $f_{3}(x)$
respectively. In this case, we have $\theta\in{\mathbb{R}}^{8}$, eight
complementarity constraints and nine equality constraints in the DCS (28).
Using the Heaviside step reformulation for this problem allows us to reduce
the number of regions we need to define from eight to only three.

Figure 9: The optimal controls for the example of $N=60$ control stages.

To compare the performances of the two reformulations, we run an experiment in
which the robot reaches a target position $q_{\mathrm{target}}=(3,0.4,0,0)$
with zero velocity $v_{\mathrm{target}}=\mathbf{0}_{4,1}$ in $T=3.0$ seconds.
The initial state is $x(0)=(0,0.4,0,0,0,0,0,0,0)$. Given a reference
$x^{\mathrm{ref}}(t)$, which is a spline interpolation between the initial and
final position, and includes two jumps, we define the least-squares objective
with the running and terminal costs

$\displaystyle L(x(\tau),u(\tau))$
$\displaystyle=(x(\tau)-x^{\mathrm{ref}}(\tau))^{\top}Q(x(\tau)-x^{\mathrm{ref}}(\tau))+\rho_{u}u(\tau)^{\top}u(\tau),$
$\displaystyle R(x(T))$
$\displaystyle=(x(T)-x^{\mathrm{ref}}(T)^{\top}Q_{T}(x(T)-x^{\mathrm{ref}}(T)),$

where the weight matrices are
$Q=\mathrm{diag}(1,1,10,1,10^{-6},10^{-6},10^{-6},10^{-6},0)$,
$\rho_{u}=0.01$, and
$Q_{T}=\mathrm{diag}(10^{3},10^{3},10^{3},10^{3},10,10,10,10,0)$. We define
the state and control bound constraints:

$\displaystyle x_{\mathrm{lb}}\leq x(\tau)\leq x_{\mathrm{ub}},$
$\displaystyle u_{\mathrm{lb}}\leq u(\tau)\leq u_{\mathrm{ub}},$

where $x_{\mathrm{ub}}=(3.5,10,\pi,\pi,100,100,100,100,\infty)$,
$x_{\mathrm{lb}}=(-0.5,0,-\pi,-\pi,-100,-100,-100,-100,-\infty)$,
$u_{\mathrm{ub}}=(100,100)$, and $u_{\mathrm{lb}}=-u_{\mathrm{ub}}$.

Collecting all the above, we can define an OCP of the form of (44), which we
discretized with the FESD Radau IIA scheme of order 3 (${n_{\mathrm{s}}}=2$),
with $N_{\mathrm{FE}}=3$ finite elements on every control interval. This OCP
is discretized and solved with nosnoc in a homotopy loop with IPOPT [51].
Figure 7 illustrates several frames of an example solution ($N=60$). Figure 8
shows the relevant vertical and horizontal positions and velocities, and
Figure 9 shows the optimal controls. From these plots we can see that the
robots make several small jumps, mostly landing with low vertical velocities
to reduce energy dissipation, and successfully reaches the target position
with zero terminal velocity.

Next, we solve this OCP for different values $N$ (number of control intervals)
from 30 to 90 in increments of 10 and compare FESD for the Heaviside step
reformulation (this paper) to FESD derived for Stewart’s reformulation [38].
We compare the CPU time per NLP iteration and total CPU time for both
approaches in Figure 10. As expected, due to the smaller number of variables
and constraints, the Heaviside step reformulation leads to faster NLP
iterations than the Stewart reformulation. In our experiments, we try
different linear solvers from the HSL library [30]. The linear solver choice
has a particularly strong influence on performance for both reformulations.
After evaluation, we select the best linear solver for each one, which is ma27
for Stewart reformulation and ma97 for the Heaviside step reformulation. We
can see the linear algebra costs increase significantly, for $N>60$, as the
linear solvers need more time to factorize larger systems.

Figure 10: The total CPU time of the homotopy loop for different $N$ (left
plot) and the CPU time per NLP solver iteration (right plot).

The total computation time is influenced by several factors: homotopy
algorithm, initialization, NLP solver performance, linear algebra solver,
etc., and as such shows a less clear trend. In this example, Stewart’s
reformulation achieves a consistently lower computation times despite a higher
cost per iteration. On the other hand, in our experiments in [32], we used a
similar setting and the Heaviside step reformulation led to lower total
computational times. We can conclude that which reformulation is the better
depends strongly on the example and the chosen optimization algorithm
parameters. Further comparisons of the two approaches can be found in [40,
Section 5.2].

##  8 Summary

This paper extends the Finite Elements with Switch Detection (FEDS) method to
nonsmooth dynamical systems with set-valued Heaviside step functions. The step
functions allow to encode logical relations within a dynamical system. These
systems cam be equivalent to Filippov systems, and this paper focuses on this
case. The set-valuedness of the Heaviside step functions leads to a
differential inclusion. However, by using the optimality condition of a
parametric linear program whose solution map corresponds to the step function,
one obtains an equivalent Dynamic Complementarity System (DCS). Now the
nonsmoothness and combinatorial structure is expressed algebraically and is
thus suitable for numerical computations. In the derivation of FESD, we
exploit the continuity of the Lagrange multipliers in the DCS, which in turns
enables the accurate detection the nonsmooth transition in time. This is also
necessary for the correct computation of sensitivities of the discretized
nonsmooth system. We show how to apply the FESD method to discretize optimal
control problems subject to nonsmooth systems with set-valued step functions
and provide some convergence results of the method in simulation problems.
Compared to Stewart’s reformulation used in [38], the reformulation considered
here allows for a more compact reformulation of the same system, but with a
smaller number of algebraic variables. The comparison on an optimal control
example shows that the new approach has a lower cost per iteration. In
summary, we extend the FESD method to a problem class that allows more
modeling flexibility than in our initial study in [38]. Our claims are
verified by numerical examples. An implementation of the new method is
provided in the open-source software package nosnoc [35].

## References

  * [1] NOSNOC.  https://github.com/nurkanovic/nosnoc, 2022.
  * [2] nosnoc_py.  https://github.com/FreyJo/nosnoc_py, 2022.
  * [3] Vincent Acary, Olivier Bonnefon, and Bernard Brogliato.  Nonsmooth modeling and simulation for switched circuits, volume 69.  Springer Science & Business Media, 2010.
  * [4] Vincent Acary and Bernard Brogliato.  Numerical methods for nonsmooth dynamical systems: applications in mechanics and electronics.  Springer Science & Business Media, 2008.
  * [5] Vincent Acary, Hidde De Jong, and Bernard Brogliato.  Numerical simulation of piecewise-linear models of gene regulatory networks using complementarity systems.  Physica D: Nonlinear Phenomena, 269:103–119, 2014.
  * [6] J. Albersmeyer and M. Diehl.  The lifted Newton method and its application in optimization.  SIAM Journal on Optimization, 20(3):1655–1684, 2010.
  * [7] 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, 11(1):1–36, 2019.
  * [8] Mihai Anitescu, Paul Tseng, and Stephen J Wright.  Elastic-mode algorithms for mathematical programs with equilibrium constraints: global convergence and stationarity properties.  Mathematical programming, 110(2):337–371, 2007.
  * [9] Brian T Baumrucker and Lorenz T Biegler.  Mpec strategies for optimization of a class of hybrid dynamic systems.  Journal of Process Control, 19(8):1248–1256, 2009.
  * [10] A. Bemporad and M. Morari.  Control of systems integrating logic, dynamics, and constraints.  Automatica, 35(3):407–427, 1999.
  * [11] Mario Bernardo, Chris Budd, Alan Richard Champneys, and Piotr Kowalczyk.  Piecewise-smooth dynamical systems: theory and applications, volume 163.  Springer Science & Business Media, 2008.
  * [12] Anas Bouali.  Hybrid optimal control: optimality conditions and applications.  PhD thesis, Avignon Université, 2023.
  * [13] Bernard Brogliato and Aneel Tanwani.  Dynamical systems coupled with monotone set-valued operators: Formalisms, applications, well-posedness, and stability.  SIAM Review, 62(1):3–129, 2020.
  * [14] Irene Cantone, Lucia Marucci, Francesco Iorio, Maria Aurelia Ricci, Vincenzo Belcastro, Mukesh Bansal, Stefania Santini, Mario di Bernardo, Diego di Bernardo, and Maria Pia Cosma.  A yeast synthetic network for in vivo assessment of reverse-engineering and modeling approaches.  Cell, 137(1):172–181, 2009.
  * [15] Jan Carius, René Ranftl, Vladlen Koltun, and Marco Hutter.  Trajectory optimization with implicit hard contacts.  IEEE Robotics and Automation Letters, 3(4):3316–3323, 2018.
  * [16] Jorge Cortes.  Discontinuous dynamical systems.  IEEE Control systems magazine, 28(3):36–73, 2008.
  * [17] Luca Dieci and Luciano Lopez.  Sliding motion in filippov differential systems: theoretical results and a computational approach.  SIAM Journal on Numerical Analysis, 47(3):2023–2051, 2009.
  * [18] Luca Dieci and Luciano Lopez.  Sliding motion on discontinuity surfaces of high co-dimension. a construction for selecting a filippov vector field.  Numerische Mathematik, 117(4):779–811, 2011.
  * [19] Asen L Dontchev and R Tyrrell Rockafellar.  Implicit Functions and Solution Mappings: A View from Variational Analysis.  Springer, 2014.
  * [20] F. Facchinei and J.-S. Pang.  Finite-dimensional variational inequalities and complementarity problems, volume 1-2.  Springer-Verlag, 2003.
  * [21] A.F. Filippov.  Differential Equations with discontinuous right hand side.  AMS Transl., 42:199–231, 1964.
  * [22] Alexei F. Filippov.  Differential Equations with Discontinuous Righthand Sides: Control Systems, volume 18.  Springer Science & Business Media, 1988.
  * [23] Christian Gehring.  Operational space control of single legged hopping.  Master’s thesis, Eidgenössische Technische Hochschule Zürich, Autonomous Systems Lab, 2011.
  * [24] Nicola Guglielmi and Ernst Hairer.  An efficient algorithm for solving piecewise-smooth dynamical systems.  Numerical Algorithms, 89(3):1311–1334, 2022.
  * [25] Lei Guo and Jane J Ye.  Necessary optimality conditions for optimal control problems with equilibrium constraints.  SIAM Journal on Control and Optimization, 54(5):2710–2733, 2016\.
  * [26] E. Hairer, S.P. Nørsett, and G. Wanner.  Solving Ordinary Differential Equations I.  Springer Series in Computational Mathematics. Springer, Berlin, 2nd edition, 1993.
  * [27] E. Hairer and G. Wanner.  Solving Ordinary Differential Equations II – Stiff and Differential-Algebraic Problems.  Springer, Berlin Heidelberg, 2nd edition, 1991.
  * [28] Mathew Halm.  Addressing Stiffness-Induced Challenges in Modeling and Identification for Rigid-Body Systems With Friction and Impacts.  PhD thesis, University of Pennsylvania, 2023.
  * [29] Mathew Halm and Michael Posa.  Set-valued rigid-body dynamics for simultaneous, inelastic, frictional impacts.  Under Review, 2023.
  * [30] HSL.  A collection of Fortran codes for large scale scientific computation.  http://www.hsl.rl.ac.uk, 2011.
  * [31] Anna Machina and Arcady Ponosov.  Filippov solutions in the analysis of piecewise linear models describing gene regulatory networks.  Nonlinear Analysis: Theory, Methods & Applications, 74(3):882–900, 2011.
  * [32] Armin Nurkanović, Sebastian Albrecht, Bernard Brogliato, and Moritz Diehl.  The time-freezing reformulation for numerical optimal control of complementarity lagrangian systems with state jumps.  Automatica, 158:111295, 2023.
  * [33] Armin Nurkanović, Sebastian Albrecht, and Moritz Diehl.  Limits of MPCC Formulations in Direct Optimal Control with Nonsmooth Differential Equations.  In 2020 European Control Conference (ECC), pages 2015–2020, 2020\.
  * [34] Armin Nurkanović and Moritz Diehl.  Continuous optimization for control of hybrid systems with hysteresis via time-freezing.  IEEE Control Systems Letters, 2022.
  * [35] Armin Nurkanović and Moritz Diehl.  NOSNOC: A software package for numerical optimal control of nonsmooth systems.  IEEE Control Systems Letters, 2022.
  * [36] Armin Nurkanović, Anton Pozharskiy, and Moritz Diehl.  Solving mathematical programs with complementarity constraints arising in nonsmooth optimal control.  Vietnam Journal of Mathematics (accepted for publication), 2024\.
  * [37] Armin Nurkanović, Tommaso Sartor, Sebastian Albrecht, and Moritz Diehl.  A Time-Freezing Approach for Numerical Optimal Control of Nonsmooth Differential Equations with State Jumps.  IEEE Control Systems Letters, 5(2):439–444, 2021.
  * [38] Armin Nurkanović, Mario Sperl, Sebastian Albrecht, and Moritz Diehl.  Finite Elements with Switch Detection for Direct Optimal Control of Nonsmooth Systems.  Numerische Mathematik (accepted for publication), 2024.
  * [39] Jong-Shi Pang and David E Stewart.  Differential variational inequalities.  Mathematical programming, 113(2):345–424, 2008.
  * [40] Anton Pozharskiy.  Evaluating methods for solving mathematical programs with complementarity constraints arising from nonsmooth optimal control.  Master’s thesis, Albert-Ludwigs-University Freiburg, 2023.
  * [41] Daniel Ralph and Stephen J. Wright.  Some properties of regularization and penalization schemes for mpecs.  Optimization Methods and Software, 19(5):527–556, 2004.
  * [42] J. B. Rawlings, D. Q. Mayne, and M. M. Diehl.  Model Predictive Control: Theory, Computation, and Design.  Nob Hill, 2nd edition, 2017.
  * [43] Stefan Scholtes.  Convergence properties of a regularization scheme for mathematical programs with complementarity constraints.  SIAM Journal on Optimization, 11(4):918–936, 2001.
  * [44] David Stewart.  A high accuracy method for solving odes with discontinuous right-hand side.  Numerische Mathematik, 58(1):299–328, 1990.
  * [45] David E Stewart.  A numerical method for friction problems with multiple contacts.  The ANZIAM Journal, 37(3):288–308, 1996.
  * [46] David E Stewart.  Existence of solutions to rigid body dynamics and the painlevé paradoxes.  Comptes Rendus de l’Académie des Sciences-Series I-Mathematics, 325(6):689–693, 1997.
  * [47] David E Stewart.  Uniqueness for index-one differential variational inequalities.  Nonlinear Analysis: Hybrid Systems, 2(3):812–818, 2008.
  * [48] David E Stewart.  Dynamics with Inequalities: impacts and hard constraints.  SIAM, 2011.
  * [49] David E Stewart and Mihai Anitescu.  Optimal control of systems with discontinuous differential equations.  Numerische Mathematik, 114(4):653–695, 2010.
  * [50] Wim Van Roy, Armin Nurkanović, Ramin Abbasi-Esfeden, Jonathan Frey, Anton Pozharskiy, Jan Swevers, and Moritz Diehl.  Continuous optimization for control of finite-state machines with cascaded hysteresis via time-freezing.  IEEE Conference on Decision and Control, 2023.
  * [51] Andreas Wächter and Lorenz T. Biegler.  On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming.  Mathematical Programming, 106(1):25–57, 2006.