# Set based velocity shaping for robotic manipulators

Ryan McGovern  School of Electronics, Electrical Engineering and Computer
Science, Queen’s University Belfast, Northern Ireland, UK. E-mail:
<EMAIL_ADDRESS>Nikolaos Athanasopoulos
School of Electronics, Electrical Engineering and Computer Science, Queen’s
University Belfast, Northern Ireland, UK. E-mail:
<EMAIL_ADDRESS>Seán McLoone  School of
Electronics, Electrical Engineering and Computer Science, Queen’s University
Belfast, Northern Ireland, UK. E-mail:
<EMAIL_ADDRESS>
###### Abstract

We develop a new framework for trajectory planning on predefined paths, for
general N-link manipulators. Different from previous approaches generating
open-loop minimum time controllers or pre-tuned motion profiles by time-
scaling, we establish analytic algorithms that recover all initial conditions
that can be driven to the desirable target set while adhering to environment
constraints. More technologically relevant, we characterise families of
corresponding safe state-feedback controllers with several desirable
properties. A key enabler in our framework is the introduction of a state
feedback template, that induces ordering properties between trajectories of
the resulting closed-loop system. The proposed structure allows working on the
nonlinear system directly in both the analysis and synthesis problems. Both
offline computations and online implementation are scalable with respect to
the number of links of the manipulator. The results can potentially be used in
a series of challenging problems: Numerical experiments on a commercial
robotic manipulator demonstrate that efficient online implementation is
possible.

††footnotetext: R.M. gratefuly acknowledges support from the UK DfE and N.A.
from EU 2020-1-UK01-KA203-079283 and EPSRC EP/T021942/1.

##  I Introduction

(a)

(b)

(c)

Figure 1: (a) Workspace representation of a path for a two DOF robot. (b)
Joint space representation of the two DOF manipulator. (c) State space
representation of the projeced path dynamics. The constraints curve is in
blue, and the target set is in red.

In recent years, there has been growing demand for safer robots, especially in
the context of Industry 4.0 and environments containing fragile agents such as
humans [1]. Robots face challenges in their integration with the workforce[2],
in particular related to safety, inclusion of temporal specifications, and
reliability [3], [4].

Planning of robotic manipulators has received much attention over the years
with a wide range of approaches available [5]. The challenge lies in computing
collision free paths in a nonconvex environment, and in a sufficiently fast
time [6].

Established approaches to producing safe controllers aim to place constraints
on speed, momentum and potential collision energies of the system in order to
ensure safety [7, 8, 9]. These controllers rely on the trajectory planner
found within the typical motion controller.

Trajectory planning, which is the focus of this article, is concerned with
shaping the velocity profile as the robot moves from one configuration to
another. Typically, the enforcement of hard constraints on the system dynamics
is imposed on the velocity profile, see, e.g., [10, 11, 12, 13], where an
optimal velocity profile is calculated. These optimal approaches have also
been extended to account for other state dependent and time dependent
constraints [14], [15].

Optimal control approaches, a version of which concerns time scaling
algorithms, produce a single optimal trajectory fed to a robot to execute the
desired motion. These algorithms have been successfully utilised in the last
decades, however, they inevitably come with shortcomings, for example: They
are discontinuous, they are restricted to kinodynamic planning problems that
specify a unique acceptable velocity for the end of the path, and offer little
ability to adapt the profile along the path, as most versions generate open-
loop controllers.

To address these shortcomings, we investigate a larger class of state-feedback
controllers, using the same reasoning for projecting the dynamics of the
robotic manipulator on a prespecified path as in the traditional
aforementioned approaches. However, instead of finding time-optimal solutions
for the zero initial condition, we characterise the whole set of admissible
states, namely, the distance traversed in the path and its pseudo velocity,
that can reach a target set in finite time and satisfy constraints throughout.

By projecting the dynamics on a path, any $N$-degree of freedom (DOF)
manipulator is represented by a constrained double integrator. The control
input corresponds to the pseudo acceleration across the path and is subject to
non linear and nonconvex constraints, induced mainly by the physical
constraints on the torques of the actuators. At this stage additional
constraints can be imposed, related, e.g., to potential collision energy, time
constraints, or constraints relating to end effector forces that could prove
useful in high precision applications, e.g., machining [9], [16].

Technically, rather than solving a single optimal control problem as, e.g., in
[10, 11], our main objective is to answer the question: Can we compute the
subset of states for which a state feedback controller exists driving the
system to a target set without violating any constraints in finite time?

Sets with these properties are called _reach-avoid sets_ [17] [18]. Many
issues related to robots and path planning are often naturally expressed in
the context of reach-avoid problems, examples include motion planning,
collision avoidance, space vehicle docking, and object tracking applications
[19, 20, 21, 22]. To the best of our knowledge, this is the first time it is
suggested to cast the kinodynamic planning problem as a reach-avoid set
computation.

###### Illustrative Example 1

Figures 1(a) \- 1(c) depict with purple color the same motion of a two link
manipulator moving from point A to point B in three different spaces. The end
effector work space is shown in Figure 1(a), the space of joints is shown in
Figure 1(b). In the general case the joint space can be $N$ dimensional,
depending upon the number of actuators on the robot. The state space shown in
Figure 1(c) is always two dimensional, having as states the pseudo distance of
the path travelled ($x_{1}$) and the end effector pseudo velocity ($x_{2}$).
We focus on designing state feedback controllers in this space (Figure 1(c)).
We can incorporate and visualise additional constraints based on the system
state (configuration and speed) with blue. The target set is depicted in red
in Figure 1(c).

Highly relevant for safety-critical systems, reach-avoid problems are not
trivial. Efficient solutions depend on the nature of the system, with several
techniques available for a variety of system types [23, 24, 25]. In our case,
we are exploiting the monotonicity of the projected trajectories with respect
to a novel parameterisation of the control input. This parameterisation allows
ordering relations between generated closed-loop trajectories to be
established in terms of both the initial conditions and the control input.
These observations constitute the building blocks of an algorithmic procedure
that recovers an approximation of the largest reach-avoid set. Compared to
interesting related approaches [26], [27], our work deals directly with the
nonlinear dynamics and constraints, and constructs the boundaries of the
reach-avoid set using a combination of extreme trajectories. Thus, there is no
added conservatism caused by the linearisation of dynamics.

We also establish set-based state feedback controllers, induced by the
parameterisation of the control strategy. Taking into account the preference
for continuous, smooth, and robust state feedback control reducing wear on
actuators, we establish general properties the control template must satisfy,
and show how simple, Lipschitz continuous, continuous, and sliding mode-like
controllers can be established. Last, we apply our framework to numerical
experiments on commercial robotic manipulators, and discuss practical
considerations in algorithmic implementation to show the efficacy of our
approach. Preliminary ideas on computing reach-avoid sets for the studied
problem are reported in [28]. In comparison, we formally show continuity
properties of the suggested control template, establish analytic algorithms on
computations of the reach-avoid set, propose new families of stabilizing
feedback controllers, highlight practical challenges in implementation, and
perform a formal comparison of our results with the conventional time optimal
control approach.

Section II discusses preliminaries. In Section III we introduce the problem
and the control input parameterisation, and establish the Lipshitz properties
of the closed-loop system dynamics. Section IV establishes ordering relations
between generated trajectories, while section V illustrates in detail the
algorithmic procedure for producing the reach-avoid set. Section VI deals with
the characterisation and implementation of families of safe state feedback
controllers. Section VII presents examples of the algorithms running on a
Python based simulation using the model for the Universal Robots UR5 [29]
available within the Matlab file exchange [30]. Conclusions are drawn in
Section VIII. For ease of exposition the proofs are placed in the Appendix.

##  II Preliminaries

Notation: For a vector $x\in\mathbb{R}^{N}$, $\|x\|$ is its 2-norm and $x_{i}$
its $i^{th}$ element. For a vector $x_{\text{label}}\in\mathbb{R}^{N}$ with a
label in its subscript, we write its $i^{th}$ element as
$(x_{\text{label}})_{i}$. We denote sets, e.g., $\mathcal{S},\mathcal{R}$,
with capital letters in italics. For a set $\mathcal{S}$,
$\text{conv}(\mathcal{S})$, $\operatorname{bd}(\mathcal{S})$ and
$\operatorname{int}{(\mathcal{S})}$ denote its convex hull, boundary and
interior respectively. The ball centered at zero with radius $\varepsilon$ is
denoted by $\mathbb{B}(\varepsilon)$. The distance of a point $x$ from a
compact set $\mathcal{S}$
$\text{dist}(x,\mathcal{S})=\min_{y\in\mathcal{S}}\|x-y\|$. Vector
inequalities hold elementwise.

The motion of $N$-link manipulators can be described using standard Lagrangian
dynamics by $M_{L}(q)\ddot{q}+C_{L}(q,\dot{q})\dot{q}+g_{L}(q)=\tau,$

where $q\in\mathbb{R}^{N}$ corresponds to joint positions and
$\tau\in\mathbb{R}^{N}$ collects the actuator torques for each joint. The
matrices $M_{L}(q)\in\mathbb{R}^{N\times N}$,
$C_{L}(q,\dot{q})\in\mathbb{R}^{N\times N}$ are the inertial and
centrifugal/Coriolis matrices respectively. The gravitational force effects
are represented by $g_{L}(q)$. The matrix and vector functions $M_{L}(q)$,
$C_{L}(q)$ and $g_{L}(q)$ are globally Lipschitz, [31, 32, 33].

The Lagrangian dynamics can be projected to path dynamics: The first step is
to consider an end effector path described by a function
$q(s):[0,1]\rightarrow\mathbb{R}^{n}$ known as a path parameterisation where
$s\in[0,1]$ is a scalar representing each configuration of the robot as it
moves along the path.

Details can be found in the seminal papers [10, 11] and the book [34, Chapter
9].

###### Assumption 1

The path parameterisation $q(s)$ is two times differentiable. Moreover,
$\left\|\frac{dq(s)}{ds}\right\|\geq\alpha_{0}$, for $s\in[0,1]$ and a
constant positive number $\alpha_{0}>0$.

Assumption 1 is not restrictive and is common in the literature [10, 11],
[34]. Intuitively, it requires that the configuration must be able to change
such that the end effector can traverse a path in finite time.

The parameterised system becomes $M(s)\ddot{s}+C(s)\dot{s}^{2}+g(s)=\tau$
where $M(s)\in\mathbb{R}^{N}$, $C(s)\in\mathbb{R}^{N}$,
$g(s)\in\mathbb{R}^{N}$, are the transformed vectors, and
$\tau\in\mathbb{R}^{N}$ describes the torques and forces produced by the
actuators. We define
$x=\begin{bmatrix}s&\dot{s}\end{bmatrix}^{T}=\begin{bmatrix}x_{1}&x_{2}\end{bmatrix}^{\top}$.
The torques are limited by the actuator dynamics as
$\tau^{\min}_{i}(x)\leq\tau_{i}(x)\leq\tau^{\max}_{i}(x)$ where
$\tau^{\min}_{i}(x)$ and $\tau^{\max}_{i}(x)$ describe the maximum torque that
can be generated in either direction. The $i^{th}$ element, $i=1,...,N$, of
the vectors within the parameterised system generates constraints of the form

$\tau^{\min}_{i}(x)\leq
M_{i}(x_{1})\dot{x}_{2}+C_{i}(x_{1})x_{2}^{2}+g_{i}(x_{1})\leq\tau^{\max}_{i}(x).$
(1)

###### Assumption 2

The state-dependent constraint bounds $\tau^{\min}_{i}(x)$ and
$\tau^{\max}_{i}(x)$ are locally Lipschitz in x for the whole range of
admissible motions, for all $i=1,...,N$.

Assumption 2 is not restrictive for electrical motors due to their continuous
relationships between torque and speed, see, e.g., [35] [36, Chapter 10].

