# Model Error Propagation via Learned Contraction Metrics for
Safe Feedback Motion Planning of Unknown Systems

Glen Chou, Necmiye Ozay, and Dmitry Berenson1 1Electrical Engineering and
Computer Science, University of Michigan, Ann Arbor, MI, {gchou, necmiye,
<EMAIL_ADDRESS>
###### Abstract

We present a method for contraction-based feedback motion planning of locally
incrementally exponentially stabilizable systems with unknown dynamics that
provides probabilistic safety and reachability guarantees. Given a dynamics
dataset, our method learns a deep control-affine approximation of the
dynamics. To find a trusted domain where this model can be used for planning,
we obtain an estimate of the Lipschitz constant of the model error, which is
valid with a given probability, in a region around the training data,
providing a local, spatially-varying model error bound. We derive a trajectory
tracking error bound for a contraction-based controller that is subjected to
this model error, and then learn a controller that optimizes this tracking
bound. With a given probability, we verify the correctness of the controller
and tracking error bound in the trusted domain. We then use the trajectory
error bound together with the trusted domain to guide a sampling-based planner
to return trajectories that can be robustly tracked in execution. We show
results on a 4D car, a 6D quadrotor, and a 22D deformable object manipulation
task, showing our method plans safely with learned models of high-dimensional
underactuated systems, while baselines that plan without considering the
tracking error bound or the trusted domain can fail to stabilize the system
and become unsafe.

##  I Introduction

Provably safe motion planning algorithms for unknown systems are critical for
deploying robots in the real world. While planners are reliable when the
system dynamics are known exactly, the dynamics often may be poorly modeled or
unknown. To address this, data-driven methods (i.e. model-based reinforcement
learning) learn the dynamics from data and plan with the learned model.
However, such methods can be unsafe, in part because the planner can and will
exploit errors in the learned dynamics to return trajectories which cannot
actually be tracked on the real system, leading to unpredictable, unsafe
behavior when executed. Thus, to guarantee safety, it is of major interest to
establish a bound on the error that the true system may see when attempting to
track a trajectory planned with the learned dynamics, and to use it to guide
the planning of robustly-trackable trajectories.

One key property of learned dynamics models is that they have varying error
across the state space: they should be more accurate on the training data, and
that accuracy should degrade when moving away from it. Thus, the model error
that the system will see in execution will depend on the domain that it
visits. This reachable domain also depends on the tracking controller; for
instance, a poor controller will lead to the system visiting a larger set of
possible states, and thus experiencing a larger possible model error. To
analyze this, we need a bound on the trajectory tracking error for a given
disturbance description (a tracking tube). In this paper, we consider tracking
controllers based on contraction theory. Introduced in [1] for autonomous
systems and extended to the control-affine case in [2], control contraction
theory studies the incremental stabilizability of a system, making it uniquely
suited for obtaining trajectory tracking tubes under disturbance. In the past,
tracking tubes have been derived for contraction-based controllers under
simple uniform disturbance bounds [3] (i.e. a UAV subject to wind with a known
uniform upper bound). However, these assumptions are ill-suited for handling
learned model error. Assuming a uniform disturbance bound over the space can
be highly conservative, since the large model error far from the training data
would yield enormous tracking tubes, rendering planning entirely infeasible.
To complicate things further, obtaining an upper bound on the model error can
be challenging, as we only know the value of the error on the training data.

To address this gap, we develop a method for safe contraction-based motion
planning with learned dynamics models. In particular, our method is designed
for high-dimensional neural network (NN) learned dynamics models, and provides
probabilistic guarantees on safety and goal reachability for the true system.
Our core insight is that we can derive a tracking error bound for a
contraction-based controller under a spatially-varying model error
description, and that we can use this error bound to bias planning towards
regions in the state/control space where trajectories can be more robustly
tracked. We summarize our contributions as:

  * •

A trajectory tracking error bound for contraction-based controllers subjected
to a spatially-varying, Lipschitz constant-based model error bound that
accurately reflects the learned model error.

  * •

A deep learning framework for joint learning of dynamics, control contraction
metrics (CCMs), and contracting controllers that are approximately optimized
for planning performance under this model error description.

  * •

A sampling-based planner that returns plans which can be safely tracked under
the learned dynamics/controller.

  * •

Evaluation of our method on learned dynamics up to 22D, and demonstrating that
it outperforms baselines.

##  II Related Work

Our work is related to contraction-based control of uncertain systems: [3]
applies contraction to feedback motion planning for systems with a known
disturbance bound, while [4, 5] apply contraction to adaptive control under
known model uncertainty structure, i.e. the uncertainty lies in the range of
known basis functions. In this paper, the uncertainty arises from the error
between the true dynamics and a learned NN approximation, which lacks such
structure. It is also only known at certain states (the training data), making
the disturbance bound a priori unknown and nontrivial to obtain. These methods
use sum-of-squares (SoS) optimization to find CCMs, which apply to moderate-
dimensional polynomial systems [3], and cannot be used for NN models. Thus,
[6], [7] model the CCM as an NN and learn it from data, assuming known
dynamics subjected to disturbances with known uniform upper bound. Our method
differs by learning the dynamics and CCM together to optimize planning
performance under model error. Also related is [8], which learns a CCM with
the dynamics, but does not consider how the model error affects tracking.

More broadly, our work is related to safe learning-based control. Many methods
learn stability certificates for a single equilbirium point [9, 10], but this
is not especially useful for point-to-point motion planning. Other methods use
Gaussian processes to bound the reachable tube of a trajectory [11] or safely
explore a set [12, 13], but these methods assume a feedback controller is
provided; we do not, as we learn a CCM-based controller. [14] directly learns
tracking tubes around trajectories, so plans must remain near trajectories
seen in training to be accurate; our method only requires plans to be near
state/controls seen in training. Finally, perhaps most relevant is [15], which
plans safely with learned dynamics by obtaining a tube around a plan inside a
“trusted domain”. A restrictive key assumption of [15] is that the unknown
system has as many control inputs as states. We remove this assumption in this
work, requiring fundamental advancements in the method of [15], i.e. in
deriving a new tracking bound, controller, trusted domain, and planner.

##  III Preliminaries and Problem Statement

###  III-A System models, notation, and differential geometry

We consider deterministic unknown continuous-time nonlinear systems
$\dot{x}=h(x,u)$, where
$h:\mathcal{X}\times\mathcal{U}\rightarrow\mathcal{X}$,
$\mathcal{X}\subseteq\mathbb{R}^{n_{x}}$, and
$\mathcal{U}\subseteq\mathbb{R}^{n_{u}}$. We define
$g:\mathcal{X}\times\mathcal{U}\rightarrow\mathcal{X}$ to be a control-affine
approximation of the true dynamics:

$g(x,u)=f(x)+B(x)u.$ (1)

While we do not assume that the true dynamics are control-affine, we do assume
that they are locally incrementally exponentially stabilizable, that is, there
exists a $\beta$, $\lambda>0$, and feedback controller such that
$\|x^{*}(t)-x(t)\|\leq\beta e^{-\lambda t}\|x^{*}(0)-x(0)\|$ for all solutions
$x(t)$ in a domain. Many underactuated systems satisfy this, and it is much
weaker than requiring $n_{x}=n_{u}$, as in [15]. Also, this only needs to hold
in a task-relevant domain $D$, defined later.

For a function $\eta$, a Lipschitz constant over a domain $\mathcal{Z}$ is any
$L$ such that for all $z_{1},z_{2}\in\mathcal{Z}$,
$\|\eta(z_{1})-\eta(z_{2})\|\leq L\|z_{1}-z_{2}\|$. Norms $\|\cdot\|$ are
always the 2-norm. We define $L_{h-g}$ as the smallest Lipschitz constant of
the error $h-g$. The argument of $h-g$ is a state-control pair $(x,u)$ and its
value is a state. We define a ball $\mathcal{B}_{r}(x)$ as
$\\{y\enspace|\enspace\|y-x\|<r\\}$, also referred to as a $r$-ball about $x$.
We suppose the state space $\mathcal{X}$ is partitioned into safe
$\mathcal{X}_{\textrm{safe}}$ and unsafe $\mathcal{X}_{\textrm{unsafe}}$ sets
(e.g., collision states). We denote $\widehat{Q}\doteq Q+Q^{\top}$ as a
symmetrization operation on matrix $Q$, and $\bar{\lambda}(\hat{Q})$ and
$\underline{\lambda}(\hat{Q})$ as its maximum and minimum eigenvalues,
respectively. We overload notation when $Q(x)$ is a matrix-valued function,
denoting
$\bar{\lambda}_{\mathcal{Q}}(Q)\doteq\sup_{x\in\mathcal{Q}}\bar{\lambda}(Q(x))$
and
$\underline{\lambda}_{\mathcal{Q}}(Q)\doteq\inf_{x\in\mathcal{Q}}\underline{\lambda}(Q(x))$.
Let $\mathbf{I}_{n}$ be the identity matrix of size $n\times n$. Let
$\mathbb{S}_{n}^{>0}$ denote the set of symmetric, positive definite $n\times
n$ matrices. Let the Lie derivative of a matrix-valued function
$Q(x)\in\mathbb{R}^{n\times n}$ along a vector $y\in\mathbb{R}^{n}$ be denoted
as $\partial_{y}Q(x)\doteq\sum_{i=1}^{n}y^{i}\frac{\partial Q}{\partial
x^{i}}$. Let $x^{i}$ denote the $i$th element of vector $x$. Let the notation
$Q_{\perp}(x)$ refer to a basis for the null-space of matrix $Q(x)$.

Finally, we introduce the needed terminology from differential geometry. For a
smooth manifold $\mathcal{X}$, a Riemannian metric tensor
$M:\mathcal{X}\rightarrow\mathbb{S}_{n_{x}}^{>0}$ equips the tangent space
$T_{x}\mathcal{X}$ at each element $x$ with an inner product
$\delta_{x}^{\top}M(x)\delta_{x}$, providing a local length measure. Then, the
length $l(c)$ of a curve $c:[0,1]\rightarrow\mathcal{X}$ between points
$c(0)$, $c(1)$ can be computed by integrating the local lengths along the
curve: $l(c)\doteq\int_{0}^{1}\sqrt{V(c(s),c_{s}(s))}ds$, where for brevity
$V(c(s),c_{s}(s))\doteq c_{s}(s)^{\top}M(c(s))c_{s}(s)$, and
$c_{s}(s)\doteq\partial c(s)/\partial s$. Then, the Riemann distance between
two points $p,q\in\mathcal{X}$ can be defined as
$\textrm{dist}(p,q)\doteq\inf_{c\in\mathcal{C}(p,q)}l(c)$, where
$\mathcal{C}(p,q)$ is the set of all smooth curves connecting $p$ and $q$.
Finally, we define the Riemann energy between $p$ and $q$ as
$\mathcal{E}(p,q)\doteq\textrm{dist}^{2}(p,q)$.

Figure 1: Method flowchart. Left: First, we learn a model of the dynamics
using dataset $\mathcal{S}$ and obtain a contracting controller for this
learned model (Prob. 1). Center: Next, within a trusted domain $D$, we verify
the correctness of the controller, bound the model error, and bound the
trajectory tracking error under this model error (Prob. 2). Right: Finally, we
use the error bounds to plan trajectories within $D$ that can be safely
tracked in execution (Prob. 3).

###  III-B Control contraction metrics (CCMs)

Contraction theory studies how the distance between trajectories of a system
changes with time to infer properties on incremental stability. This can be
formalized with a contraction metric
$M(x):\mathcal{X}\rightarrow\mathbb{S}_{n_{x}}^{>0}$ to measure if the
differential distances between trajectories
$V(x,\delta_{x})=\delta_{x}^{\top}M(x)\delta_{x}$ shrink with time. Control
contraction metrics (CCMs) adapt this analysis to control-affine systems (1).
For dynamics of the form (1), the differential dynamics can be written as
$\dot{\delta}_{x}=(\frac{\partial f}{\partial
x}+\sum_{i=1}^{n_{u}}u^{i}\frac{\partial B^{i}}{\partial
x})\delta_{x}+B(x)\delta_{u}$ [3], where $B^{i}(x)$ is the $i$th column of
$B(x)$. Then, we call $M(x):\mathcal{X}\rightarrow\mathbb{S}_{n_{x}}^{>0}$ a
CCM if there exists a differential controller $\delta_{u}$ such that the
closed-loop system satisfies $\dot{V}(x,\delta_{x})<0$, for all $x$,
$\delta_{x}$.

How do we find a CCM $M(x)$ ensuring the existence of $\delta_{u}$? First,
define the dual metric $W(x)\doteq M^{-1}(x)$. Then, two sufficient conditions
for contraction are (2)-(2c) [3, 6]:

$\small\dot{M}(x)+\savestack{\tmpbox}{\stretchto{\scaleto{\scalerel*[\widthof{M(x)(A(x)+B(x)K(\tilde{x},x^{*},u^{*}))}]{\kern-0.6pt\bigwedge\kern-0.6pt}{\rule[-505.89pt]{3.87498pt}{505.89pt}}}{}}{0.5ex}}\stackon[1pt]{M(x)(A(x)+B(x)K(\tilde{x},x^{*},u^{*}))}{\tmpbox}+2\lambda
M(x)\prec 0$ (2c)

where $A\doteq\frac{\partial f}{\partial
x}+\sum_{i=1}^{n_{u}}u^{i}\frac{\partial B^{i}}{\partial x}$ and
$K=\frac{\partial u(\tilde{x},x^{*},u^{*})}{\partial x}$, where
$u:\mathcal{X}\times\mathcal{X}\times\mathcal{U}\rightarrow\mathcal{U}$ is a
feedback controller which takes as input the tracking deviation
$\tilde{x}(t)\doteq x(t)-x^{*}(t)$ from a nominal state $x^{*}(t)$, as well as
a state/control $x^{*}(t)$, $u^{*}(t)$ on the nominal state/control trajectory
that is being tracked $x^{*}:[0,T]\rightarrow\mathcal{X}$,
$u^{*}:[0,T]\rightarrow\mathcal{U}$. We refer to the LHSs of
(LABEL:eq:strong_con) and (2c) as $C^{s}(x)$ and
$C^{w}(\tilde{x},x^{*},u^{*})$, respectively. Intuitively,
(LABEL:eq:strong_con) is a contraction condition simplified by the
orthogonality condition (LABEL:eq:strong_orth), which together imply that all
directions where the differential dynamics lack controllability must be
naturally contracting at rate $\lambda$. The conditions (2) are stronger than
(2c), which does not make this orthogonality assumption.

How do we recover a tracking feedback controller $u(\tilde{x},x^{*},u^{*})$
for (1) from (2) and (2c)? For (2), the controller is implicit in the dual
metric $W(x)$, and can be computed by solving a nonlinear optimization
problem, which can be solved at runtime with pseudospectral methods [3, 16].
In (2c), $u(\tilde{x},x^{*},u^{*})$ is directly involved as the function
defining $K$; as a consequence, $M(x)$ and $u(\tilde{x},x^{*},u^{*})$ both
need to be found. Thus for our purposes, the benefit of using (2) is that we
have fewer parameters to learn. However, as some systems may not satisfy the
properties needed to apply (2), we resort to using (2c) in these cases (see
Sec. IV-B2). Finally, for a given CCM $M(x)$ and associated controller
$u(\tilde{x},x^{*},u^{*})$, for an unperturbed system tracking a nominal
trajectory $x^{*}(t)$, the Riemannian energy $\mathcal{E}(x^{*}(t),x(t))$
satisfies $\|x(t)-x^{*}(t)\|\leq\beta\|x(0)-x^{*}(0)\|e^{-\lambda t}$ for an
overshoot constant $\beta$, and thus the Euclidean distance also decays at
this rate. If the system is subjected to bounded perturbations, it is instead
guaranteed to remain in a tube around $x^{*}(t)$.

###  III-C Problem statement

Our method has three major components. First, we learn a model (1), and then
learn a contraction metric $M(x)$ and/or controller $u(\tilde{x},x^{*},u^{*})$
for (1). Next, we analyze the learned (1), $M(x)$, and/or
$u(\tilde{x},x^{*},u^{*})$ to determine a trusted domain
$D\subseteq\mathcal{X}\times\mathcal{U}$ where trajectories can be robustly
tracked. Finally, we design a planner which steers between states in $D$, such
that under the tracking controller $u(\tilde{x},x^{*},u^{*})$, the system
remains safe in execution and reaches the goal. In this paper, we represent
the approximate dynamics $g(x,u)$ with an NN, though our method is agnostic to
the structure of the model and how it is derived. Let
$\mathcal{S}=\\{(x_{i},u_{i},h(x_{i},u_{i}))\\}_{i=1}^{N}$ be the training
data for $g$ obtained by any means (i.e. random sampling, expert
demonstrations, etc.), and let
$\Psi=\\{(x_{j},u_{j},h(x_{j},u_{j}))\\}_{j=1}^{M}$ be a set of independent
and identically distributed (i.i.d.) samples collected near $\mathcal{S}$.
Then, our method involves solving the following:

######  Problem 1 (Learning).

Given $\mathcal{S}$, learn a control-affine model $g$, a contraction metric
$M(x)$, and find a contraction-based controller $u(\tilde{x},x^{*},u^{*})$
that satisfies (2) or (2c) over $\mathcal{S}$.

######  Problem 2 (Analysis).

Given $\Psi$, $g$, $M(x)$, and $u(\tilde{x},x^{*},u^{*})$, design a trusted
domain $D$. In $D$, find a model error bound $\|h(x,u)-g(x,u)\|\leq e(x,u)$,
for all $(x,u)\in D$, and verify if for all $x\in D$, $M$ and $u$ are valid,
i.e. satisfying (2)/(2c).

######  Problem 3 (Planning).

Given $g$, $M(x)$, $u(\tilde{x},x^{*},u^{*})$, start $x_{I}$, goal $x_{G}$,
goal tolerance $\mu$, maximum tracking error tolerance $\hat{\mu}$, trusted
domain $D$, and $\mathcal{X}_{\textrm{safe}}$, plan a nominal trajectory
$x^{*}:[0,T]\rightarrow\mathcal{X}$, $u^{*}:[0,T]\rightarrow\mathcal{U}$ under
the learned dynamics $g$ such that $x(0)=x_{I}$, $\dot{x}=g(x,u)$,
$\|x(T)-x_{G}\|\leq\mu$, and $x(t)$, $u(t)$ remains in
$D\cap\mathcal{X}_{\textrm{safe}}$ for all $t\in[0,T]$. Also, guarantee that
in tracking $(x^{*}(t),u^{*}(t))$ under the true dynamics $h$ with
$u(\tilde{x},x^{*},u^{*})$, the system remains in
$D\cap\mathcal{X}_{\textrm{safe}}$ and reaches
$\mathcal{B}_{\hat{\mu}+\mu}(x_{G})$.

##  IV Method

We first describe a spatially-varying, Lipschitz constant-based model error
bound, and derive a trajectory tracking error bound for a CCM-based controller
under this error description (Sec. IV-A). We then show how we can learn a
dynamics model, CCM, and tracking controller to optimize the tracking error
bound (Sec. IV-B). Then we show how we can design a trusted domain $D$ and
verify the correctness of the model error bound, tracking error bound, and
controller within $D$ (Sec. IV-C). Finally, we show how this tracking bound
can be embedded into a sampling-based planner to ensure that plans provably
remain safe in execution and reach the goal (Sec. IV-D). We summarize our
method in Fig. 1.

###  IV-A CCM-based tracking tubes under Lipschitz model error

We first establish a spatially-varying bound on model error within a trusted
domain $D$ which can be estimated from the model error evaluated at training
points. For a single training point $(\bar{x},\bar{u})$ and a novel point
$(x,u)$, we can bound the error between the true and learned dynamics at
$(x,u)$ using the triangle inequality and Lipschitz constant of the error
$L_{h-g}$:

$\displaystyle\|h($ $\displaystyle x,u)-g(x,u)\|$ (2d) $\displaystyle\leq
L_{h-g}\|(x,u)-(\bar{x},\bar{u})\|+\|h(\bar{x},\bar{u})-g(\bar{x},\bar{u})\|.$

As this holds between the novel point and all training points, the following
(possibly) tighter bound can be applied:

$\displaystyle\|h(x,u)-g(x,u)\|\leq\min_{1\leq i\leq N}$
$\displaystyle\Big{\\{}L_{h-g}\|(x,u)-(x_{i},u_{i})\|$ (2e)
$\displaystyle+\|h(x_{i},u_{i})-g(x_{i},u_{i})\|\Big{\\}}.$

To exploit higher model accuracy near the training data, we define $D$ as the
union of $r$-balls around $\mathcal{S}$, where $r<\infty$:

$D=\bigcup_{i=1}^{N}\,\mathcal{B}_{r}(x_{i},u_{i}).$ (2f)

For these bounds to hold, $L_{h-g}$ must be a valid Lipschitz constant over
$D$. In Sec. IV-C, we discuss how to obtain a probabilistically-valid estimate
of $L_{h-g}$ and how to choose $r$. We now derive an upper bound
$\bar{\epsilon}(t)$ on the Euclidean tracking error $\epsilon(t)$ around a
nominal trajectory $(x^{*}(t),u^{*}(t))\subseteq D$ for a given metric $M(x)$
and feedback controller $u(\tilde{x},x^{*},u^{*})$, such that the executed and
nominal trajectories $x(t)$ and $x^{*}(t)$ satisfy
$\|x(t)-x^{*}(t)\|\leq\bar{\epsilon}(t)$, for all $t\in[0,T]$, when subject to
the model error description (2e). In Sec. IV-B, we discuss how $M$ and $u$ can
be learned from data.

In [3], it is shown that by using a controller which is contracting with rate
$\lambda$ according to metric $M(x)$ for the nominal dynamics (1), the
Riemannian energy $\mathcal{E}(t)$ of a perturbed control-affine system
$\dot{x}(t)=f(x(t))+B(x(t))u(t)+d(t)$ is bounded by the following differential
inequality:

$D^{+}\mathcal{E}(t)\leq-2\lambda\mathcal{E}(t)+2\sqrt{\mathcal{E}(t)\bar{\lambda}_{D}(M)}\|d(t)\|,$
(2g)

where $\bar{\lambda}_{D}(M)=\sup_{x\in D}\bar{\lambda}(M(x))$ and
$D^{+}(\cdot)$ is the upper Dini derivative of $(\cdot)$. Here, the energy
$\mathcal{E}(t)=\mathcal{E}(x^{*}(t),x(t))$ is the squared trajectory tracking
error according to the metric $M(x)$ at a given time $t$, and $d(t)$ is an
external disturbance. Suppose that the only disturbance to the system comes
from the discrepancy between the learned and true dynamics, i.e.
$d(t)=h(x(t),u(t))-g(x(t),u(t))$111In addition to model error, we can also
handle runtime external disturbances with a known upper bound; we assume the
training data is noiseless.. For short, let
$e_{i}\doteq\|h(x_{i},u_{i})-g(x_{i},u_{i})\|$ be the training error of the
$i$th data-point. In this case, we can use (2e) to write:

$\|d(t)\|\leq\min_{1\leq i\leq
N}\Bigg{\\{}L_{h-g}\Bigg{\|}\begin{bmatrix}x(t)\\\
u(t)\end{bmatrix}-\begin{bmatrix}x_{i}\\\
u_{i}\end{bmatrix}\Bigg{\|}+e_{i}\Bigg{\\}}.$ (2h)

As (2h) is spatially-varying, it suggests that in solving Prob. 3, plans
should stay near low-error regions to encourage low error in execution.
However, (2h) is only implicit in the plan, depending on the state visited and
feedback control applied in execution: $x(t)=x^{*}(t)+\tilde{x}(t)$ and
$u(\tilde{x}(t),x^{*}(t),u^{*}(t))=u^{*}(t)+u_{\textrm{fb}}(t)$. To derive a
tracking bound that can directly inform planning, we first introduce the
following lemma:

######  Lemma 1.

The Riemannian energy $\mathcal{E}(t)$ of the perturbed system
$\dot{x}(t)=f(x(t))+B(x(t))u(t)+d(t)$, where $\|d(t)\|$ satisfies (2h),
satisfies the differential inequality (2m), where
$\underline{\lambda}_{D}(M)=\inf_{x\in D}\underline{\lambda}(M(x))$,
$\bar{u}_{\textrm{fb}}(t)$ is a time-varying upper bound on the feedback
control $\|u(t)-u^{*}(t)\|$, and $i^{*}(t)$ achieves the minimum in (2h).

###### Proof sketch.

We use the triangle inequality to simplify (2h):

$\footnotesize\begin{split}\|d(t)\|\leq\min_{1\leq i\leq
N}\Bigg{\\{}L_{h-g}\Bigg{(}\Bigg{\|}\begin{bmatrix}x^{*}(t)\\\
u^{*}(t)\end{bmatrix}-\begin{bmatrix}x_{i}\\\
u_{i}\end{bmatrix}\Bigg{\|}+\Bigg{\|}\begin{bmatrix}\tilde{x}(t)\\\
u_{\textrm{fb}}(t)\end{bmatrix}\Bigg{\|}\Bigg{)}+e_{i}\Bigg{\\}}\\\ \leq
L_{h-g}\Bigg{\|}\begin{bmatrix}\tilde{x}(t)\\\
u_{\textrm{fb}}(t)\end{bmatrix}\Bigg{\|}+\min_{1\leq i\leq
N}\Bigg{\\{}L_{h-g}\Bigg{\|}\begin{bmatrix}x^{*}(t)\\\
u^{*}(t)\end{bmatrix}-\begin{bmatrix}x_{i}\\\
u_{i}\end{bmatrix}\Bigg{\|}+e_{i}\Bigg{\\}}.\end{split}$

Note that as $\|d(t)\|$ depends on $\tilde{x}(t)$, the disturbance bound
itself depends on $\epsilon(t)$. To make this explicit, we use
$\|\tilde{x}(t)\|=\epsilon(t)$ and
$\|u_{\textrm{fb}}(t)\|\leq\bar{u}_{\textrm{fb}}(t)$ to obtain

$\begin{array}[]{>{\displaystyle}c
>{\displaystyle}l}\|d(t)\|\leq&L_{h-g}\big{(}\epsilon(t)+\bar{u}_{\textrm{fb}}(t)\big{)}+\\\
&\min_{1\leq i\leq N}\Bigg{\\{}L_{h-g}\Bigg{\|}\begin{bmatrix}x^{*}(t)\\\
u^{*}(t)\end{bmatrix}-\begin{bmatrix}x_{i}\\\
u_{i}\end{bmatrix}\Bigg{\|}+e_{i}\Bigg{\\}}.\end{array}$ (2k)

To obtain $\bar{u}_{\textrm{fb}}(t)$, if we use CCM conditions (2), we can use
the optimization-based controller in [3] (c.f. Sec. III-B), which admits the
upper bound [3, p.28]:

$\small\|u_{\textrm{fb}}(t)\|\leq\epsilon(t)\sup_{x\in
D}\frac{\bar{\lambda}(L(x)^{-\top}F(x)L(x)^{-1})}{2\underline{\sigma}_{>0}(B^{\top}(x)L(x)^{-1})}\doteq\epsilon(t)\delta_{u},$
(2l)

where $W(x)=L(x)^{\top}L(x)$,
$F(x)=-\partial_{f}W(x)+\savestack{\tmpbox}{\stretchto{\scaleto{\scalerel*[\widthof{\frac{\partial
f(x)}{\partial
x}W(x)}]{\kern-0.6pt\bigwedge\kern-0.6pt}{\rule[-505.89pt]{3.87498pt}{505.89pt}}}{}}{0.5ex}}\stackon[1pt]{\frac{\partial
f(x)}{\partial x}W(x)}{\tmpbox}+2\lambda W(x)$, and
$\underline{\sigma}_{>0}(\cdot)$ is the smallest positive singular value. If
we instead use condition (2c), we must estimate $\bar{u}_{\textrm{fb}}(t)$ for
the learned controller (c.f. Sec. IV-C).

To obtain the result, we plug (2k) into (2g) after relating $\epsilon(t)$ with
$\mathcal{E}(t)$. Since
$\mathcal{E}(x^{*}(t),x(t))=\textrm{dist}^{2}(x^{*}(t),x(t))\geq\underline{\lambda}_{D}(M)\|x^{*}(t)-x(t)\|^{2}$,
we have that
$\epsilon(t)\leq\sqrt{\mathcal{E}(t)/\underline{\lambda}_{D}(M)}$. Finally, we
can plug all of these components into (2g) to obtain (2m), where $i^{*}(t)$
denotes a minimizer of (2h). ∎

$D^{+}\mathcal{E}(t)\leq-2\Bigg{(}\lambda-
L_{h-g}\sqrt{\frac{\bar{\lambda}_{D}(M)}{\underline{\lambda}_{D}(M)}}\Bigg{)}\mathcal{E}(t)+2\sqrt{\mathcal{E}(t)\bar{\lambda}_{D}(M)}\Bigg{(}L_{h-g}\Bigg{(}\Bigg{\|}\begin{bmatrix}x^{*}(t)\\\
u^{*}(t)\end{bmatrix}-\begin{bmatrix}x_{i^{*}(t)}\\\
u_{i^{*}(t)}\end{bmatrix}\Bigg{\|}+\bar{u}_{\textrm{fb}}(t)\Bigg{)}+e_{i^{*}(t)}\Bigg{)}$
(2m)

For intuition, let us interpret the spatially-varying disturbance bound (2k).
Note that (2k) depends on several components. First, it depends on
$\epsilon(t)$, which in turn relies on the disturbance magnitude: intuitively,
this is because with better tracking performance, the system will visit a
smaller set of states and thus experience lower worst-case model error.
Second, it depends on $u_{\textrm{fb}}(t)$: if a large feedback is applied,
the combined control input $u(t)=u^{*}(t)+u_{\textrm{fb}}(t)$ can be far from
the control inputs that the learned model is trained on, possibly leading to
high error. Finally, it is driven by the model error via closeness to the
training data and the corresponding training error (minimization term of
(2k)). We can also gain some insight by comparing our derived tracking error
bound (2m) with the tracking bound for a uniform disturbance description (2g).
Notice that the “effective” contraction rate $\lambda-
L_{h-g}\sqrt{\frac{\bar{\lambda}_{D}(M)}{\underline{\lambda}_{D}(M)}}$ shrinks
with $L_{h-g}$, as the model error grows with tracking error. If the
optimization-based controller [3] is used, the $\epsilon(t)$ dependence of
(2l) reduces this rate to $\lambda-
L_{h-g}\sqrt{\frac{\bar{\lambda}_{D}(M)}{\underline{\lambda}_{D}(M)}}(1+\delta_{u})$.
Now, we are ready to obtain the tracking bound:

######  Theorem 1 (Tracking bound under (2h)).

Let $\mathcal{E}_{\textrm{RHS}}$ denote the RHS of (2m). Assuming that the
perturbed system $\dot{x}(t)=f(x(t))+B(x(t))u(t)+d(t)$ satisfies
$\mathcal{E}(t_{1})\leq\mathcal{E}_{t_{1}}$ and $\|d(t)\|$ satisfies (2h).
Then, $\bar{\epsilon}(t)$ is described at some $t_{2}>t_{1}$ as:

$\bar{\epsilon}(t_{2})=\sqrt{\big{(}\textstyle\int_{\tau=t_{1}}^{t_{2}}\mathcal{E}_{\textrm{RHS}}(t)d\tau\big{)}/\underline{\lambda}_{D}(M)},\quad\mathcal{E}(t_{1})=\mathcal{E}_{t_{1}}.$
(2n)

###### Proof sketch.

We apply the Comparison Lemma [17] on (2m). To use the Comparison Lemma, the
right hand side of (2m) must be Lipschitz continuous in $\mathcal{E}$ and
continuous in $t$ [17]. Lipschitz continuity in $\mathcal{E}$ holds for (2m)
since it only contains a linear and a square-root term involving
$\mathcal{E}$, each with finite coefficients; continuity in $t$ follows by
noting that the minimization in (2k) is the pointwise minimum of $N$
continuous functions of $t$, and thus is itself a continuous function of $t$.
∎

Note that Thm. 1 provides a Euclidean tracking error tube under the model
error bound (2h) for any nominal trajectory. Moreover, as (2n) can be
integrated incrementally in time, it is well-suited to guide planning in an
RRT (Rapidly-exploring Random Tree [18]); see Sec. IV-D for more details.

###  IV-B Optimizing CCMs and controllers for the learned model

Having derived the tracking error bound, we discuss our solution to Prob. 1,
i.e. how we learn the control-affine dynamics (1), a contraction metric
$M(x)$, and (possibly) a stabilizing controller $u$ in a way that optimizes
the size of (2n). In this paper, we represent $f(x)$, $B(x)$, $M(x)$, and
$u(\tilde{x},x^{*},u^{*})$ as deep neural networks.

Ideally, we would learn the dynamics jointly with the contraction metric to
minimize the size of the tracking tubes (2n). However, we observe this leads
to poor learning, generally converging to a valid CCM for highly inaccurate
dynamics. Instead, we elect to use a simple two step procedure: we first learn
$g$, and then fix $g$ and learn $M(x)$ and $u(\tilde{x},x^{*},u^{*})$ for that
model. While this is sufficient for our examples, in general an alternation
procedure may be helpful.

Dynamics learning. Inspecting (2m), we note that the model-error related terms
are the Lipschitz constant $L_{h-g}$ and training error $e_{i}$,
$i=1,\ldots,N$. Thus, in training the dynamics, we use a loss function
penalizing the mean squared error and a batch-wise estimate of the Lipschitz
constant:

$\small
L_{\textrm{dyn}}=\frac{1}{N_{b}}\sum_{i=1}^{N_{b}}e_{i}^{2}+\alpha_{1}\max_{1\leq
i,j\leq
N_{b}}\Bigg{\\{}\frac{\|e_{i}-e_{j}\|}{\|(x_{i},u_{i})-(x_{j},u_{j})\|}\Bigg{\\}},$
(2o)

where $e_{i}=\|g(x_{i},u_{i})-h(x_{i},u_{i})\|$, $N_{b}\leq N$ is the batch
size, and $\alpha_{1}$ trades off the objectives. Note that (2o) promotes
$e_{i}$ to be small while remaining smooth over the training data, in order to
encourage similar properties to hold over $D$.

CCM learning. We describe two variants of our learning approach, depending on
if the stronger CCM conditions (LABEL:eq:strong_con) and
(LABEL:eq:strong_orth) or the weaker condition (2c) is used.

####  IV-B1 Using (LABEL:eq:strong_con) and (LABEL:eq:strong_orth)

We parameterize the dual metric as
$W(x)=W_{\theta_{w}}(x)^{\top}W_{\theta_{w}}(x)+\underline{w}\mathbf{I}_{n\times
n}$, where $W_{\theta_{w}}(x)\in\mathbb{R}^{n_{x}\times n_{x}}$, $\theta_{w}$
are the NN weights, and $\underline{w}$ is a minimum eigenvalue
hyperparameter. This structure ensures that $W(x)$ is symmetric positive
definite for all $x$. To enforce (LABEL:eq:strong_con), we follow [6],
relaxing the matrix inequality to an unconstrained penalty
$L_{\textrm{NSD}}^{s}$ over training data, where:

$L_{\textrm{NSD}}^{(\cdot)}=\max_{1\leq i\leq
N_{b}}\bar{\lambda}\big{(}C^{(\cdot)}(x_{i})\big{)}.$ (2p)

As we ultimately wish (LABEL:eq:strong_con) to hold everywhere in $D$, we can
use the continuity in $x$ of the maximum eigenvalue
$\bar{\lambda}(\lambda^{s}(x))$ to verify if (LABEL:eq:strong_con) holds over
$D$ (c.f. Sec. IV-C). However, the equality constraints (LABEL:eq:strong_orth)
are problematic; by using unconstrained optimization, it is difficult to even
satisfy (LABEL:eq:strong_orth) on the training data, let alone on $D$. To
address this, we follow [8] by restricting the dynamics learning to sparse-
structured $B(x)$ of the form, where $\theta_{B}$ are NN parameters:

$B(x)=[\mathbf{0}_{n_{x}-n_{u}\times n_{u}}^{\top},\ \
B_{\theta_{B}}(x)^{\top}]^{\top}.$ (2q)

Restricting $B(x)$ to this form implies that to satisfy
(LABEL:eq:strong_orth), $W(x)$ must be a function of only the first
$n_{x}-n_{u}$ states [8], which can be satisfied by construction. When this
structural assumption does not hold, we use the method in Sec. IV-B2

In addition to the CCM feasibility conditions, we introduce novel losses to
optimize the tracking tube size (2n). As (2n) depends on the nominal
trajectory, it is hard to optimize a tight upper bound on the tracking error
independent of the plan. Instead, we maximize the effective contraction rate,

$\small L_{\textrm{opt}}^{s}=\alpha_{2}\max_{1\leq i\leq N_{b}}\Big{(}\lambda-
L_{h-g}\sqrt{\frac{\bar{\lambda}(M(x_{i}))}{\underline{\lambda}(M(x_{i}))}}(1+\delta_{u}(\tilde{x}_{i}))\Big{)},$
(2r)

where $\alpha_{2}$ is a tuned parameter. Optimizing (2r) while ensuring
(LABEL:eq:strong_con) holds over the dataset is a challenging task for
unconstrained NN optimizers. To ameliorate this, we use a linear penalty on
constraint violation and switch to a logarithmic barrier [19] to maintain
feasibility upon achieving it. Let the combination of the logarithmic barrier
and the linear penalty be denoted $\texttt{logb}(\cdot)$. Then, the full loss
function can be written as
$\texttt{logb}(-L_{\textrm{NSD}}^{s})+L_{\textrm{opt}}^{s}$.

####  IV-B2 Using (2c)

For systems that do not satisfy (2q), we must use the weaker contraction
conditions (2c). In this case, we cannot use the optimization-based
controllers proposed in [3], and we instead learn $u(\tilde{x},x^{*},u^{*})$
in tandem with $M(x)$. As in (2p), we enforce (2c) by relaxing it to
$L_{\textrm{NSD}}^{w}$. We represent $u(\tilde{x},x^{*},u^{*})$ with the
following structure:

$u(\tilde{x},x^{*},u^{*})=|\theta_{1}^{u}|\tanh\big{(}u_{\theta_{2}^{u}}(\tilde{x},x^{*})\tilde{x}\big{)}+u^{*},$
(2s)

where $\theta_{i}^{u}$ are NN weights. The structure of (2s) simplifies
$\bar{u}_{\textrm{fb}}$ estimation, since
$\|u(\tilde{x},x^{*},u^{*})-u^{*}\|<|\theta_{1}^{u}|$ for all $x$, $x^{*}$,
$u^{*}$. We define $L_{\textrm{opt}}^{w}$ as in (2r), without the $\delta_{u}$
term. Then, our full loss function is
$\texttt{logb}(-L_{\textrm{NSD}}^{w})+L_{\textrm{opt}}^{w}+\alpha_{3}|\theta_{1}^{u}|$.

We note that since the optimizer can reach a local minima, we may not find a
valid CCM even if one exists. Some strategies we found to improve training
reliability were the log-barrier and by gradually increasing $\alpha_{2}$ and
$\alpha_{3}$ with the training epoch. If we can find a valid CCM on
$\mathcal{S}$, we can verify if it is also valid over $D$, as we now discuss.

###  IV-C Designing and verifying the trusted domain

The validity of the tracking bound (2n) depends on having overestimates of
$L_{h-g}$, $\bar{\lambda}_{D}(M)$, and $\delta_{u}$, an underestimate of
$\underline{\lambda}_{D}(M)$, and the validity of (LABEL:eq:strong_con)/(2c)
over $D$. In this section, we describe our solution to Prob. 2, showing how to
design a $D$ and how to estimate these constants over $D$.

First, let us consider how to estimate the constants over a given $D$. To
obtain probabilistic over/under-estimates of these constants that are each
valid with a user-defined probability $\rho$, we use a stochastic approach
from extreme value theory. For brevity, we describe the basics and refer to
[15, p.3], [20] for details. This approach estimates the maximum of a function
$\eta(z)$ over a domain $\mathcal{Z}$ by collecting $N_{s}$ batches of i.i.d.
samples of $z\in\mathcal{Z}$ of size $N_{b}$, and evaluating
$\\{s_{j}\\}_{j=1}^{N_{s}}\doteq\\{\max_{1\leq i\leq
N_{b}}\eta(z_{i}^{j})\\}_{j=1}^{N_{s}}$ to obtain $N_{s}$ samples of empirical
maxima. If the true maximum is finite and the distribution of sampled $s_{j}$
converges with increasing $N_{s}$, the Fisher-Tippett-Gnedenko (FTG) theorem
[21] dictates that the samples converge to a Weibull distribution. This can be
empirically verified by fitting a Weibull distribution to the $s_{j}$, and
validating the quality of the fit with a Kolmogorov-Smirnov (KS) goodness-of-
fit test [22]. If this KS test passes, the location parameter of the fit
Weibull distribution, adjusted with a confidence interval which scales in size
with the value of the user-defined probability $\rho$, can serve as an
estimate for the maximum which overestimates the true maximum with probability
$\rho$.

To estimate $L_{h-g}$, we follow [15] to obtain a probabilistic overestimate
of $L_{h-g}$ by defining $\mathcal{Z}=D$ and $\eta$ to be the slopes between
pairs of points drawn i.i.d. from $D$. This approach can also be used as
follows to estimate $\bar{\lambda}_{D}(M)$, $-\underline{\lambda}_{D}(M)$, and
$\delta_{u}$. Since the eigenvalues of a continuously parameterized matrix
function are continuous in the parameter [23] (here, the parameter is $x$) and
$D$ is bounded, these constants are finite, so by the FTG theorem, we can
expect the samples $s_{j}$ to be Weibull. Hence, we can estimate these
constants by defining $\mathcal{Z}=\textrm{proj}_{x}(D)$, where
$\textrm{proj}_{x}(D)\doteq\bigcup_{\bar{x}\in\mathcal{S}}\mathcal{B}_{r}(\bar{x})\supset\\{x\mid\exists
u,(x,u)\in D\\}$, and by setting $\eta$ appropriately for each constant.
Finally, FTG can also verify (LABEL:eq:strong_con) and (2c), since the
verification is equivalent to ensuring
$\sup_{x\in\textrm{proj}_{x}(D)}\bar{\lambda}(C^{(\cdot)}(x))\leq\lambda_{\textrm{CCM}}^{(\cdot)}$
for some $\lambda_{\textrm{CCM}}^{(\cdot)}<0$. $\lambda_{\textrm{CCM}}^{s}$
can be estimated by setting $\mathcal{Z}=\textrm{proj}_{x}(D)$ and
$\eta(x)=\bar{\lambda}(C^{s}(x))$. To estimate $\lambda_{\textrm{CCM}}^{w}$,
we set $\mathcal{Z}=\mathcal{B}_{\epsilon_{\textrm{max}}}(0)\times D$ and
$\eta(\tilde{x},x^{*},u^{*})=\bar{\lambda}(C^{w}(\tilde{x},x^{*},u^{*}))$, and
sample $(x^{*},u^{*})\in D$ and
$\tilde{x}\in\mathcal{B}_{\epsilon_{\textrm{max}}}(0)$. Here,
$\epsilon_{\textrm{max}}\leq\hat{\mu}$ will upper-bound the allowable tracking
tube size during planning (c.f. Alg. 1, line 8); thus, to ensure that planning
is minimally constrained, $\epsilon_{\textrm{max}}$ should be selected to be
as large as possible while maintaining $\lambda_{\textrm{CCM}}^{w}<0$. As all
samples are i.i.d., the probability of (2n) holding, and thus the overall
safety probability assured by our method, is the product of the user-selected
$\rho$ for each constants.

Before moving on, we note that other than for $L_{h-g}$, the estimation
procedure does not affect data-efficiency, as it queries the learned dynamics
and requires no new data of the form $(x,u,h(x,u))$. Moreover, some methods
[24, 25] deterministically give guaranteed upper bounds on the Lipschitz
constant of NNs, and can be used to estimate all constants except $L_{h-g}$.
We do not use these methods due to their scalability issues, but as further
work is done in this area, these methods may also become applicable.

Figure 2: Left: an example of the trusted domain $D$, with the dataset
$\mathcal{S}$ being shown as black dots. Note that a careful choice of $r$ is
needed; for a slightly smaller $r$ than that shown in the figure, the upper
and lower portions of $D$ will become disconnected, leading to plan
infeasibility. Right: An example of LMTCD-RRT in action. Regions in $D$ that
are shaded darker blue have smaller model error $\|d\|$; lighter shades have
higher error $\|d\|$. The orange extension to the pink branch of the RRT is
rejected, since the tube around that extension (dark magenta) exits $D$ and
intersects with the obstacle; the larger size of this tube results from the
pink branch traveling through higher error regions. In contrast, the cyan
branch (lower) accepts the yellow candidate extension, as its corresponding
tube (dark cyan) remains inside $D$ and collision-free; the smaller tube sizes
reflect that the blue branch has traveled through lower-error regions. This
type of behavior biases the planner to ultimately return a path that travels
through lower-error regions.

Finally, we discuss how to select $r$, which determines $D$ (Fig. 2, left). A
reasonable choice of $r$ is one that is maximally permissive for planning,
during which we will need to ensure that the tracking tube around the planned
trajectory remains entirely in $D$ (c.f. Sec. IV-D for more details). However,
finding this $r$ is non-trivial to achieve and requires trading off many
factors. For large $r$, planning may become easier since this increases the
size of $D$; however, model error and $L_{h-g}$ also degrades with increased
$r$, which may make $\bar{\epsilon}(t)$ and $\bar{u}_{\textrm{fb}}(t)$ grow,
which in turn grows the tracking tube size, making it harder to fit the tube
within $D$. Also, (LABEL:eq:strong_con)/(2c) may not be satisfied over $D$ for
large $r$. For small $r$, the model error and $L_{h-g}$ remain smaller due to
the closeness to $\mathcal{S}$, leading to smaller tubes, but planning can be
challenging, as $D$ may be too small to contain even these smaller tubes. In
particular, planning between two states in $D$ can become infeasible if $D$
becomes disconnected.

Input: $x_{I}$, $x_{G}$, $\mathcal{S}$, $\\{e_{i}\\}_{i=1}^{N}$, estimated
constants, $\mu$, $\mathcal{E}_{0}$

1

$\mathcal{T}\leftarrow\\{(x_{I},\sqrt{\mathcal{E}_{0}/\underline{\lambda}_{D}(M)},0)\\};\mathcal{P}\leftarrow\\{(\emptyset,\emptyset)\\}$

// node: state, energy, time; parent: previous control/dwell time

2

3while  _True_ do

$(x_{\textrm{n}},\bar{\epsilon}_{\textrm{n}},t_{\textrm{n}})\leftarrow$
SampleNode(_$\mathcal{T}$_)

// sample a node from the tree for expansion

$(u_{\textrm{c}},t_{\textrm{c}})\leftarrow$ SampleCandidateControl ()

// sample a control action and dwell time

$(x_{\textrm{c}}^{*}(t),u_{\textrm{c}}^{*}(t))\leftarrow$ IntegrateLearnedDyn
($x_{\textrm{n}}$, $u_{\textrm{c}}$, $t_{\textrm{c}}$)

// apply control for dwell time; get candidate tree extension

$\bar{\epsilon}_{c}(t)\leftarrow$ TrkErrBndEq12
($\bar{\epsilon}_{\textrm{n}}$, $x_{\textrm{c}}^{*}(t)$,
$u_{\textrm{c}}^{*}(t)$, $\mathcal{S}$, $\\{e_{i}\\}_{i=1}^{N}$)

// compute tracking error tube for candidate tree extension

$D_{\textrm{chk}}^{1}\leftarrow(x^{*}(t),u^{*}(t))\in
D_{\bar{\epsilon}_{c}(t)-\bar{u}_{\textrm{fb}}(t)},\forall
t\in[t_{\textrm{n}},t_{\textrm{n}}+t_{\textrm{c}})$

// check if tube around cand. extension remains within $D$

4 if  _controller learned using ( 2c)_ then
$D_{\textrm{chk}}^{2}\leftarrow\bar{\epsilon}_{c}(t)\leq\epsilon_{\textrm{max}},\forall
t\in[t_{\textrm{n}},t_{\textrm{n}}+t_{\textrm{c}})$

5 else $D_{\textrm{chk}}^{2}\leftarrow$ True // if using a controller
satisfying (2c), check if accumulated tracking error is below tolerance

$C\leftarrow$ InCollision ($x^{*}(t)$, $u^{*}(t)$, $\bar{\epsilon}_{c}(t)$)

// check if tracking error tube collides with obstacles

6 if  _$D_{\textrm{chk}}^{1}\wedge D_{\textrm{chk}}^{2}\wedge\neg C$_ then
$\mathcal{T}\leftarrow\mathcal{T}\cup\\{(x^{*}(t_{\textrm{n}}+t_{\textrm{c}}),\bar{\epsilon}_{c}(t_{\textrm{n}}+t_{\textrm{c}}),t_{\textrm{c}})\\}$;
$\mathcal{P}\leftarrow\mathcal{P}\cup\\{(u_{\textrm{c}},t_{\textrm{c}})\\}$

7 else continue // add node and corresponding parent if all checks pass

8 if  _$\exists t,x_{c}^{*}(t)\in\mathcal{B}_{\mu}(x_{G})$_ then break; return
plan // return path upon reaching goal

9

Algorithm 1 LMTCD-RRT

To trade off these competing factors, we propose the following solution for
selecting $r$. We first find a minimum $r$, $r_{\textrm{connect}}$, such that
$D$ is fully-connected. Depending on how $\mathcal{S}$ is collected, one may
wish to first filter out outliers that lie far from the bulk of the data. We
calculate the connected component by considering the dataset as a graph, where
an edge between $(x_{i},u_{i}),(x_{j},u_{j})\in\mathcal{S}$ exists if
$\|(x_{i},u_{i})-(x_{j},u_{j})\|\leq r$. We then determine if the contraction
condition (LABEL:eq:strong_con)/(2c) is satisfied for
$r=r_{\textrm{connect}}$, using the FTG-based procedure. If it is not
satisfied, we decrement $r$ until (LABEL:eq:strong_con)/(2c) holds, and select
$r$ as the largest value for which (LABEL:eq:strong_con)/(2c) are satisfied.
Since $r<r_{\textrm{connect}}$ in this case, planning can only be feasible
between start and goal states within each connected component; to rectify
this, more data should be collected to train the CCM/controller. If the
contraction condition is satisfied at $r=r_{\textrm{connect}}$, we
incrementally increase $r$, starting from $r_{\textrm{connect}}$. In each
iteration, we first determine if the contraction condition
(LABEL:eq:strong_con)/(2c) is still satisfied for the current $r$, using the
FTG-based procedure. If the contraction condition is satisfied, we evaluate an
approximate measure of planning permissiveness under “worst-case”
conditions222Roughly, this compares the size of $D$ to the tracking error tube
size and feedback control bound, c.f. Sec. IV-D and Thm. 2 for further
justification.: $r-\bar{\epsilon}(t)-\bar{u}_{\textrm{fb}}(t)$, evaluated at a
fixed time $t=T_{\textrm{query}}$, where $\bar{\epsilon}(t)$ and
$\bar{u}_{\textrm{fb}}(t)$ are computed assuming that for all
$t\in[0,T_{\textrm{query}}]$,
$\|(x^{*}(t),u^{*}(t))-(x_{i^{*}(t)},u_{i^{*}(t)}))\|=\max_{1\leq i\leq
N}\min_{1\leq j\leq N}\|(x_{i},u_{i})-(x_{j},u_{j})\|$, i.e. the dispersion of
the training data, and experiences the worst training error (i.e.
$e_{i^{*}(t)}=\max_{1\leq i\leq N}e_{i}$, for all $t$). If the contraction
condition is not satisfied, we terminate the search and select the $r$ with
the highest permissiveness, as measured by the aforementioned procedure.

