New metrics for rigid body motion interpolation

Proceedings of
A Symposium Commemorating the

Legacy, Works, and Life of
Sir Robert Stawell Ball

Upon the 100th Anniversary of
A Treatise on the Theory of Screws

July 9-11, 2000
University of Cambridge, Trinity College

© 2000

New metrics for rigid body motion interpolation

Calin Belta and Vijay Kumar

fcalin j [email protected]

General Robotics, Automation, Sensing and Perception (GRASP) Laboratory
University of Pennsylvania, Philadelphia, PA, USA

Abstract

This paper develops a method for generating smooth trajectories for a moving rigid body with

specified conditions at end and intermediate points. It is well known that SE3, the set of all rigid

body translations and orientations, is a non Euclidean space. In general, the problem of determining
trajectories that are optimal with respect to some meaningful metric does not have an analytical
solution. Thus it is difficult to develop efficient solutions for real time applications in computer
graphics and robotics.

Given two points on SE3, there is a natural trajectory between the two points that is bi-

invariant or invariant with respect to the choice of coordinate systems. This trajectory is the well-
known screw motion [1]. In an earlier paper, we showed that the screw motion minimizes an in-
definite metric [21]. However, there is no clear interpretation of optimality for a trajectory that
minimizes an indefinite metric.

In this paper, we consider SEn to be a submanifold (and a subgroup) of GLn + 1; IR.
Our method involves two key steps: (1) the generation of optimal trajectories in GLn + 1; IR;
and (2) the projection of the trajectories from GLn + 1; IR to SEn. The overall procedure is

invariant with respect to both the local coordinates on the manifold and the choice of the inertial
frame. The benefits of the method are two-fold. First, it is possible to apply any of the variety

3401 Walnut St., Philadelphia, PA 19104-6228, Tel:(215) 898-5814

of well-known, efficient techniques to generate optimal curves on GLn; IR [7, 8]. Second, the
method yields solutions for general choices of Riemannian metrics on SE3. For example, we

can incorporate the dynamics of arbitrary rigid bodies, or the added-mass effects in the dynamics of
bodies submerged in fluids [13].

We first define a metric in the ambient space, GLn + 1; IR, one that induces an appropriate
metric on SEn. We define a suitable projection operator and establish its invariance with respect
to changes in coordinates. The fact that the standard metric in GLn; IR induces the bi-invariant
metric on SOn falls out of our analysis. The projection operator allows us to project the optimal
curves onto SEn to yield motions that are near optimal. Finally, we present results that illustrate

the near optimality of the projected curves.

1 Introduction

We address the problem of finding a smooth motion that interpolates between two given positions
and orientations. This problem finds applications in robotics and computer graphics. The prob-
lem is well understood in Euclidean spaces [7, 11], but it is not clear how these techniques can be
generalized to curved spaces. There are two main issues that need to be addressed, particularly on
non-Euclidean spaces. It is desirable that the computational scheme be independent of the descrip-
tion of the space and invariant with respect to the choice of the coordinate systems used to describe
the motion. Secondly, the smoothness properties and the optimality of the trajectories need to be
considered.

Shoemake [18] proposed a scheme for interpolating rotations with Bezier curves based on the
spherical analog of the de Casteljau algorithm. This idea was extended by Ge and Ravani [10] and
Park and Ravani [17] to spatial motions. The focus in these articles is on the generalization of the
notion of interpolation from the Euclidean space to a curved space.

Another class of methods is based on the representation of Bezier curves with Bernstein poly-

nomials. Ge and Ravani [9] used the dual unit quaternion representation of SE3 and subsequently

applied Euclidean methods to interpolate in this space. Ju¨tler [12] formulated a more general ver-
sion of the polynomial interpolation by using dual (instead of dual unit) quaternions to represent

SE3. In such a representation, an element of SE3 corresponds to a whole equivalence class

of dual quaternions. Park and Kang [16] derived a rational interpolating scheme for the group of

2

rotations SO3 by representing the group with Cayley parameters and using Euclidean methods in

this parameter space. The advantage of these methods is that they produce rational curves.

It is worth noting that all these works (with the exception of [17]) use a particular coordinate
representation of the group. In contrast, Noakes et al. [15] derived the necessary conditions for cu-
bic splines on general manifolds without using a coordinate chart. These results are extended in [5]
to the dynamic interpolation problem. Necessary conditions for higher-order splines are derived in
Camarinha et al. [3]. A coordinate free formulation of the variational approach was used to generate

shortest paths and minimum acceleration and jerk trajectories on SO3 and SE3 in [20]. How-

ever, analytical solutions are available only in the simplest of cases, and the procedure for solving
optimal motions, in general, is computationally intensive. If optimality is sacrificed, it is possible
to generate bi-invariant trajectories for interpolation and approximation using the exponential map
on the Lie Algebra [19]. While the solutions are of closed-form, the resulting trajectories have no
optimality properties.

In this paper, we build on the results from [20, 19] to generate smooth curves for general Rie-
mannian metrics. We pursue a geometric approach and require that our results be invariant with
respect to the choice of reference frames and independent of the parameterization of the manifold.

First, we focus our attention to SOn and then extend the results to SEn. The bi-ivariant metric
on SOn [4] and the left invariant metric proposed by Park and Brockett (1994) are particular cases

of our general treatment. Also, this paper generalizes the results presented in [2].

We first show that a left (right) invariant metric on SOn or SEn , 1 is inherited from
the higher dimensional space GLn; IR equipped with the appropriate metric. Next, a projection

operator is defined and subsequently used to project optimal curves from the ambient space onto

SOn or SEn , 1. Several examples are presented to illustrate the merits of the method and to

show that it yields near-optimal results, especially when the excursion of the trajectories is “small”.
In certain cases, we are also able to establish quantitative results that measure the closeness of the
generated trajectory to the optimal trajectory.

3

2 Background

2.1 The Lie groups SO3 and SE3

