On Sub-Riemannian Geodesics in SE(3) whose spatial projections do not have cusps

We consider the problem P curve of minimizing L (cid:2)


Mathematics Subject Classification (2010) 53C17 • 22E30 • 49J15 1 Introduction
In the space of smooth curves in R 3 , we define the energy functional with L ∈ R + being the length (free) of a curve s → x(s) ∈ R 3 .Here, ξ > 0 is a constant, s denotes the arclength of the curve x and κ : (0, L) → R + denotes the absolute curvature κ(s) = x (s) of the curve x for all s ∈ (0, L).
In this paper, we consider the problem P curve of minimizing the functional E(x) among all smooth curves s → x(s) in R 3 , satisfying the boundary conditions (see Fig. 1) Here, we parameterize x by spatial arclength, i.e., x (s) = 1, and via ordinary parallel transport on the tangent bundle T (R 3 ) the tangent vector x (s) ∈ T x(s) (R 3 ) can be identified with a point n(s) ∈ S 2 .
The two-dimensional analog of this variational problem was studied as a possible model of the mechanism used by the visual cortex V1 of the human brain to reconstruct curves which are partially corrupted or hidden from observation.The two-dimensional model was initially due to Petitot (see [28,29] and references therein).Subsequently, Citti and Sarti [9,34] recognized the sub-Riemannian Euclidean motion group structure of the problem.In [5], the existence of minimizers was studied by Boscain, Charlot, and Rossi.It turned out that only for certain end conditions, the 2D problem P curve is well-posed.Characterization of the set of end conditions for which P curve is well-posed can be found in [11].The more general 2D problem related to a mechanical problem was completely solved by Sachkov [25,32,33], who in particular derived explicit formulas for the geodesics in sub-Riemannian arclength parameterization.Later, an alternative expression in spatial arclength parameterization for cuspless sub-Riemannian geodesics was derived in [6,13].Application of problem P curve to contour completion in corrupted images was studied in [23].The problem was also studied by Hladky and Pauls in [22] and by Ben-Yosef and Ben-Shahar in [4].However, many imaging applications such as diffusion weighted magnetic resonance imaging (DW-MRI) require an extension to three dimensions [12,14,17,31], which motivates us to study the three-dimensional curves minimizing the energy functional E(x).

Statement of the Problem P curve
Let x 0 , x 1 ∈ R 3 and n 0 , n 1 ∈ S 2 = {v ∈ R 3 | v = 1}.Our goal is to find an arc-length parameterized curve s → x(s) such that

E(y).
We assume that the boundary conditions (x 0 , n 0 ) and (x 1 , n 1 ) are chosen such that a minimizer exists.Due to rotation and translational invariance of the problem, it is equivalent to the problem with the same functional and boundary conditions (0, e z ) and (R T n 0 (x 1 − x 0 ), R T n 0 n 1 ), where e z denotes the unit vector in the z-axis in the right handed {x, y, z} coordinate system and R n 0 ∈ SO(3) such that n 0 = R n 0 e z .Therefore, without loss of generality, we set (unless explicitly stated otherwise) x 0 = 0 and n 0 = e z for the remainder of the article.Hence, the problem now is to find a sufficiently smooth arc-length parameterized curve s → x(s) such that x = arg inf y ∈ C ∞ ([0, L], R 3 ), L ≥ 0, y(0) = 0, y (0) = e z , y(L) = x 1 , y (L) = n 1 .

E(y).
We refer to the above problem as P curve .
In this paper, we use two different parameterizations: spatial arclength s and sub-Riemannian arclength t (s) = s 0 ξ 2 + κ 2 (σ ) dσ .We denote the derivative d ds by a prime, and d dt by a dot.