###  IV-D Planning with the learned model and metric

Finally, we discuss our solution to safely planning with the learned dynamics
(Prob. 3). We develop an incremental sampling-based planner akin to a
kinodynamic RRT [18], growing a search tree $\mathcal{T}$ by forward-
propagating sampled controls held for sampled dwell-times, until the goal is
reached. To ensure the system remains within $D$ in execution (where the
contraction condition and (2n) are valid), we impose additional constraints on
where $\mathcal{T}$ is allowed to grow.

Denote $D_{q}=D\ominus\mathcal{B}_{q}(0)$ as the state/controls which are at
least distance $q$ from the complement of $D$, where $\ominus$ refers to the
Minkowski difference. Since (2n) defines tracking error tubes for any given
nominal trajectory, we can efficiently compute tracking tubes along any
candidate edge of an RRT. Specifically, suppose that we wish to extend the RRT
from a state on the planning tree $x_{\textrm{cand}}^{*}(t_{1})$ with initial
energy satisfying $\mathcal{E}_{\textrm{cand}}(t_{1})\leq\mathcal{E}_{t_{1}}$
to a candidate state $x_{\textrm{cand}}^{*}(t_{2})$ by applying control $u$
over $[t_{1},t_{2})$. This information is supplied to (2n), and we can obtain
the tracking error $\bar{\epsilon}_{\textrm{cand}}(t)$, for all
$t\in[t_{1},t_{2})$. Then, if we enforce that $(x^{*}(t),u^{*}(t))\in
D_{\bar{\epsilon}_{\textrm{cand}}(t)+\bar{u}_{\textrm{fb}}(t)}$ for all
$t\in[t_{1},t_{2})$, we can ensure that the true system remains within $D$
when tracked with a controller that satisfies
$u_{\textrm{fb}}(t)\leq\bar{u}_{\textrm{fb}}(t)$ in execution. Otherwise, the
extension is rejected and the sampling continues. When using a learned
$u(\tilde{x},x^{*},u^{*})$ with (2c), an extra check that
$\bar{\epsilon}_{\textrm{cand}}(t)\leq\epsilon_{\textrm{max}}$ is needed to
remain in $\mathcal{B}_{\epsilon_{\textrm{max}}}(0)\times D$ (c.f. Sec. IV-C).
Since $D$ is a union of balls, exactly checking $(x^{*}(t),u^{*}(t))\in
D_{\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}$ can be unwieldy. However, a
conservative check can be efficiently performed by evaluating (2t):

