# Geodesic turnpikes for robot motion planning

Yann de Mont-Marin, Martial Hebert, Jean Ponce

► To cite this version:

Yann de Mont-Marin, Martial Hebert, Jean Ponce. Geodesic turnpikes for robot motion planning. 2024. [https://arxiv.org/abs/2405.04522](https://arxiv.org/abs/2405.04522)

HAL Id: hal-04522485

https://inria.hal.science/hal-04522485v1

Preprint submitted on 26 Mar 2024 (v1), last revised 27 Jun 2024 (v2)

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.

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

HAL
open science

# Geodesic turnpikes for robot motion planning

Yann de Mont-Marin, Martial Hebert, Jean Ponce

► To cite this version:

Yann de Mont-Marin, Martial Hebert, Jean Ponce. Geodesic turnpikes for robot motion planning. WAFR 2024 - Workshop on the Algorithmic Foundations of Robotics, Oct 2024, Chicago (IL), United States. hal-04522485v3

HAL Id: hal-04522485

https://inria.hal.science/hal-04522485v3

Submitted on 20 Jan 2025

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.

Public Domain
# Geodesic turnpikes for robot motion planning

Yann de Mont-Marin*†, Martial Hebert‡ and Jean Ponce†§

*Inria Paris, France

†Departement d'Informatique de l'ENS (CNRS, PSL Research University), France

‡Robotics Institute, School of Computer Science, Carnegie Mellon University, United States

§Courant Institut and Center for Data Science, New York University, United States

Corresponding mail: yann.de-mont-marin@inria.fr

***Abstract***—Endowing the configuration space of a robot with an appropriate metric structure and characterizing and computing the corresponding geodesics are central issues in motion planning. As recently observed in [1], the geodesics of SE(2) equipped with the so-called minimum swept-volume distance exhibit in practice a behavior akin to the *turnpike property* in optimal control, with transient phases separated by a longer steady state close to prototypical trajectories, the *turnpikes* [2]. This presentation gives a theoretical counterpoint to this empirical observation with a formal definition of geodesic turnpikes using vector fields on Finsler manifolds, a simple differential characterization of geodesics in the case where the manifold is a Lie group and the Finsler distance is left-invariant, and, in the case where the corresponding operator is also reversible, a conjecture characterizing the turnpikes by vector fields satisfying simple conditions in the corresponding Lie algebras. As a proof of concept, closed-form (resp. numerical) procedures for computing these vector fields according to this conjecture are given for SE(2) equipped with the left-invariant Riemannian (resp. minimum swept-volume) distance introduced in [3] (resp. [1]) for rectangular shapes. The solutions empirically match, in both cases, the observed turnpike behavior of the corresponding geodesics. In the minimum swept-volume distance case, using the turnpikes for initialization also yields an order-of-magnitude speedup in computing geodesics.

## I. INTRODUCTION

### A. Context

The **configuration space** of a robot and the corresponding **forward kinematics** can be represented in many different ways, notably in terms of the associated **Lie group** and **Lie algebra** [4]. The choice of the **distance** function used to endow this space with a **metric structure** as well as the nature of the corresponding **geodesics** —that is, roughly speaking, the locally or globally shortest paths in that space— play a key role in planning the best possible motions in tasks such as grasping and manipulation or navigation in the presence of obstacles [5, 6, 7]. Conversely, this choice may be guided by various task heuristics, such as minimizing the chance of collision along a path [1, 8] or end-effector behavior [9].

**Riemannian geometry** and its **Finslerian generalization** provide a natural framework for constructing well-defined distances satisfying these heuristics and characterizing their geodesics. For example, Jaquier et al. [10, 11, 12] propose Riemannian geometry as a unifying framework for robot motion learning and control, Ratliff et al. [9, 13, 14] use Finsler geometry to model motion policies called “geometric fabrics”. The calculation of geodesics is computationally

<figure><img src="image_1.png" /><figcaption>Fig. 1. Geodesics on SE(2) for a rotating and translating rectangle under (top) the minimum swept-volume distance proposed in [1] and (bottom) [3, Fig. 3(c)] a left-invariant Riemannian distance proposed in [3]. Reprinted with permission from [1, 3]. Copyright © 1998, 2023, IEEE. In both cases, the behavior of geodesics hints at a turnpike phenomenon.</figcaption></figure>

challenging. Hence, simple metric structures, for which closed forms are available, are employed today, although they offer limited possibilities for integrating heuristics. To use distances that integrate heuristics, we need to leverage properties that accelerate the computation of geodesics.

Figure 1 shows the geodesics joining two configurations of a rectangle rotating and translating in the plane associated with two distances over SE(2), the Finslerian minimum swept-volume distance proposed in de Mont-Marin et al. [1] (Fig. 1(top)), and, a Riemannian distance introduced in Žefran et al. [3] (Fig. 1(bottom)). In both cases, the motion along the trajectory is (essentially), except near its endpoints, a pure translation parallel to the long side of the rectangle in the Finslerian case and parallel to the short side in the
# Geodesic turnpikes for robot motion planning

Yann de Mont-Marin<sup>1,2</sup>, Martial Hebert<sup>3</sup>, and Jean Ponce<sup>2,4</sup>

<sup>1</sup> Inria Paris, France

<sup>2</sup> Departement d'Informatique de l'ENS (CNRS, PSL Research University), France

<sup>3</sup> School of Computer Science, Carnegie Mellon University, United States

<sup>4</sup> Courant Institute and Center for Data Science, New York University, United States

**Abstract.** Endowing the configuration space of a robot with an appropriate metric structure and characterizing and computing the corresponding geodesics are central issues in motion planning. As recently observed in [23], the geodesics of SE(2) equipped with the so called minimum swept-volume distance exhibit in practice a behavior akin to the *turnpike property* in optimal control, with transient phases separated by a longer steady state close to prototypical trajectories, the *turnpikes* [32]. This presentation gives a theoretical counterpoint to this empirical observation with a formal definition of geodesic turnpikes using vector fields on Finsler manifolds, a simple differential characterization of geodesics in the case where the manifold is a Lie group and the Finsler distance is left-invariant, and, in the case where the corresponding operator is also reversible, a conjecture characterizing the turnpikes by vector fields satisfying simple conditions in the corresponding Lie algebras. As a proof of concept, closed-form (resp. numerical) procedures for computing the vector fields predicted by this conjecture are given for SE(2) equipped with the left-invariant Riemannian (resp. minimum swept-volume) distance introduced in [35] (resp. [23]) for rectangular shapes. The solutions empirically match, in both cases, the observed turnpike behavior of the corresponding geodesics. In the minimum swept-volume distance case, using the turnpikes for initialization also yields an order of magnitude speedup in computing geodesics.

**Keywords:** Turnpikes · Geodesics · Motion planning · Control Theory and Optimization

# 1 Introduction

## 1.1 Context

The configuration space of a robot and the corresponding forward kinematics can be represented in many different ways, notably in terms of the associated Lie group and Lie algebra [7]. The choice of the distance function used to endow this space with a metric structure, as well as the nature of the corresponding geodesics – roughly speaking, the locally or globally shortest paths in that space– play a key role in planning the best possible motions in tasks such as grasping, manipulation and navigation in the presence of obstacles [19,21,26]. Conversely,
Riemannian one. This behavior, where geodesics appear to stay close to simpler prototypical trajectories, except for short transient phases, is reminiscent of the **turnpike phenomenon** in optimal control [2, 15, 16]. Indeed, it can be shown that, for certain optimal control problems with a sufficiently large time horizon, any optimal trajectory remains, for most of the time, close to a **turnpike**, that is, the optimal solution of an associated stationary optimal control problem [2, 16]. In this setting, any optimal trajectory can be decomposed into three segments: it first converges exponentially fast from its start point to the turnpike, then spends most of its time there, and finally converges exponentially fast to its endpoint. Trélat and Zuazua demonstrate in [2, 17] that turnpikes can be used to initialize optimal control problems and significantly accelerate their computation.

To the best of our knowledge, the apparent turnpike behavior in Fig. 1 has not been formalized for Riemannian and Finslerian distance in the literature. Nor has it been exploited in the efficient computation of geodesics. We introduce in this paper a framework for **geodesic turnpikes**. It involves identifying a vector field, called the geodesic turnpike, and showing that geodesic velocities are close to that field for most of the geodesic length.

In section II, we clarify the mathematical background of distance, metric structure, geodesics, and Finsler geometry, and in section III, following a similar construction done in [16] for the optimal control setting, we introduce the framework for geodesic turnpike of a Finsler distance. Subsequently, in Proposition 6, we propose a new Lie algebra formulation of the geodesic equation for left-invariant Finsler distances on Lie groups, and we leverage this formulation with classical tools from dynamical systems [18] to propose a conjecture on the conditions for a left-invariant vector field to be a geodesic turnpike for a left-invariant Finslerian distance. In section IV-B, we give a proof of concept of the conjecture usage to find the vector fields for the examples illustrated in Fig. 1: analytically for the Riemannian distance on SE(2) proposed in [3] (Fig. 1(bottom)) and numerically for the minimum swept-volume distance proposed in [1] (Fig. 1(top)). In both cases, the vector fields match the turnpike behavior observed. For the minimum swept-volume distance, we use the vector fields to initialize a geodesic solver and observe an order of magnitude speed-up.

Our main contributions are:

1) A formal definition of geodesics turnpikes on Finsler manifolds.

2) A geodesic equation for left-invariant Finsler distances on Lie groups.

3) A conjecture for a sufficient condition for a left-invariant vector field to be a geodesic turnpike for a left-invariant Finsler distance.

4) A proof of concept of geodesic computation acceleration using geodesic turnpikes for initialization.

## B. Related work

**Metric structures on configuration space.** Many non-canonical metric structures on the configuration space have been extensively explored. For instance, in [19], Zhang et al. study the maximum vertex distance, previously proposed by Lavalle [6]. In [1], the authors construct the minimum swept-volume distance that reduces collision probabilities along geodesics. An advantage of the latter is its compatibility with the Finslerian formalism, which allows it to benefit from the treatment proposed in this paper.

Žefran et al. provide in [20, 3] a comprehensive examination of Riemannian geometries on the configuration space of a solid, establishing properties necessary for a geometric structure to adhere to fundamental principles of mechanics. However, their study is confined to the case of a configuration space isomorphic to SE(3) and lacks insights for practical implementations.

**Turnpikes.** Turnpike properties have a long-standing history in finite-dimensional optimal control problems within economics [15]. Trélat and Zuazua conducted in-depth investigations, introducing exponential turnpikes that provide a strong bound on the proximity between a trajectory and its turnpike [2]. Sakamoto and Zuazua [16] reformulated essential turnpike properties in optimal control using geometric and dynamical system tools, significantly influencing our current work. Faulwasser et al. introduced the concept of a velocity turnpike in [21], where the turnpike is characterized not as a position but by the steadiness of the velocity. However, this notion needs to be revised for characterizing a turnpike behavior for geodesics, given the inherent invariance in the problem as discussed in section II. Turnpikes in robotics are closely related to the trim primitives studied in [22].

# II. GEOMETRY BACKGROUND

## A. Metric spaces

Consider some metric space $(X, d)$ defined by a set $X$ and a distance function $d: X^2 \to \mathbb{R}^+$. Let $I = [a, b]$ denote some interval of the real line and, given some positive integer $N$, let $\sigma_N(I)$ denote the set of all sequences $t = \{t_0, t_1, \dots, t_N\}$ of size $N+1$ such that $a = t_0 < t_1 < \dots < t_N = b$. Given a continuous parametric curve $\gamma: I \to X$, we define its length $L(\gamma)$ as

$$L(\gamma) = \sup_{N \in \mathbb{N}^*, t \in \sigma_N(I)} \sum_{i=0}^{N-1} d(\gamma(t_{i+1}), \gamma(t_i)). \quad (1)$$

We say that $\gamma$ is a rectifiable parametric curve when $L(\gamma)$ is finite.

Given a parametric curve $\gamma: I \to X$ and some interval $J$, a continuous and monotone map $\phi: I \to J$, $\gamma \circ \phi$ is called a *reparameterization* of $\gamma$. The image of a parametric curve, $\gamma(I) \subset X$, is invariant to parameterization: $\gamma(I) = \gamma \circ \phi(J)$, and the length of a curve $L(\gamma)$ is also invariant to parameterization: $L(\gamma) = L(\gamma \circ \phi)$. The set of all reparameterizations of a parametric curve forms an equivalence class, the *geometric curve*, with a well-defined length. We say that a parametric curve *represents* the corresponding geometric curve. From
<figure><img src="image_2.png" /><figcaption>**Fig. 1.** Geodesics on SE(2) for a rotating and translating rectangle under (**left**) [35, Fig. 3(c)] a left-invariant Riemannian distance proposed in [35] and (**right**) the minimum swept-volume distance proposed in [23]. In both cases, the behavior of geodesics hints at a turnpike phenomenon. Reprinted with permission from [23,35]. Copyright © 1998, 2023, IEEE.</figcaption></figure>

this choice may be guided by various task heuristics, such as minimizing the chance of collision along a path [23,25] or achieving some desired end-effector behavior [29].

Riemannian geometry and its Finslerian generalization provide a natural framework for constructing well-defined distances satisfying these heuristics and characterizing their geodesics. For example, Jaquier et al. [13,14,17] propose Riemannian geometry as a unifying framework for robot motion learning and control, and Ratliff et al. [28,29,33] use Finsler geometry to model motion policies called “geometric fabrics”. The construction of geodesics is computationally challenging; thus, simple metric structures, for which closed forms are available, are commonly employed today despite their limited capacity for integrating heuristics. To go further, we need to leverage whatever favorable properties of specific distance are available to accelerate the computation of geodesics.

Figure 1 shows the geodesics joining two configurations of a rectangle rotating and translating in the plane associated with two distances over SE(2), a Riemannian distance introduced in Žefran et al. [35] (Fig. 1, left), and, the Finslerian minimum swept-volume distance proposed in de Mont-Marin et al. [23] (Fig. 1, right). In both cases, the motion along the trajectory is essentially, except near its endpoints, a pure translation parallel to the short side of the rectangle in the Riemannian case and to its long side in the Finslerian one. This behavior, where geodesics appear to stay close to simpler prototypical trajectories, except for short transient phases, is reminiscent of the turnpike phenomenon in optimal control [22,30,32]. Indeed, it can be shown that, for certain optimal control problems with a sufficiently large time horizon, any optimal trajectory remains, for most of the time, close to a turnpike, that is, the optimal solution of an associated stationary optimal control problem [30,32]. In this setting, optimal trajectories can be decomposed into three segments: converging exponentially fast from the start point to the turnpike, spending most of their time there, and finally converging exponentially fast to the endpoint. Trélat and Zuazua have also shown in [11,32] that turnpikes can be used to initialize optimal control problems and significantly accelerate their computation.

To the best of our knowledge, the apparent turnpike behavior in Fig. 1 has not been formalized yet for Riemannian and Finslerian distances in the literature, nor has it been exploited in the efficient computation of geodesics. We
now on, we assume that all parametric curves of interest are defined over $I = [0, 1]$, thanks to the length invariance to reparameterization.

We say that a rectifiable parametric curve $\gamma$ represents a geodesic when for any $t < t'$ in $I$ we have

