# Safe Machine-Learning-supported Model Predictive Force and Motion Control in
Robotics

Janine Matschek  Johanna Bethge  and Rolf Findeisen Janine Matschek and Rolf
Findeisen are with the Control and Cyber-Physical Systems Laboratory, TU
Darmstadt, (rolf.findeisen, janine.matschek@iat.tu-darmstadt.de); Johanna
Bethge<EMAIL_ADDRESS>is with the Laboratory of Systems Theory and
Automatic Control, Otto-von-Guericke-Universität Magdeburg.

###### Abstract

Many robotic tasks, such as human-robot interactions or the handling of
fragile objects, require tight control and limitation of appearing forces and
moments alongside sensible motion control to achieve safe yet high-performance
operation. We propose a learning-supported model predictive force and motion
control scheme that provides stochastic safety guarantees while adapting to
changing situations. Gaussian processes are used to learn the uncertain
relations that map the robot’s states to the forces and moments. The model
predictive controller uses these Gaussian process models to achieve precise
motion and force control under stochastic constraint satisfaction. As the
uncertainty only occurs in the static model parts - the output equations - a
computationally efficient stochastic MPC formulation is used. Analysis of
recursive feasibility of the optimal control problem and convergence of the
closed loop system for the static uncertainty case are given. Chance
constraint formulation and back-offs are constructed based on the variance of
the Gaussian process to guarantee safe operation. The approach is illustrated
on a lightweight robot in simulations and experiments.

Force control, motion control, robotics, model predictive control, machine
learning, Gaussian processes, safety, constraint-satisfaction, chance-
constraints.

##  1 Introduction

Robots are increasingly used for interactive and cooperative tasks in a wide
range of applications, which require safe interaction with the environment.
For instance, robots should safely support humans in production processes,
without the traditional separation between the robotic and human coworkers
[1]. Robots, furthermore, should reliably assist physicians in medical
treatments[2] , should securely support elderly people [3], or should help in
rehabilitation tasks [4]. Safe interaction requires tight motion control
limiting the forces and appearing moments.

While many force control schemes exist, most do not ensure satisfaction of
constraints, e.g., with respect to the appearing forces and moments [5, 6, 7].
Existing approaches are furthermore often limited with respect to the
flexibility to formulate the force-motion control task in a structured manner.

This work proposes a flexible learning-supported model predictive motion and
force control scheme, which ensures the safe satisfaction of force and motion
constraints despite uncertainty in the wrench model.

Model predictive control (MPC), an optimization based control strategy, allows
considering constraints or limitations while optimizing the systems predicted
behavior. MPC schemes are by now widely used for robotic tasks, including the
explicit consideration or limitation of forces, see e.g., [8, 9, 10, 11, 12].
A series of motion and force control schemes using MPC exist [13, 14, 15, 16,
17], spanning from hybrid motion and force control [13], admittance control
[14, 15] to unified approaches for MPC force control [16, 17].

Most results, however, rely on the availability of good force models. They
can, e.g., be obtained via the identification of tailored linear and nonlinear
spring and damper models [18]. However, the resulting models are often limited
in their precision. Machine learning approaches can be used to overcome this
challenge, allowing for data-driven or hybrid modeling. We use Gaussian
processes (GPs) [19, 20] to capture and learn the state-force interactions.
Force modelling via Gaussian processes is, e.g., considered in [21, 22, 23,
24, 25]. Learning of friction and grasping forces using GPs is considered in
[21, 22], while [23] uses GPs to process sensor data from a tactile array.
Contact forces occurring in human-robot interactions are considered in [24]
via GPs. The interaction of a robot with a static environment using GPs to
obtain a learned contact force model for MPC is considered in [25].

This work proposes a combined robot motion and force control scheme. An MPC
scheme is presented assuming that forces and moments are described by an
(unknown or uncertain) static mapping of the robot’s states and inputs. This
mapping is learned using GPs, while ensuring constraint satisfaction on the
forces with high probability to ensure safe interaction. Compared to [8, 9,
10, 26, 27, 28, 11, 29, 30, 31, 12, 13, 14, 15, 16, 17, 25], we propose to
combine first-principle models, e.g., linear spring models, with GP models.
The resulting hybrid model is used in a model predictive control scheme for
combined motion and force control. As only the static mapping from the states
to the forces is learned, it is not necessary to propagate the influence of
the resulting (“output”) uncertainty through the dynamics to ensure safe
operation, as often considered in other learning-supported MPC schemes [32,
33, 34, 35, 36]. Inspired by [37, 38, 39, 35, 33, 40, 41], we utilize the
variance in the hybrid output model to tighten the MPC constraints. In
contrast to existing works, however, we tighten only the constraints mapping
from the states to the “output” (=forces), instead of using tubes [42] for
state constraint tightening of the dynamics. Since the tightening is based on
the variances it leads to a chance constraint formulation of the MPC.

The main contribution of this work is the development of a safe force and
motion control scheme combining Gaussian process model learning and MPC where
model uncertainty enters the static mapping from the states to the controlled
variables. We derive stability/recursive feasibility conditions for the
learning-supported predictive controller. In the predictive controller, the
uncertainty estimates provided by the Gaussian process are integrated as
chance constraints to guarantee stochastic constraint satisfaction. As the
uncertainty is related to the “static output” mapping, one can calculate it
offline efficiently, i.e., there is no need for computational intense online
propagation through the system dynamics. The approach allows offline learning,
as well as learning in between batches of iterative tasks. Summarizing, the
uncertainty estimates allow us to find suitable back-offs of the constraints
to robustify the model predictive controller. The theoretical findings are
illustrated in simulations and experiments considering force and motion
control of a lightweight KUKA robot, see Figure 1.

Figure 1: Example application: a lightweight robot equipped with a robotic
hand should write on a soft surface.

The remainder of the paper is structured as follows: The considered, learning
supported control problem is introduced in Section II. Section III proposes
the model predictive force controller. It is extended by a Gaussian-process-
learning component, which is introduced in Section IV. The overall resulting
learning-supported force controller is presented in Section V. Section VI
illustrates the proposed approach in simulations and experiments for a
lightweight robot. Finally, Section VII concludes this work with a summary and
an outlook.

##  2 Problem Setup

We aim to develop a control scheme that guarantees safe interaction and
satisfaction of force constraints, e.g., between a robot and its environment.
An example might be a robot that is supposed to write on a delicate surface
limiting the maximum force, see Fig. 1, a robot that should polish a surface
with desired intensity, or a robot that should interact safely with a human
not exceeding a certain force. To achieve this goal, we fuse model-based
predictive motion and force controller with Gaussian process based machine-
learning to capture the occurring, often difficult to model, forces.

MPC allows tackling a wide variety of force and motion control tasks [15, 13,
14, 29, 25, 12, 17], spanning from direct and indirect force control to joint
hybrid motion/position and force control. We focus on hybrid position and
force control, where some positions, as well as specific forces/moments,
should be controlled jointly[43, 7].

Specifically, we focus on motions where a robot should move (with a tool)
along a ”surface”, cf. Fig. 1. To do so, the interaction forces need to be
tightly controlled and should be limited for safety reasons. To account for
changing environments and to improve performance, the controller should be
able to adapt and learn based on data. We formalize this task in the following
sections.

###  2.1 Constrained safe force and motion control

We consider that the dynamical system for the motion and force control task,
e.g., a robot and its environment, is described by the nonlinear dynamics

$\displaystyle\dot{x}(t)$ $\displaystyle=$ $\displaystyle f(x(t),u(t)),\quad
x(0)=x_{0},$ (1a) $\displaystyle y(t)$ $\displaystyle=$ $\displaystyle
h(x(t)).$ (1b)

Here, $t\in\mathbb{R}$ represents the time, $x(t)\in\mathbb{R}^{n_{\text{x}}}$
denotes the state, and $u(t)\in\mathbb{R}^{n_{\text{u}}}$ is the input of the
system. The states might for example describe the angular positions and
velocities of the robot, while the inputs might represent the torques in the
joints. The map
$f:\mathbb{R}^{n_{\text{x}}}\times\mathbb{R}^{n_{\text{u}}}\to\mathbb{R}^{n_{\text{x}}}$
describes the dynamics, while the “output” $y(t)\in\mathbb{R}^{n_{\text{y}}}$,
given by $h:\mathbb{R}^{n_{\text{x}}}\to\mathbb{R}^{n_{\text{y}}}$ maps the
states to the variables of interest – the controlled variables, such as
positions, velocities, forces and moments111The output $y$ might also
explicitly depend on the input $u$. While the derived results hold for this
case, we do not consider the influence of the inputs directly for simplicity
of presentation.. We assume, for simplicity of presentation, that full state
measurements are availabe. Note that the output does not relate to measured
variables, it is used to evaluate the control performance via the controlled
variables. Section 6 presents an example for force and motion control of a
lightweight robot, cf. Fig. 1.

We want to satisfy constraints on the states, inputs, and outputs, which are
given by the sets $\mathcal{X}\subset\mathbb{R}^{n_{\text{x}}}$,
$\mathcal{Y}\subset\mathbb{R}^{n_{\text{y}}}$, and
$\mathcal{U}\subset\mathbb{R}^{n_{\text{u}}}$, i.e., to limit the forces,
torques, or states. Depending on the formulation, these will be hard – set
based – constraints, or probabilistic constraints, see Section 5. With respect
to the sets we assume that

###### Assumption 1

The sets $\mathcal{X}$, $\mathcal{Y}$ are closed, and $\mathcal{U}$ is
compact.

With respect to the dynamical system we furthermore assume that:

###### Assumption 2

$f$, and $h$ are sufficiently often continuously differentiable and locally
Lipschitz222One might relax this condition to only hold in a region of
interest, which we avoid to streamline the presentation.. Furthermore, for any
continuous input signal with $u(\tau)\in\mathcal{U}$ and for all
$x_{0}\in\mathcal{X}$, (1) admits a unique absolutely continuous solution.