######  Theorem 2.

If (2t) holds for some index $1\leq i\leq N$ in $\mathcal{S}$,

$\|(x^{*}(t),u^{*}(t))-(x_{i},u_{i})\|\leq
r-\bar{\epsilon}(t)-\bar{u}_{\textrm{fb}}(t),$ (2t)

then $(x^{*}(t),u^{*}(t))\in D_{\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}$.

###### Proof.

By (2t), all
$(\bar{x},\bar{u})\in\mathcal{B}_{\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}(x^{*}(t),u^{*}(t))$
satisfy $\|(x_{i},u_{i})-(\bar{x},\bar{u})\|\leq r$ by the triangle
inequality. Thus,
$\mathcal{B}_{\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}(x^{*}(t),u^{*}(t))\subset\mathcal{B}_{r}(x_{i},u_{i})\subset
D$. As
$\mathcal{B}_{\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}(x^{*}(t),u^{*}(t))\subset
D$, $(x^{*}(t),u^{*}(t))$ is at least
${\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}$ distance from the complement of
$D$; thus, $(x^{*}(t),u^{*}(t))\in
D_{\bar{\epsilon}(t)+\bar{u}_{\textrm{fb}}(t)}$. ∎

We perform collision checking between the tracking tubes and the obstacles,
which we assume are expanded for the robot geometry; this is made easier since
(2n) defines a sphere for all time instants. We visualize our planner (Fig. 2,
right), which we denote Learned Models in Trusted Contracting Domains (LMTCD-
RRT), and summarize it in Alg. 1. We conclude with the following correctness
result:

######  Theorem 3 (LMTCD-RRT correctness).

Assume that the estimated $L_{h-g}$, $\bar{\lambda}_{D}(M)$,
$\bar{u}_{\textrm{fb}}(t)$, and $\lambda_{\textrm{CCM}}^{(\cdot)}$
overapproximate their true values and the estimated
$\underline{\lambda}_{D}(M)$ underapproximates its true value. Then, when
using a controller $u(\tilde{x},x^{*},u^{*})$ derived from (2), Alg. 1 returns
a trajectory $(x^{*}(t),u^{*}(t))$ that remains within $D$ in execution on the
true system. Moreover, when using a controller $u(\tilde{x},x^{*},u^{*})$
derived from (2c), Alg. 1 returns a trajectory $(x^{*}(t),u^{*}(t))$ such that
$(\tilde{x}^{*}(t),x^{*}(t),u^{*}(t))$ remains in
$\mathcal{B}_{\epsilon_{\textrm{max}}}(0)\times D$ in execution on the true
system.

###### Proof.

First, we consider a controller that is derived from (2). By Thm. 2, Alg. 1
returns a plan $(x^{*}(t),u^{*}(t))$ such that for all $t\in[0,T]$,
$(x^{*}(t),u^{*}(t))\in D_{\epsilon(t)+\bar{u}_{\textrm{fb}}(t)}$. If the
constants are estimated correctly, Thm. 1 holds, ensuring that the tracking
error $\epsilon(t)$ in execution is less than $\bar{\epsilon}(t)$, for all
$t\in[0,T]$. Furthermore, by the correct estimation of
$\bar{u}_{\textrm{fb}}(t)$, the feedback control satisfies
$\|u_{\textrm{fb}}(t)\|\leq\bar{u}_{\textrm{fb}}(t)$. Thus, for all
$t\in[0,T]$, the state/control in execution $(x(t),u(t))$ remains in
$\mathcal{B}_{\epsilon(t)+\bar{u}_{\textrm{fb}}(t)}(x^{*}(t),u^{*}(t))$. As
$(x^{*}(t),u^{*}(t))\in D_{\epsilon(t)+\bar{u}_{\textrm{fb}}(t)}$,
$(x(t),u(t))\in D$, for all $t\in[0,T]$. To apply this result for the learned
NN controller derived using (2c), we note that Alg. 1 further ensures that
$\bar{\epsilon}(t)\leq\epsilon_{\textrm{max}}$, for all $t\in[0,T]$. As the
preceding discussion shows that $\epsilon(t)\leq\bar{\epsilon}(t)$ in
execution, we have that $\epsilon(t)\leq\epsilon_{\textrm{max}}$; therefore,
$(\tilde{x}^{*}(t),x^{*}(t),u^{*}(t))$ remains within
$\mathcal{B}_{\epsilon_{\textrm{max}}}(0)\times D$ in execution. ∎