Let GLn; IR denote the general linear group of dimension n. As a manifold, GLn; IR can be

regarded as an open subset of IRn2. Moreover, matrix multiplication and inversion are both smooth

operations, which make GLn; IR a Lie group. The special orthogonal group is a subgroup of the

general linear group, defined as

SOn = nR 2 GLn; IR : RRT = I; detR = 1o

SOn is referred to as the rotation group on IRn. SEn = SOn IRn is the special Euclidean
group, and is the set of all displacements in IRn. Special consideration will be given to SO3 and

SE3.

Consider a rigid body moving in free space. Assume any inertial reference frame F fixed
in space and a frame M fixed to the body at point O0 as shown in Figure 1. At each instance,

the configuration (position and orientation) of the rigid body can be described by a homogeneous

transformation matrix, A, corresponding to the displacement from frame F to frame M . SE3 is

the set of all rigid body transformations in three-dimensions:

SE3 8 j A = 2 R d 3 R 2 IR33;
4
=A 1 5;
:0

d 2 IR3; RT R = I; detR = 1o :

SE3 is a closed subset of GL4; IR, and, therefore, a Lie group.

On any Lie group the tangent space at the group identity has the structure of a Lie algebra. The

Lie algebras of SO3 and SE3, denoted by so3 and se3 respectively, are given by:

so3 = n!^ 2 ;IR33 !^T = ,!^o :

se3 82 !^ v 3 j !^ 2 ;IR33 v 2 IR3; !^T 9
5
=4 0 = ,!^= :
:0 ;

A 3 3 skew-symmetric matrix !^ can be uniquely identified with a vector ! 2 IR3 so that for an
arbitrary vector x 2 IR3, !^x = ! x, where is the vector cross product operation in IR3. Each

4

{M}
z’

x’ t3

{M} z’ y’
y’
z’

{M}

x’ t2 y’

{F} z t1
x’

y
x

Figure 1: The inertial (fixed) frame and the moving frame attached to the rigid body

element S 2 se3 can be thus identified with a vector pair f!; vg. Given a curve

At : ,a; a ! SE3; At = 2 Rt dt 3
4 5
01

an element St of the Lie algebra se3 can be associated to the tangent vector A_t at an arbitrary

point t by:

St = A,1tA_ t = 2 !^t RT d_ 3 (1)
4
00 5:

where !^t = RT R_ is the corresponding element from so3.

A curve on SE3 physically represents a motion of the rigid body. If f!t; vtg is the vector
pair corresponding to St, then ! physically corresponds to the angular velocity of the rigid body
while v is the linear velocity of the origin O0 of the frame M , both expressed in the frame M . In
kinematics, elements of this form are called twists and se3 thus corresponds to the space of twists.
The twist St computed from Equation (1) does not depend on the choice of the inertial frame F .
For this reason, St is called the left invariant representation of the tangent vector A_. Alternatively,
the tangent vector A_ can be identified with a right invariant twist (invariant with respect to the choice
of the body-fixed frame M ).

Since so3 is a vector space, any element can be expressed as a 3 1 vector of components

5

corresponding to a chosen basis. The standard basis for so3 is:

23 23 2 0 ,1 3

000 001 0
L , L L= 0 0 1o 6 7 7 = 1 0 0o 6 7
6 7 = 0 0 0o 66 7 6 7 (2)
6 7 6 7 6 7
1 4 5 2 4 5 3 4 5

010 ,1 0 0 000

L1o, L2o and Lo3 represent instantaneous rotations about the Cartesian axes x, y and z, respectively.
The components of a !^ 2 se3 in this basis are given precisely by the angular velocity vector !.

The standard basis for se3 is:

L1 2 Lo1 3 L2 2 L2o 3 L3 2 L3o 3

=4 0 0 =4 0 0 =4 0 0
5 5 5
2 (3)
0 0 0
e3 2 e3 2 e3
0 15 0 25 0 35
L4 =4 0 L5 =4 0 L4 =4 0
0 0 0

where iT iT 1 iT

e1 h 1 0 0 ; e2 h 0 1 0 ; e3 h 0 0

= = =

The twists L4, L5 and L6 represent instantaneous translations along the Cartesian axes x, y and
z, respectively. The components of a twist S 2 se3 in this basis are given precisely by the velocity
vector pair, f!; vg.

2.2 Left invariant vector fields

A differentiable vector field is a smooth assignment of a tangent vector to each element of the

manifold. At each point, a vector field defines a unique integral curve to which it is tangent [6].

Formally, a vector field X is a (derivation) operator which, given a differentiable function, returns
its derivative (another function) along the integral curves of X. Using the definition of a tangent

(differential) map between two manifolds, it is straightforward to prove that the action of the tangent

map of the left translation with A on SE3 at some point is simply multiplication from the left with
A, at any point on the manifold.

An example of a differentiable vector field, X, on SE3 is obtained by left translation of an
element S 2 se3. The value of the vector field X at an arbitrary point A 2 SE3 is given by:

XA = SA = AS: (4)

6

A vector field generated by Equation (4) is called a left invariant vector field and we use the notation

S to indicate that the vector field was obtained by left translating the Lie algebra element S. Right

invariant vector fields can be defined analogously. In general, a vector field need not be left or right

invariant. By construction, the space of left invariant vector fields is isomorphic to the Lie algebra

se3.
Since the vectors L1; L2; : : : ; L6 are a basis for the Lie algebra se3, the vectors L1A; : : : ;

L6A form a basis of the tangent space at any point A 2 SE3. Therefore, any vector field X can

be expressed as

X = X6 XiLi; (5)

i=1
where the coefficients Xi vary over the manifold. If the coefficients are constants, then X is left

invariant. By defining:

! = X1;X2;X3 T; v = X4;X5;X6 T;

