Lie theory, Riemannian geometry, and the dynamics of coupled rigid bodies

Transcription

1 Z. angew. Math. Phys. 51 (2000) /00/ $ /0 c 2000 Birkhäuser Verlag, Basel Zeitschrift für angewandte Mathematik und Physik ZAMP Lie theory, Riemannian geometry, and the dynamics of coupled rigid bodies F. C. Park and M. W. Kim Abstract. In this article we formulate, in a Lie group setting, the equations of motion for asystemofncoupled rigid bodies subject to holonomic constraints. A mapping f : M Nis constructed, where M is the m-dimensional configuration manifold of the system, and N = SE(3) SE(3) (n copies) is endowed with the left-invariant Riemannian metric h corresponding to the total kinetic energy of the system, where SE(3) is the special Euclidean group. The generalized inertia tensor of the system is given by the pullback metric f h;the equations of motion are then the geodesic equations on M with respect to this metric. We show how this coordinate-free formulation leads directly to a factorization of the generalized inertia tensor of the form S T L T HLS, wheresis a constant block-diagonal matrix consisting only of kinematic parameters, H is a constant block-diagonal matrix consisting only of inertial parameters, and L is a block lower-triangular matrix composed of Adjoint operators on se(3). Such a factorization is useful for various multibody system dynamics applications, e.g., inertial parameter identification, adaptive control, and design optimization. We also show how in many practical situations N can be reduced to a submanifold, thereby considerably simplifying the derivation of the equations of motion. Our geometric formulation not only suggests ways to choose the best coordinates for analysis and computation, but also provides high-level insight into the structure of the equations of motion. Keywords. Dynamics, rigid body, Euclidean group, multibody system. 1. Introduction The ideas and methods of modern differential geometry have found wide application in classical mechanics, particularly in the theory of rigid body motion, and the Lagrangian and Hamiltonian formalisms of mechanics. The classical books of [2] and [3] show, among other things, how the Euler equations for a rotating rigid body can be viewed as the geodesic equations for a left-invariant Riemannian metric on SO(3), and how the equations of motion for multibody systems can be formulated using variational principles on Riemannian manifolds. Needless to say these viewpoints have been quite profitable in furthering our understanding of multibody systems; see, e.g., [8] for a discussion on the geometry of mechanical systems, and [6] for further examples on applications of differential geometry to

2 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies 821 multibody systems analysis. One of the aims of this present paper is to extend the geometric formalism for single rigid body dynamics as discussed in [2] to the case of coupled rigid bodies subject to holonomic constraints. Using the machinery of Lie groups and Riemannian geometry, we present a coordinate-free formulation of the dynamics of such systems. One result is a characterization of the generalized inertia tensor of the system as a certain pullback metric of the map f : M N,whereMis the configuration space manifold, and N = SE(3) SE(3) (n copies, where n is the total number of rigid bodies comprising the system) is a Riemannian manifold with the left-invariant Riemannian metric corresponding to the total kinetic energy. The equations of motion are then given by the geodesic equations on M with respect to the pullback of this metric by f. That the generalized inertia in M is given by the pullback metric in itself is not a profound insight (although surprisingly we have yet to find this explicitly mentioned anywhere in the mechanics literature). One of the contributions of this article is that for the case of coupled rigid bodies, the special structure of the map f, in combination with the pullback metric characterization of the generalized inertia tensor, can be exploited to construct both high-level factorizations and efficient computational formulas for the generalized inertia tensor, and thus the dynamics. In particular, the effect of local coordinate and reference frame transformations on the equations of motion now becomes immediately transparent; one now has detailed insight into how these choices affect both the symbolic and computational complexity of the governing equations. Based on this geometric perspective, a second contribution of this paper is that, in many practical situations, N can be reduced to one of its submanifold. Given such a submanifold of N,sayE,f:M N can then be expressed as the composition f = ψ φ, whereψ:e Nis the natural lifting, and φ : M E. The generalized inertia tensor is then given by the pullback of the metric ψ h by φ, i.e., φ (ψ h). This notion can be generalized even further, by choosing E to be an arbitrary manifold of dimension lower than N, and requiring φ and ψ to be locally 1-1. By carefully choosing an appropriate E, the derivation of the equations of motion can be simplified considerably. We illustrate this approach with both open and closed chain examples. In terms of related literature, Rodriguez et al [7] were the first to point out the existence of a factorization for the generalized inertia tensor. Their approach relies on some specialized results from the theory of discrete-time Kalman filtering theory, and also on an open chain dynamics formulation of Featherstone [3] that is based on classical screw theory; both of these results rely on a local coordinate formulation of the kinematics. In previous work by the author [5] the factorization was duplicated for open chains within a Lie theoretic framework, but again the use of local coordinates unnecessarily complicated the derivation and statement of the results. In fact, most existing approaches to multibody system dynamics are based on