##  V Results

To demonstrate LMTCD-RRT on a wide range of systems, we show our method on a
4D nonholonomic car, a 6D underactuated quadrotor, and a 22D rope manipulation
task. Throughout, we will compare with four baselines to show the need to both
use the bound (2n) and to remain within $D$, where the bound is accurate: B1)
planning inside $D$ and assuming the model error is uniformly bounded by the
average training error $\|d(t)\|\leq\sum_{i=1}^{N}e_{i}/N$ to compute
$\bar{\epsilon}(t)$, B2) planning inside $D$ and using the maximum training
error $\|d(t)\|\leq\max_{1\leq i\leq N}e_{i}$ as a uniform bound, B3) not
remaining in $D$ in planning and assuming a uniform bound on model error
$\|d(t)\|\leq\max_{1\leq i\leq N}e_{i}$ in computing $\bar{\epsilon}(t)$ for
collision checking, and B4) not remaining in $D$ and using our error bound
(2n). We note that B3-type assumptions are common in prior CCM work [3, 6]. In
baselines that leave $D$, the space is unconstrained:
$\mathcal{X}=\mathbb{R}^{n_{x}}$, $\mathcal{U}=\mathbb{R}^{n_{u}}$. We set the
FTG-based estimation probability $\rho=0.975$ for each constant. Please see
Table I for planning statistics and https://tinyurl.com/lmtcdrrt for a
supplementary video which overviews the method and visualizes our results.

