# Discrete fully probabilistic design: towards a control pipeline for the
synthesis of policies from examples

Enrico Ferrentino, Pasquale Chiacchio, Giovanni Russo All authors are with the
Department of Computer and Electrical Engineering and Applied Mathematics
(DIEM), University of Salerno, 84084 Fisciano, SA, Italy, e-mail:
<EMAIL_ADDRESS>
###### Abstract

We present the principled design of a control pipeline for the synthesis of
policies from examples data. The pipeline, based on a discretized design which
we term as discrete fully probabilistic design, expounds an algorithm recently
introduced in [1] to synthesize policies from examples for constrained,
stochastic and nonlinear systems. Contrary to other approaches, the pipeline
we present: (i) does not need the constraints to be fulfilled in the possibly
noisy example data; (ii) enables control synthesis even when the data are
collected from an example system that is different from the one under control.
The design is benchmarked numerically on an example that involves controlling
an inverted pendulum with actuation constraints starting from data collected
from a physically different pendulum that does not satisfy the system-specific
actuation constraints. We also make our fully documented code openly
available.

###### Index Terms:

Control design pipeline, control from examples, data-driven control

##  I Introduction

Over the past few years, much research effort has been devoted to the problem
of synthesizing control polices directly from data, bypassing the need to
devise and identify a mathematical model in the form of
difference/differential equations [2]. An appealing data-driven control
framework is that of designing controllers by using example data [3, 1, 4, 5].
Within this control from examples framework, one seeks to synthesize policies
so that the closed-loop system tracks some desired behavior extracted from the
examples. In this context, a key challenge is that of developing an end-to-end
pipeline to synthesize control policies for nonlinear, stochastic and
constrained systems from noisy example data. Towards this aim, we present the
principled design of a pipeline that enables control synthesis in these
situations. With our pipeline, situations can be considered when data are
collected from a system that is different from the one under control and does
not satisfy the system-specific constraints, which might hence be unknown when
the examples are collected. We refer to [6, 7, 8, 9] for detailed
interdisciplinary surveys on the related topics of (imitation) learning and
sequential decision making and we now briefly survey a number of works that
are related to the specific methodological framework we leverage in this
paper.

#### Related work

We leverage a Bayesian approach [10] to dynamical systems that allows to
represent the behaviors of these systems via probability functions. In the
context of control, the approach has been leveraged in e.g. [11, 12, 13, 14,
15, 16] for the design of randomized control policies that enable tracking of
a given target behavior. In these papers, the tracking problem is tackled by
setting-up an unconstrained optimization problem. Closely related works,
include [17, 18, 19]. These works, by leveraging a similar framework,
formalize the control problem as the (unconstrained) problem of minimizing a
cost that captures the discrepancy between an ideal probability density
function and the actual probability density function of the system under
control. An online version of these algorithms has been proposed in [20]: in
such a work, by leveraging an average cost formulation, the probability mass
function for the state transitions is found. Finally, we also recall here [21,
22], where policies are obtained from the minimization of similar costs by
leveraging multiple, specialized, datasets. These last papers, together with
[1], introduce constraints to the probabilistic formulation. Finally, we also
recall [23], where a decision making architecture for Robust Model-Predictive
Path Integral Control is proposed and this allows the introduction of the
constraints (see also references therein).

#### Statement of contributions and organization of the paper