3 822 F. C. Park and M. W. Kim ZAMP local coordinate-based formulations that can become quite unwieldy; they often use their own set of ad hoc notation and definitions, as well as a specific choice of local coordinates for M and N. Not surprisingly, these formulations provide very little insight into the high-level structure of the equations of motion, nor the effect of coordinate transformations on equation complexity. One of the viewpoints put forth in this paper is that coordinate-free methods not only shed light on the qualitative structure of the equations of motion, but also provide the most insight into choosing coordinates for analysis and computation. The present work begins with a review of the geometry of SE(3) and its Lie algebra se(3), followed by a compact formulation of the equations of motion for a single rigid body in terms of standard linear operators on se(3). We then present a coordinate-free definition of the generalized inertia tensor for a system of coupled rigid bodies the kinematic chains can now contain an arbitrary number of closed loops. We show how the special structure of the map f leads to an explicit factorization of the generalized inertia tensor. The effects of local coordinate and link reference frame transformations on the equations of motion are also discussed. We then present the method for efficiently deriving the symbolic equations of motion based on reducing N to an appropriate submanifold. Open and closed chain examples illustrate the main features of our geometric approach. 2. Geometry of SE(3) SE(3) and se(3) Suppose an inertial frame and length scale for physical space have been chosen, and consider a rigid body with a body-fixed reference frame attached. The configuration of the rigid body can then be described by an element of the special Euclidean group SE(3). SE(3) admits the matrix representation [ ] R p, (1) 0 1 where R SO(3) and p R 3. For notational convenience elements of SE(3) will also be denoted by the pair (R, p). Its Lie algebra se(3) admits the matrix representation [ ] [ω] v, (2) 0 0 where [ω] = 0 ω 3 ω 2 ω 3 0 ω 1. (3) ω 2 ω 1 0 is an element of so(3), and v R 3. denoted by the pair (ω, v). Elements of se(3) will also sometimes be

4 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies 823 Suppose G is a matrix Lie group with matrix Lie algebra g. Then recall that elements of a Lie algebra can also be identified with a certain linear mapping via the Lie bracket. Given an element V g, its adjoint representation is given by the linear map ad V : g g defined by ad V (V )=VV V V. This is simply the Lie bracket between V and V, which on matrix Lie algebras is given by the matrix commutator. If V =(ω, v) andv =(ω,v ) are elements of se(3), then a simple calculation reveals that which also admits the matrix representation ad V (V )=(ω ω,ω v ω v), (4) [ ][ ad V (V [ω] 0 ω )= [v] [ω] v ]. (5) Similarly, the matrix representation of the dual operator ad V :se(3) se(3) is given by its transpose: if F =(M,F) is an element of se(3),then [ ][ ] ad [ω] [v] M V (F) =. (6) 0 [ω] F Recall also that for every G G the adjoint map Ad G : g g is defined by Ad G (g) =GgG 1.IfG=(R, p) is an element of SE(3), then its adjoint map Ad G acting on an element V =(ω, v) of se(3) is given by which also admits the 6 6 matrix representation Ad G (V )=(Rω, p Rω + Rv), (7) [ ][ ] R 0 ω Ad G (V )=. (8) [p]r R v The dual operator Ad G :se(3) se(3) also has a matrix representation (with respect to the standard dual basis on se(3) ) given by the transpose of Ad G : [ Ad R G (F) = T R T [p] T 0 R T ][ M F ]. (9) The general formulas Ad 1 G =Ad G 1 and Ad GAd H =Ad GH for any G, H G will also be useful in what follows.