The goal is to follow a given reference path, which is defined as a geometric
curve in the “output” space, as good as possible while satisfying constraints.
For example the robot shown in Fig. 1 should write on a flexible and possibly
uneven surface. Hence, it should follow a given path while keeping forces at
desired values and constrained, even under disturbances and uncertainty. The
reference path is given by

$\mathcal{P}:=\left\\{y_{\text{r}}\in\mathbb{R}^{n_{\text{y}}}|y_{\text{r}}=r_{\text{pf}}(\theta(t))\right\\},$
(2)

with the parametrization $r_{\text{pf}}:\Theta\to\mathbb{R}^{n_{\text{y}}}$.
Note that the reference only indirectly depends on the time via the path
parameter $\theta(t)\in\Theta=[-1,0]$. The evolution of the path parameter
over time is not fixed a priori – it is adjusted by the controller online [44,
45, 13], which provides additional degrees of freedom that can be exploited by
the controller. For example, in case of disturbances, the controller can
adjust the reference speed while compensating for the disturbance, thus
avoiding performance deterioration, cf. [13].

Given the reference path $\mathcal{P}$, we aim to solve the following task
[46, 45, 13]:

######  Task 1 (Path Following Force and Motion Control)

Given the system (1) and path $\mathcal{P}$, design a controller that
achieves:

  1. 1.

Convergence: The “output” (1b) should converge to the set $\mathcal{P}$, i.e.,
$\lim\limits_{t\to\infty}y(t)-r_{\textup{pf}}(\theta(t))=0$.

  2. 2.

Forward Motion along the Path: The reference $r_{\textup{pf}}(\theta(t))$
moves along $\mathcal{P}$ in the direction of increasing $\theta$ values,
i.e., $\dot{\theta}(t)\geq 0$ and $\lim\limits_{t\to\infty}\theta(t)=0$.

  3. 3.

Constraint Satisfaction: The states, inputs, and “outputs” should satisfy
$x\\!\in\\!\mathcal{X}$, $u\\!\in\\!\mathcal{U}$, and $y\\!\in\\!\mathcal{Y}$.

Note that $y$ and the path $\mathcal{P}$ can describe (desired) motions and
forces/moments. Hence, one can include constraints on the forces by choosing
appropriate constraint sets.

In comparison to earlier works [46, 45, 13], we consider that the “output”
mapping $h$ might be unknown or uncertain, as relations between the robot’s
states, the environment and the forces/moments are, in general, challenging to
model. To do so, we use methods from machine learning, as outlined in the
following subsection.

###  2.2 Learning-supported force/output modeling

We propose to use a hybrid model combining machine learning and first
principle model components. The hybrid model is used in the controller to
predict, control, and limit the forces for a reliable and safe operation. It
is given by

$\tilde{h}(x)=h_{\mathrm{fp}}(x)+h_{\mathrm{ml}}(x),$ (3)

where the first principle part
$h_{\text{fp}}:\mathbb{R}^{n_{\text{x}}}\to\mathbb{R}^{n_{\text{y}}}$ encodes
prior knowledge, while
$h_{\text{ml}}:\mathbb{R}^{n_{\text{x}}}\to\mathbb{R}^{n_{\text{y}}}$ is
obtained from data via machine-learning achieving the following task:

######  Task 2 (Output/Force Model Learning)

Learn a hybrid model $\tilde{h}(x)=h_{\mathrm{fp}}(x)+h_{\mathrm{ml}}(x)$
exploiting data $D$ such that:

  1. 1.

$\tilde{h}(x)$ fits the data, i.e., $\tilde{h}(x_{i})\approx\hat{y}_{i}$ for
$(x_{i},\hat{y}_{i})\in D$, while trading off between complexity and
consistency.

  2. 2.

The error $(\tilde{h}(x)-h(x))$ can be bounded.

We outline in Section 4 how to use Gaussian processes to achieve this task. We
use the resulting hybrid model in a tailored model predictive control scheme
to achieve safe motion and force control, as outlined in Section 5.

##  3 Model Predictive Force and Motion Control

Model predictive control is a model based approach that repeatedly solves an
optimal control problem [47, 48]. It allows controlling linear and nonlinear
systems, the direct consideration of constraints, as well as preview
information about disturbances, changing references, and paths [44]. The
possibility to explicitly account for constraints makes MPC a valuable tool
for the control and decision-making of autonomous systems and robots.
Constraint consideration enables safe operation by limiting torques, robot
positions, velocities, or forces and taking obstacles directly into account,
as outlined in the following.

###  3.1 Optimal control problem formulation

For path following motion and force control (Task 1) we propose to use a
tailored sampled-data MPC formulation [46, 45, 13]. At every sampling time
$t_{k}$ an optimal control problem, exploiting the reference speed along the
path as an additional degree of freedom, is solved:

$\underset{\bar{u},\bar{v}}{\text{min}}~{}J_{\text{pf}}\left(\bar{e}_{\text{pf}},\bar{\theta},\bar{u},\bar{v},\bar{x},\bar{z}\right)$
(4a) subject to $\forall\tau\in[0,T]$ $\displaystyle\dot{\bar{x}}(\tau)$
$\displaystyle=f(\bar{x}(\tau),\bar{u}(\tau)),\quad\bar{x}(0)=x(t_{k}),$ (4b)
$\displaystyle\dot{\bar{z}}(\tau)$
$\displaystyle=g(\bar{z}(\tau),\bar{v}(\tau)),\quad\;\bar{z}(0)=z(t_{k}),$
(4c) $\displaystyle\quad\bar{\theta}(\tau)$ $\displaystyle=l(\bar{z}(\tau)),$
(4d) $\displaystyle\quad\bar{e}_{\text{pf}}(\tau)$
$\displaystyle=r_{\text{pf}}(l(\bar{z}(\tau))-h(\bar{x}(\tau)),$ (4e)
$\displaystyle\quad\bar{x}(\tau)$
$\displaystyle\in\mathcal{X},~{}\bar{u}(\tau)\in\mathcal{U},~{}h(\bar{x}(\tau))\in\mathcal{Y},$
(4f) $\displaystyle\quad\bar{z}(\tau)$
$\displaystyle\in\mathcal{Z},~{}\bar{v}(\tau)\in\mathcal{V},~{}\bar{\theta}(\tau)\in\Theta,$
(4g) $\displaystyle(\bar{x}(T$
$\displaystyle),\bar{z}(T))^{\top}\in\mathcal{F}_{\text{pf}}$ (4h)

Predictions are indicated by $\bar{\cdot}$. Besides the system dynamics (4b),
a “virtual reference dynamics” (4c)-(4d) is used, which allows shaping the
dynamics of the speed along the path [45, 13]. The virtual (reference) state,
input, and output are $z(t)\in\mathbb{R}^{n_{\text{z}}}$, $v(t)\in\mathbb{R}$,
and $\theta(t)\in\mathbb{R}$, which are described by the virtual system
dynamics
$g:\mathbb{R}^{n_{\text{z}}}\times\mathbb{R}\to\mathbb{R}^{n_{\text{z}}}$ and
the virtual output $l:\mathbb{R}^{n_{\text{z}}}\to\mathbb{R}$. The virtual
input $v$ provides an additional degree of freedom in the optimal control
problem allowing to adjust the reference path progress/evolution. The
constraint (4h)
$\mathcal{F}_{\text{pf}}\subseteq(\mathcal{X}\times\mathcal{Z})\cap(h^{-1}(\mathcal{Y})\times
l^{-1}(\Theta))$ is a final terminal region constraint. It is used to ensure
repeated feasibility and stability/convergence [45, 13].

The cost function penalizes the error $e_{\text{pf}}(t)$ (4e) to achieve path
following:

$\displaystyle
J_{\text{pf}}\left(\bar{e}_{\text{pf}},\bar{\theta},\bar{u},\bar{v},\bar{x},\bar{z}\right):=\int\limits_{0}^{T}$
$\displaystyle
L_{\text{pf}}\left(\bar{e}_{\text{pf}}(\tau),\bar{\theta}(\tau),\bar{u}(\tau),\bar{v}(\tau)\right)\text{d}\tau$
$\displaystyle+$ $\displaystyle
E_{\text{pf}}\left(\bar{x}(T),\bar{z}(T)\right).$ (5)

Here,
$L_{\text{pf}}:\mathbb{R}^{n_{\text{y}}}\times\mathbb{R}\times\mathbb{R}^{n_{\text{u}}}\times\mathbb{R}\to\mathbb{R}_{0}^{+}$
is a stage cost and
$E_{\text{pf}}:\mathbb{R}^{n_{\text{x}}}\times\mathbb{R}^{n_{\text{z}}}\to\mathbb{R}_{0}^{+}$
is a terminal penalty term.

State, input and output constraints are enforced by (4f), (4g) (Task 1.3).
Forward motion along the path (Task 1.2) is ensured by requiring that
$\Theta:=[-1,0]$ and that $\dot{\theta}\geq 0$.

The optimal control problem is solved at all sampling times $t_{k}$. From the
resulting optimal input signal, only the first part until the next sampling
instant is used and the optimization is repeated in a receding-horizon
fashion.

Convergence to the path/stability of path following MPC can be guaranteed,
similar to standard MPC, by suitable choice of the cost function and the
terminal constraints [45, 44]. We consider that the following assumptions and
conditions hold:

###### Assumption 3

The stage cost
$L_{\textup{pf}}:\mathbb{R}^{n_{\textup{y}}}\times\mathbb{R}\times\mathbb{R}^{n_{\textup{u}}}\times\mathbb{R}^{n_{\textup{v}}}\to\mathbb{R}_{0}^{+}$
is continuous and lower bounded by a class $\mathcal{K}_{\infty}$ function
$\alpha_{1}$ such that
$L_{\textup{pf}}(e_{\textup{pf}},\theta,u,v)\geq\alpha_{1}(\|(e_{\textup{pf}},\theta-\theta_{\textup{end}})^{\top}\|)$.

###### Assumption 4

The terminal cost
$E_{\textup{pf}}:\mathbb{R}^{n_{\textup{x}}}\times\mathbb{R}^{n_{\textup{z}}}\to\mathbb{R}^{+}_{0}$
is positive semi-definite and cont. differentiable in $x$ and $z$, and the
terminal set $\mathcal{F}_{\textup{pf}}\subseteq\mathcal{X}\times\mathcal{Z}$
is closed. Furthermore, for all
$(\tilde{x},\tilde{z})^{\top}\in\mathcal{F}_{\textup{pf}}$ there exist inputs
$(u_{\mathcal{F}},v_{\mathcal{F}})^{\top}(\cdot)\in\mathcal{U\times V}$ such
that for all $\tau\in[0,T_{\textup{s}})$

$\displaystyle\left(\begin{smallmatrix}\dfrac{\partial
E_{\textup{pf}}}{\partial x},&\dfrac{\partial E_{\textup{pf}}}{\partial
z}\end{smallmatrix}\right)\cdot\left(\begin{smallmatrix}f(x(\tau),u_{\mathcal{F}}(\tau))\\\
g(z(\tau),v_{\mathcal{F}}(\tau))\end{smallmatrix}\right)$
$\displaystyle+L_{\textup{pf}}(e_{\textup{pf}}(\tau),\theta(\tau),u_{\mathcal{F}}(\tau),v_{\mathcal{F}}(\tau))\leq
0$

and that $x(\tau)=x(\tau,\tilde{x}|u_{\mathcal{F}})$ and
$z(\tau)=z(\tau,\tilde{z}|v_{\mathcal{F}})$ stay in
$\mathcal{F}_{\textup{pf}}$, i.e., $\mathcal{F}_{\textup{pf}}$ is control
invariant.

Following [45, 44] one can establish the following result:

###### Theorem 1

If Assumptions 1-4 hold, and if the optimal control problem (4) is initially
feasible, then (4) is recursively feasible and the path-following error
$e_{\textup{pf}}$ converges to zero under sampled-data NMPC.

For details we refer to [45, 44]. We use the outlined path following MPC
scheme as a basis and adjust it in Section 5 to guarantee constraint
satisfaction/safety despite the uncertainty due to the hybrid uncertain output
model.

##  4 Gaussian processes

We propose to use Gaussian processes to learn the unknown output mapping
$h_{\text{ml}}$. Gaussian processes (GPs) are stochastic processes that follow
an infinite-dimensional joint normal distribution [19, 20]. They have gained
increasing attention in the control community [49, 50, 51, 52, 53, 54, 35]. We
propose to use GPs, as they provide robustness against noise, inclusion of
physical knowledge, and their stochastic nature allows fitting data without
overfitting (Tasks 2.1 and 2.2). Furthermore, GPs provide a posterior
uncertainty measure to calculate a reliability bound (Task 2.3), which can be
used to achieve safe operation as outlined in Section 5.2.

###  4.1 Gaussian process force modeling

In the following we focus on modeling a part of the output/controlled
variable, e.g., the contact forces, in dependence of the state of the
system/robot. In general, multiple output parts $F\in\mathbb{R}^{n_{F}}$ with
$n_{F}>1$ can be considered. In such a case multiple one-dimensional GPs or a
multidimensional GP could be used. For dimension $i$, we assume that noisy
observations $\hat{F}_{i}$ are available that origin from

$\hat{F}_{i}=h_{\text{i}}(x)+\eta_{i}$ (6)

where the mapping $h_{i}$ (the considered force component of $h$) from the
system state $x\in\mathbb{R}^{n_{x}}$ to the measured variable
$\hat{F}_{i}\in\mathbb{R}$ is corrupted by the uncertainty or noise
$\eta_{i}$. This noise is assumed to follow an independent, identically
distributed Gaussian distribution $\eta_{i}\sim\mathcal{N}(0,\sigma_{i}^{2})$
with zero mean and variance $\sigma_{i}^{2}$.

We use a GP to model the underlying function $h_{\text{ml},i}$ in
$\tilde{h}_{i}$ which approximates $h_{i}$ via

$h_{\text{ml},i}(x)\sim\mathcal{GP}\left(m(x),\kappa\left(x,x^{\prime}\right)\right).$
(7)

Here, $m(x)\in\mathbb{R}$ denotes the mean of the GP and
$\kappa(x,x^{\prime})\in\mathbb{R}$ denotes the covariance. This covariance is
a measure of the joint variability of two random variables and the function
$\kappa:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{x}}\to\mathbb{R}$ is positive
semi-definite and symmetric. Once a prior assumption on the mean and
covariance function is posed, the training of the GP is performed to obtain
posterior distributions. The GP is trained on the data
$D:=(\boldsymbol{x},\boldsymbol{\hat{F}})$, where
$\boldsymbol{x}:=(x^{1},\ldots,x^{n_{D}})^{\top}$ and
$\boldsymbol{\hat{F}}:=(\hat{F}^{1}{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}-h_{\text{fp},i}(x^{1})},\ldots,\hat{F}^{n_{D}}{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}-h_{\text{fp},i}(x^{n_{D}})})^{\top}$.
The superscript $i$ in $x^{i},\hat{F}^{i}_{\text{n}}$ with
$i=1,2,\ldots,n_{D}$ enumerates the available measuring instances. The
training determines the hyperparameters $\phi\in\mathbb{R}^{n_{\phi}}$ of the
GP, which include the parameters of the mean and covariance functions as well
as the noise variance. The number of the hyperparameters $n_{\phi}$ depends
hereby on the assumed prior structure, i.e., on the specific choice of the
mean and covariance functions. Often an estimate of the most likely
hyperparameters is obtained via the maximization of the logarithmic marginal
likelihood [20, 19]. Given these hyperparameters, the posterior distribution
of the GP conditioned on the prior and the data can be derived. The joint
posterior distribution at previously seen data points $\boldsymbol{x}$ and at
the query point $x^{*}$ is given by

$\begin{pmatrix}\boldsymbol{\hat{F}}\\\
h_{\text{ml},i}(x^{*})\end{pmatrix}\sim\mathcal{GP}\left(\begin{pmatrix}\boldsymbol{m}(\boldsymbol{x})\\\
m(x^{*})\end{pmatrix},\begin{pmatrix}K+\sigma^{2}I&{k}\\\
{k}^{\top}&\kappa(x^{*},x^{*})\end{pmatrix}\right),$

where $\boldsymbol{m}(\boldsymbol{x}):=(m(x^{1}),\ldots,m(x^{n_{D}}))^{\top}$.
The covariance matrix $K$ specifies the covariance between all of the training
data points and is given by

$K=\begin{pmatrix}\kappa(x^{1},x^{1})&\cdots&\kappa(x^{1},x^{n_{D}})\\\
\kappa(x^{2},x^{1})&\cdots&\kappa(x^{2},x^{n_{D}})\\\ \vdots&\ddots&\vdots\\\
\kappa(x^{n_{D}},x^{1})&\cdots&\kappa(x^{n_{D}},x^{n_{D}})\end{pmatrix}.$

The cross covariance between the trainings and the query points is given by
$k:=(\kappa(x^{1},x^{*}),\kappa(x^{2},x^{*}),\ldots,\kappa(x^{n_{D}},x^{*}))^{\top}$.
Hence, the GP posterior is given by the posterior mean function
$m^{+}:\mathbb{R}^{n_{x}}\to\mathbb{R}$ that is defined by

$\displaystyle m^{+}(x^{*}):=$ $\displaystyle
m(x^{*})+{k}^{\top}(K+\sigma^{2}I)^{-1}\big{(}\boldsymbol{\hat{F}}-\boldsymbol{m}(\boldsymbol{x})\big{)}$
(8)

and the posterior covariance
$\kappa^{+}:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{x}}\to\mathbb{R}$ that is
given by

$\displaystyle\kappa^{+}(x^{*},x^{*}):=$
$\displaystyle\kappa(x^{*},x^{*})-{k}^{\top}\big{(}K+\sigma^{2}I\big{)}^{-1}{k}.$

##  5 Learning-supported Model Predictive Force and Motion control

We aim for precise and safe control of robot motions and interaction forces
under constraint satisfaction. To do so, we use GPs for hybrid modeling of the
system output (1b) while we assume negligible model plant mismatch in the
system dynamics (1a) in combination with MPC. We exploit the fact that the
”output” model uncertainty does not need to be propagated through the dynamics
of the system and can be used for constraint tightening: In MPC, predictions
are performed with the model. If the prediction model of the dynamical system
is uncertain, i.e., contains a GP, uncertainty need to be propagated through
the system and the resulting distribution is typically non-Gaussian [35]. When
the uncertainty only appears in the output projection, such uncertainty
propagation is unnecessary. Therefore, the output variance, which reflects the
uncertainty in the model, does not need to be approximated. The posterior of
the GP forms a Gaussian distribution over the full prediction horizon without
necessarily growing over time or becoming increasingly uncertain. We show how
this structure leads to a simplified stochastic MPC formulation and derive a
tailored chance-constrained formulation.

We start the discussion by considering that the model uncertainty is neglected
in the MPC controller. Afterwards, we consider how the covariance of the GPs
can be used for constraint tightening to achieve safe control via chance
constrained MPC.

###  5.1 Nominal model predictive control – neglecting the uncertainty of the
learned model

In practice, the model uncertainty is often neglected in the MPC predictions,
i.e., a nominal MPC scheme with the learned model is used. The (unknown)
output $h$ in the prediction is replaced by the learned hybrid model
$\tilde{h}(x)=h_{\textup{fp}}(x)+h_{\textup{ml}}(x)$. In case of the hybrid
model outlined in Section 6.1 we use the mean value for the prediction. The
optimal control problem (4) with the hybrid output model (3) in constraint
(4e) remains otherwise unchanged.