we can associate a vector pair of functions f!; vg to an arbitrary vector field X. If a curve At
describes a motion of the rigid body and V = ddAt is the vector field tangent to At, the vector
f gpair !; v associated with V corresponds to the instantaneous twist (screw axis) for the motion. In

general, the twist f!; vg changes with time.

2.3 Exponential map and exponential coordinates

For every S 2 se3, let AS : IR ! SE3 denote the integral curve of the left invariant vector field
XS passing through I at t = 0. That is, AS0 = I and ddt ASt = XSASt. It follows that (see
DoCarmo pg.80) ASs + t = AStASs which means that ASt is a one-parameter subgroup
of SE(3). The function exp : se3 ! SE3 defined by expS = AS1 is called the exponential

map of the Lie algebra se3 into SE3. It is easy to show [14] that the exponential map takes the

line tS 2 se3; t 2 IR into a one-parameter subgroup of SE3, i.e.

exptS = ASt: (6)

Using Equation (1) we can show that the exponential map agrees with the usual exponentiation of

the matrices in IR44: X1 tkSk
k=0 k!
exptS = ; (7)

7

where S denotes the matrix representation of the twist S. The sum of this series can be computed
explicitly and the resulting expression, when restricted to SO3, is known as Rodrigues’ formula.
The formula for the sum in SE3 is derived in [14].

2.4 Riemannian metrics on Lie groups

If a smoothly varying, positive definite, bilinear, symmetric form :; : is defined on the tangent

space at each point on the manifold, such a form is called a Riemannian metric and the manifold is

Riemannian [6]. If the form is non-degenerate but indefinite, the metric is called semi-Riemannian.

COn a n dimensional manifold, the metric is locally characterized by a n n matrix of 1 functions
gij = Xi; Xj where Xi are basis vector fields1. Note that the entries gij at each point on the

manifold are functions of the local coordinates at that point. If the basis vector fields can be defined

globally, then the matrix gij completely defines the metric.
On SE3 (on any Lie group), an inner product on the Lie algebra can be extended to a Rie-

mannian metric over the manifold using left (or right) translation. To see this, consider the inner

product of two elements S1; S2 2 se3 defined by

S1; S2 jI = s1T W s2; (8)

where s1 and s2 are the 6 1 vectors of components of S1 and S2 with respect to some basis
and W is a positive definite matrix. If V1 and V2 are tangent vectors at an arbitrary group element
A 2 SE3, the inner product V1; V2 jA in the tangent space TASE3 can be defined by:

V1; V2 jA = A,1V1; A,1V2 I : (9)

The metric obtained in such a way is said to be left invariant [6] since left translation by any element

A is an isometry.

3 Riemannian metric on SOn and SEn

In this section we will show that there is a simple way of defining a left (or right) invariant metric

in SOn (or SEn , 1) by introducing an appropriate constant metric in GLn; IR. Defining a

1The basis vector fields need not be the coordinate basis; we only require that they are smooth.

8

metric (i.e. the kinetic energy) at the Lie algebra son (or sen , 1) and extending it through left

(right) translations will be equivalent to inheriting the appropriate metric from the ambient space.

3.1 A metric in the ambient space

Let W be a symmetric positive definite n n matrix. For any A 2 GLn; IR and any X; Y 2
TAGLn; IR, define

X; Y W;A= T rXT Y W = T rW XT Y = T rY W XT (10)

where the first subscript stands for the chosen matrix weight and the second denotes the point at

which the form is defined. By definition, form (10) is the same at all points in GLn; IR. Why the

second subscript then? This will become useful when studying the invariance of the above form. It

2is clear that form (10) is quadratic in the entries of X and Y . Let x; y IRn2 be the column vectors
obtained by abutting the transposed lines from X and Y . Then,

X; Y W;A= xT W y;

where

W = blockdiagonalW T ; W T ; : : : ; W T ; W 2 IRn2n2:

It is easy to see that W is symmetric and positive definite if and only if W is symmetric and positive
definite. Therefore, (10) is a metric when W is symmetric and positive definite. Some interesting

results are in order.

Proposition 3.1 Metric (10) defined on GLn; IR is left invariant when restricted to SOn.

Metric (10) restricted to SOn is bi-invariant if W = I; 0, I is the n n identity

matrix.

Proof: Let any A; B 2 GLn; IR and any X; Y 2 TAGLn; RR. Then, we have
X; Y W;A= T rXT Y W ; BX; BY W;BA= T rXT BT BY W

2from which we conclude that the metric is invariant under left translations with elements B
SOn. When restricted to SOn, (i.e A 2 SOn), the metric becomes left invariant. Bi-

invariance is right invariance when the metric is left invariant. Note that

X; Y W;A= T rY W XT ; XB; Y B W;AB= T rY BW BTXT

9

So, right invariance is guaranteed under the condition that BW BT = W , or equivalently, W com-

mutes with all the elements B 2 SOn, which is equivalent to W = I. 2

Remark 3.2 If right invariance on SOn is desired, we can define

X; Y W;A= T rXY T W = T rY T W X = T rW XY T

Metric ; will be right invariant on SOn for W symmetric and positive definite and bi-
invariant if W = I. The proof is similar to that for metric ; and is omitted.

3.2 The induced metric on SO3

Even though the following derivation can be done in the general case of a nn , 1=2-dimensional
manifold SOn in the ambient n2-dimensional manifold GLn; IR, we will restrict to the case
n = 3 for practical purposes. Some general results will be presented and proved for the general
case though. Let R be an arbitrary element from SO3. Let X; Y be two vectors from TRSO3
and Rxt; Ryt the corresponding local flows so that

X = R_ x0; Y = R_ y0; Rx0 = Ry0 = R:

The metric inherited from GL3; IR can be written as:

X; Y W;R= T rR_ xT 0R_ y0W = T rR_ xT 0RRT R_ y0W = T r!^xT !^yW

where !^x = Rx0T R_ x0 and !^y = Ry0T R_ y0 are the corresponding twists from the Lie
algebra so3. If we write the above relation using the vector form of the twists, some elementary

