Interpolation schemes for rigid body motions - CiteSeerX
Interpolation schemes for rigid body motions Milos Z efran and Vijay Kumar 1
2
California Institute of Technology, Pasadena, CA, USA 2 General Robotics and Active Sensory Perception (GRASP) Laboratory University of Pennsylvania, Philadelphia, PA, USA 1
Abstract This paper investigates methods for computing a smooth motion that interpolates a given set of positions and orientations. The position and orientation of a rigid body can be described with an element of the group of spatial rigid body displacements, SE (3). To nd a smooth motion that interpolates a given set of positions and orientations is therefore the same as nding an interpolating curve between the corresponding elements of SE (3). To make the interpolation on SE (3) independent of the representation of the group, we use the coordinate-free framework of di erential geometry. It is necessary to choose inertial and body- xed reference frames to describe the position and orientation of the rigid body. We rst show that trajectories that are independent of the choice of these frames can be obtained by using the exponential map on SE (3). However, these trajectories may exhibit rapid changes in the velocity or higher derivatives. The second contribution of the paper is a method for nding the maximally smooth interpolating curve. By adapting the techniques of the calculus of variations to SE (3), necessary conditions are derived for motions that are equivalent to cubic splines in the Euclidean space. These necessary conditions result in a boundary value problem with interior-point constraints. A simple and ecient numerical method for nding a solution is then described. Finally, we discuss the dependence of the computed trajectories on the metric on SE (3) and show that independence of the trajectories from the choice of the reference frames can be achieved by using a suitable metric.
Keywords: motion interpolation, Euclidean group, Riemannian geometry
1 Introduction The problem of nding a smooth motion that interpolates a given set of positions and orientations is frequently encountered in robotics (planning of trajectories in welding and painting), computer graphics (animation, interpolation of motion through a set of key frames) and computer-aided geometric design (interactive interpolation schemes for design), and has recently received considerable attention. The problem is well understood in Euclidean spaces [6, 9], but it is not clear how these techniques can be generalized to curved spaces. There are several 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. A method for computing points on the curve (analytical expressions, numerical methods, or such geometric construction techniques as the de Casteljau algorithm) and the algebraic properties of the trajectories (rational versus nonrational expressions) are also important. Finally, the interpolation properties of the trajectories (interpolation versus approximation) and their smoothness properties need to be considered. Shoemake [21] 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 [8] and Park and Ravani [18] to spatial motions. The focus in these articles is how to generalize the notion of interpolation from the Euclidean space to a curved space. Shoemake [21] uses interpolation with one-parameter subgroups. The fact that on SO(3), the group of rigid body rotations in IR3 , one-parameter subgroups are at the same time projections of screw motions from SE (3) as well as the geodesics for the natural (bi-invariant) metric, led to two di erent extensions of this work: Ge and Ravani [8] formulated a de Casteljau algorithm on the group of rigid-body motions SE (3) using the screw motions for interpolation, while Park and Ravani [18] proposed a de Casteljau algorithm for Riemannian manifolds where the interpolation was performed with geodesics. On SO(3), the latter scheme produces the same curves as the original Shoemake's method. None of these schemes produces rational motions. Another class of methods is based on the representation of Bezier curves with Bernstein polynomials. Ge and Ravani [7] used the dual unit quaternion representation of SE (3) and subsequently applied Euclidean methods to interpolate in this space. Jutler [10] formulated a more general version of the polynomial interpolation by using dual (instead of dual unit) quaternions to represent SE (3). In such representation, an element of SE (3) corresponds to a whole equivalence class of dual quaternions. Jutler investigated di erent methods for lifting a curve 2
on SE (3) to a curve in the space of dual quaternions and identi ed schemes that have certain invariance properties with respect to the choice of reference frames. Kim et al. [12] generalized di erent polynomial interpolation schemes from Euclidean space to SO(3) by \mapping" the expressions from the Euclidean space through the exponential map. Park and Kang [17] derived a rational interpolating scheme for the group of rotations SO(3) by representing the group with Cayley parameters and using Euclidean methods in this parameter space. The advantage of the methods described in this paragraph (the exception is [12]) that use Euclidean methods to interpolate in a parameter space is that they produce rational curves. Although all these works (with the exception of [18]) use a particular coordinate representation of either the group of rotations SO(3) or the Euclidean group SE (3) it would be wrong to conclude that they produce motions that depend on the chosen representation. More specifically, the schemes in [21], [8], [18], and [12] are all based on geometric algorithms and do not depend on the particular choice of representation. However, except for [18], this is not obvious and seems not to have been achieved deliberately. Further, the schemes in [8], [18], and [12] can not be directly used for interpolation (they assume that the Bezier or B-spline control polygon is given) and it is not clear that interpolation can be easily achieved. On the other hand, [21], [7], [10], and [17] generate interpolating curves, but [21] and [17] are limited to SO(3) while [7] and [10] base their method on a particular representation of SE (3). The works discussed above focus on the approximation or interpolation capability of Bezier (or B-spline) curves. However, an additional property of cubic splines in the Euclidean space is that they minimize the integral of the norm of the acceleration along the curve. This has motivated several researchers to generalize splines to the curved spaces as curves minimizing an appropriate cost functional. Noakes et al. [15] derived the necessary conditions for cubic splines on general manifolds. These results are extended in [4] to the dynamic interpolation problem. Necessary conditions for higher-order splines are derived in Camarinha et al. [2]. All these results are derived in a general setting and are only specialized to compact Lie groups. Since the group SE (3) is not compact, there are several additional issues that need to be addressed before these methods can be applied. Of particular importance is the choice of metric given that there is no natural metric for SE (3) [3, 13]. In [23] and [24], the variational approach was applied speci cally to interpolation of smooth motions on SE (3). Several metrics suitable for trajectory planning were identi ed and a computational method was described for computing maximally smooth trajectories. 3
In this paper, we build on the results from [23, 24] to investigate various motion interpolation schemes on SE (3). We will 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. Our goal is to develop ecient methods for nding trajectories that minimize meaningful performance functions. However, we will show that there is a trade-o between the computational eciency and optimality of the generated trajectories. We rst introduce some basic tools from di erential geometry that are used in the rest of the paper. We then describe a simple scheme for nding an interpolating motion that does not depend on the choice of the inertial and body- xed reference frames. The scheme is based on the properties of the exponential map on SE (3). Next, we outline a method for computing minimum acceleration curves on SE (3), which are a generalization of cubic splines. This scheme follows the approach in [15], [4], and [2], but focuses on the problems speci c to SE (3) and addresses the computational issues. We therefore base our work on the results from [23]. In particular, we show that curves with desired invariance properties can be computed by properly selecting a metric on SE (3). Finally, we compute the curves for two choices of the metric and compare them with the trajectories obtained by the scheme based on the exponential map.
2 Mathematical preliminaries 2.1 The Lie group SE (3) Consider a rigid body moving in free space. Assume any inertial reference frame fFg xed in space and a frame fMg xed to the body at point O0 as shown in Figure 1. At each instance, the con guration (position and orientation) of the rigid body can be described by a homogeneous transformation matrix corresponding to the displacement from frame fFg to frame fMg. The set of all such matrices is a group for the standard matrix multiplications and is called SE (3), the special Euclidean group of rigid body transformations in three-dimensions:
SE (3) =
82 > < 6 4 > :
3
R d7 0 1
5
jR2
3 3
IR
;d 2
3
IR
9 > =
; RT R = I; det(R) = 1> : ;
(1)
It is well known that SE (3) is a Lie group [11, 14]. On a Lie group, the tangent space at the group identity can be endowed with the so-called Lie bracket operation that makes it a Lie algebra. The Lie algebra of SE (3), denoted by se(3), 4
FIGURE COMES HERE.
Figure 1: The inertial ( xed) frame, fFg, and the moving frame, fMg, attached to the rigid body. is given by:
82 > < 6 4 > :
9 > =
3
v7 33 ; v 2 IR3; T = ? > ; 5 j 2 IR ; 0 0 and the Lie bracket of two elements S1; S2 2 se(3) corresponds to their commutator:
se(3) =
(2)
[S1; S2] = S1 S2 ? S2S1 : A 3 3 skew-symmetric matrix can be uniquely identi ed 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 element S 2 se(3) can be thus identi ed with a vector pair f!; v g. This identi cation will be denoted by: vec(S ) = f!; v g (3) If vec(S1 ) = f!1 ; v1g and vec(S2) = f!2; v2g, it is easy to verify that: vec([S1; S2]) = f!1 !2 ; !1 v2 + v1 !2 g:
(4)
In the rest of the paper, the vector pair f!; v g will be interchangeably treated as a pair of two vectors or as a 6 1 column vector. The exact form will be clear from the context. Given a curve A(t) : [?a; a] ! SE (3), 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: 2
3
T _ T_ 6 R R R d 7
S (t) = A?1 (t)A_ (t) = 4
0
0
5
:
(5)
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 5
body while v is the linear velocity of the origin O0 of the frame fMg, both expressed in the frame fMg. In kinematics, elements of this form are called twists [14] and se(3) is thus isomorphic to the space of twists. It is easy to check that the twist S (t) computed from Eq. (5) does not depend on the choice of the inertial frame fFg. For this reason, S (t) is called the left invariant representation of the tangent vector A_ . Alternatively, the tangent vector A_ could be identi ed with a right invariant twist (invariant with respect to the choice of the body- xed frame fMg). Since se(3) is a vector space, any element can be expressed as a 6 1 vector of components corresponding to a chosen basis. The standard basis for se(3) is: 2
L1 =
6 6 6 6 6 6 6 4 2
L4 =
6 6 6 6 6 6 6 4
0 0 0 0 0 0 0 0
0 0 0 ?1 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0
3 7 7 7 7 7 7 7 5 3 7 7 7 7 7 7 7 5
2
0 6 6 6 0 L2 = 666 6?1 4 0 2 0 6 6 6 0 L5 = 666 6 0 4 0
0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0
3
2
7 7 7 7 7 7 7 5
6 6 6 6 6 6 6 4
L3 =
3
2
7 7 7 7 7 7 7 5
6 6 6 6 6 6 6 4
L6 =
0 ?1 1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0
3 7 7 7 7 7 7 7 5 3
(6)
7 7 7 7 7 7 7 5
If interpreted as twists, L1 , L2 and L3 represent instantaneous rotations about and L4, L5 and L6 instantaneous translations along the Cartesian axes x, y and z of the body- xed frame fMg, respectively. The components of a twist S 2 se(3) in this basis are given precisely by the velocity vector pair, f!; v g.
2.2 Left invariant vector elds A (di erentiable) vector eld on a manifold is a (smooth) assignment of a tangent vector to each element of the manifold. At each point, a vector eld de nes a unique integral curve to which it is tangent [20]. Formally, a vector eld X is a (derivation) operator which, given a di erentiable function f , returns its derivative (another function) along the integral curves of X . In other words, if (t) is a curve tangent to a vector eld X at point p = (t0), then: df (
( t )) Xf jp = dt : t0
(7)
On SE (3), which is a matrix Lie group, a tangent vector can be computed using vector calculus since the group is embedded into the Euclidean space IR44 . In other words, the tangent vector 6
X to a curve A(t) at point A(t0) is given by: daji X jA(t0) = dA = dt t0 dt "
#
t0
(8)
where aji are the entries of the matrix A. However, the matrix representation is only a computational tool; formally, the tangent vector must be understood as an operator in the sense of Eq. (7). Using the matrix representation of tangent vectors, an example of a (di erentiable) vector eld, X , is obtained by setting at each point A:
X (A) = Tb(A) = AT;
(9)
where T belongs to the Lie algebra of the group. Such a vector eld is called a left invariant vector eld. We use the notation Tb to indicate that the vector eld is obtained by left translating the Lie algebra element T . The set of all left invariant vector elds is a vector space and by construction it is isomorphic to the Lie algebra. Right invariant vector elds can be de ned in analogous way. Note that a vector eld need not be left or right invariant. Since L1 ; L2; : : :; L6 are a basis for the Lie algebra se(3), the left invariant vector elds Lb 1; Lb 2; : : :; Lb 6 form a basis for the space of the left invariant vector elds. Finally, because at any point A 2 SE (3) the vectors Lb 1 (A); : : :; Lb 6 (A) form a basis of the tangent space at that point, any vector eld X can be expressed as
X=
6 X
i=1
X iLb i;
(10)
where the coecients X i vary over the manifold { if they are constant then X is left invariant. This implies that we can associate a vector pair f!; v g (that in general varies over the manifold) de ned by: ! = [X 1; X 2; X 3]T ; v = [X 4; X 5; X 6]T : to an arbitrary vector eld X .
2.3 Exponential map and local coordinates A motion of the rigid body in IR3 is described by a curve, A(t), on SE (3). If V = dA dt is the vector eld tangent to A(t), the vector pair f!; v g associated with V corresponds to the instantaneous twist (screw axis) for the motion. In general, the twist f!; v g changes with time. Motions for 7
which the instantaneous twist f!; v g remains constant are called screw motions in the kinematics literature [14]. Let the twist S 2 se(3) be represented by a vector pair f!; v g and let A(t) be a screw motion with the screw axis f!; v g such that A(0) = I . We de ne the exponential map exp : se(3) ! SE (3) by: exp[tS ] = A(t); (11) 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, pp. 413]. For t = 1, the function exp[] is a surjective map of se(3) onto SE (3). However, for a small enough neighborhood of zero in se(3) it is a di eomorphism onto a neighborhood of the identity element in SE (3). We can thus de ne local coordinates, i , for A 2 SE (3) suciently close to the identity by: i
h
A() = exp 1L1 + 2L2 + 3L3 + 4L4 + 5L5 + 6L6 :
(12)
These coordinates are valid provided Trace [A] 6= 0 (a point that represents a rotation through ). If such a point is encountered, we can rede ne the coordinate chart by constructing a similar representation in the neighborhood of a di erent element of SE (3) using the operation of left translation. The geometric interpretation of this particular parameterization of SE (3) in terms of screw coordinates can be found in [14].
2.4 Riemannian metrics on Lie groups If a smoothly varying, positive de nite, bilinear, symmetric form is de ned on the tangent space at each point on the manifold, we say the manifold is Riemannian. The bilinear form is an inner product on the tangent space at each point and is called a Riemannian metric. If the bilinear form is inde nite but non degenerate, it is called a pseudo-Riemannian metric. On a Lie group, and thus on SE (3), an inner product on the tangent space at the identity can be extended to a Riemannian metric (everywhere on the manifold) using the idea of left translation. Assume that the inner product of two elements T1; T2 2 se(3) is de ned by
< T1; T2 >I = tT1 Wt2;
(13)
where t1 and t2 are the coordinates of T1 and T2 with respect to some basis and W is a positive de nite matrix. If V1 and V2 are tangent vectors at an arbitrary group element A 2 SE (3), the 8
inner product < V1; V2 >A in the tangent space TA SE (3) can be de ned by:
< V1; V2 >A =< A?1V1; A?1V2 >I :
(14)
The metric obtained in such a way is called a left invariant metric [20]. Physically, left invariance corresponds to independence on the choice of the inertial frame [14].
2.5 Ane connection and covariant derivative The motion of a rigid body is represented by a curve, A(t), on SE (3). The velocity at an arbitrary point is the tangent vector to the curve at that point. In order to obtain the acceleration, or to engage in a dynamic analysis, we need to be able to di erentiate a vector eld along the curve. At each point A 2 SE (3), the value of a vector eld belongs to the tangent space TA SE (3) and to di erentiate a vector eld along a curve, we must be able to compare vectors from tangent spaces at di erent points on the curve. But as opposed to IRn , where vectors can be translated to a common origin, on curved spaces no canonical way of translating vectors exists and must be explicitly de ned. To di erentiate vector elds along a curve it suces to de ne how to transport a vector along the curve from one tangent space to another. This process of translation of vectors along a curve is called parallel transport [20]. Parallel transport is clearly not unique. However, given a parallel transport, we can de ne derivative of a vector eld along a curve. Let X be a vector eld de ned along A(t), and let X (t) stand for X (A(t)). Denote by X t0 (t) the parallel transport of the vector X (t) to the point A(t0). The covariant derivative of X along A(t) is: DX = lim X t0 (t) ? X (t0) : (15)
dt
t0
t
t!t0
By taking covariant derivatives along integral curves of a vector eld Y , we obtain a covariant derivative of the vector eld X with respect to the vector eld Y . We can therefore de ne an operator r, called ane connection:
DX rY X jA0 = dt ; (16) t0 where DX dt is taken along the integral curve of Y passing through A0 at time t0 . In order to compute the covariant derivative rY X , it suces to know the covariant derivatives of the basis P P vector elds. For two arbitrary vector elds X = 6i=1 X i Lb i and Y = 6i=1 Y i Lb i , the expression for rY X is then given by: i i j b b (17) rY X = dX dt Li + X Y rLb Li j
9
where dtd is the derivative along the integral curve of Y . Given a manifold with a Riemannian or pseudo-Riemannian metric, there exists a unique connection [20] which is compatible with the metric:
and symmetric:
X =< rX Y; Z > +
(18)
rX Y ? rY X = [X; Y ]:
(19)
This connection is called the Levi-Civita connection. The velocity, V (t), of the rigid body moving along the curve A(t) is given by the tangent vector eld. Given a connection, we can de ne the acceleration and higher derivatives of the velocity. The acceleration, A(t), is the covariant derivative of the velocity along the curve: dA D A= = r V: (20)
dt dt
V
Higher derivatives of the velocity can be de ned similarly. Note that the acceleration and the other higher derivatives depend on the choice of the connection.
2.6 Curvature tensor The curvature of a Riemannian manifold is a correspondence R that associates to a pair of vector elds X and Y a mapping:
R(X; Y ) : Z 7! rY rX Z ? rX rY Z + r[X;Y ]Z
(21)
where Z is a vector eld and r is the Riemannian connection on . The curvature tensor is a multi-linear mapping which maps a quadruple of vectors (X; Y; U; V ) into a real number. The value of the curvature tensor on the quadruple (X; Y; U; V ) is given by < R(X; Y )U; V >. If Xi is a basis, the components of the curvature tensor are given by:
Rijkl =< R(Xi; Xj )Xk; Xl >
(22)
3 Interpolation on SE (3) The basic interpolation problem on SE (3) can be posed in the following way:
Problem 3.1 Given a sequence fAigni of elements of SE (3) and a sequence of times fti gni , =1
where t1 t2 : : : tn , nd a curve (t) such that (ti ) = Ai .
10
=1
This basic problem can be re ned for speci c applications. We study the case when the velocities of the rigid body at times t1 and tn are prescribed. Further, we wish to impose certain smoothness requirements on the motion. In particular, we concentrate on motions that are continuous at the level of acceleration (C 2). Most of the results in the paper can be generalized in a straight forward way if higher level of continuity is required. We therefore focus on the following problem:
Problem 3.2 Given: 1. a sequence fAigni=1 of elements of SE (3); 2. a sequence of times fti gni=1 , where t1 t2 : : : tn ; 3. the instantaneous twists V1 and V2 (V1; V2 2 se(3)) at times t1 and tn , respectively; nd a curve (t) 2 C 2[t1 ; tn ] such that (ti ) = Ai .
The ane nature of the space (in which the rigid body moves) and practical considerations suggest that the interpolating curve should have some additional properties: (1) The trajectories must be independent of the representation of the motion in space (parameterization of SE (3)). In this way, computations performed with di erent choices of coordinates for SE (3) will produce consistent results. (2) The trajectories should be independent of the choice of the inertial reference frame fFg and the body xed frame fMg (Figure 1). That is, it should not matter where we put the inertial frame and which frame we choose on the rigid body. The resulting trajectory of the rigid body should be the same. (3) The trajectories must have good performance for the intended use. In this paper, we formulate a cost functional that re ects the smoothness of the trajectory and show how to compute a trajectory that minimizes this cost functional. We will present two di erent solutions to Problem 3.2. The rst method is similar to those in [7], [10], and [17] in that it is based on a particular representation of SE (3) and interpolation in the appropriate parameter space using Euclidean methods. However, we choose a representation that allows easy geometric analysis of the computed trajectories and base our method on this geometric analysis, thereby guaranteeing its independence of the chosen representation of SE (3). 11
We show that the method satis es (1) and (2) from the above, but it is dicult to quantify the performance of the solution. In the second part of this section we therefore present a method that computes trajectories that optimize a chosen measure of smoothness. As before, by using di erential geometry and basing the method on geometric analysis, we achieve its independence of the parameterization of the space. The method therefore satis es (1) and (3). We show that (2) can be achieved by appropriate choice of Riemannian (or pseudo-Riemannian) metric on SE (3). The drawback of this approach is that it is computationally expensive. This section therefore demonstrates that there is a trade-o between the computational eciency and the optimality of the generated trajectories.
3.1 Interpolation based on the exponential map We sketch a method that produces trajectories that do not depend on the parameterization of SE (3), are independent on the choice of the inertial and body- xed frame and have the required smoothness properties. However, the trajectories do not optimize a geometrically meaningful cost functional1 . The main idea is to use properties of the exponential map to achieve the invariance of the trajectories with respect to the choice of the body- xed and inertial reference frames. Let fAi gni=1 and fti gni=1 be the sequences in Problem 3.2. We will look for an interpolating curve of the form:
(t) = A1 (t): (23) In this way, the interpolation problem becomes to nd a curve (t) that interpolates the sequence fA?1 1Aigni=1. Next, let the position A?1 1 Ai be represented with a 6 1 vector i according to Eq. (12). Using an interpolation method in the Euclidean space IR6, it is easy to nd a curve (t) such that: (ti) = i: (24) Obviously, the curve:
(t) = A1 exp[(t)] ;
(25)
where vec() = , will pass through the point Ai at the time ti and it is a suitable candidate for the interpolation. If we also wish to satisfy the boundary conditions on the velocities at t1 and tn , They can be shown to minimize a cost functional formulated in the parameter space, but this cost functional does not have any geometric meaning. 1
12
this poses additional requirements for the curve (t). If vec(S ) denotes the vector corresponding to an element S 2 se(3), the time derivative of the exponential map can be written in the form: vec (t)?1 d (t) = vec exp[?(t)] d exp[(t)] = ( (t))_(t):
dt
dt
(26)
The 6 6 matrix can be computed from the formula for the exponential map. The explicit expressions for the derivative of the exponential map on SE (3) can be found in [1]. The instantaneous twist, V , representing the velocity of the rigid body, is given by Eq. (5):
V = (t)?1 dtd (t):
Hence, the expression for the velocity, expressed in the local coordinates, is: _ = ()?1vec(V ) :
(27) (28)
Let V1 and V2 denote the instantaneous twists at t1 and tn , respectively, and _1 and _n the vectors computed from Eq. (28). To satisfy the boundary conditions on the velocities, the curve (t) must therefore satisfy the following equations: (29) _(t) = (n)?1vec(Vn) _(t) = (1)?1vec(V1) tn
t1
By computing a curve (t) (in the Euclidean space) that satis es the conditions (24) and (29), we therefore obtain a curve that solves the interpolation problem. We now investigate properties of the trajectories generated in this way.
Smoothness Since the exponential map is smooth, the interpolating curve (t) will be as smooth as the curve (t). We can therefore control the smoothness of the interpolating curve directly by controlling the smoothness of the curve in the Euclidean space IR6 .
Left invariance The curve of the form (23) will always be left invariant, that is, invariant with
respect to the choice of the inertial reference frame fFg. To see this, let us displace fFg so that its new position with respect to its initial position is given by the homogeneous transformation matrix C . Now the set of points that have to be interpolated becomes fCAigni=1 . But given the form (23), this means that the curve in Eq. (23) has to interpolate between the points (CA1)?1 CAi = A?1 1 Ai , which are the same as before. In addition, the velocity of the rigid body in the new frame is (CA)?1C A_ = A?1 A_ , again the same as before. We conclude that if in the old frame the curve was (t) = A1 exp[(t)], the curve in the new frame will simply be
e(t) = CA1 exp[(t)] = C (t), which proves left invariance. A similar proof was presented in [17]. 13
Right invariance Consider again Problem 3.2 and the interpolating curve (t) = A exp[(t)]. 1
Now displace the body- xed frame fMg through a displacement C . The interpolation points become fAi C gni=1 , and the instantaneous twist in the new frame, denoted by V~ is related to the twist in the original frame by the adjoint map [5, 11]:
V~ = AdC?1 V = C ?1V C:
(30)
Consider a new coordinate curve ~(t) = vec((t)), given by: ~ (t) = C ?1(t)C:
(31)
It is a property of the exponential map [5] that for any element S 2 se(3): h i exp C ?1 SC = C ?1 exp[S ] C:
Now take the curve:
(32)
i
h
e t) ;
e(t) = Ae1 exp (
(33)
where Ae1 = A1C . A short derivation shows: h
i
h
i
e t) = A C exp C ?1 (t)C =
e(t) = Ae1 exp ( 1 A1C (C ?1 exp[(t)] C ) = A1 exp[(t)] C = (t)C:
(34)
Since (t) interpolates the points fAi gni=1 , the curve e (t) will interpolate the points fAi C gni=1 and it will satisfy the new boundary conditions Vei = C ?1 Vi C , i = 1; 2. In order to show that the interpolation scheme is right invariant, we need to show that the curve e(t) is exactly the curve that would be produced by running the interpolation scheme in the transformed coordinate system. In other words, we need to show that the following diagram commutes: Interpolation Ξ(t), γ(t)
{Ai} n1, , V1, V2
Ξ −> C−1 Ξ C
Change of body−fixed frame
{Ai C } n1, , C−1 V1 C, C−1 V2 C
γ −> γ C
Interpolation
(35)
~ Ξ (t), ~γ (t)
In general this will not be true and we need to know how the interpolation between the coordinates i is performed. Obviously the interpolation scheme will be right invariant if and only if 14
the set of curves (t) = vec((t)) that are generated by the scheme is invariant with respect to the transformation: AdC ?1 : (t) 7! C ?1 (t)C; (36) for an arbitrary C 2 SE (3). Examples of families of curves that have this property and will therefore lead to a right invariant interpolation scheme are cubic splines and polynomials of a xed degree which is greater or equal to n ? 1. The transformation in (36) is linear and therefore maps a cubic polynomial to a cubic polynomial. It is also not dicult to see that a cubic spline that solves the interpolation problem 3.2 formulated in the space of the coordinates is unique. This implies that the diagram in (35) must commute. A similar argument holds for the interpolation with polynomials of degree n ? 1: there is a unique polynomial of degree n ? 1 that will interpolate between n points. The diagram (35) will also commute for the polynomials of higher degree, although in this case the interpolating curve is not unique.
Independence of the parameterization At the rst sight, it seems that the proposed
interpolation scheme is based on the particular choice of coordinates in Eq. (12). However, while this choice of coordinates is convenient for the analysis, the actual interpolation does not necessarily have to be based on this representation. More precisely, the above analysis shows that we only need to establish the invariance of the family of the curves used for interpolation with respect to the map de ned in (36).
Remark 3.3 The analysis in this section is not limited to SE (3) and is applicable to any matrix Lie group.
3.2 Maximally smooth interpolating curves The interpolation scheme presented in the previous section yields smooth trajectories and is bi-invariant if the family of the interpolating curves in the parameter space is properly chosen. However, to prevent fast changes in the velocities or their higher derivatives, it is often desired to impose stricter smoothness requirements. More precisely, we can formulate a cost functional measuring the smoothness of a curve (the choice of the cost functional in general depends on the task) and nd a curve that minimizes this cost functional. Intuitively, the smoothness of a curve is related to the magnitude of its derivatives. This implies that we need to be able to compute velocity and higher derivatives of a curve and that we need a notion of magnitude of these derivatives. The group SE (3) is not a Euclidean space and it is thus necessary to 15
use the formalism of Riemannian geometry to de ne these quantities. We can subsequently de ne a cost functional that measures the smoothness of a curve. To compute the trajectory that minimizes the chosen cost functional we then employ methods of variational calculus on Riemannian manifolds.
Variational calculus on manifolds It is well known [6] that in a Euclidean space cubic splines are the minima of the functional:
J=
Z
b
a
k k dt: 2
(37)
Noakes et al. [15] used this property to de ne cubic splines on an arbitrary manifold as curves minimizing the properly de ned norm of the acceleration. In Section 2.5 it was shown that the acceleration of a curve (t) with the velocity vector V (t) is given by rV V . Cubic splines are therefore the curves minimizing:
La =
Z
a
b
< rV V; rV V > dt:
(38)
In the function under the integral, < :; : > stands for the Riemannian metric while r is the corresponding Riemannian connection. In [15], necessary conditions are derived for such curves. Results for higher-order splines are presented in [2], while a more thorough investigation of this framework on SE (3) is presented in [23]. The result in [15] can be summarized with the following theorem:
Theorem 3.4 (Noakes et al.) Let A(t) be a curve on a Riemannian manifold that satis es
the boundary conditions (that is, it starts and ends at the prescribed points with the prescribed velocities) and let V = dA dt . If A(t) minimizes the functional La , then:
rV rV rV V + R(V; rV V )V = 0:
(39)
In Eq. (39), R(:; :) stands for the curvature.
Choice of metric on SE (3) There is no natural choice of metric for SE (3) [3, 13]. In this
paper we consider two choices. The rst is the left invariant metric that was proposed by Park and Brockett [16]. The metric is de ned through Eq. (14), where the matrix W is given by: 2
W = 64
I 0 0 I 16
3 7 5
(40)
and and are arbitrary constants. An attractive property of this metric is that the acceleration computed from Eq. (20) is exactly the acceleration as we know from kinematics [24]. In addition, this metric is a product metric [22], which means that the rotational and the translational component of motion can be treated separately. Finally, this is also the metric induced by the kinetic energy of a spherical mass, a generalization of the concept of a point mass. In [23] we show that the Riemannian connection corresponding to metric (40) is given by:
rY X = f d!dtx + 21 !y !x; dvdtx + !y vxg;
(41)
where dtd is the derivative along the integral curve of Y and f!x ; vx g and f!y ; vy g are the coordinates of the vector elds X and Y with respect to the left invariant basis (6). Note that the connection does not depend on the constants and . Direct calculation shows that the curvature is given by the following expression:
R(X; Y )Z = f 14 (!x !y ) !z ; 0g:
(42)
Another interesting metric is the Klein form [11]. It is also de ned through Eq. (14), where the matrix W is given by: 2 3 0 I W = 64 33 33 75 : (43) I33 033 It is not dicult to see that the Klein form is a pseudo-Riemannian metric. The unique symmetric connection that is compatible with this metric is given by [24]:
rL Lj = 21 [Li; Lj ]: bi
b
b
b
(44)
This leads to the following expression for the covariant derivative of a vector eld X along a vector eld Y : (45) rY X = f d!dtx + 12 !y !x; dvdtx + 12 (!y vx + vy !x)g: As before, dtd is the derivative along the integral curve of Y and f!x ; vxg and f!y ; vy g are the coordinates of the vector elds X and Y with respect to the left invariant basis (6). Direct calculation shows that the expression for the curvature for the pseudo-Riemannian metric (43) is given by: (46) R(Lb i; Lb j )Lb k = 41 [[Lb i; Lb j ]; Lb k ]: 17
The pseudo-Riemannian metric (43) has the important property that it is bi-invariant [24]2. This means that the curves computed from (39) using this metric will be left and right invariant. Another interesting property of the metric is that the geodesics (shortest distance curve) are screw motions [24].
Necessary conditions Having the expressions for the connection and the curvature, it is easy
to explicitly write the di erential equations that result from the Theorem 3.4. We obtain the following two corollaries:
Corollary 3.5 Let
2
A(t) = 64
3
R(t) d(t) 7 0
1
5
be a curve between two prescribed points on SE (3) that has prescribed initial and nal velocities. If f!; v g is the vector pair corresponding to V = dA dt , the curve minimizes the cost function La derived from the metric (40) only if the following equations hold:
!(3) + ! ! = 0
(47)
d = 0; (4)
where ()(n) denotes the nth derivative of ().
Corollary 3.6 Let
2
3
6 R(t) d(t) 7
A(t) = 4
0
1
5
be a curve between two prescribed points on SE (3) that has prescribed initial and nal velocities. If f!; v g is the vector pair corresponding to V = dA dt , the curve minimizes the cost function La derived from the metric (43) only if the following equations hold:
d3 V + [V; d2 V ] = 0: dt3 dt2
(48)
where ddt3 3V stands for ddt3 V3 Lb i and [V; ddt2 V2 ] denotes the Lie bracket (Eq. 4) of the twists with components V i and ddt2 V2 , respectively. i
i
In [24], it is shown that any bi-invariant metric on SE (3) is generated from a linear combination of the Klein form and the Killing form. Since the Killing form is degenerate, the metric (43) is therefore the unique (modulo the degenerate part) bi-invariant metric for SE (3). 2
18
Finding a solution In general, the rotational components of the cubic splines for the metric
(40) and both, the rotational and the translational components of the cubic splines for the metric (43) cannot be computed analytically. However, in the special case when the initial velocities are collinear with the initial velocity of the geodesic between the two endpoints, and the nal velocities are collinear with the nal velocity of the geodesic, it is possible to obtain a solution for these trajectories in terms of the geodesic curve [23]. If the geodesic curve can be computed analytically, so can the cubic splines.
The nite-di erence method For a general case, Theorem 3.4 yields a boundary value
problem that has to be solved numerically. The nite-di erence method proves to be ecient for solving such boundary-value problems. The basic idea of the method is to discretize the di erential equations in time: at each point, the system of di erential equations is converted into a system of algebraic equations by approximating derivatives with nite di erences. In this way, a system of coupled algebraic equations is obtained. The number of equations is usually fairly large. For detailed description of the method we refer the reader to [19] and the references therein. To apply the nite di erence method to Eq. (39), this higher order di erential equation must be converted into a system of rst-order di erential equations. To do this, we have to de ne a set of local coordinates and compute the solution in the coordinates. One possible choice are the coordinates in Eq. (12). We de ne: T
h
i
x = xT1 xT2 xT3 xT4 ; where
x1 x2 x3 x4
=
h
= !T vT
T
i
= x_ 2 = x_ 3 :
(49)
With these de nitions and the relation given by Eq. (26), we can obtain the system of equations in the desired form. Because of the requirement that the trajectory must pass through the intermediate points, we will have additional constraints of the form:
x1(ti) = i: 19
(50)
The constraints can be simply handled with the nite-di erence method since all we have to do at such points is replace Eq. (39) that gives the expression for x_ 4 , with Eq. (50), no additional computation is necessary. The boundary conditions at t1 and tn are also easily handled. They are given by: x1(t1) = 1 x2(t1) = vec(V1) (51) x1(tn) = n x2(tn) = vec(V2)
Remark 3.7 Since we based the numerical method on computing the trajectories on which the
rst variation of the cost functional vanishes ( rst order optimality conditions), it is necessary to use additional tests to establish that the computed trajectory is a minimum. While this can be done relatively easily, it is in general much more dicult to prove that the obtained minimum is a global minimum. For the problem at hand, we can conclude this with some certainty by analyzing the cases for which the global minimum can be analytically obtained and extrapolating the results to the case when the solution is computed numerically.
4 Examples In this section we compare the interpolating trajectories computed with three di erent methods:
The bi-invariant trajectories based on the exponential map and Euclidean interpolation in the space of coordinates with the coordinate chart de ned in Eq. (12).
The left-invariant trajectories obtained through minimization of the norm of the acceleration de ned through the left-invariant metric (40).
The bi-invariant trajectories obtained through minimization of the \norm" of the acceleration de ned through the bi-invariant metric3 (43).
In Figure 2, the trajectories of motion for a rectangle in the plane z = 0 are computed. The frame fMg is attached at the center of mass of the rectangle with the axes parallel to the sides of the rectangle. Let P = f; x; y g denote the orientation and position of the frame fMg in the plane and V = f!z ; vx; vy g the corresponding velocity. The interpolation problem was given by:
P1 = f0; 2; 8g P2 = f?1; 12; 4g P3 = f 3 ; 23; 10g 3
t1 = 0 t2 = 12 t3 = 1
V1 = f?1; 3; 10g V2 = f2; 2; 5g
Since the metric is not positive-de nite, the norm can be negative. See [22] for a discussion.
20
FIGURE COMES HERE.
Figure 2: (a) Interpolation with geodesic segments connecting speci ed positions; (b) maximally smooth interpolating trajectory computed from the metric (40); (c) spline interpolation in the space of coordinates; (d) maximally smooth interpolation trajectory computed from the metric (43). In the gure, the three prescribed positions are represented by a shaded rectangle. Figure 2.a shows a trajectory composed of geodesics (with the metric (40)) between the consecutive positions. Obviously, this trajectory is not smooth at the intermediate point. Further, it does not satisfy the boundary conditions on the velocity. Figure 2.b shows the maximally smooth trajectory computed from the necessary conditions (47). The translational part of the motion in this case is given by the cubic spline in IR2, as shown in Eq. (47). Since the metric used to nd this motion is left invariant but not bi-invariant, the motion is independent of the choice of the inertial reference frame fFg, but it does depend on the choice of the body- xed reference frame fMg. The next gure, 2.c, shows the trajectory computed by interpolation in the coordinates and with the use of exponential map. As shown in Section 3.1, this trajectory is bi-invariant. Finally, the trajectory shown in Figure 2.d is the maximally smooth trajectory for the metric (43) and was computed from Eq. (48). Since the metric is bi-invariant, so are the maximally smooth trajectories. The last two trajectories are similar, but they are not the same. Also note that in the planar case, the rotational part of Eq. (47) reduces to:
!(3) = 0; which implies that the solution will be a cubic spline. In other words, in the planar case the rotational parts of the motions obtained by any of the above schemes are the same. Figure 3 shows an example of spatial trajectories for a solid block. As before, the body- xed frame fMg was attached at the center of mass of the block with the axes parallel to the sides of the block. Let be the vector of coordinates de ned in Eq. (12) describing the position and 21
FIGURE COMES HERE.
Figure 3: (a) Maximally smooth interpolating trajectory computed from metric (40); (b) spline interpolation in the space of coordinates; (c) maximally smooth interpolating trajectory computed from metric (43); (d) curve obtained by combining the rotational part in (b) with the translational part in (a). orientation of the frame fMg and let V = f! T ; v T g be the instantaneous twist for the motion. The interpolation problem for the example in Figure 3 was given by:
1 = f0:20; ?0:10; 0:40; 1:24; 0:88; 0:85g 2 = f?0:51; 0:17; ?0:15; 11:54; 5:79; ?18:60g 3 = f1:75; ?0:40; 0:24; 28:36; ?8:56; ?35:69g
t1 = 0 t2 = 21 t3 = 1
V1 = f0; 0; 10; 0; ?10; ?10g V2 = f0; ?10; 0; 0; ?10; 0g
The same three interpolation schemes as above were used to compute the trajectories. The maximally smooth trajectory computed from the metric (40) is shown in Figure 3.a. This trajectory does not depend on the choice of the inertial reference frame fFg, but it does depend on the choice of the body- xed frame fMg. The translational part of the trajectory is exactly the cubic spline in IR3 , as can be seen from Eq. (47). Figure 3.b shows the trajectory computed by the method from Section 3.1, and the maximally smooth trajectory computed from the metric (43) is shown in Figure 3.c. These two trajectories are both bi-invariant, but in both cases the translational part of the motion is more curved compared to the motion in Figure 3.a. It is also interesting to combine the rotational part of motion in Figure 3.b with the translational part of motion in Figure 3.a. In other words, we separately compute the rotational motion on the group of rotations SO(3) using the method from Section 3.1 adapted to SO(3) and the translational motion on IR3 given by the cubic spline. The resulting motion is shown in Figure 3.d. The motion seems almost identical to the maximally smooth motion in Figure 3.d. A closer look reveals that the rotational parts are not the same: a cubic spline is in general not a solution for the rotational part of (47) (for some special cases in which this is true see [22]). However, 22
the similarity indicates that the cubic curve might be an approximation of the solution of this equation (see also [17]). Finally, note that the trajectory in Figure 3.d obtained by interpolating separately in SO(3) and IR3 is not bi-invariant.
5 Concluding remarks We discussed di erent methods for interpolation between a given set of positions and orientations. A typical approach to the problem is to interpolate the orientations and the positions separately and then combine the resulting interpolating curves. We show that by viewing the problem as an interpolation problem on SE (3), we can investigate several properties of the trajectories. In particular, we can systematically analyze the invariance of the interpolating curves with respect to the choice of the inertial and the body- xed frames and the smoothness properties of the trajectories. We proposed two di erent interpolation schemes: 1. The given set of elements of SE (3) is represented by appropriate coordinates (de ned in Equation (12) and interpolation with cubic splines is performed on these coordinates by treating them as vectors in the Euclidean space. The exponential map is then used to obtain the interpolating trajectories on SE (3) and these trajectories are bi-invariant. 2. Given a suitable measure of smoothness the methods of variational calculus on SE (3) are used to compute the maximally smooth trajectories. To demonstrate the method we computed the trajectories that are a generalization of cubic splines from the Euclidean case. The advantage of the rst scheme is that the trajectories are easy to compute. The second scheme is attractive since it produces the \best possible" interpolating trajectories, but the solution must be computed numerically. In this paper, the trajectories are obtained by solving a boundary value problem with a nite-di erence method. We computed the interpolating trajectories for a case when the motion takes place in a plane and for a spatial case. In both cases, the maximally smooth motions computed from the metric (40) appear to be the most natural. However, these trajectories must be computed numerically and they depend on the choice of the body- xed reference frame. The examples also suggest that these motions might be well approximated with the trajectories that are obtained by separate 23
interpolation on SO(3) and IR3 with the rst method. An interesting direction for future work would be to compute the estimates on the di erence between trajectories obtained in this way and the optimal trajectories computed using the methods of variational calculus.
Acknowledgment We thank Professor Chris Croke, Department of Mathematics, University of Pennsylvania, for many invaluable discussions on metrics and screw motions. It was our joint work [24] that eventually led to the research published here. We are also grateful for the nancial support provided by the NSF grants MIP 94-20397, MSS 91-57156 and CISE/CDA 88-22719, and the ARO MURI Grant DAAH04-96-1-0007.
References [1] F. Bullo and R. Murray. Proportional derivative (pd) control on the Euclidean group. In 1995 European Control Conference, Rome, Italy, 1995. [2] 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. [3] J. Cheeger and D. G. Ebin. Comparison Theorems in Riemannian Geometry. North-Holland Publishing Company, Amsterdam, 1975. [4] 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. [5] M. P. do Carmo. Riemannian geometry. Birkhauser, Boston, 1992. [6] G. E. Farin. Curves and surfaces for computer aided geometric design : a practical guide. Academic Press, Boston, 3 edition, 1992. [7] Q. J. Ge and B. Ravani. Computer aided geometric design of motion interpolants. ASME Journal of Mechanical Design, 116:756{762, 1994. [8] Q. J. Ge and B. Ravani. Geometric construction of Bezier motions. ASME Journal of Mechanical Design, 116:749{755, 1994. 24
[9] J. Hoschek and D. Lasser. Fundamentals of Computer Aided Geometric Design. AK Peters, 1993. [10] B. Jutler. Visualization of moving objects using dual quaternion curves. Computers and graphics, 18(3):315{326, 1994. [11] A. Karger and J. Novak. Space Kinematics and Lie Groups. Gordon and Breach Science Publishers, 1985. [12] M.-J. Kim, M.-S. Kim, and S. Y. Shin. A general construction scheme for unit quaternion curves with simple high order derivatives. In ACM Siggraph'95, pages 369{376, Los Angeles, CA, 1995. [13] J. Loncaric. Normal forms of sti ness and compliance matrices. IEEE Journal of Robotics and Automation, RA-3(6):567{572, 1987. [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 R. W. Brockett. Kinematic dexterity of robotic mechanisms. International Journal of Robotics Research, 13(1):1{15, 1994. [17] 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. [18] 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. [19] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery. Numerical Recipes in C. Cambridge University Press, Cambridge, 1988. [20] B. F. Schutz. Geometrical Methods of Mathematical Physics. Cambridge University Press, Cambridge, 1980. [21] K. Shoemake. Animating rotation with quaternion curves. ACM Siggraph, 19(3):245{254, 1985. 25
[22] M. Z efran. Continuous methods for motion planning. PhD thesis, U. of Pennsylvania, Philadelphia, PA, 1996. [23] M. Z efran and V. Kumar. Planning of smooth motions on SE (3). In Proceedings of 1996 International Conference on Robotics and Automation, pages 121{126, Minneapolis, MN, April 1996. [24] 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.
26
List of Figures 1 2
3
The inertial ( xed) frame, fFg, and the moving frame, fMg, attached to the rigid body. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 (a) Interpolation with geodesic segments connecting speci ed positions; (b) maximally smooth interpolating trajectory computed from the metric (40); (c) spline interpolation in the space of coordinates; (d) maximally smooth interpolation trajectory computed from the metric (43). . . . . . . . . . . . . . . . . . . . . . . 21 (a) Maximally smooth interpolating trajectory computed from metric (40); (b) spline interpolation in the space of coordinates; (c) maximally smooth interpolating trajectory computed from metric (43); (d) curve obtained by combining the rotational part in (b) with the translational part in (a). . . . . . . . . . . . . . . 22
27
Computer-Aided Design, Z efran & Kumar, Figure 1
{M} t3
x’ {M}
z’
y’
z’
y’
{M} t2 {F}
x’
t1
z
z’
x’
y x
28
y’
Computer-Aided Design, Z efran & Kumar, Figure 2
y
y 14 12 10 8 6 4 2 0
5
10
15
20
25
14 12 10 8 6 4 2 0
x
a)
10
15
20
25
5
10
15
20
25
x
b) y
y 14 12 10 8 6 4 2 0
5
5
10
15
20
25
14 12 10 8 6 4 2 0
x
c)
d)
29
x
Computer-Aided Design, Z efran & Kumar, Figure 3
y 0
5
15 10
0
10
y
x 20
30
0
0
5
15 10
0
10
x 20
30
0
z
z
-10
-10
-20
-20
a)
b)
y 0
5
15 10
0
10
y
x 20
30
0
0
0
z
z
-10
-10
-20
-20
c)
d)
30
5
15 10
0
10
x 20
30
Biographies Milos Z efran
received B.E. and M.Sc. degrees in Electrical Engineering and a B.Sc. degree in Mathematics from the University of Ljubljana, Slovenia in 1989, 1992, and 1992, respectively. He received a Ph.D. degree in Computer Science from the University of Pennsylvania. From 1993 to 1995 he was a graduate fellow of the Institute for Research in Cognitive Science at the University of Pennsylvania. He is currently a NSF Postdoctoral Fellow at the California Institute of Technology. His research interests include robotics, optimal control, and control of hybrid systems.
Vijay Kumar received his M.Sc. and Ph.D. in Mechanical Engineering from The Ohio State
University in 1985 and 1987 respectively. He has been on the Faculty in the Department of Mechanical Engineering and Applied Mechanics at the University of Pennsylvania since 1987. He is currently an Associate Professor and also holds a secondary appointment in the Department of Computer and Information Science. He has served on the editorial board of the IEEE Transactions on Robotics and Automation. He is currently on the Editorial Board of the Journal of Franklin Institute and an Associate Editor of the ASME Journal of Mechanical Design. He is the recipient of the 1991 National Science Foundation Presidential Young Investigator award. His research interests include robotics, mechanisms, dynamics and control.
31
2
California Institute of Technology, Pasadena, CA, USA 2 General Robotics and Active Sensory Perception (GRASP) Laboratory University of Pennsylvania, Philadelphia, PA, USA 1
Abstract This paper investigates methods for computing a smooth motion that interpolates a given set of positions and orientations. The position and orientation of a rigid body can be described with an element of the group of spatial rigid body displacements, SE (3). To nd a smooth motion that interpolates a given set of positions and orientations is therefore the same as nding an interpolating curve between the corresponding elements of SE (3). To make the interpolation on SE (3) independent of the representation of the group, we use the coordinate-free framework of di erential geometry. It is necessary to choose inertial and body- xed reference frames to describe the position and orientation of the rigid body. We rst show that trajectories that are independent of the choice of these frames can be obtained by using the exponential map on SE (3). However, these trajectories may exhibit rapid changes in the velocity or higher derivatives. The second contribution of the paper is a method for nding the maximally smooth interpolating curve. By adapting the techniques of the calculus of variations to SE (3), necessary conditions are derived for motions that are equivalent to cubic splines in the Euclidean space. These necessary conditions result in a boundary value problem with interior-point constraints. A simple and ecient numerical method for nding a solution is then described. Finally, we discuss the dependence of the computed trajectories on the metric on SE (3) and show that independence of the trajectories from the choice of the reference frames can be achieved by using a suitable metric.
Keywords: motion interpolation, Euclidean group, Riemannian geometry
1 Introduction The problem of nding a smooth motion that interpolates a given set of positions and orientations is frequently encountered in robotics (planning of trajectories in welding and painting), computer graphics (animation, interpolation of motion through a set of key frames) and computer-aided geometric design (interactive interpolation schemes for design), and has recently received considerable attention. The problem is well understood in Euclidean spaces [6, 9], but it is not clear how these techniques can be generalized to curved spaces. There are several 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. A method for computing points on the curve (analytical expressions, numerical methods, or such geometric construction techniques as the de Casteljau algorithm) and the algebraic properties of the trajectories (rational versus nonrational expressions) are also important. Finally, the interpolation properties of the trajectories (interpolation versus approximation) and their smoothness properties need to be considered. Shoemake [21] 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 [8] and Park and Ravani [18] to spatial motions. The focus in these articles is how to generalize the notion of interpolation from the Euclidean space to a curved space. Shoemake [21] uses interpolation with one-parameter subgroups. The fact that on SO(3), the group of rigid body rotations in IR3 , one-parameter subgroups are at the same time projections of screw motions from SE (3) as well as the geodesics for the natural (bi-invariant) metric, led to two di erent extensions of this work: Ge and Ravani [8] formulated a de Casteljau algorithm on the group of rigid-body motions SE (3) using the screw motions for interpolation, while Park and Ravani [18] proposed a de Casteljau algorithm for Riemannian manifolds where the interpolation was performed with geodesics. On SO(3), the latter scheme produces the same curves as the original Shoemake's method. None of these schemes produces rational motions. Another class of methods is based on the representation of Bezier curves with Bernstein polynomials. Ge and Ravani [7] used the dual unit quaternion representation of SE (3) and subsequently applied Euclidean methods to interpolate in this space. Jutler [10] formulated a more general version of the polynomial interpolation by using dual (instead of dual unit) quaternions to represent SE (3). In such representation, an element of SE (3) corresponds to a whole equivalence class of dual quaternions. Jutler investigated di erent methods for lifting a curve 2
on SE (3) to a curve in the space of dual quaternions and identi ed schemes that have certain invariance properties with respect to the choice of reference frames. Kim et al. [12] generalized di erent polynomial interpolation schemes from Euclidean space to SO(3) by \mapping" the expressions from the Euclidean space through the exponential map. Park and Kang [17] derived a rational interpolating scheme for the group of rotations SO(3) by representing the group with Cayley parameters and using Euclidean methods in this parameter space. The advantage of the methods described in this paragraph (the exception is [12]) that use Euclidean methods to interpolate in a parameter space is that they produce rational curves. Although all these works (with the exception of [18]) use a particular coordinate representation of either the group of rotations SO(3) or the Euclidean group SE (3) it would be wrong to conclude that they produce motions that depend on the chosen representation. More specifically, the schemes in [21], [8], [18], and [12] are all based on geometric algorithms and do not depend on the particular choice of representation. However, except for [18], this is not obvious and seems not to have been achieved deliberately. Further, the schemes in [8], [18], and [12] can not be directly used for interpolation (they assume that the Bezier or B-spline control polygon is given) and it is not clear that interpolation can be easily achieved. On the other hand, [21], [7], [10], and [17] generate interpolating curves, but [21] and [17] are limited to SO(3) while [7] and [10] base their method on a particular representation of SE (3). The works discussed above focus on the approximation or interpolation capability of Bezier (or B-spline) curves. However, an additional property of cubic splines in the Euclidean space is that they minimize the integral of the norm of the acceleration along the curve. This has motivated several researchers to generalize splines to the curved spaces as curves minimizing an appropriate cost functional. Noakes et al. [15] derived the necessary conditions for cubic splines on general manifolds. These results are extended in [4] to the dynamic interpolation problem. Necessary conditions for higher-order splines are derived in Camarinha et al. [2]. All these results are derived in a general setting and are only specialized to compact Lie groups. Since the group SE (3) is not compact, there are several additional issues that need to be addressed before these methods can be applied. Of particular importance is the choice of metric given that there is no natural metric for SE (3) [3, 13]. In [23] and [24], the variational approach was applied speci cally to interpolation of smooth motions on SE (3). Several metrics suitable for trajectory planning were identi ed and a computational method was described for computing maximally smooth trajectories. 3
In this paper, we build on the results from [23, 24] to investigate various motion interpolation schemes on SE (3). We will 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. Our goal is to develop ecient methods for nding trajectories that minimize meaningful performance functions. However, we will show that there is a trade-o between the computational eciency and optimality of the generated trajectories. We rst introduce some basic tools from di erential geometry that are used in the rest of the paper. We then describe a simple scheme for nding an interpolating motion that does not depend on the choice of the inertial and body- xed reference frames. The scheme is based on the properties of the exponential map on SE (3). Next, we outline a method for computing minimum acceleration curves on SE (3), which are a generalization of cubic splines. This scheme follows the approach in [15], [4], and [2], but focuses on the problems speci c to SE (3) and addresses the computational issues. We therefore base our work on the results from [23]. In particular, we show that curves with desired invariance properties can be computed by properly selecting a metric on SE (3). Finally, we compute the curves for two choices of the metric and compare them with the trajectories obtained by the scheme based on the exponential map.
2 Mathematical preliminaries 2.1 The Lie group SE (3) Consider a rigid body moving in free space. Assume any inertial reference frame fFg xed in space and a frame fMg xed to the body at point O0 as shown in Figure 1. At each instance, the con guration (position and orientation) of the rigid body can be described by a homogeneous transformation matrix corresponding to the displacement from frame fFg to frame fMg. The set of all such matrices is a group for the standard matrix multiplications and is called SE (3), the special Euclidean group of rigid body transformations in three-dimensions:
SE (3) =
82 > < 6 4 > :
3
R d7 0 1
5
jR2
3 3
IR
;d 2
3
IR
9 > =
; RT R = I; det(R) = 1> : ;
(1)
It is well known that SE (3) is a Lie group [11, 14]. On a Lie group, the tangent space at the group identity can be endowed with the so-called Lie bracket operation that makes it a Lie algebra. The Lie algebra of SE (3), denoted by se(3), 4
FIGURE COMES HERE.
Figure 1: The inertial ( xed) frame, fFg, and the moving frame, fMg, attached to the rigid body. is given by:
82 > < 6 4 > :
9 > =
3
v7 33 ; v 2 IR3; T = ? > ; 5 j 2 IR ; 0 0 and the Lie bracket of two elements S1; S2 2 se(3) corresponds to their commutator:
se(3) =
(2)
[S1; S2] = S1 S2 ? S2S1 : A 3 3 skew-symmetric matrix can be uniquely identi ed 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 element S 2 se(3) can be thus identi ed with a vector pair f!; v g. This identi cation will be denoted by: vec(S ) = f!; v g (3) If vec(S1 ) = f!1 ; v1g and vec(S2) = f!2; v2g, it is easy to verify that: vec([S1; S2]) = f!1 !2 ; !1 v2 + v1 !2 g:
(4)
In the rest of the paper, the vector pair f!; v g will be interchangeably treated as a pair of two vectors or as a 6 1 column vector. The exact form will be clear from the context. Given a curve A(t) : [?a; a] ! SE (3), 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: 2
3
T _ T_ 6 R R R d 7
S (t) = A?1 (t)A_ (t) = 4
0
0
5
:
(5)
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 5
body while v is the linear velocity of the origin O0 of the frame fMg, both expressed in the frame fMg. In kinematics, elements of this form are called twists [14] and se(3) is thus isomorphic to the space of twists. It is easy to check that the twist S (t) computed from Eq. (5) does not depend on the choice of the inertial frame fFg. For this reason, S (t) is called the left invariant representation of the tangent vector A_ . Alternatively, the tangent vector A_ could be identi ed with a right invariant twist (invariant with respect to the choice of the body- xed frame fMg). Since se(3) is a vector space, any element can be expressed as a 6 1 vector of components corresponding to a chosen basis. The standard basis for se(3) is: 2
L1 =
6 6 6 6 6 6 6 4 2
L4 =
6 6 6 6 6 6 6 4
0 0 0 0 0 0 0 0
0 0 0 ?1 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0
3 7 7 7 7 7 7 7 5 3 7 7 7 7 7 7 7 5
2
0 6 6 6 0 L2 = 666 6?1 4 0 2 0 6 6 6 0 L5 = 666 6 0 4 0
0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0
3
2
7 7 7 7 7 7 7 5
6 6 6 6 6 6 6 4
L3 =
3
2
7 7 7 7 7 7 7 5
6 6 6 6 6 6 6 4
L6 =
0 ?1 1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0
3 7 7 7 7 7 7 7 5 3
(6)
7 7 7 7 7 7 7 5
If interpreted as twists, L1 , L2 and L3 represent instantaneous rotations about and L4, L5 and L6 instantaneous translations along the Cartesian axes x, y and z of the body- xed frame fMg, respectively. The components of a twist S 2 se(3) in this basis are given precisely by the velocity vector pair, f!; v g.
2.2 Left invariant vector elds A (di erentiable) vector eld on a manifold is a (smooth) assignment of a tangent vector to each element of the manifold. At each point, a vector eld de nes a unique integral curve to which it is tangent [20]. Formally, a vector eld X is a (derivation) operator which, given a di erentiable function f , returns its derivative (another function) along the integral curves of X . In other words, if (t) is a curve tangent to a vector eld X at point p = (t0), then: df (
( t )) Xf jp = dt : t0
(7)
On SE (3), which is a matrix Lie group, a tangent vector can be computed using vector calculus since the group is embedded into the Euclidean space IR44 . In other words, the tangent vector 6
X to a curve A(t) at point A(t0) is given by: daji X jA(t0) = dA = dt t0 dt "
#
t0
(8)
where aji are the entries of the matrix A. However, the matrix representation is only a computational tool; formally, the tangent vector must be understood as an operator in the sense of Eq. (7). Using the matrix representation of tangent vectors, an example of a (di erentiable) vector eld, X , is obtained by setting at each point A:
X (A) = Tb(A) = AT;
(9)
where T belongs to the Lie algebra of the group. Such a vector eld is called a left invariant vector eld. We use the notation Tb to indicate that the vector eld is obtained by left translating the Lie algebra element T . The set of all left invariant vector elds is a vector space and by construction it is isomorphic to the Lie algebra. Right invariant vector elds can be de ned in analogous way. Note that a vector eld need not be left or right invariant. Since L1 ; L2; : : :; L6 are a basis for the Lie algebra se(3), the left invariant vector elds Lb 1; Lb 2; : : :; Lb 6 form a basis for the space of the left invariant vector elds. Finally, because at any point A 2 SE (3) the vectors Lb 1 (A); : : :; Lb 6 (A) form a basis of the tangent space at that point, any vector eld X can be expressed as
X=
6 X
i=1
X iLb i;
(10)
where the coecients X i vary over the manifold { if they are constant then X is left invariant. This implies that we can associate a vector pair f!; v g (that in general varies over the manifold) de ned by: ! = [X 1; X 2; X 3]T ; v = [X 4; X 5; X 6]T : to an arbitrary vector eld X .
2.3 Exponential map and local coordinates A motion of the rigid body in IR3 is described by a curve, A(t), on SE (3). If V = dA dt is the vector eld tangent to A(t), the vector pair f!; v g associated with V corresponds to the instantaneous twist (screw axis) for the motion. In general, the twist f!; v g changes with time. Motions for 7
which the instantaneous twist f!; v g remains constant are called screw motions in the kinematics literature [14]. Let the twist S 2 se(3) be represented by a vector pair f!; v g and let A(t) be a screw motion with the screw axis f!; v g such that A(0) = I . We de ne the exponential map exp : se(3) ! SE (3) by: exp[tS ] = A(t); (11) 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, pp. 413]. For t = 1, the function exp[] is a surjective map of se(3) onto SE (3). However, for a small enough neighborhood of zero in se(3) it is a di eomorphism onto a neighborhood of the identity element in SE (3). We can thus de ne local coordinates, i , for A 2 SE (3) suciently close to the identity by: i
h
A() = exp 1L1 + 2L2 + 3L3 + 4L4 + 5L5 + 6L6 :
(12)
These coordinates are valid provided Trace [A] 6= 0 (a point that represents a rotation through ). If such a point is encountered, we can rede ne the coordinate chart by constructing a similar representation in the neighborhood of a di erent element of SE (3) using the operation of left translation. The geometric interpretation of this particular parameterization of SE (3) in terms of screw coordinates can be found in [14].
2.4 Riemannian metrics on Lie groups If a smoothly varying, positive de nite, bilinear, symmetric form is de ned on the tangent space at each point on the manifold, we say the manifold is Riemannian. The bilinear form is an inner product on the tangent space at each point and is called a Riemannian metric. If the bilinear form is inde nite but non degenerate, it is called a pseudo-Riemannian metric. On a Lie group, and thus on SE (3), an inner product on the tangent space at the identity can be extended to a Riemannian metric (everywhere on the manifold) using the idea of left translation. Assume that the inner product of two elements T1; T2 2 se(3) is de ned by
< T1; T2 >I = tT1 Wt2;
(13)
where t1 and t2 are the coordinates of T1 and T2 with respect to some basis and W is a positive de nite matrix. If V1 and V2 are tangent vectors at an arbitrary group element A 2 SE (3), the 8
inner product < V1; V2 >A in the tangent space TA SE (3) can be de ned by:
< V1; V2 >A =< A?1V1; A?1V2 >I :
(14)
The metric obtained in such a way is called a left invariant metric [20]. Physically, left invariance corresponds to independence on the choice of the inertial frame [14].
2.5 Ane connection and covariant derivative The motion of a rigid body is represented by a curve, A(t), on SE (3). The velocity at an arbitrary point is the tangent vector to the curve at that point. In order to obtain the acceleration, or to engage in a dynamic analysis, we need to be able to di erentiate a vector eld along the curve. At each point A 2 SE (3), the value of a vector eld belongs to the tangent space TA SE (3) and to di erentiate a vector eld along a curve, we must be able to compare vectors from tangent spaces at di erent points on the curve. But as opposed to IRn , where vectors can be translated to a common origin, on curved spaces no canonical way of translating vectors exists and must be explicitly de ned. To di erentiate vector elds along a curve it suces to de ne how to transport a vector along the curve from one tangent space to another. This process of translation of vectors along a curve is called parallel transport [20]. Parallel transport is clearly not unique. However, given a parallel transport, we can de ne derivative of a vector eld along a curve. Let X be a vector eld de ned along A(t), and let X (t) stand for X (A(t)). Denote by X t0 (t) the parallel transport of the vector X (t) to the point A(t0). The covariant derivative of X along A(t) is: DX = lim X t0 (t) ? X (t0) : (15)
dt
t0
t
t!t0
By taking covariant derivatives along integral curves of a vector eld Y , we obtain a covariant derivative of the vector eld X with respect to the vector eld Y . We can therefore de ne an operator r, called ane connection:
DX rY X jA0 = dt ; (16) t0 where DX dt is taken along the integral curve of Y passing through A0 at time t0 . In order to compute the covariant derivative rY X , it suces to know the covariant derivatives of the basis P P vector elds. For two arbitrary vector elds X = 6i=1 X i Lb i and Y = 6i=1 Y i Lb i , the expression for rY X is then given by: i i j b b (17) rY X = dX dt Li + X Y rLb Li j
9
where dtd is the derivative along the integral curve of Y . Given a manifold with a Riemannian or pseudo-Riemannian metric, there exists a unique connection [20] which is compatible with the metric:
and symmetric:
X =< rX Y; Z > +
(18)
rX Y ? rY X = [X; Y ]:
(19)
This connection is called the Levi-Civita connection. The velocity, V (t), of the rigid body moving along the curve A(t) is given by the tangent vector eld. Given a connection, we can de ne the acceleration and higher derivatives of the velocity. The acceleration, A(t), is the covariant derivative of the velocity along the curve: dA D A= = r V: (20)
dt dt
V
Higher derivatives of the velocity can be de ned similarly. Note that the acceleration and the other higher derivatives depend on the choice of the connection.
2.6 Curvature tensor The curvature of a Riemannian manifold is a correspondence R that associates to a pair of vector elds X and Y a mapping:
R(X; Y ) : Z 7! rY rX Z ? rX rY Z + r[X;Y ]Z
(21)
where Z is a vector eld and r is the Riemannian connection on . The curvature tensor is a multi-linear mapping which maps a quadruple of vectors (X; Y; U; V ) into a real number. The value of the curvature tensor on the quadruple (X; Y; U; V ) is given by < R(X; Y )U; V >. If Xi is a basis, the components of the curvature tensor are given by:
Rijkl =< R(Xi; Xj )Xk; Xl >
(22)
3 Interpolation on SE (3) The basic interpolation problem on SE (3) can be posed in the following way:
Problem 3.1 Given a sequence fAigni of elements of SE (3) and a sequence of times fti gni , =1
where t1 t2 : : : tn , nd a curve (t) such that (ti ) = Ai .
10
=1
This basic problem can be re ned for speci c applications. We study the case when the velocities of the rigid body at times t1 and tn are prescribed. Further, we wish to impose certain smoothness requirements on the motion. In particular, we concentrate on motions that are continuous at the level of acceleration (C 2). Most of the results in the paper can be generalized in a straight forward way if higher level of continuity is required. We therefore focus on the following problem:
Problem 3.2 Given: 1. a sequence fAigni=1 of elements of SE (3); 2. a sequence of times fti gni=1 , where t1 t2 : : : tn ; 3. the instantaneous twists V1 and V2 (V1; V2 2 se(3)) at times t1 and tn , respectively; nd a curve (t) 2 C 2[t1 ; tn ] such that (ti ) = Ai .
The ane nature of the space (in which the rigid body moves) and practical considerations suggest that the interpolating curve should have some additional properties: (1) The trajectories must be independent of the representation of the motion in space (parameterization of SE (3)). In this way, computations performed with di erent choices of coordinates for SE (3) will produce consistent results. (2) The trajectories should be independent of the choice of the inertial reference frame fFg and the body xed frame fMg (Figure 1). That is, it should not matter where we put the inertial frame and which frame we choose on the rigid body. The resulting trajectory of the rigid body should be the same. (3) The trajectories must have good performance for the intended use. In this paper, we formulate a cost functional that re ects the smoothness of the trajectory and show how to compute a trajectory that minimizes this cost functional. We will present two di erent solutions to Problem 3.2. The rst method is similar to those in [7], [10], and [17] in that it is based on a particular representation of SE (3) and interpolation in the appropriate parameter space using Euclidean methods. However, we choose a representation that allows easy geometric analysis of the computed trajectories and base our method on this geometric analysis, thereby guaranteeing its independence of the chosen representation of SE (3). 11
We show that the method satis es (1) and (2) from the above, but it is dicult to quantify the performance of the solution. In the second part of this section we therefore present a method that computes trajectories that optimize a chosen measure of smoothness. As before, by using di erential geometry and basing the method on geometric analysis, we achieve its independence of the parameterization of the space. The method therefore satis es (1) and (3). We show that (2) can be achieved by appropriate choice of Riemannian (or pseudo-Riemannian) metric on SE (3). The drawback of this approach is that it is computationally expensive. This section therefore demonstrates that there is a trade-o between the computational eciency and the optimality of the generated trajectories.
3.1 Interpolation based on the exponential map We sketch a method that produces trajectories that do not depend on the parameterization of SE (3), are independent on the choice of the inertial and body- xed frame and have the required smoothness properties. However, the trajectories do not optimize a geometrically meaningful cost functional1 . The main idea is to use properties of the exponential map to achieve the invariance of the trajectories with respect to the choice of the body- xed and inertial reference frames. Let fAi gni=1 and fti gni=1 be the sequences in Problem 3.2. We will look for an interpolating curve of the form:
(t) = A1 (t): (23) In this way, the interpolation problem becomes to nd a curve (t) that interpolates the sequence fA?1 1Aigni=1. Next, let the position A?1 1 Ai be represented with a 6 1 vector i according to Eq. (12). Using an interpolation method in the Euclidean space IR6, it is easy to nd a curve (t) such that: (ti) = i: (24) Obviously, the curve:
(t) = A1 exp[(t)] ;
(25)
where vec() = , will pass through the point Ai at the time ti and it is a suitable candidate for the interpolation. If we also wish to satisfy the boundary conditions on the velocities at t1 and tn , They can be shown to minimize a cost functional formulated in the parameter space, but this cost functional does not have any geometric meaning. 1
12
this poses additional requirements for the curve (t). If vec(S ) denotes the vector corresponding to an element S 2 se(3), the time derivative of the exponential map can be written in the form: vec (t)?1 d (t) = vec exp[?(t)] d exp[(t)] = ( (t))_(t):
dt
dt
(26)
The 6 6 matrix can be computed from the formula for the exponential map. The explicit expressions for the derivative of the exponential map on SE (3) can be found in [1]. The instantaneous twist, V , representing the velocity of the rigid body, is given by Eq. (5):
V = (t)?1 dtd (t):
Hence, the expression for the velocity, expressed in the local coordinates, is: _ = ()?1vec(V ) :
(27) (28)
Let V1 and V2 denote the instantaneous twists at t1 and tn , respectively, and _1 and _n the vectors computed from Eq. (28). To satisfy the boundary conditions on the velocities, the curve (t) must therefore satisfy the following equations: (29) _(t) = (n)?1vec(Vn) _(t) = (1)?1vec(V1) tn
t1
By computing a curve (t) (in the Euclidean space) that satis es the conditions (24) and (29), we therefore obtain a curve that solves the interpolation problem. We now investigate properties of the trajectories generated in this way.
Smoothness Since the exponential map is smooth, the interpolating curve (t) will be as smooth as the curve (t). We can therefore control the smoothness of the interpolating curve directly by controlling the smoothness of the curve in the Euclidean space IR6 .
Left invariance The curve of the form (23) will always be left invariant, that is, invariant with
respect to the choice of the inertial reference frame fFg. To see this, let us displace fFg so that its new position with respect to its initial position is given by the homogeneous transformation matrix C . Now the set of points that have to be interpolated becomes fCAigni=1 . But given the form (23), this means that the curve in Eq. (23) has to interpolate between the points (CA1)?1 CAi = A?1 1 Ai , which are the same as before. In addition, the velocity of the rigid body in the new frame is (CA)?1C A_ = A?1 A_ , again the same as before. We conclude that if in the old frame the curve was (t) = A1 exp[(t)], the curve in the new frame will simply be
e(t) = CA1 exp[(t)] = C (t), which proves left invariance. A similar proof was presented in [17]. 13
Right invariance Consider again Problem 3.2 and the interpolating curve (t) = A exp[(t)]. 1
Now displace the body- xed frame fMg through a displacement C . The interpolation points become fAi C gni=1 , and the instantaneous twist in the new frame, denoted by V~ is related to the twist in the original frame by the adjoint map [5, 11]:
V~ = AdC?1 V = C ?1V C:
(30)
Consider a new coordinate curve ~(t) = vec((t)), given by: ~ (t) = C ?1(t)C:
(31)
It is a property of the exponential map [5] that for any element S 2 se(3): h i exp C ?1 SC = C ?1 exp[S ] C:
Now take the curve:
(32)
i
h
e t) ;
e(t) = Ae1 exp (
(33)
where Ae1 = A1C . A short derivation shows: h
i
h
i
e t) = A C exp C ?1 (t)C =
e(t) = Ae1 exp ( 1 A1C (C ?1 exp[(t)] C ) = A1 exp[(t)] C = (t)C:
(34)
Since (t) interpolates the points fAi gni=1 , the curve e (t) will interpolate the points fAi C gni=1 and it will satisfy the new boundary conditions Vei = C ?1 Vi C , i = 1; 2. In order to show that the interpolation scheme is right invariant, we need to show that the curve e(t) is exactly the curve that would be produced by running the interpolation scheme in the transformed coordinate system. In other words, we need to show that the following diagram commutes: Interpolation Ξ(t), γ(t)
{Ai} n1, , V1, V2
Ξ −> C−1 Ξ C
Change of body−fixed frame
{Ai C } n1, , C−1 V1 C, C−1 V2 C
γ −> γ C
Interpolation
(35)
~ Ξ (t), ~γ (t)
In general this will not be true and we need to know how the interpolation between the coordinates i is performed. Obviously the interpolation scheme will be right invariant if and only if 14
the set of curves (t) = vec((t)) that are generated by the scheme is invariant with respect to the transformation: AdC ?1 : (t) 7! C ?1 (t)C; (36) for an arbitrary C 2 SE (3). Examples of families of curves that have this property and will therefore lead to a right invariant interpolation scheme are cubic splines and polynomials of a xed degree which is greater or equal to n ? 1. The transformation in (36) is linear and therefore maps a cubic polynomial to a cubic polynomial. It is also not dicult to see that a cubic spline that solves the interpolation problem 3.2 formulated in the space of the coordinates is unique. This implies that the diagram in (35) must commute. A similar argument holds for the interpolation with polynomials of degree n ? 1: there is a unique polynomial of degree n ? 1 that will interpolate between n points. The diagram (35) will also commute for the polynomials of higher degree, although in this case the interpolating curve is not unique.
Independence of the parameterization At the rst sight, it seems that the proposed
interpolation scheme is based on the particular choice of coordinates in Eq. (12). However, while this choice of coordinates is convenient for the analysis, the actual interpolation does not necessarily have to be based on this representation. More precisely, the above analysis shows that we only need to establish the invariance of the family of the curves used for interpolation with respect to the map de ned in (36).
Remark 3.3 The analysis in this section is not limited to SE (3) and is applicable to any matrix Lie group.
3.2 Maximally smooth interpolating curves The interpolation scheme presented in the previous section yields smooth trajectories and is bi-invariant if the family of the interpolating curves in the parameter space is properly chosen. However, to prevent fast changes in the velocities or their higher derivatives, it is often desired to impose stricter smoothness requirements. More precisely, we can formulate a cost functional measuring the smoothness of a curve (the choice of the cost functional in general depends on the task) and nd a curve that minimizes this cost functional. Intuitively, the smoothness of a curve is related to the magnitude of its derivatives. This implies that we need to be able to compute velocity and higher derivatives of a curve and that we need a notion of magnitude of these derivatives. The group SE (3) is not a Euclidean space and it is thus necessary to 15
use the formalism of Riemannian geometry to de ne these quantities. We can subsequently de ne a cost functional that measures the smoothness of a curve. To compute the trajectory that minimizes the chosen cost functional we then employ methods of variational calculus on Riemannian manifolds.
Variational calculus on manifolds It is well known [6] that in a Euclidean space cubic splines are the minima of the functional:
J=
Z
b
a
k k dt: 2
(37)
Noakes et al. [15] used this property to de ne cubic splines on an arbitrary manifold as curves minimizing the properly de ned norm of the acceleration. In Section 2.5 it was shown that the acceleration of a curve (t) with the velocity vector V (t) is given by rV V . Cubic splines are therefore the curves minimizing:
La =
Z
a
b
< rV V; rV V > dt:
(38)
In the function under the integral, < :; : > stands for the Riemannian metric while r is the corresponding Riemannian connection. In [15], necessary conditions are derived for such curves. Results for higher-order splines are presented in [2], while a more thorough investigation of this framework on SE (3) is presented in [23]. The result in [15] can be summarized with the following theorem:
Theorem 3.4 (Noakes et al.) Let A(t) be a curve on a Riemannian manifold that satis es
the boundary conditions (that is, it starts and ends at the prescribed points with the prescribed velocities) and let V = dA dt . If A(t) minimizes the functional La , then:
rV rV rV V + R(V; rV V )V = 0:
(39)
In Eq. (39), R(:; :) stands for the curvature.
Choice of metric on SE (3) There is no natural choice of metric for SE (3) [3, 13]. In this
paper we consider two choices. The rst is the left invariant metric that was proposed by Park and Brockett [16]. The metric is de ned through Eq. (14), where the matrix W is given by: 2
W = 64
I 0 0 I 16
3 7 5
(40)
and and are arbitrary constants. An attractive property of this metric is that the acceleration computed from Eq. (20) is exactly the acceleration as we know from kinematics [24]. In addition, this metric is a product metric [22], which means that the rotational and the translational component of motion can be treated separately. Finally, this is also the metric induced by the kinetic energy of a spherical mass, a generalization of the concept of a point mass. In [23] we show that the Riemannian connection corresponding to metric (40) is given by:
rY X = f d!dtx + 21 !y !x; dvdtx + !y vxg;
(41)
where dtd is the derivative along the integral curve of Y and f!x ; vx g and f!y ; vy g are the coordinates of the vector elds X and Y with respect to the left invariant basis (6). Note that the connection does not depend on the constants and . Direct calculation shows that the curvature is given by the following expression:
R(X; Y )Z = f 14 (!x !y ) !z ; 0g:
(42)
Another interesting metric is the Klein form [11]. It is also de ned through Eq. (14), where the matrix W is given by: 2 3 0 I W = 64 33 33 75 : (43) I33 033 It is not dicult to see that the Klein form is a pseudo-Riemannian metric. The unique symmetric connection that is compatible with this metric is given by [24]:
rL Lj = 21 [Li; Lj ]: bi
b
b
b
(44)
This leads to the following expression for the covariant derivative of a vector eld X along a vector eld Y : (45) rY X = f d!dtx + 12 !y !x; dvdtx + 12 (!y vx + vy !x)g: As before, dtd is the derivative along the integral curve of Y and f!x ; vxg and f!y ; vy g are the coordinates of the vector elds X and Y with respect to the left invariant basis (6). Direct calculation shows that the expression for the curvature for the pseudo-Riemannian metric (43) is given by: (46) R(Lb i; Lb j )Lb k = 41 [[Lb i; Lb j ]; Lb k ]: 17
The pseudo-Riemannian metric (43) has the important property that it is bi-invariant [24]2. This means that the curves computed from (39) using this metric will be left and right invariant. Another interesting property of the metric is that the geodesics (shortest distance curve) are screw motions [24].
Necessary conditions Having the expressions for the connection and the curvature, it is easy
to explicitly write the di erential equations that result from the Theorem 3.4. We obtain the following two corollaries:
Corollary 3.5 Let
2
A(t) = 64
3
R(t) d(t) 7 0
1
5
be a curve between two prescribed points on SE (3) that has prescribed initial and nal velocities. If f!; v g is the vector pair corresponding to V = dA dt , the curve minimizes the cost function La derived from the metric (40) only if the following equations hold:
!(3) + ! ! = 0
(47)
d = 0; (4)
where ()(n) denotes the nth derivative of ().
Corollary 3.6 Let
2
3
6 R(t) d(t) 7
A(t) = 4
0
1
5
be a curve between two prescribed points on SE (3) that has prescribed initial and nal velocities. If f!; v g is the vector pair corresponding to V = dA dt , the curve minimizes the cost function La derived from the metric (43) only if the following equations hold:
d3 V + [V; d2 V ] = 0: dt3 dt2
(48)
where ddt3 3V stands for ddt3 V3 Lb i and [V; ddt2 V2 ] denotes the Lie bracket (Eq. 4) of the twists with components V i and ddt2 V2 , respectively. i
i
In [24], it is shown that any bi-invariant metric on SE (3) is generated from a linear combination of the Klein form and the Killing form. Since the Killing form is degenerate, the metric (43) is therefore the unique (modulo the degenerate part) bi-invariant metric for SE (3). 2
18
Finding a solution In general, the rotational components of the cubic splines for the metric
(40) and both, the rotational and the translational components of the cubic splines for the metric (43) cannot be computed analytically. However, in the special case when the initial velocities are collinear with the initial velocity of the geodesic between the two endpoints, and the nal velocities are collinear with the nal velocity of the geodesic, it is possible to obtain a solution for these trajectories in terms of the geodesic curve [23]. If the geodesic curve can be computed analytically, so can the cubic splines.
The nite-di erence method For a general case, Theorem 3.4 yields a boundary value
problem that has to be solved numerically. The nite-di erence method proves to be ecient for solving such boundary-value problems. The basic idea of the method is to discretize the di erential equations in time: at each point, the system of di erential equations is converted into a system of algebraic equations by approximating derivatives with nite di erences. In this way, a system of coupled algebraic equations is obtained. The number of equations is usually fairly large. For detailed description of the method we refer the reader to [19] and the references therein. To apply the nite di erence method to Eq. (39), this higher order di erential equation must be converted into a system of rst-order di erential equations. To do this, we have to de ne a set of local coordinates and compute the solution in the coordinates. One possible choice are the coordinates in Eq. (12). We de ne: T
h
i
x = xT1 xT2 xT3 xT4 ; where
x1 x2 x3 x4
=
h
= !T vT
T
i
= x_ 2 = x_ 3 :
(49)
With these de nitions and the relation given by Eq. (26), we can obtain the system of equations in the desired form. Because of the requirement that the trajectory must pass through the intermediate points, we will have additional constraints of the form:
x1(ti) = i: 19
(50)
The constraints can be simply handled with the nite-di erence method since all we have to do at such points is replace Eq. (39) that gives the expression for x_ 4 , with Eq. (50), no additional computation is necessary. The boundary conditions at t1 and tn are also easily handled. They are given by: x1(t1) = 1 x2(t1) = vec(V1) (51) x1(tn) = n x2(tn) = vec(V2)
Remark 3.7 Since we based the numerical method on computing the trajectories on which the
rst variation of the cost functional vanishes ( rst order optimality conditions), it is necessary to use additional tests to establish that the computed trajectory is a minimum. While this can be done relatively easily, it is in general much more dicult to prove that the obtained minimum is a global minimum. For the problem at hand, we can conclude this with some certainty by analyzing the cases for which the global minimum can be analytically obtained and extrapolating the results to the case when the solution is computed numerically.
4 Examples In this section we compare the interpolating trajectories computed with three di erent methods:
The bi-invariant trajectories based on the exponential map and Euclidean interpolation in the space of coordinates with the coordinate chart de ned in Eq. (12).
The left-invariant trajectories obtained through minimization of the norm of the acceleration de ned through the left-invariant metric (40).
The bi-invariant trajectories obtained through minimization of the \norm" of the acceleration de ned through the bi-invariant metric3 (43).
In Figure 2, the trajectories of motion for a rectangle in the plane z = 0 are computed. The frame fMg is attached at the center of mass of the rectangle with the axes parallel to the sides of the rectangle. Let P = f; x; y g denote the orientation and position of the frame fMg in the plane and V = f!z ; vx; vy g the corresponding velocity. The interpolation problem was given by:
P1 = f0; 2; 8g P2 = f?1; 12; 4g P3 = f 3 ; 23; 10g 3
t1 = 0 t2 = 12 t3 = 1
V1 = f?1; 3; 10g V2 = f2; 2; 5g
Since the metric is not positive-de nite, the norm can be negative. See [22] for a discussion.
20
FIGURE COMES HERE.
Figure 2: (a) Interpolation with geodesic segments connecting speci ed positions; (b) maximally smooth interpolating trajectory computed from the metric (40); (c) spline interpolation in the space of coordinates; (d) maximally smooth interpolation trajectory computed from the metric (43). In the gure, the three prescribed positions are represented by a shaded rectangle. Figure 2.a shows a trajectory composed of geodesics (with the metric (40)) between the consecutive positions. Obviously, this trajectory is not smooth at the intermediate point. Further, it does not satisfy the boundary conditions on the velocity. Figure 2.b shows the maximally smooth trajectory computed from the necessary conditions (47). The translational part of the motion in this case is given by the cubic spline in IR2, as shown in Eq. (47). Since the metric used to nd this motion is left invariant but not bi-invariant, the motion is independent of the choice of the inertial reference frame fFg, but it does depend on the choice of the body- xed reference frame fMg. The next gure, 2.c, shows the trajectory computed by interpolation in the coordinates and with the use of exponential map. As shown in Section 3.1, this trajectory is bi-invariant. Finally, the trajectory shown in Figure 2.d is the maximally smooth trajectory for the metric (43) and was computed from Eq. (48). Since the metric is bi-invariant, so are the maximally smooth trajectories. The last two trajectories are similar, but they are not the same. Also note that in the planar case, the rotational part of Eq. (47) reduces to:
!(3) = 0; which implies that the solution will be a cubic spline. In other words, in the planar case the rotational parts of the motions obtained by any of the above schemes are the same. Figure 3 shows an example of spatial trajectories for a solid block. As before, the body- xed frame fMg was attached at the center of mass of the block with the axes parallel to the sides of the block. Let be the vector of coordinates de ned in Eq. (12) describing the position and 21
FIGURE COMES HERE.
Figure 3: (a) Maximally smooth interpolating trajectory computed from metric (40); (b) spline interpolation in the space of coordinates; (c) maximally smooth interpolating trajectory computed from metric (43); (d) curve obtained by combining the rotational part in (b) with the translational part in (a). orientation of the frame fMg and let V = f! T ; v T g be the instantaneous twist for the motion. The interpolation problem for the example in Figure 3 was given by:
1 = f0:20; ?0:10; 0:40; 1:24; 0:88; 0:85g 2 = f?0:51; 0:17; ?0:15; 11:54; 5:79; ?18:60g 3 = f1:75; ?0:40; 0:24; 28:36; ?8:56; ?35:69g
t1 = 0 t2 = 21 t3 = 1
V1 = f0; 0; 10; 0; ?10; ?10g V2 = f0; ?10; 0; 0; ?10; 0g
The same three interpolation schemes as above were used to compute the trajectories. The maximally smooth trajectory computed from the metric (40) is shown in Figure 3.a. This trajectory does not depend on the choice of the inertial reference frame fFg, but it does depend on the choice of the body- xed frame fMg. The translational part of the trajectory is exactly the cubic spline in IR3 , as can be seen from Eq. (47). Figure 3.b shows the trajectory computed by the method from Section 3.1, and the maximally smooth trajectory computed from the metric (43) is shown in Figure 3.c. These two trajectories are both bi-invariant, but in both cases the translational part of the motion is more curved compared to the motion in Figure 3.a. It is also interesting to combine the rotational part of motion in Figure 3.b with the translational part of motion in Figure 3.a. In other words, we separately compute the rotational motion on the group of rotations SO(3) using the method from Section 3.1 adapted to SO(3) and the translational motion on IR3 given by the cubic spline. The resulting motion is shown in Figure 3.d. The motion seems almost identical to the maximally smooth motion in Figure 3.d. A closer look reveals that the rotational parts are not the same: a cubic spline is in general not a solution for the rotational part of (47) (for some special cases in which this is true see [22]). However, 22
the similarity indicates that the cubic curve might be an approximation of the solution of this equation (see also [17]). Finally, note that the trajectory in Figure 3.d obtained by interpolating separately in SO(3) and IR3 is not bi-invariant.
5 Concluding remarks We discussed di erent methods for interpolation between a given set of positions and orientations. A typical approach to the problem is to interpolate the orientations and the positions separately and then combine the resulting interpolating curves. We show that by viewing the problem as an interpolation problem on SE (3), we can investigate several properties of the trajectories. In particular, we can systematically analyze the invariance of the interpolating curves with respect to the choice of the inertial and the body- xed frames and the smoothness properties of the trajectories. We proposed two di erent interpolation schemes: 1. The given set of elements of SE (3) is represented by appropriate coordinates (de ned in Equation (12) and interpolation with cubic splines is performed on these coordinates by treating them as vectors in the Euclidean space. The exponential map is then used to obtain the interpolating trajectories on SE (3) and these trajectories are bi-invariant. 2. Given a suitable measure of smoothness the methods of variational calculus on SE (3) are used to compute the maximally smooth trajectories. To demonstrate the method we computed the trajectories that are a generalization of cubic splines from the Euclidean case. The advantage of the rst scheme is that the trajectories are easy to compute. The second scheme is attractive since it produces the \best possible" interpolating trajectories, but the solution must be computed numerically. In this paper, the trajectories are obtained by solving a boundary value problem with a nite-di erence method. We computed the interpolating trajectories for a case when the motion takes place in a plane and for a spatial case. In both cases, the maximally smooth motions computed from the metric (40) appear to be the most natural. However, these trajectories must be computed numerically and they depend on the choice of the body- xed reference frame. The examples also suggest that these motions might be well approximated with the trajectories that are obtained by separate 23
interpolation on SO(3) and IR3 with the rst method. An interesting direction for future work would be to compute the estimates on the di erence between trajectories obtained in this way and the optimal trajectories computed using the methods of variational calculus.
Acknowledgment We thank Professor Chris Croke, Department of Mathematics, University of Pennsylvania, for many invaluable discussions on metrics and screw motions. It was our joint work [24] that eventually led to the research published here. We are also grateful for the nancial support provided by the NSF grants MIP 94-20397, MSS 91-57156 and CISE/CDA 88-22719, and the ARO MURI Grant DAAH04-96-1-0007.
References [1] F. Bullo and R. Murray. Proportional derivative (pd) control on the Euclidean group. In 1995 European Control Conference, Rome, Italy, 1995. [2] 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. [3] J. Cheeger and D. G. Ebin. Comparison Theorems in Riemannian Geometry. North-Holland Publishing Company, Amsterdam, 1975. [4] 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. [5] M. P. do Carmo. Riemannian geometry. Birkhauser, Boston, 1992. [6] G. E. Farin. Curves and surfaces for computer aided geometric design : a practical guide. Academic Press, Boston, 3 edition, 1992. [7] Q. J. Ge and B. Ravani. Computer aided geometric design of motion interpolants. ASME Journal of Mechanical Design, 116:756{762, 1994. [8] Q. J. Ge and B. Ravani. Geometric construction of Bezier motions. ASME Journal of Mechanical Design, 116:749{755, 1994. 24
[9] J. Hoschek and D. Lasser. Fundamentals of Computer Aided Geometric Design. AK Peters, 1993. [10] B. Jutler. Visualization of moving objects using dual quaternion curves. Computers and graphics, 18(3):315{326, 1994. [11] A. Karger and J. Novak. Space Kinematics and Lie Groups. Gordon and Breach Science Publishers, 1985. [12] M.-J. Kim, M.-S. Kim, and S. Y. Shin. A general construction scheme for unit quaternion curves with simple high order derivatives. In ACM Siggraph'95, pages 369{376, Los Angeles, CA, 1995. [13] J. Loncaric. Normal forms of sti ness and compliance matrices. IEEE Journal of Robotics and Automation, RA-3(6):567{572, 1987. [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 R. W. Brockett. Kinematic dexterity of robotic mechanisms. International Journal of Robotics Research, 13(1):1{15, 1994. [17] 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. [18] 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. [19] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery. Numerical Recipes in C. Cambridge University Press, Cambridge, 1988. [20] B. F. Schutz. Geometrical Methods of Mathematical Physics. Cambridge University Press, Cambridge, 1980. [21] K. Shoemake. Animating rotation with quaternion curves. ACM Siggraph, 19(3):245{254, 1985. 25
[22] M. Z efran. Continuous methods for motion planning. PhD thesis, U. of Pennsylvania, Philadelphia, PA, 1996. [23] M. Z efran and V. Kumar. Planning of smooth motions on SE (3). In Proceedings of 1996 International Conference on Robotics and Automation, pages 121{126, Minneapolis, MN, April 1996. [24] 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.
26
List of Figures 1 2
3
The inertial ( xed) frame, fFg, and the moving frame, fMg, attached to the rigid body. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 (a) Interpolation with geodesic segments connecting speci ed positions; (b) maximally smooth interpolating trajectory computed from the metric (40); (c) spline interpolation in the space of coordinates; (d) maximally smooth interpolation trajectory computed from the metric (43). . . . . . . . . . . . . . . . . . . . . . . 21 (a) Maximally smooth interpolating trajectory computed from metric (40); (b) spline interpolation in the space of coordinates; (c) maximally smooth interpolating trajectory computed from metric (43); (d) curve obtained by combining the rotational part in (b) with the translational part in (a). . . . . . . . . . . . . . . 22
27
Computer-Aided Design, Z efran & Kumar, Figure 1
{M} t3
x’ {M}
z’
y’
z’
y’
{M} t2 {F}
x’
t1
z
z’
x’
y x
28
y’
Computer-Aided Design, Z efran & Kumar, Figure 2
y
y 14 12 10 8 6 4 2 0
5
10
15
20
25
14 12 10 8 6 4 2 0
x
a)
10
15
20
25
5
10
15
20
25
x
b) y
y 14 12 10 8 6 4 2 0
5
5
10
15
20
25
14 12 10 8 6 4 2 0
x
c)
d)
29
x
Computer-Aided Design, Z efran & Kumar, Figure 3
y 0
5
15 10
0
10
y
x 20
30
0
0
5
15 10
0
10
x 20
30
0
z
z
-10
-10
-20
-20
a)
b)
y 0
5
15 10
0
10
y
x 20
30
0
0
0
z
z
-10
-10
-20
-20
c)
d)
30
5
15 10
0
10
x 20
30
Biographies Milos Z efran
received B.E. and M.Sc. degrees in Electrical Engineering and a B.Sc. degree in Mathematics from the University of Ljubljana, Slovenia in 1989, 1992, and 1992, respectively. He received a Ph.D. degree in Computer Science from the University of Pennsylvania. From 1993 to 1995 he was a graduate fellow of the Institute for Research in Cognitive Science at the University of Pennsylvania. He is currently a NSF Postdoctoral Fellow at the California Institute of Technology. His research interests include robotics, optimal control, and control of hybrid systems.
Vijay Kumar received his M.Sc. and Ph.D. in Mechanical Engineering from The Ohio State
University in 1985 and 1987 respectively. He has been on the Faculty in the Department of Mechanical Engineering and Applied Mechanics at the University of Pennsylvania since 1987. He is currently an Associate Professor and also holds a secondary appointment in the Department of Computer and Information Science. He has served on the editorial board of the IEEE Transactions on Robotics and Automation. He is currently on the Editorial Board of the Journal of Franklin Institute and an Associate Editor of the ASME Journal of Mechanical Design. He is the recipient of the 1991 National Science Foundation Presidential Young Investigator award. His research interests include robotics, mechanisms, dynamics and control.
31