5 824 F. C. Park and M. W. Kim ZAMP 2.2. Generalized velocities and generalized forces Recall that the tangent vector Ġ(t) of a curve G(t) =(R(t),p(t)) in SE(3) can be identified with an element of se(3) by either left or right translation: if G(t) describes the motion of a rigid body relative to an inertial frame, then ĠG 1 = (ṘR 1, ṗ ṘR 1 p)andg 1 Ġ=(R 1 Ṙ, R 1 ṗ) are both elements of se(3). Observe that R 1 Ṙ and R 1 ṗ are the angular and linear velocities of the rigid body relative to its body-fixed frame, respectively; for this reason we call G 1 Ġ the generalized velocity in the body. Similarly, ĠG 1 is referred to as the generalized velocity in space. Under a change of reference frame, generalized velocities transform according to the adjoint mapping Ad. That is, consider a rigid body whose body-fixed frame relative to the inertial frame is given by G(t). Then under a change of inertial frame, G(t) transforms according to G (t) =CG(t) for some constant C SE(3), and Ġ G 1 = Ad C (ĠG 1 ), while G 1 Ġ = G 1 Ġ. Similarly, under a change of body-fixed frame, G(t) transforms according to G (t) =G(t)Cfor some constant C SE(3), and G 1 Ġ = Ad 1 C (G 1 Ġ), while Ġ G 1 = ĠG 1. Generalized forces, in contrast, are elements of se(3), and under a change of reference frame they transform according to the dual adjoint map. Specifically, let F =(M,F) be the moment-force pair acting on a rigid body about the origin of its body frame, expressed in body frame coordinates. Suppose this frame is now relocated to another point on the rigid body, so that the new body frame relative to the old body frame is given by the displacement H =(R, p) SE(3). Then the moment-force pair F =(M,F ) applied about the origin of this new frame is given in the new body-fixed coordinates by [ ] [ M R T R F = T [p] T 0 R T or F =Ad H(F) Equations of motion for a single rigid body ][ ] M, (10) F The key to formulating the equations of motion for a rigid body as a geodesic in SE(3) is to construct the left-invariant Riemannian metric corresponding to kinetic energy. Arnold [2] illustrates the procedure for SO(3); this can be easily extended to SE(3). First, given a rigid body let V =(ω, v) denote its generalized velocity in the body frame attached at the center of mass. Let A and m be the inertia tensor and mass of the rigid body, respectively, so that the kinetic energy is given by 1 2 ωt Aω+ m 2 v T v. The kinetic energy is therefore given by a left-invariant Riemannian metric that on se(3) assumes the following 6 6 symmetric positive-definite quadratic form: [ ] A 0 J = (11) 0 mi

