University of Pennsylvania ScholarlyCommons

Department of Mechanical Engineering & Departmental Papers (MEAM) Applied Mechanics

January 2002

Euclidean metrics for motion generation on SE(3)

Calin Andrei Belta University of Pennsylvania

R. Vijay Kumar University of Pennsylvania, [email protected]

Follow this and additional works at: https://repository.upenn.edu/meam_papers

Recommended Citation Belta, Calin Andrei and Kumar, R. Vijay, "Euclidean metrics for motion generation on SE(3)" (2002). Departmental Papers (MEAM). 150. https://repository.upenn.edu/meam_papers/150

Reprinted from Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, Volume 216, Number 1, 2002, pages 47-60. Publisher URL: http://dx.doi.org/10.1243/0954406021524909

This paper is posted at ScholarlyCommons. https://repository.upenn.edu/meam_papers/150 For more information, please contact [email protected]. Euclidean metrics for motion generation on SE(3)

Abstract Previous approaches to trajectory generation for rigid bodies have been either based on the so-called invariant screw motions or on ad hoc decompositions into rotations and translations. This paper formulates the trajectory generation problem in the framework of Lie groups and Riemannian geometry. The goal is to determine optimal curves joining given points with appropriate boundary conditions on the Euclidean group. Since this results in a two-point boundary value problem that has to be solved iteratively, a computationally efficient, analytical method that generates near-optimal trajectories is derived. The method consists of two steps. The first step involves generating the optimal trajectory in an ambient space, while the second step is used to project this trajectory onto the Euclidean group. The paper describes the method, its applications and its performance in terms of optimality and efficiency.

Keywords interpolation, lie groups, invariance, optimality

Comments Reprinted from Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, Volume 216, Number 1, 2002, pages 47-60. Publisher URL: http://dx.doi.org/10.1243/0954406021524909

This journal article is available at ScholarlyCommons: https://repository.upenn.edu/meam_papers/150 SPECIAL ISSUE PAPER 47

Euclidean metrics for motion generation on SE 3

C Belta* and V Kumar General Robotics, Automation, Sensing and Perception Laboratory, University of Pennsylvania, Philadelphia, USA

Abstract: Previous approaches to trajectory generation for rigid bodies have been either based on the so-called invariant screw motions or on ad hoc decompositions into rotations and translations. This paper formulates the trajectory generation problem in the framework of Lie groups and Riemannian geometry. The goal is to determine optimal curves joining given points with appropriate boundary conditions on the Euclidean group. Since this results in a two-point boundary value problem that has to be solved iteratively, a computationally efŽ cient, analytical method that generates near-optimal trajectories is derived. The method consists of two steps. The Ž rst step involves generating the optimal trajectory in an ambient space, while the second step is used to project this trajectory onto the Euclidean group. The paper describes the method, its applications and its performance in terms of optimality and efŽ ciency.

Keywords: interpolation, lie groups, invariance, optimality