Rearranging (1), the limits on possible accelerations $\dot{x}_{2}=\ddot{s}$
from any given state $x$ can be formed. We set
$A_{i}(x)=\frac{\tau^{\min}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$,
$D_{i}(x)=\frac{\tau^{\max}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$
for $M_{i}(x_{1})<0$, and
$A_{i}(x)=\frac{\tau^{\max}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$,
$D_{i}(x)=\frac{\tau^{\min}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$
for $M_{i}(x_{1})>0$. The case of zero inertia points, i.e., when
$M_{i}(x_{1})=0$, for one or more indices $i=1,..,N$ is taken into account by
posing algebraic constraints on $x$ [34] of the form $\tau^{\min}_{i}(x)\leq
C_{i}(x_{1})x_{2}^{2}+g_{i}(x_{1})\leq\tau^{\max}_{i}(x).$ By grouping the
above constraints, we can write

$D(x)\leq\ddot{s}\leq A(x)$ (2)

where

$\displaystyle A(x)$ $\displaystyle=\min_{\\{i\in[1,N]:M_{i}(x)\neq
0\\}}A_{i}(x),$ (3) $\displaystyle{D}(x)$
$\displaystyle=\max_{\\{i\in[1,N]:M_{i}(x)\neq 0\\}}{D_{i}(x)}.$ (4)

The projected system is a double integrator

$\dot{x}=\Phi x+Eu(x),$ (5)

with $\Phi=\begin{bmatrix}0&1\\\ 0&0\end{bmatrix}$, $E=\begin{bmatrix}0\\\
1\end{bmatrix}$. The state feedback
$u(\cdot):\mathbb{R}^{2}\rightarrow\mathbb{R}$ accounts for the acceleration
$\ddot{s}$, whose admissible range is state-dependent (2). The solution of (5)
at time $t$ starting from an initial condition $x_{0}$, under the control
function $u(x)$ is

$\phi_{\text{f}}(t;x_{0},u(x))=e^{\Phi
t}x_{0}+\int_{0}^{t}e^{\Phi(t-\tau)}Eu(x)d\tau.$ (6)

The state and input constraints are $x\in\mathcal{X}^{\prime},\ \
u(x)\in\mathcal{U}_{x},$ where
$\mathcal{X}^{\prime}=\mathcal{C}\cap\mathcal{C}_{0}$, with
$\mathcal{C}=\left\\{x\in\mathbb{R}^{2}:B(x)\leq 0\right\\}$,
$B(x)=\begin{bmatrix}-x_{1}&x_{1}-1&-x_{2}&D(x)-A(x)\end{bmatrix}^{\top}$.
Moreover, $\mathcal{C}_{0}=\\{x\in\mathbb{R}^{2}:(\exists
j\in[1,N]:M_{j}(x_{1})=0,\quad\tau^{min}_{j}-C_{j}(x)x_{2}^{2}-g_{j}(x)\leq
0,\quad-\tau^{max}_{j}+C_{j}(x)x_{2}^{2}+g_{j}(x)\leq 0)\\}$, and
$\mathcal{U}_{x}=\\{u(x)\in\mathbb{R}:D(x)\leq u(x)\leq A(x)\\}$. We note that
within our framework we can incorporate additional constraints. We assume we
can eventually describe all constraints via two functions
$C^{u}(x_{1}):[0,1]\to\mathbb{R}$ and $C^{l}(x_{1}):[0,1]\to\mathbb{R}$,
representing upper and lower bounds respectively. This leads to the
description of the constraint set

$\mathcal{X}=\left\\{x\in\mathbb{R}^{2}:C^{l}(x_{1})\leq x_{2}\leq
C^{u}(x_{1}),x_{1}\in[0,1]\right\\}.$ (7)

The overall state (we note $\mathcal{X}\subseteq\mathcal{X}^{\prime})$ and
input constraints are

$x\in\mathcal{X},\ \ u(x)\in\mathcal{U}_{x}.$ (8)

###### Assumption 3

The functions $x_{2}=C^{u}(x_{1})$ and $x_{2}=C^{l}(x_{1})=x_{2}$ are semi-
differentiable continuous bijections.

We note Assumption 3 is indeed somewhat restrictive, as, for example, it does
not allow islanding. Nevertheless, it covers many realistic and challenging
cases, including polynomial and trigonometric constraints.

##  III Control parameterisation and properties

In this section, we propose a parameterisation of the state feedback
controller as a convex combination of the state dependent input constraints.
The generally non-restrictive Assumptions 1, 2 allow us to characterize the
parameterisation with Lipschitz continuity, which constitutes our first main
technical result. Preliminary results and the proof of Proposition 1 can be
found in the Appendix.

###### Proposition 1

The functions $A(x)$, $D(x)$ are locally Lipschitz continuous in
$\mathcal{X}^{\prime}$.

In agreement with approaches in the literature [10], [34], we set
$u(x)=\dot{x_{2}}$ as the input variable. We build all subsequent results in
the following parameterisation of the control law via the _actuation level
function_ $\lambda(x):\mathcal{X}\rightarrow[0,1]$ as follows

$u(x,\lambda(x))=D(x)+\lambda(x)(A(x)-D(x)).$ (9)

The following result is a consequence of Proposition 1.

###### Corollary 1

Consider the system (5) constraints (8), and a Lipschitz continuous function
$\lambda(x):\mathcal{X}\rightarrow[0,1]$. Then (9) is locally Lipschitz
continuous in $\mathcal{X}$.

The forward and backward dynamics of the system are

$\displaystyle\dot{x}$ $\displaystyle=\Phi x+Eu(x,\lambda(x)),$ (10)
$\displaystyle\dot{x}$ $\displaystyle=-\Phi x-Eu(x,\lambda(x))$ (11)

respectively, with $\Phi$, $E$ defined in (5).

We consider target sets of the form

$\mathcal{X}_{\text{T}}=\\{x\in\mathcal{X}:x_{1}=c,\ \ x^{\ast}_{l}\leq
x_{2}\leq x^{\ast}_{u}\\},$ (12)

where $0<c\leq 1$ and $x^{\ast}_{u},x^{\ast}_{l}\in\mathbb{R}$. Given a
particular choice of the actuation level function $\lambda(x)$ (9) and an
initial condition $x_{0}\in\mathcal{X}$, the solution of (10) is
$\phi_{\text{f}}(t;x_{0},\lambda(x))=e^{\Phi
t}x_{0}+\int_{0}^{t}e^{\Phi(t-\tau)}Eu(x,\lambda(x))d\tau$ where $e^{\Phi
t}=\begin{bmatrix}1&t\\\ 0&1\end{bmatrix}$. Correspondingly, the solution of
the backwards dynamics (11) is $\phi_{\text{b}}(t;x_{0},\lambda(x)))=e^{-\Phi
t}x_{0}-\int_{0}^{t}e^{-\Phi(t-\tau)}Eu(x,\lambda(x))d\tau$.

###### Definition 1

A set $\mathcal{Y}\subseteq\mathcal{X}$ is a _reach-avoid set_ with respect to
the system (10), constraints (8) and a target set $\mathcal{X}_{\text{T}}$
(12) if any initial condition $x_{0}\in\mathcal{Y}$ can be transferred to
$\mathcal{X}_{T}$ in finite time under an admissible control law $u(x)$ (9),
i.e., there is $t^{\star}>0$ such that
$\phi_{\text{f}}(t^{*};x_{0},u(x))\in\mathcal{X}_{\text{T}}$, and
$\phi_{\text{f}}(t;x_{0},u(x))\in\mathcal{X}$ for all $t\in[0,t^{*}]$.

###### Definition 2

Consider the system (10), the constraint set $\mathcal{X}$ (7), a target set
$\mathcal{X}_{\text{T}}$ (12) and the controller $u(x)$ given by (9). The
_maximal reach-avoid set_ is

$\displaystyle\mathcal{R}(\mathcal{X}_{\text{T}})=$
$\displaystyle\\{x_{0}\in\mathcal{X}:(\exists
t^{*}>0:\phi_{f}(t^{*},x_{0},u(x))\in\mathcal{X}_{\text{T}},$
$\displaystyle\;\;\;\;\phi_{f}(t,x_{0},u(x))\in\mathcal{X},\forall
t\in[0,t^{*}])\\}.$ (13)

##  IV Ordering properties of closed-loop trajectories

We collect the admissible trajectories of the closed loop system in the
forward and backwards direction in the sets

$\mathcal{T}_{\text{f}}(x_{0},\lambda(x))=\\{x\in\mathcal{X}:(\exists t\geq
0:x=\phi_{\text{f}}(t;x_{0},\lambda(x)))\\},$ (14)
$\mathcal{T}_{\text{b}}(x_{0},\lambda(x))=\\{x\in\mathcal{X}:(\exists t\geq
0:x=\phi_{\text{b}}(t;x_{0},\lambda(x)))\\},$ (15)

respectively. In the remainder, and to discuss trajectories without needing to
specify the direction in which they are formed, we write
$\mathcal{T}(x_{0},\lambda(x))$. We define intervals
$\mathcal{I}=[(x_{a})_{1},(x_{b})_{1}]$ $(x_{a})_{1}\leq(x_{b})_{1}\leq 1$.
The _slice_ of a set $\mathcal{X}$ on an interval $\mathcal{I}$ is

$\mathcal{W}(\mathcal{X},\mathcal{I})=\mathcal{X}\cap\left\\{x:x_{1}\in\mathcal{I}\right\\}.$
(16)

The mapping (16) is valid also when $\mathcal{I}=\\{x_{a}\\}$ is a singleton.
In this case, we write for simplicity $\mathcal{W}(\mathcal{X},x_{a})$.

###### Lemma 1

Let $n\in[0,1]$. Consider the slice $\mathcal{W}(\mathcal{X},n)$, two states
$x_{u},x_{l}\in\mathcal{W}(\mathcal{X},n)$ such that
$(x_{u})_{2}>(x_{l})_{2}$, and a constant actuation level
$\lambda(x)=\lambda\in[0,1]$. Then,

$\mathcal{T}(x_{u},\lambda)\cap\mathcal{T}(x_{l},\lambda)=\emptyset.$

###### Lemma 2

Let $n\in[0,1]$. Consider two states
$x_{u},x_{l}\in\mathcal{W}(\mathcal{X},n)$ such that $x_{u}\neq x_{l}$.
Consider two fixed actuation levels $0\leq\lambda_{l}<\lambda_{u}\leq 1$.
Then, the trajectories can intersect at most once in
$\operatorname{int}{(\mathcal{X})}$, i.e., either
$\mathcal{T}(x_{u},\lambda_{u})\cap\mathcal{T}(x_{l},\lambda_{l})=\\{x^{\star}\\}$,
for some $x^{\star}\in\operatorname{int}{\mathcal{X}}$, or
$\mathcal{T}(x_{u},\lambda_{u})\cap\mathcal{T}(x_{l},\lambda_{l})=\emptyset$.

A straightforward consequence of Lemma 2 is that for any two trajectories
evolving from a single state with different constant actuation levels the one
with the higher actuation level will remain above the other for the duration
of the motion. Specifically, for any $x_{1}^{\star}\in[0,1]$, any
$\lambda_{b}\leq\lambda_{a}\leq 1$ it holds that $x_{b}\leq x_{a}$, where
$x_{a}=\mathcal{W}(\mathcal{X},x_{1}^{\ast})\cap\mathcal{T}_{f}(x_{0},\lambda_{a})$,
$x_{b}=\mathcal{W}(\mathcal{X},x_{1}^{\ast})\cap\mathcal{T}_{f}(x_{0},\lambda_{b})$.
This is summarised in the following corollary.

###### Corollary 2

Consider the system (10), the constraint set $\mathcal{X}$ (7), trajectories
(14) and (15) with constant actuation levels $\lambda_{a},\lambda_{b}$. The
following hold.

$\mathcal{W}(\mathcal{X},n)\cap\mathcal{T}_{f}(x_{0},\lambda_{a})\geq\mathcal{W}(\mathcal{X},n)\cap\mathcal{T}_{f}(x_{0},\lambda_{b}),$
$\mathcal{W}(\mathcal{X},n)\cap\mathcal{T}_{b}(x_{0},\lambda_{b})\geq\mathcal{W}(\mathcal{X},n)\cap\mathcal{T}_{b}(x_{0},\lambda_{a}),$

for any $0<\lambda_{b}\leq\lambda_{a}\leq 1$, for all $x_{0}\in\mathcal{X}$,
and $n\in[0,1]$.

Moreover, Lemma 2 implies that in the case where $(x_{u})_{2}>(x_{l})_{2}$, we
have $\mathcal{T}_{1}\cap\mathcal{T}_{2}=\emptyset$ for any two trajectories
$\mathcal{T}_{1}=\mathcal{T}(x_{u},\lambda_{u}),\mathcal{T}_{2}=\mathcal{T}(x_{l},\lambda_{l})$,
where $x_{u}$, $x_{l}$ lie on a slice $\mathcal{W}(\mathcal{X},x_{1})$ where
$x_{1}\in[0,1]$ such that $x_{u}\neq x_{l}$ and $\lambda_{u}>\lambda_{l}$.

###### Corollary 3

Consider the system (10), the constraint set $\mathcal{X}$ (7), trajectories
(14) with constant actuation levels $\lambda_{u},\lambda_{l}$ and $n\in[0,1]$.
Then, for any $x_{u},x_{l}$ such that $(x_{u})_{1}=(x_{l})_{1}$,
$(x_{u})_{2}>(x_{l})_{2}$, any $0\leq\lambda_{l}<\lambda_{u}\leq 1$ it holds

$\mathcal{W}(\mathcal{X},n)\cap\mathcal{T}_{f}(x_{u},\lambda_{u})>\mathcal{W}(\mathcal{X},n)\cap\mathcal{T}_{f}(x_{l},\lambda_{l}).$

##  V Approximation of the maximal Reach-avoid set

We first define the hypograph and epigraph of a set
$\mathcal{Z}\subseteq\mathcal{X}$ respectively

$\displaystyle\mathcal{R}_{u}(\mathcal{Z})$
$\displaystyle=\\{x\in\mathcal{X}:(\exists
y\in\mathcal{Z}:x_{1}=y_{1},x_{2}\leq y_{2}\\},$
$\displaystyle\mathcal{R}_{l}(\mathcal{Z})$
$\displaystyle=\\{x\in\mathcal{X}:(\exists
y\in\mathcal{Z}:x_{1}=y_{1},x_{2}\geq y_{2}\\}.$

We also define the set-valued operation $\mathcal{Q}(\cdot)$ that returns the
‘leftmost’ state of a trajectory $\mathcal{T}=\mathcal{T}(x_{0},\lambda(x))$
in $\mathcal{X}$

$\mathcal{Q}(\mathcal{T})=\\{x^{\ast}\in\mathcal{X}\cap\mathcal{T}:x_{1}^{\ast}\leq
x_{1},\forall x\in\mathcal{X}\cap\mathcal{T}\\}.$

The sets representing the upper and lower boundaries of the $\mathcal{X}$ (7)
are defined below

$\displaystyle\mathcal{V}^{u}$
$\displaystyle=\left\\{x\in\mathcal{X}:C^{u}(x_{1})=x_{2}\right\\},$ (17)
$\displaystyle\mathcal{V}^{l}$
$\displaystyle=\left\\{x\in\mathcal{X}:C^{l}(x_{1})=x_{2}\right\\}.$ (18)

Algorithm 1 Reach-avoid set computation

1:$\mathbf{Input:}$ $\mathcal{X}$, $\mathcal{X}_{\text{T}}$,
$\mathcal{V}^{l}$, $\mathcal{V}^{u}$, $\epsilon>0$

2:$\mathbf{Output:}$ $\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$

3:$\mathcal{T}_{u}^{*}\leftarrow\mathcal{T}_{\text{b}}(x^{*}_{u},0)$,

4:$\\{x_{d}\\}\leftarrow\mathcal{Q}(\mathcal{T}_{u}^{*})$

5:$\mathcal{T}_{l}^{*}\leftarrow\mathcal{T}_{\text{b}}(x^{*}_{l},1)$

6:$\\{x_{a}\\}\leftarrow\mathcal{Q}(\mathcal{T}_{l}^{*})$

7:if $x_{a}\in\mathcal{V}^{l}\textbf{ and }x_{d}\in\mathcal{V}^{l}$ then

8:
$\mathcal{Z}_{l}\leftarrow\mathcal{T}_{l}^{*}\cup\text{extend}(\mathcal{V}^{l},[(x_{d})_{1},(x_{a})_{1}],1,\epsilon)$

9: $\mathcal{Z}_{u}\leftarrow\mathcal{T}_{u}^{*}$

10:else if $x_{a}\in\mathcal{V}^{u}\textbf{ and }x_{d}\in\mathcal{V}^{u}$ then

11:
$\mathcal{Z}_{u}\leftarrow\mathcal{T}_{u}^{*}\cup\text{extend}(\mathcal{V}^{u},[(x_{d})_{1},(x_{a})_{1}],0,\epsilon)$

12: $\mathcal{Z}_{l}\leftarrow\mathcal{T}_{l}^{*}$

13:else

14: if $x_{a}\in\mathcal{V}^{l}$ then

15:
$\mathcal{Z}_{l}\leftarrow\mathcal{T}_{l}^{*}\cup\text{extend}(\mathcal{V}^{l},[0,(x_{a})_{1}],1,\epsilon)$

16: else

17: $Z_{l}\leftarrow\mathcal{T}_{l}^{*}$

18: if $x_{d}\in\mathcal{V}^{u}$ then

19:
$\mathcal{Z}_{u}\leftarrow\mathcal{T}_{u}^{*}\cup\text{extend}(\mathcal{V}^{u},[0,(x_{d})_{1}],0,\epsilon)$

20: else

21: $\mathcal{Z}_{u}\leftarrow\mathcal{T}_{u}^{*}$

22:return
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})\leftarrow\mathcal{R}_{l}(\mathcal{Z}_{u})\cap\mathcal{R}_{u}(\mathcal{Z}_{l})$

$x_{1}$$x_{2}$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{a}$
(a) Situation 1

$x_{1}$$x_{2}$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{a}$
(b) Situation 2

$x_{1}$$x_{2}$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{a}$
(c) Situation 3

$x_{1}$$x_{2}$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{a}$
(d) Situation 4

$x_{1}$$x_{2}$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{a}$
(e) Situation 5

$x_{1}$$x_{2}$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{d}$
(f) Situation 6

Figure 2: Illustration of the possible configurations that can result from
Lines 3-5 from Algorithm 1. Figure 3 details the area indicated in subfigure
(e).

The results of Section IV lay the foundation for the development of the reach
avoid set computation, outlined in Algrorithm 1. Initially, the two
trajectories $T_{l}^{*}$ and $T_{u}^{*}$, produced from backward integration
of the extreme points of $\mathcal{X}_{\text{T}}$, are computed in Lines 3 and
5 respectively. We denote with $x_{a}$ and $x_{d}$ the intersections of these
trajectories with the constraint set (Lines 4, 6), with $x^{*}_{u}$,
$x^{*}_{l}$ defining the interval $\mathcal{X}_{\text{T}}$ (12). Lines 7, l0
and 13 represent the different cases that can occur, depending on where
$x_{d}$ and $x_{a}$ lie. Figure 2 illustrates all possible situations: Line 7
covers Figure 2(a), where $x_{d},x_{a}$ are in $\mathcal{V}^{l}$. In this case
no additional computations are needed as
$\mathcal{Z}_{u}=\mathcal{T}^{*}_{u}$. On the other hand, the lower bound
needs to be extended to obtain $\mathcal{Z}_{l}$. Line 10 covers Figure 2(b).
In this case, the lower bound is complete, however, the upper bound possibly
needs to be modified. Lines 13-21 cover the remaining cases shown in Figures
2(c) \- 2(f), that occur when $\mathcal{T}^{*}_{u}$ and $\mathcal{T}^{*}_{l}$
do not intersect the same boundary $\mathcal{V}^{u}$ or $\mathcal{V}^{l}$. For
these instances, the extend operation is applied, which is described in detail
below in Algorithm 2. After all the relevant extensions have been made Line 22
returns $\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ which is constructed
as the intersection of the sets $\mathcal{R}_{l}(\mathcal{Z}_{u})$, and
$\mathcal{R}_{u}(\mathcal{Z}_{l})$.

We note that the target set $\mathcal{X}_{\text{T}}$ is the only _admissible
exit facet_ , i.e., the area of the boundary where the trajectory
$\mathcal{T}_{f}(x_{0},\lambda(x))$ will escape
$\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$ in finite time under appropriate
control $\lambda(x)$. Several existing works on _control-to-facet_ strategies
focus on non-linear hybrid systems with sets defined by simplices or other
simple polynomial descriptions of the set boundaries [37, 38, 39].

###  V-A Extension of bounds of
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$

The operation extend($\mathcal{V}$,
$[(x_{\text{end}})_{1},(x_{\text{start}})_{1}]$, $\lambda$, $\epsilon$) is
utilised in Algorithm 1 on Lines 8, 11, 15, and 19. We use $\mathcal{V}$ to
denote either the lower or the upper boundary, i.e.,
$\mathcal{V}\in\\{\mathcal{V}^{u},\mathcal{V}^{l}\\}$ and
$\mathcal{V}^{u},\mathcal{V}^{l}$ are defined by (17) (18). The operation
returns a curve that constrains any trajectory
$\mathcal{T}(x_{0},\lambda(x))$, with
$x_{0}\in\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$, when $\lambda(x)$ is
suitably chosen. The set $\mathcal{Z}$ is effectively formed as a union of
extreme trajectories and segments of $\mathcal{V}$ by applying the backward
dynamics (11). To describe the proposed procedure we define Bouligand’s
tangent cone, see, e.g., [40, p. 122].

###### Definition 3

The tangent cone of a vector $x\in\mathcal{X}$ to $\mathcal{X}$ (7) is

$\mathcal{B}(x)=\left\\{z:\liminf\limits_{\tau\to 0}\frac{\text{dist}(x+\tau
z,\mathcal{X})}{\tau}=0\right\\}.$ (19)

###### Definition 4

Consider a state $x\in\mathcal{V}$ and the system dynamics (10)
$\dot{x}=f(x,\lambda)=\begin{bmatrix}x_{2}&u(x,\lambda)\end{bmatrix}^{\top}$,
with $\lambda\in[0,1]$. We define the set

$\displaystyle\mathcal{F}(x)=\\{f(x,\lambda):\lambda\in[0,1]\\}.$ (20)

Utilising (19), (20), we can write the geometric condition that verifies
whether there exists an admissible input for a state $x\in\mathcal{X}$ that
allows it to remain in $\mathcal{X}$ as follows

$\mathcal{B}(x)\cap\mathcal{F}(x)\neq\emptyset.$ (21)

In our setting, the boundary of $\mathcal{X}$ is defined either on
$\mathcal{V}^{u}$ or $\mathcal{V}^{l}$. Let us consider the equations
$C^{u}(x_{1})$ (17) and $C^{l}(x_{1})$(18). The functions are semi-
differentiable. Taking into account dynamics (10), each vector $y$ in the set
$\mathcal{F}(x)$ has a nonnegative first element, i.e., $y_{1}=x_{2}\geq 0$.
Consequently, in the set (20) it is sufficient to consider only the _right-
derivative_ of $C^{u}(x_{1})$ and $C^{l}(x_{1})$ in order to verify (21). We
write the right derivative of the upper and lower constraint curves as

$\displaystyle m_{u}(y_{1})=\nabla^{+}(C^{u}(y_{1})),$ (22) $\displaystyle
m_{l}(y_{1})=\nabla^{+}(C^{l}(y_{1})).$ (23)

This is computationally convenient, allowing us to define a half-space
including the set of vectors from (19) that can possibly intersect (20). The
resultant half-spaces can be explicitly defined as

$\displaystyle\mathcal{B}^{u}(y)$
$\displaystyle=\\{x\in\mathbb{R}^{2}:x_{2}\leq(m_{u}(y_{1})x_{1}-y_{1})-y_{2}\\},$
(24) $\displaystyle\mathcal{B}^{l}(y)$
$\displaystyle=\\{x\in\mathbb{R}^{2}:x_{2}\geq(m_{l}(y_{1})x_{1}-y_{1})-y_{2}\\}.$
(25)

Thus, condition (21) can be verified by checking (26)

$\mathcal{B}(x)\cap\mathcal{F}(x)=\begin{cases}\mathcal{B}^{u}(x)\cap\mathcal{F}(x)\;\text{when}\;x\in\mathcal{V}^{u},\\\
\mathcal{B}^{l}(x)\cap\mathcal{F}(x)\;\text{when}\;x\in\mathcal{V}^{l}.\end{cases}$
(26)

We define the function $S(\cdot):\mathcal{V}\to\mathbb{R}$,

$S(x)=\begin{cases}\begin{bmatrix}-m_{u}(x)&1\end{bmatrix}^{\top}f(x,0),&\text{when}\;\;\;x\in\mathcal{V}^{u},\\\
\begin{bmatrix}m_{l}(x)&-1\end{bmatrix}^{\top}f(x,1),&\text{when}\;\;\;x\in\mathcal{V}^{l}.\end{cases}$
(27)

where $m_{u}(x)$ and $m_{l}(x)$ are given by (22) and (23), respectively.

###### Lemma 3

Consider the system (5) and the input and state constraints (8). Then, for any
$x\in\mathcal{V}^{u}$ (17), or any $x\in\mathcal{V}^{l}$ (18), it holds

$S(x)\leq 0\Leftrightarrow\mathcal{B}(x)\cap\mathcal{F}(x)\neq\emptyset.$

We consider a partition that will be later used to characterise the constraint
curve where (21) is satisfied.

###### Lemma 4

Consider the system (5), the constraints (8), and the boundaries
$\mathcal{V}=\left\\{\mathcal{V}^{u},\mathcal{V}^{l}\right\\}$ where
$\mathcal{V}^{u}$ and $\mathcal{V}^{l}$ are defined by (17) (18),
respectively. Consider the function $S(x)$ (27). Then, for each
$\mathcal{V}\in\\{\mathcal{V}^{u},\mathcal{V}^{l}\\}$, there is an integer
$M\geq 1$ and a finite set of mutually disjoint intervals
$\mathcal{I}_{in}=\\{\mathcal{I}_{in}(1),...,\mathcal{I}_{in}(M)\\}$,
$\cup_{i=1}^{M}\mathcal{I}_{in}\subseteq[0,1]$, where it holds $S(x)\leq 0$
for all $x\in\mathcal{I}_{in}$.

###### Definition 5

Consider the system (5), constraints (8) and an interval
$\begin{bmatrix}(x_{end})_{1},(x_{start})_{1}\end{bmatrix}$.

We partition the interval in an ordered set of intervals
$\mathcal{I}=\\{\mathcal{I}(1),...,\mathcal{I}(L)\\}$,
$\mathcal{I}(i)\subseteq[(x_{\text{end}})_{1},(x_{\text{start}})_{1}]$,
$i=1,..,L$, constructed from two sets, namely,
$\mathcal{I}_{\text{in}}=\\{\mathcal{I}_{\text{in}}(1),...,\mathcal{I}_{\text{in}}(M)\\}$
and
$\mathcal{I}_{\text{out}}=\\{\mathcal{I}_{\text{out}}(1),...,\mathcal{I}_{\text{out}}(P)\\}$.
Moreover, $|\mathcal{I}_{\text{in}}|+|\mathcal{I}_{\text{out}}|=M+P=L$, with
$\mathcal{I}_{\text{in}}(i)\neq\mathcal{I}_{\text{out}}(j)$, for all
$i=1,...,M,j=1,...,P$ and

$\displaystyle x\in(\mathcal{I}_{\text{in}})_{i}$
$\displaystyle\Rightarrow\mathcal{B}(x)\cap F(x,u(x))\neq\emptyset,$
$\displaystyle x\in(\mathcal{I}_{\text{out}})_{j}$
$\displaystyle\Rightarrow\mathcal{B}(x)\cap F(x,u(x))=\emptyset.$

We note any two intervals $\mathcal{I}_{i}$, $\mathcal{I}_{i+1}$ are adjacent
to each other and
$\mathcal{I}_{i}\cap\mathcal{I}_{i+1}=(x_{\text{end}})_{i}=(x_{\text{start}})_{i+1}$.
Moreover, for any $i=1,..,L-1$, if
$\mathcal{I}_{i}\in\mathcal{I}_{\text{in}}$, then
$\mathcal{I}_{i+1}\in\mathcal{I}_{\text{out}}$ and vice versa.

###### Illustrative Example 2

Figure 3 focuses on the part enclosed by the rectangle in Figure 2(e). It
illustrates a partition according to Definition 5, showing how the section in
$\mathcal{V}^{l}$ is extended. We represent $\mathcal{I}_{in}$ with
$\mathcal{I}_{out}$ with green and red dotted lines respectively. The cones
generated by 20, evaluated at two states
$x_{\text{C}},x_{\text{D}}\in\mathcal{V}^{l}$, are represented with purple
vectors. The halfspaces $\mathcal{B}^{u}({x_{\text{C}}})$,
$\mathcal{B}^{u}({x_{\text{D}}})$ 24, which in this case coincide with the
tangent cones $\mathcal{B}({x_{\text{C}}})$ and $\mathcal{B}({x_{\text{D}}})$
(19) are shown in grey. We observe that for any $x\in\mathcal{V}^{l}$ in an
interval $\mathcal{I}\in\mathcal{I}_{\text{in}}$, the system can avoid
crossing the boundary. On the other hand, for any $x\in\mathcal{V}^{l}$ in
$\mathcal{I}\in\mathcal{I}_{\text{out}}$, there is no control action
preventing crossing $\mathcal{V}^{l}$.

As highlighted in Example 2, the boundary $\mathcal{V}^{u}$ or
$\mathcal{V}^{l}$ often does not belong in its entirety to the reach avoid
set, thus, a modification is needed. The procedure, called extend, is
presented in Algorithm 2. Given the set of intervals for which condition
$S(x)\leq 0$ holds as in Lemma 4, Algorithm 2 either adds the whole interval
to the set $\mathcal{Z}$ or propagates the closed-loop dynamics backwards in
order to form a curve that will serve as the upper or lower bound of the
computed set in Algorithm 1.

Lines 5 and 17 of Algorithm 2 separate the cases where
$\mathcal{I}\in\mathcal{I}_{\text{in}}$ and
$\mathcal{I}\in\mathcal{I}_{\text{out}}$. When
$\mathcal{I}\in\mathcal{I}_{\text{in}}$ (Line 9), the entire interval is added
to the reach avoid set boundary. Lines 11-14 add a parts of the generated
trajectory and existing constraint curve defined by $\mathcal{V}$. Line 16
represents the case where only an extreme trajectory is added to the set
$\mathcal{Z}$. When $\mathcal{I}\in\mathcal{I}_{\text{in}}$ (Line 19) an
$\epsilon$ step change from the boundary is performed, leading to the
trajectory generation in Line 20. The following central result establishes
that Algorithm 1 returns a reach-avoid set, in finite time.

Algorithm 2 extend($\mathcal{V}$,
$[(x_{\text{end}})_{1},(x_{\text{start}})_{1}]$, $\lambda$, $\epsilon$)

1:$\mathbf{Input:}$ Set $\mathcal{V}\in\\{\mathcal{V}^{l},\mathcal{V}^{u}\\}$
(17), (18), start state $x_{\text{start}}$, end state $x_{\text{end}}$,
$\epsilon>0$, $\delta=(-1)^{\lambda+1}\epsilon$, partition $\mathcal{I}$ as in
Definition 5.

2:$\mathbf{Output:}$ $\mathcal{Z}$

3:$\mathcal{Z}\leftarrow\emptyset$, $i\leftarrow 1$,
$y\leftarrow\\{x\in\mathcal{V}:x_{1}=(x_{\text{start}})_{1}\\}$

4:while $i\leq L$ do

5: if $\mathcal{I}(i)\in\mathcal{I}_{\text{in}}$ then

6: if $y\in\mathcal{V}$ then

7:
$\mathcal{Z}\leftarrow\mathcal{Z}\cup\mathcal{W}(\mathcal{V},\mathcal{I}(i))$

8: else

9:
$\mathcal{T}_{\mathcal{I}}=\mathcal{W}(\mathcal{T}_{b}(y,\lambda),\mathcal{I}(i))$

10: if $\mathcal{T}_{\mathcal{I}}\cap\mathcal{V}\neq\emptyset$ then

11:
$x_{\text{int}}\leftarrow\mathcal{Q}(\mathcal{T}_{\mathcal{I}}\cap\mathcal{V})$

12:
$\mathcal{T}_{\text{int}}\leftarrow\mathcal{T}_{\mathcal{I}}\cap\\{x\in\mathcal{T}_{\mathcal{I}}:x_{1}\geq
x_{\text{int}}\\}$

13: $\mathcal{V}_{\text{int}}\leftarrow\\{x\in\mathcal{V}:x_{1}\leq
x_{\text{int}},x\in\mathcal{I}(i)\\}$

14:
$\mathcal{Z}\leftarrow\mathcal{Z}\cup\mathcal{T}_{\text{int}}\cup\mathcal{V}_{\text{int}}$

15: else

16: $\mathcal{Z}\leftarrow\mathcal{Z}\cup\mathcal{T}_{\mathcal{I}}$

17: else if $\mathcal{I}(i)\in\mathcal{I}_{\text{out}}$ then

18: if $y\in\mathcal{V}$ then

19: $y\leftarrow\begin{bmatrix}y_{1}&y_{2}+\delta\end{bmatrix}^{\top}$

20:
$\mathcal{T}_{\mathcal{I}}=\mathcal{T}_{b}(y,\lambda)\cap\\{x\in\mathbb{R}^{2}:(\exists
l\in\mathcal{I}(i):x_{1}=l_{1})\\}$

21: $\mathcal{Z}\leftarrow\mathcal{Z}\cup\mathcal{T}_{\mathcal{I}}$

22: $i\leftarrow i+1$

23: $y\leftarrow\mathcal{Q}(\mathcal{Z})$

24:return $\mathcal{Z}$

$x_{1}$$x_{2}$$\mathcal{B}^{u}(x_{\text{C}})$$\mathcal{B}^{u}(x_{\text{D}})$$\mathcal{V}^{u}$$\mathcal{V}^{l}$$\mathcal{X}_{\text{T}}$$x^{*}_{l}$$x^{*}_{u}$$T^{*}_{u}$$x_{d}$$T^{*}_{l}$$x_{a}$$\mathcal{I}(1)$$\mathcal{I}(2)$$\mathcal{I}(3)$$x_{\text{C}}$$\mathcal{F}({x}_{\text{C}})$$x_{\text{D}}$$\mathcal{F}({x_{D}})$
Figure 3: Illustration of a typical set of intervals obtained along the lower
velocity limit curve in the region to be extended from Figure 2(e). The red
regions $\mathcal{I}(1),\mathcal{I}(3)\in\mathcal{I}_{out}$ and the green
regions $\mathcal{I}(2)\in\mathcal{I}_{in}$. At both states $x_{\text{C}}$,
$x_{\text{D}}$, the extremes of $\mathcal{F}(\cdot)$ are represented by purple
vectors, and lower boundary of the halfspace $\mathcal{B}(\cdot)$ is shown as
a tangent to $\mathcal{V}^{l}$ at the respective states.

###### Theorem 1

Consider the system (10), constraints (8) and the target set
$\mathcal{X}_{\text{T}}$ (12). The following hold.

  * (i)

Algorithm 1 terminates in finite time.

  * (ii)

The set $\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$ is a reach-avoid set.

##  VI Controller design

We turn our attention to the control problem, namely, constructing safe state-
feedback mechanisms. First, we characterise the generic properties the
controller should have so that any state $x$ in
$\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$ can be transferred to the target set
$\mathcal{X}_{T}$ in finite time without violating state and input
constraints. Let us denote the upper and lower boundary of the reach avoid set
by $\mathcal{Z}_{u}$ and $\mathcal{Z}_{l}$ respectively.

###### Theorem 2

Consider the system (5) with state and input constraints $\mathcal{X}$,
$\mathcal{U}_{x}$ (8), and a target set $\mathcal{X}_{\text{T}}$. Consider a
reach avoid set $\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ and an
initial condition $x_{0}\in\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$,
and the control law $u(x,\lambda(x))$ (9). Then, the trajectory
$\mathcal{T}_{f}(x_{0},\lambda(x))$ lies within
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ and reaches
$\mathcal{X}_{\text{T}}$ in finite time if

$\lambda(x)=\begin{cases}1&\forall x\in\mathcal{Z}_{l},\\\ 0&\forall
x\in\mathcal{Z}_{u},\\\ c(x)&\forall
x\in\operatorname{int}{(\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}}))},\\\
\end{cases}$ (28)