6 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies 825 where I R 3 3 is the identity. J can also be regarded as a linear map J :se(3) se(3) ; JV is the generalized momentum of the rigid body, which tensorially is an element of se(3). The equations of motion for a rigid body can now be expressed succinctly as J V ad V (JV )=F (12) Here J is the generalized inertia of the body, V its generalized velocity in the body frame and V its derivative, and F the resultant generalized force acting on the body. 3. Generalized inertia tensor as a Pullback metric Given an m-dimensional holonomic system of n coupled rigid bodies in space, let (q 1,...,q m ) denote generalized coordinates for its configuration manifold M. Note that M may possess boundaries and other singular points depending on the system s physical structure. Ignoring potential forces, the equations of motion can be written m m m τ k = g kj (q) q j + Γ ijk (q) q i q j (13) j=1 i=1 j=1 for k = 1,...,m, where the τ k are the external applied inputs, g kj (q) isthe generalized inertia tensor, and Γ ijk are the Christoffel symbols (of the first kind) relative to g ij : Γ ijk = 1 2 ( gkj q i + g ki q j g ) ij q k with Γ ijk =Γ jik. In this section we show how g ij can be regarded as the pullback, via a map to be constructed below, of the left-invariant Riemannian metric corresponding to the total kinetic energy of the system. We first assume that a length scale for physical space has been chosen, and an inertial frame selected. We also assume that body frames have been attached to each of the n rigid bodies comprising the system. Define the manifold N = SE(3) SE(3) (n copies). Let h i denote the left-invariant Riemannian metric corresponding to the kinetic energy of rigid body i, and define the Riemannian metric h on N as the direct tensor sum (14) h = h 1 h 2... h n (15) h is then simply the left-invariant Riemannian metric corresponding to the sum of the kinetic energies of each rigid body. Define f 0i : M SE(3) to be the map that locates the body frame of the ith rigid body relative to the inertial frame, and define f : M N by f(p) =(f 01 (p),f 02 (p),...,f 0n (p)) (16)

7 826 F. C. Park and M. W. Kim ZAMP With the metric h defined as above, we now define a metric g on M by pulling back h via f: g = f h (17) The generalized inertia tensor g ij (q) is then this pullback metric expressed in local coordinates q on M. Given a choice of local coordinates on N, one can then express the pullback metric explicitly in coordinates as G(q) =P T (q)h(f(q))p(q) (18) where P(q) is the matrix representation for the derivative of f, andh(f)isthe matrix representation for the metric h on N. In the next section we explicitly derive this factorization by taking advantage of the special structure of the map f. 4. Factorizations of the generalized inertia tensor 4.1. Open chains We first consider rigid bodies connected serially that contain no closed loops, i.e., an open kinematic chain. Label the rigid bodies, or links, in the chain from 1 to n. Each link in the chain is connected to its preceding link by a one degree-of-freedom joint, in which case the kinematic degrees of freedom are equal to the number of rigid bodies (m = n). One can also assume without loss of generality that M is flat, i.e., locally isometric to R n. Take the local coordinates (q 1,...,q n )tobethe joint variables, where q i represents the joint connecting links i 1andi(link 0 is the inertial frame). Let H i denote the quadratic form on se(3) from which the left-invariant Riemannian metric h i on SE(3) is obtained; it has the same matrix representation as (11), and prescribes the kinetic energy of link i. Define f ij to be the displacement of the link j frame relative to the link i frame; from the definition f 1 ij = f ji,and f ii = identity. Then for open chains the following relation holds: f 0k (q 1,...,q k )=f 01 (q 1 )f 12 (q 2 )f 23 (q 3 ) f k 1,k (q k ) (19) for k =1,...,n,whereeachf i 1,i is purely a function of the i-th joint coordinate q i. Also, let V ij denote the generalized velocity in the body of the link j frame, relative to the link i frame. We now derive the total kinetic energy of the system in terms of the above quantities. First, observe that V i 1,i = f 1 i 1,i f i 1,i (20) Since links i 1andiare connected by a one degree-of-freedom joint q i, it follows that V i 1,i = S i q i (21)