NOTATION T PM tangent space at P [ M to manifold M v linear velocity in {M } A homogeneous transformation matrix V velocity vector [ SE n x, y, z Cartesian axes A acceleration vector X, Y tangent vectors B afŽ ne transformation [ GA 3 d position vector in {F} s exponential coordinates on SO 3 D dt covariant derivative angular velocity in {M } {F} reference (Ž xed) frame , Riemmanian metric G matrix of metric in SO 3 G˜ matrix of metric in SE 3 GA n afŽ ne group GL n general linear group of dimension n 1 INTRODUCTION Li basis vector in se 3 0 Li basis vector in so 3 M non-singular matrix [ GL 3 The problem of Ž nding a smooth motion that inter- {M } mobile frame polates between two given positions and orientations in 3 O origin of {F} is well understood in Euclidean spaces [1, 2], but it is O origin of {M } not clear how these techniques can be generalized to R rotation matrix [ SO n curved spaces. There are two main issues that need to be n Euclidean space of dimension n addressed, particularly on non-Euclidean spaces. It is se n Lie algebra of SE n desirable that the computational scheme be independent so n Lie algebra of SO n of the description of the space and invariant with respect S twist [ se 3 to the choice of the coordinate systems used to describe SE n special Euclidean group the motion. Secondly, the smoothness properties and the SO n special orthogonal group optimality of the trajectories need to be considered. Tr matrix trace Shoemake [3] proposed a scheme for interpolating rotations with Bezier curves based on the spherical analogue of the de Casteljau algorithm. This idea was The M S was received on 2 M arch 2001 and was accepted after revision extended by Ge and Ravani [4] and Park and Ravani [5] for publication on 6 November 2001. to spatial motions. The focus in these articles is on the * Corresponding author: General Robotics, Automation, Sensing and Perception ( GRASP) Laboratory, University of Pennsylvania, 3401 generalization of the notion of interpolation from the W alnut S treet, Philadelphia, PA 19104–6228, USA. Euclidean space to a curved space.

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 48 C BELTA AND V KUMAR

2 Another class of methods is based on the representa- subset of n . M oreover, matrix multiplication and tion of Bezier curves with Bernstein polynomials. Ge inversion are both smooth operations, which make and Ravani [6] used the dual unit quaternion representa- GL n a Lie group. The special orthogonal group is a tion of SE 3 and subsequently applied Euclidean subgroup of the general linear group, deŽ ned as methods to interpolate in this space. Ju¨tler [7] formu- T lated a more general version of the polynomial inter- SO n R R [ GL n , RR I, det R 1 polation by using dual (instead of dual unit) quaternions where SO n is referred to as the rotation group on to represent SE 3 . In such a representation, an element n n of SE 3 corresponds to a whole equivalence class of , GA n GL n 6 is the afŽ ne group and SE n SO n 6 n is the special Euclidean group and is the dual quaternions. Park and Kang [8] derived a rational n interpolating scheme for the group of rotations SO 3 set of all rigid displacements in . Special consideration by representing the group with Cayley parameters and will be given to SO 3 and SE 3 . using Euclidean methods in this parameter space. The Consider a rigid body moving in free space. Assume advantage of these methods is that they produce rational an inertial reference frame F Ž xed in space and a curves. frame M Ž xed to the body at point O as shown in It is worth noting that all these works (with the Fig. 1. At each instance, the conŽ guration (position and exception of reference [5]) use a particular coordinate orientation) of the rigid body can be described by a representation of the group. In contrast, Noakes et al. homogeneous transformation matrix, A, corresponding [9] derived the necessary conditions for cubic splines on to the displacement from frame F to frame M . general manifolds without using a coordinate chart. SE 3 is the set of all rigid body transformations in three These results are extended in reference [10] to the dimensions: dynamic interpolation problem. Necessary conditions R d for higher-order splines are derived in reference [11]. A SE 3 A A , R SO 3 , d 3 0 1 [ [ coordinate-free formulation of the variational approach » µ ¶ ¼ was used to generate shortest paths and minimum SE 3 is a closed subset of GA 3 , and therefore a Lie acceleration and jerk trajectories on SO 3 and SE 3 in group. reference [12]. However, analytical solutions are avail- On any Lie group the tangent space at the group able only in the simplest of cases, and the procedure for identity has the structure of a Lie algebra. The Lie solving optimal motions, in general, is computationally algebras of SO 3 and SE 3 , denoted by so 3 and se 3 intensive. If optimality is sacriŽ ced, it is possible to respectively, are given by generate bi-invariant trajectories for interpolation and approximation using the exponential map on the Lie T so 3 ^ ^ [ 363, ^ ^ algebra [13]. While the solutions are of closed form, the ¡ resulting trajectories have no optimality properties. This paper is built on the results from references [12] ^ v 3 3 3 T and [13]. It is shown that a left or right invariant metric se 3 ^ [ 6 , v[ , ^ ^ 0 0 ¡ on SO 3 SE 3 is inherited from the higher-dimen- »µ ¶ ¼ sional manifold GLŠ 3 GA 3 equipped with the appro- Š A 363 skew-symmetric matrix ^ can be uniquely priate metric. Next, a projection operator is deŽ ned and 3 identiŽ ed with a vector [ so that for an arbitrary subsequently used to project optimal curves from the ambient manifold onto SO 3 SE 3 . It is proved that the geodesic on SO 3 and the projecŠ ted geodesic from GL 3 follow the same path, but with a different parameterization. The line from GL 3 is then shown to be parameterizable to yield the exact geodesic on SO 3 by projection. Several examples are presented to illustrate the merits of the method and to show that it produces near-optimal results, especially when the excursion of the trajectories is ‘small’.

2 BACKGROUND

2.1 Lie groups SO 3 and SE 3

Let GL n denote the general linear group of dimension Fig. 1 Inertial (Ž xed) frame and the moving frame attached n. As a manifold, GL n can be regarded as an open to the rigid body

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002 EUCLIDEAN METRICS FOR MOTION GENERATION ON SE 3 49 vector x [ 3, ^x 6x, where 6 is the vector cross- example of a differentiable vector Ž eld, X, on SE 3 is product operation in 3. Each element S [ se 3 can thus obtained by left translation of an element S [ se 3 . The be identiŽ ed with a vector pair , v . Given a curve value of the vector Ž eld X at an arbitrary point A [ S E 3 is given by R t d t A t a, a ?SE 3 , A t ¡ Š 0 1 X A S· A AS 4 µ ¶ an element S t of the Lie algebra se 3 can be A vector Ž eld generated by equation (4) is called a left associated with the tangent vector A t at an arbitrary invariant vector Ž eld and the notation S· is used to point t by indicate that the vector Ž eld was obtained by left translating the Lie algebra element S. ^ t RTd S t A 1 t A t 1 Since the vectors L1, L2, , L6 are a basis for the Lie ¡ 0 0 · · µ ¶ algebra se 3 , the vectors L1 A , , L6 A form a basis of the tangent space at any point A [ SE 3 . Therefore, where ^ t RTR is the corresponding element from any vector Ž eld X can be expressed as so 3 . A curve on SE 3 physically represents a motion of 6 i the rigid body. If t , v t is the vector pair X X Li 5 corresponding to S t , then physically corresponds i 1 to the angular velocity of the rigid body while v is the where the coefŽ cients X i vary over the manifold. If the linear velocity of the origin O of the frame M , both coefŽ cients are constants, then X is left invariant. By expressed in the frame M . In kinematics, elements of deŽ ning this form are called twists and se 3 thus corresponds to the space of twists. The twist S t computed from X 1, X 2, X 3 T , v X 4, X 5, X 6 T equation (1) does not depend on the choice of the Š Š inertial frame F . For this reason, S t is called the left a vector pair of functions , v can be associated with invariant representation of the tangent vector A. an arbitrary vector Ž eld X. If a curve A t describes a The standard basis for the vector space so 3 is motion of the rigid body and V dA dt is the vector Ž eld tangent to A t , the vector pair , v associated 0 0 0 L1 e1, L2 e^2, L3 e^3 2 with V corresponds to the instantaneous twist (screw axis) for the motion. In general, the twist , v changes where with time. T T T e1 1 0 0 , e2 0 1 0 , e3 0 0 1 Š Š Š 0 0 0 and L1, L2 and L3 represent instantaneous rotations about the Cartesian axes x, y and z respectively. The 2.3 Riemannian metrics on Lie groups components of a ^ [ se 3 in this basis are given If a smoothly varying, positive deŽ nite, bilinear, precisely by the angular velocity vector . symmetric form , is deŽ ned on the tangent space The standard basis for se 3 is at each point on the manifold, such a form is called a 0 0 0 Riemannian metric and the manifold is Riemannian L1 0 L2 0 L3 0 L1 , L2 , L3 [14]. On an n-dimensional manifold, the metric is locally 0 0 0 0 0 0 characterized by an n6n matrix of C? functions 0 e1 0 e2 0 e3 gij X i, X j , where X i are basis vector Ž elds. If the L4 , L5 , L6 0 0 0 0 0 0 basis vector Ž elds can be deŽ ned globally, then the µ ¶ µ ¶ µ ¶ 3 matrix gij completely deŽ nes the metric. On SE 3Š (on any Lie group), an inner product on the

The twists L4, L5 and L6 represent instantaneous Lie algebra can be extended to a Riemannian metric translations along the Cartesian axes x, y and z over the manifold using left (or right) translation. To see respectively. The components of a twist S [ se 3 in this, consider the inner product of two elements S 1, this basis are given precisely by the velocity vector pair S 2 [ se 3 deŽ ned by

, v . T S 1, S 2 I s1 Gs2 6

where s1 and s2 are the 661 vectors of components of S 1 2.2 Left invariant vector Ž elds and S 2 with respect to some basis and G is a positive deŽ nite matrix. If V 1 and V 2 are tangent vectors at an A differentiable vector Ž eld is a smooth assignment of a arbitrary group element A [ SE 3 , the inner product tangent vector to each element of the manifold. An V 1, V 2 A in the tangent space T AS E 3 can be deŽ ned

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 50 C BELTA AND V KUMAR by Using homogeneity of geodesics, it is easy to prove [14] that s ts which gives exp tV t . Also, 1 1 gtV gV q gV V 1, V 2 A¡ V 1, A¡ V 2 7 A I expq is a diffeomorphism of a neighbourhood of 0 [ T qM to a neighbourhood of q M. This gives a local chart for The metric obtained in such a way is said to be left [ M called normal coordinates. These coordinates are invariant [14]. convenient for computations (as in this work) because rays through 0 are geodesics. The exponential map on SO 3 with metric G aI is 2.4 AfŽ ne connection, covariant derivative and geodesic given special consideration in this paper. For R [ SO 3  ow and V [ T RSO 3 , it is possible to deŽ ne RT exp V Re V . If v 1 2 3 is the expansion of Any motion of a rigid body is described by a smooth R Š V in the local basis of T RSO 3 (i.e. curve A t [ SE 3 . The velocity is the tangent vector to V L·0 L· 0 L·0), it is easy to see that the curve V t dA dt t . 1 1 2 2 3 3 exp V Re . As a special case, for S [ so 3 , An afŽ ne connection on SE 3 is a map that assigns to R exp S es, where is the expansion of ? I s s1 s2 s3 each pair of C vector Ž elds X and Y on SE 3 another S in the basis L0, L0, L0. This giveŠ s a local parameter- ? 1 2 3 C vector Ž eld HX Y which is -bilinear in X and Y and, ization of S O 3 around identity known as exponential for any smooth real function f on SE 3 , satisŽ es coordinates. Y f Y and f Y f Y X f Y . Hf X HX HX HX In this paper, a parameterization of SE 3 induced by The Christoffel symbols i of the connection at a Gjk the product structure SO 3 3 is chosen. In other · i · 6 point A [ SE 3 are deŽ ned by HL· Lk Gjk Li, where j words, a set of coordinates s1, s2, s3, d1, d2, d3 for an L· , , L· is the basis in T SE 3 and the summation is 1 6 A arbitrary element A R, d [ SE 3 is deŽ ned so that understood. 3 d1, d2, d3 are the coordinates of d in . Exponential If A t is a curve and X is a vector Ž eld, the covariant coordinates, as deŽ ned in Section 2.5, are chosen as the derivative of X along A is deŽ ned by local parameterization of SO 3 . For R [ SO 3 sufŽ - DX ciently close to the identity [i.e excluding the points HA t X Tr R 1 Tr A 0 , or, equivalently, rotations dt ¡ through angles up to p], the exponential coordinates X is said to be autoparallel along A if DX dt 0. A are given by curve A is a geodesic if A is autoparallel along A. An equivalent characterization of a geodesic is the following R es, s [ 3 set of equations:

i i j k a Gjk a a 0 8 2.6 Screw motions where ai, i 1, , 6, is an arbitrary set of local coordinates on SE 3 . One of the fundamental results in rigid body kinematics For a manifold with a Riemannian (or pseudo- was proved by Chasles at the beginning of the nine- Riemannian) metric, there exists a unique symmetric teenth century: ‘Any rigid body displacement can be connection which is compatible with the metric [14]. realized by a rotation about an axis combined with a Given a connection, the acceleration and higher translation parallel to that axis.’ Note that a displace- derivatives of the velocity can be deŽ ned. The accelera- ment must be understood as an element of SE 3 , while tion, A t , is the covariant derivative of the velocity a motion is a curve on SE 3 . If the rotation from along the curve: Chasles’s theorem is performed at constant angular velocity and the translation at constant translational D dA A H V 9 velocity, the motion leading to the displacement dt dt V ³ ´ becomes a screw motion. Chasles’s theorem then says that ‘any rigid body displacement can be realized by a screw motion’. A curve A t on a Lie group is called a one-parameter 2.5 Exponential map and local parameterization subgroup if A t s A t A s . The following are of SE 3 equivalent ways of deŽ ning a screw motion A t [ SE 3 : 1 If M is a manifold with a connection H, the exponential 1. A¡ t A t is constant. map at an arbitrary q [ M is deŽ ned as follows. Let gV t 2. , v is constant. be the unique geodesic passing through q at t 0 with 3. A t is a one-parameter subgroup of SE 3 . velocity V, i.e. gV 0 q and gV 0 V . Then, by 4. The tangent vectors A t to the curve form a left deŽ nition, expq maps V [ T qM to the point gV 1 [ M. invariant vector Ž eld.

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002 EUCLIDEAN METRICS FOR MOTION GENERATION ON SE 3 51

With this mathematical deŽ nition, Chasles’s theorem same at all points in GL 3 . It is easy to see that , GL is can be restated in the form: ‘For every element in SE 3 a positive deŽ nite quadratic form in the entries of X and different from identity, there is a unique one-parameter Y, and therefore a metric. This induces the Euclidean subgroup to which that element belongs.’ Note that the norm on T MGL 3 , which is also called the Frobenius deŽ nition of a one-parameter subgroup is not dependent matrix norm. on a metric. Given two end positions on SE 3 , it can be concluded that there always exists an interpolating Proposition 1 screw motion. Is this motion physically meaningful and/or optimal from some point of view? To talk about The metric given by (11) deŽ ned on GL 3 is bi-invariant optimality, a metric on the manifold must Ž rst be found. when restricted to SO 3 . Optimal interpolating motions with respect to a given metric are geodesics, minimum acceleration curves, Proof. Let any M [ GL 3 and any vectors X, Y in the minimum jerk curves and so on. tangent space at an arbitrary point of GL 3 . Then What is the connection between geodesics as deŽ ned T in Section 2.4 and screw motions (one-parameter X , Y GL Tr X Y subgroups)? The following result is true for any Lie MX , MY Tr X T MT MY group [14]: for a bi-invariant metric, the geodesics that GL start from identity are one-parameter subgroups. from which it can be concluded that the metric is As a particular case, geodesics through identity on invariant under left translations with elements from SO 3 with metric G aI are one-parameter sub- SO 3 . Therefore, when restricted to SO 3 , the metric groups constant . Also, for the bi-invariant semi- becomes left invariant. For right invariance, if Riemannian metric on SE 3 R [ S O 3 , then

aI3 bI3 T G , a, b 0 10 X , Y GL Tr Y X bI3 0 µ ¶ T T T X R, Y R GL Tr Y RR X Tr Y X geodesics through identity are screw motions. The conclusion is that an interpolating screw motion is not and the claim is proved. the appropriate choice if the metric on SE 3 is different To Ž nd the induced metric on SO 3 , let R be an from the bi-invariant metric (10), which is the case of the arbitrary element from SO 3 , X, Y be two vectors from kinetic energy metric. T RSO 3 and Rx t , Ry t be the corresponding local  ows so that

3 RIEMANNIAN METRICS ON SO 3 AND SE 3 X Rx 0 , Y Ry 0 , Rx 0 Ry 0 R

The metric inherited from GL 3 can be written as In this section it is shown that there is a simple way of deŽ ning a left or right invariant metric in SO 3 [SE 3 ] T X , Y X , Y Tr R 0 R 0 by introducing an appropriate constant metric in GL 3 SO GL x y [GA 3 ]. DeŽ ning a metric at the Lie algebra so 3 [or T T T Tr Rx 0 RR Ry 0 Tr ^x ^y se 3 ] and extending it through left (right) translations is T T equivalent to inheriting the appropriate metric from the where ^x Rx 0 Rx 0 and ^y Ry 0 Ry 0 are the ambient manifold at each point. In this paper, only corresponding twists from the Lie algebra so 3 . If metrics on SE 3 that are products of the bi-invariant the above relation is written using the vector form of the metric on SO 3 and the Euclidean metric on 3 are twists, some elementary algebra leads to considered. A more general treatment accommodating T arbitrary metrics on SO 3 is to be published elsewhere. X , Y SO 2 x y 12 A different equivalent way of arriving at expression (12) would be deŽ ning the metric in so 3 [i.e. at an identity 3.1 Metrics on GL 3 and SO 3 of SO 3 ] as being the one inherited from T I GL 3 :

For any M [ GL 3 and any X , Y [ T MGL 3 , deŽ ne 0T 0 gij Tr Li Lj dij, i, j 1, 2, 3 X , Y Tr X T Y 11 GL 0 0 0 where L1, L2, L3 is the basis in so 3 and dij is the where Tr denotes the trace of a square matrix and Kronecker symbol. Left or right translating this metric T M GL 3 is the tangent space to GL 3 at M, which is throughout the manifold is equivalent to inheriting the isomorphic to GL 3 . By deŽ nition, form (11) is the metric at each three-dimensional tangent space of SO 3

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 52 C BELTA AND V KUMAR from the corresponding nine-dimensional tangent space ward calculations lead to of GL 3 . T T T X , Y SE Tr S x S y Tr ^x ^y vx vy

or, equivalently, Remark 1

The matrix G of the metric as deŽ ned in (6) is G 2I, T T · y · 2I3 0 X , Y SE x vx G , G 14 Š vy 0 I3 which is the standard scale-independent bi-invariant µ ¶ µ ¶ metric on SO 3 . This is consistent with the above proposition. Remark 3

Remark 2 Metric (14) is the scale-independent metric on S E 3 proposed by Park and Brockett [15] for a 2 and b 1. The metric given by (12) can be interpreted as the It is a product metric and has been extensively studied in (rotational) kinetic energy metric of a spherical rigid reference [12]. body.

Remark 4 3.2 Metrics on GA 3 and SE 3 Straightforward calculations show that SE 3 can be Let X and Y be two vectors from the tangent space at an provided with the same metric (14) by inheriting the arbitrary point of GA 3 (X and Y are 464 matrices metric from the ambient space at se 3 : with all entries of the last row equal to zero). Similarly 2dij, i, j 1, 2, 3 to Section 3.1, a quadratic form deŽ ned by T gij Tr Li Lj gij dij, i, j 4, 5, 6 T 0, elsewhere X , Y GA Tr X Y 13 and left translating it throughout the manifold. There- is a point-independent Riemmanian metric on fore, the metric Tr X TY from GA 3 becomes left GA 3 . invariant when restricted to SE 3 . It is possible to obtain a left invariant metric on SE 3 by inheriting the metric GA given by (13) from GA 3 . To derive the induced metric in SE 3 , the same Remark 5 procedure as in Section 3.1 is followed. Let A be an arbitrary element from SE 3 . Let X, Y be Metric (14) can be interpreted as being the kinetic two vectors from T ASE 3 and Ax t and Ay t the energy of a moving (rotating and translating) spherical corresponding local  ows so that rigid body when the body Ž xed frame M is placed at the centroid of the body and aligned with its principal X Ax 0 , Y Ay 0 , Ax 0 Ay 0 A axes.

Let 4 PROJECTION ON SO 3 R t d t A t i i , i x, y i 0 1 [ µ ¶ The norm induced by metric (11) can be used to deŽ ne and the corresponding twists at time 0 the distance between elements in GL 3 . Using this distance, for a given M [ GL 3 , the projection of M on 1 ^i vi SO 3 is deŽ ned as being the closest R [ SO 3 with S i A¡ 0 Ai 0 , i [ x, y i 0 0 respect to norm GL . The following proposition µ ¶ gives the solution of the projection problem for the The metric inherited from GA 3 can be written as general case of GL n .

T X , Y SE X , Y GA Tr Ax 0 Ay 0 Proposition 2 T T Tr S x A AS y Let M [ GL n and M U V T be its singular value Now, using the orthogonality of the rotational part of A decomposition. Then the projection of M on SO n is and the special form of the twist matrices, straightfor- given by R UV T .

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002 EUCLIDEAN METRICS FOR MOTION GENERATION ON SE 3 53

Proof. The problem to be solved is a minimization of M· is R· LUV T LR, which proves left problem: invariance. Similarly, if M is acted from the right by L [ SO n , then M· ML U V TL projects to 2 T min M R GL R· UV L RL, which implies right invariance. R [ SO n ¡

If M U V T, then It is worth noting that other projection methods do not exhibit bi-invariance. For instance, it is customary to 2 T M R Tr M R M R Ž nd the projection R SO n by applying a Gram– ¡ GL ¡ ¡ Š [ Tr MT M MTR RT M RT R Schmidt procedure (QR decomposition). In this case it is ¡ ¡ easy to see that the solution is left invariant, but in Note that RT R I, Tr MTR Tr RTM and the general it is not right invariant. quantity MTM is a constant and therefore does not affect the optimization. Therefore, the problem to be solved becomes 5 PROJECTION ON SE(N ) max Tr MTR R [ SO n Similarly to the previous section, if a metric of form (13) T Let diag s1, , sn and C R U and consider is deŽ ned in GA n , the corresponding projection on columnwise partitions for V and C SE n can be found.

V 1, , n , C c1, , cn Š Š Then Proposition 4 n n T T T Tr M R Tr si ici si i ci Let B [ GA n with the following block partition: i 1 ¡ i 1 B B B 1 2 , B GL n , B n Now C and V are both orthogonal, and then 0 1 1 [ 2 [ ci i 1. On the other hand, according to µ ¶ T 2 2 2 T Cauchy-Schwartz, i ci 4 i ci 1 and the and B1 U V the singular value decomposition of B1. n equality holds for i ci or V C. Therefore, i 1 si Then the projection of B on SE n is given by is an upper bound for Tr MT R which is attained for T UV T B R UV . A 2 [ SE n 0 1 µ ¶ Proof. Let Remark 6 R d n It is easy to see that the distance between M and R in A , R [ SO n , d [ n 2 0 1 metric (11) is given by si 1 , which is the µ ¶ i 1 ¡ standard way of describing how ‘far’ a matrix is from The problem to be solved can be formulated as follows: being orthogonal. A question that might be asked is 2 what happens with the solution to the projection min B A GA A [ SE n ¡ problem when the manifold GL n is acted upon by the group SO n . The answer is given in the following Then proposition. B A 2 Tr B A T B A ¡ GA ¡ ¡ Š Tr BTB 2Tr BTA Tr AT A Proposition 2 ¡ The quantity BTB is not involved in the optimization. The solution to the projection problem described above The observation that is both left and right invariant under actions of elements T T T from SO n . Tr B A Tr B1 R B2 d 1 T t T Tr A A 4 d d Proof. Let M [ GL n , M USV and the corresponding projection R SO n , R UV T . Let [ separates the initial problem into two subproblems: any L [ SO n and M· LM. Then an SVD for M· can be found from the SVD for M in the form T T a max Tr B1 R M· LU SV . Then, by proposition 1, the projection R [ SO n

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 54 C BELTA AND V KUMAR and is

T T B1R B1d B2 b min 2B2 d d d BX d [ n ¡ Š 0 1 µ ¶

From proposition 4, the solution to subproblem (a) is T T T UV R UV d B R UV . For the second subproblem, let AX 2 0 1 µ ¶ n T T f ? , f x 2B2 x x x T ¡ It is easy to see that B1R U V R. If only rotations d 0 are taken into consideration, right invariance is The critical points of the scalar function f are given by proved.

Hf x 2B2 2x 0 x B2 ¡ and the Hessian H2f x 2I is always positive deŽ nite. 6 GENERATING SMOOTH CURVES ON SE 3 Therefore, the solution is d B2, which concludes the proof. Based on the results from the previous sections, a procedure for generating near-optimal curves on SE 3 Similar to SO n , invariance properties are exhibited by follows: generate the curves in the ambient space and the projection on SE n . project them onto SE 3 . Owing to the fact that the deŽ ned metric in GA 3 is the same at all points, the corresponding Christoffel symbols are all zero. Conse- quently, the optimal curves in the ambient space assume simple analytical forms (i.e geodesics—straight lines, Proposition 5 minimum acceleration curves—cubic polynomial curves, minimum jerk curves—Ž fth-order polynomial curves, all The solution to the projection problem on SE n is parameterized by time). The resulting curve in GA 3 left invariant under actions of elements is linear in the boundary conditions, and therefore from SE n . The projection is bi-invariant under left and right invariant. Recall that the projection rotations. procedure on SE 3 is left invariant, and so is the overall procedure. Proof. Let The focus is on SO 3 . Owing to the product structure 3 of both SE 3 SO 3 6 and the metric , SE for B B B 1 2 GA n a 0, all the results can be extended straightforwardly 0 1 [ µ ¶ to SE 3 . and deŽ ne A, U, and V such that

T 6.1 Geodesics on SO 3 T UV B2 B1 U V , A [ SE n 0 1 The problem to be solved is generating a geodesic R t µ ¶ between given end positions R1 R 0 and R2 R 1 Let on SO 3 . Without loss of generality, it is assumed that R1 I. Indeed, a geodesic between two arbitrary R d X positions R1 and R2 is the geodesic between I and 0 1 R 1R left translated by R . Exponential coordinates , µ ¶ 1¡ 2 1 s1 s2, s3 are considered as local parameterization of SO 3 . o0 be an arbitrary element from SE n . Under left actions If R2 e , then the geodesic is the exponential of X, the solution pair becomes mapping of the uniformly parameterized segment passing through 0 and o0 s t o0t from the expo- RB RB d X B 1 2 nential coordinates: 0 1 µ ¶ t t R t es eo0

RUV T RB d The geodesic in the ambient manifold GL 3 satisfying X A 2 0 1 the given boundary conditions on SO 3 is µ ¶ M t I R2 I t, t [ 0, 1 which proves left invariance of the projection. For the ¡ Š second part, note that the right translated solution pair An analytical expression for the projection of an

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002 EUCLIDEAN METRICS FOR MOTION GENERATION ON SE 3 55 arbitrarily parameterized line in the ambient GL 3 onto 2. The singular values are given by s2 t 1 f t 6 i ¡ SO 3 is derived, which will answer the following three 1 f t li (it will be shown shortly that the right- questions: hand¡ side of this equality is always positive). 3. The time dependence of the projection will be 1. Does the projection of a geodesic from GL 3 follow contained in the same path as the true geodesic on SO 3 ? If the answer is yes, then question 2 makes sense. U t u1 t u2 t u3 t Š 2. Do the above two curves have the same parameter- M t vi ui t , i 1, 2, 3 ization? si 3. If the answer is no, can one Ž nd an appropriate o0 parameterization of the line in the ambient manifold Using the Rodrigues formula for R2 e , it is easy to so that the projection is identical to the true geodesic see that on SO 3 ? 1 cos 0 2 2T The following proposition is the key result of this N ¡ 2 o0 o0 0 section. from which it follows that the eigenvalues of N are given by

l N 0, 2 1 cos 0 , 2 1 cos 0 Proposition 6 ¡ ¡ and a set of three orthonormal eigenvectors by Let M t I R2 I f t , t [ 0, 1 be a line in GL 3 o0 ¡ Š with R2 e [ SO 3 [ f continuous, f 0 0, f 1 \ 3 1]. Then the projection of this line R t on to 0 1 1 , ¡0 , SO 3 is the exponential mapping of a segment drawn 0 2 2 2 2 2 2 2 2 2 3 1 1 2 1 3 1 3 2 between the origin and o0 in exponential coordinates parameterized by y t : 2 1 ¡2 2 6 3 1 T \ T M t U t t V t R t U t V t 3 2 ¡ eo0 y t 15 T where 0 1 2 3 . With the eigenstructure of N determined, it is possiblŠ e to write 1 y t atan 2 1 f t f t cos 0 , S t diag 1, s t , s t 0 ¡ 2 f t sin 0 16 s t 2 1 cos 0 f t 2 1 cos 0 f t 1 ¡ ¡ ¡ 17 Proof. The SVD decomposition of M t I T where the binomial under the square root is always positive R2 I f t U t t V t is needed, where R2 ¡ because it is positive at zero and 1 cos 0 [ 0, 2 eo0 and f t is a continuous function deŽ ned on [0, 1] ¡ gives a negative discriminant. Some straightforward but satisfying f 0 0, f 1 1. The Ž rst observation is rather tedious calculation leads to T M t M t I f t 1 f t N 2 ¡ ¡ T o0 o0 T U t V I g2 t 1 g1 t N 2I R2 R ¡ 2 ¡ ¡ 2 0 0 The eigenstructure of the constant and symmetric matrix where N completely determines the SVD of M t . Because N is symmetric and real, its eigenvalues will be real and the 1 f t f t cos 0 g1 t ¡ corresponding eigenspaces orthogonal. Let li, vi be an s t eigenvalue–eigenvector pair of N. Then, f t sin 0 g2 t T s t Nvi livi M t M t 1 f t 1 f t livi ¡ ¡ Therefore, theoretically, the desired SVD decomposition The discussion is restricted to 0 [ 0, p (in accordance with the exponential coordinates) which will give g2 t 0. is determined at this moment: 2 2 Note that g1 t g2 t 1, so it is appropriate to deŽ ne a 1. The matrix V t can be chosen as a constant of the function y t [ 0, 1 so that form V v1 v2 v3 , where v1, v2 and v3 are ortho- Š normal eigenvectors of N. g1 t cos 0 y t , g2 t sin 0 y t

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 56 C BELTA AND V KUMAR

By use of the Rodrigues formula again,

U t VT eo0 y t so the projected line is the exponential mapping of a segment between the origin and 0 in exponential coordi- nates. The parameterization of the segment is given by 1 y t atan 2 1 f t f t cos 0 , 0 ¡

f t sin 0

This is the end of the proof.

Note that the obtained parameterization y t satisŽ es the boundary conditions y 0 0, y 1 1. As a particular case of the above proposition for f t t, the following corollary answers the Ž rst two questions at the beginning of this section.

Corollary 1

The true geodesic on SO 3 and the projected geodesic from GL 3 with ends on S O 3 follow the same path on SO 3 but with different parameterizations. The pro- jected curve is the exponential mapping of the same segment from the exponential coordinates

R t eo0 y t with the following parameterization Fig. 2 (a) Function y t and (b) the derivative d dty t 1 y t atan 2 1 t t cos 0 , t sin 0 0 ¡

The derivative of the function y t is given by

d sin 0 y t GL 3 with ends on SO 3 , which gives uniform dt s t 0 parameterization t of the projected curve in exponential where s t is given by (17). Plots of the function y t and coordinates. The solution of the following equation its derivative are given in Fig. 2 for t [ 0, 1 and the in f magnitude of the displacement on the Šmanifold atan2 1 f t f t cos 0 , 0 [ 0, p . ¡ The conclusion is that, even though the line in GL 3 f t sin 0 t is followed at constant velocity, the projected curve on the manifold has low speed at the beginning, attains its is of the form maximum in the middle and slows down as it sin 0 t approaches the end-point. The larger the displacement f t sin 0 1 t sin 0 t 0 , the larger the discrepancy in speeds. Also note ¡ that the middle of the line is projected into the middle of The answer to the third question is stated in the the true geodesic because y 0 5 0 5 [i.e the functions t following corollary. and y t are equal at t 0 5]. This result has been stated in reference [3] in the context of unit quaternions as local 3 parameters of S O 3 (viewed as the unit sphere S in the Corollary 2 projective space P3). To answer the third question, it is necessary to Ž nd a The true geodesic on SO 3 starting at I and ending at parameterization f t f 0 0, f 1 1 of the line in R2 e 0 is the projection of the following line from the

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002 EUCLIDEAN METRICS FOR MOTION GENERATION ON SE 3 57

parameter u moving from 0 to 1, is given by

sin 1 u y sin uy Slerp q1, q2 u ¡ q1 q2 sin y sin y

where q1 q2 cos y.

6.2 Minimum acceleration curves on SO 3

Firstly, the computation of optimal trajectories described in reference [12] is summarized. Then, near- optimal trajectories are generated via the projection method. The necessary conditions for the curves that minimize the square of the L 2 norm of the acceleration are found by considering the Ž rst variation of the acceleration functional

b L a HV V , HV V dt 18 a where V t dR t dt, H is the symmetric afŽ ne connection compatiblŠ e with a suitable Riemmanian metric and R t is a curve on the manifold. The initial and Ž nal points as well as the initial and Ž nal velocities for the motion are prescribed. The following result is stated and proved in reference [12].

Proposition 7

Let R t be a curve between two prescribed points on SO 3 with metric , that has prescribed initial and Fig. 3 (a) Function f t and (b) the derivative d dtf t SO Ž nal velocities. If is the vector from so 3 correspond- ing to V dR dt, the curve minimizes the cost function L a derived from the canonical metric only if the following equation holds: ambient manifold GL 3 : 3 6 0 19

M t I R2 I f t , t [ 0, 1 n ¡ Š where denotes the nth derivative of . sin t The above equation can be integrated to obtain f t 0 sin 0 1 t sin 0 t ¡ 2 6 constant 20 Illustrative plots of f t and its derivative are given in However, this equation cannot be further integrated Fig. 3 for t [ 0, 1 and different values of the displace- Š analytically for arbitrary boundary conditions. In the ment 0 [ 0, p . As expected, to attain a uniform special case where the initial and Ž nal velocities are speed on SO 3 , the line in GL 3 should be followed at tangential to the geodesic passing through the same high speed at the beginning, slowing down in the middle points, the solution can be found by reparameterizing and accelerating again near the end-point. the geodesic [12]. In the general case, equation (19) must be solved numerically. A local parameterization of SO 3 should be chosen, and three Ž rst-order differ- Remark 7 ential equations will augment the system. The most convenient local coordinates are the exponential coor- The result in Corollary 2 is similar to the formula for dinates. Eventually, this ends up with solving a system spherical linear interpolation ‘Slerp’ in terms of qua- of 12 Ž rst-order non-linear coupled differential equa- ternions [3]. The curve interpolating q1 and q2, with tions with six boundary conditions at each end.

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 58 C BELTA AND V KUMAR

A solution can be found using iterative procedures The initial velocity is the one corresponding to the such as the shooting method or the relaxation method. geodesic passing through the two positions: The latter has been chosen in this paper. A more attractive and much simpler approach is the p p p T projection method described above. The main idea is to 0 6 3 2 relax the problem to GL 3 , while keeping the proper boundary conditions on SO 3 with the corresponding Cases (a), (b) and (c) differ by the velocity at the end- velocities. Minimum acceleration curves are found in point. Figure 4a corresponds to a Ž nal velocity 1 0, GL 3 and eventually projected back onto SO 3 . and therefore a minimum acceleration curve is obtained In what follows, the time interval will be t [ 0, 1 and Š for which the end velocities are along the velocity of the the boundary conditions R 0 , R 1 , R 0 , R 1 are corresponding geodesic, leading to a geodesic parame- assumed to be speciŽ ed. The minimum acceleration terized by a cubic of time [12]. The Ž nal velocity is 1 curve in GL 3 with a constant metric , GL is a cubic 0 e1 in case (b) and 1 0 5e1 in case (c), where T given by e1 1 0 0 . Š 2 3 As can be seen in Fig. 4a, the paths of the projected M t M0 M1t M2t M3t and the optimal curves are the same, the parameteriza- tions are slightly different though, as expected. In cases where M0, M1, M2, M3 [ GL 3 are (b) and (c), although the deviation of the Ž nal velocity from being homogeneous is large, the curves are close. M0 R 0 , M1 R 0 Note that the boundary conditions are rigorously M2 3R 0 3R 1 2R 0 R 1 ¡ ¡ ¡ satisŽ ed. M3 2R 0 2R 1 R 0 R 1 ¡ Now the curve on SO 3 is obtained by projecting M t on to S O 3 as described in Section 4. The following examples present comparisons between 6.3 Motion generation on SE 3 the minimum acceleration curves generated using the projection method and the curves obtained directly on Since a method to generate (near) optimal curves on SO 3 has been developed, the extension to SE 3 is SO 3 by solving equations (19) using the relaxation 3 method. All the generated curves are drawn in simply adding the well-known optimal curves from . exponential local coordinates. A homogeneous cubic rigid body is assumed to move In Fig. 4, the following position boundary conditions (rotate and translate) in free space. The body frame were used: M is placed at the centre of mass and aligned with the principal axes of the body. A small square is drawn on T T p p p one of its faces. The trajectory of the centre of the cube s 0 0 0 0 , s 1 21 Š 6 3 2 is starred.

Fig. 4 Minimum acceleration curves on SO 3 with a canonical metric: (a) velocity boundary conditions along the geodesic; (b) end velocity perturbed by e1; (c) end velocity perturbed by 5e1

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002 EUCLIDEAN METRICS FOR MOTION GENERATION ON SE 3 59

Fig. 5 Minimum acceleration motion for a cube in free space: (a) relaxation method; (b) projection method

The following boundary conditions were considered: geodesic from GL 3 and the actual geodesic on SO 3 were shown to have identical paths on SO 3 . The T T p 2p p parameterization of the line whose projection is the s 0 0 0 0 , s 1 Š 6 3 2 actual geodesic was derived. µ ¶ 0 1 2 3 T , 1 2 1 1 T Š Š d 0 0 0 0 T, d 1 8 10 12 T Š Š REFERENCES d 0 1 1 1 T, d 1 1 5 3 T Š Š 1 Farin, G. E. Curves and Surfaces for Computer Aided True and projected minimum acceleration motions for a Geometric Design: A Practical Guide, 3rd edition, 1992 cubic rigid body with a 2 and m 12 are given in Fig. (Academic Press, Boston). 5 for comparison. 2 Hoschek, J. and Lasser, D. Fundamentals of Computer In this example, the total displacement between initial Aided Geometric Design, 1993 (AK Peters). and Ž nal positions on SO 3 is large. If the rotational 3 Shoemake, K. Animating rotation with quaternion curves. displacement is restricted near the origin of exponential ACM Siggraph, 1985, 19 3 , 245–254. 4 Ge, Q. J. and Ravani, B. Geometric construction of Bezier coordinates, the simulated motions look identical. motions. Trans. ASM E, J. M ech. Des., 1994, 116, 749–755. 5 Park, F. C. and Ravani, B. Bezier curves on Riemannian manifolds and Lie groups with kinematics applications. 7 CONCLUSION Trans. ASM E, J. M ech. Des., 1995, 117(1), 36–40. 6 Ge, Q. J. and Ravani, B. Computer aided geometric design of motion interpolants. Trans. ASM E, J. M ech. Des., 1994, This paper has presented a method for generating 116, 756–762. smooth trajectories for a moving rigid body with 7 Ju¨tler, B. Visualization of moving objects using dual speciŽ ed boundary conditions. SE 3 , the set of all rigid quaternion curves. Computers and Graphics, 1994, 18 3 , body translations and orientations, was seen as a 315–326. submanifold (and a subgroup) of the Lie group of 8 Park, F. C. and Kang, I. G. Cubic interpolation on the afŽ ne maps in 3, GA 3 . The method involved two key rotation group using Cayley parameters. In Proceedings of steps: ASME 24th Biennial Mechanisms Conference, Irvine, California, 1996. (a) the generation of optimal trajectories in GA 3 , 9 Noakes, L., Heinzinger, G. and Paden, B. Cubic splines on (b) the projection of the trajectories from GA 3 to curved spaces. IM A J. M ath. Control and Inf., 1989, 6, 465– S E 3 . 473. 10 Crouch, P. and Silva Leite, F. The dynamic interpolation The overall procedure proved to be invariant with problem: on Riemannian manifolds, Lie groups, and respect to both the local coordinates on the manifold symmetric spaces. J. Dynamic Control Syst., 1995, 1(2), and the choice of the inertial frame. The projected 177–202.

C03301 # IMechE 2002 Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineering Science 60 C BELTA AND V KUMAR

11 Camarinha, M., Silva Leite, F. and Crouch, P. Splines of 13 ZÏ efran, M. and Kumar, V. Interpolation schemes for rigid class ck on non-Euclidean spaces. IM A J. M ath. Control body motions. Computer-Aided Des., 1998, 30 3 . and Inf., 1995, 12(4), 399–410. 14 do Carmo, M. P. Riemannian Geometry, 1992 (Birkhauser, 12 ZÏ efran, M., Kumar, V. and Croke, C. On the generation of Boston). smooth three-dimensional rigid body motions. IEEE 15 Park, F. C. and Brockett, R. W. Kinematic dexterity of Trans. Robotics and Automn, 1995, 14(4), 579–589. robotic mechanisms. Int. J. Robotics Res., 1994, 13(1), 1–15.

Proc Instn Mech Engrs Vol 216 Part C: J Mechanical Engineerin g Science C03301 # IMechE 2002