Figure 3: 4D car; planned (solid lines) and executed trajectories (dotted
lines). The filled red circles are obstacles. Tracking tubes for all methods
are drawn in the same color as the planned trajectory. To aid in visualizing
$D$, the small black dots are a subsampling of $\mathcal{S}$. We plot state
space projections of the trajectories: Left: projection onto the $x,y$
coordinates; Right: projection onto the $\theta$, $v$ coordinates. For this
example, LMTCD-RRT, B1, and B2 remain in $D$ in execution, while B3 and B4
exit $D$, and also exit their respective tracking tubes, leading to crashes.

Avg. trk. error (Car) Goal error (Car) Avg. trk. error (Quadrotor) Goal error
(Quadrotor) Avg. trk. error (Rope) Goal error (Rope) lightgray!50!LMTCD-RRT
lightgray!50! 0.008 $\pm$ 0.004 (0.024) lightgray!50! 0.009 $\pm$ 0.004
(0.023) lightgray!50! 0.0046 $\pm$ 0.0038 (0.0186) lightgray!50! 0.0062 $\pm$
0.0115 (0.0873) lightgray!50! 0.0131 $\pm$ 0.0063 (0.0278) lightgray!50!
0.0125 $\pm$ 0.0095 (0.0352) B1: Mean, in $D$ 0.019 $\pm$ 0.012 (0.054) 0.023
$\pm$ 0.016 (0.078) 0.0052 $\pm$ 0.0051 (0.0311) 0.0104 $\pm$ 0.0161 (0.0735)
18.681 $\pm$ 55.917 (167.79) 42.307 $\pm$ 126.81 (380.45) lightgray!50!B2:
Max, in $D$ lightgray!50!0.02 $\pm$ 0.01 (0.05) [19/50] lightgray!50!0.019
$\pm$ 0.012 (0.062) [19/50] lightgray!50! — [65/65] lightgray!50! — [65/65]
lightgray!50! 17.539 $\pm$ 52.380 (157.22) lightgray!50!21.595 $\pm$ 64.295
(193.05) B3: Max, $\notin D$ 0.457 $\pm$ 0.699 (3.640) 1.190 $\pm$ 1.479
(7.434) 0.1368 $\pm$ 0.2792 (1.5408) 0.8432 $\pm$ 1.3927 (9.0958) 111.86 $\pm$
39.830 (170.96) 236.34 $\pm$ 72.622 (331.83) lightgray!50!B4: Lip., $\notin D$
lightgray!50!0.704 $\pm$ 2.274 (13.313) lightgray!50!2.246 $\pm$ 8.254 (58.32)
lightgray!50! 0.4136 $\pm$ 0.4321 (1.9466) lightgray!50! 1.8429 $\pm$ 1.5260
(6.9859) lightgray!50!17.301 $\pm$ 49.215 (148.43) lightgray!50! 36.147 $\pm$
52.092 (147.76)

