Generalized Nonlinear and Finsler Geometry for Robotics

Nonlinear geometry is in many ways fundamental to robotics. Robotic configuration spaces are naturally modeled as manifolds [1], the classical mechanical dynamics of the robot is intimately linked to a Riemannian geometry [2], Gauss-Newton optimization has strong application in vision [3] and motion optimization [4] and those algorithms are closely related to Riemannian geometry [5, 6] natural gradients [7] which are core to many modern machine learning methods [8]. Despite their importance, however, even classical Riemannian geometry remains inaccessible to many roboticists. And beyond that, Finsler [9] and spray geometry [10], natural generalizations of Riemannian, are largely unheard of within the community. How many applications are out-of-reach as a result?

Our own work on Riemannian Motion Policies (RMPs) [11, 12] has led us to a study of what we call geometric fabrics [13, 14] as a formal provably stable model of reactive behavior, and to get there, we had to address this gap. Both Finsler and spray geometries proved critical for the development of that work, but were largely confined within opaque mathematical manuscripts mired with abstract concepts and foreign notation. The importance of the material, however, led us to entirely re-derive the foundations, rigorously, but in a language we could understand. This paper presents these re-derivations in a hope that they will be more accessible to a broader robotics audience and inspire new future applications.

We use notations of advanced calculus [15] as much as possible and build on the Calculus of Variations [16] (Finsler geometry maybe viewed as the geometry of these functional optima), and we draw connections to classical mechanics [17], Riemannian geometry [18, 2], and the interrelations observed in geometric mechanics [2]. We limit ourselves to a study of geometric path consistency which we call generalized nonlinear geometry (see Section II),¹¹1Our terminology differs from the literature. Equations exhibiting geometric path consistency are commonly called sprays in mathematics [10], and literature further develops more general geometry with a very similar name (a semi-spray), which doesn’t exhibit such path consistency. We, therefore, restrict the term geometry to imply this concrete notion of path consistency to aid intuition and reserve the term semi-spray in our applications [13] for these more general non-path-consistent equations. and show that Finsler geometries are a type of generalized nonlinear geometry (see Section III), inheriting their geometric consistency.

Manifolds [19] are a standard mathematical foundation of modern nonlinear geometry. However, these concepts can be very abstract and daunting for those unfamiliar. We, therefore, present our derivations in specific coordinates, following the conventions of many texts on classical mechanics [17]. For those familiar with the concepts, we can say we do so without loss of generality—the Euler-Lagrange equation is naturally covariant, enabling curvilinear changes of coordinates as necessary while traversing a manifold with no change to the underlying behavior; covariant behavior of more general nonlinear geometries can be expressed through a transform tree as was done in [12] in the definition of structured Geometric Dynamical Systems (GDS).

A generalized nonlinear geometry (known as a spray in mathematics [10]) is a second-order differential equation describing a smooth collection of paths. These paths are equivalence classes of trajectories all passing through the same points but differing in speed profile. In essence, we allow a trajectory to speed up or slow down arbitrarily, and as long as its geometric shape remains consistent (i.e. it passes through the same one-dimensional set (submanifold) of points in space) we say the trajectory follows the same path (i.e. is part of the equivalence class). Colloquially, similar to how we can think of an arbitrary second-order differential equation as a collection of trajectories (its integral curves), we can think of the geometry as a collection of tubes. Each tube represents a path and contains multiple trajectories (infinitely many of them), the collection of all trajectories following that path with differing speed profiles (see Figure 1).

Concretely, two trajectories are said to be equivalent, and hence along the same path, if they are a time reparameterization away from one another. Given a trajectory with time index $t$ denoted $\mathbf{x}_{t}(t)$, a time reparameterization is a smooth, strictly monotonically increasing, nonlinear function $t:\mathbb{R}\rightarrow\mathbb{R}$ denoted $t(s)$ giving rise to a new time index $s$. The time reparameterization creates a new trajectory $\mathbf{x}_{s}(s)=\mathbf{x}_{t}\big{(}t(s)\big{)}$. Since, for a given $s_{0}$ and corresponding $t_{0}=t(s_{0})$, the points of the trajectories align

and since this relationship is a bijection (due to the strict monotonicity),²²2Formally, it is a diffeomorphism between coordinate charts of the same one-dimensional manifold of points. the two trajectories $\mathbf{x}_{s}(s)$ and $\mathbf{x}_{t}(t)$ pass through the same set of points and we say they follow the same path. Since $\frac{d}{ds}\mathbf{x}_{t}\big{(}t(s)\big{)}=\frac{d\mathbf{x}_{t}}{dt}\frac{dt}{ds}$ and $\frac{d^{2}}{ds^{2}}\mathbf{x}_{t}\big{(}t(s)\big{)}=\frac{d^{2}\mathbf{x}_{t}}{dt^{2}}\left(\frac{dt}{ds}\right)^{2}+\frac{d\mathbf{x}_{t}}{dt}\frac{d^{2}t}{ds^{2}}$, we see velocities and accelerations under the time reparameterization are linked to one another as

where notationally the dots are understood to be time derivatives w.r.t. their respective time indices. E.g. ${\dot{\mathbf{x}}}_{s}=\frac{d\mathbf{x}_{s}}{ds}$, ${\dot{\mathbf{x}}}_{t}=\frac{d\mathbf{x}_{t}}{dt}$ and so forth. A smooth nonlinear geometry is defined by the collection of all time-reparameterization invariant smooth paths in a space. For every point $\mathbf{x}_{0}$ and speed-independent direction vector $\widehat{\mathbf{v}}$, the nonlinear geometry defines a unique path eminating from that point $\mathbf{x}_{0}$ following the specified initial direction $\widehat{\mathbf{v}}$.

