h-NESDF: An Infinitely Differentiable Signed Distance Between Two Sets with Robotic Applications

Introduction

Distance computations are central to robot motion planning, collision detection, and control. Their integration into gradient-based planners and feedback controllers often requires more than accurate distance values: the underlying functions must also provide sufficiently regular derivatives. Control Barrier Functions (CBFs), for example, rely on gradients, and some formulations require higher-order derivatives of distance-like functions to enforce safety constraints [1, 2, 3, 4, 5, 6, 7]. Insufficient regularity can lead to excessively large control inputs [8] or noisy control signals [9]. Motion planners such as CHOMP [10] similarly use distance gradients to update trajectories and resolve collisions during optimization. In this setting, signed distance-like functions are particularly useful because their negative values encode overlap and provide information about infeasible configurations.

However, Euclidean distance functions are generally not differentiable at every configuration [11]. A common source of nonsmoothness is a change in the geometric features that determine the closest points. Such transitions often involve nonunique witness points, namely, pairs of points, one on each object, that attain the minimum distance. This can produce abrupt changes in the distance gradient as the relative pose of the objects varies.

Several works have investigated differentiable alternatives to Euclidean distance functions for robotic applications. The approach in [8] introduced an infinitely differentiable, unsigned distance-like function between convex sets and applied it to second-order kinematic control. The method generalized von Neumann’s alternating projection algorithm into an iterative distance computation, but remained cumbersome to evaluate and did not guarantee a zero value when the sets overlapped. Subsequently, [9] introduced a substantially simpler formulation that correctly vanishes upon overlap, while retaining the convexity assumption and unsigned nature of the function. The work in [12] developed a differentiable distance-like function between a point and an object represented as a set of discrete points, and used it to construct safe and smooth guiding vector fields that avoid collision with obstacles represented as point clouds. Signed differentiable distance metric between convex sets were also developed in [6, 13], using different smoothing techniques and demonstrating applications to CBF-based control. Related constructions also arise outside robotics, particularly in surface reconstruction. For example, [14] reconstructs a smooth approximation of a point-to-set signed distance field from sampled surface data. Similarly, [15] proposes an explicit differentiable level-set formulation that approximates the signed distance to a geometry. The methods discussed above either assume convexity for both extended objects or address point-to-set distances. More recently, [16] introduced a smooth signed distance metric for nonconvex objects aiming to use it within a contact simulation framework. Table I shows comparisons between different differentiable distance approaches.

Work	Type of Set	Signed	Detect Collision	Time Complexity
Liu et al. [14] [14]	A point and a possibly nonconvex set in $\mathbb{R}^{3}$, represented as a point cloud	Yes	No	$\mathcal{O}(K)$ per query for the full formula, in which $K$ is the number of points. Local evaluation uses $k=10$ nearby points, plus neighbor-search cost
Hicken et al. [15] [15]	A point and a possibly nonconvex set in $\mathbb{R}^{2}$ or $\mathbb{R}^{3}$, represented as a point cloud	Yes	No	$\mathcal{O}(K)$ per query for the full formula, in which $K$ is the number of points. The implementation uses $k=10$ nearest points, plus neighbor-search cost
Gonçalves et al. [8] [8]	Two convex sets in $\mathbb{R}^{n}$. Only give practical approximate formulae for balls, boxes and cylinders.	No	No, with arbitrarily small error as $h \to+0$	Iterative, no fixed number of steps
Beker et al. [16] [16]	Almost arbitrary sets in $\mathbb{R}^{n}$, represented as point clouds	Yes	No, with arbitrarily small error as $h \to+0$	$\mathcal{O}(K_AK_B)$, where $K_A$ and $K_B$ are the numbers of points in the respective objects
Wu et al. (2025) [6]	Two convex polyhedral sets in $\mathbb{R}^{2}$	Yes	No; conservative for finite smoothing, with arbitrarily small error as $h \to+0$	$\mathcal{O}(V_AV_B)$, where $V_A$ and $V_B$ are the numbers of vertices
Gonçalves et al. [9] [9]	Two convex sets in $\mathbb{R}^{n}$. Only gives practical formulae for polyhedra.	No	Yes	Iterative, no fixed number of steps. If the two objects are polyhedral and have $F_A,F_B$ faces, respectively, each step is $\mathcal{O}(F_A+F_B)$
Nunes et al. [12] [12]	Point cloud and a point in $\mathbb{R}^{n}$	No	Yes	$\mathcal{O}(K)$, where $K$ is the number of points
Bartelt et al. [13] [13]	Two convex polyhedral sets in $\mathbb{R}^{3}$	Yes	Not exactly, but collision detection can become arbitrarily accurate by increasing the number of normals $N$	$\mathcal{O}(V_AV_BN)$, where $V_A$ and $V_B$ are the numbers of vertices and $N$ is the number of normals used. The paper proposes, by default, $N=\mathcal{O}(V_A+V_B)$.
Ours	Almost arbitrary sets in $\mathbb{R}^{n}$, one of them at least represented as a point cloud	Yes	No, with arbitrarily small error as $h \to+0$	If $A$ is represented by a point cloud with $K_A$ points, the complexity is $\mathcal{O}(K_A)$ calls of a function related to $B$. If $B$ is also a point cloud, each call is $\mathcal{O}(K_B)$, resulting in $\mathcal{O}(K_AK_B)$.

In this work, we propose a signed, infinitely differentiable distance-like function between two possibly nonconvex objects. We first introduce the Normal Euclidean Signed Distance Function (NESDF), a potentially nonsmooth function whose sign characterizes separation, contact, and interior overlap. We then apply a smoothing procedure to obtain the $h$-NESDF, which is infinitely differentiable with respect to