Structure and Results of the Article
In Section 2, we lift problem P curve on R 3 to a sub-Riemannian problem P mec on the quotient R 3 S 2 := SE(3)/({0} × SO(2)), (1.2) where SO (2) is identified with all rotations in R 3 about reference axis e z .Such an extension and naming ('mec' refers to mechanical) was also done for the problem P curve on R 2 , cf. [6,11].
To state the problem P mec on the quotient (1.2), we first resort to the corresponding leftinvariant sub-Riemannian problem P MEC on the Lie group SE (3).We formulate problem P MEC in Definition 1 and problem P mec in Definition 3.
The main result in Section 2 is Theorem 1, where we show the two requirements for sub-Riemannian geodesics γ (•) = (x(•), R(•)) in P MEC to have the property that the corresponding spatially projected curve x(•) is indeed a stationary curve of problem P curve .There, we also show that these sub-Riemannian geodesics in SE(3) relate to well-defined geodesics of problem P mec on the quotient R 3 S 2 .One of the two requirements is a vanishing momentum component, the other is a requirement on the end-condition (x 1 , n 1 = R n 1 e z ) which should belong to a set R ⊂ R 3 S 2 that we express as the range of an exponential map of problem P curve .In fact, this set R is precisely the set of end conditions for P mec where the spatial projection of geodesics does not exhibit a cusp.The formal definition of a cusp will follow in Definition 8.For an illustration of cases (x 1 , n 1 ) ∈ R where cusps occur on the spatial projection of sub-Riemannian geodesics, see Fig. 2. The geodesic of P mec is said to be cuspless if its spatial projection does not have a cusp.The study of cuspless sub-Riemannian geodesics in SE( 3) is important for image analysis applications.In particular for the tracking of smoothly varying curvilinear structures (e.g., blood vessels or neural fibers) in noisy 3D (medical) images (see [12,17,31]).There, the presence of cusps in the spatial projection of a geodesic is typically undesirable, as it yields a non-smooth path in 3D images.Likewise, to the 2D case [11], it may even be used as a criterium for not connecting two given boundary conditions in an image.
In Section 3, we apply the Pontryagin maximum principle (PMP) [1,30,36] to problem P MEC in Section 3.1.In Section 3.2, Theorem 2, we prove Liouville integrability.In Section 3.3, we express the canonical equations of PMP in terms of the − Cartan connection in Theorem 3.Then, a natural choice of SE(3) matrix representation arises in the matrix representation of the Cartan connection, i.e., the Cartan-matrix.We employ this in Theorem 4 containing one of the two key ingredients that we use for integrating the canonical equations of P mec .The other ingredient is the well-known co-adjoint orbit structure in SE(3) characterized in Lemma 1.
In Section 4, we combine the two ingredients to compute the first cusp-time (Theorem 5 in Section 4.1), and to integrate the canonical equations for geodesics of P mec .As a result, we obtain, for the first time, explicit analytic formulas for both problems P curve and P mec .These analytic formulas involve elliptic integrals of the first and the third kind.This is summarized in Theorem 6, which is the main result of this article.Subsequently, in Section 4.3, we derive many geometric properties of the sub-Riemannian geodesics such as: Fig. 2 The spatial projection of the geodesics of P mec can have singularities (the cusp points).Here, the spatial projection of the geodesics of P mec is shown in green before the first cusp point, and in red after the first cusp point.The range R of the exponential map of the problem P curve consists of the end conditions reachable by the cuspless geodesic in P mec (i.e., the end conditions reachable by only the green curves).In all cases, the end condition n 1 = R n 1 e z is depicted in red.The other black arrows show the remaining vectors R n 1 e x and R n 1 e y • A uniform bound on torsion of the spatial part of the geodesics in Theorem 7.
• Sufficient and necessary conditions for sub-Riemannian geodesics to be planar in Theorem 8 and Corollary 6, for which we have global optimality in Corollary 7. • Monotony along a spatial axis (determined by the initial momentum) in Corollary 8.
• In most cases (see Corollaries 9,10,and 11), we prove that the spatial part of sub-Riemannian geodesics stays in the upper half space of the initial direction (if n 0 = e z this upper half space is z ≥ 0).In particular, we prove z ≥ 0 for all planar geodesics (Corollary 10), and z ≥ 0 for all geodesics departing from a cusp and ending in a cusp.• In the case of planar geodesics and/or geodesics departing from a cusp and ending in a cusp, we show in Corollaries 11 and 12 that z = 0 can only be reached with opposite tangent −e z via a U-shaped planar geodesic that departs from a cusp and ends in a cusp.• The rotational and reflectional symmetries as we show in Corollary 13 in Section 4.4.
In Section 5, we conclude with numerical analysis of problem P curve on R 3 .Numerical experiments in Section 5.1, see Fig. 7, indicate that the first conjugate time comes after the first cusp time, as in the 2D-case [6].Numerical experiments in Section 5.2 on the set R and the cones of reachable angles, see Fig. 8, put a conjecture on homeomorphic/diffeomorphic properties on the exponential map (cf.Conjecture 1).Finally, we use the analytic formulas for the sub-Riemannian geodesics for numerical solutions to the boundary value problem as briefly explained in Section 5.3.Wolfram Mathematica code for solving the boundary value problem can be downloaded from http://bmia.bmt.tue.nl/people/RDuits/final.rar.

