# A minimum swept-volume metric structure for configuration space

Yann de Mont-Marin, Jean Ponce, Jean-Paul Laumond

► To cite this version:

Yann de Mont-Marin, Jean Ponce, Jean-Paul Laumond. A minimum swept-volume metric structure for configuration space. IEEE International Conference on Robotics and Automation (ICRA), 2023, pp.3686-3692. 10.48550/arXiv.2211.11811. [https://arxiv.org/abs/2211.11811](https://arxiv.org/abs/2211.11811)

HAL Id: hal-03856704

https://inria.hal.science/hal-03856704v2

Submitted on 21 Nov 2022

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# A minimum swept-volume metric structure for configuration space

Yann de Mont-Marin*,a,b, Jean Ponceb,c and Jean-Paul Laumonda,b

***Abstract***— Borrowing elementary ideas from solid mechanics and differential geometry, this presentation shows that the volume swept by a regular solid undergoing a wide class of volume-preserving deformations induces a rather natural metric structure with well-defined and computable geodesics on its configuration space. This general result applies to concrete classes of articulated objects such as robot manipulators, and we demonstrate as a proof of concept the computation of geodesic paths for a free flying rod and planar robotic arms as well as their use in path planning with many obstacles.

## I. INTRODUCTION

### A. Context

The concept of configuration space, first formalized in the robotics context by Lozano-Pérez [24], is a key element for any approach to motion planning [20], [21]. In turn, developing effective algorithms for this task requires equipping the configuration space with an adequate metric structure, which is a key factor in developing the sampling and interpolation strategies that often are at the core of these algorithms.

Lavalle underlines in [21] the importance of the choice of distance and argues that it must capture within the same quantity different degrees of freedom of the system which have different units (for example an angle and a length) and that this issue can be fixed by considering a physically meaningful distance. In this paper we postulate that the minimal volume swept by a robot between two configurations is a good candidate in presence of obstacles. This follows Kuffner's suggestion in [18] in the case of the motion of a solid. He writes: *Intuitively, an ideal metric for path planning in SE(3) would correspond to a measure of the minimum swept volume [...]. Intuitively, minimizing the swept volume will minimize the chance of collision with obstacles.*

We propose in this paper to formalize and extend this intuition to configuration spaces for regular bodies undergoing a wide class of volume-preserving deformations using tools from solid mechanics and differential geometry. We introduce the notion of *minimum swept volume distance*<fnref n="1" /> as the minimum amount over all possible paths of volume to swept from one configuration to another. First in a very broad set-theoretical setting and then in a differential setting where the configuration space is identified to a set of diffeomorphisms resembling the construction of Lin and Burdwick in

the case of rigid bodies [23]. Under some mild assumptions, we prove that the minimum swept volume is indeed a distance with well-defined and computable geodesics. This is intuitively rather natural, but we are not aware of any previous statements of this result.

In addition to this theoretical contribution, our construction is an important step toward making the use of swept volumes in trajectory optimization [15], [30] computationally realistic. Indeed, traditionally, one first computes the region swept along a given trajectory, which is very costly and then measures the volume of the region. This prevents the optimization of the trajectory with respect to the swept volume. In comparison, our construction approaches the swept volume as a sum of local contributions. This allows us to formulate the problem of computing the minimum swept volume as an energy minimization problem and to use a state of the art second-order optimization solver [34].

In our experiments, we compute some geodesics for different solids and for some planar robotic arms. We finally use the minimum swept volume distance and its geodesics for interpolation in the well-known motion planning algorithm RRT [21] and show with multiple challenging examples a significant reduction of the number of nodes explored.

The main contributions of the paper are:

* the construction of a *local swept volume* operator,

* proofs that the minimum swept volume is a distance on the configuration space in a set-theoretical setting and in a differential setting using the local operator,

* a closed form for the operator in the case of a poly articulated system with rigid bodies, and

* the numerical computation of the minimum swept volume distance, its geodesics, and their use in RRT.

### B. Related work

**Swept volume.** Several algorithms are available to compute the region of space, swept by a moving robot, using voxels [29], [35] or its boundary using meshes [17], [31]. Despite algorithmic progress both computations remain extremely expensive. Blackmore and Leu discuss in [5] an analytical formulation of the swept region that may eventually lead to an efficient swept volume formulation but since then, this approach does not appear to have been explored further. Recent works such as [3], [26], [28] build heuristics around the swept volume to weight a Euclidean distance, unfortunately this approach tacitly assumes that the swept volume distance have a Euclidean structure, which is limiting. More recently Chiang first computes the swept volume of some trajectories and uses neural networks to train an estimator of the swept volume [10]. Although this partially addresses the problem of

<footnote>* Corresponding author</footnote>

<footnote>a Inria</footnote>

<footnote>b Département d'informatique de l'Ecole normale supérieure (ENS-PSL, CNRS, Inria)</footnote>

<footnote>c Center for Data Science, New York University</footnote>

<footnote n="1">¹Here the term swept volume refers to the measure of the swept region and not to the region itself. In addition, the notion of *minimum swept volume distance* is new and is not to be confused with the notion of *swept volume distance*</footnote>
<figure><img src="image_2.png" /><figcaption>Fig. 1. **Left:** Set-theoretical notations for the volume swept by a body along a path $\gamma$. **Right:** Illustration of a region swept twice and counted one.</figcaption></figure>

computational cost, neither the quantity used, nor its estimate constitute a distance on the configuration space.

**Configuration space metric structure.** Zhang et al. study in [37] the maximum vertex distance originally proposed by Lavalle in [21]. Although this distance is derived from a physical quantity and thus allows the homogenization of the different degrees of freedom, it may not be adapted to the problem of exploration in a constrained environment since the interaction of the system with the environment will depend on the displacement of all the points and not only the largest displacement. Zefran et al. extensively study Riemannian geometries on the configuration space of a solid and establish the properties necessary for a geometric structure to respect the fundamental principles of mechanics, but this study is restricted to the case of a configuration space isomorphic to SE(3) and does not cover the swept volume case.

## II. SWEPT-VOLUME DISTANCE CONSTRUCTION

### A. A set theoretical formulation

Let us equip the set of all compacts of the affine Euclidean space $\mathbb{E}^3$ with the topology induced by the Hausdorff distance and define a regular body as a compact equal to the closure of its interior [27]. For the sake of simplicity, we identify $\mathbb{E}^3$ as well as the vector space of its translations with $\mathbb{R}^3$, in the rest of the paper. Although this implies the choice of a coordinate system, all our construction is independent of that choice.

In this section we call configuration space, denote by $\mathcal{C}$, a set of regular bodies with constant volume $U > 0$. We require $\mathcal{C}$ to be path connected under the Hausdorff topology. Note that the largest possible configuration space formed by the set containing all the regular bodies of volume $U$ is itself path connected. Given a continuous path $\gamma: [0, 1] \to \mathcal{C}$ we define the region swept along the motion $\gamma$ and the corresponding swept volume by:

$$A[\gamma] = \bigcup_{t \in [0,1]} \gamma(t) \text{ and } V[\gamma] = V(A[\gamma]) - U, \quad (1)$$

where $V$ denotes the usual volume. Figure 1 (left) illustrates the notations.

**Lemma 1:** Given two configuration $B_0$ and $B_1$ in $\mathcal{C}$, let us denote by $\Gamma(B_0, B_1)$ the set of all continuous paths joining $B_0$ and $B_1$ in $\mathcal{C}$. The mapping $d: \mathcal{C}^2 \to \mathbb{R}^+$ defined by

$$d(B_0, B_1) = \inf_{\gamma \in \Gamma(B_0, B_1)} V[\gamma] \quad (2)$$

is a distance on $\mathcal{C}$ and we call it the minimum swept volume distance between $B_0$ and $B_1$.

<figure><img src="image_3.png" /><figcaption>Fig. 2. **Left:** Notations of the differential point of view of the volume swept by a body along a path. **Right:** Highlight of the local contribution at time $t$, in green the outgoing flux, in red the incoming flux. We also represent in purple the flow of the static velocity field $v_{q,q}$.</figcaption></figure>

*Proof:* $d$ is well defined on $\mathcal{C}^2$, indeed $\Gamma(B_0, B_1)$ is never empty because $\mathcal{C}$ is path connected. Given any configurations $B_0$ and $B_1$, we have $d(B_0, B_1) \ge 0$ since for any $\gamma$ in $\Gamma(B_0, B_1)$, $B_0$ (and of course $B_1$) is a subset of $A[\gamma]$ with volume $U$. The function $d$ is symmetric by definition (any path joining $B_0$ and $B_1$ also joins $B_1$ and $B_0$), and it satisfies the triangular inequality because given any path $\gamma_0$ joining $B_0$ to $B_1$ and any path $\gamma_{12}$ joining $B_1$ and $B_2$, the path $\gamma_0$ obtained by concatenating $\gamma_0$ and $\gamma_{12}$ in $B_1$ joins $B_0$ and $B_2$, with $V[\gamma_0] = V(A[\gamma_0] \cup A[\gamma_{12}]) - U \le V(A[\gamma_0]) + V(A[\gamma_{12}]) - 2U \le V[\gamma_0] + V[\gamma_{12}]$ since $B_2 \subset A[\gamma_{12}] \cap A[\gamma_{23}]$ with volume $U$. Now suppose $B_1 \ne B_0$. We can always find an open ball which is in $B_0 \setminus B_1$ (by swapping $B_0$ and $B_1$ if needed), so we have: $V(B_0 \cup B_1) = V(B_1) + V(B_0 \setminus B_1) > U$. For any path $\gamma$ joining $B_0$ to $B_1$ we have $V[\gamma] = V(A[\gamma]) - U \ge V(B_0 \cup B_1) - U > 0$. Trivially, we have $d(B_0, B_0) = 0$ because a path $\gamma_0$ such that $\gamma_0(t) = B_0$ for any $t$ in an open interval verify $V[\gamma_0] = V(B_0) - U = 0$. ■

The minimum swept volume distance $d$ is generally not a geodesic distance. The length of a geodesic must be the sum of the lengths of its two half sub-paths, but the swept volume $V$ count a region visited $n$ times only once. Then for a candidate geodesic the sum of the swept volumes for two sub-paths visiting the same region may not be equal to the swept volume of the complete path. We illustrate the phenomenon in Figure 1 (right)

In robotics we need to be able to build geodesics to interpolate between configurations. To obtain a distance with geodesics we now use a differential setting and construct the distance as an integral of local contributions.

### B. A differential formulation

Using a slightly different point of view, borrowed from solid mechanics [2], we now consider a regular solid of reference $B$ in $\mathbb{R}^3$ whose boundary $\partial B$ is a smooth orientable surface without boundary. We consider a space $\mathcal{F}$ of diffeomorphisms of $\mathbb{R}^3$ to allow for deformations of the reference body $B$. We require that $\mathcal{F}$ be a path connected differentiable manifold with respect to the strong functional topology, and that its elements be isochoric, i.e., preserve volume and orientation in $\mathbb{R}^3$ [2]. Thus, for all $q$ in $\mathcal{F}$, $q$ is a diffeomorphism of $\mathbb{R}^3$ and $q(B)$ is a regular body with
the same volume as B. We denote by $\mathcal{E}$ the mapping from $\mathcal{F}$ to $\mathcal{C}$ defined by $q \mapsto B_q$ and we make a first hypothesis (H1): *The embedding function $\mathcal{E}$ is injective*. With (H1), $\mathcal{E}$ is a bijection between $\mathcal{F}$ and $\mathcal{C} = \mathcal{E}(\mathcal{F})$ so we can identify the diffeomorphism $q$ and the regular body $B_q = \mathcal{E}(q) = q(B)$.

A differentiable path $\gamma$ in $\mathcal{F}$ is an $\mathcal{F}$-isotopy of the ambient space, i.e., a smooth map $I \times \mathbb{R}^3 \to \mathbb{R}^3$ where $I$ is some interval of the real line such that for any time $t$, the mapping $\gamma: x \mapsto \gamma(t,x)$ is an element of $\mathcal{F}$. We can define its time derivative $\dot{\gamma}_t: \mathbb{R}^3 \to \mathbb{R}^3$ for $x \in \mathbb{R}^3$ as $\dot{\gamma}_t(x) = \frac{\partial}{\partial t}\gamma(t,x)$. For each $t$, the mapping $\dot{\gamma}_t$ belongs to the tangent space $T_\gamma \mathcal{F}$. Regardless of any path, a configuration $q$ in $\mathcal{F}$ and an element $\dot{q}$ in $T_q \mathcal{F}$ induces a velocity field on $\mathbb{R}^3$ defined as $v_{q,\dot{q}}(y) = \dot{q} \circ q^{-1}(y)$. Note that we use the variable name $x$ to describe a point of the reference body $B$ in $\mathbb{R}^3$ before the transformation and $y = q(x)$ for a point of $B_q$. We illustrate the notation in Figure 2 (left). Note also that since the elements of $\mathcal{F}$ are isochoric diffeomorphisms, the vector field $v_{q,\dot{q}}$ has zero divergence<fnref n="2" /> [2]. We can now define the local swept-volume as the flux of the velocity vector field through the boundary $\partial B_q$ of $B_q$:

$$F(q, \dot{q}) = \int_{\partial B_q} \max(0, \langle v_{q,\dot{q}}(y), n_q(y) \rangle) dA(y), \quad (3)$$

where $n_q(y)$ is the unit normal vector pointing outward at point $y$ of the boundary $\partial B_q$. Note that taking the max ensures we only consider the outgoing flux since incoming flux does not contribute to the swept volume as illustrated in Figure 2 (right). For a path $\gamma$ the additional volume swept by $B_\gamma$ between $t$ and $t+\delta t$ is $F(\gamma, \dot{\gamma}_t)\delta t$. We can now define the flux-based swept volume associated with $\gamma$ as the sum of all local contribution:

$$V'[\gamma] = \int_{0}^{1} F(\gamma_t, \dot{\gamma}_t) dt. \quad (4)$$

Note that unlike the ordinary swept volume $V[\gamma]$, which counts the contribution of a region of space visited $n$ times only once, $V'[\gamma]$ counts it $n$ times. When no region is visited more than once, $V'[\gamma] = V[\gamma]$.

In Eq. (4), $F$ can be interpreted as a Lagrangian on $\mathcal{F}$ and the flux-based swept volume $V'[\gamma]$ as the action of $F$ along $\gamma$. We can define the least action of $F$ between any pair of configurations $B_0$ and $B_1$ as

$$d'(B_0, B_1) = \inf_{\gamma \in \Gamma^1(B_0, B_1)} V'[\gamma], \quad (5)$$

where $\Gamma^1(B_0, B_1)$ is the set of differentiable paths in $\mathcal{F}$ such that $\mathcal{E}(\gamma_0) = \gamma_0(B) = B_0$ and $\mathcal{E}(\gamma_1) = \gamma_1(B) = B_1$.

In the remaining part of this subsection, we prove that $d'$ as in (5) defines a distance on and we discuss the existence of its geodesics. The key point to prove is that for all $q$ in $\mathcal{F}$ the mapping $F_q: \dot{q} \mapsto F(q, \dot{q})$ defines a norm on the vector space $T_q \mathcal{F}$, which makes $F$ a reversible Finslerian geometry [19] and in turn ensures that $d'$ is a distance. In this context, the existence of a geodesic between any pair of configurations is

given by the well-known Hopf-Rinow theorem that we will detail in our case. The study of the differential regularity of the operator is out of the scope of this article.

For a path $\gamma$ in $\mathcal{F}$ the velocity field $v_{\gamma_t, \dot{\gamma}_t}$ is not static in general. If the velocity field is static, the path is exactly the flow induced by the ordinary differential equation (ODE) associated to this static field. Moreover, for an element $q$ and an element $\dot{q}$ of the tangent $T_q \mathcal{F}$, we can uniquely create a path $\gamma^{q,\dot{q}}$ solution of the ODE associated to the static velocity field $v_{q,\dot{q}}$ with the initial conditions given by $\gamma_0^{q,\dot{q}} = q$ and $\dot{\gamma}_0^{q,\dot{q}} = \dot{q}$. $\gamma^{q,\dot{q}}$ is the unique path passing through $q$ with derivative $\dot{q}$ which induce a static vector field equal to $v_{q,\dot{q}}$. The vector field $v_{q,\dot{q}}$ and the path $\gamma^{q,\dot{q}}$ are illustrated in Figure 2 (right). We now need a second hypothesis: these paths remain at least locally in $\mathcal{F}$:

$$(H2): \forall q, \dot{q}, \exists \varepsilon > 0, \forall t \in (-\varepsilon, +\varepsilon), \gamma_t^{q,\dot{q}} \text{ is an element of } \mathcal{F}.$$

**Theorem 2:** Under the hypotheses (H1) and (H2) the mapping:

$$F_q: \dot{q} \mapsto \int_{\partial B_q} \max(0, \langle v_{q,\dot{q}}(x), n_q(x) \rangle) dA(x) \quad (6)$$

defines a norm on $T_q \mathcal{F}$.

*Proof:* Let us consider some fixed $q$ in $\mathcal{F}$. Using Stokes' theorem, the fact that $v_{q,\dot{q}}$ has a null divergence, that $\max(0, C)$ is the positive part of a real $C$ and that $\partial B_q = q(\partial B)$ we obtain:

$$F_q(\dot{q}) = \frac{1}{2} \int_{q(\partial B)} |\langle v_{q,\dot{q}}(x), n_q(x) \rangle| dA(x). \quad (7)$$

From this formulation it follows easily from the linearity of the integral and the inner product as well as from the norm properties of the absolute value that $F_q$ is positive $F_q(\dot{q}) \ge 0$, homogeneous $F_q(\lambda \dot{q}) = |\lambda| F_q(\dot{q})$ and sub-additive $F_q(\dot{q} + \dot{q}') \le F_q(\dot{q}) + F_q(\dot{q}')$ for any $\dot{q}, \dot{q}'$ in $T_q \mathcal{F}$ and $\lambda$ in $\mathbb{R}$.

Now, let us consider $\dot{q}$ such that $F_q(\dot{q}) = 0$. We use (H2) to construct the path $\gamma^{q,\dot{q}}$. Since the integrand in Eq. (6) is always positive, $F_q(\dot{q}) = 0$ implies that $\langle v_{q,\dot{q}}(y), n_q(y) \rangle \ge 0$ for every $y$ in $\partial B_q$. Recall that $\partial B_q$ is a compact surface without boundary because it is isomorphic to $\partial B$. If a vector field is everywhere tangent to a compact surface without boundary, its integral curves passing by a point on this surface stay on the surface. But $\gamma^{q,\dot{q}}$ is exactly the flow of $v_{q,\dot{q}}$ so we have that $\gamma_t^{q,\dot{q}}(y)$ is in $\partial B_q$ for every $y$ in $\partial B_q$ and $t$ in $(-\varepsilon, +\varepsilon)$.

We have $\gamma_t^{q,\dot{q}}(\partial B) \subset q(\partial B)$ and in turn $\mathcal{E}(\gamma_t^{q,\dot{q}}) = \mathcal{E}(q)$ because $\partial B$ is a compact orientable surface without boundary for all $t$ in $(-\varepsilon, +\varepsilon)$. Using $\mathcal{E}$ injectivity from (H1) we have $\gamma_t^{q,\dot{q}} = q$ which makes $\gamma^{q,\dot{q}}$ a constant path on $(-\varepsilon, +\varepsilon)$ and in turn $\dot{q} = \dot{\gamma}_0^{q,\dot{q}} = 0$, which concludes the proof. ■

**Remark 1:** Both (H1) and (H2) are necessary, and we can construct a counter example when either (H1) or (H2) is not satisfied.

When (H1) and (H2) and thus Theorem 2 holds, $F$ is a reversible Finslerian geometry and $d'$ as defined in Eq. (5) is automatically a distance on the configuration space. To

<footnote n="2">²It is a direct consequence of the transport theorem.</footnote>
<figure><img src="image_4.png" /><figcaption>Fig. 3. Geodesics for different systems and different pairs of configurations using the minimum swept volume metric structure (top row) and the canonical Riemannian one (bottom row). From left to right: a rod (x2), an arrow-head shape, a bracket shape, a double pendulum, and a triple pendulum.</figcaption></figure>

certify the existence of $d'$ geodesics between all pairs of configurations, we rely on topological properties of the space $\mathcal{F}$ equipped with $d'$ and the well-known Hopf-Rinow [6] theorem. In our case it can be written as follows:

**Theorem 3 (Hopf-Rinow):** If (H1) and (H2) hold and $\mathcal{F}$ equipped with the distance $d'$ is locally compact, complete, and path connected, then any pair of configurations can be joined by a geodesic.

### C. The case of a poly articulated system with rigid bodies

**Single rigid body.** When the system is a solid $B$ freely moving the diffeomorphisms are exactly the isometries and we can identify $\mathcal{F}$ with the space $SE(3)$ of solid placements. Given $[R, p]$ in $SE(3)$ where $R$ is a rotation matrix and $p$ a vector of $\mathbb{R}^3$, as well as $[\dot{R}, \dot{p}]$ in $T_{[R,p]}SE(3)$, we define the related diffeomorphism and its tangent element as $q(x) = Rx + p$ and $\dot{q}(x) = \dot{R}x + \dot{p}$.

**Lemma 4:** Let $[\omega]_\times = R^T \dot{R}$ the skew-symmetric form of $\omega$ and $u = R^T \dot{p}$, where $\omega$ and $u$ are respectively the instantaneous rotation axis and the instantaneous linear velocity of the local frame. We have

$$F(q, \dot{q}) = G_B(\omega, u) \quad (8)$$

$$= \frac{1}{2} \int_{\partial B} |\det(\omega, x, n(x)) + \langle u, n(x) \rangle| dS(x), \quad (9)$$

where $n(x)$ is the normal to $\partial B$ in $x$, and $dS(x)$ is the element of surface on $\partial B$ at the same point.

**Proof:** We start from the formulation of $F$ in Eq. (7). Since $q(\partial B)$ is from $\partial B$ by the rigid transformation defined by $R$ and $p$, we have $n_q(y) = Rn(q^{-1}(y))$. Recall that $v_{q,\dot{q}} = \dot{q} \circ q^{-1}$. Using the change of variables $x = q^{-1}(y)$ with a Jacobian determinant equal to 1 because of the isochorism, we have:

$$F(q, \dot{q}) = \frac{1}{2} \int_{\partial B} |\langle \dot{R}x + i, Rn(x) \rangle| dS(x). \quad (10)$$

Using the adjoint $R^T$ of $R$ and the relation $\langle [\omega]_\times x, n \rangle = \langle \omega \times x, n \rangle = \det(\omega, x, n)$ concludes the proof. ■

In Eq. (8), $(\omega, u)$ is a twist, an element of the spatial algebra as detailed by Featherstone in [13]. The integral $G_B$

is calculated over the fixed reference body $B$ which allows us with a simple, if tedious, calculation to find a closed form of $G_B$ for a polytope body $B$. See Appendix IV-A for details.

It is important to note that $F$ depends on $q$ and $\dot{q}$ only through the twist $(\omega, u)$ which comes from $[[\omega]_\times, u] = [R, p]^{-1}[\dot{R}, \dot{p}]$ which is the Left lie algebra representation of $[\dot{R}, \dot{p}]$ as detailed in [36]. This means that the local swept volume operator $F$ is left invariant, which in turn implies that the local swept volume does not depend on the position and orientation of the system but only on its velocity. The left invariance of $F$ allows us to use Theorems 2 and 3 in the following corollary:

**Corollary 4.1:** If $B$ has no continuous symmetry group, the distance $d'$ is defined on $SE(3)$ and any pair of configurations can be joined by a geodesic.

**Proof:** For (H1) to hold with $\mathcal{F} = SE(3)$, it suffices by definition that the body $B$ does not have any continuous symmetry group. The symmetries of a compact are the group of solid transformations by which the image of the compact is the compact itself. If the functional space contains such a group, $\mathcal{E}$ is not injective. On the other hand, if $B$ has no symmetry group, $\mathcal{E}$ is necessarily injective on $SE(3)$.

Since $SE(3)$ is a Lie group, the hypothesis (H2) is always verified. On a Lie group, the path induced by the static velocity field are exactly the exponential map of the group [11]. If the configuration space is the whole group $SE(3)$, the exponential map output always belongs to $SE(3)$ and (H2) is verified. $F$ is left invariant [19] and $SE(3)$ equipped with a norm is always locally compact and path connected. It is known that if a norm on a group is left invariant then it defines a complete topology [19]. Using the Hopf-Rinow theorem 3 concludes the proof. ■

**Multiple rigid bodies.** When $B$ is the disjoint union of its $N$ rigid links $B_i$ we can associate the configuration space to a subset of $SE(3)^N$. If we assume first that the configuration space does not contain configuration for which the system is self colliding, $\mathcal{C}$ will be made of isochoric diffeomorphisms and the local swept volume is the sum of the volumes swept
by each link:

$$F(q, \dot{q}) = \sum_{i=1}^{N} G_{B_i}(J_i(q)\dot{q}), \qquad (11)$$

where $J_i(q)$ is the Jacobian of the forward kinematic for the link $i$. More precisely, $J_i(q)\dot{q}$ is the vector of the twists which represents the instantaneous velocity of the link $i$ when the system is in configuration $q$ and varies according to $\dot{q}$, see [13] for details. Analytic derivatives of the forward kinematic Jacobian are well known [7] and we can easily compute the local operator $F$ for a poly-articulated system. Moreover, note that if we interpret the forward kinematic as an immersion of the configuration space in $SE(3)^N$, $F$ is nothing else than the pullback of the norm on the Cartesian product of $N$ copies of $SE(3)$ respectively equipped with the norms $G_{B_i}$. The previous reasoning still holds even when the poly-articulated system may intersect itself, the only difference being that a collision region will be counted once for each link involved in the collision.

**Corollary 4.2:** For a poly-articulated system with $N$ rigid links such that none of the $B_i$ possesses a continuous symmetry group and the Jacobian of the forward kinematic always has full rank, the distance $d'$ is well defined on the configuration space and any pair of configurations can be joined by a geodesic.

**Proof:** If the Jacobian has full rank, the forward kinematic is an immersion of $\mathcal{C}$ in $SE(3)^N$. If none of the $B_i$ possesses a continuous symmetry group, each $G_{B_i}$ defines a complete Finslerian geometry on $SE(3)$. Using that the pullback by an immersion of the Cartesian product of a complete Finslerian geometry is a complete Finslerian geometry [6] concludes the proof. ■

### III. EXPERIMENTS

#### A. Technical details

To illustrate our construction we restrict ourselves to the case of a 2D physical space with 2D solids in motion and planar poly-articulated arms. The previous section provides a method for the computation of the distance and its geodesics in these cases. First, we implement $G_B$ in the automatic differentiation framework CasADi [1]. Second, we use CasADi in conjunction with the rigid body library Pinocchio [8] to obtain a differentiable implementation of the forward kinematic Jacobian and in turn a differentiable implementation of $F$ when using a smooth approximation of the absolute value as in [33]. In Finslerian geometry [12], geodesics are exactly the stationary trajectories for the energy functional associated with $F$ [19]. Now we translate the infinite dimensional minimization problem into a finite dimensional one using $M$ time steps. For a pair of configurations $(a, b)$ in $\mathcal{C}^2$, we now call $q = (q_i)_{i=0..M}$ an array of $M+1$ elements of the configuration space $\mathcal{C}$ joining $a = q_0$ and $b = q_M$ as well as $w = (w_i)_{i=1..M}$ an array of $M$ elements of the configuration space lie algebra $\mathfrak{c}$ which represent the $M$ tangential vectors that maps $q_i$ to $q_{i+1}$ through the group exponential map [11].

We solve the multiple shooting problem:

$$ \begin{gathered} \min_{w \in \mathcal{C}^M, q \in \mathcal{C}^{M+1}} \sum_{k=0}^{M-1} F(q_k, w_k)^2 \qquad (12) \\ q_{k+1} = \text{Exp}_{q_k}(w_k), \quad k = 0..M-1; \quad q_0 = a, q_M = b, \end{gathered} $$

where $\text{Exp}_q$ is the exponential map of the configuration space Lie group in configuration $q$ implemented through the Pinocchio library [9]. We solve this minimization program using an off the shelf optimizer based on the interior point method [25] because we can construct an initial guess that respects the constraints. As this problem is not convex in general, we use multiple initial guesses which are constructed with the Riemannian geodesic and a random perturbation. To compute $d'(a, b)$ it is sufficient to compute the sum of the terms $F$ taken on the steps of path solution to the problem (12).

**Note on complexity.** We could use a Riemannian approximation of $F$ given by $g = \partial_{\dot{q}\dot{q}}F^2$ to obtain differential equations characterizing the geodesics [14], but it would still require solving a bounded value problem with the same order of complexity. The problem we solve is in general non-convex, and a global convergence study is out of the scope of this paper. The overall complexity of the geodesics' computation is of the same order as that of trajectory optimization algorithms such as dynamic differential programming [30], [15] but without the dynamical constraints because in our case the control variable is exactly the velocity. In motion planning, we use the geodesics as the paths associated to the tree edges [21] and the distance for the nearest neighbor searches. Thus, when we use the new metric, the computational complexity of the planning is of the same order as that of methods combining RRT and trajectory optimization such as [16]. However, in our case the cost of the trajectory optimization problem is a distance, and we can combine the use of efficient spatial trees structure such as KD tree [4] with the optimization procedure for every new node instead of a two stages approach as in [16]. Finally, even if dealing with obstacles is the main motivation behind the choice of the minimum swept volume distance as a metric structure, the distance and its geodesics construction do not depend on any specific environment choice. For a given system, we can first compute the solution of the optimization procedure for a large exploration of the free space and then use this computation to construct initial guesses for the geodesics and estimators for the distances when we explore specific environments. On the other hand, most trajectory optimization methods use the knowledge of the environment in the optimization procedure and no such pre-computation can be done.

In the remaining part of this section, we give a proof of concept of the minimum swept volume distance advantages over the canonical Riemannian distance by visualizing the geodesics of different systems and by observing the gain in terms of nodes needed in RRT. In the context of rigid body dynamic, the geodesic of the canonical Riemannian metric structure is the linear or spherical linear interpolation of each
<figure><img src="image_5.png" /><figcaption>Fig. 4. Different motion planning problems solved with RRT using the minimum swept volume metric structure (top row) and the canonical Riemannian one (bottom row). From left to right: a. some random boxes, b. a narrow passage, c. a crossroad, d. a random cluttered environment and e. a narrow passage. The shape in red (resp green) is the starting (resp goal) configuration. In blue some snapshots of the final trajectory. We plot in dark green the position of the end effector for all the configurations in the tree.</figcaption></figure>

degree of freedom.

## B. Geodesics

We compute the geodesics of the minimum swept volume distance, and we compare them with the canonical Riemannian geodesics for different systems and different pairs of configurations. The trajectories obtained are illustrated in Figure 3. We observe that the geodesics can have quite varied behaviors, but we often observe some characteristic sub paths of the motion that seems to be steady in some sense. This is an instance of the *turnpike* property extensively studied by Trelat [32] in optimal control; the turnpike property of an optimal control problem is the fact that optimal trajectories decompose into transient phases and a steady state for most of the motion. A steady state is a solution of the static optimization problem associated to an optimal control problem. In the context of rigid body motion and minimal swept volume metric structure, it ma correspond to regular motions such as pure translation or pure rotation. For the rod and the arrowhead shape the turnpike is a translation in the direction perpendicular to the shorter edge. For the bracket shape it is a rotation with a center such that the edges are tangential to the circles induced by the rotation. For the arms the turnpike is the rotation of the base link when the remaining links are orthogonal to the base link.

## C. Motion planning

Since the optimization problem 12 is in general non-convex, it is important to check that the empirical solutions verify the axioms of a distance. To validate the use of the minimum swept volume distance in RRT [22] we verify the triangular inequality on 100000 triplets of random configurations. The accuracies are 97.1% for the rod, 98.2% for the arrowhead shape, 96.7% for the bracket shape, 91.2% for the double pendulum and 87.2% for the triple pendulum.

We compare the usage of the minimum swept volume metric structure to the canonical Riemannian one in RRT

when solving the motion planning problems illustrated in Figure 4. We solve the problem of an arrowhead shape moving in a randomly generated set of boxes, a narrow passage, and a crossroad. We also solve the motion of a two-link arm moving in a randomly generated cluttered environment and in a set of tightened obstacles. For each scenario we select a pair of configurations and we run RRT until they are connected. We can observe in Figure 4 how RRT needs far fewer nodes to solve the problem when using the minimum swept volume metric structure instead of the canonical Riemannian one.

For the case of the narrow passage Fig. 4 b., we plot in Figure 5 the average number of nodes needed to solve the problem in function of the tunnel width. The advantage becomes more marked as the passage narrows. Let us consider two configurations of the arrowhead shape on either side of the narrow passage with their centers aligned with the tunnel. Based on the geodesics observed in Figure 3, the Riemannian geodesics will be this pure translation if in addition the orientation of the configurations are the same and aligned with the tunnel direction. On the other hand, the turnpike property of the minimum swept volume geodesics will naturally produce this translation whatever the orientation of the configurations. Thus, the narrowness of the tunnel affects the requirements in position but also in terms of orientation in the Riemannian case. This explains why the advantage of the minimum swept volume metric structure is even more important when the passage gets narrower.

# IV. CONCLUSION

This paper lays the theoretical foundations of the minimum swept volume metric structure on the configuration space. Our construction allows us to formulate the swept volume as an integral of local contributions so tools from Finslerian geometry ensure the existence of the geodesics and provide a computation procedure.
<figure><img src="image_6.png" /><figcaption>Fig. 5. Average number of nodes required to solve a narrow passage problem using RRT with the minimum swept volume metric structure or the canonical Riemannian one over 50 random seeds.</figcaption></figure>

Our metric structure addresses some issues raised in the introduction because it is unique and does not rely on any parametrization choice. Our experiments do suggest it is appropriate for exploration in the presence of obstacles. These two points makes the strength of our construction. The fact that it is useful for dense environment while being agnostic to the environment is a powerful feature.

There are several avenues for further work. First, we plan to implement a dedicated geodesic solver in C++ in a general 3D setting. We will then properly benchmark the total computational time of the approach. Second, the turnpike property framework [32] seems to be a powerful tool to study the convergence of algorithms using the minimum swept volume metric structure. On the other hand, the Riemannian approximation $g = \partial_{\dot{q},\dot{q}} F^2$ may also lead to other applications: it can be used to characterize the geodesics by differential equations, but it is also a simple way to create a new sampling method for the configuration space. We can indeed sample the configuration space following a law proportional to $\det(\partial_{\dot{q},\dot{q}} F^2)$ the volume element of our geometry. This will favor tricky regions where displacements sweep more volume.

Geometric structures may have properties useful in robotic applications and we think that the formalism behind the minimum swept volume distance is a step in this direction.

# APPENDIX

## A. Closed form for a solid in motion

The following calculation apply for a generic polyhedral body in a 3D physical space. Let us consider a polyhedral reference body B with I facets and each facet $f_i$ is a collection of $J_i$ edges represented by the pair of points $x_{i,j}^s, x_{i,j}^e$. We call $A_i$ the area of facet i and $n_i$ the normal vector to the facet.

We have $G_B(\omega, u) = \frac{1}{2} \sum_{i=1}^{I} g_i(\omega, u)$ with:

$$g_i(\omega, u) = \begin{cases} A_i |<n_i, u>| & \omega \propto n_i \\ \frac{1}{6} \sum_{k=1}^{J_i} \frac{[l_{i,j}^e l_{i,j}^e + l_{i,j}^s l_{i,j}^s] + \chi(l_{i,j}^e l_{i,j}^s)](z_{i,j}^s - z_{i,j}^e)}{|\omega|^2 - <\omega, n_i>|} & \omega \asymp n_i \end{cases}$$