smoothly parameterized rigid motions. Its sampled approximation is amenable to GPU parallelization. Figure 1 shows an example of the $h$-NESDF computed for two $L$ shapes as one of them rotates, with three different values of $h$, one of them being as $h \to+0$, that is, the non-differentiable case.

Our construction differs from the proposed metric in [16] in several respects, bringing some benefits. One distinction concerns the use of the gradient of the metric in a gradient-ascent procedure for resolving overlap between two objects, useful, for example, in motion planning. In some deeply overlapping configurations, the metric in [16] produces an ascent direction that increases geometric overlap, whereas ours produces the correct behavior. These distinctions are examined in detail throughout the paper.

The contributions of this paper are then:

1. NESDF, the Normal Euclidean Signed Distance Function, a novel signed distance metric between two general, possibly non-convex sets in $\mathbb{R}^{n}$. We establish that this metric has the expected behavior of being positive when the two objects do not overlap and negative when they overlap. This metric is also amenable for a smoothification procedure.

2. $h$-NESDF, a smoothed version of NESDF that is infinitely differentiable, with smoothing parameter $h$ that controls the smoothing. We show that when $h \to+0$, NESDF is recovered. This metric can be implemented in an approximated manner using sampled point clouds and normal vectors, and parallelized on a GPU. This metric brings benefits when compared to other comparable approaches, as [16].

3. We present extensive computational experiments assessing $h$-NESDF efficiency and usefulness in motion planning.

For readability, all proofs and auxiliary lemmas are deferred to the Appendix.

Basic Notation

Any vector is a column vector unless stated otherwise. $A^{\mathsf{T}}$ represents the transpose of a matrix. For $f:\mathbb{R}^{n}\mapsto\mathbb{R}$, $\nabla f$, the gradient, is a column vector. For a set $\mathcal{X}$, $\partial\mathcal{X}$ represents its boundary, $int(\mathcal{X})$ its interior, and $\overline{\mathcal{X}}$ its closure. $\mathbb{S}_{n}$ represents the set of $n$-dimensional normalized vectors. $\mathcal{X}-\mathcal{Y}$ represents the set subtraction (all points in $\mathcal{X}$ not in $\mathcal{Y}$). Uniform convergence of $f_h(y)$ to $f(y)$ as $h \to+0$ in a set $\mathcal{Y}$ means that for any $\delta>0$, there exists $\epsilon(\delta,\mathcal{Y})$ such that

Methodology

The Normal Euclidean Signed Distance Function (NESDF)

In this Subsection, we will propose NESDF, the Normal Euclidean Signed Distance Function, a novel signed metric between two nonconvex shapes. It will be used as a starting point for the true goal of this work, which is to define an infinitely differentiable metric between two objects. We start with the following definition.

There is no single standard definition of the ESDF between two sets $\mathcal{A}$ and $\mathcal{B}$. One definition uses penetration depth [18]; alternatively, one can minimize the point-to-set ESDF over the other set and define $D_{\mathcal{A},\mathcal{B}}=\min_{b\in\mathcal{B}}D_{\mathcal{A}}(b)$, as done for point-cloud clearance in [19]. Neither is readily amenable to smoothing. We therefore will introduce NESDF, that is easier to smooth. First, we need some auxiliary definitions.

The function $E_{\mathcal{X}}(y)$ generalizes the half-squared Euclidean distance function used in the unsigned case, where $d_{\mathcal{X}}=0$ (e.g., [8], [9]) to account for signed distances.

We can now define, under some circumstances, the Normal Euclidean Signed Distance Function (NESDF).

The name reflects the role of the normal vector $n_{\mathcal{B}}$. At this stage, the definition of the NESDF is incomplete, since it is not defined for pairs that are not nondegenerate. This is not a problem: the NESDF was only created to motivate its smoothed version, in which this nondegeneracy hypothesis will no longer be necessary.

We will now show some properties of the NESDF, starting with the most important one.

Thus, the NESDF has the expected sign behavior of a signed distance between two objects. In addition, in one particular case, the NESDF is exactly the ESDF.

Proposition 2. If the formula for $N_{\mathcal{A},\mathcal{B}}$ is extended to a singleton $\mathcal{A}=\{a\}$, then, if $(\{a\},\mathcal{B})$ is a non-degenerate pair, $N_{\mathcal{A},\mathcal{B}}=D_{\mathcal{B}}(a)$.

The h-NESDF

In this subsection, we will show how to use the NESDF to obtain a smoothed, infinitely differentiable version of this metric. First, we assume a smoothed version of the ESDF:

The last four properties just guarantee that $E_{\mathcal{X}}^{h}\to E_{\mathcal{X}}$ with sufficient regularity for the convergence result below. See Subsection III-B for examples on how to compute the $h$-ESDF. Nevertheless, using $D_{\mathcal{A}}^{h}(b)$, we can define the smoothed version of our NESDF:

Or, more explicitly:

■

Note that, as opposed to the definition of NESDF, the $h$-NESDF does not require that $(\mathcal{A},\mathcal{B})$ is a nondegenerate pair. In addition, note that, since $\mathcal{B}$ is assumed regular, the set of points $b$ in which $n_{\mathcal{B}}(b)$ is undefined has zero measure in $\mathbb{R}^{n}$, and thus their choice of values is irrelevant for the value of the integral that defines $N_{\mathcal{A},\mathcal{B}}^{h}$.

Figure 2 shows, in the 2D case, the $h$-NESDF between two objects, a rounded square $\mathcal{A}$ and a U-shaped object $\mathcal{B}$. More precisely, it shows the contour curves of the function $f:\mathbb{R}^{2}\mapsto\mathbb{R}$ defined by $f(s)=N_{\mathcal{A}(s),\mathcal{B}}^{h}$, in which $\mathcal{A}(s)$ represents the rounded square depicted in Figure 2 with center $s\in\mathbb{R}^{2}$.