Problem P curve on R 3 , P MEC on SE(3), and P mec on R 3 S and Their Connection
In this section, we relate the problem P curve to a sub-Riemannian problem P mec on the quotient R 3 S 2 = SE(3)/({0} × SO(2)), as was also done for the P curve on R 2 , cf. [6,11].To state the problem P mec on this Lie group quotient, we first resort to the corresponding left-invariant sub-Riemannian problem P MEC in the Lie group SE (3).The group SE(3) = R 3 SO(3) denotes the Lie group of rigid body motions on R 3 , which is a semidirect product of R 3 and SO(3).An element g ∈ SE(3) is represented by the pair (x, R) ∈ R 3 SO(3), and the group product is given by g We define sub-Riemannian problem P MEC by means of the left-invariant frame (see Fig. 3).
The left-invariant frame consists of the following left-invariant vector fields over SE(3): where we parameterize R 3 by {x, y, z} and SO(3) by angles {α, Fig. 3 The left-invariant frame representing a moving frame of reference along a curve on R 3 S 2 Since Re z = n = (sin β, − sin γ cos β, cos γ cos β) T , we have that (γ , β) are spherical coordinates on S 2 .One needs multiple charts to cover S 2 .However, outside of ±(1, 0, 0) T , this choice of spherical coordinates is 1-to-1 and regular, and there it is preferable over standard Euler angles (which are singular at the unity element e z = (0, 0, 1) T ), [15, ch:2, Fig. 4].
The corresponding co-frame is given by the co-vectors {ω 1 , . . ., ω 6 } satisfying ω i , A j = δ i j for i, j ∈ {1, . . ., 6}, with δ i j the Kronecker delta.The structure constants c k i,j of the Lie algebra of left-invariant vector fields in SE(3) are given in Table 1.
We consider the sub-Riemannian manifold (M, , G ξ ), [26], with For those horizontal curves γ (i.e., γ ∈ ) in SE(3) that can be parameterized by spatial arclength, one has s) ds, which in view of Problem P curve , motivates our choice of (M, , G ξ ).Details can be found in Appendix A (A.1).The sub-Riemannian distance on (SE(3), , G ξ ) is given by Next, we will address the quotient structure (1.2) and the connection of problem P curve on R 3 to problem P MEC on SE(3), and problem P mec on R 3 S 2 .
We obtain a well-defined distance on the quotient R 3 S 2 , recall (1.2) and (2.3), by where we use both left-invariance and invariance under the specific conjugations g → h −1 α gh α and where we have set α = α 2 − α 1 .Hence, we get the following definition.
Definition 3 Problem P mec is defined as follows on R 3 S 2 .Let (y with γ a Lipschitzian curve in SE(3) with velocity γ ∈ , such that sub-Riemannian length )dt is minimal under boundary conditions γ (0) = (0, I ) and ), where both T ≥ 0 and α ∈ [0, 2π) are free variables in the optimization process.
Later on, for sub-Riemannian geodesics whose spatial projections do not exhibit cusps (see the green curves in Fig. 2), we shall rely on the spatial arclength parameter s as this parametrization produces much simpler formulas.To distinguish between derivatives, we write γ (t) := d dt γ (t) and γ (s) := d ds γ (s).
The next theorem provides us the terminal conditions of interest, the appropriate choice of representant in each equivalent class, i.e., the α that minimizes (2.5).In fact, this sets the choice of (y 1 , R n 1 ) ∈ SE(3) in P MEC such that the spatial projection x * (•) of the optimizer of γ * (•) = (x * (•), R * (•)) in P MEC coincides with the optimizer of P curve .The proof relies on notations and results in Section 3.1, but we formulate the theorem here as it fully explains the connection between problem P curve on R 3 , problem P MEC on SE(3), and problem P mec on R 3 S 2 .At this point, the reader is advised to skip the proof and return to it after Section 3.1.Proof The proof can be found in Appendix A. It relies on notation and results of Section 3. See Fig. 4 for an illustration of an explicit case.3) denote the exponential map of P MEC .This exponential map is the solution operator that solves the Hamiltonian system of PMP, with the Hamiltonian H (λ(0)) given by Eq. 2.6, departing from e = (0, I ) and thereby it maps initial momentum λ(0) and sub-Riemannian arclength t onto the end-point (x(t), R(t)) ∈ SE(3) of the corresponding geodesic of problem P MEC .

Theorem 1 If the terminal point g
For the required case λ 6 = 0, we will derive the operator Exp e ((λ 1 (0), λ 2 (0), λ 3 (0), λ 4 (0), λ 5 (0), 0), t) explicitly in Section 4.  3) in P MEC .By Theorem 1, the minimizer in Eq. 2.5 is found by setting λ 6 (0) = 0. Here, the spatial projection of the minimizing geodesic is depicted in green, and the spatial projection of a geodesic with λ 6 = 0 in blue.In order to show the choice of rotation Next, we formally define the exponential map of the problem P curve , where we rely on the action of SE(3) onto R 3 S 2 , recall (2.4).

Definition 6
The exponential map Exp : D 0 → R 3 S 2 of P curve is defined by: , where x * denotes the minimizer of P curve , γ * denotes a minimizer of P MEC , and with domain ) where s max (λ(0)) ∈ R ∪ {∞} is the maximal length of the spatial projection x * of the sub-Riemannian geodesic γ * to the first cusp.
We exclude λ (1) (0) = 0 from the domain D to avoid pure rotations which are not solutions to the problem P curve .This is also done in the SE(2) case [11,Remark 5.5].By Theorem 1, we have the following result.

Corollary 1
The set R equals the range of the exponential map of P curve : i.e., it coincides with all possible end conditions (x(L), x (L)) of geodesics of P curve .

P MEC : Sub-Riemannian Problem on SE(3)
In this section, we study the problem P MEC .The optimal control theoretical formulation of this sub-Riemannian problem is to find a Lipschitzian curve γ : [0, T ] → SE(3), with boundary conditions γ (0) = e := (0, I ) and (3.1) where the control variables u i ∈ L ∞ ([0, T ]) for i = 3, 4, 5.In particular, we only consider the curves for which the absolute curvature of the spatial projections is in L ∞ ([0, T ]).The control variables are contravariant components of the velocity vector, so we index them with upper indices.