As a first step, we establish recursive feasibility and convergence with the
learned output model, assuming that the GP matches reality ideally, i.e.,
$\mathbb{E}\left(\tilde{h}(x)\right)=h(x)$, and that no noise is present:
$\sigma_{y}(x)=\sqrt{\kappa^{+}(x,x)}=0$. To do so, the GP part of the hybrid
model must satisfy the conditions required for Theorem 1, i.e., Assumptions
1-4. Only Assumption 2, specifically the smoothness of the output model, is
critical. The required smoothness can be ensured, posing the following
conditions on the GP part of the learned output map:

###### Assumption 5

The prior mean and covariance function $m:\mathbb{R}^{n_{x}}\to\mathbb{R}$,
$\kappa:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{x}}\to\mathbb{R}$, and the
first-principles model part
$h_{\text{fp}}:\mathbb{R}^{n_{\text{x}}}\to\mathbb{R}^{n_{\text{y}}}$ are
continuously differentiable for all $x,x^{\prime}\in\mathcal{X}$.

Provided that these conditions hold, we can establish the following lemma:

###### Lemma 1

Assumption 5 implies that the learned output function
$h:\mathbb{R}^{n_{\textup{x}}}\to\mathbb{R}^{n_{\textup{y}}}$ is continuously
differentiable and locally Lipschitz for all $x\in\mathcal{X}$.

######  Proof 5.2.

The output function $h$ is given by
$h(x)=\mathbb{E}(\tilde{h}(x))=h_{\textup{fp}}(x)+m(x)+{k}^{\top}(K+\sigma^{2}I)^{-1}\big{(}\boldsymbol{\hat{F}}-\boldsymbol{m}(\boldsymbol{x})\big{)}$,
where the entries of $K$ and ${k}$ are $K^{i,j}=\kappa(x^{i},x^{j})$ and
${k}^{i}=\kappa(x^{i},x)$ with $i,j\in\\{1,\ldots,n_{D}\\}$ cf. (3) and (8).
Differentiability of $h$ is guaranteed via differentiability of the summands.
Local Lipschitz continuity of $h$ on each compact set
$\tilde{\mathcal{X}}\subset\mathcal{X}$ follows since the restriction of any
continuously differentiable function on a compact set is Lipschitz.

######  Corollary 5.3.

Provided that Assumptions 1 \- 5 hold. Then the model predictive feedback
resulting from repeatedly solving (4) using the nominal learned output model
(3) for prediction without model-reality mismatch and noise is recursively
feasible, and the control error converges to zero.

Common prior mean and covariance functions that allow satisfying Assumption 5
are the squared exponential covariance, periodic covariances, or specific
parametrization of the Matérn covariance [20].

###  5.2 Ensuring safety – output chance constraint model predictive control

Neglecting the uncertainty in the output does not allow to ensure constraint
satisfaction. We outline how to use the posterior variance to include a
measure of the modeling errors into the controller and thus increase safety.
To do so, we reformulate the output constraints as chance constraints and
tighten them by a set which includes most of the uncertainty. Doing so
requires that the following assumption with respect to the error of the hybrid
model holds:

###### Assumption 6

There exists a finite bound ${b}_{j}:\tilde{\mathcal{X}}\to\mathbb{R}$ for
each approximation error
$\Big{(}h_{j}(x)-\mathbb{E}\big{(}\tilde{h}_{j}(x)\big{)}\Big{)}$ on a compact
set $\tilde{\mathcal{X}}\subset\mathbb{R}^{n_{\textup{x}}}$ such that
$p\left(|h_{j}(x)-\mathbb{E}(\tilde{h}_{j}(x))|>{b}_{j}(x)\right)<1-\epsilon$
for all $x\in\tilde{\mathcal{X}}$, $j\in\\{1,\ldots,n_{y}\\}$ and
$\epsilon\in(0,1)$, where $p$ denotes the probability.

Deriving such bounds is, for example, outlined in [55] and in [56]. In
practice, often the posterior variance of a GP is assumed to approximate these
error bounds ${b}_{j}$ directly, cf. [52, 38, 41]. It has been reported that
this posterior variance can lead to an underestimation of the uncertainty,
especially in multi-step-ahead predictions [50]. In our setup, no uncertainty
propagation is needed. Hence, we exploit multiples of the posterior standard
deviation as a measure for the approximation quality. Including chance
constraints for the output, the optimal control problem (4) is slightly
modified. In particular the control error equation (4e) is changed into

$\displaystyle e_{\text{pf}}(\tau)$
$\displaystyle=r_{\text{pf}}\left(l(z(\tau))\right)-\mathbb{E}\left(\tilde{h}\left(x(\tau)\right)\right).$
(9)

This way, the control error and objective function are deterministic. Also,
the output constraints in (4f) are altered into chance constraints
$p\left(\tilde{h}(x(\tau))\in\mathcal{Y}\right)\geq p_{\mathcal{Y}},$ where
$p_{\mathcal{Y}}\in(0,1)$ denotes a chosen probability for the satisfaction of
the constraints. This chance constraint can be reformulated into

$\mathbb{E}\left(\tilde{h}(x(\tau))\right)\in\tilde{\mathcal{Y}},$ (10)

where the modified constraint set $\tilde{\mathcal{Y}}$ is defined via
$\tilde{\mathcal{Y}}:=\mathcal{Y}\ominus\mathcal{R}.$ The set $\mathcal{R}$ is
constructed via the respective confidence level or error bound that belong to
the desired reliability $p_{\mathcal{Y}}$. For example,
$\mathcal{R}=[-2\sigma_{y}(x),2\sigma_{y}(x)]$ for a one dimensional output
with $p_{\mathcal{Y}}\approx 95.45$, where $\sigma_{y}$ is the standard
deviation of the learning based output $\tilde{h}(x)$. Alternatively, the
constraint tightening can consider the worst case realization of the
uncertainty over a compact set $\tilde{\mathcal{X}}$ such that
$\sigma_{y,\text{max}}:=\max\limits_{x\in\tilde{\mathcal{X}}}\sigma_{y}(x)$ is
used in the construction of $\mathcal{R}$ instead of $\sigma_{y}$. Figure 2
illustrates the constraint tightening. In both cases, we need to ensure the
existence of solutions and (initial) feasibility of the optimal control
problem with tightened constraints. Therefore, we rely on the following
assumptions.

###### Assumption 7

The tightened output set $\tilde{\mathcal{Y}}$ is closed. Furthermore, the
intersection of the state constraint set $\mathcal{X}$ and the preimage
$h^{-1}\left(\tilde{\mathcal{Y}}\right)$ of the tightened output constraint
set $\tilde{\mathcal{Y}}$ is nonempty and contains the reference
$r_{\textup{pf}}$.

$0$$1$$2$$3$$4$$5$$-1$$0$$1$$\mathcal{X}$$\tilde{h}(x)$$\tilde{\mathcal{Y}}$95%
conf.data$\mathbb{E}\left(\tilde{h}(\bar{x}(\tau))\right)$ (a) Based on the
posterior variance (gray area) of the output model the original constraints
(red dashed line) are tightened.

$0$$1$$2$$3$$4$$5$$-1$$0$$1$$\mathcal{X}$$\tilde{h}(x)$$\tilde{\mathcal{Y}}$95%
conf.data$\mathbb{E}\left(\tilde{h}(\bar{x}(\tau))\right)$ (b) Based on the
maximum value $\sigma_{y,\text{max}}$ of the posterior variance (gray area)
the original constraints (red dashed) are tightened.

Figure 2: Constraint tightening.

When considering the uncertainty in the output via tightened constraints, the
following Output Chance Constrained MPC setup is used:
Output Chance Constraint MPC: In the repeatedly solved optimal control problem
(4), the learned output model
$\tilde{h}(x)=h_{\textup{fp}}(x)+h_{\textup{ml}}(x)$ is used in the control
error formulation $e_{\textup{pf}}$ (9). Furthermore, the tightened output
constraint set $\tilde{\mathcal{Y}}$ instead of $\mathcal{Y}$ is used in (4f)
and (4h).

In comparison to the nominal case, alterations in the output constraints are
performed. In contrast to classical stochastic MPC, the state constraints
remain untouched by the uncertainty. Hence, stability/convergence results can
be established similarly as in Section 5.1. The assumption on the closedness
of $\tilde{\mathcal{Y}}$ is the only assumption at stake, which is, however,
guaranteed by Assumption 7. Moreover we pose additional conditions on the
nonemptyness of the intersection of $\tilde{\mathcal{Y}}$ and
$h^{-1}\left(\tilde{\mathcal{Y}}\right)$ which is necessary for an (initial)
feasible solution to exist.

###### Observation 1

Given that Assumptions 1\- 7 hold, then the output chance constrained
predictive controller is recursively feasible with the probabilistic
satisfaction of the constraints and the control error $e_{\textup{pf}}$ from
(9) converges to zero.

Note that the constraint satisfaction of the true outputs $h(x)$ with respect
to $\mathcal{Y}$ can be guaranteed only with probability $p_{\mathcal{Y}}$.

##  6 Application example

The benefits of a learning-supported MPC force controller are shown here on a
robotic application example. The robot should follow a Cartesian reference
path. At the same time, the robot is in contact with a flexible surface and
should apply a desired force along the Cartesian reference normal to the
contact surface. As an illustrative example, we use a calligraphic writing
task, where different contact forces along the reference are desired. The y-
and z-axes of the base frame are parallel to the whiteboard surface, while the
x-axis is normal to the surface, see also Fig. 3. The position reference is a
sinusoidal curve depicted as a black line in Fig. 3 and the force reference is
indicated via a blue shaded area along the x-axis. Since the reference
consists of three dimensions, we consider the control of three joints of the
robot to obtain a square input-output structure $n_{\text{y}}=n_{\text{u}}=3$.