As $h\to0^{+}$, under some conditions, $N_{\mathcal{A},\mathcal{B}}^{h}\to N_{\mathcal{A},\mathcal{B}}$.

Proposition 3: Let $(\mathcal{A},\mathcal{B})$ be a nondegenerate pair. Then:

■

Note that the $h$-NESDF does not have exactly the same sign properties as the NESDF shown in Proposition 1. Thus, it is not guaranteed to be positive when the objects do not touch or negative when their interiors overlap. However, the $h$-NESDF implements an approximate version of this result, approximating the original result as $h\to+0$ at the expense of potentially larger derivative magnitudes and sharper transitions. Even though the exact sign property in Proposition 1 does not hold, it is still useful in robotic control and motion planning with the appropriate care, as will be shown.

We will now state results about the differentiability. We will assume that the objects $\mathcal{A}(t)\subseteq\mathbb{R}^{n}$ and $\mathcal{B}(t)\subseteq\mathbb{R}^{n}$ move as rigid bodies. Thus, there exists $Q_{\mathcal{A}}(t),Q_{\mathcal{B}}(t)\in SO(n)$ and $s_{\mathcal{A}}(t),s_{\mathcal{B}}(t)\in\mathbb{R}^{n}$ along with static sets $a,\beta$ such that $\mathcal{A}(t)=Q_{\mathcal{A}}(t)a+s_{\mathcal{A}}(t)$ and $\mathcal{B}(t)=Q_{\mathcal{B}}(t)\beta+s_{\mathcal{B}}(t)$. Given the $h$-ESDF $E_{\mathcal{A}(t)}^{h}(b)$, we use:

This choice guarantees that

is constant, avoiding a possible source of nondifferentiability due to the fact that minimum is not a differentiable operator. Then:

Proposition 4: If $Q_{\mathcal{A}}(t),Q_{\mathcal{B}}(t)$ and $s_{\mathcal{A}}(t),s_{\mathcal{B}}(t)$ are $k$-times differentiable in $t$, so is $f(t)=N_{\mathcal{A}(t),\mathcal{B}(t)}^{h}$.

Thus, when the motions are infinitely differentiable, so is the distance function.

Implementation

Implementing the h-NESDF in practice

Assuming that we have the $h$-ESDF function $D_{\mathcal{A}}^{h}(b)$ (see the next subsection), we sample $K$ points $b_k$ on $\partial\mathcal{B}$ with respective normals $n_k$, obtaining $K$ pairs $(b_k,n_k)$. Since the formula for $N_{\mathcal{A},\mathcal{B}}^{h}$ is a weighted average, we replace the integrals by sums, assuming that the points $b_k$ are distributed approximately uniformly over the surface:

For numerical stability reasons, one should subtract from each $E_{\mathcal{A}}^{h}(b_k)$ the value $\delta=\min_k E_{\mathcal{A}}^{h}(b_k)$. This does not modify the result theoretically, but in practice help avoiding underflow with the exponentials.

Computing h-ESDF for arbitrary $\mathcal{X}$

To obtain an $h$-ESDF $D_{\mathcal{X}}^{h}$ for a regular set $\mathcal{X}$, we can use the construction in Subsection III-A with $\mathcal{A}=\{y\}$ and $\mathcal{B}=\mathcal{X}$. In this case, we can take $D_{\mathcal{A}}^{h}(b)=D_{\mathcal{A}}(b)=\frac{1}{2}\lVert b-y\rVert^{2}$ for every $h$, since $E_{\mathcal{A}}^{h}(b)=E_{\mathcal{A}}(b)=0.5\lVert b-y\rVert^{2}$ is infinitely differentiable. We therefore obtain

This formula can be applied to different representations of $\mathcal{X}$. For example, when its boundary is represented by sampled point–normal pairs, we can use the approximation discussed in Subsection III-A. See Figure 3 for an example of the $h$-ESDF implemented with this technique. In this case, given a set of points and normals $(x_k,n_k)$ sampled approximately uniformly on $\partial\mathcal{X}$, the formula reduces to

which is essentially a result that appeared in other papers, as in [14, 16].

Now suppose that $\mathcal{X}\subset\mathbb{R}^{n}$ is a possibly non-convex polyhedron. Partition its boundary into $K$ $(n-1)$-dimensional polyhedral pieces $\mathcal{X}_k$. Each piece lies in a plane $\mathbf{n}_k^{\mathsf{T}}\mathbf{x}+c_k=0$, where $\mathbf{n}_k$ is a unit outward normal. Define

(14) then becomes

All weights in (17) can underflow when $y$ is far from the boundary. To alleviate this, we can multiply every weight by the same factor $e^{\delta(y)/h^2}$, for any function $\delta(y)$, similarly as it was done in Subsection III-A. We then obtain

Since $\omega_k^h(y)=e^{-\delta(y)/h^2}\widetilde{\omega}_k^h(y)$, the common factor cancels:

The ideal $\delta$ is $\delta(y)=0.5D_{\partial\mathcal{X}}(y)^2$, but it can be relatively expensive to compute. We can use a relatively inexpensive proxy. Let $\{v_i\}$ contain the vertices of all the pieces $\mathcal{X}_k$. Let

In three dimensions, we can take the pieces $\mathcal{X}_k$ to be triangles. Let $S(a,b,c)$ be a canonical 2D triangle with vertices $(0,0)$, $(a,0)$, and $(b,c)$, where $a,c>0$. Let $Q_k\in\operatorname{SO}(3)$, $s_k\in\mathbb{R}^3$ be the rigid transformation that maps the 2D canonical triangle, after lifting it to 3D by adding a 0 coordinate, to $\mathcal{X}_k$.