where $l_{i,j}^\alpha = \det(\omega, x_{i,j}^\alpha, n_i) + <u, n_i>$ and $z_{i,j}^\alpha = <\omega, x_{i,j}^\alpha> - <\omega, n_i><x_{i,j}^\alpha, n_i>$ where $\chi(x,y) = xy^{\frac{|x|-|y|}{x-y}}$, $\chi(x,x) = x|x|$.

**Proof:** We decompose the integral into a sum of the integrals on each facet and we have $g_i(\omega, u) = \int_{f_i} |\det(\omega, x, n_i) + <u, n_i>| dS(x)$. If there is a real $\lambda$ such that $\omega = \lambda n_i$, the integrand does not depend on $x$ and we have the result. Otherwise let us construct the direct orthonormal basis $(u_z, u_l, n_i)$ with $u_z = \frac{\omega - <\omega, n_i>n_i}{\sqrt{|\omega|^2 - <\omega, n_i>|}}$, such that he plan span by $(u_z, u_l)$ contain the facet and we have $dS = dldz$ where $l$ and $z$ are the corresponding coordinate. With this basis we have

$$\det(\omega, x, n_i) = \det(\omega - <\omega, n_i>n_i, x, n_i) = \sqrt{|\omega|^2 - <\omega, n_i>|^2 l}.$$