x y z Figure 3: Illustration of the base frame and the reference for the robot
writing task.

###  6.1 Robot and force model

The derivation of the dynamical system equation based on first principles is
given in Appendix 8. In this subsection, the hybrid output modeling is
discussed in more detail. The system outputs are given via
$y=(p_{\text{e,y}},p_{\text{e,z}},F_{\text{n}},z_{1})^{\top}$, where
$p_{\text{e,y}}$ and $p_{\text{e,y}}$ are the Cartesian end-effector (pen tip)
position in y- and z-direction. These are obtained via the direct kinematics
of the robot. The third output $F_{\textup{n}}\in\mathbb{R}$ denotes the
normal contact force between the pen and the whiteboard in x-direction of the
base frame and $\theta=z_{1}$ represents the path parameter. We assume that
sufficiently precise first-principle output models for
$p_{\text{e,y}},p_{\text{e,z}}$ exist. In contrast, the contact forces also
depend on the robots surroundings and can vary in their complexity
arbitrarily. Therefore, the modeling of the contact forces is performed with a
hybrid model. We use a simple first-principle models to capture the basic
physical knowledge and enhance extrapolation. It is supported by a Gaussian
process to increase the model accuracy and flexibility. To underline the
benefits of the hybrid model (increased model accuracy and uncertainty
quantification for robustification), three different modeling schemes are
investigated and compared in the following.

$\delta$$F_{\text{n}}$end-
effectorenvironment$p_{\text{e}}=h_{\text{fk}}(q)$$p_{0}$ Figure 4: The
penetration depth $\delta$ is the distance between the end-effector pose
$p_{\text{e}}=h_{\text{fk}}(q)$ and the initial contact position $p_{0}$ along
the normal of the environment surface. The resulting normal force
$F_{\text{n}}$ is modeled as a function of $\delta$.

Among the purely elastic models, which can be used to quantify the
interaction, are the linear spring model of Hook

$F_{\text{n,Hk}}:=h_{F_{\text{n,Hk}}}(x)=K_{\text{e}}\delta$ (11)

and the nonlinear spring model of Hertz

$F_{\text{n,Hz}}:=h_{F_{\text{n,Hz}}}(x)=K_{\text{e}}\delta^{\alpha}$ (12)

where $K_{\text{e}}\in\mathbb{R}$ is a spring constant and the coefficient
$\alpha\in\mathbb{R}_{0}^{+}$ introduces nonlinearity and is equal to 1.5 in
the original work of Hertz. The penetration depth $\delta$ is shown in Fig. 4.
As the third model, we propose a hybrid model composed of a linear spring
model and a GP via $h_{\text{ml}}(x)\in\mathbb{R}$. It is given by

$F_{\text{n,HkGP}}:=h_{F_{\text{n,HkGP}}}(x)=K_{\text{e}}h_{\delta}(q)+h_{\text{ml}}(x).$
(13)

Figure 5: Comparison of linear (left), nonlinear (middle), and hybrid force
model (right) plotted over the Cartesian x-direction (top), y-direction
(middle), and z-direction (bottom).

#### Training data

The parameters of the first-principle models are estimated on data that was
collected by a baseline controller. This baseline controller is outlined in
[13] for a similar control task. Multiple runs of the controller are performed
in the vicinity of the original reference. The full available data set
consists of 20000 data points, where each data point represents the measured
joint angle positions and the corresponding contact force, as depicted in Fig.
5 as gray crosses. The training data used for the GP consisted of 85 data
points, which is a subset of the evaluation data set. These points are shown
in Fig. 5 (right hand side) as black crosses. They have been chosen with
$0.015\text{\,}\mathrm{r}\mathrm{a}\mathrm{d}$ minimum Euclidean distance in
terms of the angular positions between each other to obtain an equally spread
coverage of the considered space.

#### Modeling and learning results

Fig. 5 plots from top to bottom the measured and modeled forces over the
Cartesian x-, y-, and z-directions of the base frame. Additionally to the
data, the results for the linear spring (Fig. 5, left), the nonlinear spring
(Fig. 5, middle) and the hybrid model (Fig. 5, right) are depicted. An
optimization-based identification of the parameters resulted in $K_{e}=341.56$
for the linear, and $K_{e}=2.5276\cdot 10^{8}$, $\alpha=3.7651$ for the
nonlinear model. The latter was parameterized using a two-stage identification
procedure[57]. The hybrid model combines the linear model with a Gaussian
process with zero prior mean and squared exponential prior covariance
function. Since, the board surface is assumed to be placed parallel to the
y-z-plane of the robots base frame, the penetration depth is defined along the
x-axis with a assumed initial contact position of -0.49m.

As can be seen in Fig. 5 (left), the linear model (solid gray line) is a poor
approximation of the measured forces. The root mean square error between the
linear force model output and the data is $1.06\text{\,}\mathrm{N}$. The
nonlinear force model (black dashed line) approximates the interaction forces
better than the linear one, cf. Fig. 5, middle. The corresponding root mean
square error is $0.91\text{\,}\mathrm{N}$. The hybrid model can also take the
non-perfect alignment of the whiteboard as well as small unevenness and
stiffness changes of it into account. This can also be seen in Fig. 5, middle
and bottom. While the contact forces are often underestimated by the linear
model, the nonlinear spring model sometimes overestimates them, see Fig. 5
(center) in $y\in\left[0.16,0.22\right]$ or Fig. 5 (bottom middle) in
$z\in\left[0.66,0.7\right]$. In contrast, the hybrid model nicely fits the
data without overfitting its noise. It shows a root mean square error of
$0.41\text{\,}\mathrm{N}$, which corresponds to an error reduction of
$61\text{\,}\%$ compared to the linear spring model and of $55\text{\,}\%$
compared to the nonlinear model.

###  6.2 Controller setup

Using the hybrid model, a model predictive controller is implemented. The
simulations of the system are performed in MATLAB and the optimal control
problem is solved using ACADO [58]. In the experiments, the fast research
interface establishes the communication between the KUKA lightweight robot and
the work station PC [59]. Joint torques are sent over the interface as inputs
to the robot and the robot’s inbuild joint position and velocity sensors are
used to measure the states which are sent back to the PC.  A CAN-bus builds
the connection to the wrist force-torque sensor [60], which provides the
contact force data. The communication interface for the control of the robot
from MATLAB was designed in [61]. The sensor interface is outlined in [62].
Figure 6 describes this overall setup. For the following simulations and
experiments the same cost function and control parameters are used, which are
given in Appendix 9.

Can busFRIMatlab +ACADOWork station PCKUKALightweightRobot IVBarrett F/T Wrist
Sensor Figure 6: Linux Work station PC with Ubuntu 12.04 and an Intel Xeon(R)
X5675 processor with 3.07 GHz x6 connected to the KUKA robot via the fast
research interface (FRI) and to the force/torque sensor via can bus.  Figure
7: Cartesian end-effector position in simulations. Figure 8: Virtual system in
simulation. The path parameter $\theta$ (top) is constrained to non-positive
values (red area depicts constraints) while the path speed (represented by
$\dot{\theta}$, bottom) is restricted to non-negative values for forward
motion. Figure 9: Contact force in simulation. Simulated force (solid blue
line) and predictions inside the optimal control problem (thin gray solid
line). The dark red area depicts original constraints, medium red shows
constraint tightening based on state dependent variances, light red area
depicts shrunken constraints for state-independent over-approximation of
uncertainty.  Figure 10: Comparison of control performance for using original
constraints and tightened constraints when disturbance occurs.

###  6.3 Simulation results

#### Learning-supported hybrid force control simulation

First simulations are performed following a reference path. In these
simulations, no model-plant mismatch in the dynamic equations is assumed.
Instead, a simulated mismatch in the contact forces is considered. To do so, a
purely data based force model using 515 data points was trained. Since this
model is using more than 6 times as much data as the hybrid GP, it gives an
even better representation of the true contacts. Note that the computational
demand for GP inference grows in general cubically with the amount of data
points. Thus, such a big GP model is neither suited for fast online
simulations nor for the use in a real-time optimal controller. Nevertheless,
it allows us to test model-plant mismatch in simulations.

The Cartesian end-effector position in this simulation in the y-z-plane can be
seen in Fig. 7. The reference path is shown in black dashed line, while the
simulated robots position is depicted as solid blue line. To achieve an
optimal tracking performance, the model predictive controller is adjusting the
reference evolution. The virtual system states are depicted in Fig. 8. The
path parameter $\theta$ starts at $-1$ and moves toward its end value $0$
without exceeding it. The non-admissible area, i.e., the area where the
constraints are violated, is depicted as a red-shaded area. The path parameter
derivative is shown in Fig. 8, bottom. As can be seen, the path-following
control formulation leads to a nontrivial velocity profile. It enables optimal
path following while considering limitations, e.g., in the joint velocities.

The evolution of the contact force over time is depicted in Fig. 9. The model-
plant mismatch in the output equation leads to a deviation of the achieved
contact force from its reference, cf. Fig. 9. Even though the MPC prediction
model claims that the contact force is close to the reference (thin gray
line), the true contact force (blue solid line) deviates from it. The original
force constraint (dark red area) might be violated due to this model-plant
mismatch. To cope for this model-plant mismatch the original force constraint
is adjusted using the posterior variance of the GP. The reliability of the
hybrid GP model along the motion is calculated via the 2$\sigma_{y}$
confidence bound. It is tightening the original constraint
[$0\text{\,}\mathrm{N}$ $6\text{\,}\mathrm{N}$] , which is indicated by the
medium red area. An over-approximation of this uncertainty related tightening
is performed, which is indicated by the light red area in Fig. 9. The
resulting robustly tightened box constraints are [$0.7\text{\,}\mathrm{N}$
$5.3\text{\,}\mathrm{N}$]. The MPC controls the system such that the force
model predictions stay inside the feasible area (white space). Hence, the true
contact force (blue solid line) stays inside the original constraints of
[$0\text{\,}\mathrm{N}$ $6\text{\,}\mathrm{N}$] with a chosen probability.