For $z=[z_1\ z_2\ z_3]^{\mathsf{T}}$ and $\zeta=[\zeta_1\ \zeta_2]^{\mathsf{T}}$, define

Then:

This function $\Phi^h$ can be approximated using Gauss-Legendre quadrature in 2D after using Duffy Transformation [20] to map the triangular region to a square. The nodes and weights should be selected independent of $y$, so the resulting approximation continues infinitely differentiable in $y$. Figure 4 shows the result when this polyhedral procedure is implemented for an “A” shaped polyhedra.

Experimental Evaluation and Comparisons

Qualitative Comparison with beker2025smooth

As mentioned in the introduction, [16] also proposes a differentiable distance metric between two general sets. Just like in our proposed approach, they start from a non-differentiable counterpart and then apply a smoothing procedure.

In their case, they start from:

and then smooth it with a parameter $h$, obtaining a metric $M_{A,B}^h$. Their procedure to smooth this formula is similar to what is proposed here, and includes a smoothfication of $D_{\mathcal{X}}(y)$ into $D_{\mathcal{X}}^h(y)$ that is essentially a slight generalization of (15) (just with a general function $B_h(x-y)$ instead of $e^{-\lVert x-y\rVert^2/(2h^2)}$). Nevertheless, following the implementation in [16], we will consider, if $\mathcal{A}$ and $\mathcal{B}$ has $K_{\mathcal{A}}$ and $K_{\mathcal{B}}$ points respectively, we will define $M_{\mathcal{A},\mathcal{B}}^h$ as

which is comparable with the point-cloud based formula that we propose in (13).

Since both approaches start with a non-differentiable metric and smooth it out, obtaining approximations of their original counterparts, it is fair to begin by comparing their unsmoothed counterparts: $N_{\mathcal{A},\mathcal{B}}$ and $M_{\mathcal{A},\mathcal{B}}$.

Let $\mathcal{A}$ be a sphere with center $q \ne0$ and radius $R$, and $\mathcal{B}$ a sphere with center $0$ and radius $R$. One can clearly see that $D_{\mathcal{A}}(b)=\lVert b-q\rVert-R$, $D_{\mathcal{B}}(a)=\lVert a\rVert-R$ and also that:

and thus $M_{\mathcal{A},\mathcal{B}}=\lvert\lVert q\rVert-R\rvert-R$.

Now, $b^*=\arg\min_{b\in\partial\mathcal{B}}E_{\mathcal{A}}(b)=Rq/\lVert q\rVert$. In this case, $n_{\mathcal{B}}(b^*)=q/\lVert q\rVert$, $d_{\mathcal{A}}=-R$ and $\nabla E_{\mathcal{A}}(b^*)=(b^*-q)=Rq/\lVert q\rVert-q$. Thus $N_{\mathcal{A},\mathcal{B}}=-R-(R-\lVert q\rVert)=\lVert q\rVert-2R$.

The difference now is clear, $M_{\mathcal{A},\mathcal{B}}(q)=\lvert\lVert q\rVert-R\rvert-R$ and $N_{\mathcal{A},\mathcal{B}}(q)=\lVert q\rVert-2R$. If one uses the gradient of these metrics on $q$ to move $q$ to make the sphere no longer overlap, $N_{\mathcal{A},\mathcal{B}}$ gives the proper direction $\nabla N_{\mathcal{A},\mathcal{B}}(q)=q/\lVert q\rVert$, whereas $\nabla M_{\mathcal{A},\mathcal{B}}(q)=\operatorname{sign}(\lVert q\rVert-R)q/\lVert q\rVert$. Thus, if $\lVert q\rVert<R$, then $\nabla M_{\mathcal{A},\mathcal{B}}(q)=-q/\lVert q\rVert$, making the two spheres overlap even more.

Comparison with beker2025smooth: Escape from Overlapping Configurations

The example in the previous subsection shows that the gradient of a signed distance-like function may not always provide a suitable direction for resolving interpenetration. We therefore compare the $h$-NESDF with the smooth metric of [16], named hereafter “Beker” in a large-scale rigid-body separation task. Two initially overlapping objects, $\mathcal{A}$ and $\mathcal{B}$, are considered: $\mathcal{A}$ remains fixed, while $\mathcal{B}$ can translate and rotate freely in $SE(3)$. For each method, the objective $f$ is its corresponding smoothed metric, $N_{\mathcal{A},\mathcal{B}}^h$ or $M_{\mathcal{A},\mathcal{B}}^h$, we maximize the corresponding smoothed function, evaluated at the current relative pose.

Let $f(Q_{\mathcal{A}},s_{\mathcal{A}},Q_{\mathcal{B}},s_{\mathcal{B}})$ denote either the proposed $h$-NESDF or the smooth metric of [16], expressed as a function of the rigid-body poses. We consider the row vector $L[f](Q_{\mathcal{A}},s_{\mathcal{A}},Q_{\mathcal{B}},s_{\mathcal{B}})\in\mathbb{R}^{1\times12}$, defined such that

where $\xi\in\mathbb{R}^{12}$ is a column vector given by

Parameter	$h$-NESDF	Beker
Smoothing $h$	0.1	0.06
Gain $\kappa$	2.0	0.5
Momentum $\beta$	0.3	0.3
Step $\Delta t$ (s)	0.2	0.1
Time limit (s)	20	20

Here, $\omega_i$ and $v_i$ denote, respectively, the angular velocity and the linear velocity vectors of each body, both expressed in world coordinates.

The operator $L[f]$ plays the role of the gradient $\nabla f$ if its arguments were Euclidean vectors. The row vector $L[f]$ can be partitioned as $L[f]=[L_{\mathcal{A}}[f]\ L_{\mathcal{B}}[f]]$, corresponding to the two bodies, respectively.

