New metrics for rigid body motion interpolation - helix

New metrics for rigid body motion interpolation
Calin Belta and Vijay Kumar
fcalin j kumarg@grip.cis.upenn.edu
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 SE(3), 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 SE(3), there is a natural trajectory between the two points that is biinvariant
or invariant with respect to the choice of coordinate systems. This trajectory is the wellknown
screw motion [1]. In an earlier paper, we showed that the screw motion minimizes an indefinite
metric [21]. However, there is no clear interpretation of optimality for a trajectory that
minimizes an indefinite metric.
In this paper, we consider SE(n) to be a submanifold (and a subgroup) of GL(n +1; IR).
Our method involves two key steps: (1) the generation of optimal trajectories in GL(n +1; IR);
and (2) the projection of the trajectories from GL(n +1; IR) to SE(n). 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 GL(n; IR) [7, 8]. Second, the
method yields solutions for general choices of Riemannian metrics on SE(3). 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, GL(n +1; IR), one that induces an appropriate
metric on SE(n). We define a suitable projection operator and establish its invariance with respect
to changes in coordinates. The fact that the standard metric in GL(n; IR) induces the bi-invariant
metric on SO(n) falls out of our analysis. The projection operator allows us to project the optimal
curves onto SE(n) 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 problem
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 description
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 polynomials.
Ge and Ravani [9] used the dual unit quaternion representation of SE(3) and subsequently
applied Euclidean methods to interpolate in this space. Jütler [12] formulated a more general version
of the polynomial interpolation by using dual (instead of dual unit) quaternions to represent
SE(3). In such a representation, an element of SE(3) corresponds to a whole equivalence class
of dual quaternions. Park and Kang [16] derived a rational interpolating scheme for the group of
2