algebra leads to:

X; Y W;R= !xT G!y (11)

where (12)

G = T rW I3 , W

is the matrix of the metric on SO3 as defined by (8). A different equivalent way of arriving at

the expression of G would be defining the metric in so3 (i.e. at identity of SO3) as being the
one inherited from TIGL3; IR: gij = T rLioT LjoW ; i; j = 1; 2; 3 (L1o; Lo2; L3o is the basis in

10

so3). Left translating this metric throughout the manifold is equivalent to inheriting the metric at
each 3-dimensional tangent space of SO3 from the corresponding 9-dimensional tangent space of
GL3; IR.

Proposition 3.3 G is symmetric if and only if W is symmetric.

If W is positive definite, then G is positive definite.

If G is positive definite, then W is positive definite if and only the eigenvalues of G are the

lengths of the sides of a triangle.

Proof: The first part follows immediately from (12). For the second part, note that the eigenvalues

of G are given by
iG = X jW , iW = X jW

j j6=i

Because W is positive definite, it follows that iW 0 which implies iG 0, i.e. G is

positive definite. For the third part, we start from the equivalent form of (12)

W = 1 T rGI3 , G
2

which implies iW = 1 X jG , iG
and the claim is proved. 2
j

Remark 3.4 In the particular case when W = I; 0, from (12), we have G = 2 I, which is
the standard bi-invariant metric on SO3. This is consistent with the second assertion in Proposi-
tion 3.1. For = 1, metric (10) will induce the well known Frobenius norm on GL3; IR.

3.3 The induced metric in SE3

Similarly, we can get a left invariant metric on SEn , 1 by inheriting the metric : W~ given
by (10) from GLn; IR. For practical purposes, we will assume the following form for the n n

matrix of the metric in (10): 2 3

W~ =4 W 0 (13)
5
0
w

11

where W is a n , 1 n , 1 symmetric and positive definite matrix and w is a positive scalar. It
follows that W~ is symmetric and positive definite, and, therefore, gives a Riemmanian metric. To
derive the induced metric in SE3 we follow the same procedure as in section 3.2 for the particular
case n = 4.

Let A be an arbitrary element from SO3. Let X; Y be two vectors from TASE3 and
Axt; Ayt the corresponding local flows so that

X = A_x0; Y = A_y0; Ax0 = Ay0 = A:

Let 2 Rit dit 3
4
Ait = 0 1 5; i 2 fx; yg

and the corresponding twists at time 0:

Si = A,i 10A_i0 = 2 !^i vi 3 i 2 fx; yg
4
0 0 5;

The metric inherited from GL4; IR can be written as:

X; Y W~ ;A= T rA_xT 0A_y0W~ = T rSxT AT ASyW~ =

Now using the orthogonality of the rotational part of A and the special form of the twist matrices,

straightforward calculations lead to

X; Y W~ ;A= T rSxT SyW~ = T r!^xT !^yW + vxT vyw

Some comments on the notation are in order. The symbol W for the upper right part of W~ is

in accordance with the 3 3 matrix of the metric in GL3; IR used to define the metric in SO3.

Keeping the notation from section 3.2, if G is the matrix of the induced metric in SO3, then

X; Y W~ ;A= h !xT vxT 2 !y 32 G 3 (14)
vy
i G~ 4 5 ; G~ = 4 0 0
5

wI3

and G is given by (12).