In the separation experiments, $\mathcal{A}$ remains fixed, while $\mathcal{B}$ is free to move in $SE(3)$. Consequently, $\xi_{\mathcal{A}}=0$ and

Therefore, at a given configuration $(Q_{\mathcal{A}},s_{\mathcal{A}},Q_{\mathcal{B}},s_{\mathcal{B}})$ of both objects, $L_{\mathcal{B}}[f](Q_{\mathcal{A}},s_{\mathcal{A}},Q_{\mathcal{B}},s_{\mathcal{B}})$ provides the local ascent direction of the selected metric with respect to the six degrees of freedom of $\mathcal{B}$.

To perform the separation task, we employ a gradient-ascent, with $L_{\mathcal{B}}[f]$ playing the role of the gradient, controlled with Nesterov acceleration [21]. Let $S:\mathbb{R}^6\mapsto\mathfrak{se}(3)$ be the linear operator such that, for any $p\in\mathbb{R}^3$, $q\in\mathbb{R}$:

If $H[k]$ is the pose of object $\mathcal{B}$ at step $k$, given parameters $(\Delta t,\beta,\kappa,t_{\max})$, then the Nesterov update can be written as:

in which $H[0]$ starts with the initial pose and $\xi[0]$ as the zero vector. This updates run up until there is a separation (validated through Euclidean distance computation) or $k\Delta t>t_{\max}$.

The parameters selected for each algorithm are shown in Table II, together with the selected smoothing factor $h$. The parameters were selected empirically through trial and error, with the aim of maximizing the success rate of each method in achieving geometrically certified separation. Once selected, these parameters were kept unchanged across all configurations. The results can be seen in Table III, in which 40,000 pairs of objects with random initial poses were sampled. The objects were taken from the TetWild dataset [22] and had their surfaces sampled to obtain discrete points that were used in each algorithm. Both algorithms were evaluated on every object pair and initial configuration. The results indicate that $h$-NESDF can perform separations more often than the algorithm in “[16]”.

Metric	$h$-NESDF	Beker
Successful separations	36,410	28,124
Unsuccessful separations	3,590	11,876
Rate (%)	91.03	70.31

Computational Efficiency

We evaluate the computational efficiency of the $h$-NESDF algorithm, approximated as discussed in Subsection III-A, when the $h$-ESDF for $\mathcal{A}$ is computed using a point cloud, as described in Subsection III-B. In this case, assuming $\mathcal{A}$ and $\mathcal{B}$ have $K_{\mathcal{A}}$ and $K_{\mathcal{B}}$ points, respectively, the computational complexity of the algorithm is of order $\mathcal{O}(K_{\mathcal{A}}K_{\mathcal{B}})$, since the $h$-NESDF makes $K_{\mathcal{B}}$ calls to the $h$-ESDF of $\mathcal{A}$, which in turn has a complexity of $\mathcal{O}(K_{\mathcal{A}})$. In comparison, as mentioned in Subsection IV-A, [16] uses a similar implementation whose computational complexity, when applied to point clouds, is also $\mathcal{O}(K_{\mathcal{A}}K_{\mathcal{B}})$.

The $h$-NESDF code was implemented in PyTorch using FP32 precision on an NVIDIA T4 GPU. The shapes were chosen randomly: because the algorithm is explicit rather than iterative, the computational time depends primarily on the number of points in each shape, apart from minor random fluctuations.

We tested 6,000 pairs with $K_{\mathcal{A}}=K_{\mathcal{B}}$ ranging from 100 to 2,000. For each pair, we computed $N_{\mathcal{A},\mathcal{B}}^{h}$ as well as its gradient with respect to the poses of both objects. More precisely, let $N_{\mathcal{A},\mathcal{B}}^{h}(Q_{\mathcal{A}},s_{\mathcal{A}},Q_{\mathcal{B}},s_{\mathcal{B}})$ denote the $h$-NESDF as a function of the object poses $(Q_{\mathcal{A}},s_{\mathcal{A}})$ and $(Q_{\mathcal{B}},s_{\mathcal{B}})$. We compute the SE(3) gradient-like quantity $L[N_{\mathcal{A},\mathcal{B}}^{h}](Q_{\mathcal{A}},s_{\mathcal{A}},Q_{\mathcal{B}},s_{\mathcal{B}})$ as in (26). This gradient was computed using PyTorch’s automatic differentiation. We then aggregated the execution times based on the product $K_{\mathcal{A}}K_{\mathcal{B}}$. Figure 5 presents the results in milliseconds per distance computation, showing the median (second quartile) alongside the third quartile.

The GPU implementation also supports batching to compute distances for multiple pairs in parallel. To avoid out-of-memory errors, the batch size $k_p$ was determined empirically as $k_p=\operatorname{round}(4\times10^7/(K_{\mathcal{A}}K_{\mathcal{B}}))$.

Motion Planning Application

To evaluate the applicability of the $h$-NESDF to motion planning, we constructed a synthetic underwater environment represented by an oriented point cloud, as shown in Figure 7. We consider a fully actuated underwater vehicle with independent control of its three translational and three rotational degrees of freedom, such as the BlueROV2 in its Heavy configuration (Figure 6) [23]. For geometric planning, the vehicle is modeled as a rigid body with a box-shaped approximation of its geometry.

Given initial and goal poses $H_{\mathrm{start}},H_{\mathrm{goal}}\in\mathrm{SE}(3)$, the motion-planning problem is to find a continuous path $H:[0,1]\to\mathrm{SE}(3)$ connecting these poses while avoiding collisions with the environment. We use a CHOMP-inspired trajectory optimization scheme [10]. The path is discretized into $T$ segments, with $T+1$ poses $H[0],\ldots,H[T]$. The endpoints are fixed at $H[0]=H_{\mathrm{start}}$ and $H[T]=H_{\mathrm{goal}}$, and the intermediate poses $H[1],\ldots,H[T-1]\in\mathrm{SE}(3)$ are optimized by maximizing $f(H[1],\ldots,H[T-1])$, defined by