and $\lambda(\cdot):\mathcal{X}\rightarrow[0,1]$ is a Lipschitz continuous
function.

The control parameterisation above is quite general. A possible realisation of
(28) is to consider two locally Lipschitz functions $g_{1}(x)$ and $g_{2}(x)$,
with $g_{1}(x_{1})>g_{2}(x_{1})$, $x\in\mathcal{X}$. The continuous actuation
level function is

$\lambda(x)=\begin{cases}0&\text{if $x_{2}\geq g_{1}(x_{1})$}\\\ 1&\text{if
$x_{2}\leq g_{2}(x_{1})$}\\\
\frac{x_{2}-g_{1}(x_{1})}{g_{2}(x_{1})-g_{1}(x_{1})}&\text{if
$g_{2}(x_{1})<x_{2}<g_{1}(x_{1})$}\end{cases}$ (29)

We note that if we use the upper and lower bounds of
$\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$ to define $g_{1}(x_{1})$ and
$g_{2}(x_{1})$ respectively, a valid controller can be defined as
$\lambda(x)=\frac{x_{2}-x_{2}^{u}}{x_{2}^{l}-x_{2}^{u}}$.

This is a convex linear mapping between the upper an lower bounds.

It is straightforward to see that this choice satisfies all conditions (28) of
Theorem 2, excluding the Lipschitz continuity of $\lambda(x)$. Indeed, due to
the formation of the bounds detailed in Algorithm 1, there may be
discontinuities if steps are used in the production of the boundaries. A
practical way to make these bounds continuous, and therefore utilise the
aforementioned simple controller, is to approximate the reach-avoid set bounds
by (piecewise) polynomial functions.