Remark 3.5 Similar to section 3.2, some tedious calculations show that we can provide SE3 with
the same metric (14) by inheriting the metric from the ambient space at se3 (g~ij = T rLTi LjW~ ; i; j =
1; : : : ; 6 and left translating it throughout the manifold. Therefore, the metric T rXT Y W~ from

GL4; IR becomes left invariant when restricted to SE3.

12

Remark 3.6 If W is symmetric and positive definite, then G given by (12) is symmetric and posi-
tive definite and, provided that w 0, G~ is symmetric and positive definite.

4 Projection on SOn

We can use the norm induced by the metric (10) to define the distance between elements in GL3; IR.
Using this distance, for a given A 2 GL3; IR, we define the projection of A on SO3 as being
the closest R 2 SO3 with respect to metric (10).

The solution of the projection problem is derived for the general case of GLn; IR and is based

on the following lemma:

Lemma 4.1 Let A 2 GLn; IR and U; ; V its singular value decomposition. Then R = UV T is

the solution to the maximization problem

R2mSaOxn T rAT R:
Proof: Let = diagf 1; : : : ; ng, C = RT U and consider columnwise partitions for V and C

V = v1; : : : ; vn ; C = c1; : : : ; cn

Then, we have:

T rAT R = T rV UT R = T rV CT =
= T rXn iviciT

i=1

= Xn iT rviciT = Xn iviT ci

i=1 i=1

Now C and V are both orthogonal, and then kcik = kvik = 1 On the other, by Cauchy-
Schwartz, viT ci2 kvik2kcik2 = 1 and the equality holds for vi = ci, or V = C. Therefore,
Pin=1 i is an upper bound for T rAT R which is attained for R = U V T .

The following proposition is the main result of this section:

Proposition 4.2 Let A 2 GLn; IR and U; ; V the singular value decomposition of AW (i.e
AW = U V T ). Then the projection of A on SOn with respect to the elliptic metric (10) is given
by R = UV T .

13

Proof: The problem to be solved is a minimization problem:

R2mSOinn kA , Rk2W

We have

kA , Rk2W = A , R; A , R W = T r A , RT A , RW =
= T rAT AW , AT RW , RT AW + RT RW

where the second subscript of the metric has been dropped. Note that T rRT AW = T rW AT R =
T rAT RW and the quantities AT AW and RT RW = W are constant and therefore does not affect

the optimization. Therefore, the problem to be solved becomes:

R2mSaOxn T rW AT R
With AW = U V T , the solution is given by Lemma 4.1 and the claim is proved.

Remark 4.3 It is easy to see that the minimum value of the W-distance in Proposition 4.2 is given

by T rW ,1V 2V T + T rW , 2T r. For the particular case when W = I3, the minimum
,distance becomes Pni=1 i 12, which is the common way of describing how ”far” is a matrix

from being orthogonal.

The question we might ask is what happens with the solution to the projection problem when

the manifold GLn; IR is acted upon by the group SOn. The answer is given in the following

proposition:

Proposition 4.4 The solution to the projection problem described above is left invariant under ac-

tions of elements from SOn. If W = I3, the solution is bi-invariant.

Proof: Let A 2 GLn; IR, AW = UV T and the corresponding projection R 2 SOn, R =
UV T . Let any L 2 SOn and A = LA. Then an SVD for AW can be found from the SVD for
AW in the form A = LUV T . Then, by Proposition 4.2, the projection of A is R = LUV T =
LR, which proves left invariance. Similarly, right invariance would imply that W commutes with
arbitrary elements from SOn, which leads to W = I.

14

Remark 4.5 For the case W = I, it is worthwhile to note that other projection methods do not
exhibit bi-invariance. For instance, it is customary to find the projection R 2 SOn by applying a

Gram-Schmidt procedure (QR decomposition). In this case it is easy to see that the solution is left

invariant, but in general it is not right invariant.

4.1 Projection on SEn

Similarly to the previous section, if a metric of the form (10) is defined in GLn; IR with the matrix
of the metric given by (13), we can find the corresponding projection on SEn , 1:

Proposition 4.6 Let B 2 GLn; IR with the following block partition

3

2 5 1 IRn,1n,1

4
2 1
3
B BB BB ; B 2 ;= 4

2 2 2B ; B ; B2 IRn,11 3 IR1n,1 4 IR

and U; ; V the singular value decomposition of B1W . Then the projection of B on SEn , 1 is

given by 2 3

A=4 UV T B2 5 2 SEn , 1

01

Proof: Let 2 R d 3

A=4 0 1 5; R 2 SOn , 1; d 2 IRn,1

The problem to be solved can be formulated as follows:

A2SmEinn,1 kB , AkW2~

We have:

kB , Ak2W~ = T r B , AT B , AW~ =
= T rBT BW~ , 2T rBT AW~ + T rAT AW~

The quantity BT BW~ is not involved in the optimization. Therefore, the problem becomes

A2SmEinn,1 ,2T rBT AW~ + T rAT AW~

15

The observation that

T rAT AW~ = T rW + dT d + 1w;
T rBT AW~ = T rB1T RW + B2T d + B4w

separates the initial problem into two subproblems:

a R2SmOanx,1 T rB1T RW = R2SmOanx,1 T rW B1T R

and

b d2mIRinn,1 h,2B2T d + dT di
From lemma 4.1, the solution to subproblem (a) is R = U V T . For the second subproblem, let

f : IRn,1 ! IR; fx = ,2B2T x + xT x

The critical points of the scalar function f are given by

rfx = ,2B2 + 2x = 0 x = B2

and the Hessian r2f x = 2I is always positive definite. Therefore, the solution is d = B2 which

concludes the proof.

Similarly to SOn, invariance properties are exhibited by the projection on SEn:

Proposition 4.7 The solution to the projection problem on SEn,1 is left invariant under actions
of elements from SOn , 1.

Proof: Let 2 B1 B2 3
B3 B4
B=4 5 2 GLn; IR; B1W = UV T

2 UV T B3
25
A=4 01

be a solution pair for the projection problem. Let

2 R d3
5
Le = 4 01

16

be an arbitrary element from SEn , 1. Under left actions of Le, the pair becomes

LeB 2 RB1 + dB3 RB2 + dB4 3
B3 B4
=4 5;

LeA = 2 RUV T RB2 + d 3
4 5
01

from which it is easy to see that a necessary condition for left invariance (i.e LeA is the projection
of LeB) is dB4 = d; 8B4 2 IR which implies d = 0. Conversely, it is easy to see that the problem
is left invariant under actions of Le with d = 0.

Remark 4.8 If we let B4 = 1 and B3 = 0 in the above proof of left invariance, it is straightforward

to see that if the projection problem is restricted to the subset

2 B1 B2 3

B=4 0 1 5 2 GLn; IR

then it becomes left invariant under all actions of SEn , 1.

Remark 4.9 In the special case when W = In,1 (i.e. the bi-invariant metric on SOn , 1), “left

invariant” in the above proposition becomes “bi-invariant”. See [2] for a more detailed treatment.

5 Generating smooth curves on SO3

Based on the results we proved so far, a procedure for generating near optimal curves on SO3

follows: generate the curve in the ambient space and project it on the rotation group. Due to the fact

that the metric we defined in GL3; IR is the same at all points, the corresponding Christoffel sym-

bols are all zero. Consequently, the optimal curves in the ambient space assume simple analytical
forms (i.e goedesics - straight lines, minimum acceleration curves - cubic polinomial curves, mini-
mum jerk curves - fifth order polynomial curves, all parameterized by time). The resulting curve in

GL3; IR is linear in the boundary conditions, and therefore left and right invariant. Recall that the

projection procedure is left invariant in the general case and bi-invariant in the particular case of a
canonical metric, and so is the overall procedure.

17

5.1 Geodesics on SO3 with the Euclidean metric

The problem we approach is generating a geodesic Rt between given end positions R1 = R0

and R2 = R1 on SO3. Assume the metric in the ambient space GL3; IR is the canonical

metric W = I, which gives the bi-invariant metric G = 2 I on SO3. Without loss of gener-

ality, we will assume R1 = I. Indeed, a geodesic between two arbitrary positions R1 and R2 is

the geodesic between I and R R,1 left translated by R1. Exponential coordinates ; ;1 2 3 are
12

considered as local parameterization of SO3. If R2 = e!^0, then the geodesic is the exponential

mapping of the uniformly parameterized segment passing through 0 and !0 ( t = !0t) from the

exponential coordinates:

Rt = e t^ = e!^0t

The geodesic in the ambient space satisfying the given boundary conditions on SO3 is

At = I + R2 , It; t 2 0; 1

We derive an analytical expression for the projection of an arbitrarily parameterized line in the

ambient space GL3; IR onto SO3, which will answer the following three questions.

Does the projection of a geodesic from GL3; IR follow the same path as the true geodesic
on SO3? If the answer is yes, then the following question makes sense:

Do the above two curves have the same parameterization? If the answer is no,

Can we find an appropriate parameterization of the line in the ambient space so that the

projection will be identical to the true geodesic on SO3?

Proposition 5.1 is the key result of this section.

Proposition 5.1 Let At = I + R2 , Ift; t 2 0; 1 be a line in GL3:IR with R2 = e!^0 2
SO3 (f continuous, f0 = 0; f1 = 1). Then the projection of this line R?t onto SO3 is

the exponential mapping of a segment drawn between the origin and !0 in exponential coordinates

parameterized by t:

At = UttV T t R?t = UtV T t = e!^0t (15)
1 (16)
t = atan21 , f t + f t cos k!0k; f t sin k!0k
k!0k

18

The proof is rather involved and can be found in the Appendix.

Note that the obtained parameterization t satisfies the boundary conditions 0 = 0, 1 =

1

As a particular case of Proposition 5.1 for f t = t, we have the following corollary answering

the first two questions at the beginning of this section:

Corollary 5.2 The true geodesic on SO3 and the projected geodesic from GL3; IR with ends
on SO3 follow the same path on SO3 but with different parameterizations. The projected curve

is the exponential mapping of the same segment from the exponential coordinates

R?t = e!^0t

with the following parameterization

t = 1 atan21 , t + t cos k!0k; t sin k!0k

k!0k

The derivative of the function t is given by

d t = sin k!0k
dt k!0kst

where st is given by (24) in the Appendix. Plots of the function t and its derivative are given
below for t 2 0; 1 and the magnitude of the displacement on the manifold k!0k 2 0; .

(a) (b)

1 1 2 1
0.75 0.8 1.5 0.8
0.6 0.6
0.5 0.4 Time 1 0.4 Time
0.25 0.5
0.2 0.2
0 0
30 30
1 2 1 2
Displacement
Displacement

Figure 2: (a) Function t; (b) The derivative ddt t.

19

The conclusion is that even though the line in GL3; IR is followed at constant velocity, the

projected curve on the manifold has low speed at the begining, attains its maximum in the middle,

k kand slows down as approaching the end point. The larger the displacement !0 , the larger the

discrepancy in speeds. Also note that the middle of the line is projected into the middle of the true

geodesic because 0:5 = 0:5 (i.e the functions t and t are equal at t = 0:5). This result has
been stated without proof in [18] in the context of unit quaternions as local parameters of SO3
(viewed as the unit sphere S3 in the projective space IRP 3).

To answer the third question, we need to find a parameterization f t (f 0 = 0, f 1 = 1) of
the line in GL3; IR with ends on SO3, which gives uniform parameterization t of the projected
curve in exponential coordinates. The solution of the following equation in f

atan21 , ft + ft cos k!0k; ft sin k!0k = t

is of the form sink!0kt
sink!0k1 , t + sink!0kt
f t =

The answer to the third question is stated in the following

Corollary 5.3 The true geodesic on SO3 starting at I and ending at R2 = e!^0 is the projection
of the following line from the ambient space GL3; IR:

At =I + R2 , Ift; t 2 0; 1 ; f t = sink!0kt
sink!0k1 , t + sink!0kt

Illustrative plots of f t and its derivative are given below for t 2 0; 1 and different values of

the displacement k!0k 2 0; .
As expected, to get a uniform speed on SO3, the line in GL3; IR should be followed at high

speed at the beginning, slow down in the middle, and accelerating again near the end point.

Remark 5.4 The result in Corollary 5.3 is similar to the formula for spherical linear interpolation

in terms of quaternions Slerp, suggested by Glenn Davis and alluded to by Shoemake in [18].

5.2 Minimum acceleration curves on SO3 with the Euclidean metric

First we will summarize the computation of optimal trajectories described in [20]. We will then
generate the near optimal trajectories by the projection method.

20

(a) (b)

1 1 1.5 1
0.75 0.8 1 0.8
0.6 0.6
0.5 0.4 Time 0.5 0.4 Time
0.25 0
0.2 0.2
0
30 30
1 2 1 2
Displacement Displacement

Figure 3: (a) Function f t; (b) The derivative ddt f t.

The necessary conditions for the curves that minimize the square of the L2 norm of the acceler-

ation are found by considering the first variation of the acceleration functional

La = Zb rV V; rV V dt; (17)
a

rwhere V t = dRdtt , is the affine connection compatible with a suitable Riemmanian metric, and
Rt is a curve on the manifold. The initial and final points as well as the initial and final velocities

for the motion are prescribed.

The following result is stated and proved in [20]:

Proposition 5.5 Let Rt be a curve between two prescribed points on SO3 with canonical met-

ric that has prescribed initial and final velocities. If ! is the vector from so3 corresponding to
V = ddRt , the curve minimizes the cost function La derived from the canonical metric only if the

following equation hold:

!3 + ! ! = 0 (18)

where n denotes the nth derivative of .

The above equation can be integrated to obtain (19)

!2 + ! !_ = constant

21

However, this equation cannot be further integrated analytically for arbitrary boundary conditions.

In the special case when the initial and final velocities are tangential to the geodesic passing through

the same points, the solution can be found by reparameterizing the geodesic [20]. In the general

case, one must solve equation (18) numerically. A local parameterization of SO3 should be

chosen and three first order differential equations will augment the system. The most convenient
local coordinates 2 are the exponential coordinates. Eventually, one ends up with solving a system

of 12 first order nonlinear coupled differential equations with 6 boundary conditions at each end.

_ t = !t (20)
!3 + ! ! = 0 (21)

A solution can be found using iterative procedures such as the shooting method or the relaxation

method. The latter has been chosen in this paper.

A more attractive and much simpler approach is the projection method described above. The

main idea is to relax the problem to GL3; IR, while keeping the proper boundary conditions on
SO3 with the corresponding velocities. Minimum acceleration curves are found in GL3; IR and
eventually projected back onto SO3.

2In what follows, the time interval will be t 0; 1 and the boundary conditions R0, R1,
R_ 0, R_ 1 are assumed to be specified. The minimum acceleration curve in GL3; IR with a

constant metric is a cubic given by

Rt = R0 + R1t + R2t2 + R3t3;

where R0; R1; R2; R3 2 GL3; IR are

R0 = R0; R1 = R_ 0;

R2 = ,3R0 + 3R1 , 2R_ 0 , R_ 1;
R3 = 2R0 , 2R1 + R_ 0 + R_ 1:
Now the curve on SO3 is obtained by projecting Rt onto SO3.

2All coordinate charts exhibit singularities. A solution to avoiding singularities could be using quaternions, which on
the other hand would imply a further augmentation of the state space.

22

1.6 True 1.6
1.4 Projection 1.4
1.2
True 1.2
1 Projection
0.8
0.6 1
0.4 2
0.2
0.8
0 1.5
1.5 σ3 True
σ3 Projection
a)
σ3
1 1.4 0.6