Remark 3
The problem P MEC given by Eq. 3.1 can be seen as a motion planing problem for a spacecraft, that can move forward/backward (in direction A 3 ) and rotate about axis The control u 3 determines spacial velocity (the sign of u 3 determines forward/backward propagation), while the controls u 4 and u 5 determine the angular velocity, cf.Fig. 3.
The existence of minimizers for the problem P MEC is guaranteed by the theorems by Chow-Rashevskii and Filippov on sub-Riemannian structures [1].
Remark 4 About smoothness of minimizers of P MEC .
• We have that for P MEC , there are no abnormal extremals.It follows from the fact that any sub-Riemannian manifold with a two-generating distribution does not allow abnormal extremals (see Chapter 20.5.1 in [1]).This is the case here as we have for • Due to the non-existence of abnormal extremals, the geodesics are always analytic [1] and so are the extremal controls.A priori, in the application of the Pontryagin maximum principle (PMP) [1,36], one cannot restrict to smooth controls where one relies on L ∞ -controls for P MEC (and L 1 -controls for P curve , similar to the SE(2)-case [6, ch:5.1,App.B]).In retrospect, however, the minimizers are analytic, and one can write the sub-Riemannian distance from g to e = (0, I ) on Next, in the application of PMP to the problem P MEC , we rely on the sub-Riemannian arclength parameter t, which is well-defined for all SR-geodesics in (SE (3), , G ξ ).Before a cusp occurs, recall Fig. 2, one can also use spatial arclength parameterization s, related by t (s) = s 0 ξ 2 + κ 2 (σ ) dσ .The formal definition of a cusp time is given bellow.Definition 8 A cusp time is a time 0 < t cusp < T when the third control component u 3 (t) (responsible for spatial propagation in P MEC ) vanishes, u 3 (t cusp ) = 0 and moreover u3 (t cusp ) = 0.

Application of Pontryagin Maximum Principle
A first-order necessary optimality condition is given by PMP [1,30].Note that PMP gives a necessary but not a sufficient condition of optimality.Geodesics of P MEC loses local optimality after the first conjugate point and global optimality after the first Maxwell point (see [1]).
The Cauchy-Schwarz inequality implies that the minimization problem for the sub-Riemannian length functional is equivalent to the minimization problem for the action functional (see [1]) Next, we apply PMP to problem P MEC using the equivalent action functional, i.e., to the following optimal control problem: which is identified with an element from SE(3) via (2.1).The natural momentum coordinates {λ i } for left-invariant sub-Riemannian problems come along with the left-invariant Hamiltonians h i : T * (SE(3)) → R (see [1]), given by where λ(q) = p 1 (q)dx| q +p 2 (q)dy| q +p 3 (q)dz| q +p 4 (q)dγ | q +p 5 (q)dβ| q +p 6 (q)dα| q = 6 i=1 λ i (q) ω i | q ∈ T * q (SE( 3)) denotes a momentum covector expressed in respectively the fixed and the moving dual frame.Now, we apply PMP.By Remark 4, we only need to consider the normal case.The control-dependent Hamiltonian reads Optimization over all controls produces the (maximized) Hamiltonian and gives expression for the extremal controls By virtue of the Lie brackets (see Table 1), we have the Poisson brackets Thus, the Hamiltonian system of PMP reads as follows: -vertical part (for adjoint variables), -horizontal part (for state variables).The sub-Riemannian geodesics are solutions to the Hamiltonian system.Finding a parameterization of the sub-Riemannian geodesics is a nontrivial problem.In order to guarantee that such a parametrization exists, we first prove Liouville integrability of the system.Here, we follow the same approach as in [24].
The next remark shows that we can restrict ourselves to the case ξ = 1.
The homothety boils down to making the problem dimensionless, as the physical dimension of ξ −1 is spatial length.
Remark 6 All prerequisites for the proof of Theorem 1 are now given.See Appendix A.