We introduce, and benchmark, the principled design of a pipeline for the
control synthesis from examples data. The pipeline expounds an algorithm
presented in [1] for the synthesis of control policies from examples. We term
this discretized design discrete fully probabilistic design (DFPD). While [1]
seeks to find an analytical solution to the control problem, here we rely on
finding a purely numerical solution, exploiting convexity of the underlying
optimization problems that, as we show, need to be solved in order to
synthesize the policy. We also discuss a number of properties of DFPD. In
contrast to other works on imitation learning and control synthesis from
example, by exploiting [1], our design: (i) does not need the constraints to
be fulfilled in the possibly noisy example data; (ii) enables control
synthesis even when the data are collected from an example system that is
different from the one under control. Finally, the design is numerically
benchmarked on an inverted pendulum and we also make the code openly available
and fully documented (see https://github.com/unisa-acg/discrete-fpd).

The paper is organized as follows. After giving the mathematical preliminaries
and formalizing the statement of the problem (Section II), we present the
DFPD. This is done in Section III, where we also discuss a number of its
properties. In Section IV we describe a design pipeline to use DFPD. The
pipeline illustrates a process to determine, via DFPD, control inputs from the
data. Finally, the effectiveness of DFPD is illustrated in Section V on a
pendulum with actuation constraints. Concluding remarks are given in Section
VI.

##  II Mathematical Set-up and Problem Statement

We denote sets via calligraphic capital characters and vectors in bold.
Multidimensional random variables and their realizations are both denoted by
lower-case bold letters. All the random variables we consider are discrete and
sampled from probability mass functions (pmfs) with compact supports.
Throughout the paper, we also refer to pmfs as (normalized) histograms and to
its domain as (discrete) alphabet. The notation $\mathbf{z}\sim P(\mathbf{z})$
denotes the fact that the random variable $\mathbf{z}$ is sampled from the pmf
$P(\mathbf{z})$. Given the set $\mathcal{Z}\subseteq\mathbb{R}^{n_{z}}$, its
_indicator function_ is denoted by $\mathds{1}_{\mathcal{Z}}({\bf z})$, so
that $\mathds{1}_{\mathcal{Z}}({\bf z})=1\,\forall{\bf z}\in\mathcal{Z}$ and
$0$ otherwise. We also let $\mathbb{E}_{P}[\mathbf{h}(\mathbf{z})]:=\sum
P(\mathbf{z})\mathbf{h}(\mathbf{z})$, where the sum is implicitly assumed to
be taken over the support of $P(\mathbf{z})$, be the expectation of a
function, say $\mathbf{h}(\cdot)$, of $\mathbf{z}$. The joint pmf of
$\mathbf{z}_{1}$ and $\mathbf{z}_{2}$ is denoted by
$P(\mathbf{z}_{1},\mathbf{z}_{2})$ and the conditional pmf of $\mathbf{z}_{1}$
given $\mathbf{z}_{2}$ is denoted by $P(\mathbf{z}_{1}|\mathbf{z}_{2})$. The
control problem considered in this paper is stated in terms of the Kullback-
Leibler (or simply KL) divergence [24]. Given the histograms $P(\bm{\alpha})$
and $Q(\bm{\alpha})$ defined over the discrete alphabet $\mathcal{A}$, the KL-
divergence is defined as

$\mathcal{D}_{KL}(P||Q):=\sum_{\bm{\alpha}\in\mathcal{A}}P(\bm{\alpha})\ln\frac{P(\bm{\alpha})}{Q(\bm{\alpha})}.$
(1)

###  II-A Formulation of the control problem

Let ${\bf x}\in\mathcal{X}\subset\mathbb{R}^{d_{x}}$ be the state of a
dynamical system, and ${\bf u}\in\mathcal{U}\subset\mathbb{R}^{d_{u}}$ be the
control variable. The time variable is $t\in[0,T]$, $T<+\infty$. The time
interval $[0,T]$ is discretized in $n+1$ instants with step $\Delta t>0$. This
yields the sequence of time-instants $t(k)=k\Delta t$, with $k=0,\ldots,n$ and
$T=n\Delta t$. We also let ${\bf x}(k)$ and ${\bf u}(k)$ be ${\bf x}$ and
${\bf u}$ evaluated in the discretized time. We recall that $\mathcal{X}$ and
$\mathcal{U}$ are both compact and we set

$\mathcal{X}:=\\{{\bf x}_{0},\ldots,{\bf x}_{m-1}\\},\ \ \
\mathcal{U}:=\\{{\bf u}_{0},\ldots,{\bf u}_{z-1}\\}~{}\forall k.$ (2)

For notational convenience, we use the shorthand notation $\mathbf{x}_{j}(k)$
(resp. $\mathbf{u}_{h}(k)$) to denote that $\mathbf{x}$ (resp. $\mathbf{u}$)
at time-instant $k$ equals the value of the $j$-th (resp. $h$-th) element in
$\mathcal{X}$ (resp. $\mathcal{U}$).

Following [10, 11] one can describe the behavior of a given system by
computing the joint pmf $P^{n}:=P\big{(}{\bf x}(0),\ldots,{\bf x}(n),{\bf
u}(0),\ldots,{\bf u}(n-1)\big{)}$, obtained from e.g. state-input data
collected from the system. Likewise, one can also define a desired (or
reference) behavior for the system in terms of a joint pmf, say
$Q^{n}:=Q\big{(}{\bf x}(0),\ldots,{\bf x}(n),{\bf u}(0),\ldots,{\bf
u}(n-1)\big{)}$. Then, by making the standard Markov’s assumption, the chain
rule for pmfs yields the following convenience factorization:

$\begin{split}P^{n}=\prod_{k=1}^{n}&P\big{(}{\bf x}(k)|{\bf x}(k-1),{\bf
u}(k-1)\big{)}\\\ &P\big{(}{\bf u}(k-1)|{\bf x}(k-1)\big{)}P(\mathbf{x}(0))\\\
=:\prod_{k=1}^{n}&\tilde{P}_{X}^{k}\tilde{P}_{U}^{k-1}P(\mathbf{x}(0))=:\prod_{k=1}^{n}\tilde{P}^{k}P(\mathbf{x}(0)),\\\
Q^{n}=\prod_{k=1}^{n}&Q\big{(}{\bf x}(k)|{\bf x}(k-1),{\bf u}(k-1)\big{)}\\\
&Q\big{(}{\bf u}(k-1)|{\bf x}(k-1)\big{)}Q(\mathbf{x}(0))\\\
=:\prod_{k=1}^{n}&\tilde{Q}_{X}^{k}\tilde{Q}_{U}^{k-1}Q(\mathbf{x}(0))=:\prod_{k=1}^{n}\tilde{Q}^{k}Q(\mathbf{x}(0)).\end{split}$
(3)

The former term in the first product of the definitions takes the name of
_state evolution model_ , while the latter is the _randomized control law_
[25]. In the context of this paper, the pmf $Q^{n}$ is collected from example
data. This allows us to state the problem of synthesizing control policies
from example data for systems with actuation constraints as follows.

######  Problem II.1 (Global optimization problem).

Find a solution to the optimization problem

$\begin{split}\min_{\tilde{P}_{U}^{0},\ldots,\tilde{P}_{U}^{n-1}}&\mathcal{D}_{KL}(P^{n}||Q^{n})\\\
\mbox{s.t.~{}}&{\bf f}_{k}(p^{k}_{hi})\leq{\bf 0}~{}\forall k\\\ &{\bf
g}_{k}(p^{k}_{hi})={\bf 0}~{}\forall k\end{split}$ (4)

where ${\bf f}_{k}(p^{k}_{hi})$, ${\bf g}_{k}(p^{k}_{hi})$ are constraint
vector functions defining a convex feasibility domain and
$p^{k}_{hi}:={P}\left({\bf u}_{h}(k)|{\bf x}_{i}(k)\right)$.

For Problem II.1 we make the following observations. Minimizing the KL-
divergence amounts at minimizing the discrepancy between $P^{n}$ and $Q^{n}$.
These pmfs can be obtained by observing different systems and hence the
formulation embeds the possibility of synthesizing policies for a given system
by using examples collected on a different one.

##  III The Discrete Fully Probabilistic Design Algorithm

The pseudo-code for the DFPD is given in Algorithm 1. The DFPD takes as input
the probabilistic descriptions $\tilde{P}_{X}^{k+1}\big{(}{\bf x}(k+1)|{\bf
x}_{i},{\bf u}_{h}\big{)}$, $\tilde{Q}_{X}^{k+1}\big{(}{\bf x}(k+1)|{\bf
x}_{i},{\bf u}_{h}\big{)}$, $\tilde{Q}_{U}^{k}\big{(}{\bf u}(k)|{\bf
x}_{i}\big{)}$ and the constraints of Problem II.1 (optional). DFPD then
outputs the optimal solution to Problem II.1.111As noted in [1], given the
convexity of the cost, if the constraints define a convex set, the existence
of the solution is guaranteed. The core of the procedure is a backward
recursion that solves, at each iterate, the following

######  Problem III.1 (Local optimization problem).

Find a solution, for each ${\bf x}_{i}\in\mathcal{X}$, to the following

$\begin{split}\min_{p_{1i},\ldots,p_{zi}}&d\left({\bf x}_{i}\right)=p_{1i}\ln
p_{1i}+p_{1i}(d_{1i}^{x}+r_{1i}-a_{1i})+\ldots+\\\ &+p_{zi}\ln
p_{zi}+p_{zi}(d_{zi}^{x}+r_{zi}-a_{zi})\\\
\mbox{s.t.}&\sum_{h=1}^{z}p_{hi}=1\\\ &p_{hi}\geq 0~{}\forall h\\\ &{\bf
f}_{k}(p_{hi})\leq{\bf 0}\\\ &{\bf g}_{k}(p_{hi})={\bf 0}\end{split}$ (5)

where $p_{hi}=p_{hi}^{k}$ are the probabilities at the current iterate,
$d^{x}_{hi}=\mathcal{D}_{KL}(\tilde{P}_{X}^{k+1}||\tilde{Q}_{X}^{k+1})$,
$a_{hi}=\ln\left({Q}\left({\bf u}_{h}(k)|{\bf x}_{i}(k)\right)\right)$ and

$r_{hi}=\sum_{j=0}^{m-1}P\big{(}{\bf x}_{j}(k+1)|{\bf x}_{i}(k),{\bf
u}_{h}(k)\big{)}d\big{(}{\bf x}_{j}(k+1)\big{)}.$ (6)

At each step of the optimization horizon, and for each state ${\bf
x}_{i}\in\mathcal{X}$, Algorithm 1 computes the scalars $d_{hi}$, $a_{hi}$ and
$r_{hi}$, needed for the resolution of Problem III.1. We note that, for the
$k$-th step and the $i$-th state, the computation of $r_{hi}$ requires the
recursion of the optimal cost $d({\bf x}(k+1))$ _for all states_. Therefore,
at $k$, each single cost $d({\bf x}_{i}(k))$ is temporarily stored in the
auxiliary variable $c({\bf x}_{i})$ in order to be reused at $k-1$. We now
give the following

######  Proposition III.2.

Algorithm 1 returns the optimal solution to Problem II.1.

###### Proof.

The proof follows the technical derivations of [1] where an analytical
solution to Problem 1 is given via a backward recursion and a primal-dual
argument. The difference is that Algorithm 1 numerically solves, at each $k$,
a reformulation of the problem solved in the paper mentioned above. This
reformulation is in fact Problem III.1 and, for completeness, a sketch of its
derivation is given in Appendix A. ∎

Inputs: $\tilde{P}_{X}^{k+1}\big{(}{\bf x}(k+1)|{\bf x}_{i},{\bf
u}_{h}\big{)}$, $\tilde{Q}_{X}^{k+1}\big{(}{\bf x}(k+1)|{\bf x}_{i},{\bf
u}_{h}\big{)}$, $\tilde{Q}_{U}^{k}\big{(}{\bf u}(k)|{\bf x}_{i}\big{)}$,
constraints of Problem II.1 (optional)

Output: solution to Problem II.1

Initialize: $d({\bf x}_{i})\leftarrow 0~{}\forall i=0,\ldots,m-1$

Main loop:

for  _$k\leftarrow n-1$ to $0$_ do

for  _each ${\bf x}_{i}\in\mathcal{X}$_ do

$d^{x}_{hi}\leftarrow\mathcal{D}_{KL}(\tilde{P}^{k+1}_{X}||\tilde{Q}^{k+1}_{X})~{}\forall
h$

$a_{hi}\leftarrow\ln\left(\tilde{Q}_{U}^{k}({\bf u}_{h}|{\bf
x}_{i})\right)\forall h$

$r_{hi}\leftarrow\sum_{x_{j}\in\mathcal{X}}\tilde{P}_{X}^{k+1}({\bf
x}_{j}|{\bf x}_{i},{\bf u}_{h})d({\bf x}_{j})~{}\forall h$

Find $\tilde{P}_{U}^{k}$ by solving Problem III.1 with the coefficients above

Store optimal cost $c({\bf x}_{i})$

end for

$d({\bf x}_{i})\leftarrow c({\bf x}_{i})~{}\forall i=0,\ldots,m-1$

end for

Algorithm 1 Discrete fully probabilistic design algorithm

We also make the following

######  Remark III.3.

Consider a receding horizon set-up with time-invariant constraints and where,
at each $k$, the optimization problem solved in the receding horizon window
remains the same. Further, note that Algorithm 1, by construction, finds all
the possible (for each state) optimal conditional probability functions
$\tilde{P}_{U}^{0}$. This implies that, in principle, for this special case,
one can find the policy off-line. Specifically, once the problem is solved
off-line, the control input can be simply obtained by sampling, at each $k$,
from $\tilde{P}_{U}^{0}$.

The rationale behind Remark III.3 is as follows. Let us assume that $t_{0}$ is
the current time and Problem II.1 is solved via Algorithm 1. In this case,
$t=t_{0}+k\Delta t$, so that a finite horizon $T$ of $n$ optimization steps is
considered. Although the current state ${\bf x}(t_{0})={\bf x}(k=0)$ is known,
Algorithm 1 solves Problem II.1 $\forall{\bf x}(t_{0})\in\mathcal{X}$. Let us
name this optimal solution $P({\bf u}(k)|{\bf x}(k))$. In a receding horizon
setup, $P({\bf u}(0)|{\bf x}(0))$ is the control action at $t=t_{0}$. Now, let
us consider a generic time $t_{1}>t_{0}$ at which Problem II.1 is solved again
with the same strategy, with $t=t_{1}+k\Delta t$ and $k=0,\ldots,n-1$
corresponding to the same finite horizon $T$ and with ${\bf
x}(t_{1})\in\mathcal{X}$. Let us name this optimal solution $P_{t_{1}}({\bf
u}(k)|{\bf x}(k))$. If the probabilistic models and the constraints are time-
invariant, then $P_{t_{1}}({\bf u}(k)|{\bf x}(k))=P({\bf u}(k)|{\bf
x}(k))\,\forall k$. As a consequence, in our receding horizon setup, the
control action at $t=t_{1}$ is, again, $P({\bf u}(0)|{\bf x}(0))$. Last, let
us assume to pick $t_{1}=t_{i}+i\Delta t$, with $i\in\mathbb{N}_{0}$ and that
a control action is taken at every $t_{1}$. In view of the above, the optimal
control action is $P({\bf u}(0)|{\bf x}(0))\,\forall t_{1}$, as long as ${\bf
x}(t_{1})={\bf x}(k=0)\in\mathcal{X}$, that is true by construction.

######  Remark III.4.

Depending on applications, the control action/state space dimensions of the
problem can be reduced. For example, in the context of connected cars, as
shown in [26] the state space can be conveniently reduced to only the
links/states that are reachable by the vehicle within the time horizon. We
leave for future research the problem of finding applications-agnostic methods
to reduce the dimensionality of the control action/state space suitable for
Algorithm 1.

##  IV The Proposed Design Pipeline: From Data to Control Inputs

Before proceeding with the illustration of the pipeline to compute the inputs
required by Algorithm 1, we note here that, as for any other control approach
that relies solely on the available data, these need to be sufficiently
informative. In this paper we do not consider the problem of obtaining
sufficiently informative datasets and refer to e.g. [27, 28, 29, 30] for
recent results on this topic.

The first step of the pipeline is to gather the data, which are then processed
to obtain the probability functions required as input by Algorithm 1. The
last, optional, step of the pipeline consists in formalizing the constraints
for Problem II.1.

###  IV-A Data gathering

We refer to the system under control as _target system_ and we term as
reference system the one that is used to collect example data. In what
follows, we do not require any knowledge on the (possibly) nonlinear and
stochastic dynamics that is generating the data. Also, data for the target and
reference system might be generated from different unknown dynamics. Our
starting point is the collection of the data to compute $\tilde{Q}_{X}^{k}$,
$\tilde{P}_{X}^{k}$ and $\tilde{Q}_{U}^{k}$. Namely, we assume the
availability of the following data recorded within the observation window
$[0,T]$: the triplets $\\{{\bf x}^{t}(k),{\bf u}^{t}(k),{\bf x}^{t}(k+1)\\}$
and $\\{{\bf x}^{r}(k),{\bf u}^{r}(k),{\bf x}^{r}(k+1)\\}$, and the pairs
$\\{{\bf x}^{r}(k),{\bf u}^{r}(k)\\}$, where ${\bf x}^{t}(k)$ and ${\bf
x}^{r}(k)$ are the target and reference systems’ states and ${\bf u}^{t}(k)$
and ${\bf u}^{r}(k)$ are the target and reference systems’ inputs at $t(k)$,
respectively. Before illustrating how these data are used, we remark here that
for physical applications (modeled via continuous dynamics) the data give a
view of a discretized version of the processes. Hence, the probability
functions that we obtain implicitly depend on the (discretization) step
$\Delta t$. From the practical viewpoint, this parameter needs to be in
accordance with the duration of the full control cycle.

######  Remark IV.1.

For time-invariant, we drop the superscripts in the above probability
functions as these are stationary. Due to the effects of the discretization
and quantization, stationary probability functions might, in principle, still
be obtained from time-varying dynamical processes.

###  IV-B Quantization

Once the data are obtained, these need to be quantized to obtain discrete sets
of the form (2) required by Algorithm 1. We let ${\bf\Delta x}$ and
${\bf\Delta u}$ be the uniform quantization steps for state and input. Then,
each given observation of the state, say ${\bf x}$, is mapped onto ${\bf
x}_{j}$ defined in (2) if

${\bf x}_{j}-\frac{\bf\Delta x}{2}\leq{\bf x}<{\bf x}_{j}+\frac{\bf\Delta
x}{2}.$ (7)

Analogously, a given observed input, say ${\bf u}$, is mapped onto ${\bf
u}_{h}$ defined in (2) if

${\bf u}_{h}-\frac{\bf\Delta u}{2}\leq{\bf u}<{\bf u}_{h}+\frac{\bf\Delta
u}{2}.$ (8)

At the boundaries, ${\bf x}$ is mapped onto ${\bf x}_{0}$ when ${\bf x}<{\bf
x}_{0}+\Delta{\bf x}/2$ and onto ${\bf x}_{m-1}$ when ${\bf x}\geq{\bf
x}_{m-1}-\Delta{\bf x}/{2}$. Equivalent considerations hold for the inputs.

######  Remark IV.2.

An interesting open problem, which we leave for future research, is to
characterize how control performance are affected by discretization (see also
our conclusions).

###  IV-C Computation of the probability functions

As in e.g. [1], we obtain the probability functions by computing the empirical
distributions from the data. This can be achieved by defining three counting
functions: (i) $c_{X}:\mathcal{X}\rightarrow\mathbb{N}$ counts the occurrences
of a given state in the collected reference (or target) system data; (ii)
${\bf c}_{X|X,U}:\mathcal{X}\times\mathcal{U}\rightarrow\mathbb{N}^{m}$, for
each visited state and selected input in the same state, counts the
occurrences of every state in the following time instant in the collected
reference (or target) system data; (iii) ${\bf
c}_{U|X}:\mathcal{X}\rightarrow\mathbb{N}^{z}$, for each visited state, counts
the occurrences of every selected input in the collected reference system
data. The probabilistic functions for the target thus be computed, for each
$i$ and $h$, as:

$\tilde{Q}_{X}({\bf x}|{\bf x}_{i},{\bf u}_{h})=\frac{{\bm{\tau}}_{X|X,U}({\bf
x}_{i},{\bf u}_{h})}{{\bm{\tau}}_{U|X}^{h}({\bf x}_{i})},\ \
\tilde{Q}_{U}({\bf u}|{\bf x}_{i})=\frac{{\bm{\tau}}_{U|X}({\bf
x}_{i})}{\tau_{X}({\bf x}_{i})}.$ (9)

Since the counting functions are vector functions, the superscript indicates
the element in the vector. In the above expressions, we have $\tau_{X}({\bf
x}):=o_{s}+c_{X}({\bf x}),{\bm{\tau}}_{X|X,U}({\bf x},{\bf u}):=o_{n}+{\bf
c}_{X|X,U}({\bf x},{\bf u})$ and ${\bm{\tau}}_{U|X}({\bf x}):=o_{i}+{\bf
c}_{U|X}({\bf x})$, with $o_{s}$, $o_{n}$ and $o_{i}$ being small constants
(offsets) added in order to avoid divisions by $0$ in (9) which would happen
when a given event is not seen in the data, as well as the computation of
$\log{(0)}$ for, e.g., the coefficients in Problem III.1. Clearly, the same
steps can be followed to obtain $\tilde{P}_{X}({\bf x}|{\bf x}_{i},{\bf
u}_{h})$ and these are omitted here for brevity.

######  Remark IV.3.

A possible choice for the offsets $o_{s}$ is to set $o_{s}\leq 1/m$. Once
$o_{s}$ is fixed, all the other offsets must be designed so as to guarantee
that probabilities sum to one. Then one needs to set $o_{i}=o_{s}/z$ and
$o_{n}=o_{i}/m$.

######  Remark IV.4.

When an input is never selected in a given state or a state is never visited,
the data do not contain any information about $\tilde{Q}_{X}({\bf x}|{\bf
x}_{i},{\bf u}_{h})$ and $\tilde{Q}_{U}({\bf u}|{\bf x}_{i})$ respectively. In
these cases the best we can do is assuming that any state can be reached at
the next time step with equal probability by applying ${\bf u}_{h}$ in ${\bf
x}_{i}$ and that any input is selected with equal probability when in ${\bf
x}_{i}$, respectively. Equations in (9) automatically implement this
condition.

###  IV-D Respecting the range of feasible inputs

Problem (III.1) accounts for soft and hard constraints imposed through
probabilities. Here we discuss about the possibility of manipulating the input
domains so as to respect the range of feasible inputs, regardless of the
optimization performed by Algorithm 1. Let us assume that the reference and
target systems are commanded with inputs ${\bf v}_{r,t}$, and that the
(symmetrical) ranges of feasible inputs are $-\overline{\bf v}_{r,t}\leq{\bf
v}_{r,t}\leq\overline{\bf v}_{r,t}$, where $\pm\overline{\bf v}_{r,t}$
represent the boundaries, extracted, for instance, from the plants’ datasheet.
The inputs of the probabilistic model are the normalized ones, i.e.

${\bf u}_{r}=\frac{{\bf v}_{r}}{\overline{\bf v}_{r}},{\bf u}_{t}=\frac{{\bf
v}_{t}}{\overline{\bf v}_{t}}$ (10)

which implies $-{\bf 1}\leq{\bf u}_{\\{r,t\\}}\leq{\bf 1}$, with ${\bf 1}$
representing the vector of ones of appropriate size.

Once the control policy $\tilde{P}_{k}^{U}$ is available, the probabilistic
controller reconstructs the target system input by inverting (10), hence
setting ${\bf u}_{0}=-{\bf 1},{\bf u}_{z-1}={\bf 1}$ in (2) ensures that the
ranges of feasible inputs are respected without defining explicit constraints
in Problem II.1.

###  IV-E Adding constraints in the optimization problem

This last step in the pipeline is optional and is only requested if the
problem has actuation constraints, beyond respecting the range of feasible
inputs. We recall that the constraint functions that the DFPD takes as input
are generic and one only needs to verify that the set is convex (recall that
convexity of the set guarantees the existence of an optimal solution to
Problem II.1). Constraints of practical interest, which can be captured with a
proper choice of constraint functions, include moment constraints [31, 32, 33]
and bound constraints [34, 35]. For example, the inequality constraint
$\mathbb{E}_{\tilde{P}^{k}_{U}}[U^{i}]-m_{i}\leq 0$ expresses the fact that
the $i$-th moment of the control input is not larger than $m_{i}$. Bound
constraints instead formalize the fact that
$\mathbb{P}(\mathbf{U}_{k}\in\bar{\mathcal{U}}_{k})\geq 1-\varepsilon$, where
$\bar{\mathcal{U}}_{k}\subset\mathcal{U}$ is and $\varepsilon\geq 0$. That is,
the constraint captures the fact that the control variable belongs to some
(e.g. desired) $\bar{\mathcal{U}}_{k}$ with some desired probability. This
constraint can be included in Algorithm 1 as
$\mathbb{E}_{\tilde{f}_{\mathbf{U}}^{k}}\left[\mathds{1}_{\bar{\mathcal{U}}_{k}}(\mathbf{U}_{k})\right]$.

##  V Benchmarking the DFPD on an Inverted Pendulum

We now numerically investigate the effectivenes of the DFPD by using an
inverted pendulum with actuation constraints as test-bed. We first describe
the environment, then we describe how the inputs to Algorithm 1 are obtained.
We finally discuss the numerical results. The fully documented code to
replicate the results can be found at https://github.com/unisa-acg/discrete-
fpd.

###  V-A The environment

We consider the task of stabilizing a pendulum on its unstable equilibrium.
The pendulum we want to stabilize (i.e. the target system) has parameters that
are different from the one used to collect the data (i.e. the reference
system). Specifically, the parameters of the reference system were as follows:
rod length, $l_{r}=0.2$ m, mass, $m_{r}=0.5$ kg, friction $b_{r}=8\cdot
10^{-5}$ Nm/(deg/s). Instead, for the target pendulum we have $l_{t}=0.4$ m,
$m_{t}=1$ kg, $b_{t}=1\cdot 10^{-5}$ Nm/(deg/s). As usual, the input to the
pendulum is the torque applied to its hinged end, i.e. ${\bf u}=u$, while the
state is ${\bf x}=\left[x_{1},x_{2}\right]^{T}$, with $x_{1}$ being the
angular position and $x_{2}=\dot{x}_{1}$ the angular velocity. Finally, in one
of our simulations, we impose the constraint that the target system cannot use
a torque that is larger than $50\%$ of the torque capacity, say
$\tau_{t,max}=11.5$ Nm, i.e. $-0.5\leq{\bf u}\leq 0.5$.

###  V-B Obtaining the inputs to Algorithm 1.

Data were obtained by simulating the nonlinear dynamics of the target and the
reference system.222Mathematical models are only used to generate the data and
are not leveraged to compute the control action The dynamics of the two
systems were stochastic: zero mean Gaussian noise processes with variances
$\sigma_{r}^{2}=20$ and $\sigma_{t}^{2}=10$ were additively added to the
acceleration of the reference and target dynamics, respectively. Data were
generated by making the reference system follow a path that takes it from the
stable equilibrium state ${\bf x}=\left[-\pi/2,0\right]^{T}$ to the unstable
equilibrium state ${\bf x}=\left[\pi/2,0\right]^{T}$. Examples were generated
via a model-based PID controller, simulating a number of control policies that
differed in the time law used by the pendulum to reach the target position.
The time parametrization was obtained through the phase plane technique, see
e.g. [36]. We used this technique as it allowed us to formulate the trajectory
generation problem for the pendulum with a reduced set of parameters. In fact
all the trajectories belong to one of two classes having either one or three
characteristic switching points (simply switching points in what follows – see
[36] for the precise definition) in the phase plane, that could be suitably
assigned through randomized uniform sampling in a given range. The switching
points were then connected through linear interpolation. Figure 1 and Figure 2
provide two different views of reference system’s position, velocity and
torque signals over 100 simulations, where $30\%$ of trajectories belong to
the 1-switching-point class, while the remaining ones belong to the
3-switching-point class. In order to collect data on the state evolution model
of the target pendulum, we provided it with open loop torques that excited the
system at different configurations. All simulations for the reference and
target systems were executed with $\Delta t=0.01$ s.

Figure 1: Reference system data (single trajectories) over 100 simulations.

Figure 2: Reference system data over 100 simulations (same as Figure 1); bold
lines denote means, shaded areas the confidence intervals corresponding to the
standard deviation.

The generation of the probabilistic model followed the arguments of Section
IV. First, the state and input domains were discretized, then the
probabilistic models were computed as in (9), assuming time invariance.
Discrete state domain boundaries were determined on the basis of the maximum
and minimum values observed in the reference and target data, thus ${\bf
x}_{0}=\left[-1.5872,-1.8107\right]^{T}$ and ${\bf
x}_{m-1}=\left[1.9583,19.6537\right]^{T}$. The state discretization step
$\Delta{\bf x}$ was selected from a trade-off between accuracy and computation
time. In our numerical experiments we set $\Delta{\bf
x}=\left[0.1223,0.7402\right]^{T}$, as this empirically yielded satisfactory
results. For the inputs, we observed that the target system is bigger and
heavier than the reference one, from which one should expect higher torques.
In our implementation, we considered symmetric torque limits and inputs of the
reference system were normalized as $u=\tau_{r}/\tau_{r,max}$, while those of
the target system as $u=\tau_{t}/\tau_{t,max}$, so that $u$ takes the common
semantics of an effort capacity. In these expressions $\tau_{r}$ and
$\tau_{t}$ are the observed torques of the reference and target system
respectively, $\tau_{r,max}$ and $\tau_{t,max}$ are the maximum torques of the
reference and target system respectively, as observed from data. Finally we
set $\Delta u=0.0513$ and, in (2), $u_{0}=-1$ and $u_{z-1}=1$, so as to
respect the ranges of feasible inputs by construction. Given this set-up, we
computed the probability functions $\tilde{Q}_{X}$, $\tilde{Q}_{U}$ and
$\tilde{P}_{X}$ needed by Algorithm 1. For the last experiment, as last input
to Algorithm 1, we defined a constraint on the torque of the pendulum (see the
first paragraph of this section). The constraint was captured via a bound
constraint.

###  V-C Results

We obtained the optimal policy to control the target system from data
collected from the reference system via Algorithm 1. In particular, by
leveraging the stationarity of the system and of the constraints, we used the
observation given in Remark III.3 to retrieve the optimal policy. Namely, once
the problem was solved off-line, we sampled, at each $k$, the control input
from the conditional probability function $\tilde{P}^{0}_{U}$. This policy was
the one used at run-time to generete the actual torque inputs to the pendulum.
Specifically, at each control cycle: (i) the current state (pendulum position
and velocity) is read; (ii) quantization is retrieved; (iii) the right
histogram for $\tilde{P}^{0}_{U}({\bf u}|{\bf x}_{j})$ is queried from the
memory (note indeed that $\tilde{P}^{0}_{U}({\bf u}|{\bf x}_{j})$ is
conditioned and hence the histogram depends on the current state of the
pendulum). From the histogram, the control input with the highest-probability
value was selected and the torque was obtained as
$\tau_{t}=u\cdot\tau_{t,max}$. The discrete FPD optimization was executed with
$n=10$. The pendulum position, velocity and torque obtained with the FPD
control policy are reported in blue in Figure 3 (colors online): the data
driven control policy correctly stabilizes the pendulum around the state ${\bf
x}=\left[\pi/2,0\right]^{T}$, imitating the behavior of the reference system
(instead shown in Figure 2). In Figure 3 we also report, in red, the result of
using the same policy on the reference system. The figure shows that, when the
policy from the reference system is exported to control the target system,
this is not able to stabilize the pendulum on the desired equilibrium. The
reason for this is in the fact that the parameters of the two systems were
different. In these simulations there were no constraints and our last
validation step consists in verifying the ability of the DFPD to stabilize the
pendulum even in the presence of the constraints mentioned in the first
paragraph of this section. In Figure 4 the behavior of the closed loop system
is shown when these constraints were added. The figure clearly shows that DFPD
was able to stabilize the pendulum in the presence of these constraints.

Figure 3: Time evolution of position, velocity and torque when the data-driven
control policy is used to control the target (blue) and reference (red)
system. Colors online.

Figure 4: Target pendulum trajectory through data-driven control with
restricted torque constraints

##  VI Conclusions

Motivated by the problem of designing control policies from example data for
constrained systems having a possibly stochastic and nonlinear dynamics, we
introduced the principled design of a pipeline that enables control synthesis
in these situations. The pipeline expounds an algorithm from [1] for the
synthesis of control policies from data collected from a system that is
different from the one under control, without requiring that the constraints
are already fulfilled in the possibly noisy example data. We benchmarked
numerically the DFPD on an example that involves controlling an inverted
pendulum. The pendulum was affected by actuation constraints and the
demonstration data were collected from a physically different pendulum that
does not satisfy these constraints. Our simulations highlighted that DFPD is
able to stabilize the pendulum directly from the data while satisfying the
constraints embedded in the problem formulation. The fully documented code
implementing our design is also made openly available.

We plan to build on the pipeline presented here to develop a fully end-to-end
pipeline for control synthesis from demonstrations. In doing so, we also plan
to characterize how discretization affects control performance and to devise
control action/state dimensionality reduction techniques that can be useful to
scale the approach we presented. Finally, we aim to deliver a large scale
demonstrator of the end-to-end pipeline.

##  Appendix A Sketch of the derivation of Problem III.1

The derivations are adapted from [1] and we refer to such a paper for the
rigorous arguments. Here we only focus on the cost in Problem III.1 as the
constraints are obtained immediately from Problem II.1. Following the chain
rule for the KL-divergence, the cost of Problem II.1 can be rewritten

$\begin{split}\min_{\tilde{P}_{U}^{0},\ldots,\tilde{P}_{U}^{n-2}}\Bigg{\\{}&\mathcal{D}_{KL}(P^{n-1}||Q^{n-1})+\\\
&+\min_{\tilde{P}^{n-1}_{U}}\mathbb{E}_{P^{n-1}}\left[\mathcal{D}_{KL}(\tilde{P}^{n}||\tilde{Q}^{n})\right]\Bigg{\\}}.\end{split}$
(11)

The fact that $\mathcal{D}_{KL}(\tilde{P}^{n}||\tilde{Q}^{n})$ is only a
function of ${\bf x}(n-1)$ implies that the expectation in the inner
minimization can be taken over ${P\left({\bf x}(n-1)\right)}$. Linearity of
the expectation together with the fact that the decision variable of the inner
optimization does not depend on $P\left({\bf x}(n-1)\right)$, imply that (11)
can be recast as

$\min_{\tilde{P}_{U}^{0},\ldots,\tilde{P}_{U}^{n-2}}\left\\{\mathcal{D}_{KL}(P^{n-1}||Q^{n-1})+\mathbb{E}_{P\left({\bf
x}(n-1)\right)}\left[d\big{(}{\bf x}(n-1)\big{)}\right]\right\\}$ (12)

where $d\big{(}{\bf
x}(n-1)\big{)}:=\min_{\tilde{P}^{n-1}_{U}}\mathcal{D}_{KL}(\tilde{P}^{n}||\tilde{Q}^{n})$.
The chain rule applied on $\mathcal{D}_{KL}(P^{n-1}||Q^{n-1})$ allows us to
write (12) as

$\begin{split}\min_{\tilde{P}_{U}^{0},\ldots,\tilde{P}_{U}^{n-2}}\bigg{\\{}&\mathcal{D}_{KL}(P^{n-2}||Q^{n-2})+\\\
&+\mathbb{E}_{P^{n-2}}\left[\mathcal{D}_{KL}(\tilde{P}^{n-1}||\tilde{Q}^{n-1})\right]+\\\
&+\mathbb{E}_{P\left({\bf x}(n-1)\right)}\left[d\big{(}{\bf
x}(n-1)\big{)}\right]\bigg{\\}},\end{split}$ (13)

that, following the same arguments outlined above, can be written as

$\begin{split}\min_{\tilde{P}_{U}^{0},\ldots,\tilde{P}_{U}^{n-3}}\Bigg{\\{}&\mathcal{D}_{KL}(P^{n-2}||Q^{n-2})+\\\
&+\mathbb{E}_{P\left({\bf
x}(n-2)\right)}\bigg{[}\min_{\tilde{P}_{U}^{n-2}}\Big{\\{}\mathcal{D}_{KL}(\tilde{P}^{n-1}||\tilde{Q}^{n-1})+\\\
&+\mathbb{E}_{P\left({\bf x}(n-1)\right)}\left[d\big{(}{\bf
x}(n-1)\big{)}\right]\Big{\\}}\bigg{]}\Bigg{\\}}.\end{split}$ (14)

It can then be shown that, at each $k$, the inner optimization can be written
as:

$\begin{split}d\left({\bf
x}(k)\right):=\min_{\tilde{P}_{U}^{k}}\Bigg{\\{}&\mathcal{D}_{KL}(\tilde{P}_{U}^{k}||\tilde{Q}_{U}^{k})+\\\
&+\sum_{{\bf
u}(k)}\tilde{P}_{U}^{k}\mathcal{D}_{KL}(\tilde{P}_{X}^{k+1}||\tilde{Q}_{X}^{k+1})+\\\
&+\mathbb{E}_{P\left({\bf x}(k+1)\right)}\left[d\big{(}{\bf
x}(k+1)\big{)}\right]\Bigg{\\}}.\end{split}$ (15)

We now introduce node indices, yielding the equivalent formulation

$\begin{split}d\left({\bf
x}(k)\right)=\min_{\tilde{P}_{U}^{k}}&\Bigg{\\{}\sum_{h=0}^{z-1}P\big{(}{\bf
u}_{h}(k)|{\bf x}_{i}(k)\big{)}\ln\frac{P\big{(}{\bf u}_{h}(k)|{\bf
x}_{i}(k)\big{)}}{Q\big{(}{\bf u}_{h}(k)|{\bf x}_{i}(k)\big{)}}+\\\
&+\sum_{h=0}^{z-1}P\big{(}{\bf u}_{h}(k)|{\bf
x}_{i}(k)\big{)}\mathcal{D}_{KL}(\tilde{P}_{X}^{k+1}||\tilde{Q}_{X}^{k+1})+\\\
&+\sum_{h=0}^{z-1}P\big{(}{\bf u}_{h}(k)|{\bf x}_{i}(k)\big{)}\\\
&\sum_{j=0}^{m-1}P\big{(}{\bf x}_{j}|{\bf x}_{i}(k),{\bf
u}_{h}(k)\big{)}d\big{(}{\bf x}_{j}(k+1)\big{)}\Bigg{\\}},\end{split}$ (16)

from which the cost function of Problem III.1 can be derived. It is easy to
recognize that the optimization for the last time instant can be easily
obtained by setting $d=0$, from which $r_{hi}=0$ follows.

## References

  * [1] D. Gagliardi and G. Russo, “On a probabilistic approach to synthesize control policies from example datasets,” _Automatica_ , vol. 137, p. 110121, 2022\.
  * [2] Z.-S. Hou and Z. Wang, “From model-based control to data-driven control: Survey, classification and perspective,” _Information Sciences_ , vol. 235, pp. 3 – 35, 2013.
  * [3] M. Hanawal, H. Liu, H. Zhu, and I. Paschalidis, “Learning policies for Markov Decision Processes from data,” _IEEE Transactions on Automatic Control_ , vol. 64, pp. 2298–2309, 2019.
  * [4] A. A. Al Makdah, V. Krishnan, and F. Pasqualetti, “Learning robust feedback policies from demonstrations,” 2021. [Online]. Available: https://arxiv.org/abs/2103.16629
  * [5] S. Tu, A. Robey, T. Zhang, and N. Matni, “On the sample complexity of stability constrained imitation learning,” in _Proceedings of The 4th Annual Learning for Dynamics and Control Conference_ , ser. Proceedings of Machine Learning Research, vol. 168. PMLR, 23–24 Jun 2022, pp. 180–191.
  * [6] A. Hussein, M. M. Gaber, E. Elyan, and C. Jayne, “Imitation learning: A survey of learning methods,” _ACM Comput. Surv._ , vol. 50, no. 2, apr 2017.
  * [7] H. Ravichandar, A. S. Polydoros, S. Chernova, and A. Billard, “Recent advances in robot learning from demonstration,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 3, no. 1, pp. 297–330, 2020.
  * [8] L. Brunke, M. Greeff, A. W. Hall, Z. Yuan, S. Zhou, J. Panerati, and A. P. Schoellig, “Safe learning in robotics: From learning-based control to safe reinforcement learning,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 5, no. 1, pp. 411–444, 2023/01/28 2022.
  * [9] Émiland Garrabé and G. Russo, “Probabilistic design of optimal sequential decision-making algorithms in learning and control,” _Annual Reviews in Control_ , vol. 54, pp. 81–102, 2022.
  * [10] V. Peterka, _Bayesian approach to system identification_. Elsevier, 1981, pp. 239–304.
  * [11] M. Kárný, “Towards fully probabilistic control design,” _Automatica_ , vol. 32, no. 12, pp. 1719–1722, dec 1996.
  * [12] M. Kárný and T. V. Guy, “Fully probabilistic control design,” _Systems & Control Letters_, vol. 55, no. 4, pp. 259–265, 2006.
  * [13] M. Kárný and T. Kroupa, “Axiomatisation of fully probabilistic design,” _Information Sciences_ , vol. 186, no. 1, pp. 105 – 113, 2012.
  * [14] R. Herzallah, “Fully probabilistic control for stochastic nonlinear control systems with input dependent noise,” _Neural networks_ , vol. 63, pp. 199–207, 2015.
  * [15] T. V. Guy, S. F. Derakhshan, and J. Štěch, “Lazy fully probabilistic design: Application potential,” in _Multi-Agent Systems and Agreement Technologies_ , F. Belardinelli and E. Argente, Eds. Cham: Springer International Publishing, 2018, pp. 281–291.
  * [16] B. G. Pegueroles and G. Russo, “On robust stability of fully probabilistic control with respect to data-driven model uncertainties,” in _2019 18th European Control Conference (ECC)_ , 2019, pp. 2460–2465.
  * [17] E. Todorov, “Efficient computation of optimal actions,” _Proceedings of the National Academy of Sciences_ , vol. 106, no. 28, pp. 11 478–11 483, 2009\.
  * [18] ——, “Linearly-solvable Markov decision problems,” in _Advances in Neural Information Processing Systems_ , B. Schölkopf, J. Platt, and T. Hoffman, Eds., vol. 19. MIT Press, 2007\.
  * [19] H. Kappen, Goḿez, and M. Opper, “Optimal control as a graphical model inference problem,” _Machine Learning_ , vol. 87, pp. 159–182, 2012.
  * [20] P. Guan, M. Raginsky, and R. M. Willett, “Online Markov Decision processes with Kullback–Leibler control cost,” _IEEE Transactions on Automatic Control_ , vol. 59, no. 6, pp. 1423–1438, 2014.
  * [21] G. Russo, “On the crowdsourcing of behaviors for autonomous agents,” _IEEE Control Systems Letters_ , vol. 5, no. 4, pp. 1321–1326, 2021.
  * [22] E. Garrabe and G. Russo, “On the design of autonomous agents from multiple data sources,” _IEEE Control Systems Letters_ , vol. 6, pp. 698–703, 2022\.
  * [23] M. S. Gandhi, B. Vlahov, J. Gibson, G. Williams, and E. A. Theodorou, “Robust model predictive path integral control: Analysis and performance guarantees,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 1423–1430, 2021.
  * [24] S. Kullback and R. A. Leibler, “On information and sufficiency,” _The Annals of Mathematical Statistics_ , vol. 22, no. 1, pp. 79–86, mar 1951.
  * [25] M. Kárný and T. V. Guy, “Fully probabilistic control design,” _Systems & Control Letters_, vol. 55, no. 4, pp. 259–265, apr 2006.
  * [26] E. Garrabé and G. Russo, “Crawling: a crowdsourcing algorithm on wheels for smart parking,” 2022. [Online]. Available: https://arxiv.org/abs/2212.02467
  * [27] H. J. Van Waarde, J. Eising, H. L. Trentelman, and M. K. Camlibel, “Data informativity: a new perspective on data-driven analysis and control,” _IEEE Transactions on Automatic Control_ , vol. 65, no. 11, pp. 4753–4768, 2020.
  * [28] C. De Persis and P. Tesi, “Formulas for data-driven control: Stabilization, optimality, and robustness,” _IEEE Transactions on Automatic Control_ , vol. 65, no. 3, pp. 909–924, 2020.
  * [29] H. J. van Waarde, C. De Persis, M. K. Camlibel, and P. Tesi, “Willems’ fundamental lemma for state-space systems and its extension to multiple datasets,” _IEEE Control Systems Letters_ , vol. 4, no. 3, pp. 602–607, 2020\.
  * [30] K. Colin, X. Bombois, L. Bako, and F. Morelli, “Data informativity for the open-loop identification of mimo systems in the prediction error framework,” _Automatica_ , vol. 117, p. 109000, 2020.
  * [31] T. Georgiou and A. Lindquist, “Kullback-Leibler approximation of spectral density functions,” _IEEE Transactions on Information Theory_ , vol. 49, no. 11, pp. 2910–2917, 2003.
  * [32] M. Pavon and A. Ferrante, “On the Georgiou-Lindquist approach to constrained Kullback-Leibler approximation of spectral densities,” _IEEE Transactions on Automatic Control_ , vol. 51, no. 4, pp. 639–644, 2006\.
  * [33] B. Zhu and G. Baggio, “On the existence of a solution to a spectral estimation problem à la Byrnes–Georgiou–Lindquist,” _IEEE Transactions on Automatic Control_ , vol. 64, no. 2, pp. 820–825, 2019.
  * [34] C. D. McKinnon and A. P. Schoellig, “Learn fast, forget slow: Safe predictive learning control for systems with unknown and changing dynamics performing repetitive tasks,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 2180–2187, 2019.
  * [35] Y. K. Nakka, A. Liu, G. Shi, A. Anandkumar, Y. Yue, and S.-J. Chung, “Chance-constrained trajectory optimization for safe exploration and learning of nonlinear systems,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 389–396, 2021.
  * [36] J.-J. E. Slotine and H. S. Yang, “Improving the efficiency of time-optimal path-following algorithms,” _IEEE Transactions on Robotics and Automation_ , vol. 5, no. 1, pp. 118–124, 1989.