Applying the change of variable $l \mapsto \sqrt{|\omega|^2 - <\omega, n_i>|^2 l + <u, n_i>|}$ and $z \mapsto \sqrt{|\omega|^2 - <\omega, n_i>|^2}$ in the integral we can calculate:

$$g_i(\omega, u) = \frac{1}{|\omega|^2 - <\omega, n_i>|^2} \int_{\phi(f_i)} |l| dldz,$$

where $\phi(f_i)$ is the l-z transform of facet $f_i$, if we note the l-z value of a vertex $l_j^\alpha, z_j^\alpha$ where $\alpha$ stands for e or s we have the proper expression for $l_j^\alpha, z_j^\alpha$ in the frame.

We just need to calculate $|l| dldz$ over the polygon $\phi(F)$ to finish. Using the fact that the edges constitute a closed loop we have by integrating $l$:

$$\int_{\phi(f_i)} |l| dldz = \frac{1}{2} \sum_{j=1}^{L} \int_{z_j^e}^{z_j^s} l_j(z) |l_j(z)| dz$$

Using the affine parametrization of an edge $j$ we have $l_{i,j}(z) = \frac{z_{i,j}^s - z_{i,j}^e}{z_{i,j}^e - z_{i,j}^s} (l_{i,j}^e - l_{i,j}^s) + l_{i,j}^s$ and use it for a last change of variable and we have:

$$\int_{\phi(f_i)} |l| dldz = \frac{1}{6} \sum_j \frac{z_{i,j}^e - z_{i,j}^s}{l_{i,j}^e - l_{i,j}^s} (|l_{i,j}^s|^3 - |l_{i,j}^e|^3) \\ = \frac{1}{6} \sum_j (z_{i,j}^s - z_{i,j}^e) [l_{i,j}^s |l_{i,j}^s| + l_{i,j}^e |l_{i,j}^e| + \chi(l_{i,j}^e, l_{i,j}^s)]$$

Hence the result when we introduce $\chi$ such as $\chi(x,y) = xy^{\frac{|x|-|y|}{x-y}}$, $\chi(x,x) = x|x|$ for a factorization that also work when $l_{i,j}^e = l_{i,j}^s$.

# ACKNOWLEDGEMENT

This work was partially supported by the HPC resources from GENCI-IDRIS (Grant 011011181R1), the European Regional Development Fund under the project IMPACT (reg. no. CZ.02.1.01/0.0/0.0/15 003/0000468)”, the Inria-NYU collaboration agreement, the Louis Vuitton Louis Vuitton ENS Chair on Artificial Intelligence, and the French government under management of Agence Nationale de la Recherche as part of the "Investissements d'avenir" program, reference ANR-19-P3IA0001 (PRAIRIE 3IA Institute).
REFERENCES