Liouville Integrability
To prove the Liouville integrability of the Hamiltonian system (3. 5 is a first integral of the Hamiltonian system.From the vertical part of Eq. 3.5, we can immediately see one more first integral λ 6 , which is functionally independent from H . Since {H, λ 6 } = 0, we see that the integrals H and λ 6 are in involution.The Hamiltonian system directly reveals the first integral W = −λ 1 λ 4 − λ 2 λ 5 − λ 3 λ 6 .This integral is a Casimir function, i.e., {W, λ i } = 0, i = 1, ..., 6. Casimir functions are functions on the dual space of the Lie algebra commuting in the sense of Poisson brackets with all left-invariant Hamiltonians.They are universal conservation laws on the Lie group.Connected joint level surfaces of all Casimir functions are coadjoint orbits (see [20,Prop. 7.7]).Since these orbits are always even-dimensional (they are symplectic manifolds), the difference between the dimension of the Lie group and the number of Casimir functions is even.Casimir functions are found by solving {C, λ i } = 0.For polynomial functions C, we can write C with undetermined coefficients as a polynomial of degree 1, 2, 3, and solve the resulting system of equations algebraically.The second Casimir function in SE(3) is given by For details on the Casimir functions, see the work of A.A. Kirillov [19] or the book of V. Jurdjevic [18].
To construct the complete system of first integrals, we consider integrals H , λ 6 and W and find three more first integrals.Then we show that all six integrals are functionally independent on an open dense domain in T * (SE(3)) and are in involution.
The right-invariant vector fields are given by Then, the right-invariant Hamiltonians are defined by ρ i (q, λ) = λ(q), B i | q .Since the table of Poisson brackets between ρ i coincides with the commutator of corresponding vector fields (cf.Table 1), we see that only ρ 1 , ρ 2 , and ρ 3 are in involution.Their expression via the left-invariant Hamiltonian λ i reads as

Independence of Integrals
In order to study the functional independence of the integrals H , λ 6 , W , ρ 1 , ρ 2 , ρ 3 at a point (q, λ) ∈ T * (SE(3)), introduce the Jacobian matrix Liouville integrability of the Hamiltonian system follows by the study of the vertical derivatives of the integrals (i.e., the derivatives w.r.t. the variables λ i ).By analyticity, functional independence of the integrals on an open dense domain in T * (SE(3)) follows from linear independence of gradients of the integrals at a single point (q, λ) ∈ T * (SE (3)).Since we have the first integrals ρ 1 , ρ 2 , ρ 3 , I , H , λ 6 are functionally independent.Here, we use that −λ 2 λ 4 + λ 1 λ 5 = λ3 = u3 = d 2 s dt 2 ≡ 0. So we proved the following theorem.
Explicit integration of Eq. 3.5 in sub-Riemannian arclength parametrization t is a difficult problem.Further in Section 4, we show that using spatial arclength parametrization s leads to relatively simple expressions for sub-Riemannian geodesics whose spatial projections do not exhibit cusps.

The − Cartan Connection ∇
In the sub-Riemannian manifold (SE(3), , G ξ ), the directions A 1 , A 2 , and A 6 are prohibited in the tangent bundle.To get a better grasp on what this means on the manifold level, we consider principal fibre bundles.We use the minus '−' Cartan connection [8] to connect the Hamiltonian PMP approach to the Lagrangian reduction approach by Bryant and Griffiths [7].It provides more intuition and is an important tool toward explicit formulas.As is seen by the following theorem, these curves actually describe parallel transport of the momentum covectors w.r.t.Cartan connections.In the original Cartan and Schouten article [8], three Cartan connections are presented, the +, the −, and 0 connection.Here, we shall rely on the minus − Cartan connection for which the left-invariant vector fields are autoparallel [27], since our geometrical control problem is expressed in left-invariant vector fields.In order to keep track of correct tensorial computations, we deal with the general case ξ > 0 in Theorem 3.However, by Remark 5, only the case ξ = 1 is considered in the remainder of the article.

Definition 9
We define connection ∇ on the (horizontal) tangent bundle of (SE(3), , G ξ ) by with γ = a k A k and Lie algebra structure constants c k i,j given in Table 1.
It is a partial − Cartan connection that originates from a specific choice of principle fiber bundle.For details, see Appendix B below.Another ingredient in the theorem below is the standard exponential map from Lie algebra to Lie group given by T (0,I ) (SE(3)) A → e A ∈ SE(3).When setting contravariant components λ i = ξ i λ i , ξ 3 = ξ −2 , ξ 4 = ξ 5 = 1, and λ i ω i then the Hamiltonian system of PMP (3.5) can be written as: Proof Define γ i := ω i γ , γ .Note that γ i = u i .Here, we write γ i (instead of u i ) to stress the curve dependence of the control components.Then following the same approach as done in [13], [11, App.C] (for the case SE(2)), the Cartan connection on the tangent bundle is given by Eq. 3.7.From Eq. 3.7, we see that the auto-parallel curves are horizontal exponential curves: , and to ensure t to be the sub-Riemannian arclength parameter we must have ξ 2 (c 3 ) 2 + (c 4 ) 2 + (c 5 ) 2 = 1.Now the partial ('right' or −) Cartan connection ∇ on the tangent bundle naturally imposes the partial Cartan connection ∇ * on the cotangent bundle, as follows: which follows from Eq. 3.7 and d and c k i,j = −c k j,i .Now, by the horizontal part of PMP, we have γ i = λ i for all i ∈ {3, 4, 5}, so that the result follows by substituting this equality into Eq.3.9.
Next, we switch to spatial arclength parameter s, as this is convenient.Recall d ds is denoted by a prime.Also recall that s-parametrization is well defined until the spatial projection of a sub-Riemannian geodesic exhibits a cusp (recall Definition 8).
Denote by s max the minimum positive value of the parameter s where such a cusp appears.Explicit expression for s max in terms of the initial momentum λ(0) will follow (see Eq. 4.5).Next, to find the sub-Riemannian geodesics, we integrate the equation in Theorem 3 via a suitable matrix representation visible in the Cartan-matrix of the Cartan connection.Such a group representation m : SE(3) → Aut(R 6 ) is given by where σ x = 3 i=1 x i A 3+i ∈ so(3), so that σ x y = x × y, with x = 3 i=1 x i e i and A 3+i ∈ so(3) given by Here, we have σ Rx = Rσ x R −1 and thereby m(g with short notation ω j = ω j γ , λ = λ| γ , dλ = dλ| γ , and where we represent the covector λ = 6 i=1 λ i ω i γ by a row-vector λ = (λ 1 , . . ., λ 6 ).So we see that Eq. 3.10 naturally appears in Eq.Then, along the sub-Riemannian geodesics in (SE(3), , G 1 ), the following relation holds: Multiplication with g(s) from the right yields the result.
For further details, see Appendix B. These details are not necessary for the remainder of the article, where we only rely on Eqs.3.7, 3.8, 3.10, and 3.11.
Let us recall the first integrals of the Hamiltonian system and the coadjoint orbits, that we use next for the derivation of explicit formulas for the geodesics.
Lemma 1 Co-adjoint orbits of λ(0) are given by (see [21, p. 474] and Section 3.2) where C i are the Casimir functions Corollary 2 On each co-adjoint orbit, we can choose the nice representative λ(0) = (c, 0, 0, − W c , 0, 0), solve the Exponential map for this representative, and obtain the general solution by left-invariance.More precisely, by Theorem 4, we first find the geodesic γ with and then we obtain γ via γ (s) = γ −1 (0) γ (s).

Sub-Riemannian Geodesics in R 3 S 2 with Cuspless Projections
In this section, we derive sub-Riemannian geodesics with cuspless spatial projection in the quotient R 3 S 2 , and we study geometrical properties of the geodesics, such as planarity conditions and bounds on the torsion.By Remark 5, we set ξ = 1.
Recall that from Theorem 1, application of PMP to P mec follows from PMP for the problem P MEC putting initial momentum λ 6 = 0.This yields the following ODE for the horizontal part: and for the vertical part, we obtain the ODE Here, λ(t) = 5 i=1 λ i (t)ω i | γ (t) is the momentum expressed in the moving dual frame of reference.This system of ODE's takes a simple form in s parametrization, where we have This system can be easily integrated as follows: where the last expression follows from the Hamiltonian and the restriction λ 3 > 0. Thus, we obtain two hyperbolic phase portraits (see Fig. 5).

Computation of the First Cusp Time
An arbitrary geodesic in P mec cannot be extended infinitely in s-parameterization, since in the common case its spatial projection presents a cusp, where spatial arclength sparametrization breaks down.In fact, for any given initial values of λ (1) (0) and λ (2) (0), the maximum length s max of such a geodesic, where we have κ(s) → ∞ as s ↑ s max , is given by the following theorem.
Clearly, the formulas are not valid when denominators vanish.Hence, we do the whole procedure keeping in mind the special cases (4.8), (4.10) right from the start and get the required results.

Geometric Properties of the Stationary Curves
Let a stationary curve of P curve in R 3 be given by x : [0, L] → R 3 parameterized by arclength denoted by s.Let the unit tangent, the unit normal, and the unit binormal for this curve be given by T, N, and B, respectively.Let κ and τ denote curvature and torsion, then the Frenet-Serret equations are d ds Let us first study the curvature and signed torsion of the spatial projection of the sub-Riemannian geodesics.Let us recall the first integral constants W = −λ 2 λ 5 − λ 1 λ 4 and and therefore Theorem 7 The absolute curvature and the signed torsion of a stationary curve of P curve are given by with momentum components λ i given by Eq. 4.3.We have the following fundamental relation between curvature and torsion The torsion is bounded as follows Proof In the proof, we use the following properties of norm, inner product, and cross product: First part follows by straightforward computation via Eqs.4.15 and 4.16.By definition, we have κ(s) = x (s) .Thus by Eq. 4.16 and the Hamiltonian H = 1 2 (λ 2 3 + λ 2 4 + λ 2 5 ) = 1 2 , we obtain, that the curvature satisfies (4.17).For arclength parametrized curve x(s) in R 3 , the torsion is given by τ = (x ×x )•x x ×x 2 (see e.g.[35]).Thus by Eq. 4.16, we have , and thereby τ satisfies (4.17).Equation 4.18 follows by λ 3 = ds dt and Eq.4.17.

Corollary 5
The cuspless spatial projections of sub-Riemannian geodesics of P mec with 3 i=1 λ 2 i (0) = 0 (i.e., the stationary curves of P curve ) are planar if and only if W = 0.
Next, we show that when taking the end conditions to be co-planar, one gets W = 0 implying that planar curves are the only cuspless geodesic of problem P mec connecting these conditions.
Theorem 8 Let x : [0, s max ] → R 3 be the spatial part of a cuspless sub-Riemannian geodesic of P mec given by Theorem 6, i.e., let x be a stationary curve of P curve .Then for any s ∈ (0, s max ], one has that e z , x(s) and x (s) are coplanar if and only if x is a planar curve, i.e.
Proof If W = 0, it follows by Corollary 5 that the curve is planar.By Theorem 6, we indeed get that W = 0 ⇒ ỹ(s) ≡ 0 and ỹ(s Now, we focus on the other direction of the implication.Let us consider curve x : [0, s max ] → S 2 .It can have a minimum curvature of 1 if it aligns with a great circle on S 2 .By Eqs.4.16 and 4.17, it follows that the geodesic curvature K tan of x (•) is given as Thus, W = 0 ⇒ K tan (s) > 1 for any s ∈ (0, s max ).The curve x gets aligned with great circles only at cusp points where κ(s) = ∞ which never occurs in an interior point.Thus the curve n = x can intersect a great circle on S 2 at most at two points.Therefore, any three points along this curve can never lie simultaneously on a plane passing through the origin.Thus The following corollaries relate the planar cuspless sub-Riemannian geodesics in (SE (3), , G 1 ) to those in (SE(2), ˜ , G1 ) with ˜ = ker{− sin θ dx + cos θ dy} and G1 = (cos θ dx + sin θ dy) ⊗ (cos θ dx + sin θ dy) + dθ ⊗ dθ), cf.[11].
Corollary 6 Given admissible coplanar end conditions for P curve , the unique cuspless stationary curve connecting them is planar.Now by the global optimality results in [6,11], we have the following corollary.2), which coincides with the set of admissible end conditions of P curve in SE (2).Then the set

Corollary 7 Let R denote the range of the exponential map of cuspless geodesics in SE(
) is a set of end conditions admitting a unique global cuspless minimizer of P curve .
Next, we show that the sub-Riemannian geodesics do not self intersect or roll up, despite the fact that the absolute curvature κ(s) → ∞ as s ↑ s max .
Next, we want to study bounds on the set R, recall (2.11).Our numerical investigations clearly show that the spatial part of all points in R is contained in the half space z ≥ 0, and that the plane z = 0 is only reached with U-shaped planar geodesics (i.e., W = 0, c < 1) at s = s max .These numerical observations inspired us to find the natural generalization of formal results in [11,Thm.7&8] on the SE(2)-case.We partly succeeded as we show in the next three corollaries, which provide bounds on the set R.
Corollary 12 Let (x 1 , −e z ) be the end condition of P curve with the initial condition (0, e z ).Then, a solution to problem P curve exists if and only if x 1 • e z = 0.Moreover, this condition is only possible for curves departing from a cusp and ending in a cusp.
Proof Let x be a solution to problem P curve with x (0) = −x (L) for some 0 < L ≤ s max .So we have x (0) = −x (L), which implies x (0) = − x (L).But this is possible if and only if x (0) = 0 = x (L), which is possible only if λ (2) (0) = 1 and L = s max , i.e., if the geodesic both starts and ends at a cusp.Then z(s max ) = 0 (see Corollary 11).

Symmetries of the Exponential Map
We now describe the symmetries of the exponential map of P curve , recall Definition 6.Here, we are interested in the symmetries that retain curvature and torsion along the curve and preserve direction of time (i.e., we do not consider the symmetries involving time inversion s → L − s, cf.[25]).From the conservation law and Eq.4.17 in Theorem 7, we deduce the following corollary.
Proof Proof can be found in [16].

Numerical Computations of the Jacobian of Exponential Map
In this section, we provide a numerical investigation into the absence of conjugate points on sub-Riemannian geodesics associated to the problem P curve .Recall that a conjugate point is a critical value of the exponential map (cf.Definition 6), i.e., at such a point one has det ∂(Exp(λ(0),L)) ∂(λ(0),L) = 0.
To compute the Jacobian numerically, we approximate the partial derivatives by finite differences: We verified that the Jacobian is always positive for a million random points within the domain D 0 of the exponential map of P curve , recall (2.10).The points were determined as follows.The first integrals c 2 = 3 i=1 λ 2 i (0) and 5 i=3 λ 2 i (0) = 1 allow us to introduce coordinates By the rotational symmetry presented in Corollary 13, we can reduce one parameter by setting φ 1 = 0. Furthermore, we recall that λ 3 (0) ≥ 0, which implies − π 2 ≤ φ 2 ≤ π 2 .Thus, we can parameterize the domain of exponential map by φ We consider c ∈ [cos φ 2 , 10], and compute the Jacobian in both a random and a uniform grid on (φ 2 , φ 3 , c, L).Here, the restriction of c from above is not crucial, as s max → 0 when c → ∞.Furthermore, the absence of conjugate points for short arcs of geodesics follows from general theory (see [1]).Finally, by Corollary 3, we have s max ≤ 1 2 log (1+c) 2 1+c 2 , that implies s max < 0.1 for c > 10.In Fig. 7, we show several trajectories of different types (U-shaped curves for c < 1 and S-shaped curves for c > 1) and corresponding plots of the Jacobian for s ∈ [0, s max ].Remarkably, the Jacobian is not just positive, it is even a monotonically increasing function of s for the range of the plot.A similar behavior for the Jacobian can be seen on the closely related problem P curve on R 2 (see.[11]), where the absence of conjugate points was proved.

The Range of the Exponential Map P curve
There are a number of restrictions on the possible terminal points reachable by sub-Riemannian geodesics of problem P mec with cuspless spatial projection.Together, such points form the range R of the exponential map of P curve , recall (2.11).We present some special cases which help us to get an idea about the range of the exponential map of P curve .Recall that Corollary 12 gave us the possible terminal positions (at z = 0) when the final direction is opposite to the initial direction.
Based on our numerical experiments, we pose the following conjecture which is analogous to a result in the 2D case of finding cuspless sub-Riemannian geodesics in (SE(2), ˜ , G1 ) [6,11].
Conjecture 1 Let the range of the exponential map defined in Definition 6 be denoted by R and let the domain D 0 of the exponential map be given by Eq. 2.10.Note that S B ∈ R and S R ∈ R but S L ∈ R.This conjecture would imply that no conjugate points arise within R and the problem P curve (1.1) is well posed for all end conditions in R. The proof of this conjecture would be on similar lines as in Appendix F of [11].If the conjecture is true, we have a reasonably limited set of possible directions per given end positions for which a cuspless sub-Riemannian geodesic of problem P curve exists.Then the cones of admissible end conditions for P curve (recall Definition 7) in Fig. 8a form the image of the boundary of the phase space of {λ (2) (0), λ (1) (0)} under the exponential map.Recall Fig. 5.These cones represent the boundary of the possible reachable angles by stationary curves of problem P curve .Figure 8b shows the special case of the end conditions being on a unit circle containing the z-axis.The final tangents are always contained within the cones at each position.Numerical computations indeed seem to confirm that this is the case (see Fig. 9).The blue points on the boundary of the cones correspond to S B while the red points correspond to S R given in Eq. 5.1.

Solving the Boundary Value Problem Associated to P curve
Using explicit formulas for sub-Riemannian geodesics obtained in Theorem 6 in Section 4.2, we developed a Wolfram Mathematica package, that numerically solves the boundary value problem (BVP) associated to P curve .The package is available by link http:// bmia.bmt.tue.nl/people/RDuits/final.rar.Note, that the BVP can be also tackled by a software Hampath [10] developed to solve optimal control problems.Hampath is based on numerical integration of a Hamiltonian system of PMP and second-order optimality conditions.In contrast to Hampath, our program does not involve numerical integration, and is based on numerical solution of a system of algebraic equations in Theorem 6, and relies on shooting based on the exact formulas, for details see [16].
Fig. 9 The spatial part of arbitrary cuspless geodesics in (SE(3), , G) and the cones of reachable angles as depicted in Fig. 8.Note that the cuspless geodesics are always contained within the cones.We checked this for many more cases, which supports our Conjecture 1

Conclusion
In this article, we have derived explicit exact formulas of the geodesics of problem P curve in Theorem 6 and Corollary 4, where because of a scaling homothety we can set ξ = 1.We have shown in Theorem 1 that they are spatial projections of special cases of sub-Riemannian geodesics within (SE(3), , G ξ =1 ) whose spatial projections are cuspless.We have characterized the set R of admissible end conditions for problem P curve in Corollary 1.In Theorem 2, we have proved Liouville integrability for the corresponding sub-Riemannian problem.In Theorem 5, we have computed the first cusp time t 1 cusp = t (s max ) of the sub-Riemannian geodesics explicitly.We have shown the following geometric properties of geodesics of P curve : • Global bounds on torsion in Theorem 7.
• They are planar if and only if the boundary conditions are coplanar, cf.Theorem 8.
• They do not self intersect or roll up, cf.Corollary 8.
• They stay in the half space prescribed by the orientation of the initial condition (i.e., z ≥ 0 if n 0 = e z ), which was formally shown for most cases (cf.Corollaries 9, 10, and 11).Also, we analyzed cases where the plane z = 0 is reached, cf.Corollary 11. • Their rotational and reflectional symmetries in Theorem 13.
Finally, we provided a numerical Mathematica package to solve the boundary value problem via a shooting algorithm.We included numerical support for the expected absence of conjugate points on the sub-Riemannian geodesics (with λ 6 (0) = 0) with cuspless spatial projections, i.e., t conj ≥ t 1 cusp .Also, numerical computations on the cones of reachable angles (and their boundaries, cf.Fig. 8) seem to reveal the same homeo/diffeomorphic properties of the exponential map integrating the canonical ODE's in Pontryagin maximum principle, that were formally shown on the SE(2) case [11,Thm.6&9].In future work, we plan to analyze the sub-Riemannian spheres via viscosity solutions of sub-Riemannian HJBequations, i.e. extend the work [3] to the SE(3) case.This will yield a numerical computation of the sub-Riemannian spheres and the 1st Maxwell-set.

Fig. 1
Fig. 1 Left: Illustration of problem P curve .Isotropy in the brown tangent plane spanned by {A 1 , A 2 } is needed for a well-posed problem on the Lie group quotient SE(3)/({0} × SO(2)).The tangent vectors n 0 and n 1 are depicted in red.Right: the angular part n(s) = x (s) of the lifted curve (x(s), x (s)) ∈ R 3 × S 2

(4. 21 )
with Q ∈ O(2) arbitrary.Then we have the following symmetry property of the exponential map:

Fig. 6 a
Fig. 6 a Rotational symmetries in case of several planar cuspless sub-Riemannian geodesics of problem P mec departing from e in direction of e z .b Reflectional symmetry of certain cuspless geodesics of P mec .These curves are produced by rotating λ(1) (0) by certain angles while keeping λ(2) (0) fixed.The plane of reflection contains the middle curve with λ(1) (0) parallel to λ(2) (0)

Fig. 7 aFig. 8 a
Fig. 7 aCuspless projections of sub-Riemannian geodesics in problem P mec of different types.U-curves are depicted in green and blue colors, and S-curves are depicted in red and purple.b Plot of the Jacobian of exponential map, corresponding to these geodesics.We see that the Jacobian is positive (even increasing) for all s ∈ (0, s max ).This supports our conjecture that conjugate points are absent before the first cusp point t) A i | γ (t) , γ (s) = 1 A 3 | γ (s) + u 4 (s) A 4 | γ (s) + u 5 (s) A 5 | γ (s) .Lifting of a curve x(•) to a curve (x(•), n(•)) into R 3 S 2 is done by setting x (s) = n(s).Let c k i,j denote the usual structure constants of the Lie algebra of SE(3) (see Table1