TABLE I: Statistics for the car, quadrotor, and rope. Mean $\pm$ standard
deviation (worst case) [if nonzero, number of failed trials].

Nonholonomic car (4D): We consider the vehicle model

$\begin{bmatrix}\dot{p}_{x}\\\ \dot{p}_{y}\\\ \dot{\theta}\\\
\dot{v}\end{bmatrix}=\begin{bmatrix}v\cos(\theta)\\\ v\sin(\theta)\\\ 0\\\
0\end{bmatrix}+\begin{bmatrix}0&0\\\ 0&0\\\ 1&0\\\
0&1\end{bmatrix}\begin{bmatrix}\omega\\\ a\end{bmatrix},$

where $u=[\omega,a]^{\top}$. As this model satisfies (2q), we use the stronger
CCM conditions (2). We use 50000 training data-points uniformly sampled from
$[0,5]\times[-5,5]\times[-1,1]\times[0.3,1]$ to train $f(x)$, $B(x)$, and
$M(x)$, with the $x$ needed to train $M(x)$ coming directly from the state
data in $\mathcal{S}$. We model $f$ and $B$ as NNs, each with a single hidden
layer of size 1024 and 16, respectively. We model $M(x)$ as an NN with two
hidden layers, each of size 128. In training, we set $\underline{w}=0.01$ and
gradually increase $\alpha_{1}$ and $\alpha_{2}$ to $0.01$ and $10$,
respectively. We select $r=0.6$ by incrementally growing $r$ as described in
Sec. IV-C, collecting 5000 new datapoints for $\Psi$, giving us
$\lambda=0.09$, $L_{h-g}=0.006$. $\delta_{u}=1.01$,
$\bar{\lambda}_{D}(M)=0.258$, and $\underline{\lambda}_{D}(M)=0.01$.

We plan for 50 different start/goal states in $D$, taking on average 6 mins,
and compare against the four baselines. We visualize one trial in Fig. 3. Over
the trials, LMTCD-RRT and B2 never violate their respective bounds in any of
the trials, while B1, B3, and B4 violate their bounds in 6, 48, and 43 of the
50 trials, respectively, which could lead to a crash (indeed, B3 and B4 crash
in Fig. 3). This occurs as the tracking error bounds for the baselines are
invalid, as the baselines’ model error bounds underestimate the true model
error which could be seen in execution. Moreover, planning is infeasible in
19/50 of B2’s trials, because the large uniform error bound can make it
impossible to reach a goal while remaining in $D$. This suggests the utility
of a fine-grained disturbance bound like (2k), especially when planning in
$D$, which is quite constrained. Note that while B2 does not crash in this
particular example, using the maximum training error can still be unsafe (as
will be seen in later examples), as the true error can be higher in
$D\setminus\mathcal{S}$. Finally, we note that the tracking accuracy
difference between LMTCD-RRT and B1/B2 reflects that the spatially-varying
error bound steers LMTCD-RRT towards lower error regions in $D$. Overall, this
example suggests that (2n) is accurate, while coarser disturbance bounds or
exiting $D$ can be unsafe.

Figure 4: 6D quadrotor; planned (solid lines) and executed trajectories
(dotted lines). The filled red circles are obstacles. Tracking tubes for all
methods are drawn in the same color as the planned trajectory. The small black
dots are a subsampling of $\mathcal{S}$. We plot state space projections of
the trajectories onto the $p_{x}$, $p_{z}$ coordinates. Left: for this
example, LMTCD-RRT remains within its tracking tube, and all baselines violate
their respective bounds near the end of execution (see inset). Right: for this
example, LMTCD-RRT remains within its tracking tube, and B3 and B4 exit $D$
and crash.

Underactuated planar quadrotor (6D): We consider the quadrotor model in [3,
p.20] with six states and two inputs:

$\begin{bmatrix}\dot{p}_{x}\\\ \dot{p}_{y}\\\ \dot{\phi}\\\ \dot{v}_{x}\\\
\dot{v}_{z}\\\
\ddot{\phi}\end{bmatrix}=\begin{bmatrix}v_{x}\cos(\phi)-v_{z}\sin(\phi)\\\
v_{x}\sin(\phi)+v_{z}\cos(\phi)\\\ \dot{\phi}\\\
v_{z}\dot{\phi}-g\sin(\phi)\\\ -v_{x}\dot{\phi}-g\cos(\phi)\\\
0\end{bmatrix}+\begin{bmatrix}0&0\\\ 0&0\\\ 0&0\\\ 0&0\\\ 1/m&1/m\\\
l/J&-l/J\end{bmatrix}\begin{bmatrix}u_{1}\\\ u_{2}\end{bmatrix},$

where $x=[p_{x},p_{z},\phi,v_{x},v_{z},\dot{\phi}]$, modeling the
linear/angular position and velocity, and $u=[u_{1},u_{2}]$, modeling thrust.
We use the parameters $m=0.486$, $l=0.25$, and $J=0.125$. These dynamics also
satisfy (2q), so we use the stronger CCM conditions (2). We sample 245000
training points from
$[-2,2]\times[-2,2]\times[-\pi/3,\pi/3]\times[-1,1]\times[-1,1]\times[-\pi/4,\pi/4]$
to train $f(x)$, $B(x)$, and $M(x)$, with the $x$ data for training $M(x)$
being the state data in $\mathcal{S}$. We model $f$ and $B$ as NNs with a
single hidden layer of size 1024 and 16, respectively. We model $M(x)$ as an
NN with two hidden layers of size 128. In training, we set
$\underline{w}=0.01$ and gradually increase $\alpha_{1}$ and $\alpha_{2}$ to
$0.001$ and $0.33$, respectively. We select $r=1.0$ by incrementally growing
$r$ as in Sec. IV-C, resulting in 10000 new datapoints for $\Psi$. This gives
us $\lambda=0.09$, $L_{h-g}=0.007$. $\delta_{u}=1.9631$,
$\bar{\lambda}_{D}(M)=4.786$, and $\underline{\lambda}_{D}(M)=0.0909$.

We plan for 65 different start/goal states within $D$, taking 1 min on
average, and compare against the baselines. We visualize two trials in Fig. 4.
Our attempts to run B2 failed, as the error bound was too large to feasibly
plan within $D$ in all 65 trials, again suggesting the need for a local model
error bound. Over these trials, LMTCD-RRT never violates its computed bound in
execution, while B1, B3, and B4 violate their bounds 14/65, 32/65, and 65/65
times, respectively. As for the car example, these bounds are violated because
the model error descriptions assumed by these baselines can underestimate the
true model error seen in execution. From Table I, one can see that LMTCD-RRT
obtains the lowest error, though it is closely matched by B1. However, LMTCD-
RRT never violates the tracking tubes in execution, while B1 does (i.e. Fig.
4, left). B3-B4 perform poorly, with the controller failing to overcome the
model error, causing crashes (Fig. 4, right). Overall, this example highlights
the need for LMTCD-RRT’s local error bounds while demonstrating our method’s
applicability to highly-underactuated systems.