Additionally, our framework captures a family of _sliding mode-like
controllers_ [41].

To elaborate, a special case in (29) can be retrieved by setting
$g(x_{1})=g_{1}(x_{1})=g_{2}(x_{1})$, leading to the simplified actuation
level function

$\lambda(x)=\begin{cases}1&\text{if $x_{2}<g(x_{1})$}\\\ 0&\text{if $x_{2}\geq
g(x_{1})$}\end{cases}$ (30)

We note the resulting controller does not present a continuous control but
rather a bang bang style controller, that is prone to chattering [34].
However, it can be the basis for a sliding mode control strategy: We may
define the function
$H(x,\lambda)=|\begin{bmatrix}-m(x)&1\end{bmatrix}^{\top}f(x,\lambda)|$, where
$f(x,\lambda)$ represents the system dynamics as given in Definition 4 and
$m(x)=\frac{dg(x_{1})}{dx_{1}}$. By a suitable choice of $g(x_{1})$, if a
parameter $\lambda$ can be found such that $H(x,\lambda)=0$ for all $x$, the
controller meets all conditions for sliding mode operation by choosing

$\lambda(x)=\begin{cases}1&\text{if $x_{2}<g(x_{1})$},\\\ 0&\text{if
$x_{2}>g(x_{1})$},\\\
\underset{\lambda}{\operatorname{arg}\,\operatorname{min}}\;H(x,\lambda)&\text{if
$x_{2}=g(x_{1})$.}\end{cases}$ (31)