Figure 11: Comparison of controllers with first-principle and hybrid force
model in simulation. Figure 12: Comparison of controllers with first-principle
and hybrid force model in experiment.

#### Disturbance rejection

Additionally, a disturbed case is considered to underline the effect of the
constraint tightening. In this case an additive input disturbance at the
fourth joint of $-1.3\text{\,}\mathrm{N}\mathrm{m}$ occurs between
$t=$8\text{\,}\mathrm{s}$$ and $t=$9\text{\,}\mathrm{s}$$. The effect on the
contact force is illustrated in Fig. 10. The resulting contact force without
constraint tightening is depicted in the left subplot. Additionally, the
disturbed simulation results with constraint tightening are plotted in Fig. 10
right. For both cases, the prediction used inside the controller is plotted as
black thin line. As can be seen in Fig. 10, the disturbance pushes the robot
away from the board between $t=$8\text{\,}\mathrm{s}$$ and
$t=$9\text{\,}\mathrm{s}$$, which leads to smaller forces. The hybrid GP model
(thin black line) overestimates the contact forces in this area, such that it
calculates minimum forces of $0.1\text{\,}\mathrm{N}$ in the untightened case.
However, the simulated force of the large GP (blue line) is falling below
zero, i.e., indicating that the robot has lost contact. Hence, the original
lower constraint of $0\text{\,}\mathrm{N}$ is violated, due to the model-plant
mismatch. In contrast, the tightened constraints prevent from this contact
loss, cf. Fig 10 right. They become active at around
$t=$8.5\text{\,}\mathrm{s}$$, limit the occurring force error, and introduce a
safety margin such that the true force (blue line) obeys the original force
constraints, cf. Fig. 10, right. Hence, a safe and stable contact between the
robot and the whiteboard is guaranteed at least with the chosen probability
despite the occurring disturbance.

#### Comparison to first-principle model-based controllers

Additional simulation studies are performed to compare the closed loop control
performance of the learning-supported controller with controllers using the
first-principle models. To do so, the same controller parameters are used as
listed in Section 6.2 for all simulations. The difference between the three
controller setups compared in this section lies mainly in the force output
model. The linear first-principle model, the nonlinear first-principle model
and the hybrid model from Section 6.1 are used, respectively. Fig. 11 shows
the system outputs for the linear spring model (Hk), the nonlinear force model
(Hz) and the hybrid model (Hk+GP). As can be seen, the influence of the
different force models on the performance in the position controlled subspace
(Cartesian x- and y-direction) is comparably small. A clear difference between
the controller performances can be seen in the force controlled output, cf.
Fig. 11 left. The model-plant mismatch in the linear case (depicted as gray
solid line) is so large, that a constraint violation of the maximum and
minimum force limit occurs. In the nonlinear case, all constraints on the
contact forces are satisfied and the root mean square force control error is
$0.77\text{\,}\mathrm{N}$. Still, the hybrid case outperforms the nonlinear
force model MPC, cf. Fig. 11. The controller with hybrid model shows a root
mean square error for the force of $0.25\text{\,}\mathrm{N}$. This corresponds
to a force error reduction of $91.3\text{\,}\%$ compared to the MPC with
linear force model and $67.5\text{\,}\%$ with respect to the MPC with
nonlinear first-principle force model.

###  6.4 Experimental validation

Figure 13: Total calculation time $T_{\text{calc}}$ (blue) in each sampling
instance and its main components, the sensor communication delay
$T_{\text{sens}}$ and the calculation time of the optimal control problem
$T_{\text{OCP}}$.

Figure 13 underlines that the controller is real-time feasible, showing the
computation time of the experimental implementation. Real time feasibility is
given due to the total calculation time $T_{\text{calc}}$ (blue) being smaller
than the sampling time of $T_{\text{s}}=$0.01\text{\,}\mathrm{s}$$ at all
times. The controlled system outputs for the experimental validation of the
learning-supported model predictive controller are shown in Fig. 12 in blue
dash-dotted lines. For comparison, the MPC with linear and nonlinear spring
force models are depicted in Fig. 12 in gray solid and black dashed lines,
respectively. Compared to the simulation results, an additional model-plant
mismatch in the dynamics as well as additional noise in the sensor readings
occur. Therefore, all controllers show larger control errors in the
experiments compared to the simulation studies. Due to the large model-plant
mismatch in the linear force model, the controller even became unstable, cf.
Fig. 12 gray line. The MPCs with nonlinear and hybrid force model perform
better than the linear case. Their performance in the position controlled
subspace is comparable, see Fig. 12 (middle and right). Comparing their force
control errors shows that the learning-supported controller shows around
$12\text{\,}\%$ smaller maximum errors and around $14\text{\,}\%$ smaller
average control errors. Hence, the superior force tracking performance of the
learning-supported controller over the first-principle based MPC is not only
valid in simulations but also in experiments. Moreover, the MPC with nonlinear
first-principle force model violates the maximum force constraints, see black
line in Fig. 12 (left). The GP not only approximates the real forces more
accurately, but also enables the variance-based constraint tightening for
increased safety.

##  7 Conclusion

This paper proposes a learning-supported model predictive controller for
hybrid position and force control. It exploits that the stochastic uncertainty
affect only the static model parts, e.g., mappings from states to the
controlled variables, and not the dynamics itself. Driven by the increasing
demand for sensitive, and interactive, yet safe robots, we propose a model-
and data-based controller that directly controls the forces of the robot along
with its motion. The model predictive controller considers directly
constraints on inputs, states, and outputs. This way, constraints for
positions, motions, and forces are satisfied. A path following predictive
controller is used, where the reference speed is adjusted during runtime to
provide an additional degree of freedom to the MPC. A Gaussian process
supports the controller with learned hybrid force models. The presented
approach achieves improved approximation quality compared to standard first-
principle force models and additionally improves the closed-loop system
performance. Moreover, additional robustness is achieved by using the
stochastic uncertainty description of the Gaussian process force model. It
builds the basis for constraint tightening that allows for safe and reliable
contacts even in case of disturbances and model-plant mismatch.

Further research focuses on improving the controller performance via online
updates of the force models. Furthermore, changing environmental conditions,
such as moving objects or cooperating robots, should be addressed in future
research. For example, learning from past measurements/batches might allow to
build up an increasing set of possible models that allow transfering and
adapting knowledge between different situations. Moreover, online adaptation
and learning is promising, yet leads to challenges such as a priori unknown
constraints that we aim to address in future works, for example using the
approach presented in [63].

##  8 Model of the considered light-weight robot

The model of the robotic manipulator is derived based on first principles
using the Lagrangian formulation [64] leading to

$\displaystyle\left(\begin{smallmatrix}\dot{x}_{1}\\\ \small{\vdots}\\\
\dot{x}_{n_{\text{q}}}\\\ \dot{x}_{n_{\text{q}}+1}\\\ \vdots\\\
\dot{x}_{2n_{\text{q}}}\end{smallmatrix}\right)=\underbrace{\left(\begin{smallmatrix}x_{n_{\text{q}}+1}\\\
\vdots\\\ x_{2n_{\text{q}}}\\\ \\\ B^{-1}(x_{1},\hbox
to5.69054pt{.\hss.\hss.},x_{n_{\text{q}}})\left(u-J^{\top}(x_{1},\hbox
to5.69054pt{.\hss.\hss.},x_{n_{\text{q}}})F-N(x)\right)\\\
\phantom{B}\end{smallmatrix}\right)}_{f(x,u)}$ $\displaystyle
N(x)=C(x)\left(\begin{smallmatrix}x_{n_{\text{q}}+1}\\\ \vdots\\\
x_{2n_{\text{q}}}\end{smallmatrix}\right)+\tau_{\text{f}}(x_{n_{\text{q}}+1},\hbox
to5.69054pt{.\hss.\hss.},x_{2n_{\text{q}}})+\tau_{\text{g}}(x_{1},\hbox
to5.69054pt{.\hss.\hss.},x_{n_{\text{q}}}).$

Here, $x_{1},\ldots,x_{n_{\text{q}}}$ denote the joint angles, while
$x_{n_{\text{q}}+1},\ldots,x_{2n_{\text{q}}}$ represent the joint angle
velocities. For the example, we use joint one, two and four of the lightweight
robot depicted in Figure 1. Hence, $n_{\text{q}}=3$ and
$x=(q_{1},q_{2},q_{4},\dot{q}_{1},\dot{q}_{2},\dot{q}_{4})^{\top}$, where
$q_{i}$ denotes the angle of the $i$th joint. The configuration dependent
inertia matrix of the robot is denoted by
$B:\mathbb{R}^{n_{\text{q}}}\to\mathbb{R}^{n_{\text{q}}\times n_{\text{q}}}$.
The input $u$ represents the joint actuation torques. The Coriolis and
centrifugal effects are captured by
$C:\mathbb{R}^{n_{\text{q}}}\times\mathbb{R}^{n_{\text{q}}}\to\mathbb{R}^{n_{\text{q}}\times
n_{\text{q}}}$. While the gravitational force
$\tau_{\text{g}}:\mathbb{R}^{n_{\text{q}}}\to\mathbb{R}^{n_{\text{q}}}$ for a
rigid link manipulator depends only on joint positions, the friction torque
$\tau_{\text{f}}:\mathbb{R}^{n_{\text{q}}}\to\mathbb{R}^{n_{\text{q}}}$ can be
modeled via viscous and Coulomb friction such that is depends on joint
velocities. We consider contact of the robot with the environment at the end-
effector. The contact forces and moments occurring at the end-effector in a
three-dimensional Cartesian space are captured by $F\in\mathbb{R}^{6}$. They
can be mapped to the corresponding joint torques via
$\tau_{\text{ext}}=J(q)^{\top}F$ with the manipulator Jacobian
$J:\mathbb{R}^{n_{\text{q}}}\to\mathbb{R}^{6\times n_{\text{q}}}$. The model
parameters are taken from [61], supplemented by the modifications presented in
[62] to account for the used force-torque sensor. The standard deviation
$\sigma$ of the noise is approximately $0.0388\text{\,}\mathrm{N}$. This
dynamical model is used for simulations as well as for prediction in the MPC.
In contrast to [61], $N(x)=0$ is considered, as we use gravity compensation
provided by the internal KUKA controller. Furthermore, friction and Coriolis
effects only have a minor influence for small velocities, as considered here.
Finally, a compensation of external torques, e.g., due to the environment
contact, outside of the predictive controller is added to the optimal inputs.
For the virtual system dynamics, a double integrator $\dot{z}_{1}=z_{2}$,
$\dot{z}_{2}=v$ is used with virtual input $v$ and virtual output
$z_{1}=\theta$.