otations SO(3) 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 cubic
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 SO(3) and SE(3) in [20]. However,
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 Riemannian
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 SO(n) and then extend the results to SE(n). The bi-ivariant metric
on SO(n) [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 SO(n) or SE(n , 1 is inherited from
the higher dimensional space GL(n; IR) equipped with the appropriate metric. Next, a projection
operator is defined and subsequently used to project optimal curves from the ambient space onto
SO(n) or SE(n , 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 SO(3) and SE(3)
Let GL(n; IR) denote the general linear group of dimension n. As a manifold, GL(n; IR) can be
regarded as an open subset of IR n2 . Moreover, matrix multiplication and inversion are both smooth
operations, which make GL(n; IR) a Lie group. The special orthogonal group is a subgroup of the
general linear group, defined as
SO(n) =
n
o
R 2 GL(n; IR) : RR T = I; detR =1
SO(n) is referred to as the rotation group on IR n . SE(n) =SO(n) IR n is the special Euclidean
group, and is the set of all displacements in IR n . Special consideration will be given to SO(3) and
SE(3).
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 O 0 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. SE(3) is
the set of all rigid body transformations in three-dimensions:
SE(3) =
8
<
: A j A = 2
4 R d
0 1
d 2 IR 3 ;R T R = I;det(R) =1
3
5 ;R2 IR 33 ;
SE(3) is a closed subset of GL(4; 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 SO(3) and SE(3), denoted by so(3) and se(3) respectively, are given by:
n
o
so(3) = ^! 2 IR 33 ; ^! T = ,^! :
se(3) =
82
<
:
4 ^! v
0 0
3
5 j ^! 2 IR 33 ;v 2 IR 3 ; ^! T = ,^!
; :
A 3 3 skew-symmetric matrix ^! can be uniquely identified with a vector ! 2 IR 3 so that for an
arbitrary vector x 2 IR 3 , ^!x = ! x, where is the vector cross product operation in IR 3 . Each
4
o
:
9
=

{F}
z
z’
{M}
t
x’ 3
{M} z’
y’
t 2 y’
x’
t 1
x’
{M}
y’
z’
y
x
Figure 1: The inertial (fixed) frame and the moving frame attached to the rigid body
element S 2 se(3) can be thus identified with a vector pair f!; vg. Givenacurve
A(t) :[,a; a] ! SE(3); A(t) =
2
4
R(t) d(t)
0 1
3
5
an element S(t) of the Lie algebra se(3) can be associated to the tangent vector A(t) _ at an arbitrary
point t by:
S(t) =A ,1 (t) _ A(t) =
2
4 ^!(t) RT d_
0 0
3
5 : (1)
where ^!(t) =R T _R is the corresponding element from so(3).
A curve on SE(3) physically represents a motion of the rigid body. If f!(t);v(t)g is the vector
pair corresponding to S(t), then! physically corresponds to the angular velocity of the rigid body
while v is the linear velocity of the origin O 0 of the frame M, both expressed in the frame M. In
kinematics, elements of this form are called twists and se(3) thus corresponds to the space of twists.
The twist S(t) computed from Equation (1) does not depend on the choice of the inertial frame F .
For this reason, S(t) 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 so(3) 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 so(3) is:
2 3
3
0 0 0
0 0 1
6
7
6
7
L o 1 = 0 0 ,1
0 0 0
6
4
2
7
5 Lo 2 = 6
4
0 1 0
,1 0 0
2
7
5 Lo 3 = 6
4
0 ,1 0
1 0 0
0 0 0
3
7
5
(2)
L o, 1 Lo and 2 Lo 3
represent instantaneous rotations about the Cartesian axes x, y and z, respectively.
The components of a ^! 2 se(3) in this basis are given precisely by the angular velocity vector !.
The standard basis for se(3) is:
2
3
2
L 1 = 4 Lo 1
0
5 L2 = 4 Lo 2
0
5 L3 = 4 Lo 3
0
0 0
0 0
0 0
L 4 =
2
4 0 e 1
0 0
3
5 L 5 =
2
4 0 e 2
0 0
3
3
5 L 4 =
2
2
4 0 e 3
0 0
3
5
3
5
(3)
where
e 1 =
h
1 0 0
i T
; e2 =
h
0 1 0
i T
; e3 =
h
0 0 1
i T
The twists L 4 , L 5 and L 6 represent instantaneous translations along the Cartesian axes x, y and
z, respectively. The components of a twist S 2 se(3) 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 SE(3) 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, onSE(3) is obtained by left translation of an
element S 2 se(3). The value of the vector field X at an arbitrary point A 2 SE(3) is given by:
X(A) = S(A) =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
se(3).
Since the vectors L 1 ;L 2 ;:::;L 6 are a basis for the Lie algebra se(3), the vectors L 1 (A);:::;
L 6 (A) form a basis of the tangent space at any point A 2 SE(3). Therefore, any vector field X can
be expressed as
X =
6X
i=1
X i L i ; (5)
where the coefficients X i vary over the manifold. If the coefficients are constants, then X is left
invariant. By defining:
! =[X 1 ;X 2 ;X 3 ] T ; v =[X 4 ;X 5 ;X 6 ] T ;
we can associate a vector pair of functions f!; vg to an arbitrary vector field X. If a curve A(t)
describes a motion of the rigid body and V = dA is the vector field tangent to A(t), the vector
dt
pair f!; vg 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 se(3),letA S : IR ! SE(3) denote the integral curve of the left invariant vector field
X S passing through I at t =0.Thatis,A S (0) = I and d dt A S(t) =X S (A S (t)). It follows that (see
DoCarmo pg.80) A S (s + t) =A S (t)A S (s) which means that A S (t) is a one-parameter subgroup
of SE(3). The function exp : se(3) ! SE(3) defined by exp(S) =A S (1) is called the exponential
map of the Lie algebra se(3) into SE(3). It is easy to show [14] that the exponential map takes the
line tS 2 se(3); t 2 IR into a one-parameter subgroup of SE(3), i.e.
exp(tS) =A S (t): (6)
Using Equation (1) we can show that the exponential map agrees with the usual exponentiation of
the matrices in IR 44 :
exp(tS) =
1X
k=0
t k S k
; (7)
k!
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 SO(3), is known as Rodrigues’ formula.
The formula for the sum in SE(3) 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.
On a n dimensional manifold, the metric is locally characterized by a n n matrix of C 1 functions
g ij = where X i are basis vector fields 1 . Note that the entries g ij 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 [g ij ] completely defines the metric.
On SE(3) (on any Lie group), an inner product on the Lie algebra can be extended to a Riemannian
metric over the manifold using left (or right) translation. To see this, consider the inner
product of two elements S 1 ;S 2 2 se(3) defined by
j I = s T Ws 1 2; (8)
where s 1 and s 2 are the 6 1 vectors of components of S 1 and S 2 with respect to some basis
and W is a positive definite matrix. If V 1 and V 2 are tangent vectors at an arbitrary group element
A 2 SE(3), the inner product j A
in the tangent space T A SE(3) can be defined by:
j A = : (9)
I
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 SO(n) and SE(n)
In this section we will show that there is a simple way of defining a left (or right) invariant metric
in SO(n) (or SE(n , 1) by introducing an appropriate constant metric in GL(n; IR). Defining a
1 The 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 so(n) (or se(n , 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 GL(n; IR) and any X; Y 2
T A GL(n; IR), define
W;A = Tr(X T YW)=Tr(WX T Y )=Tr(YWX T ) (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 GL(n; IR). Whythe
second subscript then? This will become useful when studying the invariance of the above form. It
is clear that form (10) is quadratic in the entries of X and Y .Letx; y 2 IR n2 be the column vectors
obtained by abutting the transposed lines from X and Y . Then,
W;A = x T Wy;
where
W = blockdiagonal(W T ;W T ;:::;W T ); W 2 IR n2 n 2 :
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 GL(n; IR) is left invariant when restricted to SO(n).
Metric (10) restricted to SO(n) is bi-invariant if W = I; > 0, I is the n n identity
matrix.
Proof: Let any A; B 2 GL(n; IR) and any X; Y 2 T A GL(n; RR). Then, we have
W;A = Tr(X T YW); W;BA = Tr(X T B T BY W)
from which we conclude that the metric is invariant under left translations with elements B 2
SO(n). When restricted to SO(n), (i.e A 2 SO(n)), the metric becomes left invariant. Biinvariance
is right invariance when the metric is left invariant. Note that
W;A = Tr(YWX T ); W;AB = Tr(YBWB T X T )
9

So, right invariance is guaranteed under the condition that BWB T = W , or equivalently, W commutes
with all the elements B 2 SO(n), which is equivalent to W = I.
Remark 3.2 If right invariance on SO(n) is desired, we can define
2
> W;A = Tr(XY T W )=Tr(Y T WX)=Tr(WXY T )
Metric will be right invariant on SO(n) for W symmetric and positive definite and biinvariant
if W = I. The proof is similar to that for metric and is omitted.
3.2 The induced metric on SO(3)
Even though the following derivation can be done in the general case of a n(n , 1)=2-dimensional
manifold SO(n) in the ambient n 2 -dimensional manifold GL(n; 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 SO(3). LetX; Y be two vectors from T R SO(3)
and R x (t);R y (t) the corresponding local flows so that
X = R _ x (0); Y = R _ y (0); R x (0) = R y (0) = R:
The metric inherited from GL(3; IR) can be written as:
W;R = Tr( _ R T x (0) _ R y (0)W )=Tr( _ R T x (0)RRT _R y (0)W )=Tr(^! T x ^! yW )
where ^! x = R x (0) T _R x (0) and ^! y = R y (0) T _R y (0) are the corresponding twists from the Lie
algebra so(3). If we write the above relation using the vector form of the twists, some elementary
algebra leads to:
W;R = ! x T G! y (11)
where
G = Tr(W )I 3 , W (12)
is the matrix of the metric on SO(3) as defined by (8). A different equivalent way of arriving at
the expression of G would be defining the metric in so(3) (i.e. at identity of SO(3)) asbeingthe
one inherited from T I GL(3; IR): g ij = Tr(L o i T L o j W ); i; j = 1; 2; 3 (Lo 1 ;Lo 2 ;Lo 3
is the basis in
10

so(3)). Left translating this metric throughout the manifold is equivalent to inheriting the metric at
each 3-dimensional tangent space of SO(3) from the corresponding 9-dimensional tangent space of
GL(3; 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
i (G) =
X
j (W ) , i (W )=
X
j
j6=i
Because W is positive definite, it follows that i (W ) > 0 which implies i (G) > 0, i.e. Gis
positive definite. For the third part, we start from the equivalent form of (12)
W = 1 Tr(G)I 3 , G
2
j W
which implies
and the claim is proved.
i (W )= 1 2
X
j
j (G) , i (G)
Remark 3.4 In the particular case when W = I; > 0, from (12), we have G =2I, whichis
the standard bi-invariant metric on SO(3). This is consistent with the second assertion in Proposition
3.1. For =1, metric (10) will induce the well known Frobenius norm on GL(3; IR).
3.3 The induced metric in SE(3)
Similarly, we can get a left invariant metric on SE(n , 1) by inheriting the metric
W given
~
by (10) from GL(n; IR). For practical purposes, we will assume the following form for the n n
matrix of the metric in (10):
~W =
2
4 W 0
0 w
3
5 (13)
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 SE(3) we follow the same procedure as in section 3.2 for the particular
case n =4.
Let A be an arbitrary element from SO(3). Let X; Y be two vectors from T A SE(3) and
A x (t);A y (t) the corresponding local flows so that
Let
X =
_ A x (0); Y = _ A y (0); A x (0) = A y (0) = A:
A i (t) =
and the corresponding twists at time 0:
2
4 R i(t) d i (t)
0 1
S i = A ,1
i (0) A _ i (0) =
2
4 ^! i v i
0 0
The metric inherited from GL(4; IR) can be written as:
3
5 ; i 2fx; yg
3
5 ; i 2fx; yg
~ W;A = Tr( _ A T x (0) _ A y (0) ~ W )=Tr(S T x A T AS y ~ W =
Now using the orthogonality of the rotational part of A and the special form of the twist matrices,
straightforward calculations lead to
~ W;A = Tr(ST x S y ~ W )=Tr(^! T x ^! yW )+v T x v y w
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 GL(3; IR) used to define the metric in SO(3).
Keeping the notation from section 3.2, if G is the matrix of the induced metric in SO(3), then
2 3 2 3
h i
W;A ~ = ! x T vx
T G ~ 4 ! y
5 ; G ~ = 4 G 0 5 (14)
v y 0 wI 3
and G is given by (12).
Remark 3.5 Similar to section 3.2, some tedious calculations show that we can provide SE(3) with
the same metric (14) by inheriting the metric from the ambient space at se(3) (~g ij = Tr(L T i L j W ~ ); i; j =
1;:::;6 and left translating it throughout the manifold. Therefore, the metric Tr(X T Y ~ W ) from
GL(4; IR) becomes left invariant when restricted to SE(3).
12

Remark 3.6 If W is symmetric and positive definite, then G given by (12) is symmetric and positive
definite and, provided that w>0, G ~ is symmetric and positive definite.
4 Projection on SO(n)
We can use the norm induced by the metric (10) to define the distance between elements in GL(3; IR).
Using this distance, for a given A 2 GL(3; IR), wedefinethe projection of A on SO(3) as being
the closest R 2 SO(3) with respect to metric (10).
The solution of the projection problem is derived for the general case of GL(n; IR) and is based
on the following lemma:
Lemma 4.1 Let A 2 GL(n; IR) and U; ;V its singular value decomposition. Then R = UV T is
the solution to the maximization problem
max Tr(A T R):
R2SO(n)
Proof: Let =diagf 1 ;:::; n g, C = R T U and consider columnwise partitions for V and C
V =[v 1 ;:::;v n ]; C =[c 1 ;:::;c n ]
Then, we have:
Tr(A T R)=Tr(V U T R)=Tr(V C T )=
=
nX
= Tr(
nX
i=1
i v i c T i )
i v T i c i
X
n
i Tr(v i c T i )=
i=1 i=1
Now C and V are both orthogonal, and then kc i k = kv i k = 1 On the other, by Cauchy-
Schwartz, (vi T c i) 2 kv i k 2 kc i k 2 = 1 and the equality holds for v i = c i ,orV = C. Therefore,
P ni=1
i is an upper bound for Tr(A T R) which is attained for R = UV T .
The following proposition is the main result of this section:
Proposition 4.2 Let A 2 GL(n; IR) and U; ;V the singular value decomposition of AW (i.e
AW = U V T ). Then the projection of A on SO(n) with respect to the elliptic metric (10) is given
by R = UV T .
13

Proof: The problem to be solved is a minimization problem:
We have
min kA , Rk 2 W
R2SO(n)
kA , Rk 2 W = W = Tr[(A , R) T (A , R)W ]=
= Tr(A T AW , A T RW , R T AW + R T RW )
where the second subscript of the metric has been dropped. Note that Tr(R T AW )=Tr(WA T R =
Tr(A T RW ) and the quantities A T AW and R T RW = W are constant and therefore does not affect
the optimization. Therefore, the problem to be solved becomes:
max Tr(WA T R)
R2SO(n)
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 Tr(W ,1 V 2 V T P )+Tr(W ) , 2Tr(). For the particular case when W = I 3 , the minimum
n
distance becomes ( i=1 i , 1) 2 , 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 GL(n; IR) is acted upon by the group SO(n). The answer is given in the following
proposition:
Proposition 4.4 The solution to the projection problem described above is left invariant under actions
of elements from SO(n). IfW = I 3 , the solution is bi-invariant.
Proof: Let A 2 GL(n; IR), AW = U V T and the corresponding projection R 2 SO(n), R =
UV T . Let any L 2 SO(n) and A = LA. ThenanSVDforAW can be found from the SVD for
AW in the form A =(LU )V 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 SO(n), 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 SO(n) 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 SE(n)
Similarly to the previous section, if a metric of the form (10) is defined in GL(n; IR) with the matrix
of the metric given by (13), we can find the corresponding projection on SE(n , 1):
Proposition 4.6 Let B 2 GL(n; IR) with the following block partition
B =
2 3
4 B 1 B 2
5 ;B1 2 IR (n,1)(n,1) ;
B 3 B 4
B 2 2 IR (n,1)1 ;B 3 2 IR 1(n,1) ;B 4 2 IR
and U; ;V the singular value decomposition of B 1 W . Then the projection of B on SE(n , 1) is
given by
Proof: Let
A =
A =
2
4 R d
2
4 UV T B 2
0 1
0 1
The problem to be solved can be formulated as follows:
3
5 2 SE(n , 1)
3
5 ; R 2 SO(n , 1); d2 IR
n,1
We have:
min
A2SE(n,1)
kB , Ak 2~ W
kB , Ak 2~ W = Tr[(B , A)T (B , A) ~ W ]=
= Tr(B T B ~ W ) , 2Tr(B T A ~ W )+Tr(A T A ~ W )
The quantity B T B ~ W is not involved in the optimization. Therefore, the problem becomes
min [,2Tr(B T A W ~ )+Tr(A T A W ~ )]
A2SE(n,1)
15

The observation that
Tr(A T A ~ W )=Tr(W )+(d T d +1)w;
Tr(B T A ~ W )=Tr(B T 1 RW +(B T 2 d + B 4 )w
separates the initial problem into two subproblems:
and
(a)
max Tr(B T 1 RW )= max Tr(WB T 1 R)
R2SO(n,1)
R2SO(n,1)
(b)
h i
min ,2B T
d2IR n,1 2 d + d T d
From lemma 4.1, the solution to subproblem (a) is R = UV T . For the second subproblem, let
f : IR n,1 ! IR; f (x) =,2B T 2 x + xT x
The critical points of the scalar function f are given by
rf (x) =,2B 2 +2x =0 ) x = B 2
and the Hessian r 2 f (x) =2I is always positive definite. Therefore, the solution is d = B 2 which
concludes the proof.
Similarly to SO(n), invariance properties are exhibited by the projection on SE(n):
Proposition 4.7 The solution to the projection problem on SE(n,1) is left invariant under actions
of elements from SO(n , 1).
Proof: Let
B =
2 3
4 B 1 B 2
5 2 GL(n; IR); B1 W = U V T
B 3 B 4
A =
2
4 UV T B 2
0 1
3
5
be a solution pair for the projection problem. Let
Le =
2
4 R d
0 1
3
5
16

e an arbitrary element from SE(n , 1). Under left actions of Le, the pair becomes
LeB =
2
B 3 B 4
4 RB 1 + dB 3 RB 2 + dB 4
3
5 ;
LeA =
2
4 RUV T RB 2 + d
0 1
from which it is easy to see that a necessary condition for left invariance (i.e L e A is the projection
of L e B)isdB 4 = d; 8B 4 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.
3
5
Remark 4.8 If we let B 4 =1and B 3 =0in the above proof of left invariance, it is straightforward
to see that if the projection problem is restricted to the subset
B =
2
4 B 1 B 2
0 1
3
5 2 GL(n; IR)
then it becomes left invariant under all actions of SE(n , 1).
Remark 4.9 In the special case when W = I n,1 (i.e. the bi-invariant metric on SO(n,1)), “left
invariant” in the above proposition becomes “bi-invariant”. See [2] for a more detailed treatment.
5 Generating smooth curves on SO(3)
Based on the results we proved so far, a procedure for generating near optimal curves on SO(3)
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 GL(3; IR) is the same at all points, the corresponding Christoffel symbols
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, minimum
jerk curves - fifth order polynomial curves, all parameterized by time). The resulting curve in
GL(3; 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 SO(3) with the Euclidean metric
The problem we approach is generating a geodesic R(t) between given end positions R 1 = R(0)
and R 2 = R(1) on SO(3). Assume the metric in the ambient space GL(3; IR) is the canonical
metric W = I, which gives the bi-invariant metric G = 2I on SO(3). Without loss of generality,
we will assume R 1 = I. Indeed, a geodesic between two arbitrary positions R 1 and R 2 is
the geodesic between I and R ,1 R 1 2 left translated by R 1 . Exponential coordinates 1 ; 2 ; 3 are
considered as local parameterization of SO(3). IfR 2 = e^! 0
, then the geodesic is the exponential
mapping of the uniformly parameterized segment passing through 0 and ! 0 ((t) =! 0 t) from the
exponential coordinates:
R(t) =e^(t) = e^! 0t
The geodesic in the ambient space satisfying the given boundary conditions on SO(3) is
A(t) =I +(R 2 , I)t; t 2 [0; 1]
We derive an analytical expression for the projection of an arbitrarily parameterized line in the
ambient space GL(3; IR) onto SO(3), which will answer the following three questions.
Does the projection of a geodesic from GL(3; IR) follow the same path as the true geodesic
on SO(3)? 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 SO(3)?
Proposition 5.1 is the key result of this section.
Proposition 5.1 Let A(t) =I +(R 2 , I)f (t); t 2 [0; 1] be a line in GL(3:IR) with R 2 = e^! 0
2
SO(3) (f continuous, f (0) = 0; f(1) = 1). Then the projection of this line R ? (t) onto SO(3) is
the exponential mapping of a segment drawn between the origin and ! 0 in exponential coordinates
parameterized by (t):
A(t) =U (t)(t)V T (t) ) R ? (t) =U (t)V T (t) =e^! 0(t)
(t) = 1
k! 0 k atan2(1 , f (t) +f (t) cos k! 0k;f(t) sin k! 0 k) (16)
(15)
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 SO(3) and the projected geodesic from GL(3; IR) with ends
on SO(3) follow the same path on SO(3) but with different parameterizations. The projected curve
is the exponential mapping of the same segment from the exponential coordinates
R ? (t) =e^! 0(t)
with the following parameterization
(t) = 1
k! 0 k atan2(1 , t + t cos k! 0k;tsin k! 0 k)
The derivative of the function (t) is given by
d
dt (t) = sin k! 0k
k! 0 ks(t)
where s(t) 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! 0 k2(0;).
(a)
(b)
1
0.75
0.5
0.25
0
1
Displacement
2
3
0
1
0.8
0.6
0.4
Time
0.2
2
1.5
1
0.5
0
1
Displacement
2
3
0
1
0.8
0.6
0.4
Time
0.2
Figure 2: (a) Function (t); (b) The derivative d dt (t).
19

The conclusion is that even though the line in GL(3; IR) is followed at constant velocity, the
projected curve on the manifold has low speed at the begining, attains its maximum in the middle,
and slows down as approaching the end point. The larger the displacement k! 0 k, 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 SO(3)
(viewed as the unit sphere S 3 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 GL(3; IR) with ends on SO(3), which gives uniform parameterization t of the projected
curve in exponential coordinates. The solution of the following equation in f
atan2(1 , f (t) +f (t) cos k! 0 k;f(t) sin k! 0 k = t
is of the form
sin(k! 0 kt)
f (t) =
sin(k! 0 k(1 , t)) + sin(k! 0 kt)
The answer to the third question is stated in the following
Corollary 5.3 The true geodesic on SO(3) starting at I and ending at R 2 = e^! 0
is the projection
of the following line from the ambient space GL(3; IR):
A(t) =I +(R 2 , I)f (t); t 2 [0; 1]; f (t) =
sin(k! 0 kt)
sin(k! 0 k(1 , t)) + sin(k! 0 kt)
Illustrative plots of f (t) and its derivative are given below for t 2 [0; 1] and different values of
the displacement k! 0 k2(0;).
As expected, to get a uniform speed on SO(3), the line in GL(3; 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 SO(3) 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
0.75
0.5
0.25
0
1
Displacement
2
3
0
0.2
1
0.8
0.6
0.4
Time
1.5
1
0.5
0
1
Displacement
2
3
0
0.2
1
0.8
0.6
0.4
Time
Figure 3: (a) Function f (t); (b) The derivative d dt f (t).
The necessary conditions for the curves that minimize the square of the L 2 norm of the acceleration
are found by considering the first variation of the acceleration functional
Z b
L a = < r V V;r V V > dt; (17)
a
where V (t) = dR(t) , r is the affine connection compatible with a suitable Riemmanian metric, and
dt
R(t) 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 R(t) be a curve between two prescribed points on SO(3) with canonical metric
that has prescribed initial and final velocities. If ! is the vector from so(3) corresponding to
V = dR,
the curve minimizes the cost function L dt a derived from the canonical metric only if the
following equation hold:
where () (n) denotes the n th derivative of ().
! (3) + ! ! =0 (18)
The above equation can be integrated to obtain
! (2) + ! _! = constant (19)
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 SO(3) 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 GL(3; IR), while keeping the proper boundary conditions on
SO(3) with the corresponding velocities. Minimum acceleration curves are found in GL(3; IR) and
eventually projected back onto SO(3).
In what follows, the time interval will be t 2 [0; 1] and the boundary conditions R(0), R(1),
_R(0), _R(1) are assumed to be specified. The minimum acceleration curve in GL(3; IR) with a
constant metric is a cubic given by
R(t) =R 0 + R 1 t + R 2 t 2 + R 3 t 3 ;
where R 0 ;R 1 ;R 2 ;R 3 2 GL(3; IR) are
R 0 = R(0); R 1 = _ R(0);
R 2 = ,3R(0) + 3R(1) , 2 R(0) _ , R(1); _
R 3 =2R(0) , 2R(1) + R(0) _ + R(1): _
Now the curve on SO(3) is obtained by projecting R(t) onto SO(3).
2 All 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
1.4
1.2
True
Projection
True
Projection
1.6
1.4
1.2
1
1
2
0.8
0.8
1.5
0.6
0.6
1
1.4
0.4
1.2
0.5
0.4
0.2
1
0
0
0.2
1.5
0
0.8
0
0.8
0.2
0.6
−0.5
1
0.6
0.4
0.4
0.5
0.4
0.6
0.2 σ
0.2
2
σ 2
0 0
σ 0.8 0
1
σ 1
a) b) c)
σ 3
σ 3
σ 3
0
0.5
σ 1
1 0
True
Projection
0.5
σ 2
1
1.5
Figure 4: Minimum accleration curves on SO(3) with canonical metric: (a) Velocity boundary
conditions along the geodesic; (b) End velocity perturbed by e 1 ; and (c) End velocity perturbed by
5e 1 .
The following examples present comparisons between the minimum acceleration curves generated
using the projection method and the curves obtained directly on SO(3) by solving equations
(18) using the relaxation method. All the generated curves are drawn in exponential local coordinates.
In Figure 4 we used the following position boundary conditions
(0) =
h
0 0 0
i T
; (1) =
h
The initial velocity is the one corresponding to the geodesic passing through the two positions:
! 0 =
h
6
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 + e 1 in case (b) and ! 1 = ! 0 +5e 1 in case (c), where
e 1 = [100] 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
3
2
i T
6
3
2
i T

5.3 Geodesics on SO(3) with a general metric
The differential equations to be satisfied by the geodesics on SO(3) 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 GL(3; IR), then our method implies
the projection of the geodesic from GL(3; IR)
A(t) =R 0 +(R 1 , R 0 )t; t 2 [0; 1]
onto SO(3)
A(t)W = U V T ; R(t) =U (t)V T
An illustrative example is shown in Figure 5, where end positions in exponential coordinates are
the same as in the previous section:
(0) =
h
0 0 0
i T
; (1) =
h
6
3
2
i T
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 GL(n +1; IR); and (2) the projection of the trajectories
from GL(n +1; IR) to SE(n). 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 twofold.
First, it is possible to apply any of the variety of well-known, efficient techniques to generate
optimal curves on GL(n; IR) [7, 8]. Second, the method yields solutions for general choices of
Riemannian metrics on SE(3). For example, we can incorporate the dynamics of arbitrary rigid
24

1.6
1.6
1.6
1.4
1.2
True
Projection
1.4
1.2
True
Projection
1.4
1.2
True
Projection
1
1
1
σ 3
0.8
σ 3
0.8
σ 3
0.8
0.6
0.6
0.6
0.4
0.4
0.4
0.2
0.2
0.2
0
1.5
0
1.5
0
1.5
1
0.6
0.8
1
0.6
0.8
1
0.4
0.6
0.5
0.4
0.5
0.4
0.2
0.2
σ 2
0 0
σ
σ 2
0 0
1
σ 1
a) b) c)
σ 2
0.5
0
−0.2
0
σ 1
0.2
Figure 5: Geodesics for different metrics on SO(3): (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 SO(3), the computations in SE(3) are
a straightforward extension.
Appendix
Proof of Proposition 5.1 We need to find the SVD decomposition of A(t) =I +(R 2 , I)f (t) =
U (t)(t)V T ,whereR 2 = e^! 0
and f (t) is a continous function defined on [0; 1] satisfying f (0) = 0,
f (1) = 1. We start from
A T (t)A(t) =I , f (t)(1 , f (t))M; M =2I , R 2 , R T 2
The eigenstructure of the constant and symmetric matrix M will completely determine the SVD
of A(t). Because M is symmetric and real, its eigenvalues will be real and the corresponding
eigenspaces orthogonal. Let i ;v i be an eigenvalue-eigenvector pair of M. Then, we have:
Mv i = i v i
) A T (t)A(t) =(1, f (t)(1 , f (t)) i )v i
So, theoretically, the desired SVD decomposition is determined at this moment:
The matrix V (t) can be chosen constant of the form V = [v 1 v 2 v 3 ] where v 1 ;v 2 ;v 3 are
orthonormal eigenvectors of M;
25

The singular values are given by s 2 i (t) =1, f (t)(1 , f (t)) i (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) =[u 1 (t) u 2 (t) u 3 (t)]; u i (t) = A(t)v i
s i
; i =1; 2; 3
Using the Rodrigues formula for R 2 = e^! 0
, it is easy to see that
M = , 1 , cos k! 0k
k! 0 k 2 (^! 2 0 +^!2T 0 )
from which it follows that the eigenvalues of M are given by
(M )=f0; 2(1 , cos k! 0 k); 2(1 , cos k! 0 k)g
and a set of three orthonormal eigenvectors by
8
2 3
,! >< ! 0
k!
>:
0 k ; 1
3
q
6
0
!
4
7
5 ; 1
q
2
3 + !2 1
! 2 2
! !2 1 +(!2 3 + !2 1 )2 + ! 2 3 !2 2
1
2
6
4
3
,! 2 ! 1
! 2 3 + 7 !2 1
5
,! 3 ! 2
9
>=
>;
where ! 0 =[! 1 ! 2 ! 3 ] T . With the eigenstructure of M determined, we write
q
(t) =diagf1;s(t);s(t)g; s(t) = 2(1 , cos k! 0 k)f 2 (t) , 2(1 , cos k! 0 k)f (t) +1 (24)
where the binomial under the square root is always positive because it is positive at zero and 1 ,
cos k! 0 k2(0; 2) gives a negative discriminant. Some straightforward but rather tedious calculation
leads to:
U (t)V T = I + ^! 0
k! 0 k 2(t) +(1, 1 (t)) + ^!2 0
k! 0 k 2
where
1 (t) = 1 , f (t) +f (t) cos k! 0k
; f 2 (t) = (t) sin k! 0k
s(t)
s(t)
We will restrict our discussion to k! 0 k 2 (0;) (in accordance to the exponential coordinates)
which will give 2 (t) > 0. Note that 2 1 (t) +2 2
(t) = 1, so it is appropriate to define a function
(t) 2 (0; 1) so that
1 (t) = cos(k! 0 k(t)); 2 (t) = sin(k! 0 k(t))
26

By use of the Rodrigues formula again,
U (t)V T = e^! 0(t)
so the projected line is the exponential mapping of a segment between the origin and ! 0 in exponential
coordinates. The parameterization of the segment is given by
(t) = 1
k! 0 k atan2(1 , f (t) +f (t) cos k! 0k;f(t) sin k! 0 k)
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 c k 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.doCarmo.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 Mechanical
Design, 116:749–755, 1994.
[11] J. Hoschek and D. Lasser. Fundamentals of Computer Aided Geometric Design. AK Peters,
1993.
[12] B. Jü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. IMAJ.ofMath.
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 kinematics
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. Žefran and V. Kumar. Interpolation schemes for rigid body motions. Computer-Aided
Design, 30(3), 1998.
[20] M. Ž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. Ž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