{'fraction_non_alphanumeric': 0.04093297685996128, 'fraction_numerical': 0.012384376632555852, 'mean_word_length': 4.258038455323962, 'pattern_counts': {'":': 0, '<': 6, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 18, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Dynamic interpolation for obstacle avoidance on Riemannian manifolds

This work is devoted to studying dynamic interpolation for obstacle avoidance. This is a problem that consists of minimizing a suitable energy functional among a set of admissible curves subject to some interpolation conditions. The given energy functional depends on velocity, covariant acceleration and on artificial potential functions used for avoiding obstacles. We derive first-order necessary conditions for optimality in the proposed problem; that is, given interpolation and boundary conditions we find the set of differential equations describing the evolution of a curve that satisfies the prescribed boundary values, interpolates the given points and is an extremal for the energy functional. We study the problem in different settings including a general one on a Riemannian manifold and a more specific one on a Lie group endowed with a left-invariant metric. We also consider a sub-Riemannian problem. We illustrate the results with examples of rigid bodies, both planar and spatial, and underactuated vehicles including a unicycle and an underactuated unmanned vehicle.


I. INTRODUCTION
Motion planning is an important task in numerous engineering fields such as air traffic control, aeronautics, robotics and computational anatomy. In the last few decades research in the calculus of variations and optimization has provided several methods for trajectory planning with smooth interpolation by means of the study of higher-order variational problems [1], [14], [16], [19], [28], [35], [40], [44].
In this class of problems the aim is to plan smooth trajectories passing through determined points at specific times. To achieve this goal, second order variational methods have been successfully used, providing interpolating curves, the so-called Riemannian cubic splines and cubic splines in tension. These curves are better interpolating curves than geodesics, which usually fail smoothness requirements for the trajectories. The variational problems consist of minimizing an energy functional, depending on the covariant acceleration and velocity, among a set of admissible curves interpolating a given set of points. Dynamic interpolation problems were initially studied by A. Bloch  Crouch and Jackson [15] for applications in aeronautics and further explored from the geometric point of view by Noakes et al. [40] and Crouch and Silva Leite [16], [17], [41].
Crouch and Silva Leite [17] started the study of geometric properties of cubic polynomials on Riemannian manifolds, in particular on compact and connected Lie groups and symmetric spaces. Further extensions were developed by Bloch and Crouch [7], [8] in the context of sub-Riemannian geometry. Sub-Riemannian problems are variational in nature with additional nonholonomic constraints. Although nonholonomic variational problems do not give the correct approach to the study of nonholonomic mechanical systems, they provide an appropriate formulation for optimal control of kinematic underactuated control systems, including rigid body control systems, which are of interest in the areas of aeronautics and robotics, since they are kinematic models for aerospace and underwater unmanned vehicles [30], [37], [43].
Over the last few decades many authors have studied the problem of trajectory planning of autonomous vehicles in the presence of static obstacles in the workspace. Artificial potential functions [29] (as for instance, a Coulomb potential) have frequently been used for avoiding collision with obstacles, playing a fundamental role in these studies. These functions are created to simulate a fictitious repulsion from determined obstacles given by regions in the configuration space. This approach has been studied by Khabit for robotic manipulators (see [29] and references therein), and further studied by Koditschek [32] in the context of mechanical systems and Fiorelli and Leonard [36] for multi-agent formation. The mathematical foundations for the existence of such a smooth functions on any smooth manifold can be found in the works of Smale [42], [25].
In this paper, we aim to generate trajectories interpolating prescribed points and avoiding multiple obstacles in the workspace via the study of a second order variational problem on a Riemannian manifold M by an extension of the results presented in [5] for variational obstacle avoidance without interpolation points. We call this problem dynamic interpolation for obstacle avoidance. We study the problem in different scenarios: a general one on a Riemannian manifold and a more specific one on a Lie group endowed with a left-invariant metric, which is the appropriate setting for the examples we are interested in. We also consider the corresponding sub-Riemannian problem where we must deal with constraints on the velocities defined by a non-integrable distribution on M. We illustrate the results with the examples of rigid bodies, both planar and spatial, and underactuated vehicles including the unicycle.
Our design for interpolation among piecewise smooth trajectories is independent of the knowledge of mass and inertia coefficients, leading to robustness for parameter uncertainty. Moreover, the solution for the sub-Riemannian problem allows for vehicle designs that include fewer actuators than is typical, leading to lighter, less costly design.
The framework proposed here for dynamic interpolation with obstacle avoidance of kinematic control systems on Lie groups endowed with a left-invariant metric should be useful for control design for a general class of systems including spacecraft and underactuated vehicles. In general, the configuration space for these systems is globally described by a matrix Lie group making this model a natural choice for the controlled system. The Lie group description gives rise to coordinate-free expressions for the dynamics determining the behavior of the system. When systems on Lie groups are left invariant, there is a natural "globalization" of solutions, that is, even if we exploit local charts to design local maneuvers, the Lie group formalism allows us to move over the entire configuration space without reformulating the controls. This is because we can always left translate back to the identity of the group.
We extend previous results on dynamic interpolation on Riemannian manifolds to include obstacle avoidance.
The main advantage of this approach is that it can be used to design global motions for many systems of practical interest where obstacle avoidance is necessary. The results of this work can be applied to a wide range of problems in systems and control such as spacecraft docking [24], quantum control [18], control of quadrotor UAVs [33], multi-agent systems [34], DNA structures [22], control of marine cables/rods [10] and constrained under-actuated spacecraft [2], [3], [38] among others.
The structure of the paper is as follows.
In section 2 we review the main topics we use from Riemannian geometry and consider the variational obstacle avoidance problem on Riemannian manifolds.
In section 3 we introduce interpolation points into the previous framework in order to formulate the dynamic interpolation for obstacle avoidance problem. We derive first order necessary conditions for optimality. By introducing the structure of a left-invariant Riemannian metric we study the problem on a Lie group. In section 4 we extend our analysis to the sub-Riemannian situation characterizing stationary paths for an extended action integral with constraints in velocities.
We apply the results to the dynamic interpolation for obstacle avoidance problems of several rigid bodies type systems, both planar and spatial, on the Lie groups SE(2) and SE(3), respectively, in section 3, and underactuated vehicles in section 4. Final comments and ongoing work are discussed at the end of the paper.

A. Preliminaries on Riemannian Geometry
Let M be a smooth (C ∞ ) Riemannian manifold with the Riemannian metric denoted by ·, · . The length of a tangent vector is determined by its norm, ||v A Riemannian connection ∇ on M, is a map that assigns to any two smooth vector fields X and Y on M a new vector field, ∇ X Y , called the covariant derivative of Y with respect to X. For the properties of ∇, we refer the reader to [11], [12], [39].
Consider a vector field W along a curve x on M. The sth-order covariant derivative along x of W is denoted by D s W dt s , s ≥ 1. We also denote by D s+1 x dt s+1 the sth-order covariant derivative along x of the velocity vector field of x, s ≥ 1.
Given vector fields X, Y and Z on M, the vector field R(X, Y )Z given by Lemma 2.1 ( [11], [7]): Let ω be a one form on (M, ·, · ). The exterior derivative of a one form ω is given by for all vector fields X, Y on M. In particular, if ω(X) = W, X it follows that Let Ω be the set of all C 1 piecewise smooth curves x : [0, T] → M in M such that x(0), x(T), dx dt (0) and dx dt (T) are fixed. The set Ω is called the admissible set.
The variational vector field associated to an admissible variation α is a C 1 -piecewise smooth vector field along x defined by verifying the boundary conditions where the tangent space of Ω at x is the vector space T x Ω of all C 1 piecewise smooth vector fields X along x verifying the boundary conditions (4).
and define the functional J on Ω given by This functional is given by a weighted combination of the velocity and covariant acceleration of the curve x regulated by the parameter σ, together with an artificial potential function V : M → R used to avoid the obstacle.
The obstacle is described by a region in M bounded by S.
The potential function V is an artificial smooth (or at least C 2 ) potential function associated with a fictitious force inducing a repulsion from S. We consider S to be the regular zero level set defined by a scalar valued smooth function f , for instance, used to describe obstacles as circles in the plane for 2D vehicles, spheres or ellipsoids in the space for 3D vehicles, and orientations in the space for 3D rigid bodies.
To avoid collision with obstacles we introduce a potential function V defined as the inverse value of the function f . The function V goes to infinity near the obstacle and decays to zero at some positive level set far away from the obstacle S. This ensures that such an optimal trajectory does not intersect S. The use of artificial potential functions to avoid collision was introduced by Khatib (see [29] and references therein) and further studied by Koditschek [32].
Problem 1: The variational obstacle avoidance problem consists of minimizing the functional J on Ω.
In order to extremize the functional J on the set Ω one needs to compare the value of J at a curve x ∈ Ω to the value of J at a nearby curvex ∈ Ω, using one-parameter admissible variations of x in Ω. We recently proved in [5] the following result.
Remark 2.4: In the absence of obstacles, we consider V = 0 and equation (7) reduces to which gives the so called cubic polynomials in tension on Riemannian manifolds [41], that is, smooth trajectories on M, given by the extremals among Ω of the action functional When the parameter σ is zero, these curves are Riemannian cubic polynomials [16], [40] and, for nonzero values of σ, as σ increase the curves approximate more precisely the geodesics joining the same points. These curves have many applications in physics and engineering (see for instance [28], [31]).

III. DYNAMIC INTERPOLATION FOR OBSTACLE AVOIDANCE PROBLEMS ON RIEMANNIAN MANIFOLDS
Now we consider the following problem of dynamic interpolation for obstacle avoidance. We start by studying the general case on a Riemannian manifold and then the case of Lie groups. We illustrate the results with several examples.

A. Dynamic interpolation for obstacle avoidance: The general case
Consider a set of distinct points x 0 , x 1 , . . . , x N ∈ M such that each x j does not intersect S, j = 0, . . . , N, and a set of fixed times which are smooth on [T i−1 , T i ], i = 1, . . . , N, and verify the interpolation conditions and the boundary conditions The tangent space T x Ω to the curve x ∈ Ω is defined to be the vector space of C 1 vector fields X on [0, T], which are smooth on [T i−1 , T i ] and satisfy the conditions X(T i ) = 0, i = 0, . . . , N and Problem 2: The problem of dynamic interpolation for obstacle avoidance consists of minimizing the functional J on Ω.
Theorem 3.1: A necessary condition for x to be an extremizer of the functional (6) over the class Ω is that x is C 2 and verifies the following equation Proof: Let α be an admissible variation of x with variational vector field X ∈ T x Ω. Then By considering the gradient vector field grad V of the potential function V we have By Lemma 2.2 and the previous identity we have Since α is smooth on [T i−1 , T i ], integrating the first term by parts twice, and the third term once, on each interval, and applying the property (2) of the curvature tensor R to the second term, we obtain Next, by taking r = 0 in the last equality, we obtain Since the vector field X is C 1 , piecewise smooth on [0, T], verifies the boundary conditions (12) and the curve Now, assume x is an extremizer of J over Ω. Then d dr J(α r ) | r=0 = 0, for each admissible variation α of x with variational vector field X ∈ T x Ω.
Let us consider X ∈ T x Ω defined by Finally, let us choose the vector field X ∈ T x Ω so that

Remark 3.2:
In the absence of obstacles, we consider V = 0 and equation (8), defined on each subinterval gives rise to the generalization of cubic splines in tension to Riemannian manifolds [41].
The next result gives an extension of Theorem 3.1 for multiple obstacles. The proof does not differ of the one given in Theorem 3.1 except for the term concerning the potential function.
Assuming that in the workspace we have s obstacles, the functional (6) becomes in where each obstacle is represented by S r and the artificial potential function V r corresponding to the obstacle S r is defined as before, r = 1, . . . , s.

Corollary 3.3:
A necessary condition for x to be an extremizer of the functional (14) over the class Ω is that x is C 2 and verifies the following equation Remark 3.4: We would like to point out that it is not guaranteed that the action functional J can achieve a minimum value at an interpolating curve. Indeed, in the work [23] authors find conditions on the Riemannian manifolds for which cubic splines do not exist (Lemma 2.15 in [23]), that is, non-existence conditions for the critical paths of the dynamic interpolation problem when the artificial potential is zero everywhere and the elastic parameter τ is zero.
In [20] authors study the existence of global minimizers for the variational problem (Problem 1) in complete Riemannian manifolds when the artificial potential is zero everywhere and the elastic parameter τ is zero (the critical paths correspond to Riemannian cubic polynomials). Such a result establishes existence conditions for global minimizers by an understanding of the variational problem as one in a Hilbert manifold setting and using techniques of calculus of variations and global analysis on manifolds.

B. Dynamic interpolation for obstacle avoidance problems on a Lie group
Now we consider a Lie group G endowed with a left-invariant Riemannian metric < ·, · >, with I : g × g → R the corresponding inner product on the Lie algebra g, a positive-definite symmetric bilinear form in g. The inner product I defines the metric < ·, · > completely via left translation (see for instance [12] pp. 273).
The Levi-Civita connection ∇ induced by < ·, · > is an affine left-invariant connection and it is completely determined by its restriction to g via left-translations. This restriction, denoted by g ∇: g × g → g, is given by (see [12] p. 271) where ad * : g × g * → g * is the co-adjoint representation of g on g * and where I : g * → g, I : g → g * are the associated isomorphisms with the inner product I (see [11] for instance).
We denote by u L the left-invariant vector field associated with u ∈ g. For the left-invariant vector fields u L and w L , the covariant derivative of u L with respect to w L is given by Let x : I ⊂ R → G be a smooth curve on G. The body velocity of x is the curve v : Let {e 1 , . . . , e n } be a basis of g. The body velocity of x on the given basis is described by . . , v n are the so-called pseudo-velocities of the curve x with respect to the given basis. The velocity vector can be written in terms of the pseudo-velocities as follows.
When the body velocity is interpreted as a control on the Lie algebra, equations (17) give rise to the so called left-invariant control systems discussed in [37]. Therefore our analysis also includes this class of kinematic control systems.
To write the equations determining necessary conditions for optimality, we must use the following formulas (see [1], Section 7 for more details).
where R denotes the restriction of the curvature tensor to g.
together with equation (17), subject to the interpolation conditions As in the previous subsection, in the presence of s obstacles, the previous equation reads Example 3.6: Dynamic interpolation for obstacle avoidance on SE(3).
Next, as an application of Proposition 3.5, we study the variational interpolation problem for the motion of the attitude and translation of a rigid body where the configuration space is the special Euclidean group SE(3) and a spherical obstacle in the workspace must be avoided. Working on SE(3) we represent the orientation and position of the rigid body in a coordinate free framework . This example corresponds to the dynamic interpolation for obstacle avoidance associated with the dynamics of an aerospace or underwater vehicle (see for instance [12] p. 281).
We describe the movement of the rigid body by a curve in SE (3). The special Euclidean group SE (3) consists of all rigid displacements in R 3 , described by a translation after a rotation. Its elements are the transformations of  , where a = (a 1 , a 2 , a 3 ) ∈ R 3 . We identify the Lie algebra se(3) with R 6 via the isomorphism a b 0 0 → (a, b). The Lie bracket in R 6 is given by [(a, b), elements (a, b) are called twist coordinates.
The adjoint action is given by We consider the basis {e i } 6 i=1 of se (3), represented by the canonical basis of R 6 , given by where J i and m i are the diagonal elements of the matrix defining the dynamics of the rigid body, the inertia moments and masses, with i = 1, 2, 3 and {e i } 6 i=1 , the dual basis of {e i } 6 i=1 . The Levi-Civita connection ∇ induced by < ·, · > is completely determined by its restriction to se(3) and is given by (see for instance [12] p. 282) where J and M are the blocks of the diagonal matrix describing the dynamics of the body, representing the moments of inertia and masses respectively, and v = (a, b), z = (c, d) ∈ R 6 are the twist coordinates.
For simplicity in the exposition, we consider the case when J = M = I. Then the formula above for the Levi-Civita connection reduces to se(3) Using (1) we obtain the restriction of the curvature tensor to se(3) given by where as before v = (a, b), z = (c, d) and w = (h, f ) ∈ R 6 represent the twist coordinates.
The motion of the rigid body in space is described by a curve x in SE(3). The body velocity is given by the The first three terms correspond to infinitesimal rotations about the three axes (roll, pitch and yaw) and the later three terms to infinitesimal translations about the three axes.
We consider the potential functions R ∈ SO(3), r ∈ R 3 \S 2 , designed for avoidance of two obstacles with spherical shape, the first with unit radius centered at the origin and the second with radius √ 2 centered at p = (2, 2, 2). Here τ 1 , τ 2 ∈ R + and || · || is the Euclidean norm.
A form of Euler-Poincaré equations can be obtained as in [6] using the SO(3)-invariance of V 1 and V 2 . We will study that approach in future work. Here we study the dynamics using the representation given by the Lie algebra isomorphism se(3) R 6 . The gradient of V 1 satisfies Similarly, the gradient of V 2 satisfies T g L g −1 (grad V 2 (g)) = − 2τ 2 ( r − p 2 −2) 2 0, R T r . By Proposition 3.5 the necessary conditions for the extremizer in problem 2 are determined by the equations  In Figure 2 we show a simulation of our method. A shooting method and a symplectic Euler discretization with h = 0.01 are used to simulate the boundary value problem. The curve represents the optimal trajectory interpolating the prescribed points and boundary values. One interpolation point has been taken to be close to one obstacle and between the two prescribed obstacles.

C. Dynamic Interpolation for obstacle avoidance problems on compact and connected Lie groups
Next, we derive the equations for the dynamic interpolation problem obtained in the previous subsection in the case when the Lie group is compact and connected.
Assume G is a connected and compact Lie group. Therefore G is endowed with a bi-invariant Riemannian metric that makes G a complete Riemannian manifold. In this context the Riemannian distance between two points in G can be defined by means of the Riemannian exponential on G, that is, We need to guarantee that the exponential map exp h is a local diffeomorphism, so we assume that the point g must belong to a convex open ball around h. If we consider the geodesic from g to h given by γ g,h (s) = exp g (s exp −1 g h), s ∈ [0, 1], then, because dγ g,h ds (s) is independent of s, we can write The obstacle is represented by an element h in G and the artificial potential function, used to avoid the obstacle, is defined by .
If we consider a map α : r → α(r) verifying α(0) = g and the family of geodesics from g to α(r) given by γ(s, r) = exp h (s exp −1 h α(r)), then we have d dr and we obtain the expression of the gradient vector field as follows: The Levi-Civita connection and the curvature tensor of G, when restricted to the Lie algebra g of G, are defined Consider as before the body velocity v of a curve x on the Lie group G with respect to a basis {e 1 , . . . , e n } of the Lie algebra g. Using (25)- (26), equation (24) and Corollary 3.5, the equations describing first-order optimality conditions for Problem 2 on a connected and compact Lie group are obtained as follows:  ∇ w u = 1 2 w × u and, using (26), the restriction of the curvature tensor to so (3) is defined by The motion of the rigid body in space is described by a curve t → R(t) in SO (3). The columns of the matrix R(t) represent the directions of the principal axis of the body at time t with respect to some reference system. The body angular velocity is given by For the obstacle avoidance problem we consider the navigation functions V : SO(3) → R given by representing a repulsive potential function to avoid the obstacle Q ∈ SO(3), with τ ∈ R + , R ∈ SO(3) and exp representing the exponential map on SO(3).

Given that
the necessary conditions for optimality are determined by the equation together with the equation R = Rv, the interpolation points R(T i ) = R i and the boundary conditions R(0) = R 0 , Note that in the absence of obstacles, the extremals reduce to the cubic splines in tension on SO(3), [41] where the equations are given by solutions of the equation

IV. SUB-RIEMANNIAN DYNAMIC INTERPOLATION FOR OBSTACLE AVOIDANCE
Next, we extend our analysis to the sub-Riemannian setting, that is, we consider that the velocity vector field dx dt lies on some distribution D on M. We assume that D is a constant dimensional nonintegrable distribution and that there exist k linearly independent one-forms ω 1 , . . . , ω k , such that the codistribution annihilating D is spanned by ω 1 , . . . , ω k (k < n). The constraints on the velocity vector field are defined by where Y 1 , · · · , Y k are linearly independent vector fields on M.
To deal with the constraints we also need to define the tensors S j , (S j ) x : T x M → T x M, given by Problem 3: The sub-Riemannian dynamic interpolation problem for obstacle avoidance consists of minimizing the functional J defined in (5) on Ω with the additional constraints (28).
This type of problem, in the absence of obstacles, was studied in Bloch and Crouch [7] and Crouch and Silva Leite [17].
We derive optimality conditions for this sub-Riemannian problem, by extending our previous analysis for the general case following the result of Bloch and Crouch [7], [8].
Theorem 4.1: A necessary condition for x ∈ Ω to give a normal extremum for problem 3 is that x is C 2 and there exist smooth functions λ j , j = 1, · · · , k (the Lagrange multipliers) such that the following equation holds . . , N, together with Y j , dx dt = 0, j = 1, · · · , k. Proof: Consider the extended functional We derive necessary conditions for existence of normal extremizers by studying the equation for α an admissible variation of x with variational vector field X ∈ T x Ω and λ j the Lagrange multipliers.
Taking into account the proof of Theorem 3.1 we only need to study the influence of variations in the term After integration by parts in the second term and evaluating at r = 0, the integrand can be re-written with the additional terms Using the identity (3) the new terms compared with the ones provided by Theorem 3.1 which give rise to optimality conditions for x to be a normal extremizer in this sub-Riemannian problem are: Using the fact that dω j dx dt , X = S j dx dt , X the result follows. Remark 4.2: The introduction of constraints in the velocities for the collision avoidance variational problem causes difficulties in the study of both geometrical and analytical aspects, as remarked in [21], and leads to sophisticated situations as when abnormal minimizers appear. As far as we know there is no definitive result yet regarding existence and regularity for minimizers in this sub-Riemannian variational problem. It would be very interesting to explore the geometrical and analytical aspects for the existence of minimizers in the collision avoidance problem under constraints in the velocities.
The following corollaries are direct consequences of the results presented in Section 3 for the sub-Riemannian problem by straightforward modifications in the proof of Theorem 4.1.

Corollary 4.4:
If the number of obstacles on M is s, located at the points q r ∈ M, r = 1, . . . s, a necessary condition for x ∈ Ω to be a normal extremizer for the sub-Riemannian dynamic interpolation for obstacle avoidance is that x is C 2 and there exist smooth functions λ j , j = 1, · · · , k (the Lagrange multipliers) such that the following equation holds . . , N, together with Y j , dx dt = 0, j = 1, · · · , k. Now we consider the problem on a Lie group G as we did in section 3.2. We suppose that the constraints on the velocity vector field are defined by a left-invariant distribution D. Let {e 1 , . . . , e n } be an orthogonal basis of the Lie algebra g in such a way that the constraints are given by the left-invariant one-forms ω j associated with e j , j = 1, . . . , k. We have, as before, but now Y 1 , · · · , Y k , · · · , Y n form a basis of orthogonal left-invariant vector fields associated with the elements of the basis of g. Moreover, since the basis of g is orthogonal, the vector fields Y k+1 , · · · , Y n span the distribution D. Furthermore, the 2-forms dω j are left-invariant and there exist linear maps S j : g → g such that dω j (z, w) = I(S j (z), w), z, w ∈ g. These maps can be expressed by S j = T x L x −1 • (S j ) x • T e L x with (S j ) x as in (29).

Corollary 4.5:
A necessary condition for x ∈ Ω to be a normal extremizer for the problem 3 in the Lie group G is that x is C 2 and there exist smooth functions λ j , j = 1, · · · , k (the Lagrange multipliers) such that the following equation holds . . , N, together with the equation (17) and the constraints v j = 0, j = 1, . . . , k, subject to boundary conditions Example 4.6: Dynamic interpolation for obstacle avoidance of a unicycle.
We study the motion planning of a unicycle with one obstacle in the workspace. The unicycle is a homogeneous disk on a horizontal plane and it is equivalent to a knife edge on the plane [4], [12]. The configuration of the unicycle at any given time is completely determined by an element of the special Euclidean group SE (2). where m and J are the mass of the body and its inertia moment about the center of mass and and The Levi-Civita connection ∇ induced by < ·, · > is defined by its restriction to se(2) given by se (2) ∇ : se(2)×se(2) → se(2) and given by are the representative elements of se(2) in R 3 (see [12] p. 279). The curvature tensor is zero.
We consider the potential functions V 1 (R, r) and V 2 (R, r) given by V 1 and V 2 are introduced to avoid two obstacles with circular shape in the xy-plane. The first has unit radius and is centered at the origin. The second has radius √ 2 and is centered at p = (2, 2). τ 1 , τ 2 ∈ R + and || · || is the Euclidean norm.
The knife edge constraint is defined by the one-form ω = sin θdx − cos θdy whose associated vector field with respect to the Riemannian metric is Note that the tensor S associated with the knife edge constraint is defined by (29) and satisfies for each vector field on SE(2) denoted by U = u 1 ∂ ∂θ Here, we think of S(U)(x) as S x (U(x)), x ∈ M, where S x : T x M → T x M, x ∈ M, is the tensor S defined in (26) and U : M → T M; x → U(x) is a vector field.
We consider a basis of vector fields u i e i ∈ se(2).
By Corollary 4.5 the equations determining necessary conditions for normal extremizers in problem 3 are  In the absence of velocity constraints, the model studied in this example corresponds with a free planar rigid body. The trajectory planning without interpolation points for the obstacle avoidance problem of a planar rigid body was studied using a similar framework previously by the authors in [5] (Section V-A).

V. FINAL DISCUSSION AND FUTURE RESEARCH
We studied the problem of dynamic interpolation for obstacle avoidance on Riemannian manifolds and derived necessary optimality conditions for the trajectory planning problem of mechanical systems specified by a kinetic energy given by a Riemannian metric.
Such optimallity conditions specify a motion of a system along the workspace, interpolating specific points at given times, satisfying boundary conditions, and minimizing an energy functional which depends on an artificial potential function used to avoid static obstacles. Different scenarios were studied: the problem on a Riemannian The proposed method provides a motion planning algorithm for a class of mechanical control systems that does not require the use of local coordinates in the configuration space. While we cannot claim rigorously that equation (7) has a solution, given boundary conditions we provide a numerical solution based on Euler's symplectic method which gives a curve that satisfy the necessary and boundary conditions, and between interpolation points, we solve a boundary value problem by using a shooting method.
The variational approach proposed in this work for the obstacle avoidance problem allows us to further study second order optimality conditions for the dynamic interpolation problem and therefore it may be possible to use the approach presented in this work for necessary (first order) conditions to find sufficient (second order) optimality conditions. The existence of global minimizers for the dynamic interpolation problem with obstacle avoidance can be analyzed using similar techniques to the ones developed in [20], [21].
It is well known that the Pontryagin maximum principle (PMP) can give first order conditions for optimality. As far as we know, such an approach for obstacle avoidance with dynamic interpolation does not exist in the literature.
We believe that the study of such a dynamic interpolation problem from the point of view of PMP, as well as the comparison between both approaches, provides an interesting analysis of the problem discussed in this work.
The study of higher-order variational problems on symmetric spaces and reduction theories for variational problems has attracted considerable interest and has been carried out systematically by several authors. In future work we intend to introduce interpolation points into such problems and extend the main results presented in this paper to this setting. We will also intend to extend our work to dynamic interpolation for obstacle avoidance with moving obstacles.