8 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies 827 for some constant S i se(3). After some calculation, it can be shown that V 0k = k Ad fki (V i 1,i ) (22) i=1 k = Ad fki (S i ) q i (23) i=1 Denoting by, the standard Euclidean inner product, the total kinetic energy of the system can then be written kinetic energy = 1 n V0k T 2 kv 0k k=1 (24) = 1 n k k Ad fki (S i ) q i,h k Ad fkj (S j ) q j 2 k=1 i=1 j=1 (25) from which it follows that the generalized inertia tensor g ij on M is given by g ij = n Ad fki (S i ),H k Ad fkj (S j ) (26) k=j for j i, andg ij = g ji. The metric g ij also admits the following matrix factorization. First, assume S i se(3) is now expressed as a six-dimensional column vector, and denote by (Ad G )the6 6 matrix representation of the adjoint mapping Ad G, as given in (8). The matrix G =(g ij ) can then be written as G = S T L T HLS (27) where is a constant 6n n matrix, S =diag{s 1,...,S n } (28) H =diag{h 1,...,H n } (29) is a constant 6n 6n matrix, and I 0 0 (Ad f21 ) I 0 L = (Ad fn1 ) (Ad fn2 ) I (30)

9 828 F. C. Park and M. W. Kim ZAMP is a 6n 6n lower-triangular matrix that depends on q. The metric g can be obtained as the pullback of h via the map f : M N defined by f(q) =(f 01 (q 1 ),f 02 (q 2 ),...,f 0n (q n )) (31) In fact, f can be expressed as the composition of 2 maps: f = ψ φ, where φ : M N and ψ : N Nare defined as follows: φ(q) =(f 01 (q 1 ),f 12 (q 2 ),...,f n 1,n (q n )) (32) ψ(x 1,...,X n )=(X 1,X 1 X 2,...,X 1 X n 1 X n ) (33) From the definitions it can be easily verified that indeed f = ψ φ. The metric g is then obtained by pulling back h via ψ, thenφ. The matrix factorization of G above is simply a coordinate expression of this fact. The results of this section can also be straightforwardly modified to handle tree structure chains with multi-degree-of-freedom joints between the links Closed chains For chains with closed loops M typically will not be flat; the coordinate-free point of view is especially useful for such problems. Suppose the chain consists of n rigid bodies interconnected by r joints, and has a total of m kinematic degrees of freedom. Suppose joint j has k j degrees of freedom, and let k = r i=1 k i;for closed chains k m. M can then be regarded as an m-dimensional submanifold of a k-dimensional ambient space manifold E. Denote local coordinates on M and E by q and φ, respectively. As before let N be n copies of SE(3). Similar to the previous section, the map f : M N is also expressed as the composition of two maps, φ and ψ: f = ψ φ, whereφ:m E and ψ : E N are both smooth and locally 1-1. The generalized inertia tensor on M is obtained as before by pulling back the left-invariant kinetic energy Riemannian metric on N to M. Sincef=ψ φ, we apply the chain rule to compute the left differential of f: f 1 df q = ψ 1 (φ(q)) dψ φ(q) dφ q (34) Let H be the matrix representation of the Riemannian metric on N as before, and let Ψ denote the matrix representation for the left differential ψ 1 dψ. Also, denote the matrix representation for dφ ψ(q) by Φ. Then the generalized inertia tensor G =(g ij ) can be factorized as G =Φ T Ψ T HΨΦ (35)

10 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies Coordinate transformations Suppose in the above formulation that the link reference frames are relocated to other points on the link; this has the effect of left or right translating the maps f ij by constant elements of SE(3). More importantly, certain choices of link reference frames may simplify the computation of the generalized inertia tensor, and hence the equations of motion. To make matters more concrete let Q i SE(3) denote the transformation from the old link i reference frame to the new link i reference frame. Define the block-diagonal matrix Q = R 6n 6n by Q =diag{ad Q1, Ad Q2,...,Ad Qn } (36) with each Ad Qi the 6 6 representation of the Adjoint map. In this case the factorization of the generalized inertia tensor undergoes the following transformation: first, define the matrices S = QS (37) L = QLQ 1 (38) H = Q T HQ 1 (39) It is then easy to see that the generalized inertia tensor can be expressed as G = S T L T HLS (40) = S T L T H L S (41) Again, in the above factorization S and H are constant block matrices that respectively contain only the kinematic and inertial parameters of the system in question. The structure of S, L, andhare also preserved. For example, S = diag { S 1,S 2,... n},s,whereeachs i =Ad Qi (S i ). Similarly, H =diag{h 1,H 2,...,H n},whereeachh i =(Ad 1 Q i ) T H i (Ad 1 Q i ). The corresponding transformation rule for the ij-th block element of L is given by L ij =(Ad Q i )L ij (Ad 1 Q j ) (42) for i<j(l is also lower-triangular). The above transformation rules are just a reconfirmation of the tensorial properties of each of the elements: the S i transform as vectors, H i transforms as an inner product (or covariant 2-tensor), while L ij transforms as a linear operator (or a 1-1 tensor). Based on the above one can now attempt to simplify the equations of motion by an appropriate choice of Q; for example, the Q that maximizes the number of zero terms in S is one possibility. Whatever the choice of Q, the equations of motion can always be computed recursively. In the case of an n-link open chain, the algorithm for computing the i-th input joint torque (or force) τ i foragiven desired motion is given by