##  VII Numerical experiment

In this section we illustrate the established framework, in terms of reach-
avoid set computation and control implementation. Subsections VII.A, VII.B
deal with practical considerations in implementation, while subsection VII.C
provides the simulation results.

###  VII-A Digital implementation

We consider the issue of sampling in the implementation of the state feedback
and computation of trajectories forming the reach avoid set (Algorithms 1, 2).
The solution of system (5) piecewise constant inputs $u_{T}$ is analytic, that
is, for any $t\in[0,T]$ we have
$\phi_{\text{f}}(t;x_{0},u(x))=e^{At}x_{0}+u_{T}\int_{0}^{t}e^{A\tau}Bd\tau,$

or, $x(t)=\begin{bmatrix}x_{1}(t)\\\
x_{2}(t)\end{bmatrix}=\begin{bmatrix}\frac{t^{2}}{2}u_{T}+t(x_{0})_{2}+(x_{0})_{1}\\\
tu_{T}+(x_{0})_{2}\end{bmatrix}.$

Sampling time should be chosen to be small enough in order to approximate
sufficiently a continuous-time controller. Input $u_{T}$ can be enforced to be
admissible in $[0,t]$, since $u(x,\lambda(x))$ is by construction Lipschitz
continuous in $x$. To this purpose, for any two states
$x_{0},x(t)\in\mathcal{X}$ and considering the Lipschitz constants $L_{1}$,
$L_{2}$ so that $\|A(x_{0})-A(x(t))\|\leq L_{1}\|x_{0}-x(t)\|$ and
$\|D(x_{0})-D(x(t))\|\leq L_{2}\|x_{0}-x(t)\|$, the following inequalities
should hold

$D(x)+L_{2}\|x_{0}-x(t)\|\leq u_{T}\leq A(x_{0})-L_{1}\|x_{0}-x(t)\|.$ (32)

###  VII-B Approximation of Constraint Curves

The partition $\mathcal{I}$ in Lemma 4 requires finding the roots of the
function $S(x)$ (27). By continuity of $S(x)$, this can be done solving
conditions $S(x)=0$ and
$\left\|\frac{d(S(x))}{dx}\right\|=\left\|\lim\limits_{h\to 0}\frac{S(x+h[1\ \
m]^{\top})-S(x)}{h}\right\|\neq 0$,

with $m=m_{u}$ if $x$ is in $\mathcal{V}^{u}$ and $m=m_{l}$ if $x$ is in
$\mathcal{V}^{l}$. Solving above equations requires non-straightforward
application of numerical methods on nonlinear functions, while in practice
boundary values are evaluated at a finite number of states rather than
providing analytical expressions [42]. To tackle this challenge, we can
approximate $S(x)$ with a polynomial function $\tilde{S}(x_{1})$ so that

$\tilde{S}(x)\geq S(x)$ for all states $x$ belonging in $\mathcal{V}^{u}$ and
$\mathcal{V}^{l}$, by solving a constrained least squares problem.

To improve approximation accuracy, one can typically increase the order of the
approximating polynomial, or calculate piecewise polynomial approximations.

###  VII-C Results

We apply our framework to the two DOF planar manipulator setup illustrated in
Figure 1(a)-1(c), and the commercially available Universal robot UR5 [43]. The
UR5 simulation data are detaild below, where the two DOF manipulator setting
is in Appendix A. The UR5 manipulator consists of six joints and respective
actuators, for which
$\tau^{\text{max}}=\begin{bmatrix}150&150&150&28&28&28\end{bmatrix}$ and
$\tau^{\text{min}}=\begin{bmatrix}-150&-150&-150&-28&-28&-28\end{bmatrix}$. We
define a straight line path in the joint space as $q(s)=q(0)-s(q(0)-q(1))$
where
$q(0)=\begin{bmatrix}\frac{\pi}{2}&-\frac{\pi}{4}&\frac{\pi}{3}&\frac{2\pi}{3}&-\frac{\pi}{2}&-\frac{\pi}{3}\end{bmatrix}^{\top}$,
$q(1)=\begin{bmatrix}0&0&0&0&0&0\end{bmatrix}^{\top}$. The torque limitations
along with the path description allow the computation of the path dynamics and
$A(x)$, $D(x)$ (3), (4). We assume additional inequality constraints
translated in the state space $x_{2}\leq
4\sin(10x_{1}+5)-2\sin(18x_{1}^{3})+10$, $x_{2}\geq
4\sin(10x_{1}+5)-2\sin(18x_{1}^{3})-2$. We apply piecewise constant inputs
$u_{T}$, following the analysis Section VII.A. To compensate for the lack of
knowledge of the Lipschitz constant for the state feedback, we consider
modified maximum acceleration and deceleration bounds, so that the controller
is defined as
$u(x,\lambda(x))=\tilde{D}(x)-\lambda(x)(\tilde{A}(x)-\tilde{D}(x))$, where
$\tilde{A}(x)=A(x)-0.05(A(x)-D(x))$ and $\tilde{D}(x)=D(x)+0.05(A(x)-D(x))$.
The modified input equation creates a buffer of 5% the difference between the
limits to allow for the case where $A(x)$ may decrease or $D(x)$ may increase
over the sampling time, and is consistent with the effect the Lipschitz
constant has on the bounds (32).

The target set (12) is
$\mathcal{X}_{\text{T}}=\\{x\in\mathcal{X}:x_{1}=1,1\leq x_{2}\leq 5\\}$. The
shaded region in Figure 4 represents the reach avoid set calculated via
Algorithm 1, with the target set denoted in red. In this instance, we identify
two intervals (Definition 5) for the lower bound and 167 intervals for the
upper bound with the value of $\epsilon$ set to 0.1 for both boundaries.

$\mathcal{X}$ $\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ Figure 4:
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ is shown in shaded color. The
boundary of $\mathcal{X}$ is in blue, the target set $\mathcal{X}_{\text{T}}$
is in red.

To showcase a specific control trajectory, we set
$x_{\text{initial}}=\begin{bmatrix}x_{1}&x_{2}\end{bmatrix}=\begin{bmatrix}0&3\end{bmatrix}$.
We apply two different control strategies: The first concerns the _state
feedback_ controller described by the actuation level function in (29) with
$g_{1}(x_{1})=2.92x_{1}^{2}+-3.42x_{1}+5$,
$g_{2}(x_{1})=-4.78x_{1}^{2}+8.18x_{1}$. The second is the _open-loop time
optimal_ control [34]. To compute the time optimal path we target a final
state of
$x_{\text{final}}=\begin{bmatrix}x_{1}&x_{2}\end{bmatrix}^{\top}=\begin{bmatrix}1&4\end{bmatrix}^{\top}$.

state feedback Time optimal $g_{1}(x_{1})$ $g_{2}(x_{1})$ Figure 5: Time
optimal control trajectory (dotted purple), state feedback control trajectory
(solid purple with inputs held for 10ms), $g_{1}(x_{1})$ and $g_{2}(x_{1})$
are (light blue) .

The closed loop trajectories of the two chosen controllers is shown in Figure
5. It is worth observing the reach avoid set contains both the time optimal
trajectory as well as the state feedback controller. Moreover, we note the
form of $g_{1}(x_{1})$ and $g_{2}(x_{1})$ can be varied to affect the shape of
the state feedback trajectory. The computation times for each case are in
Table I. Both control implementation and simulations are written in python,
and ran on a standard laptop with a 7th gen intel core i5 processor.

The potential of our approach is shown in the average computation time for a
single step of the feedback control strategy. It is also worth noting that the
computation time for the reach avoid set on the UR5 added to the trajectory
generation is faster than computing the time optimal control alone.

Robot | UR5 | Two DOF Planar
---|---|---
Time optimal controller |  $82.4$ s |  $2.3\times 10^{-1}$ s
Controller (29) computation time |  $49\times 10^{-3}$ s |  $21\times 10^{-6}$ s
Sampling period T for (29) |  $50\times 10^{-3}$ s |  $1\times 10^{-3}$ s
Reach avoid set computation | 60.3 s | 9.6 s
TABLE I: Table presenting data for time optimal and state feedback
computations based on the reach avoid set.

The time response of the UR5 in the joint space is shown in Figure 6.
Unsurprisingly, the time optimal control is faster.

Figure 6: Minimum time optimal joint response (dotted blue) and state feedback
(29) response (solid red).

The torque response for the time optimal and the state feedback methods are
given in Figures 7, 8 respectively. It is clear that the time optimal control
leads to switching between maximum and minimum acceleration for at least one
actuator, exhibiting large instantaneous variations. In this instance there
are two switches, however in the general case there can be several,
contributing to the wearing of actuators and possibly leading to additional
steps for shaping the control input. In contrast, the state feedback response
gives a naturally Lipschitz continuous response through the application of
equation (29) in choosing the actuation level to apply. A real time
implementation is indeed possible using conventional computing resources as
illustrated in Figure 8(a). Nevertheless, the control response can be made
smoother with a smaller sampling period as shown in Figure 8(b), which would
be implementable, e.g., using embedded hardware implementation.

;

Figure 7: Optimal control torques ($\tau_{0},...,\tau_{5}$) time response.

;

(a) Torques ($\tau_{0},...,\tau_{5}$) time response, $T=50$ms.

;

(b) Torques $\tau_{0},...,\tau_{5}$ time response, $T=10$ms.

Figure 8: State feedback control (29) torque response for each actuator.

##  VIII Conclusions

We explored the trajectory planning problem for robotic manipulators using a
well established methodology from a different angle. We formulated and
addressed the problem of finding safe state-feedback controllers as a reach-
avoid problem in the projected path dynamics using ordering properties of
closed-loop trajectories under a suitable parameterisation of the control law.
The established algorithm is practicable and terminates in finite time, while
numerical experiments with a commercial manipulator show the method works
well. The feedback mechanism as well the scalability of the approach suggest
that the method could be potentially be used in problems in safety, e.g., in
collaborative robotics. Our future work will focus on using the established
framework as a building block on composition of trajectories between different
paths with motion primitives, using the inherent flexibility offered, with the
ultimate aim to address the path planning problem. Moreover, we aim to relax
some of the assumptions on the shapes of the constraint and target sets posed
herein, effectively increasing the generality of our approach.