0.5 1.2 0.4

0 1 0.2 1.5
0 0.8
0.6 0
0.8 0.2 −0.5
1
0.5 0.6 0.4 0.4 0 1
0.6 0.2 0.5 0.5
σ2 0.4 0.8 0 σ2 10
0.2 σ1 σ1 σ2
00
σ1 b) c)

Figure 4: Minimum accleration curves on SO3 with canonical metric: (a) Velocity boundary
conditions along the geodesic; (b) End velocity perturbed by e1; and (c) End velocity perturbed by
5e1.

The following examples present comparisons between the minimum acceleration curves gener-

ated using the projection method and the curves obtained directly on SO3 by solving equations

(18) using the relaxation method. All the generated curves are drawn in exponential local coordi-

nates.

In Figure 4 we used the following position boundary conditions

h 0 0 iT ; h iT
6
0 = 0 1 = 3 2

The initial velocity is the one corresponding to the geodesic passing through the two positions:

! = h iT
0 632

Cases (a), (b) and (c) differ by the velocity at the end point. Figure 4 (a) corresponds to a final

velocity !1 = !0, therefore we get a minimum acceleration curve for which the end velocities are

along the velocity of the corresponding geodesic, leading to a geodesic parameterized by a cubic of

time [20]. The final velocity is !1 = !0 + e1 in case (b) and !1 = !0 + 5e1 in case (c), where
e1 = 1 0 0 T .