11 830 F. C. Park and M. W. Kim ZAMP Initialization V 0 = V 0 = F n+1 = 0 (43) Forward iteration: for i =1to n do V i =Ad fi,i 1 (V i 1 )+S i q i (44) V i = S i q i +Ad fi,i 1 ( V i 1 )+ad Adfi,i 1 (V i 1 ) (S i q i ) (45) Backward iteration: for i = n to 1 do F i =Ad f i+1,i (F i+1 )+H i Vi ad V i (H i V i ) (46) τ i = S i, F i (47) One final remark is that one is not necessarily restricted to the particular blockdiagonal form of Q suggested above. Indeed, since the two factorizations for G above hold for any nonsingular Q R 6n 6n, by enlarging the class of admissible Q it is possible that the equations of motion can even be simplified further (although in this case the above recursive algorithm would have to be modified accordingly). 6. The reduction technique The reduction technique can essentially be viewed as a generalization of the formulation developed for the factorization of the generalized inertia tensor for closed chains; in what follows we adopt the same notation for consistency. Suppose f : M N can be expressed as the composition of two maps φ and ψ: f = ψ φ, where φ : M Eand ψ : E Nare both smooth and locally 1-1, and E is a differentiable manifold of dimension k, k n. The generalized inertia tensor on M is obtained as before by pulling back the left-invariant kinetic energy Riemannian metric on N to M. Sincef=ψ φ,we apply the chain rule to compute the left differential of f: f 1 df q = ψ 1 (φ(q)) dψ φ(q) dφ q Let H be the matrix representation of the Riemannian metric on N as before, and let Ψ denote the matrix representation for the left differential ψ 1 dψ. Also, denote the matrix representation for dφ ψ(q) by Φ. Then the generalized inertia tensor G =(g ij ) can be factorized as G =Φ T Ψ T HΨΦ While the equation is identical in form to that of the generalized inertia tensor for closed chains, the interpretation is different: whereas E was previously regarded as an ambient manifold containing M as a submanifold, here it is more natural to regard E as a submanifold of N. Ψ T HΨ is then the metric on E obtained as the pullback of h by ψ. An appropriate choice of E, when guided by intuition and experience, can considerably simplify the derivation of the generalized inertia tensor, and hence the equations of motion. We illustrate this technique with several examples below.