Here, $N_h(H)$ denotes the $h$-NESDF between the box $\mathcal{A}$ representing the vehicle at pose $H$ and the environment $\mathcal{B}$, represented by its oriented point cloud. The second term rewards a log-sum-exp approximation of the minimum clearance along the discretized path, with smoothing parameter $h>0$ and weight $C_{\mathrm{weight}}>0$.

The first term penalizes excessive separation between consecutive poses. For poses $H=(Q,s)$ and $H'=(Q',s')$, define the weighted pose distance

where $s,s'\in\mathbb{R}^3$, $Q,Q'\in\mathrm{SO}(3)$, and $C_{\mathrm{factor}}>0$ balances the translational and rotational contributions. We use the barrier penalty $\Delta:\mathrm{SE}(3)\times\mathrm{SE}(3)\to\mathbb{R}\cup\{+\infty\}$, defined, with $r=d_{\mathrm{pose}}(H,H')$, by

Thus, consecutive poses must satisfy the strict spacing bound $d_{\mathrm{pose}}(H[k],H[k+1])<C_{\mathrm{threshold}}$.

The smooth box field $D_{\mathcal{A}}^h(b)$ is constructed from the exact signed Euclidean distance to a box. Let $Q,s$ represent the current pose of the box $\mathcal{A}$. Let $\ell=[\ell_x\ \ell_y\ \ell_z]^{\mathsf{T}}/2$ be its half-length vector and define $u=Q^{\mathsf{T}}(b-s)$ and $v=\lvert u\rvert-\ell$.

Parameter	Value
$h$	0.1
$T$	30
$C_{\mathrm{weight}}$	25
$C_{\mathrm{factor}}$	1
$C_{\mathrm{threshold}}$	0.6
Momentum $\beta$	0.95
Step size $\Delta t$	0.2
$\kappa$	0.5

The exact distance is

where the absolute value and $\max(v,0)$ are applied componentwise. We replace maxima and minima by their log-sum-exp approximations with smoothing parameter $h$, expressing $\lvert u_i\rvert$ as $\max\{u_i,-u_i\}$. We also regularize the outer norm as $\sqrt{\lVert\cdot\rVert_2^2+\epsilon_h^2}$, with $\epsilon_h = h^2/\lVert\ell\rVert_2$. These substitutions yield a $h$-ESDF $E_A^h(b)$ for the query point $b$ and box pose for every $h>0$.

Optimization proceeds by gradient ascent on $f$, using the Lie-group differential $L[f]$ introduced in Subsection IV-B, evaluated with respect to each free pose $H[k]$. The poses are updated using the Nesterov scheme in (30), while the endpoints remain fixed and updates are restricted to the feasible domain of the spacing barrier.

We performed 200 motion-planning trials with randomly sampled pairs of initial and goal poses. Both endpoints were required to be collision-free and sufficiently far apart. In addition, the interpolated initial path was required to intersect the environment, ensuring that each trial required a modification of the initial path to achieve collision avoidance, and thus was not trivial. The initial path was generated by linear interpolation of the translation and shortest-arc quaternion spherical linear interpolation (SLERP) of the rotation [24]. Table IV lists the parameters used in these experiments.

The algorithm was implemented in a NVIDIA T4 GPU using batch calculation for the distance between the object at the $T+1$ poses $H[k]$ and the point cloud environment. Figure 8 shows an example of result obtaind from this algorithm, in four different views. The yellow box is the starting pose, the magenta box the final pose, and the red trail the optimized trajectory.

In the final result, 34% of the 200 tests returned success, with an average computational time of 0.7s.

Conclusion

In this paper, we introduced the $h$-NESDF, a signed and infinitely differentiable distance function between two sets satisfying the stated geometric assumptions. We established theoretical properties, provided guidelines for its approximate implementation, and evaluated its computational efficiency and applicability in robotic, especially motion planning, through numerical experiments. Future work will focus on the following directions:

1. The $h$-NESDF does not guarantee positivity if and only if the two objects are disjoint, although this property is desirable in many applications. Reference [13] introduced an almost everywhere smooth approximation of the minimum operator, denoted by $\min[h]$, that preserves the sign of the exact minimum, that is, $\operatorname{sign}(\min[h]{\lvert x_i\rvert}) = \operatorname{sign}(\min_i x_i)$. Such an operator may provide a useful ingredient for developing a formulation with exact collision-detection guarantees or, at least, like in [13], one that can be controlled independently of the smoothing parameter $h$ at the

expense of computational time. Its potential integration into the $h$-NESDF framework will be investigated in future work.

2. Direct evaluation of the point-cloud approximation has computational complexity $O(K_AK_B)$. This cost can become prohibitive for dense point clouds and applications requiring low-latency distance queries, even when GPUs and batching are used. However, the exponential weighting suggests opportunities for acceleration. Indeed, if $b_k$ are the points sampled from object $B$ and $\tilde{d} = \min_k E_A^h(b_k)$, if $(E_A^h(b_k)-\tilde{d})/(h^2)$ is sufficiently large, this point $b_k$ influence very little in the final calculation. A similar observation applies to the evaluation of $E_A^h(b_k)$ itself. These observations motivate investigating spatial data structures, such as k-d trees, to identify and omit negligible contributions without evaluating every point pair. Developing an efficient GPU implementation requires careful management of memory access and thread divergence, together with control of the accumulated approximation error and preservation of smoothness.

Appendix

Lemma 1: Let $(\mathcal{A},\mathcal{B})$ be a nondegenerate pair. Then there is a scalar $\lambda$ such that

Furthermore, $|\lambda| = D_A(b^*) - d_A$.

Proof: Consider the Karush–Kuhn–Tucker (KKT) conditions for the optimization problem defining $b^*$:

Since $n_B(b)=\nabla D_B(b)$ when $b\in\partial B$, the KKT stationarity condition tells us that there is a $\lambda\in\mathbb{R}$ such that $\nabla E_A(b^*)=\lambda n_B(b^*)$.

Now, note that $\nabla E_A(b)=(D_A(b)-d_A)\nabla D_A(b)$. Since $D_A(b)$ is a signed distance, whenever $\nabla D_A(b)$ exists, $\lVert\nabla D_A(b)\rVert=1$ (the Eikonal equation [17]). Thus, if $\nabla D_A(b^*)$ exists,

where we used the fact that $D_A(b)-d_A\ge0$ for all $b$. Finally, taking norms on both sides of $\nabla E_A(b^*)=\lambda n_B(b^*)$ and using $\lVert n_B(b^*)\rVert=1$ gives the desired result. ■

Lemma 2: Let $(\mathcal{A},\mathcal{B})$ be a nondegenerate pair. Then:

1. Case 1: If $\operatorname{int}(\mathcal{A})\cap\operatorname{int}(\mathcal{B})=\varnothing$, then $N_{\mathcal{A},\mathcal{B}}=D_A(b^*)$.

2. Case 2: If $\mathcal{A}\subseteq\mathcal{B}$, then $N_{\mathcal{A},\mathcal{B}}=2d_A-D_A(b^*)$.

Proof: In Case 1, we claim that we can replace the constraint $D_B(b)=0$ by $D_B(b)\le0$ without changing the minimizer. Suppose, for a contradiction, that a minimizer $b^*$ of the relaxed problem satisfies $D_B(b^*)<0$, that is, $b^*\in\operatorname{int}(\mathcal{B})$. Since the constraint is inactive, $b^*$ must also be a local minimum of the objective function $E_A(b)$. But $\operatorname{int}(\mathcal{A})\cap\operatorname{int}(\mathcal{B})=\varnothing$, so $b^*\notin\operatorname{int}(\mathcal{A})$. This is impossible: for a regular set, neither the signed distance nor $E_A$ has a local minimum outside its interior. Hence, the relaxed minimizer lies on $\partial B$ and coincides with the original $b^*$ by uniqueness. Dual feasibility in the KKT conditions then implies that the Lagrange multiplier $\lambda$ is nonpositive. Together with Lemma 1, this gives $\nabla E_A(b^*)=-(D_A(b^*)-d_A)n_B(b^*)$. Substituting this into the definition of $N_{\mathcal{A},\mathcal{B}}$ gives the desired result.

Case 2 is similar, but now we replace the constraint $D_B(b)=0$ by $D_B(b)\ge0$. Suppose, for a contradiction, that a minimizer $b^*$ of the relaxed problem satisfies $D_B(b^*)>0$, that is, $b^*\notin\mathcal{B}$. Since the constraint is inactive, $b^*$ must be a local minimum of $E_A(b)$. This is impossible: $\mathcal{A}\subseteq\mathcal{B}$ implies that $b^*\notin\mathcal{A}$, whereas $E_A$ has no local minimum outside $\mathcal{A}$. Hence, the relaxed minimizer lies on $\partial B$ and coincides with the original $b^*$ by uniqueness. Dual feasibility in the KKT conditions then implies that $\lambda$ is nonnegative. Together with Lemma 1, this gives $\nabla E_A(b^*)=(D_A(b^*)-d_A)n_B(b^*)$. Substituting this into the definition of $N_{\mathcal{A},\mathcal{B}}$ gives the desired result. ■

Proof of Proposition 1: For Cases 1 and 2, apply Case 1 of Lemma 2 to obtain $N_{\mathcal{A},\mathcal{B}}=D_A(b^*)$. If $\partial A\cap\partial B=\varnothing$, then $D_A(b^*)>0$; if $\partial A\cap\partial B\ne\varnothing$, then $D_A(b^*)=0$.

Case 3 requires two subcases. In addition to $\operatorname{int}(\mathcal{A})\cap\operatorname{int}(\mathcal{B})\ne\varnothing$, either Case 3.1 $\operatorname{int}(\mathcal{A})\cap\partial B\ne\varnothing$ or Case 3.2 $\operatorname{int}(\mathcal{A})\cap\partial B=\varnothing$ holds.

In Case 3.1, note that $D_A(b^*)<0$. But

and so $N_{\mathcal{A},\mathcal{B}}<0$. It remains to establish Case 3.2, where $\operatorname{int}(\mathcal{A})\cap\partial B=\varnothing$. Since $\operatorname{int}(\mathcal{A})$ is path connected and $\mathcal{A}=\overline{\operatorname{int}(\mathcal{A})}$, the conditions $\operatorname{int}(\mathcal{A})\cap\partial B=\varnothing$ and $\operatorname{int}(\mathcal{A})\cap\operatorname{int}(B)\ne\varnothing$ imply $\mathcal{A}\subseteq B$. We can therefore apply Case 2 of Lemma 2: $N_{\mathcal{A},\mathcal{B}}=2d_A-D_A(b^*)$. This is negative because $d_A<0$ and $D_A(b)\ge d_A>2d_A$ for all $b$. ■

Proof of Proposition 2: In this case, $d_A=0$, $E_A(b)=0.5\lVert b-a\rVert^2$, and thus

For a smooth boundary and a unique closest point, this is the ESDF between the point $a$ and the set $\mathcal{B}$ [25]. ■

Lemma 3: On any compact set $\mathcal{U}\subseteq\partial B$, define the measure:

in which $\rho_{A,h}$ is the density defined in (7). If $(\mathcal{A},\mathcal{B})$ is a non-degenerate pair, for any open set $\mathcal{N}\subseteq\partial B$ that contains $b^*$ (as defined in (3)), there exist positive constants $\Delta_{\mathcal{N}}$ and $C_{\mathcal{N}}$ such that, for $h$ sufficiently small:

Proof: Define, for any open set $\mathcal{N}$:

Note that if $b^{*} \in\mathcal{N}$ then necessarily $\Delta_{\mathcal{N}} > 0$. This comes from the fact that $b^{*}$ minimizes $E_{\mathcal{A}}(b)$ over $\partial B$ and it is the unique minimizer (this comes from the fact that the pair $(\mathcal{A},\mathcal{B})$ is nondegenerate).

Define $\sigma(U)=\int_U dS$. Now, still considering an open $\mathcal{N}\subset\partial B$ such that $b^{*}\in\mathcal{N}$, pick the set $\mathcal{M}$ with maximum $\sigma(\mathcal{M})$ such that $\mathcal{M}\subset\mathcal{N}$ and that it is small enough so:

the existence of which comes from the continuity of $E_{\mathcal{A}}(b)$ in a neighborhood of $b^{*}$ (the ESDF is always continuous everywhere), that $\Delta_{\mathcal{N}}>0$ and that $b^{*}$ minimizes $E_{\mathcal{A}}$ in $\partial B$. Thus, due to (42):

Now, choose $h$ sufficiently small so that

which is possible because $\Delta_{\mathcal{N}}>0$ and the third property in Definition 6. Now, note that:

And note that:

Let $I_{3}=\int_{\partial B}e^{-E_{\mathcal{A}}^{h}(b)/h^{2}}dS$. Note that $I_{3}\geq I_{1}+I_{2}$ since $\mathcal{M}\subset\mathcal{N}$. Thus, note that $\mu_{h}(\partial B-\mathcal{N})=I_{1}/I_{3}\leq I_{1}/(I_{1}+I_{2})\leq I_{1}/I_{2}$ since $I_{2}\geq0$. Using the two inequalities for $I_{1}$ and $I_{2}$ we obtain the desired result with $C_{\mathcal{N}}\triangleq\sigma(\partial B)/\sigma(\mathcal{M})$.

■

Proof of Proposition 3:

Let $g_{h}(\beta)=n_{B}(b)^{\top}\nabla E_{\mathcal{A}}^{h}(b)$ and $g^{*}=n_{B}(b^{*})^{\top}\nabla E_{\mathcal{A}}(b^{*})$. Let $\mathcal{N}\subset\partial B$ be an open subset containing $b^{*}$ in which $n_{B}(b)$ is continuous (guaranteed to exist by the non-degeneracy conditions). Set $\Delta g_{h}(b)=g_{h}(b)-g^{*}$.

Note that, using the triangle inequality and the fact that $\rho_{\mathcal{A},h}$ is a density and thus integrates to one:

We will analyze the integral first. Let $M_{h}=\max_{b\in\partial B}\|\nabla E_{\mathcal{A}}^{h}(b)\|$. In $\partial B-\mathcal{N}$, we can provide the bound $\|\Delta g_{h}(b)\|\leq M_{h}+|g^{*}|$.

In $\mathcal{N}$, we can use the fourth property in Definition 6 (uniform convergence of the gradient of $E_{\mathcal{A}}^{h}(b)$) and assure that for every $\eta>0$ that we choose, there exists a $h$ small enough such that:

Pick $h$ small enough so Lemma 3 and (49) holds for the chosen set $\mathcal{N}$. Thus, using the triangle inequality and the definition of $\mu_{h}$ in (40):

The upper bound of the error can go to 0 when $h\to+0$. This is because $\eta$ can be made arbitrarily small, $\mu_{h}(\mathcal{N})\leq\mu_{h}(\partial B)=1$, and the fifth property in Definition 6 guarantees that $M_{h}e^{-\Delta_{\mathcal{N}}/(2h^{2})}\to0$. Finally, the second property in Definition 6 guarantees that $|d_{\mathcal{A}}^{h}-d_{\mathcal{A}}|$ as $h\to+0$. And the proof is complete.

■

Proof of Proposition 4: Note the $f(t)$ can be written as $d_{\mathcal{A}}^{h}-f_{\mathrm{num}}(t)/f_{\mathrm{den}}(t)$, in which $f_{\mathrm{den}}(t)>0$ for all $t$. Thus, differentiability follows if the differentiability of $f_{\mathrm{num}}(t)$ and $f_{\mathrm{den}}(t)$ are established.

We will prove for $f_{\mathrm{num}}(t)$. The proof for the other follows similarly. The major step is to rewrite the integral using the formulas for the rigid motion.

Now, define $T_{\mathcal{A}}^{-1}(y,t)=Q_{\mathcal{A}}(t)^{\top}(y-s_{\mathcal{A}}(t))$, $T_{\mathcal{B}}(y,t)=Q_{\mathcal{B}}(t)y+s_{\mathcal{B}}(t)$ and $T_{\mathcal{A}}^{B}(\beta,t)=T_{\mathcal{A}}^{-1}(T_{\mathcal{B}}(\beta',t),t)$. Let $F(\beta',t)$ be defined as

Using (11) and the change of variable $\beta=T_{\mathcal{B}}(\beta',t)$, we can write

This is $k$ times differentiable as long as $F(\beta,t)$ is $k$ times differentiable in $t$. Since $E_{h}^{h}$ is infinitely differentiable, the limit of differentiability is on $Q_{A}(t)$, $Q_{B}(t)$, $s_{A}(t)$ and $s_{B}(t)$.