$$L(\gamma|_{[t,t']}) = d(\gamma(t), \gamma(t')). \quad (2)$$

A geodesic is a geometric curve for which length and distance coincide on every curve segment. A parametric curve $\gamma$ represents a local geodesic if, for any $t$ in $I$ there exists some $\epsilon > 0$ such that $\gamma|_{[t-\epsilon, t+\epsilon]}$ represents a geodesic. Every geodesic is a local geodesic. For a geodesic (resp. local geodesic), there exists a unique parametric curve $\gamma: [0, 1] \to X$ representing the geodesic (resp. local geodesic) with constant velocity, i.e., for every $t$ in $[0, 1]$ we have

$$L(\gamma|_{[0,t]}) = td(\gamma(0), \gamma(1)), \quad (3)$$

which provides a canonical representative of the geodesic.

We say that $(X, d)$ is a geodesic space if, for any pair of points $x, y$, a geodesic joining those points exists. Let $C^0(x, y)$ denote the set of all continuous parametric curves joining $x$ and $y$; the so-called Hopf-Rinow theorem gives the conditions on the existence of geodesic spaces.

**Theorem 1** (Hopf-Rinow theorem). Let $(X, d)$ denote a complete and locally compact metric space such that for any pair of points $x, y$, there exists a rectifiable curve joining those points. The inner distance, denoted by $d_i$, is defined as

$$d_i(x, y) = \inf_{\gamma \in C^0(x, y)} L(\gamma), \quad (4)$$

is well defined and $(X, d_i)$ is a geodesic space. The lengths defined by $d$ and $d_i$ are equal.

The inner-distance construction (4) shows how to construct a geodesic space from a metric space. It is, for example, the construction of the minimum swept-volume distance presented in [1]. We assume from now on that $(X, d)$ is a geodesic space with $d_i = d$ as configuration space distances in robotics usually satisfy the hypothesis of Theorem 1.

If in addition $X$ is a $C^\infty$ manifold and $d$ is $C^\infty$, we define the Finsler operator $F$ by, for any $x$ in $X$ and $v$ in $T_x X$ the tangent space to $X$ in $x$:

$$F(x, v) = \lim_{\epsilon \to 0^+} \frac{1}{\epsilon} d(\gamma(0), \gamma(\epsilon)) = \lim_{\epsilon \to 0^+} \frac{1}{\epsilon} L(\gamma|_{[0, \epsilon]}), \quad (5)$$

with any $C^\infty$ parametric curve $\gamma: [0, 1] \to X$ such that $\gamma(0) = x$ and $\dot{\gamma}(0) = v$. The construction is independent of the choice of $\gamma$, and we say that $(X, F)$ is a Finsler manifold. We refer the reader to Bucataru [23] for an extended introduction to Finsler geometry. In particular, $F$ is $C^\infty$ and for any $x$ in $X$, the map $v \in T_x X \to F(x, v)$ is a norm on $T_x X$. We assume from now on that all parametric curves of interest are $C^\infty$.

## B. Finsler geodesics

Let us assume from now on that $(X, F)$ is a $C^\infty$ Finsler manifold of dimension $n$ with the associated distance $d$. As shown in [23] for example, the length of a parametric curve $\gamma$ defined on $[0, 1]$ with endpoints, $x_0 = \gamma(0)$ and $x_1 = \gamma(1)$ verifies

$$L(\gamma) = \int_{0}^{1} F(\gamma(t), \dot{\gamma}(t))dt. \quad (6)$$

**Property 2** ([23]). The global (resp. local) minimizers of $L$ among all parametric curves joining $x_0$ to $x_1$ represent geodesics (resp. local geodesics) joining these two points.

It is also convenient to define the energy function associated with the parametric curve $\gamma$ as

$$E(\gamma) = \frac{1}{2} \int_{0}^{1} F^{2}(\gamma(t), \dot{\gamma}(t))dt. \quad (7)$$

**Property 3** ([23]). The global (resp. local) minimizers of $E$ among all parametric curves joining $x_0$ to $x_1$ represent the local geodesics (resp. geodesics) between these two points with constant velocity. A geodesic joining $x_0$ to $x_1$ has one and only one representative that is also a minimizer of $E$. The minimizers of $E$ are canonical representatives for the geodesics joining $x_0$ to $x_1$. A minimizer of $E$ is also a minimizer of $L$, and the converse is true if and only if the parametric curve has constant velocity.

A minimizer $\gamma^*$ of $E$ joining $x_0$ to $x_1$ is also a minimizer of $L$ and has constant velocity. Thus, there exists $C$ such that $F(\gamma^*(t), \dot{\gamma}^*(t)) = C$ for any $t$ in $[0, 1]$. Substituting $C$ in both (6) and (7) yields

$$2E(\gamma^*) = L(\gamma^*)^2 = d(x_0, x_1)^2. \quad (8)$$

In addition, with the re parametrization $\phi: [0, d(x_0, x_1)] \to [0, 1]$ such that $\phi(t) = \frac{t}{d(x_0, x_1)}$, we denote by $\dot{\gamma}^* = \gamma^* \circ \phi$, defined on $[0, d(x_0, x_1)]$, the unique curvilinear parametric curve representing the geodesic. For all $t$ in $[0, d(x_0, x_1)]$ we have

$$F(\gamma^*(t), \dot{\gamma}^*(t)) = 1. \quad (9)$$

In the rest of the paper, $\gamma^*$ is always defined on $[0, 1]$ and is a minimizer of the energy $E$, and $\dot{\gamma}^*$ is always a curvilinear parametrization of $\gamma^*$.

Using the minimizers of $E$ provides a unique representation of the geodesics. Perhaps more importantly, $E$ can be used to characterize the geodesics by a differential equation. For this purpose, we introduce the fundamental bilinear form associated with the Finsler operator $F$. For any $(x, v)$ in $TX$ we define the bilinear form $g(x, v)$ on $T_x X$ by, for any $a$ and $b$ in $T_x X$:

$$g(x, v)(a, b) = \frac{1}{2} \frac{\partial^2}{\partial s \partial t} F^2(x, v + sa + tb) \Big|_{s=0, t=0} . \quad (10)$$

Using the Euler-Lagrange equation from the calculus of variation, we obtain a new characterization of the geodesics.
give in Section 2 the mathematical background for our presentation. Following a construction similar to the one used in the optimal control setting [30], we introduce in Section 3 a framework for characterizing geodesic turnpikes as vector fields such that geodesic derivative remain close to these fields. We also propose a new Lie algebra formulation of the geodesic equation for left-invariant Finsler distances on Lie groups associated with robotic configuration spaces and combine this formulation with classical tools from dynamical systems [18] to propose a conjecture on the conditions for a left-invariant vector field to be a geodesic turnpike for a left-invariant Finslerian distance. Assuming it is true, we exploit this conjecture in Section 4 to find the vector fields for the examples illustrated in Fig. 1, analytically for the Riemannian distance on SE(2) proposed in [35] (Fig. 1, left), and numerically for the minimum swept-volume distance proposed in [23] (Fig. 1, right). In both cases, the vector fields match the turnpike behavior observed. We then use the corresponding turnpikes to initialize a geodesic solver and observe an order of magnitude speed-up for the minimum swept-volume distance.

Our main contributions are (1) a formal definition of geodesic turnpikes on Finsler manifolds (2) a geodesic equation for left-invariant Finsler distances on Lie groups (3) a conjecture providing a sufficient condition for a left-invariant vector field to be a geodesic turnpike (4) examples supporting the conjecture validity by predicting the empirical turnpike behavior of geodesics observed for two robotic related distance functions and (5) a proof of concept for accelerating geodesic computation using turnpikes for initialization.

## 1.2 Related work

**Metric structures on configuration spaces** have been explored extensively. For instance, Zhang et al. study the maximum vertex distance [36], previously proposed by Lavalle [21]. de Mont-Marin et al. construct the minimum swept-volume distance that reduces collision probabilities along geodesics [23]; and Žefran et al. provide a comprehensive examination of Riemannian geometries on the configuration space of a solid [34,35], establishing properties necessary for a geometric structure to adhere to fundamental principles of mechanics.

**Turnpikes** have a long-standing history in finite-dimensional optimal control within econometrics [22]. Trélat and Zuazua introduce exponential turnpikes [32], providing a strong bound on the proximity between a trajectory and its turnpike and Sakamoto and Zuazua reformulated essential turnpike properties using geometric and dynamical system tools in [30], significantly influencing our current work. Faulwasser et al. introduce the concept of a velocity turnpike in [8], characterizing the turnpike not as a constant position but as a constant velocity. However, this notion needs revision for characterizing the turnpike behavior of geodesics for reasons related to the inherent invariance of the solutions discussed in Sec. 2. Finally, turnpikes in robotics are closely related to the trim primitives studied in [10].
**Property 4** (Geodesic equation [23]). The differential equation is written in local coordinates using Einsteinian notation for $l$ in $1..n$ as

$$g_{il} \dot{\gamma}^i + \frac{1}{2} \left( \frac{\partial g_{lj}}{\partial x^j} + \frac{\partial g_{lk}}{\partial x^k} - \frac{\partial g_{jk}}{\partial x^l} \right) \dot{\gamma}^j \dot{\gamma}^k = 0, \quad (11)$$

where the dependency in $\gamma$ and $\dot{\gamma}$ of $g$ is left implicit for conciseness.

A necessary and sufficient condition for a $C^\infty$ parametric curve $\gamma$ to have constant velocity and represent a local geodesic is to be a solution of the geodesic equation.

A minimizer of $E \gamma^* : [0,1] \to X$ joining $x_0$ to $x_1$ and its curvilinear parameterization $\dot{\gamma}^\dagger : [0, d(x_0, x_1)] \to X$ represent the same geodesic and have constant velocity, hence are solutions of the geodesic equation.

The bilinear form $g$ is a cornerstone in the study of local geodesics and enjoys several properties inherited from the homogeneity of $F$. In particular, for $(x, v)$ in $TX$, we have:

* $F^2(x, v) = g(x, v)(v, v)$.

* For any $\lambda > 0$, $g(x, \lambda v) = g(x, v)$.

* For any $w$ in $T_x X$, $\frac{1}{2} \frac{\partial}{\partial s} F^2(x, v + sw) \Big|_{s=0} = g(x, v)(v, w)$.

Part of these results are given in [23], however for completeness of the paper, proofs of the results crucial for Proposition 6 of the next section are presented in Appendix A.

### III. LIE GROUPS AND GEODESIC TURNPIKES

#### A. Motivation for robotic applications

In robotics, the configuration space is often a Lie Group. For example, for a free-flying robot, the configuration space is $X = SE(3)$ the group of rigid placements, and for a serial manipulator arm with six joints, it is $X = SO(2)^6$. Those spaces are always equipped with a smooth Finsler operator. For example, $SE(3)$ is generally equipped with the left-invariant canonical Riemannian distance [24] for which the geodesics have a closed form. For more complex distances, such as the minimum swept-volume distance in [1], the geodesics are not available in closed form and must be computed efficiently. We need to solve the problem

$$\min_{\gamma \in C^\infty([0,1], X)} \frac{1}{2} \int_0^1 F^2(\gamma(t), \dot{\gamma}(t)) dt, \quad (12)$$

$$\text{s.t.} \quad \gamma(0) = x_0 \quad \text{and} \quad \gamma(1) = x_1,$$

for any $x_0$ and $x_1$ in $X$.

On the one hand, one can solve a discretized version of (12) using off-the-shelf minimization software such as [25, 26]. On the other hand, one can solve (12) using a bounded value ODE solver, such as [27] with the induced geodesic equation (11) and boundary conditions $\gamma(0) = x_0$ and $\gamma(1) = x_1$. In both cases, however, the initialization has a crucial impact on the performance of the algorithms because the underlying numerical minimization problems are not necessarily convex.

The geodesics of [1, 3], illustrated in Fig. 1, suggest a turnpike behavior. To the best of our knowledge, this behavior has not been formalized nor used to improve the computation

<figure><img src="image_2.png" /><figcaption>Fig. 2. Illustration of geodesic turnpikes. In blue, we plot two Finslerian geodesics of the minimum swept-volume distance [1], joining $x_0^1$ with $x_0^1$ and $x_0^b$ with $x_0^b$. In red, we plot three integral curves of $V$, the vector field of instantaneous translations on the small side illustrated by the black arrow. We observe that for each geodesic, there exists an integral curve of $V$ that approximates the geodesic well except at its start and end points.</figcaption></figure>

of solutions of (12). We propose formalizing this behavior in terms of geodesic turnpike using vector fields. We can initialize geodesic solvers efficiently using these vector fields as presented in section IV-B.

#### B. Geodesic turnpike definition

Let $(X, F)$ denote a Finsler manifold, and $d$ denote the associated distance. Let $V$ denote a smooth vector field on $X$. Qualitatively, $V$ is a geodesic turnpike for $F$ on $X$ if every curvilinear parametrization $\gamma^\dagger$ of a geodesic has its derivative $\dot{\gamma}^\dagger$ "close" to $V$. Figure 2 illustrates the idea, with $X = SE(2)$, $F$ the operator for the minimum swept-volume distance [1] and $V$ the vector field of instantaneous translation on the small side, illustrated with a black arrow. We observe that for each geodesic, there exists an integral curve of $V$ that approximates the geodesic well except near its start and end points.

To formalize the notion of "close", we propose a definition of exponential geodesic turnpike inspired by exponential turnpikes in the context of optimal control [2].

**Definition 1** (Exponential geodesic turnpike). We say that a smooth vector field $V$ is an exponential geodesic turnpike for $F$ on $X$ if there exist constants $K > 0$ and $\sigma > 0$ such that for any $x_0, x_1$ in $X$ and any curvilinear parametrization $\gamma^\dagger : [0, d(x_0, x_1)] \to X$ of a geodesic joining $x_0$ and $x_1$ we have for all $t$ in $[0, d(x_0, x_1)]$:

$$F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \le K (e^{-\sigma t} + e^{-\sigma(d(x_0, x_1) - t)}). \quad (13)$$

The left-hand side of Eq. (13) is the distance between $\gamma^\dagger(t)$ and $V_{\gamma^\dagger(t)}$ because the mapping $v \mapsto F(x, v)$ is a norm on $T_x X$. The right-hand side of Eq. (13) is an exponential upper bound. The bound is loose for $t$ near 0 due to the first term of the right-hand side and for $t$ near $d(x_0, x_1)$ due to the second term of the right-hand side. The bound tightens exponentially when $t$ is far from the boundaries of $[0, d(x_0, x_1)]$. Figure 3 illustrates this behavior for a minimum swept-volume distance geodesic [1]. In the proposed formalism, the turnpike is a vector field, and the bound is in the space of velocities. In this sense, it resembles the velocity turnpikes [21]. In addition, the bound on the velocity $\dot{\gamma}^\dagger$ induces a bound on $\gamma^\dagger(t)$ by integration.
# 2 Background

## 2.1 Metric spaces

Consider some metric space $(X, d)$ defined by a set $X$ and a distance function $d: X^2 \to \mathbb{R}^+$. Let $I = [a, b]$ denote some interval of the real line and, given some positive integer $N$, let $s_N(I)$ denote the set of all sequences $t = \{t_0, t_1, \dots, t_N\}$ of size $N+1$ such that $a = t_0 < t_1 < \dots < t_N = b$. Given a continuous parametric curve $\gamma: I \to X$, we define its length $L(\gamma)$ as $[3]$

$$L(\gamma) = \sup_{N \in \mathbb{N}^*, t \in s_N(I)} \sum_{i=0}^{N-1} d(\gamma(t_{i+1}), \gamma(t_i)). \quad (1)$$

We say that $\gamma$ is a rectifiable parametric curve when $L(\gamma)$ is finite. Given a parametric curve $\gamma: I \to X$, some interval $J$ and a continuous and monotone map $\phi: I \to J$, $\gamma \circ \phi$ is called a reparameterization of $\gamma$. The image of a parametric curve is invariant under parameterization, that is, $\gamma(I) = (\gamma \circ \phi)(J) \subset X$, and the length of a curve $L(\gamma)$ is also invariant under parameterization, $L(\gamma) = L(\gamma \circ \phi)$. The set of all reparameterizations of a parametric curve forms an equivalence class, the associated geometric curve, with a well-defined length. We say that a parametric curve represents the corresponding geometric curve. We say that a rectifiable parametric curve $\gamma$ represents a geodesic when, for any $t < t'$ in $I$ we have

$$L(\gamma|_{[t,t']}) = d(\gamma(t), \gamma(t')). \quad (2)$$

A geodesic is a geometric curve for which length and distance coincide on every curve segment. A parametric curve $\gamma$ represents a local geodesic if, for any $t$ in $I$ there exists some $\epsilon > 0$ such that $\gamma|_{[t-\epsilon, t+\epsilon]}$ represents a geodesic. Every geodesic is a local geodesic. There exists a unique parametric curve $\gamma: [0, 1] \to X$ with constant velocity representing any geodesic, such that for every $t$ in $[0, 1]$ we have

$$L(\gamma|_{[0,t]}) = td(\gamma(0), \gamma(1)), \quad (3)$$

which provides a canonical representative of the geodesic. We say that $(X, d)$ is a geodesic space if any two points $x, y$ in $X$ can be joined by a geodesic. Let $C^0(x, y)$ denote the set of all continuous parametric curves joining $x$ and $y$; the so-called Hopf-Rinow theorem provides a simple characterization of geodesic spaces

**Theorem 1 (Hopf-Rinow theorem).** Let $(X, d)$ denote a complete and locally compact metric space such that for any pair of points $x, y$, there exists a rectifiable curve joining those points. The inner distance $d_i$ defined by

$$d_i(x, y) = \inf_{\gamma \in C^0(x, y)} L(\gamma), \quad (4)$$

is well-defined and $(X, d_i)$ is a geodesic space and the lengths defined in (1) using $d$ or $d_i$ are equal.
The bound on $\gamma^\dagger(t)$ is not explored in the present framework and is left for future work.

It is important to note that $K$ and $\sigma$ do not depend on $x_1$ and $x_2$, and thus the upper bound in Definition 1 do not depend on $x_1$ and $x_2$, but only on the distance $d(x_0, x_1)$ between those points. It implies that with a given precision, the length of the geodesic segment for which the velocity bound (13) is looser than $\epsilon$ does not depend on the total geodesic length.

**Proposition 5.** For any $\epsilon > 0$, there exists $\tau > 0$ such that for any curvilinear parametric curve $\gamma^\dagger$ representing a geodesic, if $I_\epsilon$ denote the interval of $t$ such that $F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V\gamma^\dagger(t)) \ge \epsilon$ we have

$$L(\gamma^\dagger|_{I_\epsilon}) \le \tau. \quad (14)$$

The proof is given in Appendix B and Fig. 4 illustrates this proposition. Proposition 5 ensures that an integral curve of $V$ near $x_0$ and $x_1$ is a good approximation of $L$. Moreover, the larger $L$ is, the better the approximation is since $\tau/L \to 0$ as $L \to \infty$. In the example illustrated in Fig. 1, even configurations that appear "close" in the workspace have a "large" geodesic length from the point of view of the turnpike property.

### C. Left-invariant Finsler operators on Lie groups

We now study the case of invariant reversible Finsler distance on Lie groups to identify sufficient conditions for the geodesic turnpike property. From now on, $X$ is a Lie group.

Given some $h$ in $X$, we denote by $\tilde{L}_h$ the left tangent action (see the book of Chirikjian [28] for further details on Lie groups). For any $x$ in $X$, $\tilde{L}_h$ is a linear map between the tangent space $T_x X$ onto, the tangent space $T_{h \circ x} X$. We say that $F$ is left-invariant on $X$, if for every $(x, v)$ in $TX$ and $h$ in $X$,

$$F(x, v) = F(h \circ x, \tilde{L}_h v), \quad (15)$$

i.e., the Finsler operator does not change when we compose $x$ on the left with a group element $h$, and we compose $v$ with the associated tangent map. In particular, with $h = x^{-1}$, we have $F(x, v) = F(e, \tilde{L}_{x^{-1}} v)$ where $e$ denote the identity element of $X$. Let $C(u) = \frac{1}{2}F^2(e, u)$ denote the cost function used in (12) restricted on the Lie algebra $\mathcal{X}$ and let $u: [0, 1] \to \mathcal{X}$ such that $\dot{\gamma}(t) = \tilde{L}_{\gamma(t)}u(t)$, then (12) is equivalent to

$$\min_{\gamma \in \mathcal{C}^\infty([0,1], X), u \in \mathcal{C}^\infty([0,1], \mathcal{X})} \int_0^1 C(u(t))dt, \quad (16)$$

<figure><img src="image_3.png" /><figcaption>Fig. 3. Illustration of an exponential geodesic turnpike bound for a geodesic of the minimum swept-volume distance [1]. For visualization purposes, we attach a disc in orange to the rectangle in blue. The exponential bound in Definition 1 induces a maximum displacement of the orange disc along the geodesic. The union of the resulting varying radius discs in green is an upper bound of the workspace region guaranteed by the exponential bound for the attached disc.</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>Fig. 4. To illustrate Proposition 5, we plot the exponential bound (13) as a function of the curvilinear abscissa $t$ of geodesics of various lengths. Even for longer geodesics, the bound is upper-bounded by $\epsilon$ whenever $\tau/2 < t < L - \tau/2$. The first two curves (red and orange) correspond to geodesics with lengths lower than $\tau$.</figcaption></figure>

$$\begin{align*}
\text{s.t} \quad & \forall t \in [0, 1], \quad \dot{\gamma}(t) = \tilde{L}_{\gamma(t)}u(t), \\
& \gamma(0) = x_0, \\
& \gamma(1) = x_1.
\end{align*}$$

We now use the calculus of variations to obtain a more tractable geodesic equation associated with (16)

**Proposition 6.** If $F$ is a left-invariant Finsler distance on the Lie group $X$, then the geodesic equation associated to (16) can be written in some basis of $\mathcal{X}$:

$$\begin{align}
\dot{\gamma}(t) &= \tilde{L}_{\gamma(t)}u(t), \tag{17} \\
\dot{u}(t) &= G_{u(t)}^{-1} \mathrm{ad}_{u(t)}^{\top} G_{u(t)} u(t), \nonumber
\end{align}$$

where $G_u$ denote the matrix representation of $g(e, u)$ in the chosen basis and $\mathrm{ad}_u$ denote the matrix representation of the adjoint operator of $u$ on $\mathcal{X}$ in the chosen basis. The solutions represent local geodesics and do not depend on the basis choice.

The proof can be found in Appendix C. In the Riemannian case, $G_u$ is independent of $u$, and we recover the so-called Euler-Arnol'd equation [29]. We feel that Proposition 6 should be textbook material since it resembles classical results such as [30, 31] but, to the best of our knowledge, this geodesic equation for invariant Finsler distances has not been written in this form yet.

### D. Geodesic turnpikes for left-invariant Finsler distances

Let us now study the dynamics given by the geodesic equation (17) of the left-invariant Finsler operator $F$ on $X$. The objective is to construct a vector field $V$ that is an exponential geodesic turnpike for $F$ on $X$ as defined in Definition 1. Any curvilinear parametrization $\gamma^\dagger$ defined on $[0, L]$ and representing a geodesic is a solution of the geodesic equation (17). In other words, $u(t) = \tilde{L}_{\gamma^\dagger(t)} \dot{\gamma}^\dagger(t)$ is a solution of the dynamics $\dot{u} = D(u)$ where $D$ denote the dynamics function
The inner-distance construction (4) shows how to construct a geodesic space from a metric space satisfying certain assumptions. We assume from now on that $(X, d)$ is a geodesic space with $d_i = d$ since the configuration space distances used in robotics usually satisfy the hypotheses of Theorem 1.

If, in addition, $X$ is a $C^1$ manifold and $d$ is $C^1$, for any $C^1$ parametric curve $\gamma: [0, 1] \to X$ the quantity

$$ \lim_{\epsilon \to 0^+} \frac{1}{\epsilon} d(\gamma(0), \gamma(\epsilon)) = \lim_{\epsilon \to 0^+} \frac{1}{\epsilon} L(\gamma|_{[0,\epsilon]}), \quad (5) $$

depends only on $x = \gamma(0)$ and $v = \dot{\gamma}(0)$, and we denote this quantity by $F(x, v)$ and say that $(F, X)$ is a Finsler manifold. We refer the reader to Bucataru [4] for an extended introduction to Finsler geometry. For any $x$ in $X$, the map $v \in T_x X \to F(x, v)$ is a norm on $T_x X$ [4] the tangent to $X$ in $x$ and $F(x, v)$ can be interpreted as the velocity of a curve passing in $x$ with derivative $v$. In particular, a parametric curve $\gamma: [0, 1] \to X$ has constant velocity if there exists a scalar $C > 0$ such that for all $t$ in $[0, 1]$, $F(\gamma(t), \dot{\gamma}(t)) = C$.

## 2.2 Finsler geodesics

For simplicity, let us assume from now on that $(X, F)$ is a $C^\infty$ Finsler manifold of dimension $n$ with associated distance $d$. As shown in [4] the length of a parametric curve $\gamma$ defined on $[0, 1]$ with endpoints, $x_0 = \gamma(0)$ and $x_1 = \gamma(1)$ verifies

$$ L(\gamma) = \int_{0}^{1} F(\gamma(t), \dot{\gamma}(t))dt. \quad (6) $$

It is also convenient to define the energy function associated with the parametric curve $\gamma$ as

$$ E(\gamma) = \frac{1}{2} \int_{0}^{1} F^{2}(\gamma(t), \dot{\gamma}(t))dt. \quad (7) $$

For completeness of our presentation, we present some properties proven in [4].

**Property 1.** The global (resp. local) minimizers of $L$ among all parametric curves joining $x_0$ to $x_1$ represent geodesics (resp. local geodesics) joining these two points.

**Property 2.** The global (resp. local) minimizers of $E$ among all parametric curves joining $x_0$ to $x_1$ represent the local geodesics (resp. geodesics) between these two points with constant velocity. A geodesic joining $x_0$ to $x_1$ has one and only one representative that is also a minimizer of $E$. The minimizers of $E$ are canonical representatives for the geodesics joining $x_0$ to $x_1$. A minimizer of $E$ is also a minimizer of $L$, and the converse is true if and only if the parametric curve has constant velocity.
on the lie algebra $\mathcal{X}: D(u) = G_u^{-1} ad_u^\top G_u u$. The solutions of the geodesic equation have constant velocity and $\gamma^\dagger(t)$ is a curvilinear parametrization, so we have for any $t$ in $[0, L]$, $F(e, u(t)) = F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t)) = 1$. Thus, the dynamical system $\dot{u} = D(u)$ is well defined on $S_F(1)$ the set of all $u$ in $\mathcal{X}$ defined by $F(e, u) = 1$. We study the dynamics associated with $D$ on $S_F(1)$.

The lie algebra elements $\bar{u}$ in $S_F(1)$ verifying $D(\bar{u}) = 0$ are the equilibrium points of the dynamics on $S_F(1)$. Equivalently $\bar{u}$ satisfies

$$ad_{\bar{u}}^{\top} G_{\bar{u}} \bar{u} = 0, \quad \bar{u}^{\top} G_{\bar{u}} \bar{u} = 1, \qquad (18)$$

as detailed in Appendix D. The matrix $ad_{u_\top}^\top$ is a linear endomorphism of $\mathcal{X}$ and the mapping $u \mapsto ad_u^\top$ is linear in $u$. We denote by $P_u$ the unique matrix defined by

$$\forall v \in \mathcal{X}, \quad P_u v = ad_v^\top u. \qquad (19)$$

We prove in Appendix D that the Jacobian $J$ of the dynamics $D$ taken in an equilibrium point $\bar{u}$ is