##  9 Controller parameters

A quadratic cost function is used, where:
$L_{\textup{pf}}=(e_{\textup{pf}}^{\top},\theta)Q(e_{\textup{pf}}^{\top},\theta^{\top})^{\top}+(u^{\top},v)R(u^{\top},v)^{\top}$,
$E_{\textup{pf}}=(x^{\top},z^{\top})Q_{\textup{E}}(x^{\top},z^{\top})^{\top}$.
The weightings are $Q=\text{diag}(9\cdot 10^{6},9\cdot 10^{6},6,1\cdot
10^{2})$, $R=\text{diag}(6,6,6,6)$,
$Q_{\textup{E}}=\text{diag}(0,0,0,0,0,0,1\cdot 10^{2},0)$. The prediction
horizon spans $T=$150\text{\,}\mathrm{m}\mathrm{s}$$, while the sampling time
is $T_{\text{s}}=$10\text{\,}\mathrm{m}\mathrm{s}$$.

The state constraints are
$\mathcal{X}=[-170^{\circ},170^{\circ}]\times[-120^{\circ},120^{\circ}]\times[-120^{\circ},120^{\circ}]\times[-0.04\text{\,rad\,}\text{s}^{-1},0.04\text{\,rad\,}\text{s}^{-1}]\times[-1\text{\,rad\,}\text{s}^{-1},1\text{\,rad\,}\text{s}^{-1}]\times[-0.05\text{\,rad\,}\text{s}^{-1},0.03\text{\,rad\,}\text{s}^{-1}]$.
The virtual states in path following are constrained by
$\mathcal{Z}=[-1,0]\times[$0\text{\,}\mathrm{s}^{-1}$,$1\text{\,}\mathrm{s}^{-1}$]$.
Furthermore, box constraints on the input and virtual inputs are present:
$\mathcal{U}=[$-13\text{\,}\mathrm{N}\mathrm{m}$,$10\text{\,}\mathrm{N}\mathrm{m}$]\times[$-5\text{\,}\mathrm{N}\mathrm{m}$,$5\text{\,}\mathrm{N}\mathrm{m}$]\times[$-5\text{\,}\mathrm{N}\mathrm{m}$,$5\text{\,}\mathrm{N}\mathrm{m}$]$,
$\mathcal{V}=[$-10\text{\,}\mathrm{s}^{-2}$,$0.5\text{\,}\mathrm{s}^{-2}$]$,
while the contact force should satisfy
$[$0\text{\,}\mathrm{N}$,$6\text{\,}\mathrm{N}$]$.