[1] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi – A software framework for nonlinear optimization and optimal control,” *Mathematical Programming Computation*, vol. 11, no. 1, pp. 1–36, 2019.

[2] V. I. Arnold, K. Vogtmann, and A. Weinstein, *Mathematical methods of classical mechanics*, 2nd ed., ser. Graduate Texts in Mathematics. New York, NY: Springer, May 1989.

[3] J. Baxter, M. R. Yousefi, S. Sugaya, M. Morales, and L. Tapia, “Deep prediction of swept volume geometries: Robots and resolutions,” in *2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, Oct. 2020. [Online]. Available: https://doi.org/10.1109/iros45743.2020.9341396

[4] J. L. Bentley, “Multidimensional binary search trees used for associative searching,” *Communications of the ACM*, vol. 18, no. 9, pp. 509–517, Sept. 1975. [Online]. Available: https://doi.org/10.1145/361002.361007

[5] D. Blackmore and M. Leu, “Analysis of swept volume via lie groups and differential equations,” *The International Journal of Robotics Research*, vol. 11, no. 6, pp. 516–537, Dec. 1992. [Online]. Available: https://doi.org/10.1177/027836499201100602

[6] M. R. Bridson and A. Haefliger, “Length spaces,” in *Grundlehren der mathematischen Wissenschaften*. Springer Berlin Heidelberg, 1999, pp. 32–46. [Online]. Available: https://doi.org/10.1007/978-3-662-12494-9_3