As seen in Figure 4 (a), the paths of the projected and the optimal curves are the same, the

parameterizations are slightly different though, as expected. In cases (b) and (c), although the

deviation of the final velocity from being homogeneous is large, the curves are close. Note that the

boundary conditions are rigorously satisfied.

23

5.3 Geodesics on SO3 with a general metric
The differential equations to be satisfied by the geodesics on SO3 equipped with metric G are

derived in [20]:

_ t = !t (22)
G!_ t = !t G!t (23)

If W is the corresponding constant metric in the ambient GL3; IR, then our method implies
the projection of the geodesic from GL3; IR

onto SO3 At = R0 + R1 , R0t; t 2 0; 1
AtW = UV T ; Rt = UtV T

An illustrative example is shown in Figure 5, where end positions in exponential coordinates are

the same as in the previous section:

h 0 0 iT ; h iT

0 = 0 1 = 6 3 2

Cases (a), (b) and (c) differ by the form of the matrix G. The metric is Euclidean in case (a) and

non-Euclidean in cases (b) and (c).

6 Conclusion

This paper develops a method for generating smooth trajectories for a moving rigid body with
specified conditions on intermediate points or at the end points. Our method involves two key steps:

(1) the generation of optimal trajectories in GLn + 1; IR; and (2) the projection of the trajectories
from GLn + 1; IR to SEn. The overall procedure is invariant with respect to both the local

coordinates on the manifold and the choice of the inertial frame. The benefits of the method are two-
fold. First, it is possible to apply any of the variety of well-known, efficient techniques to generate

optimal curves on GLn; IR [7, 8]. Second, the method yields solutions for general choices of
Riemannian metrics on SE3. For example, we can incorporate the dynamics of arbitrary rigid

24

1.6 True 1.6 True 1.6 True
1.4 Projection Projection Projection
1.2 1.4 1.4
σ3
1 σ31.2 1.2
0.8 σ3
0.6 1 1
0.4 1 1 1 0.6
0.2 0.5 0.8 0.5 0.8 0.5
0.4
0 σ2 0.6 σ2 0.6 σ2
1.5 0.2
0.4 0.4
a) 0
0.2 0.2
00 00 0 −0.2 σ1
0 0
1.5 1.5
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2

b)σ1 c)σ1

Figure 5: Geodesics for different metrics on SO3: (a) G = diagf3; 3; 3g; (b) G =
diagf3; 10; 10g; and (c) G = diagf10; 10; 3g.

bodies, or the added-mass effects in the dynamics of bodies submerged in fluids [13]. While the

numerical results in this paper are limited to trajectories on SO3, the computations in SE3 are

a straightforward extension.

Appendix

Proof of Proposition 5.1 We need to find the SVD decomposition of At = I + R2 , Ift =
UttV T , where R2 = e!^0 and ft is a continous function defined on 0; 1 satisfying f0 = 0,
f1 = 1. We start from

