# Confidence-Aware Safe and Stable Control of Control-Affine Systems

Shiqing Wei, Prashanth Krishnamurthy, and Farshad Khorrami The authors are
with Control/Robotics Research Laboratory, Department of Electrical and
Computer Engineering, NYU Tandon School of Engineering, 5 Metrotech Center,
Brooklyn, NY 11201, USA. {shiqing.wei, prashanth.krishnamurthy,
<EMAIL_ADDRESS>work was supported by the New York University Abu Dhabi
(NYUAD) Center for Artificial Intelligence and Robotics funded by Tamkeen
under the NYUAD Research Institute Award CG010.

###### Abstract

Designing control inputs that satisfy safety requirements is crucial in
safety-critical nonlinear control, and this task becomes particularly
challenging when full-state measurements are unavailable. In this work, we
address the problem of synthesizing safe and stable control for control-affine
systems via output feedback (using an observer) while reducing the estimation
error of the observer. To achieve this, we adapt control Lyapunov function
(CLF) and control barrier function (CBF) techniques to the output feedback
setting. Building upon the existing CLF-CBF-QP (Quadratic Program) and CBF-QP
frameworks, we formulate two confidence-aware optimization problems and
establish the Lipschitz continuity of the obtained solutions. To validate our
approach, we conduct simulation studies on two illustrative examples. The
simulation studies indicate both improvements in the observer’s estimation
accuracy and the fulfillment of safety and control requirements.

##  I Introduction

For safety-critical control problems, it is of paramount importance to design
controllers that not only satisfy the system performance requirements, but
also ensure the system’s safety during operation. Recently, control barrier
functions (CBFs) have emerged as a popular approach to generate safe control
inputs for a wide range of control tasks [1, 2, 3, 4]. In general, the CBF is
designed based on a designated safe set, and the controls are generated to
make this safe set forward-invariant, ensuring the safety of the system.
However, this task becomes particularly challenging in scenarios where full-
state measurements are unavailable, and one instead has to rely on partial
information about the system states. In this work, we address the problem of
formulating controls that can both guarantee safety via output feedback (using
an observer) and improve confidence about the states.

To achieve these objectives, we employ an EKF (Extended Kalman Filter) based
nonlinear observer [5] and adapt control Lyapunov functions (CLFs) and CBFs to
the setting of output feedback. We extend the existing quadratic programs
(QPs), such as CLF-CBF-QP and CBF-QP, and design control inputs that can speed
up the observer’s convergence and meet the necessary safety and control
requirements.

Synthesizing safe control based on output measurements is an ongoing research
area [6, 7, 8, 9, 10]. In [6] and [10], the authors develop motion-planning
systems to generate safe trajectories based on image sensor/laser
measurements. Other works (e.g., [7, 8]) have focused on employing observers
with a quantified estimation error and solving QPs for a safe controller. We
inherited the definition of safe output-feedback controllers and observer-
robust CBFs in [7]. In [9], CBFs are formulated for stochastic systems with
incomplete state information. However, these approaches primarily follow a
“top-down” paradigm in the sense that no interaction with the observer is made
in the control design process. Typically, the observer’s estimation error is
influenced by the control inputs, and in this study, we demonstrate that
safety can be consistently achieved by selecting controls that enhance the
estimation accuracy of the observer.

Our work is also related to active sensing or observability optimization,
where a “bottom-up” approach is adopted to reduce uncertainty or enhance
system observability. In [11], optimal paths are identified to improve the
system observability for under-sensed vehicles in a planar uniform flow field.
The works [12] and [13] propose a perception-aware trajectory generation
method aimed at maximizing the information collected by output measurements
for autonomous robots. In [14], the authors address the problem of
observability-aware target tracking for mobile robots using a nonlinear model
predictive control framework. In contrast to the aforementioned studies, our
work incorporates both stability and safety requirements into the search for
control inputs that enhance system observability.

Our Contributions: (1) We present an optimization-based control approach that
addresses the design of safe and stabilizing controls for control-affine
nonlinear systems using output feedback, specifically focusing on enhancing
state confidence. (2) We extend the existing CLF-CBF-QP and CBF-QP frameworks
and formulate two optimization problems incorporating confidence-aware
considerations. (3) We prove the feasibility of these optimization problems
and demonstrate the Lipschitz continuity of the obtained solutions. (4) We
demonstrate the effectiveness of our approach through simulation studies on
two illustrative examples: a second-order nonlinear system stabilization
problem and a unicycle tracking problem. The results indicate notable
improvements in the observer’s estimation accuracy, alongside the successful
fulfillment of safety and control requirements.

##  II EKF-Based Nonlinear Observer

Consider the following nonlinear system with dynamics

$\dot{x}=p(x,u),\quad z=q(x)$ (1)

where $x\in\mathbb{R}^{n_{x}}$ is the state, $u\in\mathbb{R}^{n_{u}}$ is the
control input, and $z\in\mathbb{R}^{n_{z}}$ is the output. The functions
$p:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{u}}\rightarrow\mathbb{R}^{n_{x}}$
and $q:\mathbb{R}^{n_{x}}\rightarrow\mathbb{R}^{n_{z}}$ are assumed to be
locally Lipschitz and $\mathcal{C}^{2}$ functions. An EKF based nonlinear
observer for system (1) is proposed in [5]

$\dot{\hat{x}}=p(\hat{x},u)+K(t)[z-q(\hat{x})]$ (2)

where $\hat{x}\in\mathbb{R}^{n_{x}}$ is the estimated state and the time-
varying observer gain $K(t)$ is a $n_{x}\times n_{z}$ matrix. Denote by $A(t)$
and $C(t)$ the following partial differentials

$A(t)=\frac{\partial p}{\partial x}(\hat{x},u)\quad\text{and}\quad
C(t)=\frac{\partial q}{\partial x}(\hat{x}).$ (3)

For $\kappa\geq 0$ and symmetric positive definite matrices
$Q\in\mathbb{R}^{n_{x}\times n_{x}}$ and $R\in\mathbb{R}^{n_{z}\times n_{z}}$,
the observer gain is defined as

$K(t)=P(t)C^{\top}(t)R^{-1}$ (4)

where $P(t)$ is the solution to the Riccati equation

$\dot{P}=\kappa P+AP+PA^{\top}-PC^{\top}R^{-1}CP+Q.$ (5)

We drop the time dependence of $A(t)$, $C(t)$, and $P(t)$ for simplicity when
it does not cause confusion.

######  Assumption 1.

There exist two constants $\underline{p},\bar{p}>0$ such that

$\underline{p}I\leq P(t)\leq\bar{p}I,\quad\forall t\geq 0.$ (6)

The above assumption is made in many works on EKF-based observers (e.g., [5,
15]). As pointed out in [15], this assumption can be practically checked in
the following way: the user keeps track of the bounds $\underline{p}(t)$ and
$\bar{p}(t)$ such that $\underline{p}(t)I<P(s)<\bar{p}(t)I$ for $s\leq t$ and
verify that the bound on the estimation error associated with
$\underline{p}(t)$ and $\bar{p}(t)$ holds (at least) up to time $t$. Analogous
to the EKF in the probabilistic setting, we call $P(t)$ the uncertainty of the
estimated states and $S(t)=P^{-1}(t)$ the confidence of the observer. Noting
that $\dot{S}(t)=-P^{-1}(t)\dot{P}(t)P^{-1}(t)$, we obtain the dynamics of the
confidence $S(t)$ by rearranging (5):

$\dot{S}=-\kappa S-A^{\top}S-SA+C^{\top}R^{-1}C-SQS.$ (7)

##  III Observer-Based Safe and Stable Control

###  III-A Local Exponential Stability of the Observer

Consider the plant

$\dot{x}=f(x)+g(x)u,\quad z=q(x)$ (8)

where we assume that $f:\mathcal{X}\rightarrow\mathbb{R}^{n_{x}}$,
$g:\mathcal{X}\rightarrow\mathbb{R}^{n_{x}\times n_{u}}$, and
$q:\mathcal{X}\rightarrow\mathbb{R}^{n_{z}}$ are of class $\mathcal{C}^{2}$,
and note $\mathcal{I}=[0,t_{\text{max}})$ as the maximal interval of
existence. The state $x$, control $u$, and output $z$ are of dimensions
$n_{x}$, $n_{u}$, and $n_{z}$, respectively. To model the physical constraints
of the real world, we assume $x\in\mathcal{X}$ and $u\in\mathcal{U}$ where
$\mathcal{X}$ and $\mathcal{U}$ are compact subsets of $\mathbb{R}^{n_{x}}$
and $\mathbb{R}^{n_{u}}$, respectively. We further assume that the origin is
an equilibrium of (8) for $u=0$, $q(0)=0$, and $0\in\mathcal{X}$.

As (8) is a special case of (1), the observer (2) is equally applicable to
system (8). Recall the dynamics of the estimated state $\hat{x}(t)$ using the
confidence $S(t)$ are

$\dot{\hat{x}}=f(\hat{x})+g(\hat{x})u+S^{-1}(t)C^{\top}(t)R^{-1}[z-q(\hat{x})],$
(9)

and the partial differentials in (3) become

$A(t)=\frac{\partial f}{\partial x}(\hat{x})+\frac{\partial g}{\partial
x}(\hat{x})u\quad\text{and}\quad C(t)=\frac{\partial q}{\partial x}(\hat{x})$
(10)

where $\frac{\partial f}{\partial x}$ and $\frac{\partial q}{\partial x}$ are
Jacobian matrices, and $\frac{\partial g}{\partial x}$ is a
tensor111$\frac{\partial f}{\partial x}$ and $\frac{\partial q}{\partial x}$
are $n_{x}\times n_{x}$ and $n_{z}\times n_{x}$ matrices respectively.
$\frac{\partial g}{\partial x}$ is a three dimensional tensor of size
$n_{x}\times n_{x}\times n_{u}$. The product $\frac{\partial g}{\partial x}u$
is a $n_{x}\times n_{x}$ matrix.. As $R$ is a fixed positive definite matrix
chosen by the user, it can be bounded by $\underline{r}I\leq R\leq\bar{r}I$
with $\underline{r},\bar{r}>0$. We further assume that the estimated state
$\hat{x}\in\hat{\mathcal{X}}$ and $\hat{\mathcal{X}}$ is a compact subset of
$\mathbb{R}^{n_{x}}$. Under an output-feedback controller
$\pi:\mathcal{I}\times\hat{\mathcal{X}}\times\mathbb{R}^{n_{z}}\rightarrow\mathcal{U}$,
the closed-loop system (8) along with the observer is