[7] J. Carpentier, R. Budhiraja, and N. Mansard, “Proximal and sparse resolution of constrained dynamic equations,” in *Robotics: Science and Systems XVII*. Robotics: Science and Systems Foundation, July 2021. [Online]. Available: https://doi.org/10.15607/rss.2021.xvii.017

[8] J. Carpentier and N. Mansard, “Analytical Derivatives of Rigid Body Dynamics Algorithms,” in *Robotics: Science and Systems (RSS 2018)*, Pittsburgh, United States, June 2018. [Online]. Available: https://hal.laas.fr/hal-01790971

[9] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” *Proc. 2019 IEEE/SICE Int. Symp. Syst. Integr. SII 2019*, pp. 614–619, 2019.

[10] H.-T. L. Chiang, A. Faust, S. Sugaya, and L. Tapia, “Fast swept volume estimation with deep learning,” in *Springer Proceedings in Advanced Robotics*. Springer International Publishing, 2020, pp. 52–68. [Online]. Available: https://doi.org/10.1007/978-3-030-44051-0_4

[11] G. S. Chirikjian, *Stochastic Models, Information Theory, and Lie Groups, Volume 1*. Birkhäuser Boston, 2009. [Online]. Available: https://doi.org/10.1007/978-0-8176-4803-9