$$J_{\bar{u}} = G_{\bar{u}}^{-1} (ad_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (20)$$

The dynamics around the equilibrium point are given by the spectrum of $J_{\bar{u}}$ (see [18]). As we study the dynamics on $S_F(1)$, we restrict the Jacobian to the tangent space $A = T_{\bar{u}}S_F(1)$ and we prove in Appendix D that $A$ is exactly the vector space of elements $v$ in $\mathcal{X}$ such that $v^\top G_{\bar{u}} \bar{u} = 0$.

**Definition 2** (Saddle point on the unit sphere). An equilibrium point $\bar{u}$ of our dynamics $D$ on $S_F(1)$ is called a saddle point if the eigenvalues of the endomorphism $J_{\bar{u}}|_A$ have non-null real parts and at least one eigenvalue has a positive real part, and one eigenvalue has a negative real part.

The theory of dynamical systems [18] gives a general characterization of saddle points. Sakamoto et al. [16] sums up many of those results, with particular attention to the existence of exponential bounds for a dynamical system around saddle points. In our settings, we say that a sub-manifold $M$ of $S_F(1)$ is an invariant sub-manifold if any solution of the dynamics $\dot{u} = D(u)$ with $u(0)$ in $M$, remains in $M$ for all time $t$. Let $\bar{u}$ denote a saddle point on the unit sphere and $m$ denote the dimension of $S_F(1)$. The theory of dynamical systems [18] states that there exists an invariant sub-manifold $U$ of $S_F(1)$ of dimension $k$ tangent to the eigenvectors of $J_{\bar{u}}|_A$ associated with negative eigenvalues called the *unstable manifold* and an invariant sub-manifold $S$ of $S_F(1)$ of dimension $m-k$ tangent to the eigenvectors of $J_{\bar{u}}|_A$ associated with positive eigenvalues called the *stable manifold*. Sakamoto et al. prove the following theorem

**Theorem 7** (Theorem 3 in [16]). For any $u_0$ in $S$, any $u_1$ in $U$, any $(m-k)$-dimensional disc $\bar{D}$ transversal to $S$ at $u_0$, and any $k$-dimensional disc $\bar{E}$ transversal to $U$ at $u_1$, there exist constants $T_0 > 0$, $K > 0$ and $\sigma > 0$ such that for every $T > T_0$, there exists a $\rho > 0$, $v_0$ in $B(u_0, \rho) \cap \bar{D}$ and $v_1$ in $B(u_1, \rho) \cap \bar{E}$ such that there exists a solution $u^*$ of the dynamics $D$ such

<figure><img src="image_5.png" /><figcaption>Fig. 5. Illustration of Theorem 7. We plot a portion of the surface $S_F(1)$ around a saddle point $\bar{u}$ of the Lie algebra dynamics $\dot{u} = D(u)$. We show an eigenvector $v_+$ associated with a positive eigenvalue tangent to the stable manifold $S$ and, in red, an eigenvector $v_-$ associated with a negative eigenvalue tangent to the unstable manifold $U$. We plot the transversal discs $\bar{D}$ and $\bar{E}$ near the stable and unstable manifolds. A solution $u^*$ of the dynamics with $u^*(0) = u_0$ in $\bar{D}$ and $u^*(T) = u_1$ in $\bar{E}$ is illustrated in black and verify conditions of Theorem 7. The distance $\|u^*(t) - \bar{u}\|$ is exponentially bounded, as in (21). We illustrate with dots $u^*(t_i)$ for $i = 0..9$ where the sequence $t_i = iT/9$ form a regular partition of $[0, T]$. The distribution of the samples $u^*(t_i)$ illustrates the behavior induced by the exponential bound: $u^*$ goes quickly toward the saddle point, slows down near it, and finishes fast toward the endpoint $u_1$.</figcaption></figure>

that $u^*(0) = v_0$ and $u^*(T) = v_1$ and for any $t$ in $[0, T]$:

$$\|u^*(t) - \bar{u}\| \le K (e^{-\sigma t} + e^{-\sigma(T-t)}), \qquad (21)$$

with $u \to \|u\|$ a Euclidean norm on $\mathcal{X}$. Moreover, $\rho \to 0$ when $T \to \infty$.

This theorem is illustrated by Fig. 5. We now suppose that the set of all possible initial (resp. final) conditions on $u_0$ (resp. $u_1$) induced by the geodesic problem (16) satisfies the hypothesis of Theorem 7. In that case, constants $K > 0$ and $\sigma > 0$ guarantee an exponential bound on $\|u^*(t) - \bar{u}\|$. We have yet to prove that these assumptions are always satisfied in the case of a geodesic problem, and we leave this proof for future work. Nonetheless, we propose the following conjecture,

**Conjecture 1.** Let $F$ denote a left-invariant reversible Finsler operator on the Lie group $X$. Let $\bar{u}$ in $\mathcal{X}$ verifying

$$ad_{\bar{u}}^{\top} G_{\bar{u}} \bar{u} = 0, \quad \bar{u}^{\top} G_{\bar{u}} \bar{u} = 1, \qquad (18)$$

Let $A$ denote the vector space of elements $v$ in $\mathcal{X}$ defined by $v^\top G_{\bar{u}} \bar{u} = 0$ and $J_{\bar{u}}$ denote the matrix

$$J_{\bar{u}} = G_{\bar{u}}^{-1} (ad_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (20)$$

If every eigenvalue of the endomorphism $J_{\bar{u}}|_A$ has a nonzero real part with at least one positive and one negative eigenvalue, then the left-invariant vector field $V_x = \bar{L}_x \bar{u}$ is an exponential geodesic turnpike for the Finsler distance associated with $F$ on $X$.

Indeed, if $\bar{u}$ is a saddle point as defined in Definition 2, and the assumptions regarding the validity of Theorem 7 discussed earlier are satisfied, then for any curvilinear parametrization
A minimizer $\gamma^*$ of E joining $x_0$ to $x_1$ is also a minimizer of L and has constant velocity. Thus, there exists C such that $F(\gamma^*(t), \dot{\gamma}^*(t)) = C$ for any $t$ in $[0, 1]$. Substituting C in both (6) and (7) yields

$$2E(\gamma^*) = L(\gamma^*)^2 = d(x_0, x_1)^2. \qquad (8)$$

In addition, with the reparametrization $\phi : [0, d(x_0, x_1)] \to [0, 1]$ such that $\phi(t) = \frac{t}{d(x_0, x_1)}$, we denote by $\gamma^\dagger = \gamma^* \circ \phi$, defined on $[0, d(x_0, x_1)]$, the unique parametrization of the geodesic with constant velocity 1, for all $t$ in $[0, d(x_0, x_1)]$ we have

$$F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t)) = 1, \qquad (9)$$

and we say that $\gamma^\dagger$ is parametrized by arclength. In the rest of the paper, $\gamma^*$ is always defined on $[0, 1]$ and is a minimizer of the energy E, and $\gamma^\dagger$ is always the reparametrization by arclength of $\gamma^*$. Using the minimizers of E provides a unique representation of the geodesics. Perhaps more importantly, E can be used to characterize the geodesics by a differential equation. For this purpose, we introduce the fundamental bilinear form associated with the Finsler operator F. For any $(x, v)$ in $TX$ we define the bilinear form $g(x, v)$ on $T_x X$ by

$$g(x, v)(a, b) = \frac{1}{2} \left. \frac{\partial^2}{\partial s \partial t} F^2(x, v + sa + tb) \right|_{s=0, t=0}, \qquad (10)$$

for any a and b in $T_x X$. Using the Euler-Lagrange equation from the calculus of variation, we obtain a new characterization of the geodesics.

**Property 3 (Geodesic equation).** A necessary and sufficient condition for a $C^\infty$ parametric curve $\gamma$ to have constant velocity and represent a local geodesic is that it be a solution of the geodesic equation written in local coordinates $(x^i)$ using Einsteinian notation for i, j, k, l in 1..n as

$$g_{il} \ddot{\gamma}^i + \frac{1}{2} \left( \frac{\partial g_{lj}}{\partial x^k} + \frac{\partial g_{lk}}{\partial x^j} - \frac{\partial g_{jk}}{\partial x^l} \right) \dot{\gamma}^j \dot{\gamma}^k = 0, \qquad (11)$$

where the dependency in $\gamma$ and $\dot{\gamma}$ of g is left implicit for conciseness and $\dot{\gamma}^i$, $g_{il}$ are the coordinates of the velocity and the Finsler tensor in the local basis of $T_{\gamma(t)}X$ associated with the local coordinates $(x^i)$.

A minimizer of E $\gamma^*$: $[0, 1] \to X$ joining $x_0$ to $x_1$ and its reparameterization by arclength $\gamma^\dagger: [0, d(x_0, x_1)] \to X$ represent the same geodesic and have constant velocity, hence are solutions of the geodesic equation.

The bilinear form g is a cornerstone in the study of local geodesics, and it enjoys several properties inherited from the homogeneity of F. In particular, for $(x, v)$ in $TX$, we have:

$$-F^2(x, v) = g(x, v)(v, v),$$

$$- \text{for any } \lambda > 0, g(x, \lambda v) = g(x, v),$$

$$- \text{for any } w \text{ in } T_x X, \frac{1}{2} \left. \frac{\partial}{\partial s} F^2(x, v + sw) \right|_{s=0} = g(x, v)(v, w).$$
$\gamma^\dagger$ of a geodesic connecting $x_0$ and $x_1$, Theorem 7 with $T = d(x_0, x_1)$ implies that for all $t$ in $[0, d(x_0, x_1)]$

$$ \| \tilde{L}_{\gamma^{\dagger}(t)} \dot{\gamma}^{\dagger}(t) - \bar{u} \| \le K (e^{-\sigma t} + e^{-\sigma(d(x_0, x_1) - t)}). \quad (22) $$

The mapping $u \mapsto F(e, u)$ is a norm on $\mathcal{X}$, and all norms are equivalent on a finite-dimensional vector space, so there exists $K' > 0$ such that for all $u$ in $\mathcal{X}$, we have $F(e, u) \le K' \|u\|$. Using the left-invariance of $F$ (15) with $h = \gamma^\dagger(t)$, we obtain

$$ F(\gamma^{\dagger}(t), \dot{\gamma}^{\dagger}(t) - \tilde{L}_{\gamma^{\dagger}(t)} \bar{u}) \le KK' (e^{-\sigma t} + e^{-\sigma(d(x_0, x_1) - t)}), $$

and the vector field $V_x = \tilde{L}_x \bar{u}$ is an exponential geodesic turnpike for $F$ on $X$.

The following section presents a proof of concept of Conjecture 1 usage to analyze the examples given in Fig. 1.

## IV. EXPLORATION OF CONJECTURE 1 WITH EXAMPLES

### A. Geodesic turnpikes for a simple Riemannian distance

We study the existence of a geodesic turnpike for a simple left-invariant Riemannian metric structure on $SE(2)$. We use an example proposed by Žefran in [3] as illustrated in Fig. 1(bottom). The detailed derivations are given in Appendix E.

Let $X = SE(2)$ be the group of rigid transformation of $\mathbb{R}^2$, and $\mathcal{X} = \mathfrak{se}(2)$ the Lie algebra of instantaneous motions. We denote by $v_x$, $v_y$ and $\omega$ the three coordinates of an element $u$ in the canonical basis of $\mathfrak{se}(2)$. With a slight abuse of notations, we write $u = [v_x, v_y, \omega]$. The left-invariant Riemannian operator used by Žefran to generate the geodesic in Fig. 1(bottom) is $F(e, u) = \sqrt{u^\top G u}$ with

$$ G = \begin{bmatrix} \alpha & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \quad (23) $$

where $\alpha > 1$.

The matrices representation of $ad_u$ and $P_u$ in the canonical basis of $\mathfrak{se}(2)$ are

$$ \mathrm{ad}_u = \begin{bmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{bmatrix} \quad \text{and} \quad \mathrm{P}_u = \begin{bmatrix} 0 & 0 & v_y \\ 0 & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix}. \quad (24) $$

Let us recall that for a Riemannian distance $G_u = G$, so

$$ \mathrm{ad}_{u}^{\top} G_{u} u = \begin{bmatrix} v_{y} \omega \\ -\alpha v_{x} \omega \\ (\alpha - 1) v_{x} v_{y} \end{bmatrix}. \quad (25) $$

An equilibrium point $\bar{u}$ satisfies $\mathrm{ad}_{u}^{\top} G_{u} u$. Since $\alpha - 1 \neq 0$, (25) implies that at least two components of $\bar{u}$ must be zero. Using the fact that an equilibrium point also satisfies $\bar{u}^{\top} G_{\bar{u}} \bar{u} = 1$ we obtain exactly six equilibrium points on $S_F(1)$

$$ \begin{aligned} \bar{u}_1 &= \alpha^{-1/2} e_1, & \bar{u}_2 &= e_2, & \bar{u}_3 &= e_3, \\ \bar{u}_4 &= -\alpha^{-1/2} e_1, & \bar{u}_5 &= -e_2, & \bar{u}_6 &= -e_3, \end{aligned} \quad (26) $$

where $(e_1, e_2, e_3)$ is the canonical basis of $\mathfrak{se}(2)$.

We now calculate the Jacobian for these six equilibrium points to find the saddle points. Substituting (23) and (24) in the expression of $J_u$, (20), we obtain

$$ J_u = \begin{bmatrix} 0 & \omega/\alpha & v_y/\alpha \\ -\alpha\omega & 0 & -\alpha v_x \\ (\alpha - 1)v_y & (\alpha - 1)v_x & 0 \end{bmatrix}. \quad (27) $$

Now observe that the basis $(e_1, e_2, e_3)$ is orthogonal for the inner product induced by $G$ because $G$ is diagonal. Thus, the tangent plane to $S_F(1)$ in $\bar{u}_1$, denoted by $A_1$, is spanned by $e_2$ and $e_3$; and $J_{\bar{u}_1|A_1}$ is the 2 by 2 sub-matrix of $J_u$ composed of the two last columns and two last rows of (27). The same reasoning holds for the tangent plane $A_2$ (resp. $A_3$) in $\bar{u}_2$ (resp. $\bar{u}_3$) and we obtain

$$ \begin{aligned} J_{\bar{u}_1|A_1} &= \begin{bmatrix} 0 & -\alpha^{1/2} \\ \alpha^{-1/2}(\alpha - 1) & 0 \end{bmatrix}, & (28) \\ J_{\bar{u}_2|A_2} &= \begin{bmatrix} 0 & 1/\alpha \\ (\alpha - 1) & 0 \end{bmatrix}, & J_{\bar{u}_3|A_3} &= \begin{bmatrix} 0 & 1/\alpha \\ -\alpha & 0 \end{bmatrix}. \end{aligned} $$

The Jacobians for $\bar{u}_4$, $\bar{u}_5$ and $\bar{u}_6$ are the opposite of the ones for $\bar{u}_1$, $\bar{u}_2$ and $\bar{u}_3$. The eigenvalues of those Jacobians are given by the roots of their characteristic polynomial (see Appendix E for details).

$J_{\bar{u}_1|A_1}$ and $J_{\bar{u}_1|A_1}$ have purely imaginary eigenvalues because $\alpha - 1 > 0$, thus, the real parts of the eigenvalues are zero. $\bar{u}_1$, $\bar{u}_3$, $\bar{u}_4$ and $\bar{u}_6$ are not saddle points. On the other hand, $J_{\bar{u}_2|A_2}$ has eigenvalues $\pm\sqrt{(\alpha - 1)/\alpha}$, of which one is positive while the other one is negative.

To conclude, $\bar{u}_2 = e_2$ and $\bar{u}_5 = -e_2$ are the only saddle points. These are precisely the two translations along the y axis of the local frame, i.e., parallel to the short side of the rectangle. Conjecture 1 implies that the left-invariant vector fields of instantaneous translations parallel to the short side are geodesic turnpikes. It is precisely the turnpike behavior observed in Fig. 1(bottom), hinting at the potential validity of the conjecture.

This section has shown a prototypical strategy to find the saddle points of the geodesic dynamics analytically. In the following subsection, we present a numerical strategy for identifying the saddle points for more general distances and a procedure for using them in the geodesic computation.

### B. Geodesic turnpike for the minimum swept-volume distance

We propose a numerical procedure to accelerate the geodesic computation associated with $F$. First, an offline algorithm identifies the saddle points of the Lie algebra dynamics. Assuming Conjecture 1, they are geodesic turnpikes. Second, we propose an initialization strategy using the candidate geodesic turnpikes in a geodesic solver that significantly accelerates the convergence. It is important to note that the first part of the computation is done only once for a given Finsler operator $F$ and not for every geodesic computation. Thus, identifying candidate geodesic turnpikes does not impact the computation cost of a specific geodesic joining a pair of configurations, as done in motion planners such as RRT and its variants [6].
# 3 Lie groups and geodesic turnpikes

## 3.1 Motivation for robotic applications

In robotics, the configuration space is often a Lie Group. For instance, the configuration space for a free-flying robot is SE(3) the group of rigid placements, and for a serial manipulator arm with six joints, it is SO(2)⁶. These spaces are typically equipped with a smooth Finsler operator. For example, SE(3) is generally equipped with the left-invariant canonical Riemannian distance [9] for which the geodesics can be obtained in closed form. However, for more complex distances, such as the minimum swept-volume distance in [23], the geodesics are not available in closed form and must be computed by solving the problem

$$ \min_{\gamma \in C^\infty([0,1], X)} \frac{1}{2} \int_0^1 F^2(\gamma(t), \dot{\gamma}(t)) dt, \quad \text{s.t.} \quad \gamma(0) = x_0 \quad \text{and} \quad \gamma(1) = x_1, \quad (12) $$

for any $x_0$ and $x_1$ in X.

This problem can be solved using off-the-shelf minimization software such as [12,27] on a discretized version of (12), or with a bounded value ODE solver, such as [15], using the induced geodesic equation (11) and boundary conditions $\gamma(0) = x_0$ and $\gamma(1) = x_1$. In both cases, initialization has a crucial impact on the performance of the algorithms because the underlying minimization problems are not necessarily convex. The geodesics of [23,35], illustrated in Fig. 1, suggest a turnpike behavior.

We propose formalizing this behavior in terms of geodesic turnpikes using vector fields. We can then initialize geodesic solvers efficiently using these vector fields, as presented in Section 4.

## 3.2 Geodesic turnpike definition

Let $(X, F)$ denote a Finsler manifold, and $d$ denote the associated distance. Let $V$ denote a smooth vector field on $X$. Qualitatively, $V$ is a geodesic turnpike for $F$ on $X$ if every parametrization $\gamma^\dagger$ by arclength of a geodesic has its derivative

<figure><img src="image_3.png" /><figcaption>**Fig. 2.** Illustration of geodesic turnpikes. In blue, we plot two Finslerian geodesics of the minimum swept-volume distance [23], joining $x_0^a$ with $x_1^a$ and $x_0^b$ with $x_1^b$. In red, we plot integral curves of $V$, the vector field of instantaneous translations parallel to the long side of the rectangle. $V$ is supposedly a geodesic turnpike, as we observe that integral curves of $V$ approximate the geodesics well except at their start and end points.</figcaption></figure>
We assume that $F$ is a left-invariant Finsler operator known
in closed form as a function of $u$, an element of the Lie algebra.
It is the case for the minimum swept-volume distance [1] for
a moving rod. Note that Figures 1, 2 and 3 illustrate geodesics
computed with this operator. We use this example throughout
the section and use the geodesic turnpikes to compute the same
geodesics significantly faster.

**Saddle points identification.** We implement the symbolic expression of $F(e, u)$, $ad_u$ and $P_u$ in the symbolic automatic differentiation framework CasADi [32]. We use automatic differentiation to compute the symbolic expression of the hessian of $F$ and, in turn, the symbolic expression of $G_u$, $D(u)$, and $J_u$. We use the non-linear root finding engine implemented in CasADi with multiple initializations to identify the roots of the system (18), i.e., the equilibrium points $\bar{u}_i$, and thus the numerical matrices $J_i = J_{\bar{u}_i}$. We use a sequence of matrix factorization routines detailed in Appendix F to compute the spectrum of the Jacobian restricted to the tangent plane of $S_F(1)$ in $\bar{u}_i$. We only keep the $\bar{u}_i$ that are saddle points.

When applied to the minimum swept-volume distance of a
moving rod, this algorithm yields exactly two turnpikes: the
instantaneous translation parallel to the long side of the rod
in both directions. It is consistent with the turnpike behavior
observed in Fig. 1 hinting again at the potential validity of the
conjecture 1.

**Initialization of the geodesic solver** In [1], the geodesics are computed using a discretized version of the energy problem (7) with optimization variables ($u_j$) with $j = 0..N$. We adopt a two-stage approach. First, for each saddle point $\bar{u}_i$, we initialize the ($u_j$) with $u_j = \bar{u}_i$ for all $j$, and we solve the minimization problem with $u_j$ fixed for $j = 1..N-1$ and only $u_0$ and $u_N$ as optimization variables. Seco+nd, we keep the saddle point $\bar{u}_i$ with the lowest cost error and solve the associated minimization problem a second time with all $u_j$ as optimization variables.

On an Intel(R) Core(TM) i7-8750H CPU, averaging over 100 cases, the first optimization problem takes 13ms to converge for each turnpike, and the second optimization problem takes around 110 ms. In comparison, the standard optimization used in [1] takes 1.5s to converge.

Overall, geodesic turnpike initialization improved the tim-
ings of geodesic computation by an order of magnitude over
the standard geodesic solver used in [1].

V. CONCLUSION

This paper has laid a theoretical framework for exploring
the geodesic turnpike properties for metric structures on con-
figuration spaces. We introduced the notion of exponential
geodesic turnpikes, and we propose a conjecture for a suf-
ficient condition for a left-invariant Finsler operator to enjoy
the geodesic turnpike property. The examples in section IV
support the conjecture. In addition, using a geodesic turnpike
as an initialization for the geodesic solver of the swept-
volume distance [1] offers a significant acceleration. In future
works, we will try to prove the conjecture. The reversibility of
geodesics is expected to be the main property to exploit. Future

work will also include an in-depth study of only partially
invariant distances and further exploration of the implications
of the turnpike property on various robotics applications.

ACKNOWLEDGMENTS

This work was funded in part by the French government
under management of Agence Nationale de la Recherche as
8 part of the “Investissements d’avenir” program, reference
ANR-19-P3IA-0001 (PRAIRIE 3IA Institute). JP was sup-
ported in part by the Louis Vuitton/ENS-PSL chair in artificial
intelligence and a Global Distinguished Professorship at the
Courant Institute of Mathematical Sciences and the Center for
Data Science at New York University.

REFERENCES

[1] Y. de Mont-Marin, J. Ponce, and J.-P. Laumond, “A min-
imum swept-volume metric structure for configuration
space,” in *IEEE International Conference on Robotics
and Automation (ICRA)*, pp. 3686–3692, 2023.