10-link rope (22D): To demonstrate that our method scales to high-dimensional,
non-polynomial systems well beyond the reach of SoS-based methods, we consider
a planar rope manipulation task simulated in Mujoco [26]. We consider a
10-link (11-node) rope approximation, where each link can stretch, and the
head of the rope (see Fig. 5(d)) is velocity-controlled. The system has 22
states: the first two contain the $xy$ position of the head, and the rest are
the $xy$ positions of the other nodes, relative to the head, and the system
has two controls for the commanded $xy$ head velocities. We wish to steer the
tail of the rope to a given $xy$ region while ensuring the rope does not
collide in execution (c.f. Fig. 5). This is a challenging task, as the tail of
the rope is highly underactuated, and steering it to a goal requires modeling
the complicated friction forces that the rope is subjected to. We collect
three demonstrations to train the dynamics (see the supplementary video for a
visualization): in the first, the rope begins horizontally and performs a
counterclockwise elliptical loop; the second starts vertically and moves up,
the third begins horizontally and moves right, giving a total of 20500
datapoints. As the rope dynamics do not satisfy (2q), we learn both $M(x)$ and
$u(\tilde{x},x^{*},u^{*})$; to do so, we sample 20500 state/control
perturbations around the demonstrations and evaluate the dynamics at these
points, giving $|\mathcal{S}|=41000$. We model $f$ and $B$ as three-layer NNs
of size 512. $M(x)$ is modeled with two hidden layers of size 128, and
$u(\tilde{x},x^{*},u^{*})$ is modeled with a single hidden layer of size 128.
In training, we set $\underline{w}=1.0$ and gradually increase $\alpha_{1}$
and $\alpha_{3}$ to $0.005$ and $0.56$, respectively. To ensure that the CCM
and controller are invariant to translations of the rope, we enforce $M(x)$
and $u(\tilde{x},x^{*},u^{*})$ to not be a function of the head position. To
simplify the dynamics learning, we note that as the head is velocity-
controlled, it can be modeled as a single-integrator; we hardcode this
structure and learn the dynamics for the other 20 states. We obtain
$\epsilon_{\textrm{max}}=0.105$ and select $r=0.5$ by incrementally growing
$r$ as in Sec. IV-C, resulting in $|\Psi|=10000$, $\lambda=0.0625$,
$L_{h-g}=0.023$, $\bar{u}_{\textrm{fb}}=0.249$, $\bar{\lambda}_{D}(M)=3.36$,
and $\underline{\lambda}_{D}(M)=1$.

We plan for 10 different start/goal states within $D$, taking 9 min on
average, and compare against the baselines. As this example uses the learned
controller (2s), we adapt the baselines so that B1 and B2 remain in
$\mathcal{B}_{\epsilon_{\textrm{max}}}(0)\times D$, while B3 and B4 are
unconstrained. We visualize one task in Fig. 5: the rope starts horizontally,
with the head at $[0,0]$, and needs to steer the tail to $[3,0]$, within a
$0.15$ tolerance. LMTCD-RRT stays very close to the training data, reaching
the goal with small tracking tubes. B1 and B2 also remain close to the
training data as they plan in $D$, but as both the mean and maximum bounds may
underestimate the true model error in $D$, they stray too close to the
boundary of $D$, and the larger model error pushes them out of $D$, causing
the system to become unstable as the learned $u$ applies large inputs in an
attempt to stabilize around the plan. B3 exploits errors in the model,
planning a trajectory which is highly unrealistic. This is allowed to happen
because the maximum error severely underestimates the model error outside of
$D$, leading to a major underestimate of the tracking error that would be seen
in execution. When executing, the system immediately goes unstable due to the
large distance between the plan and the training data. The plan from B4 is
forced to remain close to the training data at first, in order to move through
the narrow passage. This is because the Lipschitz bound, while an
underestimate outside of $D$, still grows quickly with distance from
$\mathcal{S}$; attempting to plan a trajectory similar to B3 fails, since the
tracking error tube grows so large in this case that it becomes impossible to
reach the goal without the tube colliding with the obstacles. After getting
through the narrow passage, B4 drifts from $D$ and correspondingly fails to be
tracked beyond this point. Over these 10 trials, LMTCD-RRT never violates the
computed tracking bound, and B1, B2, B3, and B4 violate their bounds in 10, 6,
10, and 9 trials out of 10, respectively. Overall, this result suggests that
contraction-based control can scale to very high-dimensional systems (i.e.
deformable objects) if one finds where the model and controller are good and
takes care to stay there during planning and execution.

Figure 5: 22D planar rope dragging task. Snapshots of the planned trajectory
are in black, snapshots of the executed trajectory are in magenta, and the
tracking error tubes are in green. For further concreteness, for each
snapshot, we mark the head of the rope with an asterisk, and we mark the tail
of the rope with a solid dot. Additionally, the trajectory of the tail in the
plan is plotted in orange, while the trajectory of the tail in execution is
plotted in blue. Only LMTCD-RRT reaches the goal, while all baselines become
unstable when attempting to track their respective plans. We also show the
original Mujoco simulation environment in the bottom left.

##  VI Conclusion

We present a method for safe feedback motion planning with unknown dynamics.
To achieve this, we jointly learn a dynamics model, a contraction metric, and
contracting controller, and analyze the learned model error and trajectory
tracking bounds under that model error description, all within a trusted
domain. We then use these tracking bounds together with the trusted domain to
guide the planning of probabilistically-safe trajectories; our results
demonstrate that ignoring either component can lead to plan infeasibility or
unsafe behavior. Future work involves extending our method to plan safely with
latent dynamics models learned from image observations.

## Acknowledgments

We deeply thank Craig Knuth for insightful discussions and for feedback on the
manuscript. This work was supported in part by NSF grants IIS-1750489 and
ECCS-1553873, ONR grants N00014-21-1-2118 and N00014-18-1-2501, and a National
Defense Science and Engineering Graduate (NDSEG) fellowship.

## References

  * [1] W. Lohmiller and J. E. Slotine, “On contraction analysis for non-linear systems,” _Autom._ , vol. 34, no. 6, pp. 683–696, 1998.
  * [2] I. R. Manchester and J. E. Slotine, “Control contraction metrics: Convex and intrinsic criteria for nonlinear feedback design,” _IEEE Trans. Autom. Control._ , vol. 62, no. 6, pp. 3046–3053, 2017.
  * [3] S. Singh, B. Landry, A. Majumdar, J. E. Slotine, and M. Pavone, “Robust feedback motion planning via contraction theory,” 2019.
  * [4] A. Lakshmanan, A. Gahlawat, and N. Hovakimyan, “Safe feedback motion planning: A contraction theory and l${}_{\mbox{1}}$-adaptive control based approach,” _CDC_ , 2020.
  * [5] B. T. Lopez, J. E. Slotine, and J. P. How, “Robust adaptive control barrier functions: An adaptive & data-driven approach to safety,” _IEEE Control. Syst. Lett._ , vol. 5, no. 3, pp. 1031–1036, 2021.
  * [6] D. Sun, S. Jha, and C. Fan, “Learning certified control using contraction metric,” _CoRL_ , 2020.
  * [7] H. Tsukamoto and S. Chung, “Neural contraction metrics for robust estimation and control: A convex optimization approach,” _IEEE Control. Syst. Lett._ , vol. 5, no. 1, pp. 211–216, 2021.
  * [8] S. Singh, S. M. Richards, V. Sindhwani, J. E. Slotine, and M. Pavone, “Learning stabilizable nonlinear dynamics with contraction-based regularization,” _IJRR_ , 2020.
  * [9] G. Manek and J. Z. Kolter, “Learning stable deep dynamics models,” in _NeurIPS_ , 2019, pp. 11 126–11 134.
  * [10] N. M. Boffi, S. Tu, N. Matni, J. E. Slotine, and V. Sindhwani, “Learning stability certificates from data,” _CoRL_ , 2020.
  * [11] T. Koller, F. Berkenkamp, M. Turchetta, and A. Krause, “Learning-based model predictive control for safe exploration,” in _CDC_ , 2018.
  * [12] A. K. Akametalu, J. F. Fisac, J. H. Gillula, S. Kaynama, M. N. Zeilinger, and C. J. Tomlin, “Reachability-based safe learning with gaussian processes,” in _CDC_ , 2014, pp. 1424–1431.
  * [13] F. Berkenkamp, R. Moriconi, A. P. Schoellig, and A. Krause, “Safe learning of regions of attraction for uncertain, nonlinear systems with gaussian processes,” in _CDC_ , 2016, pp. 4661–4666.
  * [14] D. D. Fan, A. Agha-mohammadi, and E. A. Theodorou, “Deep learning tubes for tube MPC,” _RSS_ , 2020.
  * [15] C. Knuth, G. Chou, N. Ozay, and D. Berenson, “Planning with learned dynamics: Probabilistic guarantees on safety and reachability via lipschitz constants,” _IEEE Robotics and Automation Letters_ , 2021.
  * [16] K. Leung and I. R. Manchester, “Nonlinear stabilization via control contraction metrics: A pseudospectral approach for computing geodesics,” in _ACC_. IEEE, 2017, pp. 1284–1289.
  * [17] H. K. Khalil, _Nonlinear systems_. Prentice-Hall, 2002.
  * [18] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic planning,” _IJRR_ , vol. 20, no. 5, pp. 378–400, 2001.
  * [19] S. Boyd and L. Vandenberghe, _Convex Optimization_ , 2004.
  * [20] T.-W. Weng, H. Zhang, P.-Y. Chen, J. Yi, D. Su, Y. Gao, C.-J. Hsieh, and L. Daniel, “Evaluating the robustness of neural networks: An extreme value theory approach,” _ICLR_ , 2018.
  * [21] L. De Haan and A. Ferreira, _Extreme value theory: an introduction_. Springer Science & Business Media, 2007.
  * [22] M. DeGroot and M. Schervish, _Probability & Statistics_. Pearson, 2013.
  * [23] P. D. Lax, _Linear Algebra and Its Applications_ , 2007.
  * [24] M. Fazlyab, A. Robey, H. Hassani, M. Morari, and G. Pappas, “Efficient and accurate estimation of lipschitz constants for deep neural networks,” in _NeurIPS_ , 2019, pp. 11 427–11 438.
  * [25] M. Jordan and A. G. Dimakis, “Exactly computing the local lipschitz constant of relu networks,” in _NeurIPS_ , 2020.
  * [26] E. Todorov, T. Erez, and Y. Tassa, “Mujoco: A physics engine for model-based control,” in _IROS_ , 2012, pp. 5026–5033.