[12] S. Deng, *Homogeneous Finsler spaces*. New York, N.Y: Springer, 2012.

[13] R. Featherstone, *Rigid body dynamics algorithms*. New York: Springer, 2008.

[14] N. Guigui and X. Pennec, “A reduced parallel transport equation on lie groups with a left-invariant metric,” in *Lecture Notes in Computer Science*. Springer International Publishing, 2021, pp. 119–126. [Online]. Available: https://doi.org/10.1007/978-3-030-80209-7_14

[15] W. Jallet, A. Bambade, N. Mansard, and J. Carpentier, “Constrained Differential Dynamic Programming: A primal-dual augmented Lagrangian approach,” in *2022 IEEE/RSJ International Conference on Intelligent Robots and Systems*, Kyoto, Japan, Oct. 2022. [Online]. Available: https://hal.archives-ouvertes.fr/hal-03597630

[16] J. Kamat, J. Ortiz-Haro, M. Toussaint, F. T. Pokorny, and A. Orthey, “Bitkomo: Combining sampling and optimization for fast convergence in optimal motion planning,” 2022. [Online]. Available: https://arxiv.org/abs/2203.01751

[17] Y. J. Kim, G. Varadhan, M. C. Lin, and D. Manocha, “Fast swept volume approximation of complex polyhedral models,” *CAD Comput. Aided Des.*, vol. 36, no. 11, pp. 1013–1027, 2004.