[2] E. Trélat and E. Zuazua, “The turnpike property in finite-dimensional nonlinear optimal control,” *Journal of Differential Equations*, vol. 258, no. 1, pp. 81–114, 2015.

[3] M. Žefran, V. Kumar, and C. B. Croke, “On the gener-
ation of smooth three-dimensional rigid body motions,”
*IEEE Transactions on Robotics and Automation*, vol. 14,
no. 4, pp. 576–589, 1998.

[4] G. S. Chirikjian, “Partial bi-invariance of SE(3) metrics,”
*Journal of Computing and Information Science in Engi-
neering*, vol. 15, no. 1, 2015.

[5] J. J. Kuffner, “Effective sampling and distance metrics for 3d rigid body path planning,” *IEEE International Conference on Robotics and Automation (ICRA)*, vol. 2004, no. 4, pp. 3993–3998, 2004.

[6] S. M. LaValle, *Planning Algorithms*. Cambridge University Press, 2006.

[7] L. Palmieri and K. Arras, “Distance metric learning for rrt-based motion planning with constant-time inference,” *IEEE International Conference on Robotics and Automation (ICRA)*, vol. 2015, 2015.

[8] L. Paiola, G. Grioli, and A. Bicchi, “On the evaluation of collision probability along a path,” 2023.

[9] N. D. Ratliff, M. Zucker, J. A. Bagnell, and S. S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in *IEEE International Conference on Robotics and Automation (ICRA)*, IEEE, 2009.

[10] N. Jaquier, L. Rozo, and T. Asfour, “Unraveling the single tangent space fallacy: An analysis and clarification for applying riemannian geometry in robot learning,” *ArXiv*, vol. abs/2310.07902, 2023.

[11] N. Jaquier and T. Asfour, “Riemannian geometry as a unifying theory for robot motion learning and control,” in *Robotics Research* (A. Billard, T. Asfour, and O. Khatib, eds.), (Cham), pp. 395–403, Springer Nature Switzerland, 2023.

[12] H. Klein, N. Jaquier, A. Meixner, and T. Asfour, “A riemannian take on human motion analysis and retarget-
$\dot{\gamma}^\dagger$ "close" to V. Figure 2 illustrates the idea in the case where $X = SE(2)$, $F$ is the operator for the minimum swept-volume distance [23] and $V$ the vector field of instantaneous translations parallel to the long side of the rectangle, illustrated with a black arrow. We observe that for each geodesic, an integral curve of $V$ approximates the geodesic well except near its start and endpoints. To formalize the notion of "close", we propose a definition of exponential geodesic turnpike inspired by exponential turnpikes in the context of optimal control [32].

**Definition 1 (Exponential geodesic turnpike).** We say that a smooth vector field $V$ is an exponential geodesic turnpike for $F$ on $X$ if there exist constants $K > 0$ and $\sigma > 0$ such that for any $x_0, x_1$ in $X$, there exist $\alpha$ in $\{-1, 1\}$ such that $\gamma^\dagger: [0, d(x_0, x_1)] \to X$ the parametrization by arclength of a geodesic joining $x_0$ and $x_1$ verifies for all $t$ in $[0, d(x_0, x_1)]$

$$F(\gamma^\dagger(t), \alpha\dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \le K(e^{-\sigma t} + e^{-\sigma(d(x_0, x_1) - t)}). \quad (13)$$

If $\gamma^\dagger$ joins $x_0$ to $x_1$, its time-reversed reparameterization connects $x_1$ to $x_0$, and its derivative has the opposite sign. Using $\alpha$ in $\{-1, 1\}$, we can concisely state that any parameterization by arc length or its time-reversed reparameterization possesses an exponential bound around $V$. The left-hand side of Eq. (13) is the distance between $\dot{\gamma}^\dagger(t)$ and $V_{\gamma^\dagger(t)}$ because the mapping $v \mapsto F(x, v)$ is a norm on $T_x X$. The right-hand side of Eq. (13) is an exponential upper bound. The bound is loose for $t$ near 0 due to the first term and for $t$ near $d(x_0, x_1)$ due to the second term. The bound tightens exponentially when $t$ is far from the boundaries of $[0, d(x_0, x_1)]$. Figure 3 illustrates qualitatively this behavior for a minimum swept-volume distance geodesic [23]. In the proposed formalism, the turnpike is a vector field, and the bound is in the space of velocities. In this sense, it resembles the velocity turnpikes [8].

It is important to note that $K$ and $\sigma$ does not depend on $x_0$ and $x_1$, and thus the upper bound in Definition 1 do not depend on $x_0$ and $x_1$, but only on the distance $d(x_0, x_1)$ between those points. It implies that with a given precision,

<figure><img src="image_4.png" /><figcaption>**Fig. 3.** Illustration of an exponential geodesic turnpike bound for a geodesic of the minimum swept-volume distance in the configuration space SE(2) for a moving rod. To aid visualization, an orange disc is rigidly attached to the blue rectangle. The volume swept by the disc is used as a lower bound for the volume swept by the rectangle, denoted as $F_D \le F_R$. The volume swept by a disc of radius $r$ is given by $F_D(\omega, v) = 2r\|v\|$, which is proportional to the linear velocity $v$ norm. The exponential bound described in Definition 1 thus implies an exponential bound on the linear velocity: $|v(t) - W_l| \le b(t)$, where $W = [0, W_l]$ represents the geodesic turnpike. This further leads to a bound on the position $p(t)$ of the center of the orange disc and the blue rectangle: $|p(t + dt) - (p(t) + W_l dt)| \le \Delta t, b(t)$. For each time step, the union of all possible positions of the disc's center, which satisfies the bound $|c - (p(t) + W_l dt)| \le \Delta t, b(t)$, is represented in green. This union of green discs provides an upper bound on the workspace region, as ensured by the exponential bound on the orange disc rigidly attached to the rectangle.</figcaption></figure>
ing,” in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pp. 5210–5217, IEEE, 2022.

[13] N. D. Ratliff, K. Van Wyk, M. Xie, A. Li, and M. A. Rana, “Generalized nonlinear and finsler geometry for robotics,” in *IEEE International Conference on Robotics and Automation (ICRA)*, pp. 10206–10212, 2021.

[14] K. Van Wyk, M. Xie, A. Li, M. A. Rana, B. Babich, B. Peele, Q. Wan, I. Akinola, B. Sundaralingam, D. Fox, B. Boots, and N. D. Ratliff, “Geometric fabrics: Generalizing classical mechanics to capture the physics of behavior,” *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 3202–3209, 2022.

[15] L. W. McKenzie, “Turnpike theory,” *Econometrica*, vol. 44, no. 5, pp. 841–865, 1976.

[16] N. Sakamoto and E. Zuazua, “The turnpike property in nonlinear optimal control—a geometric approach,” *Automatica*, vol. 134, p. 109939, 2021.

[17] B. Geshkovski and E. Zuazua, “Turnpike in optimal control of pdes, resnets, and beyond,” *Acta Numerica*, vol. 31, p. 135–263, 2022.

[18] C. Kuehn, *Multiple Time Scale Dynamics*. Applied Mathematical Sciences, Springer International Publishing, 2016.

[19] L. Zhang, Y. J. Kim, and D. Manocha, “C-DIST,” in *ACM symposium on Solid and physical modeling (SPM)*, ACM Press, 2007.

[20] M. Žefran, V. Kumar, and C. B. Croke, “Choice of riemannian metrics for rigid body kinematics,” *International Design Engineering Technical Conferences and Computers and Information in Engineering Conference (IDETC/CIE)*, vol. 2B-1996, no. 3, pp. 1–23, 1996.

[21] T. Faulwasser and L. Grüne, “Chapter 11 - turnpike properties in optimal control: An overview of discrete-time and continuous-time results,” in *Numerical Control: Part A* (E. Trélat and E. Zuazua, eds.), vol. 23 of *Handbook of Numerical Analysis*, pp. 367–400, Elsevier, 2022.

[22] E. Frazzoli, M. A. Dahleh, and E. Feron, “Maneuver-based motion planning for nonlinear systems with symmetries,” *IEEE Transactions on Robotics*, vol. 21, no. 6, pp. 1077–1091, 2005.

[23] I. Bucataru and R. Miron, *Finsler-Lagrange Geometry: Applications to Dynamical Systems*. Editura Academiei Române, 2007.

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

[25] F. A. Potra and S. J. Wright, “Interior-point methods,” *Journal of Computational and Applied Mathematics*, vol. 124, no. 1, pp. 281–302, 2000. Numerical Analysis 2000. Vol. IV: Optimization and Nonlinear Equations.

[26] W. Jallet, A. Bambade, N. Mansard, and J. Carpentier, “Constrained differential dynamic programming: A primal-dual augmented lagrangian approach,” in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pp. 13371–13378, 2022.

[27] J. Kierzenka and L. F. Shampine, “A bvp solver based on

residual control and the maltab pse,” *ACM Transaction on Mathematical Software*, vol. 27, no. 3, p. 299–316, 2001.

[28] G. S. Chirikjian, *Stochastic Models, Information Theory and Lie Groups Volume 2, Analytic Methods and Modern Applications*. Birkhäuser Boston, 2012.

[29] V. I. Arnol'd, *Mathematical methods of classical mechanics*. Graduate Texts in Mathematics, New York, NY: Springer, 2 ed., 1989.

[30] E. Celledoni, H. Marthinsen, and B. Owren, “An introduction to lie group integrators – basics, new developments and applications,” *Journal of Computational Physics*, vol. 257, pp. 1040–1061, 2014. Physics-compatible numerical methods.

[31] D. Latifi and A. Razavi, “Bi-invariant finsler metrics on lie groups,” *Australian Journal of Basic and Applied Science*, vol. 5, no. 12, pp. 507–511, 2011.

[32] 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, p. 1–36, 2018.

[33] A. L. Onishchik and E. B. Vinberg, *Lie Groups and Algebraic Groups*. Springer Berlin Heidelberg, 1990.

[34] V. R. Kjer, “Lie group integrators for cotangent bundles of lie groups and their application to systems of dipolar soft spheres,” Oslo, Norway, 2022. Under supervision of Brynjulf, Owren, IMF.

[35] J. Solà, J. Deray, and D. Atchuthan, “A micro lie theory for state estimation in robotics,” *ArXiv*, vol. abs/1812.01537, 2018.
the length of the geodesic segment for which the velocity bound (13) is looser than $\epsilon$ does not depend on the total geodesic length.

**Proposition 1.** For any $\epsilon > 0$, there exists $\tau > 0$ such that for any curve $\gamma^\dagger$ parametrized by arclength and representing a geodesic, if $I_\epsilon$ denote the set of $t$ such that $F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \ge \epsilon$ we have

$$L(\gamma^\dagger|_{I_\epsilon}) \le \tau. \qquad (14)$$

Figure 4 illustrates this proposition. Proposition 1 ensures that integral curves of $V$ are a good approximation of "long" geodesics. This is because $\tau$ depends only on $\epsilon$ and not on any particular geodesic or its length $L$. As a result, $\tau/L \to 0$ as $L \to \infty$, meaning that the larger $L$ is, the better the approximation becomes. In the example of Fig. 1 (right), even configurations that appear "close" in the workspace have a "long" geodesic length from the point of view of the turnpike property.

## 3.3 Left-invariant Finsler operators on Lie groups

Having defined geodesic turnpikes, we now examine the specific case typical in robotics, where $X$ is a Lie group and the operator $F$ demonstrates some invariance [8]. The invariance allows to simplify the geodesic equation, making possible to study the dynamics to identify geodesic turnpike.

We now study the case of invariant reversible Finsler distances on Lie groups to identify sufficient conditions for characterizing geodesic turnpikes. From now on, $X$ is a Lie group. Given some $h$ in $X$, we denote by $\tilde{L}_h$ the left tangent action, which describes how the tangent vectors at different points in the group are related through left multiplication (see Chirikjian [6] for further details on Lie groups). For any $x$ in $X$, $\tilde{L}_h : T_x X \to T_{h \circ x} X$ is a linear isomorphism. We

<figure><img src="image_5.png" /><figcaption>**Fig. 4.** To illustrate Proposition 1, we plot the right-hand side (13) as a function of the arclength $t$ of geodesics of various lengths $L$. Given an $\epsilon > 0$, there exists a $\tau > 0$ such that for any geodesic, the velocity is guaranteed to be upper-bounded by $\epsilon$ whenever $\tau/2 < t < L - \tau/2$. The first two curves (red and orange) correspond to geodesics with lengths lower than $\tau$.</figcaption></figure>
A. Proof of Finsler operator properties

We will prove five properties of the Finslerian operator F and the associated bilinear form g as defined in (10). The ones mentioned in section II are properties A.1, A.2, A.5.

**Property A.1.** For any x in X and v in $T_x X$ we have $F^2(x, v) = g(x, v)(v, v)$.

*Proof:* Using the definition

$$ 
\begin{aligned}
g(x,v)(v,v) &= \frac{1}{2} \left. \frac{\partial^2}{\partial s \partial t} F^2(x,v+sv+tv) \right|_{s=0,t=0} \\
&= \left. \frac{\partial^2}{\partial s \partial t} \left( \frac{1}{2}(1+s+t)^2 \right) \right|_{s=0,t=0} F^2(x,v) \\
&= F^2(x,v).
\end{aligned} 
\quad (29) 
$$

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

**Property A.2.** For any x in X, v in $T_x X$ and $\lambda > 0$ we have

$$ g(x, \lambda v) = g(x, v). \quad (30) $$

*Proof:* For any a and b in $T_x X$ we have

$$ 
\begin{aligned}
g(x, \lambda v)(a, b) &= \frac{1}{2} \lim_{s \to 0^+, t \to 0^+} \frac{F^2(x, \lambda v + sa + tb)}{st} \\
&= \frac{1}{2} \lim_{s \to 0^+, t \to 0^+} \frac{\lambda^2 F^2(x, v + (s/\lambda)a + (t/\lambda)b)}{\lambda^2(s/\lambda)(t/\lambda)} \\
&= \frac{1}{2} \lim_{s' \to 0^+, t' \to 0^+} \frac{F^2(x, v + s'a + t'b)}{s't'} \\
&= g(x, v)(a, b),
\end{aligned} 
\quad (31) 
$$

where we use the changes of variables $s' = s/\lambda$ and $t' = t/\lambda$.

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

**Property A.3.** We denote by C the tri-linear symmetric form called the Cartan tensor [23] defined for any a, b, c in $T_x X$ as

$$ C(x,v)(a,b,c) = \frac{1}{2} \left. \frac{\partial^3}{\partial s \partial t \partial u} F^2(x,v+sa+tb+uc) \right|_{s=0,t=0,u=0}. \quad (32) $$

For any x in X and v, a, b in $T_x X$ we have

$$ C(x, v)(v, a, b) = 0. \quad (33) $$

*Proof:*

$$ 
\begin{aligned}
C(x,v)(v,a,b) &= \frac{1}{2} \left. \frac{\partial^3}{\partial s \partial t \partial u} F^2(x,v+sv+ta+ub) \right|_{s=0,t=0,u=0} \\
&= \left. \frac{\partial}{\partial s} \left[ \frac{1}{2} \left. \frac{\partial^2}{\partial t \partial u} F^2(x,(1+s)v+ta+ub) \right|_{t=0,u=0} \right] \right|_{s=0} \\
&= \left. \frac{\partial}{\partial s} g(x,(1+s)v)(a,b) \right|_{s=0} \\
&= \left. \frac{\partial}{\partial s} g(x,v)(a,b) \right|_{s=0} \\
&= 0,
\end{aligned} 
\quad (34) 
$$

where we used the property A.2 between the second to last and last equality.

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

**Property A.4.** For any x in X and v, a, b in $T_x X$ we have

$$ \left. \frac{\partial}{\partial s} g(x, v + sa)(v, b) \right|_{s=0} = 0, \quad (36) $$
say that $F$ is left-invariant on $X$, if for every $(x, v)$ in $TX$ and $h$ in $X$,

$$F(x, v) = F(h \circ x, \tilde{L}_h v), \qquad (15)$$

i.e., the Finsler operator does not change when we compose $x$ on the left with a group element $h$ and apply the associated left tangent action on $v$. In particular, with $h = x^{-1}$, we have $F(x, v) = F(e, \tilde{L}_{x^{-1}}v)$ where $e$ denote the identity element of $X$. Let $C(u) = \frac{1}{2}F^2(e, u)$ denote the cost function used in (12) restricted to the Lie algebra $\mathcal{X}$ and let $u : [0, 1] \to \mathcal{X}$ defined by $u(t) = \tilde{L}_{\gamma(t)-1}\dot{\gamma}(t)$, then (12) is equivalent to

$$ \begin{array}{ll} \min_{\gamma \in C^\infty([0,1], \mathcal{X}), u \in C^\infty([0,1], \mathcal{X})} & \displaystyle \int_0^1 C(u(t))dt, \qquad (16) \\ \text{s.t} & \forall t \in [0, 1], \quad \dot{\gamma}(t) = \tilde{L}_{\gamma(t)}u(t), \\ & \gamma(0) = x_0, \\ & \gamma(1) = x_1, \end{array} $$

because $\tilde{L}_{\gamma(t)-1} = \tilde{L}_{\gamma(t)}^{-1}$. The calculus of variations can now be used to obtain a more tractable geodesic equation associated with (16) than the general one (11):

**Proposition 2.** If $F$ is a left-invariant Finsler distance on the Lie group $X$, then the geodesic equation associated with (16) can be written in some basis of the Lie algebra $\mathcal{X}$ as:

$$ \begin{aligned} \dot{\gamma}(t) &= \tilde{L}_{\gamma(t)}u(t), \\ \dot{u}(t) &= G_{u(t)}^{-1} \mathrm{ad}_{u(t)}^{\top} G_{u(t)}u(t), \end{aligned} \qquad (17) $$

where $G_u$ denotes the matrix representation of $g(e, u)$ in the chosen basis, $\mathrm{ad}_u$ denotes the matrix representation in that basis of the Lie algebra adjoint operator $\mathrm{ad}_u$, which captures the action of $u$ on the rest of the algebra via the Lie bracket on $\mathcal{X}$. The solutions represent local geodesics and do not depend on the choice of basis.

In the Riemannian case, $G_u$ is independent of $u$, and we recover the so-called Euler-Arnol'd equation [2]. We feel that Proposition 2 should be textbook material since it resembles classical results [5,20] but, to the best of our knowledge, this geodesic equation for invariant Finsler distances has not been written in this form yet.

## 3.4 Geodesic turnpikes for left-invariant Finsler distances

In the optimal control literature, the exponential bounds, characteristic of turnpike properties, are often linked to the hyperbolicity of an underlying dynamic system [32]. Thus we will characterize hyperbolicity of the geodesic dynamic and use a theorem to propose a conjecture on the existence of geodesic turnpike.
Proof: We have $\frac{\partial}{\partial s}g(x, v+sa)(v, b)|_{s=0} = C(x, v)(v, a, b)$ hence the result using property A.3.

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

**Property A.5.** For any $x$ in $X$ and $v, w$ in $T_x X$ we have

$$ \frac{1}{2} \frac{\partial}{\partial s} F^2(x, v+sw) \Big|_{s=0} = g(x, v)(v, w). \qquad (37) $$

Proof: Using the property A.1, we have

$$ \begin{aligned} F^2(x, v+sw) &= g(x, v+sw)(v+sw, v+sw) \\ &= g(x, v+sw)(v, v) + 2sg(x, v+sw)(v, w) + s^2 g(x, v+sw)(w, w). \end{aligned} \qquad (38) $$

Taking the derivative with respect to $s$, we have

$$ \frac{\partial}{\partial s} F^2(x, v+sw) = \frac{\partial}{\partial s} g(x, v+sw)(v, v) \qquad (39a) $$

$$ + 2g(x, v+sw)(v, w) \qquad (39b) $$

$$ + 2s \frac{\partial}{\partial s} g(x, v+sw)(v, w) \qquad (39c) $$

$$ + 2sg(x, v+sw)(w, w) \qquad (39d) $$

$$ + s^2 \frac{\partial}{\partial s} g(x, v+sw)(w, w). \qquad (39e) $$

When $s = 0$ (39c), (39d), (39e) are naturally equal to 0 and using property A.4 the term (39a) is also equal to 0. Only (39b) is not zero, and we obtain

$$ \frac{\partial}{\partial s} F^2(x, v+sw) \Big|_{s=0} = g(x, v)(v, w) \qquad (40) $$

Hence, the final result.

In the rest of the appendices, given a map $f: \mathcal{X} \times Y \to Z$ with $Y$ a manifold and $Z$ a vector space, for any $u$ in $X$, $y$ in $Y$ we denote by $\frac{\partial}{\partial u}f|_{u,y}$ the linear map from $\mathcal{X}$ onto $Z$ defined for any $v$ in $\mathcal{X}$ as

$$ \frac{\partial}{\partial u} f|_{u,y} = \frac{\partial}{\partial s} f(u + sv, y)|_{s=0}. \qquad (41) $$

## B. Proof of proposition 5

**Proposition.** For any $\varepsilon > 0$, there exists $\tau > 0$ such that for any curvilinear parametric curve $\gamma^\dagger$ representing a geodesic, if $I_\varepsilon$ denote the interval of $t$ such that $F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \ge \varepsilon$ we have

$$ L(\gamma^\dagger|_{I_\varepsilon}) \le \tau. \qquad (14) $$

Proof: Let $\gamma^\dagger$ a parametric curve representing a geodesic joining $x_0$ to $x_1$ with constant velocity and of length, $L = d(x_0, x_1)$ verifying Definition 1. For any $\varepsilon > 0$ we chose $\tau = (2/\sigma) \ln(2K/\varepsilon)$, which is independent of the length $L$ and we verify:

* for any $t > \tau/2$ we have $e^{-\sigma t} < e^{-\sigma \frac{\tau}{2}} = e^{-\ln(2K/\varepsilon)} = \varepsilon/(2K)$,

* for any $t < L - \tau/2$ we have $e^{-\sigma(L-t)} < e^{-\sigma(L-(L-\frac{\tau}{2})}} = e^{-\sigma \frac{\tau}{2}} = \varepsilon/(2K)$.

Summing up, we have

$$ (\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \le K (e^{-\sigma t} + e^{-\sigma(L-t)}) \le \varepsilon, \quad t \in [\tau/2, L] \cap [0, L - \tau/2]. \qquad (42) $$

Thus, if $I_\varepsilon$ denote the interval of $t$ such that $F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) > \varepsilon$ we have

$$ I_{\varepsilon} \subset [0, L] \setminus ([\tau/2, L] \cap [0, L - \tau/2]) \subset [0, \tau/2] \cup [L - \tau/2, L], \qquad (43) $$

and $\mu(I_\varepsilon) \le \mu([0, \tau/2]) + \mu([0, \tau/2]) = \tau$, hence the result.

<figure><img src="image_10.png" /></figure>
Let us now study the dynamics given by the geodesic equation (17) of the left-invariant Finsler operator $F$ on $X$. The objective is to construct a vector field $V$ that is an exponential geodesic turnpike for $F$ on $X$ as defined in Definition 1. Any parametric curve $\gamma^*$ solution of the minimization problem (16) is a solution of the geodesic equation (17). Its reparametrization $\gamma^\dagger$ by arclength, defined on $[0, L]$ and representing the same geodesic, is also a solution of the geodesic equation (17). In particular, $u(t) = \tilde{L}_{\gamma^\dagger(t)-1} \dot{\gamma}^\dagger(t) = \tilde{L}_{\gamma^\dagger(t)}^{-1} \dot{\gamma}^\dagger(t)$ is a solution of $\dot{u} = D(u)$ where $D$ denote the dynamics function on the lie algebra $\mathcal{X}: D(u) = G_u^{-1} \text{ad}_u^\top G_u u$. The solutions of the geodesic equation have a constant velocity, and $\gamma^\dagger(t)$ is parametrized by arclength, so we have for any $t$ in $[0, L]$, $F(e, u(t)) = F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t)) = 1$. Thus, the dynamical system $\dot{u} = D(u)$ is well-defined on $\mathbb{S}_F(1)$, the unit sphere associated with $F$ and defined as the set of all $u$ in $\mathcal{X}$ such that $F(e, u) = 1$. Therefore, we study the dynamics associated with $D$ on $\mathbb{S}_F(1)$. The lie algebra elements $\bar{u}$ of $\mathbb{S}_F(1)$ verifying $D(\bar{u}) = 0$ are the equilibrium points of the dynamics on $\mathbb{S}_F(1)$. Using the homogeneity properties of Finsler operators, we obtain the following characterization of these points:

**Proposition 3.** An element $\bar{u}$ in the lie algebra $\mathcal{X}$ is an equilibrium point of the dynamics $D$ on $\mathbb{S}_F(1)$ if and only if

$$ \text{ad}_{\bar{u}}^{\top} G_{\bar{u}} \bar{u} = 0, \quad \bar{u}^{\top} G_{\bar{u}} \bar{u} = 1, \qquad (18) $$

In addition, the tangent space $T_{\bar{u}}\mathbb{S}_F(1)$ tangent to $\mathbb{S}_F(1)$ in $\bar{u}$ is the set of elements $v$ in $\mathcal{X}$ such that $v^{\top} G_{\bar{u}} \bar{u} = 0$ and the Jacobian of the dynamics around $\bar{u}$ is

$$ J_{\bar{u}} = G_{\bar{u}}^{-1} (\text{ad}_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}), \qquad (19) $$

where $P_{G_{\bar{u}}}$ is the unique matrix defined by

$$ \forall v \in \mathcal{X}, \quad P_{G_{\bar{u}}} v = \text{ad}_v^{\top} G_{\bar{u}}. \qquad (20) $$

Note that $P_{G_{\bar{u}}}$ is well defined because the mapping $v \mapsto \text{ad}_v$ is linear.

The dynamics around the equilibrium point are given by the spectrum of $J_{\bar{u}}$ [18]. As we study the dynamics on $\mathbb{S}_F(1)$, we restrict the Jacobian to the tangent space $A = T_{\bar{u}}\mathbb{S}_F(1)$. From [30], we know that exponential bounds typically occur around an equilibrium point when it is a saddle point.

**Definition 2 (Saddle point on the unit sphere).** An equilibrium point $\bar{u}$ of the dynamics $D$ on $\mathbb{S}_F(1)$ is called a saddle point when all eigenvalues of the endomorphism $J_{\bar{u}}|_A$ have nonzero real parts, at least one of which has a positive real part, and at least another one has a negative real part.

The theory of dynamical systems [18] gives a general characterization of saddle points. Sakamoto et al. [30] sum up many of those results, with particular attention to the existence of exponential bounds for a dynamical system around saddle points. In our setting, we say that a sub-manifold $M$ of $\mathbb{S}_F(1)$ is invariant if any solution of the dynamics $\dot{u} = D(u)$ with $u(0)$ in $M$ remains in $M$ for
C. Proof of proposition 6

**Proposition.** If F is a left-invariant Finsler distance on the Lie group X, then the geodesic equation associated to (16) can be written in some basis of X:

$$ \dot{\gamma}(t) = \tilde{L}_{\gamma(t)} u(t), \qquad (17) $$

$$ \dot{u}(t) = G_{u(t)}^{-1} \mathrm{ad}_{u(t)}^{\top} G_{u(t)} u(t), $$

where $G_u$ denote the matrix representation of $g(e, u)$ in the chosen basis and $\mathrm{ad}_u$ denote the matrix representation of the adjoint operator of $u$ on $\mathcal{X}$ in the chosen basis. The solutions represent local geodesics and do not depend on the basis choice.

*Proof:* The Hamiltonian of the variational problem (16) reads

$$ H(x, p, u) = \langle p, \tilde{L}_x u \rangle_x - C(u), \qquad (44) $$

where and $u \in \mathcal{X}$ and $(x, p) \in T^*X$ the co-tangent of $X$, i.e., $x \in X$ and $p$ is a linear form on $T_x X$. $\langle p, v \rangle_x$ is the dual bracket that takes a covector and a vector and returns the scalar obtained by applying the covector on the vector on the tangent space in $x$. The cotangent bundle $T^*X$ of a Lie group $X$ is a trivial bundle<fnref n="1" />, and the Hamiltonian equations for all $t$ are:

$$ \dot{x}(t) = \left. \frac{\partial H}{\partial p} \right|_{x(t),p(t),u(t)} = \tilde{L}_{x(t)} u(t), \qquad (45a) $$

$$ \dot{p}(t) = -\left. \frac{\partial H}{\partial x} \right|_{x(t),p(t),u(t)} = -\left. \frac{\partial \langle p, \tilde{L}_x u \rangle_x}{\partial x} \right|_{x(t),p(t),u(t)}, \qquad (45b) $$

$$ 0 = \left. \frac{\partial H}{\partial u} \right|_{x(t),p(t),u(t)} = \left. \frac{\partial \langle p, \tilde{L}_x u \rangle_x}{\partial u} \right|_{x(t),p(t),u(t)} - \left. \frac{\partial C}{\partial u} \right|_{u(t)}. \qquad (45c) $$

We use the left trivialization of the cotangent bundle described below to simplify the Hamiltonian equations. For $(x, p)$ in $T^*X$, we denote by $p_t = p \circ \tilde{L}_x \in \mathcal{X}^*$ the left trivialization of the covector $p$. The cotangent bundle, $T^*X$, is equipped with a natural group structure induced by the group structure of $X$ (see [33, 34] for details) and the mapping

$$ \phi : (x, p) \in T^*X \mapsto (x, p_b) \in X \times \mathcal{X}^*, \qquad (46) $$

is a Lie group isomorphism where $X \times \mathcal{X}^*$ is a lie-group semi-direct product with the group law

$$ (x, p_b) \circ (y, q_b) = (x \circ y, \mathrm{Ad}_y^*(p_b) + q_b), \qquad (47) $$

where $\mathrm{Ad}_y^*$ is the co-adjoint representation of $y \in X$ (see [33] for details). First, we have $\langle p, \tilde{L}_x u \rangle_x = \langle p \circ \tilde{L}_x, u \rangle_e = \langle p_b, u \rangle_e$ and the third Hamiltonian equation (45c) is equivalent to:

$$ 0 = \left. \frac{\partial \langle p_b, u \rangle_e}{\partial u} \right|_{x(t),p(t),u(t)} - \left. \frac{\partial C}{\partial u} \right|_{u(t)}, \\ p_b(t) = \left. \frac{\partial C}{\partial u} \right|_{u(t)}. \qquad (48) $$

Second, we denote by $\pi : (x, p_b) \in X \times \mathcal{X}^* \to p_b$ the projection, and we use the left Jacobian, as coined in [35], for $v$ in $\mathcal{X}$ we have

$$ \begin{aligned} \frac{\partial \pi}{\partial x}(\tilde{L}_x v) &= \left. \frac{\partial}{\partial s} \pi((x, p_b) \circ (\exp(sv), 0)) \right|_{s=0} \\ \frac{\partial p_b}{\partial x}(\tilde{L}_x v) &= \left. \frac{\partial}{\partial s} \mathrm{Ad}_{\exp(sv)}^*(p_b) + 0 \right|_{s=0} \\ &= \left. \frac{\partial}{\partial s} \exp(s \mathrm{ad}_v^*) \right|_{s=0} (p_b) \\ &= \mathrm{ad}_v^*(p_b). \end{aligned} \qquad (49) $$

We denote by $\psi$ the linear form on $\mathcal{X}$ defined as

$$ \psi = - \frac{\partial \langle p_b, u \rangle_e}{\partial x} \circ \tilde{L}_x, \qquad (50) $$

<footnote n="1">¹its topology is the one of a Cartesian product between the base space X and a fiber</footnote>
all time $t$. Let $\bar{u}$ denote a saddle point on the unit sphere $\mathbb{S}_F(1)$ of dimension $m = n - 1$. It is shown in [18] that there exists an invariant sub-manifold $S$ of $\mathbb{S}_F(1)$ of dimension $k$ tangent to the eigenvectors of $J_{\bar{u}}|_A$ associated with positive eigenvalues and called the *stable manifold* and an invariant sub-manifold $U$ of $\mathbb{S}_F(1)$ of dimension $m - k$ tangent to the eigenvectors of $J_{\bar{u}}|_A$ associated with negative eigenvalues and called the *unstable manifold*. Sakamoto et al. [30, Th. 3] prove the following theorem:

**Theorem 2.** Let $u \to \|u\|$ be a norm on $\mathcal{X}$. For any $v_0$ in $S$, any $v_1$ in $U$, any $(m-k)$-dimensional disc $D_S$ transversal to $S$ at $v_0$, and any $k$-dimensional disc $D_U$ transversal to $U$ at $v_1$, there exist constants $T > 0$, $K > 0$ and $\sigma > 0$ such that for every $L > T$, there exists $\rho > 0$, $u_0$ in $B(v_0, \rho) \cap D_S$ and $u_1$ in $B(v_1, \rho) \cap D_U$ such that there exists a solution $u^\dagger$ of the dynamics $D$ with $u^\dagger(0) = u_0$ and $u^\dagger(L) = u_1$ and for any $t$ in $[0, L]$

$$ \|u^{\dagger}(t) - \bar{u}\| \le K \left( e^{-\sigma t} + e^{-\sigma(L-t)} \right). \qquad (21) $$

Moreover, $\rho \to 0$ when $L \to \infty$.

This theorem is illustrated by Fig. 5. Informally around a saddle point, we have families of trajectories, indexed by choice of $v_0$, $v_1$, $D_S$, and $D_U$, with endpoints in $D_S$ and $D_U$ that can have arbitrarily long durations and spend most of their time very close to the saddle point. All elements of a family share the constant $T$, $K$, and $\sigma$.

<figure><img src="image_6.png" /><figcaption>**Fig. 5.** Illustration of Theorem 2. We plot a portion of the surface $\mathbb{S}_F(1)$ around a saddle point $\bar{u}$ of the Lie algebra dynamics $\dot{u} = D(u)$. We show an eigenvector $w_+$ associated with a positive eigenvalue tangent to the stable manifold $S$ and, in red, an eigenvector $w_-$ associated with a negative eigenvalue tangent to the unstable manifold $U$. We plot the discs $D_S$ and $D_U$ transversal to $S$ in $u_0$ and to $U$ $u_1$. A solution $u^\dagger$ of the dynamics with $u^\dagger(0) = u_0$ in $D_S$ and $u^\dagger(L) = u_1$ in $D_U$ is shown in black, and it verifies the conditions of Theorem 2. The distance $\|u^\dagger(t) - \bar{u}\|$ is bounded exponentially. We draw the points $u^\dagger(t_i)$ as small gray discs for $i = 0..9$ where the sequence $t_i = iL/9$ forms a regular partition of $[0, L]$. The distribution of the samples $u^\dagger(t_i)$ illustrates the behavior induced by the exponential bound: $u^\dagger$ goes quickly toward the saddle point, slows down near it, and finishes fast toward the endpoint $u_1$.</figcaption></figure>
for any $v \in \mathcal{X}$ we have

$$
\begin{align*}
\psi(v) &= - \left\langle \frac{\partial p_b}{\partial x} (\tilde{L}_x v), u \right\rangle_e \\
&= - \left\langle \mathrm{ad}_v^*(p_b), u \right\rangle_e \\
&= - \left\langle p_b, \mathrm{ad}_v(u) \right\rangle_e \\
&= \left\langle p_b, \mathrm{ad}_u(v) \right\rangle_e \\
&= \left\langle \mathrm{ad}_u^*(p_b), v \right\rangle_e, \tag{51}
\end{align*}
$$

and we conclude that $\psi = \mathrm{ad}_u^*(p_b)$. Composing the second Hamiltonian equation (45b) on both side with $\tilde{L}_x$, the second Hamiltonian equation is equivalent to

$$
\begin{align}
\dot{p}_b &= - \frac{\partial \langle p, \tilde{L}_x u \rangle_x}{\partial x} \circ \tilde{L}_x \nonumber \\
&= - \frac{\partial \langle p_b, xu \rangle_e}{\partial x} \circ \tilde{L}_x \nonumber \\
&= \mathrm{ad}_u^*(p_b). \tag{52}
\end{align}
$$

Third, substituting (48) in (52), the Hamiltonian equations (45a), (45b), (45c) are rewritten as

$$
\dot{x}(t) = \tilde{L}_{x(t)}u(t), \tag{53a}
$$

$$
\left. \frac{\partial^2 C}{\partial^2 u} \right|_{u(t)} \dot{u}(t) = \mathrm{ad}_{u(t)}^* \left( \left. \frac{\partial C}{\partial u} \right|_{u(t)} \right). \quad (53\text{b})
$$

Note that the right and left sides are in $\mathcal{X}^*$. Note also that the previous construction is valid for any invariant cost $C$. Now, using the definition of $g$ in (10) and the property A.5, we have

$$
\left. \frac{\partial^2 C}{\partial^2 u} \right|_u (v, w) = g(e, u)(v, w), \qquad (54a)
$$

$$
\left. \frac{\partial}{\partial u} C \right|_{u} (v) = g(e, u)(u, v). \qquad (54b)
$$

Using a basis of $\mathcal{X}$ and its dual basis, we identify elements of $\mathcal{X}^*$ and $\mathcal{X}$. $G_u$ is the matrix representation in this basis of $\left.\frac{\partial^2 C}{\partial^2 u}\right|_{u(t)}$, and $\mathrm{ad}^\top$ is the basis representation of $\mathrm{ad}^*$. Finally, we have the geodesic equation:

$$
\dot{x}(t) = \tilde{L}_{x(t)}u(t), \qquad (55a)
$$

$$
G_{u(t)} \dot{u}(t) = \mathrm{ad}_{u(t)}^{\top} G_{u(t)} u(t), \qquad (55\text{b})
$$

hence the result.

D. Details on Section III-D

Section III-D analyzes the dynamics of the geodesic equation to identify saddle points. In this section, we prove the results stated without proof in III-D. We recall the expression of the dynamics:

$$
D(u) = G_u^{-1} \mathrm{ad}_u^\top G_u u. \tag{56}
$$

**Property D.1.** If $u: I \to \mathcal{X}$ is a solution of $\dot{u} = D(u)$ then

$$
\frac{d}{dt} C(u) = 0. \tag{57}
$$

*Proof:* A solution of the dynamics $\dot{u} = D(u)$ verifies

$$
u^\top G_u \dot{u} = u^\top \mathrm{ad}_u^\top G_u u = (\mathrm{ad}_u u)^\top G_u u = 0, \quad (58)
$$

because the mapping $u, v \mapsto \mathrm{ad}_u v$ is antisymmetric and we have $\mathrm{ad}_u u = 0$. Now, using (37) we obtain

$$
\frac{d}{dt} C(u) = \left. \frac{\partial}{\partial u} C \right|_{u} \dot{u} = g(e, u)(u, \dot{u}) = u^{\top} G_{u} \dot{u} = 0, \quad (59)
$$

hence the result.

The dynamics solutions have constant cost $C$ and, in turn, constant Finsler operator $F(e, u)$. We restrict the dynamics domain to $S_F(1)$, the set of all $u$ such that $F(e, u) = 1$.
Multiple technical subtleties prevent a straightforward use of the theorem in the geodesic problem case. First, we do not have any conditions on $u(0)$ and $u(L)$, but we have conditions on $x(0)$ and $x(L)$ instead. Second, the constants $T$, $K$, and $\sigma$ are given independently for each family of trajectories corresponding to a choice of $v_0$, $v_1$, $D_S$, and $D_U$, whereas we are looking for a constant common for all geodesics. Third, the theorem states that there exists $u_0$ in $D_S \cap B(v_0, \rho)$ and $u_1$ in $D_U \cap B(v_1, \rho)$ such that the solution verifying $u^\dagger(0) = u_0$ and $u^\dagger(L) = u_1$ enjoys the exponential bound property. In contrast, we need that for any $u_0$ in $D_S \cap B(v_0, \rho)$ and $u_1$ in $D_U \cap B(v_1, \rho)$ such that there exists a solution verifying $u^\dagger(0) = v_0$ and $u^\dagger(L) = v_1$, the bound holds. All this makes the case of turnpike geodesics different. Now, we propose and discuss the following conjecture.

**Conjecture 1.** Let $F$ denote a left-invariant reversible Finsler operator on the Lie group $X$. If there exists $\bar{u}$ in $\mathcal{X}$ such that $\bar{u}$ and $-\bar{u}$ are the only saddle points on the unit sphere for the Lie algebra dynamics $D$ then the left-invariant vector field $V_x = \tilde{L}_x \bar{u}$ is an exponential geodesic turnpike for the Finsler distance associated with $F$ on $X$.

We now provide a sketch of the proof. Critical elements are still under development and will be addressed in future publications.

Suppose $\bar{u}$ and $-\bar{u}$ are the only saddle points as defined in Definition 2, the reversibility of geodesics implies that if $\gamma^\dagger(t)$ is a geodesic of length $L$ connecting $x_0$ to $x_1$ then $\gamma^\dagger(L-t)$ is a geodesic of length $L$ connecting $x_1$ to $x_0$. By the chain rule, if $\gamma^\dagger(t)$ verifies the exponential bound with $-\bar{u}$, $\gamma^\dagger(L-t)$ verifies the exponential bound with $\bar{u}$. Note that the mapping $u \mapsto F(e, u)$ is a norm on $\mathcal{X}$, and we can use Theorem 2 with this norm. We have the existence of constants $T$, $K$ and $\sigma$ for any choice of $v_0$ in $S$ the stable manifold of $\bar{u}$ and $v_1$ in $U$ the unstable manifold of $\bar{u}$. If we can choose the transverse discs $D_S(v_0)$ for each $v_0$ in $S$ such that the family $(D_S(v_0))_{v_0 \in S}$ forms a cover of $S_F(1)$ and similarly choose the $D_U$ such that $(D_U(v_1))_{v_1 \in U}$ is also cover of $S_F(1)$, then each geodesic will have their endpoints in a pair $(D_S(v_0), D_U(v_1))$. Under sufficient regularity conditions for the constants $T$, $K$, and $\sigma$, since $S$ and $U$ are included in $S_F(1)$, which is compact, we can take $T^*$, $K^*$ as the supremum of $T$, $K$ and $\sigma^*$ as the infimum of $\sigma$ over the set $S \times U$.

Proving (1) the existence of such covers and (2) the regularity of the constants is challenging due to the difficulty of analytically characterizing the stable and unstable manifolds, and it is left for future work. Suppose we have such covers. We have $T^*$, $K^*$, and $L^*$ such that for all $x_0$ and $x_1$, the parametrization by arclength $\gamma^\dagger: [0, d(x_0, x_1)] \to X$ of a geodesic connecting $x_0$ and $x_1$ or $x_1$ and $x_0$ we have for all $t$ in $[0, d(x_0, x_1)]$

$$F(e, \tilde{L}_{\gamma^\dagger(t)}^{-1} \dot{\gamma}^\dagger(t) - \bar{u}) \le K^* \left( e^{-\sigma^* t} + e^{-\sigma^* (d(x_0, x_1) - t)} \right), \quad (22)$$

and we also have $F(e, \tilde{L}_{\gamma^\dagger(t)-1} \dot{\gamma}^\dagger(t) - \bar{u}) = F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - \tilde{L}_{\gamma^\dagger(t)} \bar{u})$ using the left-invariance (15) of $F$ with $h = \gamma^\dagger(t)$. Thus, the vector field $V_x = \tilde{L}_x \bar{u}$ is an exponential geodesic turnpike for $F$ on $X$.
**Property D.2.** A vector $\bar{u}$ is an equilibrium point of the dynamics $D$ defined on $S_F(1)$ if:

$$ad_{\bar{u}}^{\top} G_{\bar{u}}\bar{u} = 0, \quad \bar{u}^{\top} G_{\bar{u}}\bar{u} = 1, \qquad (18)$$

*Proof:* First, an equilibrium point verifies $D(\bar{u}) = 0$. With (56) and canceling the invertible matrix $G_u$, we obtain the left part of (18). Second, using property A.1, $S_F(1)$ is exactly the set of $u$ such that $g(e, u)(u, u) = u^{\top}G_u u = 1$ hence the result. ■

**Property D.3.** Let $J_u$ denote the Jacobian of the dynamics $D$ taken in $u$. If $\bar{u}$ is an equilibrium point of the dynamics, then we have

$$J_{\bar{u}} = G_{\bar{u}}^{-1}(ad_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (20)$$

*Proof:* Taking the derivative of $D$ defined in (56), we have

$$J_{uv} = \left[ \left. \frac{\partial}{\partial u} G_u^{-1} \right|_u v \right] ad_u^{\top} G_u u \qquad (60a)$$

$$+ G_u^{-1} ad_v^{\top} G_u u \qquad (60b)$$

$$+ G_u^{-1} ad_u^{\top} \left[ \left. \frac{\partial}{\partial u} G_u \right|_u v \right] u \qquad (60c)$$

$$+ G_u^{-1} ad_u^{\top} G_u v. \qquad (60d)$$

The term (60c) is zero because $\left[ \left. \frac{\partial}{\partial u} G_u \right|_u v \right] u$ is the transpose of the matrix representation of the linear mapping

$w \in \mathcal{X} \to \left. \frac{\partial}{\partial s} g(e, u + sv)(u, w) \right|_{s=0}$ which is zero as stated in Property A.4. Using $ad_v^{\top} G_u u = P_{G_u u} v$ with $P$ defined in (19) and identifying $D$ we obtain

$$J_{uv} = \left[ \left. \frac{\partial}{\partial u} G_u^{-1} \right|_u v \right] G_u D(u) + G_u^{-1} (P_{G_u u} + ad_u^{\top} G_u) v. \qquad (61)$$

Hence with $\bar{u}$ an equilibrium point, i.e., $D(\bar{u}) = 0$ we have

$$J_{\bar{u}} = G_{\bar{u}}^{-1} (ad_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (62)$$

**Property D.4.** The tangent space to $S_F(1)$ in $u$, denoted by $T_u S_F(1)$, is the vector space of element $v$ in $\mathcal{X}$ such that $v^{\top} G_u u = 0$.

*Proof:* We have that $S_F(1) = S_C(1/2)$, i.e., $S_F(1)$ is a level set of $C$. Thus, for $u$ in $S_F(1)$ the tangent space $T_u S_F(1)$ is the set of vector $v$ in the kernel of the differential of $C$ in $u$:

$$\left. \frac{\partial}{\partial u} C \right|_u = 0, \qquad (63)$$

$$g(e, u)(u, v) = u^{\top} G_u v = 0, \qquad (63)$$

using property A.4.

E. Details on Section IV-A

**Detailed derivation of the operators**

This section details the derivation of the expression of $ad_u, P_u, J_u, ad_u^{\top} G_u u$ when $X = SE(2)$, the group of rigid transformation of $\mathbb{R}^2$. $SE(2)$ has a canonic matrix Lie group representation with the set of matrix $M$ written as

$$M = \begin{pmatrix} \cos(\theta) & -\sin(\theta) & x \\ \sin(\theta) & \cos(\theta) & y \\ 0 & 0 & 1 \end{pmatrix}, \qquad (64)$$

with parameter $\theta, x, y$. We use parenthesis for the elements of the matrix group to distinguish them from the matrix written with brackets representing endomorphisms of the lie algebra. The identity matrix, i.e., the identity element of the group, is obtained with $\theta = 0, x = 0, y = 0$. Taking the derivative of $M$ with respect to the parameters at 0,0,0, we obtain a lie algebra element $m$ written as

$$m = \begin{pmatrix} 0 & -\omega & v_x \\ \omega & 0 & v_y \\ 0 & 0 & 0 \end{pmatrix} = v_x e_1 + v_y e_2 + \omega e_3, \qquad (65)$$

$$\text{with } e_1 = \begin{pmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix}, \quad e_2 = \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{pmatrix}, \quad e_3 = \begin{pmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix},$$
# 4 Preliminary applications of Conjecture 1

## 4.1 Geodesic turnpikes for a simple Riemannian distance

We study the existence of geodesic turnpikes for a simple left-invariant Riemannian metric structure on SE(2) proposed by Žefran in [35] and illustrated in Fig. 1(left).

Let $X = \mathrm{SE}(2)$ be the group of rigid transformation of $\mathbb{R}^2$, and $\mathcal{X} = \mathfrak{sc}(2)$ the Lie algebra of instantaneous motions. We denote by $v_x, v_y$ and $\omega$ the three coordinates of an element $u$ in the canonical basis of $\mathfrak{sc}(2)$. With a slight abuse of notations, we write $u = [v_x, v_y, \omega]$. The left-invariant Riemannian operator used by Žefran to generate the geodesic in Fig. 1(left) is $F(e, u) = \sqrt{u^\top G u}$ with

$$ G = \begin{bmatrix} \alpha & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \qquad (23) $$

where $\alpha > 1$. The matrices representation of $\mathrm{ad}_u$ and $\mathrm{P}_u$ in the canonical basis of $\mathfrak{sc}(2)$ are

$$ \mathrm{ad}_u = \begin{bmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{bmatrix} \quad \text{and} \quad \mathrm{P}_u = \begin{bmatrix} 0 & 0 & v_y \\ 0 & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix}. \qquad (24) $$

Let us recall that for a Riemannian distance $G_u = G$, so

$$ \mathrm{ad}_u^\top G_u u = \begin{bmatrix} v_y \omega \\ -\alpha v_x \omega \\ (\alpha - 1) v_x v_y \end{bmatrix}. \qquad (25) $$

An equilibrium point $\bar{u}$ satisfies $\mathrm{ad}_u^\top G_u u$. Since $\alpha - 1 \neq 0$, (25) implies that at least two components of $\bar{u}$ must be zero. Using the fact that an equilibrium point also satisfies $\bar{u}^\top G_{\bar{u}} \bar{u} = 1$ we obtain exactly six equilibrium points on $\mathbb{S}_F(1)$

$$ \bar{u}_1 = \alpha^{-1/2} e_1, \quad \bar{u}_2 = e_2, \quad \bar{u}_3 = e_3, \qquad (26) $$

$$ \bar{u}_4 = -\alpha^{-1/2} e_1, \quad \bar{u}_5 = -e_2, \quad \bar{u}_6 = -e_3, $$

where $(e_1, e_2, e_3)$ is the canonical basis of $\mathfrak{sc}(2)$. We now calculate the Jacobian for these six equilibrium points to find the saddle points. Substituting (23) and (24) in the expression of $J_u$, (19), we obtain

$$ J_u = \begin{bmatrix} 0 & \omega/\alpha & v_y/\alpha \\ -\alpha\omega & 0 & -\alpha v_x \\ (\alpha - 1)v_y & (\alpha - 1)v_x & 0 \end{bmatrix}. \qquad (27) $$

Observe that the basis $(e_1, e_2, e_3)$ is orthogonal for the inner product induced by $G$ because $G$ is diagonal. Thus, the tangent plane to $\mathbb{S}_F(1)$ in $\bar{u}_1$, denoted by $A_1$, is spanned by $e_2$ and $e_3$; and $J_{\bar{u}_1|A_1}$ is the 2 by 2 sub-matrix of $J_u$ composed
and $(e_1, e_2, e_3)$ is the canonical basis of the lie algebra. We denote by $u = [v_x, v_y, \omega]$ the column vector associated to $m$ and $u' = [v'_x, v'_y, \omega']$ the column vector associated to $m'$ another element of the lie algebra. The adjoint operator on a matrix lie group is the matrix commutator:

$$[m, m'] = mm' - m'm = \begin{pmatrix} 0 & 0 & (-\omega v'_y) - (-\omega' v_y) \\ 0 & 0 & (\omega v'_x) - (\omega' v_x) \\ 0 & 0 & 0 \end{pmatrix} = (-\omega v'_y + v_y \omega') e_1 + (\omega v'_x - v_x \omega') e_2 + 0 e_3. \quad (66)$$

Thus, the representation of the adjoint operator associated with the basis $(e_1, e_2, e_3)$ denoted by $ad_u$ must verify $ad_u u' = [-\omega v'_y + v_y \omega', \omega v'_x - v_x \omega', 0]^\top$ and we have

$$ad_u = \begin{bmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{bmatrix}. \quad (67)$$

We calculate

$$ad_u^\top u' = \begin{bmatrix} 0 & \omega & 0 \\ -\omega & 0 & 0 \\ v_y & -v_x & 0 \end{bmatrix} \begin{bmatrix} v'_x \\ v'_y \\ \omega' \end{bmatrix} = \begin{bmatrix} v'_y \omega \\ -v'_x \omega \\ -v'_y v_x + v'_x v_y \end{bmatrix} = \begin{bmatrix} 0 & 0 & v'_y \\ 0 & 0 & -v'_x \\ -v'_y & v'_x & 0 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix} = P_{u'} u, \quad (68)$$

and we obtain

$$P_u = \begin{bmatrix} 0 & 0 & v_y \\ 0 & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix}. \quad (69)$$

Now recall that

$$G = \begin{bmatrix} \alpha & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \quad (70)$$

and substituting the previous expressions in (18) and (20) we obtain

$$ad_u^\top G_u u = \begin{bmatrix} 0 & \omega & 0 \\ -\omega & 0 & 0 \\ v_y & -v_x & 0 \end{bmatrix} \begin{bmatrix} \alpha v_x \\ v_y \\ \omega \end{bmatrix} = \begin{bmatrix} v_y \omega \\ -\alpha v_x \omega \\ (\alpha - 1) v_x v_y \end{bmatrix}, \quad (71a)$$

$$u^\top G_u u = \alpha v_x^2 + v_y^2 + \omega^2 \quad (71b)$$

$$J_u = G_u^{-1} ad_u^\top G_u + G_u^{-1} P_{G_u u} = \begin{bmatrix} 0 & \omega/\alpha & 0 \\ -\alpha\omega & 0 & 0 \\ \alpha v_y & -v_x & 0 \end{bmatrix} + \begin{bmatrix} 1/\alpha & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 & 0 & v_y \\ 0 & 0 & -\alpha v_x \\ -v_y & \alpha v_x & 0 \end{bmatrix} \\ = \begin{bmatrix} 0 & \omega/\alpha & v_y/\alpha \\ -\alpha\omega & 0 & -\alpha v_x \\ (\alpha - 1)v_y & (\alpha - 1)v_x & 0 \end{bmatrix}. \quad (71c)$$

## Detailed derivation of the saddle points

In this subsection, we detail how to compute the saddle points. The equation $ad_u^\top G_u u = 0$ yields $v_x \omega = 0$, $v_y \omega = 0$ and $v_x v_y = 0$, and a solution $\bar{u}$ is a linear scaling of one of the vectors $e_1, e_2, e_3$. And $u^\top G_u u = 1$ yields six solutions:

$$\bar{u}_1 = \alpha^{-1/2} e_1, \quad \bar{u}_2 = e_2, \quad \bar{u}_3 = e_3, \quad (72)$$

$$\bar{u}_4 = -\alpha^{-1/2} e_1, \quad \bar{u}_5 = -e_2, \quad \bar{u}_6 = -e_3.$$

We have

$$\bar{u}_1^\top G e_2 = \bar{u}_1^\top G e_3 = 0, \\ A_1 = T_{\bar{u}_1} S_F(1) = \operatorname{span}(e_2, e_3), \quad (73)$$

and similar results with $\bar{u}_i, A_i$ for $i = 2..6$. Thus, we obtain

$$J_{\bar{u}_1|A_1} = \begin{bmatrix} 0 & -\alpha^{1/2} \\ \alpha^{-1/2}(\alpha - 1) & 0 \end{bmatrix}, \quad (74)$$

$$J_{\bar{u}_2|A_2} = \begin{bmatrix} 0 & 1/\alpha \\ (\alpha - 1) & 0 \end{bmatrix}, \quad J_{\bar{u}_3|A_3} = \begin{bmatrix} 0 & 1/\alpha \\ -\alpha & 0 \end{bmatrix},$$
of the two last columns and two last rows of (27). The same reasoning holds for the tangent plane $A_2$ in $\bar{u}_2$ and the tangent plane $A_3$ in $\bar{u}_3$. We obtain

$$J_{\bar{u}_1}|_{A_1} = \begin{bmatrix} 0 & -\alpha^{1/2} \\ \alpha^{-1/2}(\alpha - 1) & 0 \end{bmatrix}, \qquad (28)$$

$$J_{\bar{u}_2}|_{A_2} = \begin{bmatrix} 0 & 1/\alpha \\ (\alpha - 1) & 0 \end{bmatrix}, \quad J_{\bar{u}_3}|_{A_3} = \begin{bmatrix} 0 & 1/\alpha \\ -\alpha & 0 \end{bmatrix}.$$

The Jacobians for $\bar{u}_4$, $\bar{u}_5$ and $\bar{u}_6$ are the opposite of the ones for $\bar{u}_1$, $\bar{u}_2$ and $\bar{u}_3$. The roots of their characteristic polynomial give the eigenvalues of those Jacobians. $J_{\bar{u}_1}|_{A_1}$ and $J_{\bar{u}_3}|_{A_3}$ have purely imaginary eigenvalues because $\alpha - 1 > 0$ and $\bar{u}_1$, $\bar{u}_3$, $\bar{u}_4$ and $\bar{u}_6$ are not saddle points. On the other hand, $J_{\bar{u}_2}|_{A_2}$ has eigenvalues $\pm\sqrt{(\alpha - 1)/\alpha}$, with one positive and one negative, making $\bar{u}_2 = e_2$ and $\bar{u}_5 = -e_2$ the only saddle points. These vectors are precisely the translations along the y axis of the local frame, i.e., parallel to the short side of the rectangle. Conjecture 1 implies that the left-invariant vector field $V_x = \dot{L}_x e_2$ is an exponential geodesic turnpike. This matches the turnpike behavior observed in Fig. 3 (left), suggesting the potential validity of the conjecture.

This section has demonstrated a prototypical strategy for analytically identifying the saddle points of the geodesic dynamics. In the following subsection, we present a numerical strategy for identifying the saddle points for more general distances and a procedure for using them in the computation of geodesics.

## 4.2 Geodesic turnpike for the minimum swept-volume distance

We propose a numerical procedure to accelerate the geodesic computation associated with $F$. First, an offline algorithm identifies the saddle points of the Lie algebra dynamics. Assuming Conjecture 1 holds, these points are geodesic turnpikes. Second, we propose an initialization strategy using the candidate geodesic turnpikes in a geodesic solver that significantly accelerates its convergence. It is important to note that the first part of the computation is done only once for a given Finsler operator $F$ and not for every geodesic computation. Thus, identifying candidate geodesic turnpikes benefits motion planners such as RRT and its variants [21]. We assume that $F$ is a left-invariant Finsler operator known in closed form as a function of $u$, an element of the Lie algebra. This is the case for the minimum swept-volume distance for a moving rod [23]. Note that Figures 1, 2 and 3 illustrate geodesics computed with this operator. We use this example throughout the section and use the geodesic turnpikes to compute the same geodesics significantly faster.

**Saddle points identification.** We implement the symbolic expression of $F(e, u)$, $ad_u$ and $P_u$ in the symbolic automatic differentiation framework CasADi [1]. We use automatic differentiation to compute symbolic expressions for the Hessian of $F$ and, in turn, for $G_u$, $D(u)$, and $J_u$. We use the non-linear root finding engine implemented in CasADi with multiple initializations to identify the roots of the system (18), i.e., the equilibrium points $\bar{u}_i$, and thus the numerical matrices $J_i = J_{\bar{u}_i}$. We use a sequence of matrix factorization routines to compute the
and $J_{\bar{u}_4}|_{A_4} = -J_{\bar{u}_1}|_{A_1}$, $J_{\bar{u}_5}|_{A_5} = -J_{\bar{u}_2}|_{A_2}$, $J_{\bar{u}_6}|_{A_6} = -J_{\bar{u}_3}|_{A_3}$. Now, denoting $P_i(x) = \det(J_{\bar{u}_i}|_{A_i} - xI_2)$ the characteristic polynomial, we calculate

$$P_1(x) = x^2 + (\alpha - 1), \quad P_2(x) = x^2 - \frac{1}{\alpha}(\alpha - 1), \quad P_3(x) = x^2 + 1, \qquad (75)$$

and $P_4 = P_1$, $P_5 = P_2$, $P_6 = P_3$. Only $P_2$ has real roots $\pm\sqrt{(\alpha - 1)/\alpha}$ and only $\bar{u}_2, \bar{u}_5$ are saddle points.

F. Details on Section IV-B

<caption>**Algorithm 1** Saddle points computation (see Appendix F)</caption>

Require: $u$

Require: $A: u \to F(e, u), B: u \to ad_u, P: u \to P_u$
1: $G \leftarrow hessian(A)$
2: $D \leftarrow B^\top @ G @ u$
3: $C \leftarrow u^\top @ G @ u - 1$
4: $c \leftarrow []$
5: **while** $l \le L$ **do**
6: &nbsp;&nbsp;&nbsp;&nbsp; $r \leftarrow root([D, C], gauss(0, 1, n))$
7: &nbsp;&nbsp;&nbsp;&nbsp; $M \leftarrow apply(G, r)$
8: &nbsp;&nbsp;&nbsp;&nbsp; $J \leftarrow M^{-1}(apply(B, r) @ M + apply(P, M @ r))$
9: &nbsp;&nbsp;&nbsp;&nbsp; $A \leftarrow cholesky(M)$
10: &nbsp;&nbsp;&nbsp;&nbsp; $A \leftarrow A^{-\top} @ gramm(A^{\top} @ complete(r))$
11: &nbsp;&nbsp;&nbsp;&nbsp; $A \leftarrow pinv(A[1 :]) @ J @ A[1 :]$
12: &nbsp;&nbsp;&nbsp;&nbsp; $s \leftarrow diagonalization(A)$
13: &nbsp;&nbsp;&nbsp;&nbsp; $c \leftarrow addif(c, s)$
14: &nbsp;&nbsp;&nbsp;&nbsp; $l \leftarrow l + 1$
15: **end while**
16: **return** $c$

▷ A symbolic variable of dimension n
▷ G is a symbolic expression
▷ D is a symbolic expression
▷ C is a symbolic expression
▷ Multiple initialization
▷ eq is a numerical value

* @ denotes the matrix multiplication,

* gauss(0, 1, n) is a random vector of size n following a Gaussian distribution with 0 mean and 1 standard deviation,

* root([C, D], u0) is a non-linear root solver for the combined expressions C and D in the symbolic variable u and taking an initial guess u0,

* complete(r) return an invertible square matrix of size n with the first column being r,

* cholesky(M) is the Cholesky decomposition of a symmetric positive definite matrix M,

* gramm(M) return the matrix Q of the QR-decomposition. It corresponds to the matrix where columns the basis obtained with the Gram-Schmidt procedure,

* $A = A^{-\top} gramm(A^{\top} M)$ is a trick to compute the basis obtained with the Gram-Schmidt procedure using the inner product $u, v \to u^\top G v$ with $G = AA^\top$ instead of the canonical one,

* $A[1:]$ is a basis of the tangent space to $S_F(1)$ in $e$,

* pinv(A[1 :]) is the Moore-Penrose pseudo inverse of $A[1 :]$ it correspond to the inverse of $A[1 :]|_{\mathrm{Im}(A[1 :])}$,

* Using $A[1:]$ and its pseudo-inverse, we construct the square matrix $J|_{T,S_F(1)}$ of size $n-1$ corresponding to the matrix $J$ express in the base $A[1:]$.
spectrum of the Jacobian restricted to the tangent plane of $S_F(1)$ in $\bar{u}_i$. We only
keep the $\bar{u}_i$ that are saddle points.

When applied to the minimum swept-volume distance of a moving rod, this
algorithm yields exactly two turnpikes: the instantaneous translation parallel to
the long side of the rod in both directions. This is consistent with the turnpike
behavior observed in Fig. 1(right), hinting again at the potential validity of
Conjecture 1.

**Initialization of the geodesic solver.** The geodesics in [23] are computed using a discretized version of the energy problem (16) with optimization variables $u_j$ with $j = 0..N$. To leverage the identified turnpike, we adopt a two-stage approach. First, for each saddle point $\bar{u}_i$, we initialize $u_j = \bar{u}_i$ for all $j = 0..N$, and we solve the minimization problem with $u_j$ fixed for $j = 1..N-1$ and only $u_0$ and $u_N$ as optimization variables. Second, we keep the saddle point $\bar{u}_i$ with the lowest cost error and solve the associated minimization problem a second time with all $u_j$ as optimization variables.

On an Intel(R) Core(TM) i7-8750H CPU, averaging over 100 cases, the first optimization problem takes $13 \pm 2$ ms to converge for each turnpike, and the second optimization problem takes around $110 \pm 14$ ms. In comparison, the standard optimization used in [23] takes 1.5 s to converge.

Overall, geodesic turnpike initialization improves the computation time of geodesic computation by an order of magnitude over the standard geodesic solver used in [23].

# 5 Conclusion

This paper has established a theoretical framework for exploring geodesic turn-
pike properties in metric structures on configuration spaces. We have introduced
the concept of exponential geodesic turnpikes and proposed a conjecture pro-
viding a sufficient condition for a left-invariant Finsler operator to exhibit the
geodesic turnpike property. The examples in Section 4 support the conjecture.
Additionally, using a geodesic turnpike as an initialization for the geodesic solver
of the swept-volume distance [23] offers significant computational acceleration.
Future work will aim to complete the proof of Conjecture 1 and extend the work
to partial invariance and higher order dynamics for kinodynamic planning. Fur-
ther research will also include a deeper exploration of the implications of the
turnpike property on various robotics applications.

**Acknowledgments.** This work was funded in part by the French government under management of Agence Nationale de la Recherche as part of the “Investissements d’avenir” program, reference ANR-19-P3IA-0001 (PRAIRIE 3IA Institute). JP was supported in part by the Louis Vuitton/ENS-PSL chair in artificial intelligence and a Global Distinguished Professorship at the Courant Institute of Mathematical Sciences and the Center for Data Science at New York University.
References

1. Andersson, J.A.E., Gillis, J., Horn, G., Rawlings, J.B., Diehl, M.: Casadi: a software framework for nonlinear optimization and optimal control. Mathematical Programming Computation 11(1), 1–36 (2018). https://doi.org/10.1007/s12532-018-0139-4

2. Arnol'd, V.I.: Mathematical methods of classical mechanics. Graduate Texts in Mathematics, Springer, New York, NY, 2 edn. (1989). https://doi.org/10.1007/978-1-4757-1693-1

3. Berger, M.: Geometry I. Universitext, Springer Berlin Heidelberg (2009)

4. Bucataru, I., Miron, R.: Finsler-Lagrange Geometry: Applications to Dynamical Systems. Editura Academiei Române (2007)

5. Celledoni, E., Marthinsen, H., Owren, B.: An introduction to lie group integrators – basics, new developments and applications. Journal of Computational Physics 257, 1040–1061 (2014). https://doi.org/10.1016/j.jcp.2012.12.031, physics-compatible numerical methods

6. Chirikjian, G.S.: Stochastic Models, Information Theory and Lie Groups Volume 2, Analytic Methods and Modern Applications. Birkhäuser Boston (2012). https://doi.org/10.1007/978-0-8176-4944-9

7. Chirikjian, G.S.: Partial bi-invariance of SE(3) metrics. Journal of Computing and Information Science in Engineering 15(1) (2015). https://doi.org/10.1115/1.4028941

8. Faulwasser, T., Grüne, L.: Chapter 11 - turnpike properties in optimal control: An overview of discrete-time and continuous-time results. In: Trélat, E., Zuazua, E. (eds.) Numerical Control: Part A, Handbook of Numerical Analysis, vol. 23, pp. 367–400. Elsevier (2022). https://doi.org/10.1016/bs.hna.2021.12.011

9. Featherstone, R.: Rigid body dynamics algorithms. Springer, New York (2008)

10. Frazzoli, E., Dahleh, M.A., Feron, E.: Maneuver-based motion planning for non-linear systems with symmetries. IEEE Transactions on Robotics 21(6), 1077–1091 (2005). https://doi.org/10.1109/TRO.2005.852260

11. Geshkovski, B., Zuazua, E.: Turnpike in optimal control of pdes, resnets, and beyond. Acta Numerica 31, 135–263 (2022). https://doi.org/10.1017/S0962492922000046

12. Jallet, W., Bambade, A., Mansard, N., Carpentier, J.: Constrained differential dynamic programming: A primal-dual augmented lagrangian approach. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 13371–13378 (2022). https://doi.org/10.1109/IR0S47612.2022.9981586

13. Jaquier, N., Asfour, T.: Riemannian geometry as a unifying theory for robot motion learning and control. In: Billard, A., Asfour, T., Khatib, O. (eds.) Robotics Research. pp. 395–403. Springer Nature Switzerland, Cham (2023)

14. Jaquier, N., Rozo, L., Asfour, T.: Unraveling the single tangent space fallacy: An analysis and clarification for applying riemannian geometry in robot learning. ArXiv abs/2310.07902 (2023), https://api.semanticscholar.org/CorpusID: 263908813

15. Kierzenka, J., Shampine, L.F.: A bvp solver based on residual control and the maltab pse. ACM Transaction on Mathematical Software 27(3), 299–316 (2001). https://doi.org/10.1145/502800.502801

16. Kjer, V.R.: Lie group integrators for cotangent bundles of lie groups and their application to systems of dipolar soft spheres. Oslo, Norway (2022), https://ntnuopen.ntnu.no/ntnu-xmlui/handle/11250/2450817, under supervision of Brynjulf, Owren, IMF
17. Klein, H., Jaquier, N., Meixner, A., Asfour, T.: A riemannian take on human motion analysis and retargeting. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 5210–5217. IEEE (2022). https://doi.org/10.1109/IROS47612.2022.9982127

18. Kuehn, C.: Multiple Time Scale Dynamics. Applied Mathematical Sciences, Springer International Publishing (2016)

19. Kuffner, J.J.: Effective sampling and distance metrics for 3d rigid body path planning. IEEE International Conference on Robotics and Automation (ICRA) 2004(4), 3993–3998 (2004). https://doi.org/10.1109/robot.2004.1308895

20. Latifi, D., Razavi, A.: Bi-invariant finsler metrics on lie groups. Australian Journal of Basic and Applied Science 5(12), 507–511 (2011)

21. LaValle, S.M.: Planning Algorithms. Cambridge University Press (2006)

22. McKenzie, L.W.: Turnpike theory. Econometrica 44(5), 841–865 (1976)

23. de Mont-Marin, Y., Ponce, J., Laumond, J.P.: A minimum swept-volume metric structure for configuration space. In: IEEE International Conference on Robotics and Automation (ICRA). pp. 3686–3692 (2023). https://doi.org/10.1109/ICRA48891.2023.10161367

24. Onishchik, A.L., Vinberg, E.B.: Lie Groups and Algebraic Groups. Springer Berlin Heidelberg (1990). https://doi.org/10.1007/978-3-642-74334-4

25. Paiola, L., Grioli, G., Bicchi, A.: On the evaluation of collision probability along a path (2023)

26. Palmieri, L., Arras, K.: Distance metric learning for rrt-based motion planning with constant-time inference. IEEE International Conference on Robotics and Automation (ICRA) 2015 (2015). https://doi.org/10.1109/ICRA.2015.7139246

27. Potra, F.A., Wright, S.J.: Interior-point methods. Journal of Computational and Applied Mathematics 124(1), 281–302 (2000). https://doi.org/10.1016/S0377-0427(00)00433-7, numerical Analysis 2000. Vol. IV: Optimization and Nonlinear Equations

28. Ratliff, N.D., Van Wyk, K., Xie, M., Li, A., Rana, M.A.: Generalized nonlinear and finsler geometry for robotics. In: IEEE International Conference on Robotics and Automation (ICRA). pp. 10206–10212 (2021). https://doi.org/10.1109/ICRA48506.2021.9561543

29. Ratliff, N.D., Zucker, M., Bagnell, J.A., Srinivasa, S.S.: Chomp: Gradient optimization techniques for efficient motion planning. In: IEEE International Conference on Robotics and Automation (ICRA). IEEE (2009). https://doi.org/10.1109/robot.2009.5152817

30. Sakamoto, N., Zuazua, E.: The turnpike property in nonlinear optimal control—a geometric approach. Automatica 134, 109939 (2021). https://doi.org/10.1016/j.automatica.2021.109939

31. Solà, J., Deray, J., Atchuthan, D.: A micro lie theory for state estimation in robotics. ArXiv abs/1812.01537 (2018), https://dblp.org/rec/journals/corr/abs-1812-01537.bib

32. Trélat, E., Zuazua, E.: The turnpike property in finite-dimensional nonlinear optimal control. Journal of Differential Equations 258(1), 81–114 (2015). https://doi.org/10.1016/j.jde.2014.09.005

33. Van Wyk, K., Xie, M., Li, A., Rana, M.A., Babich, B., Peele, B., Wan, Q., Akinola, I., Sundaralingam, B., Fox, D., Boots, B., Ratliff, N.D.: Geometric fabrics: Generalizing classical mechanics to capture the physics of behavior. IEEE Robotics and Automation Letters 7(2), 3202–3209 (2022). https://doi.org/10.1109/LRA.2022.3143311
34. Žefran, M., Kumar, V., Croke, C.B.: Choice of riemannian metrics for rigid body kinematics. International Design Engineering Technical Conferences and Computers and Information in Engineering Conference (IDETC/CIE) **2B-1996**(3), 1–23 (1996). https://doi.org/10.1115/96-DETC/MECH-1148

35. Žefran, M., Kumar, V., Croke, C.B.: On the generation of smooth three-dimensional rigid body motions. IEEE Transactions on Robotics and Automation **14**(4), 576–589 (1998). https://doi.org/10.1109/70.704225

36. Zhang, L., Kim, Y.J., Manocha, D.: C-DIST. In: ACM symposium on Solid and physical modeling (SPM). ACM Press (2007). https://doi.org/10.1145/1236246.1236270
# Appendix

## A Proof of Finsler operator properties

We will prove five properties of the Finslerian operator $F$ and the associated bilinear form $g$ as defined in (10). The ones mentioned in Section 2 are properties 1, 2, 5, and the remaining ones are useful for the proof of Proposition 2 in C.

**Property 1.** For any $x$ in $X$ and $v$ in $T_x X$ we have $F^2(x, v) = g(x, v)(v, v)$.

*Proof.* Using the definition

$$ 
\begin{aligned}
g(x, v)(v, v) &= \frac{1}{2} \left. \frac{\partial^2}{\partial s \partial t} F^2(x, v + sv + tv) \right|_{s=0, t=0} \\
&= \frac{\partial^2}{\partial s \partial t} \left. \frac{1}{2} (1 + s + t)^2 \right|_{s=0, t=0} F^2(x, v) \\
&= F^2(x, v).
\end{aligned} 
\quad (29) $$

**Property 2.** For any $x$ in $X$, $v$ in $T_x X$ and $\lambda > 0$ we have

$$ g(x, \lambda v) = g(x, v). \quad (30) $$

*Proof.* For any $a$ and $b$ in $T_x X$ we have

$$ 
\begin{aligned}
g(x, \lambda v)(a, b) &= \frac{1}{2} \lim_{s \to 0^+, t \to 0^+} \frac{F^2(x, \lambda v + sa + tb)}{st} \\
&= \frac{1}{2} \lim_{s \to 0^+, t \to 0^+} \frac{\lambda^2 F^2(x, v + (s/\lambda)a + (t/\lambda)b)}{\lambda^2(s/\lambda)(t/\lambda)} \\
&= \frac{1}{2} \lim_{s' \to 0^+, t' \to 0^+} \frac{F^2(x, v + s'a + t'b)}{s't'} \\
&= g(x, v)(a, b),
\end{aligned} 
\quad (31) $$

where we use the changes of variables $s' = s/\lambda$ and $t' = t/\lambda$.

**Property 3.** We denote by $C$ the tri-linear symmetric form called the Cartan tensor [4] defined for any $a, b, c$ in $T_x X$ as

$$ C(x, v)(a, b, c) = \frac{1}{2} \left. \frac{\partial^3}{\partial s \partial t \partial u} F^2(x, v + sa + tb + uc) \right|_{s=0, t=0, u=0} . \quad (32) $$

For any $x$ in $X$ and $v, a, b$ in $T_x X$ we have

$$ C(x, v)(v, a, b) = 0. \quad (33) $$
Proof.

$$
\begin{align*}
C(x, v)(v, a, b) &= \frac{1}{2} \left. \frac{\partial^3}{\partial s \partial t \partial u} F^2(x, v + sv + ta + ub) \right|_{s=0, t=0, u=0} \\
&= \frac{\partial}{\partial s} \left[ \frac{1}{2} \left. \frac{\partial^2}{\partial t \partial u} F^2(x, (1+s)v + ta + ub) \right|_{t=0, u=0} \right]_{s=0} \\
&= \frac{\partial}{\partial s} g(x, (1+s)v)(a, b) \Big|_{s=0} \\
&= \frac{\partial}{\partial s} g(x, v)(a, b) \Big|_{s=0} \qquad = 0,
\end{align*}
$$

where we used the property 2 between the second to last and last equality.

Property 4. For any $x$ in $X$ and $v, a, b$ in $T_x X$ we have

$$
\left. \frac{\partial}{\partial s} g(x, v + sa)(v, b) \right|_{s=0} = 0, \qquad (34)
$$

Proof. We have $\frac{\partial}{\partial s} g(x, v + sa)(v, b)|_{s=0} = C(x, v)(v, a, b)$ hence the result using property 3.

Property 5. For any $x$ in $X$ and $v, w$ in $T_x X$ we have

$$
\frac{1}{2} \left. \frac{\partial}{\partial s} F^2(x, v + sw) \right|_{s=0} = g(x, v)(v, w). \qquad (35)
$$

Proof. Using the property 1, we have

$$
\begin{align}
F^2(x, v + sw) &= g(x, v + sw)(v + sw, v + sw) \nonumber \\
&= g(x, v + sw)(v, v) + 2sg(x, v + sw)(v, w) + s^2 g(x, v + sw)(w, w). \tag{36}
\end{align}
$$

Taking the derivative with respect to $s$, we have

$$
\frac{\partial}{\partial s} F^2(x, v + sw) = \frac{\partial}{\partial s} g(x, v + sw)(v, v) \quad (37a)
$$

$$
+ 2g(x, v + sw)(v, w) \qquad (37b)
$$

$$
+ 2s \frac{\partial}{\partial s} g(x, v + sw)(v, w) \qquad (37c)
$$

$$
+ 2sg(x, v + sw)(w, w) \qquad (37d)
$$

$$
+ s^2 \frac{\partial}{\partial s} g(x, v + sw)(w, w). \qquad (37e)
$$

When $s = 0$ (37c), (37d), (37e) vanishes and using property 4 the term (37a) is
equal to 0. Only (37b) is non zero, and we obtain

$$
\left. \frac{\partial}{\partial s} F^2(x, v + sw) \right|_{s=0} = g(x, v)(v, w) \qquad (38)
$$

Hence, the final result.
In the rest of the appendices, given a map $f: \mathcal{X} \times Y \to Z$ with $Y$ a manifold and $Z$ a vector space, for any $u$ in $\mathcal{X}$, $y$ in $Y$ we denote by $\frac{\partial}{\partial u}|_{u,y}$ the linear map from $\mathcal{X}$ onto $Z$ defined for any $v$ in $\mathcal{X}$ as

$$ \frac{\partial}{\partial u} f \Big|_{u,y} v = \frac{\partial}{\partial s} f(u + sv, y) \Big|_{s=0} . \qquad (39) $$

## B Proof of Proposition 1

**Proposition.** For any $\epsilon > 0$, there exists $\tau > 0$ such that for any curve $\gamma^\dagger$ parametrized by arclength and representing a geodesic, if $I_\epsilon$ denote the set of $t$ such that $F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \ge \epsilon$ we have

$$ L(\gamma^\dagger|_{I_\epsilon}) \le \tau. \qquad (14) $$

*Proof.* Using the notation of Definition 1, let $\gamma^\dagger$ a parametric curve representing a geodesic joining $x_0$ to $x_1$ with constant velocity and of length, $L = d(x_0, x_1)$ verifying Definition 1. For any $\epsilon > 0$ we chose $\tau = (2/\sigma) \ln(2K/\epsilon)$, which is independent of the length $L$ and we verify:

- for any $t > \tau/2$ we have $e^{-\sigma t} < e^{-\sigma \frac{\tau}{2}} = e^{-\ln(2K/\epsilon)} = \epsilon/(2K)$,

- for any $t < L - \tau/2$ we have $e^{-\sigma(L-t)} < e^{-\sigma(L-(L-\frac{\tau}{2}))} = e^{-\sigma \frac{\tau}{2}} = \epsilon/(2K)$.

Summing up, we have

$$ (\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) \le K (e^{-\sigma t} + e^{-\sigma(L-t)}) \le \epsilon, \qquad (40) $$

$$ \forall t \in [\tau/2, L] \cap [0, L - \tau/2]. $$

Thus, if $I_\epsilon$ denote the interval of $t$ such that $F(\gamma^\dagger(t), \dot{\gamma}^\dagger(t) - V_{\gamma^\dagger(t)}) > \epsilon$ we have

$$ I_{\epsilon} \subset [0, L] \setminus ([\tau/2, L] \cap [0, L - \tau/2]) \subset [0, \tau/2] \cup [L - \tau/2, L], \qquad (41) $$

and $\mu(I_\epsilon) \le \mu([0, \tau/2]) + \mu([L, \tau/2]) = \tau$, hence the result.

## C Proof of Proposition 2

**Proposition.** If $F$ is a left-invariant Finsler distance on the Lie group $X$, then the geodesic equation associated with (16) can be written in some basis of the Lie algebra $\mathcal{X}$ as:

$$ \dot{\gamma}(t) = \tilde{L}_{\gamma(t)}u(t), \qquad (17) $$

$$ \dot{u}(t) = G_{u(t)}^{-1} \mathrm{ad}_{u(t)}^{\top} G_{u(t)} u(t), $$

where $G_u$ denotes the matrix representation of $g(e, u)$ in the chosen basis, $\mathrm{ad}_u$ denotes the matrix representation in that basis of the Lie algebra adjoint operator $\mathrm{ad}_u$, which captures the action of $u$ on the rest of the algebra via the Lie bracket on $\mathcal{X}$. The solutions represent local geodesics and do not depend on the choice of basis.
*Proof.* The Hamiltonian of the variational problem (16) reads

$$H(x, p, u) = \langle p, \tilde{L}_x u \rangle_x - C(u), \qquad (42)$$

where and $u \in \mathcal{X}$ and $(x, p) \in T^*X$ the co-tangent of $X$, i.e., $x \in X$ and $p$ is a linear form on $T_x X$. $\langle p, v \rangle_x$ is the dual bracket that takes a covector and a vector and returns the scalar obtained by applying the covector on the vector on the tangent space in $x$. The cotangent bundle $T^*X$ of a Lie group $X$ is a trivial bundle$^5$, and the Hamiltonian equations for all $t$ are:

$$\dot{x}(t) = \left. \frac{\partial H}{\partial p} \right|_{x(t),p(t),u(t)} = \tilde{L}_{x(t)} u(t), \qquad (43a)$$

$$\dot{p}(t) = -\left. \frac{\partial H}{\partial x} \right|_{x(t),p(t),u(t)} = -\left. \frac{\partial \langle p, \tilde{L}_x u \rangle_x}{\partial x} \right|_{x(t),p(t),u(t)}, \qquad (43b)$$

$$0 = \left. \frac{\partial H}{\partial u} \right|_{x(t),p(t),u(t)} = \left. \frac{\partial \langle p, \tilde{L}_x u \rangle_x}{\partial u} \right|_{x(t),p(t),u(t)} - \left. \frac{\partial C}{\partial u} \right|_{u(t)}. \qquad (43c)$$

We use the left trivialization of the cotangent bundle described below to simplify the Hamiltonian equations. For $(x, p)$ in $T^*X$, we denote by $p_b = p \circ \tilde{L}_x \in \mathcal{X}^*$ the left trivialization of the covector $p$. The cotangent bundle, $T^*X$, is equipped with a natural group structure induced by the group structure of $X$ (see [24,16] for details) and the mapping

$$\phi : (x, p) \in T^*X \mapsto (x, p_b) \in X \times \mathcal{X}^*, \qquad (44)$$

is a Lie group isomorphism where $X \times \mathcal{X}^*$ is a lie-group semi-direct product with the group law

$$(x, p_b) \circ (y, q_b) = (x \circ y, \mathrm{Ad}_y^*(p_b) + q_b), \qquad (45)$$

where $\mathrm{Ad}_y^*$ is the co-adjoint representation of $y \in X$ (see [24] for details).

First, we have $\langle p, \tilde{L}_x u \rangle_x = \langle p \circ \tilde{L}_x, u \rangle_e = \langle p_b, u \rangle_e$ and the third Hamiltonian equation (43c) is equivalent to:

$$0 = \left. \frac{\partial \langle p_b, u \rangle_e}{\partial u} \right|_{x(t),p(t),u(t)} - \left. \frac{\partial C}{\partial u} \right|_{u(t)},$$

$$p_b(t) = \left. \frac{\partial C}{\partial u} \right|_{u(t)}. \qquad (46)$$

<footnote>$^5$ its topology is the one of a Cartesian product between the base space $X$ and a fiber</footnote>
Second, we denote by $\pi : (x, p_b) \in X \times \mathcal{X}^* \to p_b$ the projection, and we use the left Jacobian, as coined in [31], for $v$ in $\mathcal{X}$ we have

$$ 
\begin{aligned}
\frac{\partial \pi}{\partial x}(\tilde{L}_x v) &= \left. \frac{\partial}{\partial s} \pi((x, p_b) \circ (\exp(sv), 0)) \right|_{s=0} \\
\frac{\partial p_b}{\partial x}(\tilde{L}_x v) &= \left. \frac{\partial}{\partial s} \mathrm{Ad}^*_{\exp(sv)}(p_b) + 0 \right|_{s=0} \\
&= \left. \frac{\partial}{\partial s} \exp(s \mathrm{ad}^*_v) \right|_{s=0} (p_b) \\
&= \mathrm{ad}^*_v(p_b).
\end{aligned}
\quad (47) $$

We denote by $\psi$ the linear form on $\mathcal{X}$ defined as

$$ \psi = - \frac{\partial \langle p_b, u \rangle_e}{\partial x} \circ \tilde{L}_x, \quad (48) $$

for any $v \in \mathcal{X}$ we have

$$ 
\begin{aligned}
\psi(v) &= - \left. \frac{\partial p_b}{\partial x}(\tilde{L}_x v), u \right|_e \\
&= - \left. \frac{\partial}{\partial s} \mathrm{ad}^*_v(p_b), u \right|_e \\
&= - \left. \frac{\partial}{\partial s} p_b, \mathrm{ad}_v(u) \right|_e \\
&= \left. \frac{\partial}{\partial s} p_b, \mathrm{ad}_u(v) \right|_e \\
&= \left. \frac{\partial}{\partial s} \mathrm{ad}^*_u(p_b), v \right|_e,
\end{aligned}
\quad (49) $$

and we conclude that $\psi = \mathrm{ad}^*_u(p_b)$. Composing the second Hamiltonian equation (43b) on both side with $\tilde{L}_x$, the second Hamiltonian equation is equivalent to

$$ 
\begin{aligned}
\dot{p}_b &= - \left. \frac{\partial \langle p, \tilde{L}_x u \rangle_x}{\partial x} \circ \tilde{L}_x \right. \\
&= - \left. \frac{\partial \langle p_b, xu \rangle_e}{\partial x} \circ \tilde{L}_x \right. \\
&= \left. \mathrm{ad}^*_u(p_b) \right).
\end{aligned}
\quad (50) $$

Third, substituting (46) in (50), the Hamiltonian equations (43a), (43b), (43c) are rewritten as

$$ \dot{x}(t) = \tilde{L}_{x(t)}u(t), \quad (51a) $$

$$ \left. \frac{\partial^2 C}{\partial^2 u} \right|_{u(t)} \dot{u}(t) = \mathrm{ad}^*_{u(t)} \left( \left. \frac{\partial C}{\partial u} \right|_{u(t)} \right). \quad (51b) $$

Note that the right and left sides are in $\mathcal{X}^*$. Note also that the previous construction is valid for any invariant cost $C$. Now, using the definition of $g$ in (10) and the property 5, we have

$$ \left. \frac{\partial^2 C}{\partial^2 u} \right|_u (v, w) = g(e, u)(v, w), \quad (52a) $$

$$ \left. \frac{\partial C}{\partial u} \right|_u (v) = g(e, u)(u, v). \quad (52b) $$
Using a basis of $\mathcal{X}$ and its dual basis, we identify elements of $\mathcal{X}^*$ and $\mathcal{X}$. $G_u$ is the matrix representation in this basis of $\frac{\partial^2 C}{\partial^2 u}|_{u(t)}$, and $ad^\top$ is the basis representation of $ad^*$. Finally, we have the geodesic equation:

$$ \dot{x}(t) = \tilde{L}_{x(t)}u(t), \qquad (53a) $$

$$ G_{u(t)}\dot{u}(t) = ad_{u(t)}^\top G_{u(t)}u(t), \qquad (53b) $$

hence the result.

## D Proof of Proposition 3

**Proposition.** An element $\bar{u}$ in the lie algebra $\mathcal{X}$ is an equilibrium point of the dynamics $D$ on $\mathbb{S}_F(1)$ if and only if

$$ ad_{\bar{u}}^\top G_{\bar{u}}\bar{u} = 0, \quad \bar{u}^\top G_{\bar{u}}\bar{u} = 1, \qquad (18) $$

In addition, the tangent space $T_{\bar{u}}\mathbb{S}_F(1)$ tangent to $\mathbb{S}_F(1)$ in $\bar{u}$ is the set of elements $v$ in $\mathcal{X}$ such that $v^\top G_{\bar{u}}\bar{u} = 0$ and the Jacobian of the dynamics around $\bar{u}$ is

$$ J_{\bar{u}} = G_{\bar{u}}^{-1}(ad_{\bar{u}}^\top G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (19) $$

where $P_{G_{\bar{u}}}$ the unique matrix defined by

$$ \forall v \in \mathcal{X}, \quad P_{G_{\bar{u}}} v = ad_v^\top G_{\bar{u}}. \qquad (20) $$

We will prove the proposition through a series of simple properties. We recall the expression of the dynamics:

$$ D(u) = G_u^{-1} ad_u^\top G_u u. \qquad (54) $$

**Property 6.** If $u : I \to \mathcal{X}$ is a solution of $\dot{u} = D(u)$ then

$$ \frac{d}{dt}C(u) = 0. \qquad (55) $$

*Proof.* A solution of the dynamics $\dot{u} = D(u)$ verifies

$$ u^\top G_u \dot{u} = u^\top ad_u^\top G_u u = (ad_u u)^\top G_u u = 0, \qquad (56) $$

because the mapping $u, v \mapsto ad_u v$ is antisymmetric and we have $ad_u u = 0$. Now, using (35) we obtain

$$ \frac{d}{dt}C(u) = \frac{\partial}{\partial u}C\bigg|_u \dot{u} = g(e, u)(u, \dot{u}) = u^\top G_u \dot{u} = 0, \qquad (57) $$

hence the result.
The dynamics solutions have constant cost C and, in turn, constant Finsler operator $F(e, u)$. We restrict the dynamics domain to $\mathbb{S}_F(1)$, the set of all u such that $F(e, u) = 1$.

**Property 7.** A vector $\bar{u}$ is an equilibrium point of the dynamics $D$ defined on $\mathbb{S}_F(1)$ if:

$$ad_{\bar{u}}^{\top} G_{\bar{u}} \bar{u} = 0, \quad \bar{u}^{\top} G_{\bar{u}} \bar{u} = 1, \qquad (18)$$

*Proof.* First, an equilibrium point verifies $D(\bar{u}) = 0$. With (54) and canceling the invertible matrix $G_u$, we obtain the left part of (18). Second, using property 1, $\mathbb{S}_F(1)$ is exactly the set of u such that $g(e, u)(u, u) = u^{\top} G_u u = 1$ hence the result.

**Property 8.** Let $J_u$ denote the Jacobian of the dynamics $D$ taken in $u$. If $\bar{u}$ is an equilibrium point of the dynamics, then we have

$$J_{\bar{u}} = G_{\bar{u}}^{-1}(ad_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (19)$$

*Proof.* Taking the derivative of $D$ defined in (54), we have

$$J_u v = \left[ \left. \frac{\partial}{\partial u} G_u^{-1} \right|_u v \right] ad_u^{\top} G_u u \qquad (58a)$$

$$+ G_u^{-1} ad_v^{\top} G_u u \qquad (58b)$$

$$+ G_u^{-1} ad_u^{\top} \left[ \left. \frac{\partial}{\partial u} G_u \right|_u v \right] u \qquad (58c)$$

$$+ G_u^{-1} ad_u^{\top} G_u v. \qquad (58d)$$

The term (58c) is zero because $[\partial_{\bar{u}} G_u]_u v$ is the transpose of the matrix representation of the linear mapping

$w \in \mathcal{X} \to \frac{\partial}{\partial s} g(e, u + sv)(u, w)|_{s=0}$ which is zero as stated in Property 4. Using $ad_v^{\top} G_u u = P_{G_u u} v$ with $P$ defined in (20) and identifying $D$ we obtain

$$J_u v = \left[ \left. \frac{\partial}{\partial u} G_u^{-1} \right|_u v \right] G_u D(u) + G_u^{-1} (P_{G_u u} + ad_u^{\top} G_u) v. \qquad (59)$$

Hence with $\bar{u}$ an equilibrium point, i.e., $D(\bar{u}) = 0$ we have

$$J_{\bar{u}} = G_{\bar{u}}^{-1}(ad_{\bar{u}}^{\top} G_{\bar{u}} + P_{G_{\bar{u}}\bar{u}}). \qquad (60)$$

**Property 9.** The tangent space to $\mathbb{S}_F(1)$ in $u$, denoted by $T_u \mathbb{S}_F(1)$, is the vector space of element $v$ in $\mathcal{X}$ such that $v^{\top} G_u u = 0$.

*Proof.* We have that $\mathbb{S}_F(1) = S_C(1/2)$, i.e., $\mathbb{S}_F(1)$ is a level set of $C$. Thus, for $u$ in $\mathbb{S}_F(1)$ the tangent space $T_u \mathbb{S}_F(1)$ is the set of vector $v$ in the kernel of the differential of $C$ in $u$:

$$\left. \frac{\partial}{\partial u} C \right|_u v = 0,$$

$$g(e, u)(u, v) = u^{\top} G_u v = 0, \qquad (61)$$

using property 4.
E Details on Section 4.1

Detailed derivation of the operators

This section details the derivation of the expression of ad$_u$, P$_u$, J$_u$, ad$_u^\top$ G$_u$ u when X = SE(2), the group of rigid transformation of R$^2$. SE(2) has a canonic matrix Lie group representation with the set of matrix M written as

$$
M = \begin{pmatrix} \cos(\theta) & -\sin(\theta) & x \\ \sin(\theta) & \cos(\theta) & y \\ 0 & 0 & 1 \end{pmatrix}, \qquad (62)
$$

with parameter $\theta, x, y$. We use parenthesis for the elements of the matrix group to distinguish them from the matrix written with brackets representing endo-morphisms of the lie algebra. The identity matrix, i.e., the identity element of the group, is obtained with $\theta = 0, x = 0, y = 0$. Taking the derivative of $M$ with respect to the parameters at 0, 0, 0, we obtain a lie algebra element $m$ written as

$$
m = \begin{pmatrix} 0 & -\omega & v_x \\ \omega & 0 & v_y \\ 0 & 0 & 0 \end{pmatrix} = v_x e_1 + v_y e_2 + \omega e_3, \qquad (63)
$$

$$
\text{with } e_1 = \begin{pmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix}, \quad e_2 = \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{pmatrix}, \quad e_3 = \begin{pmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix},
$$

and (e₁, e₂, e₃) is the canonical basis of the lie algebra. We denote by u =
[vₓ, vᵧ, ω] the column vector associated to m and u' = [v'ₓ, v'ᵧ, ω'] the column
vector associated to m' another element of the lie algebra. The adjoint operator
on a matrix lie group is the matrix commutator:

$$
\begin{align}
[m, m'] = mm' - m'm &= \begin{pmatrix} 0 & 0 & (-\omega v'_y) - (-\omega' v_y) \\ 0 & 0 & (\omega v'_x) - (\omega' v_x) \\ 0 & 0 & 0 \end{pmatrix} \nonumber \\
&= (-\omega v'_y + v_y \omega') e_1 + (\omega v'_x - v_x \omega') e_2 + 0 e_3. \tag{64}
\end{align}
$$

Thus, the representation of the adjoint operator associated with the basis (e₁, e₂, e₃)
denoted by adₐ must verify

$$
\mathrm{ad}_u u' = [-\omega v'_y + v_y \omega', \omega v'_x - v_x \omega', 0]^\top \text{ and we have}
$$

$$
\mathrm{ad}_u = \begin{bmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{bmatrix}. \qquad (65)
$$
We calculate

$$
\begin{align}
\mathrm{ad}_u^\top u' &= \begin{bmatrix} 0 & \omega & 0 \\ -\omega & 0 & 0 \\ v_y & -v_x & 0 \end{bmatrix} \begin{bmatrix} v_x' \\ v_y' \\ \omega' \end{bmatrix} = \begin{bmatrix} v_y'\omega \\ -v_x'\omega \\ -v_y'v_x + v_x'v_y \end{bmatrix} \nonumber \\
&= \begin{bmatrix} 0 & 0 & v_y' \\ 0 & 0 & -v_x' \\ -v_y' & v_x' & 0 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix} = P_{u'} u, \tag{66}
\end{align}
$$

and we obtain

$$
P_u = \begin{bmatrix} 0 & 0 & v_y \\ 0 & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix}. \qquad (67)
$$

Now recall that

$$
G = \begin{bmatrix} \alpha & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \qquad (68)
$$

and substituting the previous expressions in (18) and (19) we obtain

$$
\mathrm{ad}_u^\top G_u u = \begin{bmatrix} 0 & \omega & 0 \\ -\omega & 0 & 0 \\ v_y & -v_x & 0 \end{bmatrix} \begin{bmatrix} \alpha v_x \\ v_y \\ \omega \end{bmatrix} = \begin{bmatrix} v_y \omega \\ -\alpha v_x \omega \\ (\alpha - 1) v_x v_y \end{bmatrix}, \quad (69a)
$$

$$
u^\top G_u u = \alpha v_x^2 + v_y^2 + \omega^2 \quad (69b)
$$

$$
\begin{align}
J_u &= \begin{bmatrix} 0 & \omega/\alpha & 0 \\ -\alpha\omega & 0 & 0 \\ \alpha v_y & -v_x & 0 \end{bmatrix} + \begin{bmatrix} 1/\alpha & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 & 0 & v_y \\ 0 & 0 & -\alpha v_x \\ -v_y & \alpha v_x & 0 \end{bmatrix} \nonumber \\
&= \begin{bmatrix} 0 & \omega/\alpha & v_y/\alpha \\ -\alpha\omega & 0 & -\alpha v_x \\ (\alpha-1)v_y & (\alpha-1)v_x & 0 \end{bmatrix}. \tag{69c}
\end{align}
$$

Detailed derivation of the saddle points

In this subsection, we detail how to compute the saddle points. The equation $\mathrm{ad}_u^\top G_u u = 0$ yields $v_x\omega = 0$, $v_y\omega = 0$ and $v_x v_y = 0$, and a solution $\bar{u}$ is a linear scaling of one of the vectors $e_1, e_2, e_3$. And $u^\top G_u u = 1$ yields six solutions

$$
\bar{u}_1 = \alpha^{-1/2} e_1, \quad \bar{u}_2 = e_2, \quad \bar{u}_3 = e_3, \qquad (70)
$$

$$
\bar{u}_4 = -\alpha^{-1/2} e_1, \quad \bar{u}_5 = -e_2, \quad \bar{u}_6 = -e_3.
$$

We have

$$
\begin{align}
& \bar{u}_1^\top G e_2 = \bar{u}_1^\top G e_3 = 0, \nonumber \\
& A_1 = T_{\bar{u}_1} S_F(1) = \operatorname{span}(e_2, e_3), \tag{71}
\end{align}
$$
and similar results with $\bar{u}_i$, $A_i$ for $i = 2..6$. Thus, we obtain

$$J_{\bar{u}_1}|_{A_1} = \begin{bmatrix} 0 & -\alpha^{1/2} \\ \alpha^{-1/2}(\alpha-1) & 0 \end{bmatrix}, \qquad (72)$$

$$J_{\bar{u}_2}|_{A_2} = \begin{bmatrix} 0 & 1/\alpha \\ (\alpha-1) & 0 \end{bmatrix}, \quad J_{\bar{u}_3}|_{A_3} = \begin{bmatrix} 0 & 1/\alpha \\ -\alpha & 0 \end{bmatrix},$$

and $J_{\bar{u}_4}|_{A_4} = -J_{\bar{u}_1}|_{A_1}$, $J_{\bar{u}_5}|_{A_5} = -J_{\bar{u}_2}|_{A_2}$, $J_{\bar{u}_6}|_{A_6} = -J_{\bar{u}_3}|_{A_3}$. Now, denoting $P_i(x) = \det(J_{\bar{u}_i}|_{A_i} - x I_2)$ the characteristic polynomial, we calculate

$$P_1(x) = x^2 + (\alpha - 1), \quad P_2(x) = x^2 - \frac{1}{\alpha}(\alpha - 1), \quad P_3(x) = x^2 + 1, \qquad (73)$$

and $P_4 = P_1$, $P_5 = P_2$, $P_6 = P_3$. Only $P_2$ has real roots $\pm\sqrt{(\alpha-1)/\alpha}$ and only $\bar{u}_2, \bar{u}_5$ are saddle points.

## F General analytical procedure

The strategy can be summarized in the procedure:

- Fix a basis in the Lie algebra.

- Calculate the expression of $f(u) = \text{ad}_u^\top G_u u$ of $g(u) = u^\top G_u u$.

- Solve the equation $f(u) = 0, g(u) = 1$.

- For each solution $u$, find a basis of the tangent space $T_u S_F(1)$.

- Use tangent space basis and the basis calculate the matrix $M$ of $(G_u^{-1}(\text{ad}_u^\top G_u + P_{G_u u}))|_{T_u S_F(1)}$ in the tangent space basis.

- Study the eigenvalues of $M$.

## G Details on saddle points identification

We present the complete procedure to identify the geodesic turnpike candidate in Algorithm 1.1.

In Algorithm 1.1 the operators are the following

- Hessian compute the symbolic expression of the hessian of an operator.

- Gauss(0, 1, n) is a random vector of size n following a Gaussian distribution with 0 mean and 1 standard deviation.

- root([expr], u0) is a non-linear root solver that finds r such that all expressions expr evaluated in r are zero. u0 is an initial guess.

- complete(r) return an invertible square matrix of the size of r with the first column being r.

- cholesky(g) is the Cholesky decomposition of a symmetric positive definite matrix $g = ll^\top$.

- gramm(m) return the matrix Q of the QR-decomposition of m. It corresponds to the matrix whose columns are obtained with the Gram-Schmidt procedure on m.
<table><caption>**Algorithm 1.1: Saddle points identification**</caption>
  <thead>
    <tr>
      <th></th>
      <th>Data: u symbolic variable of dimension n and the symbolic expressions:<br>F: u → F(e, u), A: u → ad<sub>u</sub>, P: u → P<sub>u</sub></th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td>1</td>
      <td>G = hessian(F);</td>
    </tr>
    <tr>
      <td>2</td>
      <td>C = u<sup>T</sup>Gu - 1;</td>
    </tr>
    <tr>
      <td>3</td>
      <td>D = A<sup>T</sup>Gu;</td>
    </tr>
    <tr>
      <td colspan="2">// G, C and D are symbolic expressions</td>
    </tr>
    <tr>
      <td>4</td>
      <td>c ← [];</td>
    </tr>
    <tr>
      <td>5</td>
      <td>for k = 0 to k<sub>max</sub> do</td>
    </tr>
    <tr>
      <td>6</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;r = root([C, D], gauss(0, 1, n));</td>
    </tr>
    <tr>
      <td>7</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;g = G(r);</td>
    </tr>
    <tr>
      <td>8</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;p = P(Mr);</td>
    </tr>
    <tr>
      <td>9</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;a = A(r);</td>
    </tr>
    <tr>
      <td colspan="2">// g, p and a are numerical matrices.</td>
    </tr>
    <tr>
      <td>10</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;J = g<sup>-1</sup>(ag + p);</td>
    </tr>
    <tr>
      <td colspan="2">// Linear algebra routine to obtain J<sub>T<sub>u</sub>S<sub>F</sub>(1)</sub></td>
    </tr>
    <tr>
      <td>11</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;b = complete(r);</td>
    </tr>
    <tr>
      <td>12</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;l = cholesky(g);</td>
    </tr>
    <tr>
      <td>13</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;s = l<sup>-T</sup> gramm(l<sup>T</sup>b);</td>
    </tr>
    <tr>
      <td colspan="2">// The last n - 1 columns of s form a basis of T<sub>r</sub>S<sub>F</sub>(1).</td>
    </tr>
    <tr>
      <td>14</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;J ← (s<sup>-1</sup>Js)[2..n, 2..n];</td>
    </tr>
    <tr>
      <td>15</td>
      <td>&nbsp;&nbsp;&nbsp;&nbsp;c ← appendif(c, r, eigenvalues(J));</td>
    </tr>
    <tr>
      <td>16</td>
      <td>return filter(c)</td>
    </tr>
  </tbody>
</table>

- eigenvalues(*m*) return the eigenvalues of the matrix *m*.

- appendif(*c*, *r*, *e*) is a custom function that appends *r* to the list *c* if eigenvalues *e* has only nonzero values and one with a positive real part and another one with a negative real part. In practice, it uses a threshold of 10<sup>-6</sup> relative to the maximum norm eigenvalue to test if the real parts of each eigenvalue are zero.

- filter(c) use a clustering to keep only one numerical representative of each analytical solution.

The computation $s = l^{-\top} \text{gramm}(l^{\top} b)$ gives the results of the Gram-Schmidt procedure using the inner product $u, v \to u^{\top} gv$ instead of the canonical one. Indeed, if $u^{\top} gv = 0$ we have $(l^{\top} u)^{\top} g(l^{\top} v) = 0$ using the Cholesky decomposition. The resulting $s$ columns are the orthonormalization of the system $b$ using the inner product associated with $g$. Because $b$ has $r$ as the first column, $s$ also has $r$ as the first column. Thus, the last $n-1$ columns of $s$ form a system of rank $n-1$, and each column is orthogonal to $r$. The set $T_r S_F(1)$ tangent to $S_F(1)$ in $r$ is exactly the set of vector $v$ such that $v^{\top} gr = 0$ of dimension $n-1$, so the last $n-1$ columns of $s$ form a basis of that space. $s^{-1}Js[2..n, 2..n]$ is the jacobian restricted to the tangent space. Note that $g$ is invertible because the operator $F$ is a well-defined Finsler operator.
## H Details on turnpike usage for geodesic solver

The geodesic between two configurations a and b in X are computed using a discretized version of the energy problem (16) with optimization variables $u_k$ with $k = 0..N-1$.

$$ \min_{u \in \mathcal{X}^N, q \in \mathcal{C}^{N+1}} \sum_{k=0}^{N-1} C(u_k) \qquad (74) $$

$$ q_{k+1} = q_k \exp(u_k), \quad k = 0..N-1; \quad q_0 = a, \quad q_N = b, $$

where exp is the Lie group exponential. To leverage the identified turnpike, we propose to solve instead the problem where $u_k$ for $k = 1..N-2$ is constrained to be proportional to a turnpike $\bar{u}$ in $\mathbb{S}_F(1)$ and $u_0$ and $u_{N-1}$ are not constrained. It is equivalent to solving the problem:

$$ \min_{u_0, u_{N-1} \in \mathcal{X}^2, \alpha \in \mathbb{R}} C(u_0) + C(u_{N-1}) + (N-2)\alpha^2 \qquad (75) $$

$$ b = a \exp(u_0) \exp((N-2)\alpha\bar{u}) \exp(u_{N-1}), $$

because $C(\bar{u}) = 1$. Note that due to the turnpike property, for geodesic long enough, $(N-2)\alpha$ is a good approximation of the geodesic length, i.e., the distance $d(a, b)$.

In practice to compute the geodesic we adopt a two-stage approach:

1. For each saddle point obtained with Algorithm 1.1, we solve the problem (75) and we keep the saddle point $\bar{u}$ for which the previous minimal cost value is the lowest.

2. We solve the original problem (74) using as an initial guess $u_0$ and $u_{N-1}$ the solution of the previous problem and $u_k = \alpha \bar{u}$ for $k = 1..N-2$.