12 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies Examples Example 1: Dynamics of a 2R spatial open chain To illustrate the geometric formulation, consider first a 2R open chain example. Suppose reference frames are attached to the center of mass of each link. The positions of the two link reference frames relative to the inertial frame can be written as functions of the joint values q 1 and q 2 : f 01 = e S 1q 1 M 1 (48) f 02 = e S 1q 1 e S 2q 2 M 2 (49) where S 1,S 2 se(3) and M 1,M 2 SE(3) are all constant. Each S i =(ω i, ω i r i ) can in turn be obtained by first setting both joint values q 1 and q 2 to zero, and defining ω i to be a unit vector in the direction of the i-th joint axis, and r i to be an arbitrary point on the i-th joint axis. Both are expressed in terms of inertial frame coordinates. Similarly, the M i correspond to the locations of the i-th link reference frames when q 1 = q 2 =0. Neglecting gravity, the dynamic equations can be written in the form τ = M(q) q + C(q, q) q. The entries of M(q) can be expressed as m 11 = S 1,H 1 S 1 + Ad e S 2 q 2 (S 1 ),H 2 Ad e S 2 q 2 (S 1 ) (50) m 12 = Ad e S 2 q 2 (S 1 ),H 2 S 2 (51) m 22 = S 2,H 2 S 2 (52) where each H i is the 6 6 generalized inertia matrix of link i of the form H i = [ Ai 0 ] 0 m i I (53) where m i is the mass of link i, anda i denotes its 3 3 inertia matrix with respect to the chosen link reference frame. The entries of C(q, q) can also be written as c 11 = Ad e S 2 q 2 (ad S1 (S 2 )),H 2 Ad e S 2 q 2 (S 1 ) q 1 q 2 (54) c 12 = c 11 + Ad e S 2 q 2 (ad S1 (S 2 )),H 2 S 1 q 2 q 2 (55) c 21 = Ad e S 2 q 2 (ad S1 (S 2 )),H 2 Ad e S 2 q 2 (S 1 ) q 1 q 1 (56) c 22 = 0 (57)

13 832 F. C. Park and M. W. Kim ZAMP Example 2: A 2R planar open chain using reduction Here we illustrate the reduction technique by deriving the generalized inertia tensor for a 2R planar open chain. Let the links be uniform, with link lengths 2l 1,2l 2 and moments of inertia a 1, a 2 about their centers of mass, respectively. Let q 1 and q 2 denote the joint variables. For this example M is the 2-torus T 2, while N =SE(2) SE(2). Define local coordinates (ψ 1,...,ψ 6 )onn in the following fashion: ψ 1 and ψ 4 are the orientations of links 1 and 2, respectively, and (ψ 2,ψ 3 )and(ψ 5,ψ 6 ) are the coordinates for the centers of mass of links 1 and 2, respectively. The metric H on N in terms of these coordinates is H = diag{a 1,m 1,m 1,a 2,m 2,m 2 }. Choose E to be the submanifold S 1 SE(2), with local coordinates (φ 1,φ 2,φ 3,φ 4 ), and let ψ : E N be the natural lifting defined by ψ 1 = φ 1, ψ 2 = l 1 cos φ 1, ψ 3 = l 1 sin φ 1, ψ 4 = φ 2, ψ 5 = φ 3, ψ 6 = φ 4. The pullback metric on E is then given in local coordinates by Ψ T HΨ=diag{ā 1,a 2,m 2,m 2 } (58) where ā 1 = a 1 + m 1 l1 2 is simply the moment of inertia of link 1 about the fixed pivot rather than its center of mass; note that this could have easily been obtained by simple reasoning. φ : M E is given in local coordinates by φ 1 = q 1 (59) φ 2 = q 1 + q 2 (60) φ 3 =2l 1 cos q 1 + l 2 cos(q 1 + q 2 ) (61) φ 4 =2l 1 sin q 1 + l 2 sin(q 1 + q 2 ) (62) The generalized inertia tensor is now given by Φ T Ψ T HΨΦ, where Φ= l 1 sin q 1 l 2 sin(q 1 + q 2 ) l 2 sin(q 1 + q 2 ) 2l 1 cos q 1 + l 2 cos(q 1 + q 2 ) l 2 cos(q 1 + q 2 ) (63) Different choices of E will, of course, lead to different factorizations of the generalized inertia tensor. With some experience, one can bypass the definition of N altogether, and immediately begin by selecting E together with the associated kinetic energy Riemannian metric.