$\displaystyle\dot{x}$ $\displaystyle=f(x)+g(x)\pi(t,\hat{x},z),\quad z=q(x),$
$\displaystyle\dot{\hat{x}}$
$\displaystyle=f(\hat{x})+g(\hat{x})\pi(t,\hat{x},z)+S^{-1}C^{\top}R^{-1}[z-q(\hat{x})].$
(11)

Denote the estimation error of the observer by

$\zeta(t)=x(t)-\hat{x}(t).$ (12)

It is proved in [5] that the estimation error $\zeta(t)$ is locally
exponentially convergent to zero.

######  Proposition 1 ([5]).

Under Assumption 1 and assumptions on the boundedness of $\mathcal{X}$,
$\mathcal{U}$, and $\hat{\mathcal{X}}$ and the $\mathcal{C}^{2}$-smoothness of
$f$, $g$, and $q$, the EKF-based observer in (9) is a local exponential
observer. More specifically, there exist positive real numbers
$\epsilon,\eta>0$ and $\theta>\kappa/2$ such that

$\lVert\zeta(t)\rVert\leq\eta\lVert\zeta(0)\rVert e^{-\theta t}$ (13)

for $t\geq 0$ with $\zeta(0)\in B_{\epsilon}$ where
$B_{\epsilon}=\\{v\in\mathbb{R}^{n_{x}}:\lVert v\rVert<\epsilon\\}$.

###  III-B Control Lyapunov Functions

Control Lyapunov functions (CLFs) are commonly used to prove a closed-loop
system’s stability. In the context of output feedback, we introduce the
following definition.

######  Definition 1.

For system (8) and the observer (9) with known estimation error bound (13), a
class $\mathcal{C}^{2}$ positive definite function
$V:\mathcal{X}\cup\hat{\mathcal{X}}\rightarrow\mathbb{R}_{+}$ is an observer-
based exponentially stabilizing CLF, if there exists a constant $\gamma>0$ and
two class $\mathcal{K}$ functions $\alpha_{1},\alpha_{2}$ such that $\forall
x\in\mathcal{X}\cup\hat{\mathcal{X}}$

$\displaystyle\ \alpha_{1}(\lVert x\rVert)\leq V(x)\leq\alpha_{2}(\lVert
x\rVert),$ (14)
$\displaystyle\inf_{u\in\mathcal{U}}\left(L_{f}V(x)+L_{g}V(x)u+\gamma
V(x)\right)\leq 0$ (15)

where $L_{f}V(\cdot)=\nabla V^{\top}(\cdot)f(\cdot)$ and $L_{g}V(\cdot)=\nabla
V^{\top}(\cdot)g(\cdot)$ are the Lie derivatives of $V$ w.r.t. $f$ and $g$,
respectively, and $\nabla
V:\mathcal{X}\cup\hat{\mathcal{X}}\rightarrow\mathbb{R}^{n_{x}}$ is the
gradient of $V$.

Compared with the definition of the CLF in [16] and [17], Definition 1
requires the additional exponential stability of the Lyapunov function as in
[1]. As we are working with the estimated state $\hat{x}$ given by the
observer, we further extend the domain of $V$ to
$\mathcal{X}\cup\hat{\mathcal{X}}$. The $\mathcal{C}^{2}$-smoothness is
required by later analysis in Section IV. Next, consider the set

$K_{\text{clf}}(\hat{x})\\!=\\!\left\\{u\in\mathcal{U}\\!:\\!L_{f}V(\hat{x})\\!+\\!L_{g}V(\hat{x})u\\!+\\!\gamma
V(\hat{x})\\!<\\!0\right\\}.$ (16)

The following result shows that given a sufficiently accurate initial state of
the observer, a Lipschitz continuous output-feedback controller
$\pi(\hat{x})\in K_{\text{clf}}(\hat{x})$ renders system (8) asymptotically
stable.

######  Theorem 1.

Assume that the conditions of Proposition 1 hold and the initial estimation
error satisfies $x(0)-\hat{x}(0)\in B_{\epsilon}$ as in Proposition 1. If such
a CLF $V$ exists as in Definition 1, then any Lipschitz continuous controller
$\pi(\hat{x})\in K_{\text{clf}}(\hat{x})$ asymptotically stabilizes system (8)
with the observer (9) of known estimation error bound (13).

###### Proof.

Since the conditions of Proposition 1 hold and $x(0)-\hat{x}(0)\in
B_{\epsilon}$, we have $\lVert x(t)-\hat{x}(t)\rVert\leq M(t)$ with
$M(t)=\eta\lVert\zeta(0)\rVert e^{-\theta t}$. Noting that $V$ is continuously
differentiable and $\mathcal{X}\cup\hat{\mathcal{X}}$ is compact, we denote
$K_{V}$ as the Lipschitz constant of $V$. Then, for $t\geq 0$, we have
$V(x(t))\leq V(\hat{x}(t))+K_{V}M(t)\coloneqq W(t).$ By (9), we have