## References

  * [1] V. Villani, F. Pini, F. Leali, and C. Secchi, “Survey on human–robot collaboration in industrial settings: Safety, intuitive interfaces and applications,” _Mechatronics_ , vol. 55, pp. 248–266, 2018.
  * [2] J. E. Michaelis, A. Siebert-Evenstone, D. W. Shaffer, and B. Mutlu, in _Proceedings of the 2020 CHI Conference on Human Factors in Computing Systems_ , 2020, pp. 1–12.
  * [3] E. M. E. Alegue, “Human-robot collaboration with high-payload robots in industrial settings,” in _IECON 2018-44th Annual Conference of the IEEE Industrial Electronics Society_. IEEE, 2018, pp. 6035–6039.
  * [4] L. Kunze, N. Hawes, T. Duckett, M. Hanheide, and T. Krajník, “Artificial intelligence for long-term robot autonomy: A survey,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 4023–4030, 2018.
  * [5] S. M. LaValle, _Planning algorithms_. Cambridge university press, 2006.
  * [6] A. Völz and K. Graichen, “A predictive path-following controller for continuous replanning with dynamic roadmaps,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3963–3970, 2019.
  * [7] L. Joseph, V. Padois, and G. Morel, “Towards x-ray medical imaging with robots in the open: safety without compromising performances,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 6604–6610.
  * [8] ——, “Experimental validation of an energy constraint for a safer collaboration with robots,” in _International Symposium on Experimental Robotics_. Springer, 2018, pp. 575–583.
  * [9] R. Rossi, M. P. Polverini, A. M. Zanchettin, and P. Rocco, “A pre-collision control strategy for human-robot interaction based on dissipated energy in potential inelastic impacts,” in _2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2015, pp. 26–31.
  * [10] Kang Shin and N. McKay, “Minimum-time control of robotic manipulators with geometric path constraints,” _IEEE Transactions on Automatic Control_ , vol. 30, no. 6, pp. 531–541, 1985.
  * [11] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, “Time-optimal control of robotic manipulators along specified paths,” _The international journal of robotics research_ , vol. 4, no. 3, pp. 3–17, 1985.
  * [12] F. Pfeiffer and R. Johanni, “A concept for manipulator trajectory planning,” _IEEE Journal on Robotics and Automation_ , vol. 3, no. 2, pp. 115–123, 1987.
  * [13] J. M. Hollerbach, “Dynamic scaling of manipulator trajectories,” in _1983 American Control Conference_. IEEE, 1983, pp. 752–756.
  * [14] S. Ma, “Time-optimal control of robotic manipulators with limit heat characteristics of the actuator,” _Advanced Robotics_ , vol. 16, no. 4, pp. 309–324, 2002.
  * [15] Z. Shiller, “Time-energy optimal control of articulated systems with geometric path constraints,” 1996.
  * [16] A. Olabi, R. Béarée, O. Gibaru, and M. Damak, “Feedrate planning for machining with industrial six-axis robots,” _Control Engineering Practice_ , vol. 18, no. 5, pp. 471–482, 2010.
  * [17] J. D. Gleason, A. P. Vinod, and M. M. K. Oishi, “Underapproximation of reach-avoid sets for discrete-time stochastic systems via lagrangian methods,” in _2017 IEEE 56th Annual Conference on Decision and Control (CDC)_ , 2017, pp. 4283–4290.
  * [18] B. Landry, M. Chen, S. Hemley, and M. Pavone, “Reach-avoid problems via sum-or-squares optimization and dynamic programming,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 4325–4332.
  * [19] J. F. Fisac, M. Chen, C. J. Tomlin, and S. S. Sastry, “Reach-avoid problems with time-varying dynamics, targets and constraints,” in _Proceedings of the 18th international conference on hybrid systems: computation and control_ , 2015, pp. 11–20.
  * [20] L. Shamgah, T. G. Tadewos, A. Karimoddini, and A. Homaifar, “Path planning and control of autonomous vehicles in dynamic reach-avoid scenarios,” in _2018 IEEE Conference on Control Technology and Applications (CCTA)_. IEEE, 2018, pp. 88–93.
  * [21] B. HomChaudhuri, M. Oishi, M. Shubert, M. Baldwin, and R. S. Erwin, “Computing reach-avoid sets for space vehicle docking under continuous thrust,” in _2016 IEEE 55th Conference on Decision and Control (CDC)_. IEEE, 2016, pp. 3312–3318.
  * [22] N. Kariotoglou, D. M. Raimondo, S. Summers, and J. Lygeros, “A stochastic reachability framework for autonomous surveillance with pan-tilt-zoom cameras,” in _2011 50th IEEE Conference on Decision and Control and European Control Conference_. IEEE, 2011, pp. 1411–1416.
  * [23] S. Summers and J. Lygeros, “Verification of discrete time stochastic hybrid systems: A stochastic reach-avoid decision problem,” _Automatica_ , vol. 46, no. 12, pp. 1951–1961, 2010.
  * [24] S. N. Krishna, A. Kumar, F. Somenzi, B. Touri, and A. Trivedi, “The reach-avoid problem for constant-rate multi-mode systems,” in _International Symposium on Automated Technology for Verification and Analysis_. Springer, 2017, pp. 463–479.
  * [25] L. Shamgah, T. G. Tadewos, A. Karimoddini, and A. Homaifar, “A symbolic approach for multi-target dynamic reach-avoid problem,” in _2018 IEEE 14th International Conference on Control and Automation (ICCA)_. IEEE, 2018, pp. 1022–1027.
  * [26] Q. Pham, “A general, fast, and robust implementation of the time-optimal path parameterization algorithm,” _IEEE Transactions on Robotics_ , vol. 30, no. 6, pp. 1533–1540, 2014.
  * [27] H. Pham and Q. Pham, “A new approach to time-optimal path parameterization based on reachability analysis,” _IEEE Transactions on Robotics_ , vol. 34, no. 3, pp. 645–659, 2018.
  * [28] R. McGovern and N. Athanasopoulos, “Kinodynamic planning for robotic manipulators using set-based methods,” in _2022 European Control Conference (ECC)_. IEEE, 2022, pp. 1309–1314.
  * [29] U. Robots, “Universal Robots Website,” https://www.universal-robots.com/, 2021, [Online; accessed 14-July-2021].
  * [30] P. M. Kebria, S. Al-wais, H. Abdi, and S. Nahavandi, “Kinematic and dynamic modelling of UR5 manipulator,” in _2016 IEEE International Conference on Systems, Man, and Cybernetics (SMC)_. IEEE, oct 2016. [Online]. Available: https://doi.org/10.1109%2Fsmc.2016.7844896
  * [31] J.-Y. Choi, J. Uh, and J. S. Lee, “Iterative learning control of robot manipulator with i-type parameter estimator,” in _Proceedings of the 2001 American Control Conference.(Cat. No. 01CH37148)_ , vol. 1. IEEE, 2001, pp. 646–651.
  * [32] F. Bouakrif, D. Boukhetala, and F. Boudjema, “Velocity observer-based iterative learning control for robot manipulators,” _International Journal of Systems Science_ , vol. 44, no. 2, pp. 214–222, 2013.
  * [33] M. de Mathelin, R. Lozano, and D. Taoutaou, “Commande adaptative et applications,” _Hermès Science_ , 2001.
  * [34] K. M. Lynch and F. C. Park, _Modern Robotics_ , 2017.
  * [35] E. Hughes, J. Hiley, K. Brown, and I. M. Smith, _Hughes Electrical and Electronic Technology_ , 07 2021.
  * [36] M. W. Spong, S. Hutchinson, and M. Vidyasagar, _Robot Modeling and Control_ , 11 2005.
  * [37] L. Habets, P. J. Collins, and J. H. van Schuppen, “Reachability and control synthesis for piecewise-affine hybrid systems on simplices,” _IEEE Transactions on Automatic Control_ , vol. 51, no. 6, pp. 938–948, 2006.
  * [38] L. Habets, M. Kloetzer, and C. Belta, “Control of rectangular multi-affine hybrid systems,” in _Proceedings of the 45th IEEE Conference on Decision and Control_. IEEE, 2006, pp. 2619–2624.
  * [39] C. Sloth and R. Wisniewski, “Control to facet for polynomial systems,” in _Proceedings of the 17th international conference on Hybrid systems: computation and control_ , 2014, pp. 123–132.
  * [40] F. Blanchini and S. Miani, _Set-Theoretic Methods in Control_ , 07 2015.
  * [41] C. Edwards and S. Spurgeon, _Sliding mode control: theory and applications_. Crc Press, 1998.
  * [42] L. M. Gjerde Johannessen, M. Hauan Arbo, and J. T. Gravdahl, “Robot dynamics with urdf & casadi,” in _2019 7th International Conference on Control, Mechatronics and Automation (ICCMA)_ , 2019, pp. 1–6.
  * [43] P. M. Kebria, S. Al-Wais, H. Abdi, and S. Nahavandi, “Kinematic and dynamic modelling of ur5 manipulator,” in _2016 IEEE international conference on systems, man, and cybernetics (SMC)_. IEEE, 2016, pp. 004 229–004 234.
  * [44] G. Teschl, _Ordinary Differential Equations and Dynamical Systems_ , 08 2012\.
  * [45] E. Lindelöf, “Sur l’application de la méthode des approximations successives aux équations différentielles ordinaires du premier ordre,” _Comptes rendus hebdomadaires des séances de l’Académie des sciences_ , vol. 116, no. 3, pp. 454–457, 1894.

##  Appendix A Manipulator models

The simple two DOF planar manipulator shown in Figure 1(a) used throughout the
examples has the Lagrangian dynamic parameters

$\displaystyle M_{L}$
$\displaystyle=\begin{bmatrix}0.03+0.02\cos(q_{2})&0.01+0.01\cos(q_{2})\\\
0.01+0.01\cos(q_{2})&0.01\end{bmatrix}$ $\displaystyle C_{L}$
$\displaystyle=\begin{bmatrix}0&-0.01(2\dot{q_{1}}+\dot{q_{2}})\sin(q_{2})\\\
0.01\dot{q_{1}}\sin(q_{2})&0\end{bmatrix}$ $\displaystyle g_{L}$
$\displaystyle=\begin{bmatrix}0.981\cos(q1)+0.4905\cos(q_{1}+q_{2})\\\
0.4905\cos(q_{1}+q_{2})\end{bmatrix}$

We consider a circular arc path that gives us is defined by the center point
and radius. The workspace description in $x$ and $y$ directions is encoded by
$z(x_{1})$ and $y(x_{1})$ as defined by a radius of $0.15$ and center point
$(0.1,0.3)$, i.e., $z(x_{1})=0.1+0.15cos(\pi x_{1})$,
$y(x_{1})=0.3-0.15sin(\pi x_{1})$, The variation of the joints in the form
$q(x_{1})=\begin{bmatrix}q_{1}(x_{1})&q_{2}(x_{1})\end{bmatrix}^{\top}$, with

$\displaystyle q_{1}(x_{1})$ $\displaystyle=\arctan 2(y(x_{1}),z(x_{1}))-$
$\displaystyle\arccos(\frac{z^{2}(x_{1})+y^{2}(x_{1})}{0.4\sqrt{z^{2}(x_{1})+y^{2}(x_{1})}})$
$\displaystyle q_{2}(x_{1})$
$\displaystyle=\pi-\arccos(1-12.5(x^{2}(x_{1})+y^{2}(x_{1}))),$

allowing $q_{1}(x_{1})$ and $q_{2}(x_{1})$ to be written in terms of $x_{1}$
as follows

$\displaystyle q_{1}(x_{1})=\arctan 2(0.3-0.15\sin(\pi x_{1}),0.1+0.15\cos(\pi
x_{1}))-$ $\displaystyle\arccos(\frac{(0.1+0.15\cos(\pi
x_{1}))^{2}+(0.3-0.15\sin(\pi x_{1}))^{2}}{0.4\sqrt{(0.1+0.15cos(\pi
x_{1}))^{2}+(0.3-0.15\sin(\pi x_{1}))^{2}}}),$ $\displaystyle
q_{2}(x_{1})=\pi-\arccos(1-12.5((0.1+0.15\cos(\pi x_{1}))^{2}$
$\displaystyle\;\;\;\;+(0.3-0.15\sin(\pi x_{1}))^{2})).$

The relevant vectors for the path dynamics can be computed as
$M(x_{1})\dot{x_{2}}=M_{L}\frac{dq(x_{1})}{dx_{1}}$,
$C(x_{1})x_{2}^{2}=M_{L}\frac{d^{2}q(x_{1})}{dx_{1}^{2}}x_{2}^{2}+C_{L}\dot{q}(x_{1}),$
$g(x_{1})=g_{L}(q(x_{1}))$

The times presented in section VII-C concern the setting described in this
section. We also apply the same constraint vector with the target set defined
as (12) in $\mathcal{X}_{\text{T}}=\\{x\in\mathcal{X}:x_{1}=1,4\leq x_{2}\leq
8.5\\}$. The time optimal method uses the final state
$x_{\text{final}}=\begin{bmatrix}x_{1}&x_{2}\end{bmatrix}=\begin{bmatrix}1&4\end{bmatrix}$.
The feedback mechanism is of the form (29) with $g_{1}(x_{1})=2x_{1}+4$,
$g_{2}(x_{1})=3.5x_{1}+1$.

The initial state of the simulation is
$x_{\text{initial}}=\begin{bmatrix}x_{1}&x_{2}\end{bmatrix}^{\top}=\begin{bmatrix}0&3\end{bmatrix}^{\top}$.

The universal robots UR5s mathematical model can be found in [43],
https://www.mathworks.com/matlabcentral/fileexchange/72049-kinematic-and-
dynamic-modelling-of-ur5-manipulator?s_tid=srchtitle.

## Appendix B

###  B-A Proof of Proposition 1