Denoting the orthogonal projector projecting orthogonally to velocity as $\mathbf{P}_{\dot{\mathbf{x}}}^{\perp}=\mathbf{I}-\widehat{{\dot{\mathbf{x}}}}\widehat{{\dot{\mathbf{x}}}}^{T}$, the family of geometries we consider here are those characterized by a second-order differential equation of the form

where $\mathbf{h}_{2}(\mathbf{x},{\dot{\mathbf{x}}})$ is a smooth function that is positively homogeneous of degree 2 (HD2) in velocities.³³3Generally, a function $f(\mathbf{z})$ is said to be positively homogeneous of degree $k$ (abbreviated HD$k$) if $f(\lambda\mathbf{z})=\lambda^{k}f(\mathbf{z})$ for $\lambda\geq 0$ [20]. Homogeneity of degree 1 and 2 is used in the definitions below as well. In this case, $\mathbf{h}_{2}$ must be HD2, meaning $\mathbf{h}_{2}(\mathbf{x},\lambda{\dot{\mathbf{x}}})=\lambda^{2}\mathbf{h}_{2}(\mathbf{x},{\dot{\mathbf{x}}})$ for $\lambda>0$. We call equations of this form geometric equations.

Since the projector $\mathbf{P}_{\dot{\mathbf{x}}}^{\perp}$ is reduced rank, there is solution redundancy. We will see that this redundancy precisely describes the ability to arbitrarily speed up or slow down along a trajectory while sticking to the same path.

The interior equation in isolation

we call the generating equation and is said to generate the geometry via its system of trajectories. Note that since $\mathbf{P}_{\dot{\mathbf{x}}}^{\perp}$ has null space spanned by ${\dot{\mathbf{x}}}$, solutions to the geometric equation are solutions to

where $\alpha:\mathbb{R}\rightarrow\mathbb{R}$ is a smooth function defining an acceleration along the direction of motion. We call this equation the explicit form geometric equation.

The proof of the above theorem shows that any time reparameterization $t(s)$ solving

induces a generating solution. Since solutions to Equation 13 are defined by their initial conditions, given a geometric solution $\mathbf{x}_{s}$, we can choose a time reparameterization $s(t)$ (the inverse of $t(s)$) so that $\mathbf{x}_{t}=\mathbf{x}_{s}\big{(}s(t)\big{)}$ is a generating solution whose velocity matches the geometric solution’s velocity at a given time $\bar{s}$ (choose $t(s)$ solving Equation 13 using initial conditions $t(\bar{s})=\bar{s}$ and $\frac{dt}{ds}(\bar{s})=1$). That mapping from $s$ to instantaneous velocity-matching generating solution is smooth, so we can view the geometric solution as smoothly moving between generating solutions, using the redundant accelerations $\alpha(s){\dot{\mathbf{x}}}_{s}$ to do so by speeding up and slowing down along the direction of motion.

To illustrate path consistency, we designed a geometry, $\mathbf{h}_{2}(\mathbf{q},{\dot{\mathbf{q}}})$, that naturally produces particle paths that avoid a circular object in coordinates, $\mathbf{q}\in\mathbb{R}^{2}$, as

where $\phi(\mathbf{q})$ is a differentiable map that captures the distance to a circular object and $\lambda\in\mathbb{R}^{+}$ is a scaling gain. More specifically, $\phi(\mathbf{q})=\frac{\|\mathbf{q}-\mathbf{q}_{o}\|-r}{r}$, where $\mathbf{q}_{o}$ and $r$ are the circle’s center and radius, respectively. Furthermore, $\psi\big{(}\phi(\mathbf{q})\big{)}\in\mathbb{R}^{+}$ is a barrier potential function, $\psi\big{(}\phi(\mathbf{q})\big{)}=\frac{k}{\phi(\mathbf{q})^{2}}$, where $k\in\mathbb{R}^{+}$ is a scaling gain. Altogether, $\partial_{\mathbf{q}}\psi\big{(}\phi(\mathbf{q})\big{)}$ produces an increasing repulsive force as distance to the object decreases, and $\|{\dot{\mathbf{q}}}\|^{2}$ makes $\mathbf{h}_{2}(\mathbf{q},{\dot{\mathbf{q}}})$ homogeneous of degree 2 in ${\dot{\mathbf{q}}}$. For this experiment, $\lambda=0.7$, $k=0.5$ for two scenarios: 1) $\alpha=1.5$, and 2) $\alpha=0.75$ for the initial conditions, $(\mathbf{q}_{0},\alpha\hat{{\dot{\mathbf{q}}}}_{0})$. Eleven vertically spaced particles that follow the above geometry are initialized with the two different initial speeds. Traced paths at the two different speeds are overlaid as shown in Fig. 1. Noticeably, the paths generated are completely overlapping confirming path consistency.

Here, we derive a broad class of nonlinear geometries, known as Finsler geometries [9]. These geometries arise from Calculus of Variations problems [16] that generalize the notion of arc length to cases where length elements can vary by direction (Minkowski norms [something]). This section builds to a fundamental result showing that applying the Euler-Lagrange equation to these generalized length elements produces a Geometric equation (a Finsler geometry of paths) whose corresponding generator can be derived by applying the Euler-Lagrange equation to the length element’s energy form. Solutions to the latter conserve the derived energy and can thus be viewed as energy levels. And from the above analysis, we can characterize geometric solutions to Finsler geometries as smoothly transitioning between these concretely defined energy levels (generating solutions) by speeding up and slowing down along the direction of motion while remaining along the same common path.