[18] J. J. Kuffner, “Effective sampling and distance metrics for 3D rigid body path planning,” *Proc. - IEEE Int. Conf. Robot. Autom.*, vol. 2004, no. 4, pp. 3993–3998, 2004.

[19] S. Lang, *Fundamentals of Differential Geometry*. Springer New York, 1999. [Online]. Available: https://doi.org/10.1007/978-1-4612-0541-8

[20] J.-C. Latombe, *Robot Motion Planning*. USA: Kluwer Academic Publishers, 1991.

[21] S. M. LaValle, *Planning Algorithms*. Cambridge, U.K.: Cambridge University Press, 2006, available at http://planning.cs.uiuc.edu/.

[22] S. M. Lavalle, J. J. Kuffner, and Jr., “Rapidly-exploring random trees: Progress and prospects,” in *Algorithmic and Computational Robotics: New Directions*, 2000, pp. 293–308.

[23] Q. Lin and J. W. Burdick, “Objective and frame-invariant kinematic metric functions for rigid bodies,” *J. of Robotics Research*, vol. 19, pp. 612–625, 1997.

[24] Lozano-Perez, “Spatial planning: A configuration space approach,” *IEEE Transactions on Computers*, vol. C-32, no. 2, pp. 108–120, Feb. 1983. [Online]. Available: https://doi.org/10.1109/tc.1983.1676196

[25] Y. Mao, B. Acikmese, P.-L. Garoche, and A. Chapoutot, “Successive Convexification for Optimal Control with Signal Temporal Logic Specifications,” in *25th ACM International Conference on Hybrid Systems: Computation and Control (HSCC '22)*, ser. HSCC '22: 25th ACM International Conference on Hybrid Systems: Computation and Control. Milan, Italy: ACM, May 2022. [Online]. Available: https://hal-enac.archives-ouvertes.fr/hal-03663984

[26] S. Markvorsen, “A Finsler geodesic spray paradigm for wildfire spread modelling,” *Nonlinear Anal. Real World Appl.*, vol. 28, pp. 208–228, 2016.

[27] J. Milnor, *Topology from the Differentiable Viewpoint*. University Press of Virginia, 1965.

[28] A. Orthey and M. Toussaint, “Rapidly-exploring quotient-space trees: Motion planning using sequential simplifications,” in *Robotics Research*, T. Asfour, E. Yoshida, J. Park, H. Christensen, and O. Khatib, Eds. Cham: Springer International Publishing, 2022, pp. 52–68.

[29] M. Peternell, H. Pottmann, T. Steiner, and H. Zhao, “Swept Volumes,” *Comput. Aided. Des. Appl.*, vol. 2, no. 5, pp. 599–608, 2005.

[30] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential convex optimization and convex collision checking,” *Int. J. Rob. Res.*, vol. 33, no. 9, pp. 1251–1270, 2014.

[31] S. Sellán, N. Aigerman, and A. Jacobson, “Swept volumes via space-time numerical continuation,” *ACM Trans. Graph.*, vol. 40, no. 4, pp. 1–11, 2021.

[32] E. Trélat and E. Zuazua, “The turnpike property in finite-dimensional nonlinear optimal control,” *J. Differ. Equ.*, vol. 258, no. 1, pp. 81–114, 2015.

[33] S. Voronin, G. Ozkaya, and D. Yoshida, “Convolution based smooth approximations to the absolute value function with application to non-smooth regularization,” 2014. [Online]. Available: https://arxiv.org/abs/1408.6795

[34] S. J. Wright, *Primal-Dual Interior-Point Methods*. Society for Industrial and Applied Mathematics, 1997. [Online]. Available: https://epubs.siam.org/doi/abs/10.1137/1.9781611971453

[35] T. Yu, X. Zhang, W. Liu, Y. Dai, and Q. Li, “Solution to tool swept volume of multi-axis milling simulation,” in *Proceedings of the 2013 the International Conference on Education Technology and Information Systems*. Atlantis Press, 2013. [Online]. Available: https://doi.org/10.2991/icetis-13.2013.147

[36] M. Žefran, V. Kumar, and C. B. Croke, “On the generation of smooth three-dimensional rigid body motions,” *IEEE Trans. Robot. Autom.*, vol. 14, no. 4, pp. 576–589, 1998.

[37] L. Zhang, Y. J. Kim, and D. Manocha, “C-DIST,” in *Proceedings of the 2007 ACM symposium on Solid and physical modeling - SPM '07*. ACM Press, 2007. [Online]. Available: https://doi.org/10.1145/1236246.1236270