The following preliminary Fact establishes results between compositions of
Lipschitz continuous functions, while Lemma 5 shows that at least one element
of $M_{i}(x_{1})$ is strictly positive for all admissible values of $x_{1}$.

###### Fact 1

Suppose functions $f_{1}(\cdot):\mathbb{R}^{n}\rightarrow\mathbb{R}$ and
$f_{2}(.):\mathbb{R}^{n}\rightarrow\mathbb{R}$ are locally Lipschitz
continuous in a set $\mathcal{S}\subset\mathbb{R}^{n}$, with Lipschitz
constants $L_{1}$ and $L_{2}$ respectively. The following hold:

  * (i).

$f_{3}(x)=f_{1}(x)+f_{2}(x)$ is locally Lipschitz in $\mathcal{S}$.

  * (ii).

$f_{4}(x)=f_{1}(f_{2}(x))$ is locally Lipschitz in $\mathcal{S}$.

  * (iii).

Suppose that $f_{1}(x)$, $f_{2}(x)$ are bounded in $\mathcal{S}$. Then,
$f_{5}(x)=f_{1}(x)f_{2}(x)$ is locally Lipschitz in $\mathcal{S}$.

  * (iv).

Suppose that $|f_{1}(x)|>a$, $x\in\mathcal{S}$. Then the function
$f_{6}(x)=\frac{1}{f_{1}(x)}$ is locally Lipschitz in $\mathcal{S}$.

  * (v).

Consider $f_{i}(x)$, $i=1,...,N$ and the function $f(x)=\max_{i}\\{f_{i}\\}$.
Let $f_{i}(x)$ be Locally Lipschitz in
$\mathcal{S}_{i}=\\{x\in\mathcal{S}:f_{i}(x)\geq f_{j}(x),\forall
j=1,..,N,j\neq i\\}$. Then, $f$ is locally Lipschitz in $\mathcal{S}$.

  * (vi).

Consider $f_{i}(x)$, $i=1,...,N$ and the function $f(x)=\min_{i}\\{f_{i}\\}$.
Let $f_{i}(x)$ be locally Lipschitz in
$\mathcal{S}_{i}=\\{x\in\mathcal{S}:f_{i}(x)\leq f_{j}(x),\forall
j=1,..,N,j\neq i\\}$. Then, $f$ is locally Lipschitz in $\mathcal{S}$.

  * Proof