$\displaystyle\dot{W}(t)$
$\displaystyle=L_{f}V(\hat{x})+L_{g}V(\hat{x})u+K_{V}\dot{M}(t)$
$\displaystyle+$ $\displaystyle\nabla
V(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[q(x)-q(\hat{x})].$ (17)

Denote by $K_{q}$ the Lipschitz constant of $q$ and by the definition of
$C(t)$ in (10), we have $\lVert C(t)\rVert<K_{q}$. Note that $\lVert\nabla
V(\hat{x})\rVert\leq K_{V}$, $\lVert S^{-1}(t)\rVert\leq\bar{p}$ (by
Assumption 1), and $\lVert R^{-1}\rVert<\underline{r}^{-1}$, then it follows

$\left\lVert\nabla
V(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[q(x)-q(\hat{x})]\right\rVert\leq
bM(t)$ (18)

where $b=\underline{r}^{-1}\bar{p}K_{V}K_{q}^{2}$. Considering the output-
feedback controller $\pi(\hat{x})\in K_{\text{clf}}(\hat{x})$ and the bound in
(18), one has

$\displaystyle\dot{W}(t)$ $\displaystyle\leq-\gamma
V(\hat{x}(t))+bM(t)+K_{V}\dot{M}(t)$ $\displaystyle=-\gamma W(t)+(b+\gamma
K_{V})M(t)+K_{V}\dot{M}(t).$

Construct the following ODE

$\dot{y}=-\gamma y+(b+\gamma K_{V})M(t)+K_{V}\dot{M}(t),\ y(0)=W(0).$ (19)

Then, we have $W(t)\leq y(t)$ for $t\geq 0$ by Comparison Lemma [18, Lemma
B.2]. Since $M(t)=\eta\lVert\zeta(0)\rVert e^{-\theta t}$ and
$\dot{M}(t)=-\theta\eta\lVert\zeta(0)\rVert e^{-\theta t}$, we can solve for
$y(t)$.

If $\gamma=\theta$, $y(t)=(W(0)+b\eta\lVert\zeta(0)\rVert t)e^{-\gamma t}$.
Since $V(x(t))\leq W(t)\leq y(t)$ and $V(x)\geq\alpha_{1}(\lVert x\rVert)$, we
have

$\lVert x(t)\rVert\leq\alpha_{1}^{-1}\left((W(0)+b\eta\lVert\zeta(0)\rVert
t)e^{-\gamma t}\right).$ (20)

If $\gamma\neq\theta$, $y(t)=W(0)e^{-\gamma
t}+\frac{c}{\gamma-\theta}(e^{-\theta t}-e^{-\gamma t})$ with
$c=\eta\lVert\zeta(0)\rVert(b+\gamma K_{V}-\theta K_{V})$. Similarly, we have

$\lVert x(t)\rVert\leq\alpha_{1}^{-1}\left(W(0)e^{-\gamma
t}+\frac{c}{\gamma-\theta}(e^{-\theta t}-e^{-\gamma t})\right).$ (21)

As $\alpha^{-1}$ is also a class $\mathcal{K}$ function and thus continuous,
it follows that in both cases, $\lVert x(t)\rVert\rightarrow 0$ as
$t\rightarrow\infty$ by (20) and (21), i.e., the system (8) is asymptotically
stable. ∎

###  III-C Control Barrier Functions

We say that the system (8) is safe if the true state $x(t)$ stays within the
safe set $\mathcal{S}$ characterized by

$\mathcal{S}=\\{x\in\mathcal{X}:h(x)\geq 0\\}$ (22)

where $h:\mathcal{X}\cup\hat{\mathcal{X}}\rightarrow\mathbb{R}$ is a
$\mathcal{C}^{1}$ function and
$\operatorname{Int}(\mathcal{S})\neq\varnothing$. When the knowledge of the
full state $x$ is available, as commonly assumed in the existing literature
[1], one tries to find a state-feedback controller that renders $\mathcal{S}$
forward-invariant, i.e., $x(0)\in\mathcal{S}\Rightarrow
x(t)\in\mathcal{S},\forall t\in\mathcal{I}$. However, in the setting of
output-feedback control, we need to ensure the safety of the true state $x(t)$
using only $\hat{x}(t)$.

######  Definition 2 ([7]).

An output-feedback controller
$\pi:\mathcal{I}\times\hat{\mathcal{X}}\times\mathbb{R}^{n_{z}}\rightarrow\mathcal{U}$
renders system (8) safe w.r.t. the set $\mathcal{S}$ if for the closed-loop
system (III-A)

$x(0)\in\mathcal{X}_{0}\text{ and
}\hat{x}(0)\in\hat{\mathcal{X}}_{0}\Rightarrow x(t)\in\mathcal{S},\forall
t\in\mathcal{I}$ (23)

where $\mathcal{X}_{0}\subset\mathcal{S}$ and
$\hat{\mathcal{X}}_{0}\subset\hat{\mathcal{X}}$ are sets of initial conditions
for $x(t)$ and $\hat{x}(t)$, respectively.

By Proposition 1, we have $\lVert\zeta(t)\rVert\leq M(t)$ with
$M(t)=\eta\lVert\zeta(0)\rVert e^{-\theta t}$ if $\lVert\zeta(0)\rVert\in
B_{\epsilon}$. Then, it follows that

$\displaystyle\dot{h}(\hat{x})$
$\displaystyle=L_{f}h(\hat{x})+L_{g}h(\hat{x})u$ $\displaystyle\quad+\nabla
h(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[q(x)-q(\hat{x})]$
$\displaystyle\geq
L_{f}h(\hat{x})+L_{g}h(\hat{x})u-\underline{r}^{-1}\bar{p}K_{h}K_{q}^{2}M(0)$

where $K_{h}$ is the Lipschitz constant of $h$ on
$\mathcal{X}\cup\hat{\mathcal{X}}$, $\lVert C(t)\rVert<K_{q}$, $\lVert
S^{-1}(t)\rVert\leq\bar{p}$, and $\lVert R^{-1}\rVert<\underline{r}^{-1}$.
Next, we introduce the definition of a CBF in the context of output feedback.

######  Definition 3 (Adapted from [7]).

A class $\mathcal{C}^{2}$ function
$h:\mathcal{X}\cup\hat{\mathcal{X}}\rightarrow\mathbb{R}$ is an observer based
CBF for system (8) with the observer (9) of known estimation error bound (13)
if there exists a constant $0<\alpha\leq\theta$ such that for all
$x\in\mathcal{S}$

$\sup_{u\in\mathcal{U}}\left(L_{f}h(x)+L_{g}h(x)u-\underline{r}^{-1}\bar{p}K_{h}K_{q}^{2}M(0)\right)\geq-\alpha
h(x).$

For a given CBF $h$, consider the set

$\displaystyle K_{\text{cbf}}$
$\displaystyle(t,\hat{x},z)=\\{u\in\mathcal{U}:L_{f}h(\hat{x})+L_{g}h(\hat{x})u+\alpha
h(\hat{x})$ $\displaystyle+\nabla
h(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[z-q(\hat{x})]\geq 0\\}.$ (24)

The next result shows that the controller $\pi(t,\hat{x},z)\in
K_{\text{cbf}}(t,\hat{x},z)$ renders system (8) safe if the initial conditions
$x(0)$ is relatively far from the boundary of the safe set $\mathcal{S}$ and
$\hat{x}(0)$ is close to $x(0)$. We assume that system (8) is of relative
degree one, i.e., $L_{g}h(x)\neq 0$ for
$x\in\mathcal{X}\cup\hat{\mathcal{X}}$.

######  Theorem 2.

Assume the conditions of Proposition 1 hold and a CBF $h$ exists as in
Definition 3. For system (8) with the observer (9) of known estimation error
bound (13), if

$\displaystyle x(0)\in\mathcal{X}_{0}$
$\displaystyle=\\{x\in\mathcal{S}:h(x)\geq 2K_{h}M(0)\\},$ (25)
$\displaystyle\hat{x}(0)\in\hat{\mathcal{X}}_{0}$
$\displaystyle=\\{\hat{x}\in\hat{\mathcal{X}}:x(0)-\hat{x}(0)\in
B_{\epsilon}\\},$ (26)

then any Lipschitz continuous controller $\pi(t,\hat{x},z)\in
K_{\text{cbf}}(t,\hat{x},z)$ renders system (8) safe w.r.t. the safe set
$\mathcal{S}$.

###### Proof.

Since $\hat{x}(0)\in\hat{\mathcal{X}}_{0}$, if follows that $h(x(t))\geq
h(\hat{x}(t))-K_{h}M(t)\coloneqq H(t)$. Given that $x(0)\in\mathcal{X}_{0}$,
we have

$H(0)=h(\hat{x}(0))-K_{h}M(0)\geq h(x(0))-2K_{h}M(0)\geq 0.$

Since $\pi(t,\hat{x},z)\in K_{\text{cbf}}(t,\hat{x},z)$, we have

$\displaystyle\dot{H}$
$\displaystyle=L_{f}h(\hat{x})+L_{g}h(\hat{x})u-K_{h}\dot{M}(t)$
$\displaystyle\quad\quad+\nabla
h(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[z-q(\hat{x})]$
$\displaystyle\geq-\alpha h-K_{h}\dot{M}(t)=-\alpha H-\alpha
K_{h}M(t)-K_{h}\dot{M}(t).$

Noting that $M(t)=\eta\lVert\zeta(0)\rVert e^{-\theta t}$ and
$0<\alpha\leq\theta$, we have $-\alpha K_{h}M(t)-K_{h}\dot{M}(t)\geq 0$ and
thus $\dot{H}\geq-\alpha H$. Given that $H(0)\geq 0$ and $h(x(t))\geq H(t)$,
we have $h(x(t))\geq H(t)\geq 0$, i.e., system (8) is safe w.r.t. the safe set
$\mathcal{S}$. ∎

In this work, by $\alpha\leq\theta$, we require that the observer have a
faster convergence than the CBF, and this aligns with the commonly accepted
principle that the observer should always converge faster than the controller
[7]. The benefit of this control design in (III-C) is that it does not require
explicitly calculating $M(t)$. If one explicitly knows $M(t)$, another way to
design a safe controller is presented in [7].

##  IV Confidence Optimization

As introduced in Section II, $P(t)$ is analogous to the covariance of the
state in the probabilistic setting. If we can optimize some metric of $P(t)$
by selecting proper control inputs $u$, we can speed up the convergence of the
observer and thus improve the performance of the feedback controller.

Recall that the confidence matrix is defined by $S(t)=P^{-1}(t)$. We choose
$\lambda_{\text{min}}(S(t))$ as the optimization metric, where
$\lambda_{\text{min}}(\cdot)$ denotes the minimal eigenvalue of a square
matrix. If $\lambda_{\text{min}}(S(t))$ can be increased, then we increase the
convergence rate for the slowest mode of the observer. Let $\Delta t$ be the
time difference between two consecutive control inputs. At time $t$, we
generate control inputs to maximize $\lambda_{\text{min}}(S(t+\Delta t))$,
i.e., to optimize $\lambda_{\text{min}}(S(t))$ at one step into the future. In
addition, as $S(t)$ satisfies the Riccati equation (7), $S(t+\Delta t)$ can be
approximated by

$\displaystyle S(t+\Delta t)$ $\displaystyle\approx S(t)+\Delta t[-\kappa
S(t)-A(t)^{\top}S(t)-S(t)A(t)$
$\displaystyle+C(t)^{\top}R^{-1}C(t)-S(t)QS(t)]$ (27)

using the first-order approximation.

######  Assumption 2.

All eigenvalues of $P(t)$ (or equivalently, $S(t)$) are distinct.

Assumption 2 is a practical assumption to guarantee the well-posedness of the
problem. In general, for a real symmetric matrix $X$,
$\lambda_{\text{min}}(X)$ is a concave function of $X$ and is also Lipschitz
continuous in $X$(see [19, Ch. 2]). If Assumption 2 holds, we further obtain
twice differentiability of $\lambda_{\text{min}}(X)$ w.r.t. $X$ [20]. It is
also worth noting that the set of positive definite matrices with distinct
eigenvalues is dense in the set of all positive definite matrices, as we can
always do small perturbations to the entries of a positive definite matrix
with repeated eigenvalues to obtain a positive definite matrix with distinct
eigenvalues.

######  Remark 1.

Apart from the minimum eigenvalue (E-Optimality), other metrics, such as the
condition number, the trace (A-Optimality), and the determinant
(D-Optimality), can be equally employed as optimization objectives in this
work. We opt for the minimum eigenvalue as it specifically addresses the
slowest mode of the estimation error.

###  IV-A Combining CLFs and CBFs via Convex Optimization

We are ready to formulate the confidence-aware safe and stable control as a
constrained optimization problem. The main benefit of optimization-based
control is that it allows us to optimize a certain performance objective
subject to both stability and safety requirements. More specifically, given a
CLF $V$ (Definition 1) and a CBF $h$ (Definition 3) associated with a safe set
$\mathcal{S}$, they can be incorporated into finding a single controller $\pi$
that can optimize the confidence of the observer through the optimization
problem (P1):

$\displaystyle\pi(t,\hat{x}$
$\displaystyle,z)=\operatorname*{arg\,min}_{u\in\mathbb{R}^{n_{u}}}\
u^{\top}u-c_{1}\lambda_{\text{min}}(S(t+\Delta t))+c_{2}\delta^{2}$ (P1) s.t.
$\displaystyle L_{f}V(\hat{x})+L_{g}V(\hat{x})u+\gamma V(\hat{x})\leq\delta,$
$\displaystyle L_{f}h(\hat{x})+L_{g}h(\hat{x})u+\alpha h(\hat{x})$
$\displaystyle\qquad+\nabla
h(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[z-q(\hat{x})]\geq 0$

where $c_{1}\geq 0$, $c_{2}>0$, and $\delta\in\mathbb{R}$ is a relaxation
variable. The CLF is taken as a soft constraint as in [1], while the CBF is
taken as a hard constraint. This is a convex problem because
$-\lambda_{\text{min}}(S(t+\Delta t))$ is convex w.r.t. $S(t+\Delta t)$ and
$S(t+\Delta t)$ has affine dependence on $u$. The following result proves the
Lipschitz continuity and safety of the controller $\pi$ given by the
optimization problem (P1).

######  Theorem 3.

Consider system (8) and observer (9) of a known error bound (13). Suppose that
the Assumptions 1 and 2 and conditions of Proposition 1 hold, $V$ is a CLF,
and $h$ is a CBF associated with the safe set $\mathcal{S}$. If the initial
conditions $x(0)$ and $\hat{x}(0)$ satisfy (25) and (26), then the controller
$\pi:\mathcal{I}\times\hat{\mathcal{X}}\times\mathbb{R}^{n_{z}}\rightarrow\mathbb{R}^{n_{u}}$
given by (P1) renders system (8) safe and is piecewise continuous w.r.t. $t$
and Lipschitz continuous w.r.t. $\hat{x}$ and $z$.

###### Proof.

We first prove the existence and uniqueness of the solution to (P1). Let
$a_{1}(\hat{x})=L_{g}V(\hat{x})$, $b_{1}(\hat{x})=-L_{f}V(\hat{x})-\gamma
V(\hat{x})$, $a_{2}(\hat{x})=-L_{g}h(\hat{x})$, and
$b_{2}(t,\hat{x},z)=L_{f}h(\hat{x})+\alpha h(\hat{x})+\nabla
h(\hat{x})^{\top}S^{-1}C^{\top}R^{-1}[z-q(\hat{x})]$. We omit their
dependencies in the following and use $a_{1},a_{2},b_{1}$, and $b_{2}$ for
brevity. The constraints in (P1) can be written as
$T[u^{\top},\delta]^{\top}\leq[b_{1},b_{2}]^{\top}$ with
$T=[a_{1},-1;a_{2},0]$ and we see that the rows of $T$ are linearly
independent. As there are $n_{u}+1$ (with $n_{u}\geq 1$) decision variables
and two linearly independent constraints, the problem is feasible. Since the
objective function is strongly convex, there exists one unique minimizer to
(P1).

Then, we prove the Lipschitz continuity of $\pi$. As the objective function of
(P1) is twice differentiable and strongly convex, its Hessian is positive
definite. As the constraints are linearly independent, the regularity
conditions of [21, Thm. D.1] are met. Therefore, $\pi$ is Lipschitz continuous
w.r.t. the data $a_{1},a_{2},b_{1},b_{2},A,C$ and $S(t)$. As the state-
dependent data are all Lipschitz continuous w.r.t. $\hat{x}$222This results
from the boundedness of $\mathcal{X}$ and $\hat{\mathcal{X}}$ and the
$\mathcal{C}^{2}$-smoothness of $f,g,q,h,$ and $V$., $b_{2}$ and $S(t)$ are
piecewise continuous in $t$, and $b_{2}$ is Lipschitz continuous in $z$, we
see that $\pi$ is piecewise continuous w.r.t. $t$ and Lipschitz continuous
w.r.t. $\hat{x}$ and $z$. Finally, $\pi$ renders system (8) safe w.r.t.
$\mathcal{S}$ because (III-C) holds as a hard constraint in (P1) and the
conditions of Theorem 2 are met. ∎

###  IV-B Tracking a Nominal Controller

In some cases, we may already have a nominal output-feedback controller
$\pi_{n}$ and would like to optimize the confidence of the observer while
guaranteeing safety. In this case, we can consider the following problem (P2):

$\displaystyle\pi(t,\hat{x}$
$\displaystyle,z)=\operatorname*{arg\,min}_{u\in\mathbb{R}^{n_{u}}}\ \lVert
u-\pi_{n}(\hat{x})\rVert^{2}-c_{1}\lambda_{\text{min}}(S(t+\Delta t))$ (P2)
s.t. $\displaystyle L_{f}h(\hat{x})+L_{g}h(\hat{x})u+\alpha h(\hat{x})$
$\displaystyle\qquad+\nabla
h(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[z-q(\hat{x})]\geq 0$

where the CBF is incorporated as a hard constraint, and the objective function
is a weighted sum of the tracking error and the cost on the smallest
eigenvalue of $S(t+\Delta t)$. If $c_{1}=0$, we recover the CBF-QP as in [1].

######  Theorem 4.

Consider system (8) and observer (9) of a known error bound (13). Suppose that
the Assumptions 1 and 2 and conditions of Proposition 1 hold, and $h$ is a CBF
associated with the safe set $\mathcal{S}$. If the initial conditions $x(0)$
and $\hat{x}(0)$ satisfy (25) and (26), and the nominal controller $\pi_{n}$
is Lipschitz continuous w.r.t. its argument, then the controller
$\pi:\mathcal{I}\times\hat{\mathcal{X}}\times\mathbb{R}^{n_{z}}\rightarrow\mathbb{R}^{n_{u}}$
given by (P2) renders system (8) safe and is piecewise continuous w.r.t. $t$
and Lipschitz continuous w.r.t. $\hat{x}$ and $z$.

###### Proof.

There are $n_{u}\geq 1$ decision variables and one constraint, so the problem
is feasible. Since the objective function is strongly convex, there exists a
unique minimizer to the problem. Denote $a_{1}(\hat{x})=-L_{g}h(\hat{x})$ and
$b_{1}(t,\hat{x},z)=L_{f}h(\hat{x})+\alpha h(\hat{x})+\nabla
h(\hat{x})^{\top}S^{-1}(t)C^{\top}(t)R^{-1}[z-q(\hat{x})]$. Similarly, $\pi$
is Lipschitz continuous w.r.t. the data $a_{1},\allowbreak b_{1},\allowbreak
A,\allowbreak C,\allowbreak S(t)$, and $\pi_{n}$. Note that the nominal
controller $\pi_{n}$ is Lipschitz continuous w.r.t. $\hat{x}$. In addition, as
the previous arguments in the proof of Theorem 3 still hold, $\pi$ is
piecewise continuous w.r.t. $t$ and Lipschitz continuous w.r.t. $\hat{x}$ and
$z$. The rest of the proof is identical to that of Theorem 3. ∎

(a) Trajectory ($c_{1}=0$).

(b) Trajectory ($c_{1}=1000$).

(c) Eigenvalues of $P(t)$.

(d) State estimation error.

(e) Controls.

Figure 1: A second-order system stabilization problem. 1(a) and 1(b):
Trajectories with and without confidence optimization. 1(c)-1(e): Comparison
of the eigenvalues (of $P(t)$), the state estimation error, and the control
inputs, respectively.

(a) Trajectories.

(b) Controls ($c_{1}=0$).

(c) Controls ($c_{1}=1000$).

(d) Eigenvalues of $P(t)$.

(e) State estimation error.

Figure 2: A unicycle control problem. 2(a): Trajectories with and without
confidence optimization. 2(b) and 2(c): Controls with and without confidence
optimization. 2(d) and 2(e): Comparison of the eigenvalues (of $P(t)$) and
state estimation errors given by the observer.

##  V Simulation Studies

###  V-A A Second-Order Nonlinear System

Consider the following second-order nonlinear system

$\dot{x}_{1}=-x_{1}/4-x_{2},\quad\dot{x}_{2}=x_{1}^{3}-x_{2}/2+(x_{2}^{2}+1)u$
(28)

with output $z=x_{1}$. A CLF for this system is
$V(x)=x_{1}^{4}/4+x_{2}^{2}/2$, and we can verify that $\dot{V}(x)=-V(x)$ for
$u=0$. The CBF chosen for this system is $h(x)=-x_{1}/2+x_{2}+0.5$, and we
would like to make sure that the system remains in the closed half-plane where
$h(x)\geq 0$.

In Fig. 1, the system is controlled using the solution to the optimization
problem (P1). The case without confidence optimization ($c_{1}=0$) is
analogous to the setting in [7]. From Figs. 1(a), 1(b) and 1(e), we can see
that the solution with confidence optimization ($c_{1}=1000$) gives different
control inputs that lead to a different system trajectory, but the system
remains safe and stable in both cases. In Figs. 1(c) and 1(d), the larger
eigenvalue $\lambda_{\max}$ of $P(t)$ is reduced and the estimation
$x_{2}-\hat{x}_{2}$ decreases faster. In fact, for this example, both of the
eigenvalues of $P(t)$ are reduced.

###  V-B The Unicycle System

Figure 3: The unicycle system.

The dynamics of the unicycle system are

$\dot{x}=v\cos(\theta),\quad\dot{y}=v\sin(\theta),\quad\dot{\theta}=\omega$
(29)

where $x$ and $y$ are coordinates of the unicycle in the world frame, and
$\theta$ is the angle between the heading direction and the $x$-axis (see Fig.
3). The outputs of the system are $x$ and $y$. Let the coordinates of the goal
position be $(x_{g},y_{g})$. A feedback controller is proposed in [22]:

$\displaystyle v_{n}$ $\displaystyle=d_{1}e\cos(\phi)$ (30a)
$\displaystyle\omega_{n}$
$\displaystyle=d_{2}\phi+d_{1}\cos(\phi)\sin(\phi)[\phi+d_{3}(\phi+\theta)]/\phi$
(30b)

where $d_{1},d_{2},d_{3}>0$ are design parameters and

$\displaystyle e$ $\displaystyle=\sqrt{(x-x_{g})^{2}+(y-y_{g})},$ (31a)
$\displaystyle\phi$
$\displaystyle=\operatorname{atan2}(y_{g}-y,x_{g}-x)-\theta.$ (31b)

For this example, the task is to reach the goal position $(6,6)$ (by tracking
the control given by $v_{n}$ and $\omega_{n}$) while avoiding a circular
obstacle located at $(x_{o},y_{o})=(5.3,4)$ with a radius of $r_{o}=1.1$. The
CBF for this task is defined as

$h(x,y)=(x-x_{o})^{2}+(y-y_{o})^{2}-r_{o}^{2},$

and we require the unicycle robot to stay in the region where $h(x,y)\geq 0$.
The goal position is $(x_{g},y_{g})=(6,6)$, represented by a black dot in Fig.
2(a).

For this example, we introduce an impulse disturbance at $t=1$ $\mathrm{s}$ to
the dynamics of $\theta$. The height of this impulse is drawn from a uniform
distribution $\mathcal{U}(-0.5,0.5)$. From Fig. 2(a), we see that the control
inputs without confidence optimization ($c_{1}=0$) cannot complete the
navigation task, while the control inputs with confidence optimization
($c_{1}=1000$) meet the safety requirement and complete the task. From Figs.
2(b) and 2(c), it may be observed that the linear velocity (in blue) is
reduced in the beginning when $c_{1}=1000$ compared with the case where
$c_{1}=0$. The executed angular speed $\omega$ (in orange) overlaps with the
nominal angular speed $\omega_{n}$ (in red) in both Figs. 2(b) and 2(c)
because the CBF for this task is independent of $\theta$. As we reduce the
largest eigenvalue of $P(t)$ after $t=2$ $\mathrm{s}$ (see Fig. 2(d)), a
faster decrease in the estimation error $\theta-\hat{\theta}$ can be observed
in Fig. 2(e). It is also worth noticing that the maximum linear velocity that
occurred in Fig. 2(c) is smaller than that in Fig. 2(b), which means that the
control design with confidence optimization requires a smaller actuator and
still can achieve safety and control objectives in this task.

##  VI Conclusion

This work addresses the synthesis of confidence-aware, safe, and stable
control for control-affine systems in the output-feedback setting. We
formulate two confidence-aware optimization problems, demonstrate their
feasibility, and establish the Lipschitz continuity of the obtained solutions.
Simulation studies indicate improvements in estimation accuracy and the
fulfillment of safety and control requirements.

## References

  * [1] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, “Control barrier functions: Theory and applications,” in _Proc. Eur. Control Conf._ , (Naples, Italy), June 2019, pp. 3420–3431.
  * [2] S. Wei, B. Dai, R. Khorrambakht, P. Krishnamurthy, and F. Khorrami, “Diffocclusion: Differentiable optimization based control barrier functions for occlusion-free visual servoing,” _IEEE Robotics and Automation Letters_ , vol. 9, no. 4, pp. 3235–3242, 2024.
  * [3] B. Dai, R. Khorrambakht, P. Krishnamurthy, V. Gonçalves, A. Tzes, and F. Khorrami, “Safe navigation and obstacle avoidance using differentiable optimization based control barrier functions,” _IEEE Robotics and Automation Letters_ , vol. 8, no. 9, pp. 5376–5383, 2023.
  * [4] B. Dai, H. Huang, P. Krishnamurthy, and F. Khorrami, “Data-efficient control barrier function refinement,” in _Proc. American Control Conf._ , (San Diego, CA), May 2023.
  * [5] K. Reif, F. Sonnemann, and R. Unbehauen, “An EKF-based nonlinear observer with a prescribed degree of stability,” _Automatica_ , vol. 34, no. 9, pp. 1119–1123, 1998.
  * [6] R. K. Cosner, I. D. J. Rodriguez, T. G. Molnar, W. Ubellacker, Y. Yue, A. D. Ames, and K. L. Bouman, “Self-supervised online learning for safety-critical control using stereo vision,” in _Proc. International Conf. on Robotics and Automation_ , (Philadelphia, PA), May 2022, pp. 11 487–11 493.
  * [7] D. R. Agrawal and D. Panagou, “Safe and robust observer-controller synthesis using control barrier functions,” _IEEE Control Systems Letters_ , vol. 7, pp. 127–132, 2022.
  * [8] Y. Wang and X. Xu, “Observer-based control barrier functions for safety critical systems,” in _Proc. American Control Conf._ , (Atlanta, GA), June 2022, pp. 709–714.
  * [9] A. Clark, “Control barrier functions for stochastic systems,” _Automatica_ , vol. 130, p. 109688, 2021.
  * [10] S. Wei, X. Chen, X. Zhang, and C. Qi, “Towards safe and socially compliant map-less navigation by leveraging prior demonstrations,” in _International Conference on Intelligent Robotics and Applications_. Springer, 2020, pp. 133–145.
  * [11] B. T. Hinson, M. K. Binder, and K. A. Morgansen, “Path planning to optimize observability in a planar uniform flow field,” in _Proc. Amer. Control Conf._ , (Washington, DC), June 2013, pp. 1392–1399.
  * [12] P. Salaris, M. Cognetti, R. Spica, and P. R. Giordano, “Online optimal perception-aware trajectory generation,” _IEEE Transactions on Robotics_ , vol. 35, no. 6, pp. 1307–1322, 2019.
  * [13] O. Napolitano, D. Fontanelli, L. Pallottino, and P. Salaris, “Information-aware Lyapunov-based mpc in a feedback-feedforward control strategy for autonomous robots,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4765–4772, 2022.
  * [14] D. Coleman, S. D. Bopardikar, and X. Tan, “Observability-aware target tracking with range only measurement,” in _Proc. American Control Conf._ (New Orleans, LA), 2021, pp. 4217–4224.
  * [15] S. Bonnabel and J.-J. Slotine, “A contraction theory-based analysis of the stability of the deterministic extended Kalman filter,” _IEEE Trans. on Automatic Control_ , vol. 60, no. 2, pp. 565–569, 2014.
  * [16] E. D. Sontag, “A ‘universal’ construction of Artstein’s theorem on nonlinear stabilization,” _Systems & Control Lett._, vol. 13, no. 2, pp. 117–123, 1989.
  * [17] S. Wei, P. Krishnamurthy, and F. Khorrami, “Neural Lyapunov control for nonlinear systems with unstructured uncertainties,” in _Proc. American Control Conf._ , (San Diego, CA), May 2023.
  * [18] H. K. Khalil, _Nonlinear control_. Pearson New York, 2015, vol. 406.
  * [19] J. Wilkinson, “The algebraic eigenvalue problem,” in _Handbook for Automatic Computation, Volume II, Linear Algebra_. Springer-Verlag New York, 1971.
  * [20] M. L. Overton and R. S. Womersley, “Second derivatives for optimizing eigenvalues of symmetric matrices,” _SIAM J. on Matrix Analysis and Applications_ , vol. 16, no. 3, pp. 697–718, 1995.
  * [21] W. W. Hager, “Lipschitz continuity for constrained processes,” _SIAM J. on Control and Optimization_ , vol. 17, no. 3, pp. 321–338, 1979.
  * [22] M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino, “Closed loop steering of unicycle like vehicles via Lyapunov techniques,” _IEEE Robotics & Automation Magazine_, vol. 2, no. 1, pp. 27–35, 1995.


# EDMP: Ensemble-of-costs-guided Diffusion for Motion Planning

Jayaram Reddy∗1, Kallol Saha∗1, Vishal Mandadi∗1, Ajit Srikanth1,
Aditya Agarwal2, Bipasha Sen2, Arun Singh3, and Madhava Krishna1
1Robotic Research Center, IIIT Hyderabad, 2Massachusetts Institute of
Technology, 3University of Tartu ∗Denotes equal contribution; authors list
follows alphabetical order

###### Abstract

Classical motion planning for robotic manipulation includes a set of general
algorithms that aim to minimize a scene-specific cost of executing a given
plan to generate a potentially valid collision-free trajectory. This approach
offers remarkable adaptability, as they can be directly used for any new scene
without needing specific training datasets. However, without a prior
understanding of what diverse valid trajectories are and without specially
designed cost functions for a given scene, the overall solutions tend to have
low success rates. While deep-learning-based algorithms tremendously improve
success rates, they are much harder to adopt without specialized training
datasets. We propose EDMP, an Ensemble-of-costs-guided Diffusion for Motion
Planning that aims to combine the strengths of classical and deep-learning-
based motion planning. Our diffusion-based network is trained on a set of
diverse general valid trajectories enabling the model to implicitly learn the
properties of a valid trajectory. Like classical planning, for any new scene
at the time of inference, we compute collision costs and incorporate this cost
at each timestep of the diffusion network to generate valid collision-free
trajectories. Instead of a single collision cost that may be insufficient in
capturing diverse cues across scenes, we use an ensemble of collision costs to
guide the diffusion process, significantly improving the success rate compared
to classical planners. As we show in our experiments, EDMP outperforms SOTA
deep-learning-based methods in most cases while retaining the generalization
capabilities primarily associated with classical planners.

##  I INTRODUCTION

Planning a trajectory from a start to goal position while avoiding collisions
(self-collisions and collisions with the objects around the robot) is a
fundamental challenge in robotic manipulation. Over the years, many approaches
have been introduced to tackle this challenge, including classical planning
algorithms like CHOMP , TrajOpt , and more recent deep-learning-based
algorithms like MPNets , STORM , and M$\pi$nets . While the latter approaches
tremendously improved the success rate in the manipulation tasks (determined
by the number of times the manipulator reaches the goal while avoiding
collisions), classical planners greatly reduce the dependency on specialized
training datasets, generalizing to any random scene and therefore are still
the go-to off-the-shelf choice for motion planning.

In a classical planning approach, a motion plan for a new scene is generated
on the go by minimizing a cost function computed on the given scene. For
example, minimizing a cost function that includes the collision cost, path
length, and reaching a goal. This fundamental concept enables such planning
algorithms for any general scene. However, designing this cost function with
multiple constraints requires many hit-and-trials, and careful fine-tuning,
and still may not capture the diversity across scene structures.

Deep learning-based methods improve upon this by adopting data-driven
techniques that learn a mapping from a given context (or the scene) to a
solution using a neural network. This allows the network to gain an overall
understanding of different scene structures and map the structures to a viable
solution. Such methods are significantly faster and more accurate. Despite the
gains, they falter in terms of generalization – performance on out-of-
distribution scenes is significantly impacted. In this work, we aim to bridge
the gap between the two approaches by first learning a prior (as in deep-
learning-based methods) of “valid” trajectories that meet generic constraints
of what a trajectory should look like for any scene and then incorporating
scene-specific cost, such as collision cost, directly at the time of inference
(as in classical planners). As we show in the experimental section, this
builds a powerful motion planner that generalizes to many diverse scenes. For
example, the manipulator holding any arbitrary objects (as shown in Figure
LABEL:fig:teaser).

Recently, diffusion networks have been widely adopted to learn classifier-
guided policies . Unlike generative models, such as GANs, and VAEs, diffusion
models generate datapoints in multiple timesteps. In the backward pass
(generation step), a diffusion policy generates the output by slowly removing
“noise” in $h$ steps to eventually denoise a given noisy input. This allows
for a unique opportunity to “guide” the generative network to generate a plan
based on “scene-specific cues,” (such as a collision cost) as in the classical
planners, at each of the intermediate steps. In such a case, a collision cost
can be computed using a differentiable algorithm to incorporate collision
gradients that can guide the planning. We develop on top of this approach to
fuse the qualities of “classical” and “deep learning” planners, in which, we
train general planning prior unaware of any scene-specific context, such as
the “collisions”, at the train time and guide the prior to generate a
collision-free path directly at inference by incorporating a collision cost
such as GJK .

Our general prior informs the network of what a valid trajectory would look
like. For instance, going from point A to B in the joint space without any
drastic motion that could harm the manipulator, feasible trajectory, and
avoiding self-collision. At the time of inference, a random noisy trajectory
is sampled, which is guided through “collision cost” to generate a collision-
free valid trajectory. However, we observe that the chances of a successful
path are highly dependent on the kind of cost. For example, as shown in Figure
LABEL:fig:teaser, Vishal and Kallol fill in. To tackle this, we propose, EDMP,
an “Ensemble-of-costs-guided Diffusion for Motion Planning, in which, we rely
on $N$ different classifiers to guide the diffusion process, thereby
generating a diverse set of trajectories with a very high success rate.
Interestingly, based on the concepts of diffusion, our approach generates
multi-modal trajectories containing many valid collision-free trajectories
that could be handpicked on different parameters such as path length,
smoothness, etc., as we show later in the experimental section.

Several works have explored different aspects of diffusion-based motion
planning and its applications. suggests diffusion with classifier-based
guidance as an alternative to Model-based RL. It emphasizes that diffusion can
implicitly learn transition dynamics and plan based on them. proposes that the
conditional diffusion models can be composed together to generate a complex
composition of skills and constraints leveraging classifier-free guidance.
addresses the limitations of diffusion models due to limited training data
diversity. It proposes a self-supervised approach to collect diverse data and
train diffusion models effectively and this way, the diffusion model can
quickly adapt to Out-Of-Distribution scenarios by running inference, filtering
data, and adapting itself via training on filtered self-collected data.
improves behavior cloning methods in generating different robot behaviors
using class conditional diffusion models and classifier-free guidance. Unlike
them, we work explicitly on motion planning with classifier-based and apply
diffusion to improve over the existing works in motion planning. shares a
high-level similarity with our method. It introduces classifier-guided
diffusion for manipulator motion planning using Signed distance function of
environment as a classifier and also show that diffusion is a much better
prior for GPMP type optimizers when compared to traditional planners such as
CVAE , RRTConnect etc, and highlights its superiority over other optimization
methods. However, it shows limited generalization capabilities across start-
goal pairs and new obstacles within the same scene. In contrast, our approach
aims to enhance generalization across scenes and success rates via our novel
ensemble of classifiers and a large prior dataset. We also conduct various
ablation studies, including sensitivity to the dataset and multimodality
guided by CLIP loss, while benchmarking against state-of-the-art planners.

Overall, our contributions are as follows,

  1. 1.

We propose EDMP, an Ensemble-of-costs-guided Diffusion for Motion Planning
that combines the strength of classical planning and deep learning to generate
plans that generalize to diverse scenes.

  2. 2.

We generate multi-modal outputs allowing one to handpick the plans based on
varying personal parameters (such as path length, etc.)

  3. 3.

Our proposed approach greatly outperforms the classical planners while
performing comparably with the SOTA deep-learning-based approaches
demonstrating the ability to generalize to arbitrary scenes and objects.

##  II RELATED WORK

We divide the literature into 5 components, namely: search-based planners,
sampling-based planners, optimization-based planners, learning-based planners,
and hybrid planners for collision avoidance in the space of manipulators.

Search-based planners [4082128], [inproceedings], [conf/nips/LikhachevGT03]
discretize the search space into a grid, with each cell considered as a node,
and perform graph search over it to find the optimal solution. In such
planners, the search space varies inversely with the cube of the grid
resolution. In cases where finer grids are required and precision is of higher
importance, for example, medicinal or museum operations, where fine
manipulation is of great importance, these methods tend to fail due to low
resolution or may take too much time in case the resolution of the grid is too
high.

Sampling-based planners [Kuffner2000RRTconnectAE]
[LaValle1998RapidlyexploringRT] plan in the continuous space and utilize
sampling as a way to achieve faster convergence in huge continuous search
space. Although these planners are probabilistically complete, they do not
have a well-defined upper bound on planning time. MPC and MCTS-based methods
[inproceedings_bangura] [7029990_erez] are known to be computationally
expensive at finding solutions under collision and other constraints. In
addition, these methods try to find locally optimal solutions, which can lead
to robots getting stuck at undesirable local optima.

Methods such as MPPI and CEM [Williams2015ModelPP] [Jankowski2022VPSTOVS]
[7487277] [Bhardwaj2021STORMAI] are stochastic search methods, which try to
find the optimal gaussian distribution of trajectories, parameterized by mean
and covariance matrices. These methods sample several trajectories in
parallel, score them, and then update the parameters of target distribution
using the top $k$ trajectories based on the scores. Due to the ability to
parallelize this search, these methods are known to be very fast. However,
they may not always show a high success rate as demonstrated in our
experiments.

Optimization-based methods such as [5152817_ratliff]
[inproceedings_schulman_ho_jonathan_lee] take a prior trajectory as input and
improve the trajectory using gradients of a score which is usually a
combination of collision, smoothness, and other important aspects that define
an optimal trajectory. However, the success rate of these methods depends on
the prior trajectory. They rely on OMPL [6377468_sucan] planners for a good
initialization, and try to smoothen the trajectories generated by those
planners.

Learning-based planners such as [8793889_qureshi] [fishman2023motion] are
based on imitation learning [osa2018algorithmic] and require a huge number of
expert demonstrations to learn desirable behaviors. These planners rely
heavily on the quality of demonstrations. Collecting high-quality expert
demonstrations is not always feasible and can be a very expensive endeavor. In
addition, [pearce2023imitating] shows that if there is any multimodality in
the demonstration dataset (which usually exists in data collected via
teleoperation [zhao2023learning_aloha], ..etc.), behavior cloning approaches
capture the average of the behaviors, leading to sub-optimal behaviors.

Diffusion-based planning

##  III Background

Diffusion models are a category of generative models where a datapoint is
converted into an isotropic Gaussian noise by iteratively adding Gaussian
noise through a fixed forward diffusion process
$q(x_{t}|x_{t-1})=\mathcal{N}(x_{t};\sqrt{1-\beta_{t}}x_{t-1},\beta_{t}\text{I})$,
where $\beta_{t}$ is the variance schedule and $t$ is the diffusion timestep.
A forward diffusion process of $T$ timesteps can be reversed by sampling
$x_{T}\sim\mathcal{N}(0,\text{I})$ from a standard normal distribution and
iteratively removing gaussian noise using the trainable reverse process
$p_{\theta}(x_{t-1}/x_{t})=\mathcal{N}(x_{t-1};\mu_{\theta}(x_{t},t),\Sigma_{t})$
parametrized by $\theta$.

##  IV Ensemble-of-Costs guided Diffusion

Figure 1: EDMP leverages a diffusion model alongside a 12-member cost
ensemble. The diffusion model denoises a batch of trajectories while each cost
in the ensemble guides a specific slice of the batch. The cost function
calculates the intersection or swept volume from robot and environment
bounding boxes. The trajectory with minimum swept volume is chosen as the
final solution

SUMMARY OF THE METHOD SECTION Background Task setup – Learning a trajectory
prior using a diffusion model – – Cost Guidance (GJK method) – mention the
aspect of generalization to novel scenes and extend it to object in hand
scenarios even though the prior has never seen trajectories with object in the
manipulator’s hand. – Ensemble of Classifiers (expert models) such a mechanism
is not complete to represent the diversity or variation in scenes – ensemble
of classifier (guides) what it allows us to do – This setup enables
multimodality (diverse valid trajectories) and generalization – show
qualitative results in Fig.

Given a robotic manipulator made of $m$ joints corresponding to a given joint
state $s_{i}\in\mathbb{R}^{m}$ for the $i$th time-step, and a scene, $E$, our
goal is to predict a valid collision-free trajectory, $\tau$, in the
manipulator’s joint space between a start and a goal configuration, $s_{0}$
and $s_{h-1}$, respectively, as a sequence of joint states over a horizon $h$.
This is given as:

$\tau=[s_{0},s_{1},...\ s_{h-1}]^{\top}\in\mathbb{R}^{m\times h}$ (1)

A valid collision-free trajectory must comply with certain constraints: (1)
The trajectory must be kinematically smooth and avoid abrupt motions, (2) The
trajectory should avoid self-collisions, (3) The set of joint poses that make
up the trajectory must be feasible for the manipulator to reach and lead to
the goal configuration, and (4) The trajectory must minimize a collision cost,
given as $J(\tau,E).$ We call as trajectory as valid when it complies with the
first three constraints. A valid collision-free trajectory complies with all
four constraints.

As shown in Figure 1, we first learn the concept of valid trajectories by
training a prior. At the time of inference, we incorporate an ensemble of
“collision cost” to guide the prior into generating a valid collision-free
trajectory. We elaborate on the two concepts in the following sub-sections.

###  IV-A Learning a trajectory prior using Diffusion models

We want to build a generative model that implicitly understands the concept of
valid trajectories that can be later used to generate valid collision-free
trajectories. To achieve this, we train a diffusion model to learn a prior,
$p_{\theta}$, over a set of valid trajectories collected from a large-scale
dataset . For each of the trajectories, we train a forward diffusion process
by repeatedly adding Gaussian noise to the trajectory for $h$ timesteps.
Assuming the diffusion process as a Markov chain where a data sample $x_{t-1}$
solely depends on $x_{t}$, we get the joint probability
$p_{\theta}(x_{1:T})=p(x_{T})\prod_{t=2}^{T}p_{\theta}(x_{t-1}|x_{t})$. This
allows us to model the distribution:

$x_{0}\sim p_{\theta}(x_{0})=\int
p(x_{T})\prod_{t=1}^{T}p_{\theta}(x_{t-1}|x_{t})\,{dx_{1:T}}$ (2)

Note that in this stage of offline training, there is no notion of
“collisions,” and rather, we select trajectories that smoothly translate from
a start to a goal configuration. That is, the trajectory avoids any abrupt
motions (between any two sets of joint configurations) and avoids self-
collisions. In the reverse diffusion process, an initial noisy trajectory
sampled from isotropic gaussian noise (equation) is iteratively denoised to
get a valid trajectory in $h$ timesteps.

###  IV-B Collision Cost Guidance

TODO: Talk about how diffusion is enabling us to do this sort of cost-
guidance.

Once an offline prior has been trained, we want to use this prior to find a
valid collision-free trajectory for the specific scene, $E$, by introducing
the notion of collision cost, $J(\tau,E)$. At the time of inference for a
given scene $E$, we randomly sample a noisy trajectory from $p_{\theta}$ and
denoise it iteratively. In each step of the denoising process, we modify the
intermediate trajectory $\tau_{t}$ predicted by the denoiser at the $t$th
timestep by adding gradients from the scene-specific collision cost function,
before passing to the next step. This is given by,

$\tau^{*}_{t}=\tau_{t}-\alpha\frac{\partial J(\tau_{t},E)}{\partial\tau_{t}}$
(3)

where $\alpha$ is a hyperparameter. This form of conditioning guides the
trajectory to collision-free regions. This is analogous to classifier-based
guidance in , in which a trained classifier guides the diffusion towards a
goal. Although, adding cost-conditioning directly at the time of inference
results in a cost-conditioned posterior, denoising from the cost-guided
posterior is equivalent to denoising from a Gaussian distribution, as shown in
.

Computing the collision cost: The collision cost is a differentiable function
that indicates the distance of the manipulator from the objects in the given
environment. If the manipulator’s state is colliding with any of the object’s
on the table, the collision cost should indicate the “penetration” (negative
distance) that must be negated to exit the collision state.

Figure 2: (a) Gradient of link-obstacle intersection volume moves link away
from obstacle. (b) Gradient of swept volume between consecutive link poses
prevents collision between trajectory waypoints

In Fig. 4, add (a) and (b) instead of left and right

To detect and compute the collision cost between the manipulator and the
scene, we take inspiration from Gilbert Johnson Keerthi (GJK) and the
Expanding Polytope Algorithm (EPA). We compute the collision cost between two
3D bodies as a function of overlap between them along the three axes. We refer
to this as the penetration depth. To achieve this, we first approximate each
object on the table as a cuboid corresponding to the smallest 3D bounding box
that encloses the full object. Similarly, to compute the 3D bounding box for
the manipulator, we use the robot’s link (a function of its joint
configuration defined by the differentiable forward kinematics (FKs)) given in
its URDF. Given $l$ links, we construct a sequence of $l$ 3D bounding boxes
across the planning horizon $h$ defined by $FK(\tau)\in\mathbb{R}^{l\times
h\times 4\times 4}$. If there are $n$ obstacles in the scene, we can
approximate the scene as a list of $n$ bounding boxes $\in\mathbb{R}^{n\times
4\times 4}$. Each bounding box is represented as a transformation matrix of
size $4\times 4$ in the SE(3) Lie Group. We then compute the collision cost as
the intersection volume, $V$, of each link state in $\tau$ with respect to
each obstacle in $E$ given as:

$\text{V}(\text{FK}(\tau),E)=\sum_{i=0}^{h-1}\text{V}(\text{FK}(s_{i}),E)$ (4)

Here, $V$ is the intersection volume computed between the two cuboids through
a simple min-max operation given as:

$\text{V}(A,B)=\text{prod}(|\max(\min(A),\ \min(B))\\\ -\min(\max(A),\
\max(B))|)$

For a batch, $b$, of trajectories $\tau_{t}$ at a denoising step $t$, we can
define the collision cost as a summation of the intersection volumes over the
trajectories in the batch as:

$J_{inter}(\tau_{t},E)=\sum_{k=0}^{b-1}\text{V}(\text{FK}(\tau_{t}^{k}),E)$
(5)

While it is possible to compute a more accurate collision cost through EPA,
they entail intricate and lengthy computational graphs. Although, in classical
optimization methods, such a precise collision cost is of great important, in
our case - since the diffusion already understands the concept of valid
trajectories, the collision-cost guidance is needed to solve a comparatively
simpler problem of approximating the movement direction for a link to exit a
collision state based on the penetration depth.

Our method of collision-cost-based guidance introduces a slight shift in the
trajectory distribution at every denoising step. This shift progressively
increases the likelihood of the final trajectory $\tau_{0}$ being collision-
free. As we show in the experiments, this simple collision cost enables
collision avoidance even in complex scenes. Note that, in the denoising
process, trajectory samples may include joint configurations that fall beyond
the joint limits. To address this issue, we clip the joint values within the
joint limits before applying guidance.

$J_{swept}(x_{t},E)=\sum_{k=0}^{b-2}\text{V}(\text{SV}(x_{t}^{k},x_{t}^{k+1}),E)$
(6)

###  IV-C Ensemble of Collision Costs

Even though collision cost-guided diffusion enables generalization to any
arbitrary scene, often, a single cost function is not enough to model the
variations in structures across diverse scenes, as shown in Figure LABEL:. To
solve this problem, deep learning based on previous methods tries to learn a
context map between a scene context and a solution by training over large
variations of scene structures. However, incorporating such diversities in
cost-function is not-trivial for the classical planners. To tackle this, we
propose “ensemble-of-cost” that accounts for many different cost functions to
obtain the most best valid and collision-free trajectories for the given scene
context.

Problem in Section – introducing a particular cost function – underrepresented
for different kinds of scenes – as different scenes have varying structure –
deep learning methods see varying configuration and then map it into the
guidance

– not trivial to apply them to the classical planning algorithms – why???

– why an ensemble of classifier (cost function) to get a set of solutions –
and then choose the solution that is the best among the solutions – bring out
the intermediate aspect – reduce the sampling time

– our novelty is not in the classifiers themselves, but rather in the
mechanism of combining different classifiers to achieve optimal trajectories.
The empirical results are shown in Table X.

A mechanism to combine cost functions to improve the overall guidance.

Explanation of why an ensemble of classifiers works for diffusion models

Classical planners – doesn’t work well with multiple guidance

Scenes with varying structure calls for a different kind of guidance

Incorporate multiple cost functions to improve the overall guidance

A single guide with a fixed hyperparameter set is underconstrained for
generating trajectories for a diverse set of scenes. Notably, trajectory
guidance through a single cost-function presents several challenges. For
instance, an inherent challenge is that gradients tend to diminish as the
link’s penetration depth into an obstacle approache zero. Similarly, for
obstacles with high aspect ratio, like a narrow horizontal shelf, gradient-
driven movement will tend to move the manipulator upwards or downwards to
evade collision, thus failing to reach a configuration that retracts the
manipulator around the edge of the shelf. To tackle these challenges, we
introduce a set of hyperparameters that improve the trajectory guidance
process. Our collision classifier is defined using these hyperparameters as
$C(J(x_{t},E),O_{c}^{t},O_{e}^{t},\eta_{t},1_{norm})$, where $J$ is the
guidance method ($J_{inter}$ intersection volume cost or $J_{swept}$ swept
volume cost), $\alpha$ is the learning rate, $1_{norm}$ is a boolean variable
indicating whether to perform gradient normalization, and $O_{c}^{t}$ and
$O_{e}^{t}$ are obstacle clearance and obstacle expansion hyperparamters for
each step of the denoising process. $O_{c}^{t}$ is used for expanding the
dimensions of the obstacle by a marginal value and $O_{e}^{t}$ limits the
aspect ratio by expanding the narrow dimension. The gradient is thus defined
as, $G=-\nabla_{x}^{t-1}C=\frac{\partial J(x_{t},E)}{\partial x_{t}}$.

Figure 3: Left: Obstacle Clearance. Right: Obstacle Expansion Figure 4:
Obstacle Expansion widens the thinner dimension, thus altering the gradient
direction. This helps the manipulator find a configuration for retracting
around the shelf.

We make the case that for a particular scene, there are several trajectories
that are collision-free. We empirically demonstrate that using a single guide
with a specified hyperparameter setting may not identify these collision-free
regions, thus resulting in low success rates
LABEL:tab:Individual_Guide_Scores. This is primarily because different
hyperparameter settings have varying rates of success across scenes. For
instance, scenes with confined spaces such as drawers require lower clearance
in comparison to tabletop scenes with larger obstacles. To deal with these
scene-specific variations and ensure high success rate, we propose a mechanism
to combine the guidance from multiple of cost-functions called ”mixture of
experts”. more about this!. As shown in Table LABEL:, incorporating multiple
cost functions improve the overall guidance significantly. However,
incorporating such guidance in classical planners is non-trivial (why??. Deep-
learning methods are adapt at incorporating variations in configurations, and
mapping it into the guidance. Consequently, our method of a diffusion prior
guided by cost-functions naturally enables such an extension. The ensemble of
classifiers can be applied in parallel to a batch of trajectory distributions.
Each classifier in the ensemble operates on a specific slice of the batch. For
a batch size of $b$, we apply classifier $C_{i}$ to the slice of trajectories
$x_{t}^{(i-1)\frac{b}{n_{c}}\ :\ (i)\frac{b}{n_{c}}}$ After the denoising
process is complete, we select the trajectory with the minimum swept volume
$J_{swept}(x_{0},E)$.

##  V EXPERIMENTS

###  V-A Experimental Setup

Figure 5: Left-to-right: Obstacle clearance per guide, guide count per cost
function type, guide count per obstacle expansion type, guide count
normalizing gradients, guide count per gradient weight, and individual guide
contributions to the ensemble.

[] provides a large dataset, consisting of 6.54 million trajectories. Of
these, 3.27 million trajectories are generated by the Global Planner, a
typical state-of-the-art configuration space pipeline [], and another 3.27
million trajectories are generated by the Hybrid Planner, which uses AIT* []
for planning in the end-effector space and Geometric Fabrics [] for producing
a geometrically consistent motion conditioned on the generated end-effector
waypoints. Each trajectory contains 50 waypoints, including the initial and
final joint configurations. We train three different denoisers; one on the
data generated by global planners, another one on the data generated by the
hybrid planner, and the last one on both datasets, to empirically evaluate the
influence of dataset quality on our framework. Training is done using the
standard DDPM process [] cite planning with diffuser, where we generate noisy
trajectories from the ground truth, and learn to denoise to ground truth
trajectory using the reconstruction loss. However, unlike [] which tries to
denoiser over both the waypoints and the actions, we only denoise over the
waypoints.

During training, we condition the initial and final configurations of the
trajectory $x_{t}$ to correspond to the initial and final configurations of
the original trajectory $x_{0}$. We empirically demonstrate that this enhances
the trajectory quality during inference, as compared to training without
conditioning. In the latter case, the diffusion model may attempt to align the
end points of the trajectory with the immediately neighboring points, leading
to small trajectory deformations.

Each denoiser is modeled as a temporal UNet similar to [], and is trained for
20k epochs on a 2080 GeForce GTX GPU for approximately 9 hours cross-check
later.

In the experiments conducted herein, it is to be inferred, unless explicitly
stated otherwise, that the evaluations and comparisons have been executed
employing the denoiser trained on both global and hybrid datasets.

Figure 6: Graph showing the asymptotic rise in maximum success rate with
increasing number of guides in the ensemble, demonstrated across tabletop,
cubby, merged cubby, and dresser scenes.

###  V-B Test Datasets

We evaluate our framework on the following datasets:

  * •

Global Solvable Problems (aka global test set): These are a set of problems
solvable by Global Planner from [], consisting of tabletop, cupboard (cubby),
merged cupboards (merged cubby), and dresser scenes. It consists of 1800
problems in total, where each problem consists of obstacle configurations, a
start position in configuration (joint) space, and a goal position specified
in end-effector space.

  * •

Hybrid Solvable Problems (aka hybrid test set): These are a set of 1800
problems solvable by Hybrid Planner from [], mirroring the scene types and
problem specifications of the Global Test Set.

  * •

Both Solvable Problems (aka both test set): This consists of 1800 problems
solvable by both Global Planner and Hybrid Planner from []. It shares scene
types and problem specifications with the Global Test Set.

  * •

Out-of-distribution problems (aka OOD test set): This dataset presents scenes
with collision spheres placed at novel positions around the manipulator.
Notably, our training set lacks trajectories capable of solving such scenes,
rendering them out-of-distribution for our model. We empirically assess our
framework on these scenes to demonstrate its capacity for generalization, akin
to classical planners.

  * •

Object-in-hand problems (OIH test set): In this dataset, the manipulator moves
with objects of various sizes gripped between its finger joints. We
demonstrate that our framework can adapt to such scenarios with minimal cost
function adjustments, eliminating the need for resource-intensive training, in
line with classical planning methods.

###  V-C Baselines

We compare our framework against two behavior cloning-based planners
(M$\pi$Nets [], MPNets []), two stochastic optimization-based planners (G.
Fabrics [], STORM []), one optimization-based planner with quintic-spline
initialization (CHOMP []), and a set of sampling-based planners through OMPL
[] on global, hybrid, and both test sets.

We also benchmark against M$\pi$Nets, CHOMP, and OMPL, on the OOD test set to
assess the generalization capabilities of our framework to out-of-distribution
scenes.

###  V-D Quantitative Analysis

We define success rate (SR $\%$) as the percentage of problems that are
successfully solved by the given planner. We say that a problem is
successfully solved if the planner can generate a trajectory that avoids all
collisions in order to reach the goal in the simulation. We use this metric to
compare our framework’s performance against other baselines on the
aforementioned datasets.

####  V-D1 Benchmarking against the baselines

Table I presents the comparative performance of EDMP against several motion
planning approaches. Table I shows that EDMP outperforms CHOMP, OMPL, G.
Fabrics, STORM, MPNets, and M$\pi$Nets global expert, but falls slightly short
behind the M$\pi$Nets hybrid expert on both hybrid and both datasets on the
success rate. CHOMP (with quintic b-spline initialization), as expected, fails
to solve a huge number of problems due to its tendency to get stuck in local
minima during the optimization process. OMPL, while probabilistically
complete, struggles to find valid solutions within the specified 20-second
time limit, especially in complex collision-free configurations like cubby,
merged cubby, and dresser scenes. STORM and G. Fabrics also exhibit low
success rates across all datasets primarily due to their local planning nature
and susceptibility to local minima.

MPNets, which integrates classical planning with deep learning, offers
improved planning performance by leveraging a learned neural sampler, via
behavior cloning, to generate waypoints. This sampler acts as an informative
scene prior, enhancing planning success compared to purely classical planners
like OMPL. However, MPNets still lags behind M$\pi$Nets and our framework in
efficiently utilizing priors.

We outperform the M$\pi$Nets global expert by a substantial margin and
approach the performance level of the M$\pi$Nets hybrid expert, a state-of-
the-art motion planner known for its effectiveness when abundant expert
demonstrations are available.

These results demonstrate that integrating classical stochastic optimization
methods with trajectory priors that capture common scene patterns
significantly enhances performance, surpassing traditional planners. This
enhancement is achieved through diffusion, where cost-function guidance
emulates stochastic optimization across priors encoded by the diffusion model.

####  V-D2 Out-of-Distribution and Object-in-Hand

In this study, we conducted an assessment of our proposed framework using a
limited set of out-of-distribution scenarios. In these scenarios, multiple
spherical objects were randomly generated at diverse spatial positions. The
evaluation results indicated that our model achieved satisfactory performance
in 8 out of the 10 evaluated scenes, suggesting a reasonable level of
generalization capability in handling out-of-distribution scenarios.

In cases where the manipulator has objects gripped between its fingers,
conventional behavior cloning methods like M$\pi$Nets require retraining to
plan for collision avoidance between the held object and the environment.
However, our framework easily adapts to this scenario by incorporating the
object as an additional link with fixed joints and introducing an object-
environment cost to the overall cost function. This adaptation allows us to
guide the diffusion model to avoid both link-environment and object-
environment collisions, making our method compatible with objects in hand, as
illustrated in Fig. 7.

####  V-D3 Training with vs w/o conditioning

Table add table number illustrates that training with conditioning, where the
initial and the final configurations of the trajectory are aligned with those
of the ground truth trajectories generates smoother trajectories than training
without conditioning. Notably, the smoothness between the initial two
waypoints and the final two waypoints is substantially higher when the
denoiser is trained without conditioning.

This discrepancy in smoothness can be attributed to the role of conditioning.
When trained with conditioning, the denoiser’s primary objective is to acquire
the ability to generate a smooth trajectory between the specified start and
goal points. Consequently, during inference, with fixed start and goal points,
the denoiser endeavors to establish a smooth path, assuming the constancy of
these reference points.

In contrast, in the absence of conditioning during denoiser training, the
model focuses on seamlessly connecting all data points to construct a coherent
trajectory, without assuming fixed start and goal points. Consequently, during
inference, the denoiser attempts to adjust not only the intermediate points
but also the start and goal points, resulting in increased roughness between
the initial two and final two waypoints when these points are reverted to
their original values.

####  V-D4 Sensitivity towards the datasets

The table add table reference shows results

####  V-D5 Ablations on Diffusion Timesteps

Kallol to fill this

Planner CHOMP OMPL G. Fabrics STORM EDMP (both) (ours) Global Test Set (SR%)
26.67 37.27 38.44 50.22 72.28 Hybrid Test Set (SR%) 31.61 40.37 59.33 74.5
85.941 Both Test Set (SR%) 32.2 42.6 60.06 76 86.304

TABLE I: Comparison against classical planners

Planner Global Test Set Success Rate (%) Hybrid Test Set Success Rate (%) Both
Test Set Success Rate (%) MPNets Hybrid Expert 41.33 65.28 67.67 M$\pi$Nets
Global Expert 75.06 80.39 82.78 M$\pi$Nets Hybrid Expert 75.78 95.33 95.06
EDMP (global) (ours) 71.667 82.833 - EDMP (hybrid) (ours) - 86.133 85.0628
EDMP (both) (ours) 72.28 85.941 86.304

TABLE II: Comparison against behavior cloning approaches

Method Success Rate (%) ACSM ACSR ACSV Classifier w/o MM 79.25 0.981 0.124
0.001 Classifier w/ MM (0.5) 79.17 0.969 0.212 0.002 Classifier w/ MM (0.75)
78.83 0.946 0.465 0.0088 Classifier w/ MM (1.0) 75.92 0.865 0.782 0.049 EDMP -
0.892 0.899 -

TABLE III: Ablations on Multimodality Timesteps |  Success Rate (%) |  Avg Path Length |  Avg Plan Time (s)
---|---|---|---
64 | 53.8 | 9.1772 | -
128 | 67.4 | 8.0641 | -
256 | 89.3(?) | 4.1957 | 5.175
|  |  |
TABLE IV: Ablations on Diffusion Timestep Timesteps |  Success Rate (%) |  Avg Path Length |  Avg Plan Time (s)
---|---|---|---
64 | 53.8 | 9.1772 | -
128 | 67.4 | 8.0641 | -
256 | 89.3(?) | 4.1957 | 5.175
|  |  |
TABLE V: Ablations on Diffusion Timestep Figure 7: Our experiments include
M$\pi$Nets dataset scenes, trajectory generation with a grasped object, and
out-of-distribution scenes like collision spheres. Displayed from top-to-
bottom: dresser, tabletop, merged cubby, merged cubby with handheld cuboid,
dresser with handheld cylinder, and a collision spheres scene Figure 8: Left
column: Multimodal trajectories in a tabletop scene. Middle column: Multimodal
trajectories in a merged cubby scene. Multimodal trajectories in a dresser
scene.

##  VI ABLATION STUDIES

###  VI-A Multimodality Gradient

Inspired by the CLIP based guidance in text to image generation, we apply
additional gradients which we refer to as multimodality gradients guided by
the contrastive loss eq. 7 during reverse diffusion process apart from the
classifier based gradients which is primarily used for avoiding collisions, we
aim to make trajectories more diverse by applying these multimodality
gradients. To make this possible, we compute cosine similarity matrix C on the
batch of 120 trajectories and we apply contrastive loss eq. 7 which is cross
entropy between similarity matrix(C) and Identity matrix(I) in addition to the
swept volume cost eq. 6 and use the gradients of this combined cost function
eq. 8 to make our classifier guided diffusion model more multimodal in nature.

$J_{similarity}(C,I)=-\sum_{i,j}I_{ij}\log(C_{ij})$ (7)

$J_{combined}=J_{swept}+J_{similarity}$ (8)

Metrics: We employ three key metrics based on Cosine Similarity to assess
trajectory diversity for 100 randomly picked scenes from the M$\pi$Nets
validation dataset. Firstly, the Average Cosine Similarity Mean (ACSM) is the
mean of cosine similarities between all pairs of trajectories within a batch .
Lower ACSM values indicate greater diversity among trajectories. Secondly, the
Average Cosine Similarity Range (ACSR) represents the difference between
maximum and minimum cosine similarity values in the batch, with higher ACSR
values indicating increased diversity. Lastly, the Average Cosine Similarity
Variance (ACSV) measures the variance of cosine similarities across trajectory
pairs, and higher ACSV values suggest greater diversity. All these metrics are
averaged across 12 classifiers in our study for robust assessment.

We quantitatively show the effect of multimodality gradients on success rate,
ACSM, ACSR, ACSV and compare it with the ensemble of classifiers in III. We
find that ensemble of classifiers is better in making trajectories more
diverse which indicates that our model EDMP implicitly brings more
multimodality compared to any of the single classifier-guided diffusion models
additionally guided by the CLIP Loss. Multimodal trajectories are shown in
Figure 8 on three different types of scenes along with the corresponding plots
of end effector 3D co-ordinates.

##  VII CONCLUSION

While the diffusion model harnesses the benefits of sampling-based planning
methods, our gradient-based guidance harnesses the advantages of optimization-
based planners. When combined, we get a potent generative model which can
produce collision-free trajectories for a manipulator in an environment.

## ACKNOWLEDGMENT