14 Vol. 51 (2000) Geometry, and the dynamics of coupled rigid bodies 833 Example 3: A planar four-bar linkage As a final example of the reduction technique, we derive the equations of motion for a planar four-bar linkage. For simplicity let all the links be uniform and of unit length, with identical masses m and moments of inertia a. The input and output pivots are also spaced a unit length apart. Denote the input angle (i.e., the absolute orientation of the input link) by q 1, which we also choose to be the generalized coordinate for the system. For this system N =SE(2) SE(2) SE(2). Since the input and output links are constrained to rotate about their respective fixed pivots, and the coupler link always maintains a horizontal orientation, we can choose E to be the submanifold S 1 R 2 S 1. Denote local coordinates on E by (φ 1,...,φ 4 ), where φ 1 and φ 4 respectively represent the input and output angles, and (φ 2,φ 3 ) are the x-y coordinates for the coupler link mass center. The pullback metric on E is given by D =diag{ā, m, m, ā}, whereā=a+m/4 is just the moment of inertia of the link about its tip. The map φ : S 1 Eis given by φ 1 = q 1 (64) φ 2 =cosq 1 +1/2 (65) φ 3 =sinq 1 (66) φ 4 = q 1 (67) Finally, the generalized inertia tensor in terms of the generalized coordinate q 1 is given by Φ T DΦ, where Φ T =[1 sin q 1 cos q 1 1 ] (68) As before, other choices of E are possible, and will lead to different factorizations of the generalized inertia tensor. 8. Conclusions In this article we have presented a Riemannian geometric formulation of the dynamics of n coupled rigid bodies in terms of the Lie group SE(3) and its Lie algebra se(3). The main results are (i) a coordinate-invariant formulation of the equations of motion for a single rigid body using standard linear operators on se(3); (ii) a characterization of the generalized inertia tensor on the configuration space manifold M as the pullback, via a map f : M N,N =SE(3) SE(3) (n copies), of the left-invariant Riemannian metric determined by the kinetic energy; (iii) explicit factorizations of the generalized inertia tensor based on a decomposition of f, together with transformation rules under a change of local coordinates and link reference frames, (iv) an efficient recursive algorithm for computing the inverse

15 834 F. C. Park and M. W. Kim ZAMP dynamics, and (v) a reduction technique for deriving the equations of motion in an efficient manner. Our coordinate-free geometric approach not only shows how the choice of coordinates and reference frames affect the equations of motion vis-àvis their computation and analysis, but also provide high-level qualitative insight, as well as a novel technique for efficient derivation of the equations of motion. Acknowledgement The support of the International Cooperative Research Program of the Korea Science Foundation is gratefully acknowledged. References [1] R. Abraham and J. Marsden, Foundations of Mechanics. Benjamin, Reading, MA [2] V. Arnold, Mathematical Methods of Classical Mechanics. Springer, New York [3] R. Featherstone, Robot Dynamics Algorithms. Kluwer, Boston [4] F. C. Park, Optimal Kinematic Design of Mechanisms. Ph.D. Thesis, Applied Mathematics, Harvard University [5] F. C. Park, J. E. Bobrow, and S. R. Ploen, A Lie group formulation of robot dynamics, Int. J. Robotics Research 14 (1995), [6] G. Patrick, Two Axially Symmetric Coupled Rigid Bodies: Relative Equilibria, Stability, Bifurcations, and a Momentum Preserving Symplectic Integrator. Ph.D. Thesis, Mathematics, U.C. Berkeley [7] G. Rodriguez, A. Jain, and K. Kreutz-Delgado, A spatial operator algebra for manipulator modeling and control, Int. J. Robotics Research 10 (1991), [8] F. Takens, Symmetries, Conservation Laws, and Variational Principles. Springer (Lecture Notes 597), New York F. C. Park School of Mechanical and Aerospace Engineering Seoul National University Seoul Korea ( fcp@plaza.snu.ac.kr) M. W. Kim School of Electrical Engineering Korea University Seoul Korea ( mwkim@kuccnx.korea.at.kr) (Received: July 29, 1998; revised: June 7, 1999)