(i) For $x,y\in\mathcal{S}$, we have
$|f_{1}(y)+f_{2}(y)-f_{1}(x)-f_{2}(x)|\leq|f_{1}(y)-f_{1}(x)|+|f_{2}(y)-f_{2}(x)|\leq(L_{1}+L_{2})|y-x|$.
(ii) We have $|f_{1}(f_{2}(y))-f_{1}((f_{2}(x))|\leq
L_{1}|f_{2}(y)-f_{2}(x)|\leq L_{1}L_{2}|y-x|.$ (iii). Let $|f_{1}(x)|\leq
M_{1}$, $|f_{2}(x)|\leq M_{2}$ for all $x\in\mathcal{S}$. It follows
$|f_{1}(y)f_{2}(y)-f_{1}(x)f_{2}(x)|=|f_{1}(y)f_{2}(y)-f_{1}(y)f_{2}(x)+f_{1}(y)f_{2}(x)+f_{1}(x)f_{2}(x)|\leq
M_{1}L_{2}|y-x|+M_{2}L_{1}|y-x|=(M_{1}L_{2}+M_{2}L_{1})|y-x|$. (v) We can
write $\max(f_{1},f_{2})=\frac{f_{1}+f_{2}+|f_{1}-f_{2}|}{2}$. Since $|x|$ is
Lipschitz continuous it can be concluded from Fact 1 (ii) that $|f_{1}-f_{2}|$
is also Lipschitz. (vi) Since
$\min(f_{1},f_{2})=\frac{f_{1}+f_{2}-|f_{1}-f_{2}|}{2}$, the same reasoning as
Fact (v) can be applied. $\blacksquare$

###### Lemma 5

Under Assumption 1, for all $x\in\mathcal{X}^{{}^{\prime}}$ there exists at
least one index $i^{*}\in\\{1,2,...,N\\}$ such that
$||M_{i^{*}}(x_{1})||\geq\alpha_{1}$, for some $\alpha_{1}>0$.

  * Proof

By construction, it holds that
$M(x_{1})=M_{L}(x_{1})\frac{dq(x_{1})}{dx_{1}}$. Moreover, the symmetric mass
matrix $M_{L}(x_{1})$ is positive definite for all $x\in\mathcal{X}^{\prime}$.
Let us consider an arbitrary $y=\frac{dq(x_{1})}{dx_{1}}$, such that
$||y||=\alpha_{0}$, where $\alpha_{0}$ is provided in Assumption 1. Let
$z_{i}$, $i=1,...,N$, be the normalised eigenvectors of $M_{L}(x_{1})$ such
that $||z_{i}||=\alpha_{0},i=1,..,N$. The eigenvectors $z_{i}$ are mutually
orthogonal as $M_{L}(x)$ is symmetric. Consequently, we can express $y$ in the
base of eigenvectors $y=\sum^{N}_{i=1}c_{i}\hat{z}_{i}$, with $c_{i}\geq 0$,
$\sum_{i=1}^{N}c_{i}\geq 1$ and $\hat{z}_{i}=z_{i}$, or $\hat{z}_{i}=-z_{i}$.
The last inequality holds since $\|y\|=\|z_{i}\|$. It follows that
$M_{L}(x_{1})y=\sum_{i=1}^{N}\lambda_{i}c_{i}z_{i}$, where $\lambda_{i}>0$ are
the eigenvalues of $M_{L}(x_{1})$. Consider the set
$\mathcal{S}=\text{conv}(\pm z_{1},..,\pm z_{N})$. We can write
$\mathcal{S}=\\{x\in\mathbb{R}^{N}:\|Px\|_{1}\leq 1\\}$, where
$P=\left(\begin{bmatrix}z_{1}&\cdots&z_{N}\end{bmatrix}^{\top}\right)^{-1}$.
Setting $d=\sum_{i=1}^{N}\lambda_{i}c_{i}$ we can write
$M_{L}(x_{1})y=d\sum_{i=1}^{N}\left(\frac{\lambda_{i}c_{i}}{d}\right)z_{i}$,
or, $M_{L}(x_{1})y\in d\operatorname{bd}(\mathcal{S}).$ By norm equivalence we
have $\|x\|_{1}\leq N\|x\|_{2}$, thus
$d\mathcal{S}\supseteq\mathbb{B}(\frac{1}{dN\sigma_{\max}(P)})$, and
$\|M_{L}(x_{1})y\|\geq\frac{1}{dN\sigma_{\max}(P)}$, where $\sigma_{\max}(P)$
is the largest singular value of $P$. Consequently, there necessarily exists
at least one element of $M_{L}(x_{1})y$ such that
$\|M_{L}(x_{1})y\|\geq\alpha_{1}$ for some finite $\alpha_{1}$ and for all
$y\in\mathbb{B}(\alpha_{0})$. The result for $\|y\|>\alpha_{0}$ follows
directly by the homogeneity of the linear mapping. $\blacksquare$

Proof of Proposition 1 The proof is split into two parts, namely when the
elements $|M_{i}(x)|$ are bounded from below for all $i$, and where there
exists a subset of indices $\hat{I}\subset[1,N]$, where
$|M_{i}(x)|\leq\epsilon$ for any arbitrarily small $\epsilon$.

Case 1: $|M_{i}(x)|\geq\alpha_{1}>0$, $i=1,...,N$. By definition (3), (4) and
Fact 1 (v), (vi), $A(x)$ and $D(x)$ are locally Lipschitz continuous in
$\mathcal{X}_{i}$ if $A_{i}(x)$ and $D_{i}(x)$ are. We show that $A_{i}(x)$
and $D_{i}(x)$ are Lipschitz functions by analysing the numerator and the
denominator separately. The numerator in $A_{i}(x)$ and $D_{i}(x)$ is
constructed by the sum of the elements of vectors $\tau^{\max}$ and
$\tau^{\min}$, $-C(x)x_{2}^{2}$ and $-g(x_{1})$. By Fact 1 (i), (iii) if each
vector element in this sum is Lipschitz the numerator will be Lipschitz for
all $i=1,...,N$. By Assumption 2, $\tau^{\max}(x)$ and $\tau^{\min}(x)$ are
Lipschitz. It is also known from, e.g., [31], [32], [33], that $M_{L}(\cdot)$,
$C_{L}(\cdot)$ and $g_{L}(\cdot)$ are locally Lipschitz functions. The vector
$g(x_{1})=g_{L}(q(x_{1}))$, therefore $g(x_{1})$ is also Lipschitz from Fact 1
(ii). The vector $C(x)x_{2}^{2}$ can be rewritten as
$C(x)x_{2}^{2}=M_{L}(q(x_{1}))\frac{d^{2}(q(x_{1}))}{dx_{1}^{2}}x_{2}^{2}+C_{L}(q(x1),\dot{q}(x_{1}))$.
Since $M_{L}(\cdot)$, $\frac{d^{2}(q(x_{1}))}{dx_{1}^{2}}$, $x_{2}^{2}$,
$C_{L}(\cdot)$ are all locally Lipschitz continuous, by applying Fact 1 (i),
(iii) we have that $C(x)x_{2}^{2}$ is locally Lipschitz continuous. Hence, the
numerator in the definition of equations $A_{i}(x)$, $D_{i}(x)$ is locally
Lipschitz. The denominator for both $A_{i}$ and $D_{i}$ is $M_{i}(x_{1})$.
From Fact1 (iii) $M(x_{1})=M_{L}(q(x_{1}))\frac{dq(x_{1})}{dx_{1}}$ is
Lipschitz continuous since $M_{L}(q(x_{1}))$ and $\frac{dq(x_{1})}{dx_{1}}$
are. Since $|M_{i}(x_{1})|>\alpha_{1}$, by Fact 1 (iv)
$\frac{1}{M_{i}(x_{1})}$ is also Lipschitz. Thus, $A_{i}$ and $D_{i}$, and
consequently $A(x)$ and $D(x)$ are locally Lipschitz continuous in
$\mathcal{X}^{{}^{\prime}}$.

Case 2: Suppose there exists a subset of indices
$\hat{\mathcal{I}}\subset\\{1,2,...,N\\}$ such that $|M_{i}(x)|\leq\epsilon$,
where $\epsilon$ is an arbitrary small real number and $i\in\hat{I}$. By Lemma
5, there exists at least an index i such that $|M_{i}(x)|\geq\alpha_{1}$, for
some $\alpha_{1}>0$. Let
$\mathcal{I}=\\{1,2,...,N\\}\setminus\hat{\mathcal{I}}$. Let us consider a
point where $M_{i}(x_{1})$, $i\in\hat{\mathcal{I}}$, becomes arbitrarily smal.
Setting
$a_{i}(x)=\frac{\tau^{\max}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$,
$d_{i}(x)=\frac{\tau^{\min}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$

we can express $A_{i}(x)$ and $D_{i}(x)$ as

$\displaystyle A_{i}(x)=\begin{cases}a_{i}(x)&\text{if $M_{i}(x_{1})>0$},\\\
d_{i}(x)&\text{if $M_{i}(x_{1})<0$},\\\ \end{cases}$ (33) $\displaystyle
D_{i}(x)=\begin{cases}d_{i}(x)&\text{if $M_{i}(x_{1})>0$},\\\
a_{i}(x)&\text{if $M_{i}(x_{1})<0$}.\\\ \end{cases}$ (34)

Further, we set $a_{i}(x)=\frac{\beta}{\epsilon}$,
$d_{i}(x)=\frac{\gamma}{\epsilon}$.

Assuming $\epsilon\to 0^{+}$ or $\epsilon\to 0^{-}$ we have

$\displaystyle\lim\limits_{\epsilon\to
0^{+}}(a_{i})=\begin{cases}-\infty\;\;\;&\text{when}\;\;\;\beta<0,\\\
+\infty\;\;\;&\text{when}\;\;\;\beta>0,\end{cases}$ (35)
$\displaystyle\lim\limits_{\epsilon\to
0^{+}}(d_{i})\to\begin{cases}-\infty\;\;\;&\text{when}\;\;\;\gamma<0,\\\
+\infty\;\;\;&\text{when}\;\;\;\gamma>0,\end{cases}$ (36)
$\displaystyle\lim\limits_{\epsilon\to
0^{-}}(d_{i})=\begin{cases}+\infty\;\;\;&\text{when}\;\;\;\gamma<0,\\\
-\infty\;\;\;&\text{when}\;\;\;\gamma>0,\end{cases}$ (37)
$\displaystyle\lim\limits_{\epsilon\to
0^{-}}(a_{i})\to\begin{cases}+\infty\;\;\;&\text{when}\;\;\;\beta<0,\\\
-\infty\;\;\;&\text{when}\;\;\;\beta>0.\end{cases}$ (38)

Equations (35), (37) relate to $A_{i}(x)$ and (36), (38) relate to $D_{i}(x)$.
We note for the case where $\beta=0$, then $a_{i}=0$, and when $\gamma=0$,
then $d_{i}=0$, which is a Lipschitz continuous function. Last, we define

$\displaystyle A^{{}^{\prime}}(x)$ $\displaystyle:=\min\limits_{\\{i\in
I\\}}A_{i}(x),$ $\displaystyle\hat{A}(x)$
$\displaystyle=\min\limits_{\\{i\in\hat{I}\\}}A_{i}(x),$ $\displaystyle
D^{{}^{\prime}}(x)$ $\displaystyle=\max\limits_{\\{i\in I\\}}D_{i}(x),$
$\displaystyle\hat{D}(x)$
$\displaystyle=\max\limits_{\\{i\in\hat{I}\\}}D_{i}(x).$

Functions $A^{{}^{\prime}}(x),D^{{}^{\prime}}(x)\in\mathbb{R}$ have a finite
range. Consequently, $A(x)$ and $D(x)$ can be written as

$\displaystyle A(x)$ $\displaystyle=\min(A^{{}^{\prime}}(x),\hat{A}(x)),$
$\displaystyle D(x)$ $\displaystyle=\max(D^{{}^{\prime}}(x),\hat{D}(x)).$

We study the case when $\epsilon\to 0$. If $\hat{A}(x)\to\infty$, then
$A(x)=A^{{}^{\prime}}(x)$. Likewise, if $\hat{D}(x)\to-\infty$, then
$D(x)=D^{{}^{\prime}}(x)$. In these cases we revert to Case 1. Analysing
similarly (35) - (38) allows us to observe that all possible situations that
render $A(x)$ and $D(x)$ Lipschitz continuous are covered when $\beta\geq 0$
and $\gamma\leq 0$. This holds in the cases where $\epsilon\to 0^{+}$ and
$\epsilon\to 0^{-}$. Finally, we consider the case $\beta<0,\gamma>0$, in the
cases where $\epsilon\to 0^{+}$ and $\epsilon\to 0^{-}$. We define two large
positive numbers, $M_{1}$ and $M_{2}$ such that
$A(x)=\min(A^{{}^{\prime}},-M_{1})=-M_{1}$ and
$D(x)=\max(D^{{}^{\prime}},M_{2})=M_{2}$. Clearly, $A(x)<D(x)$ thus,
$x\notin\mathcal{X}^{\prime}$. Hence, any state $x$ that produces an unbounded
value for $A(x)$ or $D(x)$ is necessarily outside the admissible region, i.e.,
$x\notin\mathcal{X}^{\prime}$. We conclude the only possible arrangements
provide local Lipschitz continuity properties to (33) and (34). By combining
Case 1 and 2, the result is obtained. $\blacksquare$

###  B-B Proof of Lemma 1

By Corollary 1, $u(x)$ is locally Lipschitz continuous in $\mathcal{X}$.
Consequently, by Fact 1 (i), (iii) the backward dynamics $f(x)=-Ax-Bu(x)$ is
also locally Lipschitz continuous. Therefore by the Picard–Lindelöf theorem
the solution of $\dot{x}=f(x)$ is unique for a given $x$ [44] [45]. Suppose an
intersection between two trajectories occurs at $x$, the backward dynamics
must produce two solutions which is a contradiction. Therefore intersection is
not possible. The same reasoning can be applied to the forward dynamics.
$\blacksquare$

###  B-C Proof of Lemma 2

Suppose there are two trajectories that intersect defined as
$\mathcal{T}_{u}=\mathcal{T}_{u}(x_{u},\lambda_{u})$ and
$\mathcal{T}_{l}=\mathcal{T}_{l}(x_{l},\lambda_{l})$ with
$\mathcal{T}_{l}\cap\mathcal{T}_{u}=\\{x^{*}\\}$. We describe these two
trajectories with four pieces that emanate from $x^{*}$, namely, as
$\mathcal{T}_{u}=\mathcal{T}_{b,u}\cup\mathcal{T}_{f,u}$ and
$\mathcal{T}_{l}=\mathcal{T}_{b,l}\cup\mathcal{T}_{f,l}$, where
$\mathcal{T}_{b,u}=\mathcal{T}_{b}(x^{*},\lambda_{u})$,
$\mathcal{T}_{f,u}=\mathcal{T}_{f}(x^{*},\lambda_{u})$,
$\mathcal{T}_{b,l}=\mathcal{T}_{b}(x^{*},\lambda_{l})$,
$\mathcal{T}_{f,l}=\mathcal{T}_{f}(x^{*},\lambda_{l})$. We observe that for
all $x\in int(\mathcal{X})$, $\frac{\partial
u(x,\lambda)}{\partial\lambda}=A(x)-D(x)>0$. Thus, $\lambda_{u}>\lambda_{l}$
implies $u(x^{*},\lambda_{u})>u(x^{*},\lambda_{l})$. Let $\dot{x}=f(x)$ as
given by (10). Consider the gradients defined as
$f^{u}=f(x^{*},\lambda_{u})=\begin{bmatrix}x^{*}_{2}&u(x^{*},\lambda_{u})\end{bmatrix}^{\top}$
and
$f^{l}=f(x^{*},\lambda_{l})=\begin{bmatrix}x^{*}_{2}&u(x^{*},\lambda_{l})\end{bmatrix}^{\top}$.
We consider the _angle of emanation_ from $x^{\ast}$ for some vector
$f\in\mathbb{R}^{2}$ as $\theta(f)=\tan^{-1}{(\frac{f_{2}}{f_{1}}})$. At
intersection it holds that $f^{u}_{1}=f^{l}_{1}$, and $f^{u}_{2}>f^{l}_{2}$,
thus $\frac{f^{u}_{2}}{f^{u}_{1}}>\frac{f^{l}_{2}}{f^{l}_{1}}$, or,
$\theta(f^{u})>\theta(f^{l})$. Consequently, for any $\delta>0$ small enough
and the slice $\mathcal{X}_{f}=\mathcal{W}(\mathcal{X},x_{1}^{*}+\delta)$ with
$\bar{x}_{u}=\mathcal{T}_{f,u}\cap\mathcal{X}_{f}$ and
$\bar{x}_{l}=\mathcal{T}_{f,l}\cap\mathcal{X}_{f}$, it holds that
$\bar{x}_{u,2}>\bar{x}_{l,2}$. Similarly, for the slice
$\mathcal{X}_{b}=\mathcal{X}_{v}(x_{1}^{*}-\delta)$ with
$\tilde{x}_{u}=\mathcal{T}_{b,u}\cap\mathcal{X}_{b}$ and
$\tilde{x}_{l}=\mathcal{T}_{b,l}\cap\mathcal{X}_{b}$ it holds that
$\tilde{x}_{u,2}<\tilde{x}_{l,2}$. The proof is complete if one considers that
the trajectories are continuous, thus, no jumps are allowed and consequently a
second intersection cannot happen. $\blacksquare$

###  B-D Proof of Lemma 3

$(\Rightarrow)$ The inner product $S(x)$ is between two vectors, the first
being the normal vector
$n\in\left\\{\begin{bmatrix}-m_{u}(x)&1\end{bmatrix}^{\top},\begin{bmatrix}m_{l}(x)&-1\end{bmatrix}^{\top}\right\\}$
to $\mathcal{V}$ where $\mathcal{V}\in\\{\mathcal{V}^{u},\mathcal{V}^{l}\\}$
evaluated at $x\in\mathcal{V}$. In general $\mathcal{V}$ is not smooth,
however it is continuous.

The second part of the inner product represents the vector field of the system
dynamics choosing the extreme value of the control actuation $\lambda$, i.e,
$\dot{x}\in\left\\{f(x,0),f(x,1)\right\\}\subset\mathcal{F}(x,u(x))$.
Therefore, when $S(x)\leq 0$ then $\dot{x}\in\mathcal{F}(x)$, and
$\dot{x}\in\mathcal{B}(x)$, thus
$\mathcal{B}(x)\cap\mathcal{F}(x)\neq\emptyset$. $(\Leftarrow)$ Since for each
$x$ the input is a convex combination of two limits (9), $\mathcal{F}(x,u(x))$
is a convex polyhedral cone with generators $A(x)$ and $D(x)$. Consequently,
$\mathcal{B}(x)\cap\mathcal{F}(x)\neq\emptyset$ necessarily implies
$\begin{bmatrix}x_{2}&D(x)\end{bmatrix}^{\top}\cap\mathcal{B}(x)\neq\emptyset$
when $x\in\mathcal{V}^{u}$ and
$\begin{bmatrix}x_{2}&A(x)\end{bmatrix}^{\top}\cap\mathcal{B}(x)\neq\emptyset$.
Therefore $S(x)\leq 0$. $\blacksquare$

###  B-E Proof of Lemma 4

We need to show that $S(x)$ has a finite number of roots, thus, the partition
$\mathcal{I}$ is finite. First, notice if there are any equilibrium points for
the system (5), they induce intervals. At non equilibrium points, the terms
$-m_{u}(x)x_{2}+D(x)$ or $m_{l}(x)x_{2}-A(x)$ are locally Lipschitz continuous
functions, with a bounded derivative. Therefore, within a finite interval, the
number of roots is bounded, and thus the partition is finite. $\blacksquare$

###  B-F Proof of Theorem 1

(i) All operations within Algorithm 1 involve propagation of trajectories and
have a finite computation time. To show that Algorithm 2 terminates in finite
time, it is enough to observe that $L$ is finite by Lemma 4, and every
operation described within the loop in Lines 6-26 is finite. (ii) Consider any
initial condition state $x$ in $\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$. To
show $\mathcal{X}_{\text{T}}$ will be reached in finite time we explicitly
construct function $\lambda(x)$: We consider the slice
$\mathcal{W}(\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}}),x_{1})$ given by
(16), and define the vectors
$x^{u}(x)\in\mathcal{W}(\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}}),x_{1})$,
$x^{l}(x)\in\mathcal{W}(\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}}),x_{1})$
on the upper and lower boundary of
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$. We define the control law
$\lambda(x)=\frac{x_{2}-(x^{u}(x))_{2}}{(x^{l}(x))_{2}-(x^{u}(x))_{2}}$. The
control law is constructed such than when $x_{2}=x_{2}^{l}$, $\lambda(x)=1$
and thus $\dot{x}_{2}=u(x,\lambda(x))=A(x)$. Likewise, if $x_{2}=x_{2}^{u}$,
$\lambda(x)=0$ and thus $\dot{x}_{2}=u(x,\lambda(x))=D(x)$. By construction of
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ and Lemmas 3, 4, if the state
lies on the upper or lower boundary $\mathcal{V}^{u}$ and $\mathcal{V}^{l}$
respectively, then (21) is verified for some inputs and there exists an input
that will drive the system inside $\mathcal{X}$. Moreover, since
$\dot{x}_{1}=x_{2}\geq 0$ the value of $x_{1}$ increases with time when
$x_{2}>0$. For the case when $x_{2}=0$, $x$ is necessarily on the lower
boundary, thus, $\dot{x}_{2}=A(x)>0$, thus, the value of $x_{1}$ will increase
in finite time. Last, taking into account that the right boundary of
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ is $\mathcal{X}_{\text{T}}$,
there is necessarily a finite time $t^{*}$ such that the solution to the
system $\phi_{\text{f}}(t^{*};x_{0},\lambda(x))$ is in
$\mathcal{X}_{\text{T}}$. $\blacksquare$

###  B-G Proof of Theorem 2

The system (10) is continuous if $\lambda(x)$ (28) is Lipschitz continuous.
Thus, $\mathcal{T}_{f}(x,\lambda(x))$ represents a continuous curve in the
phase plane. This prevents crossing of the boundary of
$\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ without intersecting it.

Consider a state $x$ on the boundary
$\operatorname{bd}(\mathcal{R}_{\epsilon}(\mathcal{X}_{T}))$. Since
$\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$ is a reach-avoid set, there exists
$u(x,\lambda(x))$ such that (21) is verified.

Moreover, the value of $x_{1}$ will continually increase until
$\mathcal{X}_{T}$ is reached in finite time. Specifically, the time taken for
a trajectory between any two points $(x_{a})_{1}$ and $(x_{b})_{1}$ is given
by $T=\int_{(x_{1})_{a}}^{(x_{1})_{b}}\frac{1}{x_{2}(x_{1})}dx_{1}$ [34,
Chapter 9]. $\blacksquare$