## References

  * [1] E. Matheson, R. Minto, E. G. Zampieri, M. Faccio, and G. Rosati, “Human–robot collaboration in manufacturing applications: a review,” Robotics, vol. 8, no. 4, p. 100, 2019.
  * [2] J. Matschek, T. Gonschorek, M. Hanses, N. Elkmann, F. Ortmeier, and R. Findeisen, “Learning references with gaussian processes in model predictive control applied to robot assisted surgery,” in 2020 European Control Conference (ECC), pp. 362–367, IEEE, 2020.
  * [3] S. Bedaf, G. J. Gelderblom, and L. De Witte, “Overview and categorization of robots supporting independent living of elderly people: What activities do they support and how far have they developed,” Assistive Technology, vol. 27, no. 2, pp. 88–100, 2015.
  * [4] P. Maciejasz, J. Eschweiler, K. Gerlach-Hahn, A. Jansen-Troy, and S. Leonhardt, “A survey on robotic devices for upper limb rehabilitation,” Journal of Neuroengineering and Rehabilitation, vol. 11, no. 1, pp. 1–29, 2014.
  * [5] 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.
  * [6] A. Zacharaki, I. Kostavelis, A. Gasteratos, and I. Dokas, “Safety bounds in human robot interaction: A survey,” Safety science, vol. 127, p. 104667, 2020.
  * [7] L. Villani and J. De Schutter, “Force control,” in Springer Handbook of Robotics, pp. 195–220, Springer, 2016.
  * [8] M. Killpack, A. Kapusta, and C. Kemp, “Model predictive control for fast reaching in clutter,” Aut. Robots, vol. 40, no. 3, pp. 537–560, 2016.
  * [9] D. Müller, A. Mayer, and O. Sawodny, “Model predictive force control for robots in compliant environments with guaranteed maximum force,” in American Control Conf. (ACC), pp. 1355–1360, IEEE, 2019.
  * [10] B. Kocer, T. Tjahjowidodo, and G. Seet, “Model predictive UAV-tool interaction control enhanced by external forces,” Mechatronics, vol. 58, pp. 47–57, 2019.
  * [11] I. Mitsioni, Y. Karayiannidis, J. A. Stork, and D. Kragic, “Data-driven model predictive control for the contact-rich task of food cutting,” in Humanoid Robots (Humanoids), pp. 244–250, IEEE, 2019.
  * [12] T. Gold, A. Völz, and K. Graichen, “Model predictive position and force trajectory tracking control for robot-environment interaction,” in Int. Conf. on Intelligent Robots and Systems (IROS), 2020.
  * [13] J. Matschek, J. Bethge, P. Zometa, and R. Findeisen, “Force feedback and path following using predictive control: Concept and application to a lightweight robot,” in IFAC World Congress, pp. 10243–10248, 2017.
  * [14] K. Kazim, J. Bethge, J. Matschek, and R. Findeisen, “Combined predictive path following and admittance control,” in American Control Conf. (ACC), pp. 3153–3158, IEEE, 2018.
  * [15] A. Wahrburg and K. Listmann, “MPC-based admittance control for robotic manipulators,” in Conf. on Decision and Control (CDC), pp. 7548–7554, IEEE, 2016.
  * [16] M. Bednarczyk, H. Omran, and B. Bayle, “Model predictive impedance control,” in Int. Conf. on Rob. and Autom. (ICRA), pp. 4702–4708, 2020.
  * [17] T. Gold, A. Völz, and K. Graichen, “Model predictive interaction control for industrial robots,” in IFAC World Congress, pp. 10026–10033, 2020\.
  * [18] P. Flores and H. M. Lankarani, Contact Force Models for Multibody Dynamics, vol. 226.  Springer, 2016.
  * [19] J. Kocijan, Modelling and Control of Dynamic Systems using Gaussian Process Models.  Springer, 2016.
  * [20] C. Williams and C. Rasmussen, Gaussian Processes for Machine Learning.  MIT Press Cambridge, MA, 2006.
  * [21] F. Viña, Y. Bekiroglu, C. Smith, Y. Karayiannidis, D. Kragic, et al., “Predicting slippage and learning manipulation affordances through Gaussian process regression,” in Int. Conf. on Humanoid Robots (Humanoids), pp. 462–468, IEEE, 2013.
  * [22] Y. Li and B. Hannaford, “Gaussian process regression for sensorless grip force estimation of cable-driven elongated surgical instruments,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1312–1319, 2017.
  * [23] T. Horii, F. Giovannini, Y. Nagai, L. Natale, G. Metta, and M. Asada, “Contact force estimation from flexible tactile sensor values considering hysteresis by Gaussian process,” in Int. Conf. on Development and Learning and on Epigenetic Robotics, pp. 137–138, IEEE, 2014.
  * [24] J. R. Medina, H. Börner, S. Endo, and S. Hirche, “Impedance-based Gaussian processes for modeling human motor behavior in physical and non-physical interaction,” IEEE Transactions on Biomedical Engineering, vol. 66, no. 9, pp. 2499–2511, 2019.
  * [25] J. Matschek, R. Jordanowa, and R. Findeisen, “Direct robotic force control with learning supported model predictive control,” in Conf. on Control Technology and Applications (CCTA), pp. 8–13, 2020.
  * [26] G. Darivianakis, K. Alexis, M. Burri, and R. Siegwart, “Hybrid predictive control for aerial robotic physical interaction towards inspection operations,” in Int. Conf. on Robotics and Automation (ICRA), pp. 53–58, IEEE, 2014.
  * [27] C. Nadeau, A. Krupa, and J. Gangloff, “Automatic tracking of an organ section with an ultrasound probe: Compensation of respiratory motion,” in Int. Conf. on Medical Image Computing and Computer-Assisted Intervention, pp. 57–64, Springer, 2011.
  * [28] I. Lenz, R. Knepper, and A. Saxena, “DeepMPC: Learning deep latent features for model predictive control,” in Robot.: Science and Sys., 2015.
  * [29] Z. Erickson, H. Clever, G. Turk, C. Liu, and C. Kemp, “Deep haptic model predictive control for robot-assisted dressing,” in Int. Conf. on Robotics and Automation (ICRA), pp. 1–8, IEEE, 2018.
  * [30] J. Sheng and M. Spong, “Model predictive control for bilateral teleoperation systems with time delays,” in Canadian Conf. on Electrical and Computer Engineering, vol. 4, pp. 1877–1880, IEEE, 2004.
  * [31] J. S. Ladoiye, D. S. Necsulescu, and J. Sasiadek, “Control of force in surgical robots with random time delays using model predictive control,” in Int. Conf. on Informatics in Control, Automation and Robotics, pp. 407–428, Springer, 2018.
  * [32] J. Kocijan and R. Murray-Smith, “Nonlinear predictive control with a Gaussian process model,” Lecture Notes in Computer Science, vol. 3355, pp. 185–200, 2005.
  * [33] B. Likar and J. Kocijan, “Predictive control of a gas–liquid separation plant based on a Gaussian process model,” Computers & Chemical Engineering, vol. 31, no. 3, pp. 142–152, 2007.
  * [34] J. Umlauft, T. Beckers, and S. Hirche, “Scenario-based optimal control for Gaussian process state space models,” in European Control Conf. (ECC), pp. 1386–1392, 2018.
  * [35] L. Hewing, J. Kabzan, and M. Zeilinger, “Cautious model predictive control using Gaussian process regression,” IEEE Transactions on Control Systems Technology, vol. 28, no. 6, pp. 2736 –2743, 2019.
  * [36] X. Yang and J. Maciejowski, “Fault tolerant control using Gaussian Processes and Model Predictive Control,” Int. Journal of Applied Mathematics and Computer Science, vol. 25, no. 1, pp. 133–148, 2015\.
  * [37] Y. Wang, C. Ocampo-Martinez, and V. Puig, “Robust model predictive control based on Gaussian processes: Application to drinking water networks,” in European Control Conf. (ECC), pp. 3292–3297, 2015.
  * [38] R. Soloperto, M. Müller, S. Trimpe, and F. Allgöwer, “Learning-based robust model predictive control with state-dependent uncertainty,” in Conf. on Nonl. Model Predictive Control (NMPC), pp. 442–447, 2018.
  * [39] Y. Wang, C. Ocampo-Martinez, and V. Puig, “Stochastic model predictive control based on Gaussian processes applied to drinking water networks,” Control Theory & Appl., vol. 10, no. 8, pp. 947–955, 2016.
  * [40] C. Ostafew, A. Schöllig, and T. Barfoot, “Robust constrained learning-based NMPC enabling reliable mobile robot path tracking,” Int. Journal of Robotics Research, vol. 35, no. 13, pp. 1547–1563, 2016.
  * [41] A. Grancharova, J. Kocijan, and T. Johansen, “Explicit stochastic predictive control of combustion plants based on Gaussian process models,” Automatica, vol. 44, no. 6, pp. 1621–1631, 2008.
  * [42] W. Langson, I. Chryssochoos, S. Raković, and D. Q. Mayne, “Robust model predictive control using tubes,” Automatica, vol. 40, no. 1, pp. 125–133, 2004.
  * [43] B. Siciliano and L. Villani, Robot Force Control.  Springer, 2012.
  * [44] J. Matschek, T. Bäthge, T. Faulwasser, and R. Findeisen, “Nonlinear predictive control for trajectory tracking and path following: An introduction and perspective,” in Handbook of Model Predictive Control, pp. 169–198, Springer, 2019.
  * [45] T. Faulwasser and R. Findeisen, “Nonlinear model predictive control for constrained output path following,” IEEE Transactions on Automatic Control, vol. 61, no. 4, pp. 1026–1039, 2016.
  * [46] T. Faulwasser and R. Findeisen, “Nonlinear model predictive path-following control,” in Nonlinear Model Predictive Control, pp. 335–343, Springer, 2009.
  * [47] J. Rawlings, D. Mayne, and M. Diehl, Model Predictive Control: Theory, Computation, and Design.  Nob Hill Publishing Madison, WI, 2017.
  * [48] R. Findeisen, L. Imsland, F. Allgower, and B. A. Foss, “State and output feedback nonlinear model predictive control: An overview,” European Journal of Control, vol. 9, no. 2-3, pp. 190–206, 2003.
  * [49] X. Yang and J. Maciejowski, “Risk sensitive model predictive control with Gaussian process models,” in IFAC Symposium on System Identification (SYSID), pp. 374–379, 2015.
  * [50] L. Hewing, E. Arcari, L. Fröhlich, and M. N. Zeilinger, “On simulation and trajectory prediction with Gaussian process dynamics,” in Conf. on Learning for Dynamics and Control (L4DC), pp. 1341–1348, 2020.
  * [51] J. Kocijan, R. Murray-Smith, C. Rasmussen, and B. Likar, “Predictive control with Gaussian process models,” in Int. Conf. on Computer as a Tool, vol. 1, pp. 352–356, IEEE, 2003.
  * [52] G. Cao, E. Lai, and F. Alam, “Gaussian process model predictive control of an unmanned quadrotor,” Journal of Intelligent & Robotic Systems, vol. 88, no. 1, pp. 147–162, 2017.
  * [53] E. Klenske, M. Zeilinger, B. Schölkopf, and P. Hennig, “Gaussian process-based predictive control for periodic error correction,” Trans. on Control Systems Technology, vol. 24, no. 1, pp. 110–121, 2016.
  * [54] L. Hewing, A. Liniger, and M. Zeilinger, “Cautious NMPC with Gaussian process dynamics for autonomous miniature race cars,” in European Control Conf. (ECC), pp. 1341–1348, 2018.
  * [55] T. Koller, F. Berkenkamp, M. Turchetta, J. Bödecker, and A. Krause, “Learning-based model predictive control for safe exploration and reinforcement learning,” arXiv preprint: arXiv:1906.12189, 2019.
  * [56] A. Lederer, J. Umlauft, and S. Hirche, “Uniform error bounds for Gaussian process regression with application to safe control,” in Advances in Neural Information Processing Systems, pp. 659–669, 2019.
  * [57] N. Diolaiti, C. Melchiorri, and S. Stramigioli, “Contact impedance estimation for robotic systems,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 925–935, 2005.
  * [58] D. Ariens, H. J. Ferreau, B. Houska, and F. Logist, “ACADO for Matlab User’s Manual.” http://acado.sourceforge.net/doc/pdf/acado_matlab_manual.pdf, Sept. 2020\.
  * [59] G. Schreiber, A. Stemmer, and R. Bischoff, “The fast research interface for the KUKA lightweight robot,” in Workshop on Innovative Robot Control Architectures for Demanding (Research) Applications (ICRA), pp. 15–21, 2010\.
  * [60] “Barrett Technology Inc. Barrett hand and force/torque sensor.” https://advanced.barrett.com/barretthand, Sept. 2022.
  * [61] V. Bargsten, P. Zometa, and R. Findeisen, “Modeling, parameter identification and model-based control of a lightweight robotic manipulator,” in Int. Conf. on Control Applications (CCA), pp. 134–139, IEEE, 2013.
  * [62] J. Bethge, “Entwurf und Implementierung einer modellprädiktiven Kraft- und Pfadverfolgungsregelung für Kontaktszenarien in der Robotik,” Master’s thesis, OvGU Magdeburg, Germany, 2016.
  * [63] I. Batkovic, M. Ali, P. Falcone, and M. Zanon, “Safe trajectory tracking in uncertain environments,” arXiv preprint arXiv:2001.11602, 2020.
  * [64] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Modelling, Planning and Control.  Springer Science & Business Media, 2010.

[]Janine Matschek obtained her bachelor’s and master’s degrees in systems
engineering and engineering cybernetics from Otto von Guericke University
Magdeburg, Germany, in 2010 and 2013, respectively. In 2021, she completed her
Ph.D. in the field of model predictive control and machine learning at the
University Magdeburg, Germany. She joined the Technical University of
Darmstadt, Darmstadt, Germany, as a postdoctoral researcher in 2022. Her
research interests include machine learning for predictive control and control
in medical and industrial robotic applications. Dr. Matschek received the
award for the best dissertation of the faculty of electrical engineering at
the Otto von Guericke University Magdeburg, Germany, in 2021.

[]Johanna Bethge studied systems engineering and engineering cybernetics at
Otto von Guericke University Magdeburg in Germany and Tampere University of
Technology in Finland. She received her bachelor’s and master’s degrees in
2013 and 2016, respectively, from Otto von Guericke University Magdeburg.
Since 2016, she has been a research assistant at the same university and is
currently pursuing her Ph.D. Her research interests include learning-supported
model predictive control and safety guarantees under uncertainty for multi-
mode systems, such as autonomous vehicles. Johanna Bethge is a fellow of the
Research Training Group for Mathematical Complexity Reduction (GRK 2297) of
the German Research Foundation (DFG).

[]Rolf Findeisen earned a Diploma degree in Engineering Cybernetics from the
University of Stuttgart, an M.S. degree from the University of Wisconsin-
Madison in 1997, and his Ph.D. degree from the University of Stuttgart in
2005.

From 2007 to 2021, Rolf served as a full professor and led the Systems Theory
and Automatic Control Laboratory at the Otto von Guericke University
Magdeburg. Since August 2021, he has been heading the Control and Cyber-
Physical Systems Laboratory at the Technical University of Darmstadt. Rolf and
his group have published extensively in international journals, and he has
served as an editor and associate editor for various prestigious publications,
including IEEE Control Systems Magazine, IEEE Transactions on Networked
Systems, J. Optimal Control Applications, and Methods and Processes. He also
served as the IPC Co-Chair of the IFAC World Congress 2020.

Rolf’s research focuses on the interplay between control and machine learning,
autonomous systems, predictive control, cyber-physical systems, uncertainty,
and robustness. His group’s work has a wide range of applications, including
robotics, autonomous driving, mechatronics, biotechnology, systems biology,
battery and energy systems.