AT tAt = I , ft1 , ftM; M = 2I , R2 , R2T

The eigenstructure of the constant and symmetric matrix M will completely determine the SVD
of At. Because M is symmetric and real, its eigenvalues will be real and the corresponding
eigenspaces orthogonal. Let i; vi be an eigenvalue-eigenvector pair of M . Then, we have:

Mvi = ivi AT tAt = 1 , ft1 , ftivi

So, theoretically, the desired SVD decomposition is determined at this moment:

The matrix V t can be chosen constant of the form V = v1 v2 v3 where v1; v2; v3 are
orthonormal eigenvectors of M ;

25

The singular values are given by s2i t = 1 , ft1 , fti (it will be shown shortly that

the right hand side of this equality is always positive)

The time-dependence of the projection will be contained in

U t = u1t u2t u3t ; uit = Atvi ; i = 1; 2; 3
si

Using the Rodrigues formula for R2 = e!^0, it is easy to see that

M = ,1 , cos k!0 k !^02 + !^02T
k!0k2

from which it follows that the eigenvalues of M are given by

M = f0; 21 , cos k!0k; 21 , cos k!0kg

and a set of three orthonormal eigenvectors by

8 2 3 2 39
37
! ,! ,! !01 6 1 6 2 1 7=
k! k; ; ! !0 6 07 6 +2 7
! ! ! ! ! ! ! ! ! ,! !:q6 7 q 6 2 7
24 5 + +2 2 +2 2 2 24 3 1 5
+2 1 1 2 32
21 3 1 ;
3

32

where !0 = !1 !2 !3 T . With the eigenstructure of M determined, we write

t = diagf1; st; stg; st = q , cos k!0kf2t , 21 , cos k!0kft + 1 (24)

21

,where the binomial under the square root is always positive because it is positive at zero and 1
cos k!0k 2 0; 2 gives a negative discriminant. Some straightforward but rather tedious calculation

leads to: !^0 1t + k!!^002k2
k!0k
U tV T = I + 2t + 1 ,

where 1 , f t + ft cos k!0k f t sin k!0k
st st
1t = ; 2t =

We will restrict our discussion to k!0k 2 0; (in accordance to the exponential coordinates)

which will give 2t 0. Note that 2 t + 22t = 1, so it is appropriate to define a function
1

t 2 0; 1 so that

1t = cosk!0kt; 2t = sink!0kt

26

By use of the Rodrigues formula again,

UtV T = e!^0t

so the projected line is the exponential mapping of a segment between the origin and !0 in expo-

nential coordinates. The parameterization of the segment is given by

t = 1 atan21 , f t + f t cos k!0k; f t sin k!0k

k!0k

References

[1] R. S. Ball. The theory of screws. Hodges & Foster, Dublin, 1876.

[2] C. Belta and V. Kumar. An efficient geometric approach to rigid body motion interpolation.
In ASME 2000 Design Engineering Technical Conferences, Baltimore, MD, 2000.

[3] M. Camarinha, F. Silva Leite, and P. Crouch. Splines of class ck on non-Euclidean spaces.

IMA J. Math. Control Inform., 12(4):399–410, 1995.

[4] J. Cheeger and D. G. Ebin. Comparison Theorems in Riemannian Geometry. North-Holland
Publishing Company, Amsterdam, 1975.

[5] P. Crouch and F. Silva Leite. The dynamic interpolation problem: on Riemannian manifolds,
Lie groups, and symmetric spaces. J. Dynam. Control Systems, 1(2):177–202, 1995.

[6] M. P. do Carmo. Riemannian geometry. Birkhauser, Boston, 1992.

[7] G. E. Farin. Curves and surfaces for computer aided geometric design : a practical guide.
Academic Press, Boston, 3 edition, 1992.

[8] J. Gallier. Curves and Surfaces in Geometric Modeling - Theory and Algorithms. Morgan
Kaufmann Publishers, San Francisco, CA, 2000.

[9] Q. J. Ge and B. Ravani. Computer aided geometric design of motion interpolants. ASME
Journal of Mechanical Design, 116:756–762, 1994.

27

[10] Q. J. Ge and B. Ravani. Geometric construction of Bezier motions. ASME Journal of Mechan-
ical Design, 116:749–755, 1994.

[11] J. Hoschek and D. Lasser. Fundamentals of Computer Aided Geometric Design. AK Peters,
1993.

[12] B. Ju¨tler. Visualization of moving objects using dual quaternion curves. Computers and
graphics, 18(3):315–326, 1994.

[13] N. E. Leonard. Stability of a bottom-heavy underwater vehicle. Automatica, 33(3):331–346,
1997.

[14] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation.
CRC Press, 1994.

[15] L. Noakes, G. Heinzinger, and B. Paden. Cubic splines on curved spaces. IMA J. of Math.
Control & Information, 6:465–473, 1989.

[16] F. C. Park and I. G. Kang. Cubic interpolation on the rotation group using Cayley parameters.
In Proceedings of the ASME 24th Biennial Mechanisms Conference, Irvine, CA, 1996.

[17] F. C. Park and B. Ravani. Bezier curves on Riemannian manifolds and Lie groups with kine-
matics applications. ASME Journal of Mechanical Design, 117(1):36–40, 1995.

[18] K. Shoemake. Animating rotation with quaternion curves. ACM Siggraph, 19(3):245–254,
1985.

[19] M. Zˇ efran and V. Kumar. Interpolation schemes for rigid body motions. Computer-Aided
Design, 30(3), 1998.

[20] M. Zˇ efran, V. Kumar, and C. Croke. On the generation of smooth three-dimensional rigid
body motions. IEEE Transactions on Robotics and Automation, 14(4):579–589, 1995.

[21] M. Zˇ efran, V. Kumar, and C. Croke. Choice of Riemannian metrics for rigid body kinematics.
In Proceedings of the ASME 24th Biennial Mechanisms Conference, Irvine, CA, 1996.

28