Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems San Diego, CA, USA, Oct 29 - Nov 2, 2007
ThB6.4
Lagrangian Dynamics of Open Multibody Systems with Generalized Holonomic and Nonholonomic Joints Vincent Duindam Dept. of EECS, University of California Berkeley, CA 94720, USA
[email protected] Abstract— Standard methods to model multibody systems are aimed at systems with configuration spaces isomorphic to Rn . This limitation leads to singularities and other artifacts in case the configuration space has a different topology, for example in the case of ball joints or a free-floating mechanism. This paper discusses an extension of classical methods to allow for a very general class of joints, including all joints with a Lie group structure as well as nonholonomic joints. The model equations are derived using the Boltzmann-Hamel equations and have very similar structure and complexity as obtained using classical methods, but they do not suffer from singularities. Furthermore, the equations are explicit differential equations (both for holonomic and nonholonomic joints) and can be directly implemented in simulation software.
I. I NTRODUCTION For analysis, design, and control of robotic and other complex mechanical systems, it is often crucial to obtain a good mathematical model of the dynamics of the mechanism. This model should reflect the essential properties of the mechanism, while still being simple enough for simulation and controller design. For many applications, it is sufficient to model these mechanisms as rigid multibody systems, meaning interconnections of perfectly rigid bodies by ideal joints without mechanical play. The assumption of rigidity implies that the spatial configuration space of the mechanism is a finite-dimensional space, often given the structure of a linear space isomorphic to Rn . To represent the configuration of the mechanism, one can then choose coordinates q ∈ Rn , and these coordinates are usually chosen to represent physically meaningful quantities, e.g. rotation angles for angular joints and elongation distances for prismatic joints. Given the inertial properties of the rigid bodies in the mechanism, the dynamics can then be described by the well-known equation [1] M (q)¨ q + C(q, q) ˙ q˙ + N (q) = B(q)τ
(1)
where M (q) is the strictly positive-definite mass matrix of the system, C(q, q) ˙ q˙ represents the Coriolis and centrifugal forces, N (q) the gravity forces, τ the vector of actuation torques, and B(q) the input mapping such that B(q)τ is collocated with q. ˙ To use (1), the configuration space is required to be adequately described by Rn . For most joint types, this is not a serious problem, though strictly speaking the configuration space of a rotational joint is a circle and not R1 . However, for more general joints, such as ball joints (three degrees
1-4244-0912-8/07/$25.00 ©2007 IEEE.
Stefano Stramigioli Dept. of EEMCS, University of Twente P.O. Box 217, Enschede, Netherlands
[email protected] of freedom) and ‘free motion’ joints (unconstrained motion with six degrees of freedom), the required representation of the configuration space as Rn leads to singularities and sensitivities that do not correspond to physical singularities and sensitivities. These artificial singularities, as opposed to inherent singularities e.g. in a Cardan joint, are solely due to the choice of coordinates and hence should be eliminated. In this paper, we present an extension of (1) to mechanisms with more general joints. This general class is defined more precisely in the next section, but it includes the important subclass of all joints for which the configuration space is a Lie group, such as ball joints and ‘free motion’ joints. The class also includes nonholonomic joints (such as rolling constraints), for which the configuration space has a higher dimension than the space of instantaneously allowed velocities. The resulting equations are explicit, singularity-free differential equations that differ only slightly from (1). Lie group theory has been used extensively to develop singularity-free representations of the dynamics of a system on a single Lie group, see for example [2], [3]. Systems with nonholonomic constraints have been reduced to explicit differential equation in various ways as well, in the context of geometric mechanics and Ehresmann connections [4], and in the context of port-Hamiltonian systems and Poisson brackets [5], [6]. Both holonomic and nonholonomic constraints have been treated in the framework of Kane’s equations [7], [8] and Boltzmann-Hamel equations [9], which use a type of quasi-coordinates as we do here, but are nevertheless restricted to joints with a Euclidean joint space. This paper, which is based partially on [10], presents a systematic method that is valid for general non-Euclidean holonomic and nonholonomic joints. The structure of this paper is as follows. Section II gives the precise definition of the types of joints that are considered, and demonstrates how several specific joints can be formulated in the required framework. Section III shows how the dynamics can be derived for multibody systems composed of such joints. Section IV concludes the paper. II. G ENERAL H OLONOMIC AND N ONHOLONOMIC J OINTS This section defines a general class of joints for which the dynamics can be derived by the method of Section III. Section II-A gives a brief summary of the theory of homogeneous transformations and twists, and then presents the definitions of general holonomic joints (Section II-B)
3342
Φ(Q1 , ·) Q1
B. Holonomic Joints
ϕ2
Definition 1 (Holonomic joint): A globally parameterized holonomic joint is a smooth k-dimensional sub-manifold Q ⊆ SE(3) that describes the permitted relative configurations Hji of body j relative to body i, and satisfies the following properties:
ϕ2
ϕ1
ϕ1
ϕ2 ϕ1
1) Every joint configuration is uniquely described by a coordinate matrix Q (of arbitrary but constant dimensions), and Hji = Hji (Q). We identify an abstract element of Q with its coordinate representation Q and write Q ∈ Q; 2) Every allowed joint velocity (an element of TQ Q) is uniquely described by a vector v ∈ Rk , and the joint twist can be expressed as Tji,i = X(Q)v with X(Q) ∈ R6×k a matrix describing the instantaneously allowed twists; 3) There exists a coordinate mapping Φ : Q × Rk → Q, such that for each Q ∈ Q, Φ(Q, 0) = Q and Φ(Q, ·) defines a local diffeomorphism between a neighborhood of 0 ∈ Rk and a neighborhood of Q ∈ Q.
Φ(Q2 , ·)
Q2 Fig. 1. The mapping Φ(Q, ϕ) assigns local Euclidean coordinates ϕ ∈ Rk to a neighborhood of each global coordinate Q ∈ Q.
and nonholonomic joints (Section II-C). The definitions are illustrated with examples of commonly encountered joints. A. Configuration and Velocities of Rigid Bodies We first briefly summarize the theory and notation used to represent configurations and velocities of rigid bodies. We refer to [1] and [11] for a more extensive background; in this paper, we follow the notation of [11]. The configuration of a rigid body in three-dimensional space, meaning its position and orientation relative to some reference frame, can be described by an element of the Lie group SE(3), the Special Euclidean group. Numerically, this configuration can be expressed by a 4 × 4 homogeneous matrix Hba of the form ¸ · a Rb pab a (2) Hb = 0 1
where Rba is a 3 × 3 orthogonal matrix with det(Rba ) = 1 that describes the orientation of the body, and pab is a 3 × 1 vector that describes the position of its origin. The indices a and b refer to two right-handed coordinate frames Ψa (the reference frame) and Ψb (rigidly attached to the body). The velocity of a rigid body, meaning both its linear and angular velocity, can be concisely described by an element of se(3), the Lie algebra of SE(3). Numerically, it can be expressed as a vector Tbc,a or matrix T˜bc,a of the form · c,a ¸ ¸ · c,a ωb ω ˜b vbc,a c,a c,a 6 ˜ Tb = c,a ∈ R or Tb := ∈ R4×4 vb 0 0
with ωbc,a , vbc,a ∈ R3 the angular and linear velocity, respectively, of the rigid body, and we define for every x ∈ R3 0 −x3 x2 0 −x1 (3) x ˜ := (x∧) = x3 −x2 x1 0
The twist Tbc,a denotes the relative velocity of frame Ψb with respect to Ψa , expressed in coordinate frame Ψc . It is related to the relative configuration (2) as T˜bc,a = Hac H˙ ba Hcb with H˙ denoting the time-derivative of H.
(4)
Fig. 1 illustrates the third property: every point of the allowed configuration space is assigned a unique (global) coordinate Q, and the mapping Φ assigns (local) Euclidean coordinates ϕ to a neighborhood of every Q. Although the requirement for the mapping Φ may seem restrictive, the class of joints that satisfy Definition 1 is actually quite large. First, classical joints with a Euclidean configuration space can be formulated in the form of the definition. If k is the number of degrees of freedom, the global configuration coordinate can be chosen as Q ∈ Rk , its velocity coordinate as v = Q˙ ∈ Rk , and the coordinate mapping as Φ(Q, ϕ) = Q + ϕ with ϕ ∈ Rk . Secondly, joints with a configuration space described by a Lie group (e.g. ball joints or ‘free motion’ joints) can also be formulated as in Definition 1. In this case, the coordinate matrix Q is the appropriate matrix representation of the Lie group, and the velocity coordinates are the coordinates for the algebra of the group. Finally, the coordinate mapping Φ can be chosen as the exponential coordinate mapping of the group, i.e. the mapping Pk
Φ(Q, ϕ) = Qe
i=1
bi ϕi
(5)
with bi the appropriate basis elements for the algebra. Indeed, Φ(Q, 0) = Qe0 = Q and the mapping assigns local coordinates ϕ to a neighborhood of every Q ∈ Q. Example 1 (Ball joint): Consider the ball joint shown in Fig. 2. We choose the configuration matrix Q to be the 3 × 3 rotation matrix that describes the relative orientation of frames Ψ1 and Ψ2 , such that their relative configuration satisfies ¸ · Q 0 1 1 ∈ R4×4 (6) H2 = H2 (Q) = 0 1 As the global velocity coordinates, we pick v ∈ R3×1 to be the angular velocity vector of frame Ψ2 relative to Ψ1 ,
3343
Qj
Ψi Qi
v1 Ψ2
z
Ψ2
y
Ψ1
v3
Q5
Ψ1 x
Ψj
Ψ5
v2
Q2 Q3
Fig. 2. Two examples of generalized joints between two bodies with attached frames Ψ1 and Ψ2 : a ball joint (left) and a planar nonholonomic joint (right).
expressed in Ψ1 . The relative twist between Ψ1 and Ψ2 then becomes T v1 1 0 0 0 0 0 T21,1 = X(Q)v = 0 1 0 0 0 0 v2 (7) v3 0 0 1 0 0 0
Finally, we can choose the coordinate mapping Ψ using exponential coordinates, i.e. as µ ¶ 1 1 Φ(Q, ϕ) = Qeϕ˜ = Q I + ϕ˜ + ϕ˜2 + ϕ˜3 + . . . (8) 2 6 with ϕ˜ := (ϕ∧) ∈ R3×3 as before.
C. Nonholonomic Joints Definition 2 (Nonholonomic joint): A globally parameterized nonholonomic joint is a globally parameterized holonomic joint together with a restriction on the allowed instantaneous velocities, given by an equation AT (Q)v = ¯ 0 for some differentiable matrix A(Q) ∈ Rk×(k−k) of ¯ In addition to the properties given in constant rank (k − k). Definition 1, there must exist a differentiable rank k¯ matrix ¯ ¯ ¯ S(Q) ∈ Rk×k such that AT (Q)S(Q) = 0 for all Q ∈ Q. Following the definition, a nonholonomic joint is basically a constrained holonomic joint, implicitly restricting ¯ the allowed velocities to a k-dimensional linear subspace. The columns of the matrix S¯ form a basis for the space of allowed velocities, or in other words, allowed velocities ¯v , automatically satisfying the constraint have the form v = S¯ T T ¯ A v = A S¯ v = 0. Example 2 (Planar nonholonomic joint): Fig. 2 (right) shows an example of a planar holonomic joint. The relative motion of the two frames Ψ1 and Ψ2 is constrained to be only translation in the horizontal plane and rotation around the vertical axis. In addition, the instantaneous relative velocity is constrained to be a combination of forward rolling and pure rotation around the vertical axis: no sideways velocity in the y-direction of Ψ2 is allowed. As a global coordinate matrix, we choose the combination of a 2 × 2 rotation matrix and a 2 × 1 translation vector, i.e. a matrix of the form ¸ · £ ¤ Q11 Q12 Q13 ∈ R2×3 (9) Q = Qrot Qtra = Q21 Q22 Q23
Q4
Ψ2Ψ3
Ψ0
Ψ4
Ψ1 Q1
Fig. 3. Setup of a general open rigid mechanism with rigid links interconnected by general joints. Frame Ψ0 denotes the inertial reference frame, and each body i (with frame Ψi ) connects to the parent body in the tree by a joint with configuration coordinate Qi .
such that Qrot is a 2 × 2 rotation matrix and H21 (Q) equals Q11 Q12 0 Q13 Q21 Q22 0 Q23 (10) H21 (Q) = 0 0 1 0 0 0 0 1 We choose the velocity coordinates v to describe the angular and linear velocities in body frame, i.e. T v1 0 0 1 Q23 −Q13 0 T21,1 = X(Q)v = 0 0 0 Q11 Q21 0 v2 v3 0 0 0 Q12 Q22 0
such that v1 describes the angular velocity around the z-axis of Ψ2 , and (v2 , v3 ) the linear velocity in the (x, y) direction of Ψ2 . Finally, we use exponential coordinates for SE(2) as the coordinate mapping Φ(Q, ϕ), i.e. 2
0
6 6ϕ 4 1
Φ(Q, ϕ) = Qe 0
−ϕ1 0 0
3
ϕ2 7 ϕ3 7 5 0
(11)
The nonholonomic constraint can be expressed simply as £ ¤ AT (Q)v = 0 0 1 v = v3 = 0
due to the specific choice of coordinates for v. In this case, ¯ we can choose the matrix S(Q) as 1 0 ¯ S(Q) = 0 1 0 0 which has constant rank k¯ = 2 and satisfies AT S¯ = 0.
III. DYNAMICS OF O PEN M ULTIBODY S YSTEMS In this section, we discuss the procedure to derive the dynamics of rigid mechanisms with general joints as defined in Section II. We only consider open mechanism, i.e. systems without closed kinematic chains, and label the n joints and n links as in Fig. 3. Each joint has a global configuration coordinate Qi , velocity coordinate vi ∈ Rki , and coordinate mapping Φi (Qi , ϕi ).
3344
A. The Boltzmann-Hamel Equations The derivation of the dynamic equations is based on the Boltzmann-Hamel equations [12] as formulated in Theorem 1 below. These equations describe the dynamics of a system in quasi-coordinates, i.e. velocity coordinates v that are not the time-derivative of position coordinates q but are related as v = S(q)q˙ for some diffeomorphism S(q). Theorem 1 (Boltzmann-Hamel): Given a mechanical system with configuration q, velocity v = S(q)q˙ (with S(q) invertible and continuously differentiable), inputs torques τ collocated with v, potential energy V (q) and kinetic coenergy Uk = 12 v T M (q)v. The dynamics can be written in (q, v) coordinates as ! Ã µ ¶ X ∂L ∂L d ∂L − S −T + =τ (12) γk vk dt ∂v ∂q ∂v k
with L(q, v) := Uk (q, v) − V (q) = 21 v T M (q)v − V (q) and ¶ µ X ∂Slm ∂Slj −1 − (13) (γk )ij (q) := Sli−1 Smk ∂qm ∂qj l,m
Proof: Given the definition of the function L(q, v) and ¯ of the the coordinates v, we can write the Lagrangian L system in (q, q) ˙ coordinates as
1 ¯ q) (14) L(q, ˙ = q˙T S T (q)M (q)S(q)q˙ + V (q) 2 Expressing its partial derivatives in terms of the partial derivatives of L, and substituting into the classical EulerLagrange equations µ ¯¶ ¯ ∂L d ∂L − = B(q)τ dt ∂ q˙ ∂q with B = S T (since τ is collocated with v) gives (12). B. Mechanisms with only holonomic joints We now proceed with the derivation of the dynamic equations for general rigid mechanisms. From Definitions 1 and 2, we know that the relative twist of each joint can be expressed as Ti⋆,⋆ = Xi (Qi )vi , where ⋆ denotes the index of the parent link in the tree. Furthermore, we can use the coordinate mapping Φi (Qi , ϕi ) to represent the relative configuration of a joint as Hi⋆ (Qi , ϕi ). For fixed Qi , the time-derivative H˙ i⋆ becomes a function H˙ i⋆ (Qi , ϕi , ϕ˙ i ) that is linear in ϕ˙ i . Combining this with the fact that (4) gives a linear relation between Ti⋆,⋆ and H˙ i⋆ , we can conclude that there exists a linear relation between v and ϕ, ˙ parameterized by Qi and Φi . We express this relation as vi = Si (Qi , ϕi )ϕ˙ i
(15)
and refer to the examples further on for typical expressions for Si (Qi , ϕi ) for common joints. For joints with a Euclidean joint space, we have vi = ϕ˙ i and hence Si (Qi , ϕi ) = I. For a general mechanism with n joints, we loosely denote by Q the collection of all global joint coordinates Qi , by ϕ the vector stacking of all local coordinates ϕi around a certain configuration Q, by v the vector stacking of all joint
velocity coordinates vi , and by S(Q, ϕ) the block-diagonal matrix relating v to ϕ. ˙ Following standard procedures (see e.g. [1] or [11]), we can derive an expression for the inertia matrix M (Q) of the mechanism and write the kinetic co-energy as Uk (Q, v) = 1 T 2 v M (Q)v. The potential energy due to gravity can be derived by expressing the heights hi of the centers of mass of Pnall rigid bodies in terms of Q and writing V (Q) = i=1 mi ghi (Q), with mi the mass of body i and g the gravitational constant. Additional potential energies can be added if required. Theorem 2 (Dynamics for holonomic joints): Given an n-link open rigid mechanism with general holonomic joints, labeled as in Fig. 3, with joints described by global coordinates Q, local coordinates ϕ, coordinate mapping Φ(Q, ϕ), global velocity coordinates v, and the matrix S relating local and global velocity coordinates as v = S(Q, ϕ)ϕ˙ for fixed Q. Let it have kinetic co-energy Uk = 21 v T M (Q)v, potential energy V (Q), and input torques τ collocated with v. The dynamics of the mechanism are given by ˆ ˆ (Q) = τ M (Q)v˙ + C(Q, v)v + N
(16)
with ¯ ¯ −T ∂V ¯ ˆ N (Q) := S ∂ϕ ¯ϕ=0 ¶¯ X µ ∂Mij 1 −1 ∂Mjl ¯¯ −1 ˆ vl Cij (Q, v) := S − Ski ∂ϕk kl 2 ∂ϕk ¯ϕ=0 k,l µ ¶¯ ¶ X µ ¯ ∂Smj ∂Sms −1 −1 + Smi − Ssk Mkl ¯¯ vl ∂ϕs ∂ϕj ϕ=0 k,l,m,s
Partial derivatives of M (Q) and V (Q) with respect to ϕ should be computed by first substituting the coordinate mappings Φ(Q, ϕ) for Q and then taking the partial derivative, ∂M (Φ(Q,ϕ)) (Q) |ϕ=0 . so as ∂M ∂ϕk |ϕ=0 = ∂ϕk Proof: The theorem follows more or less directly by applying the Boltzmann-Hamel equations of Theorem 1 to the rigid mechanism expressed in (ϕ, v) coordinates around a certain point Q and evaluating at ϕ = 0. More precisely, if we parameterize the configuration Qt around a certain fixed point Q as Qt = Φ(Q, ϕ), we can write the Lagrangian in coordinates (ϕ, v), parameterized by Q, as 1 T v M (Φ(Q, ϕ))v − V (Φ(Q, ϕ)) 2 We can then apply Theorem 1 and calculate ! µ ¶ ÃX ∂M d ∂LQ = ϕ˙ k v + M v˙ dt ∂v ∂ϕk LQ (ϕ, v) =
(17)
k
∂LQ 1 ∂M ∂V = vT v− ∂ϕk 2 ∂ϕk ∂ϕk
Combining all elements (and using the definitions of Cˆ and ˆ ) results in expression (16). N It is important to note that only the global coordinates Q appear in the expression for the dynamics and are states of
3345
the dynamical system; the local coordinates ϕ are evaluated at zero. This means that (16) is a globally valid equation for all coordinates Q, and does not require different expressions across the configuration manifold that depend on its exact covering by a coordinate atlas. Furthermore, the expressions evaluated at ϕ = 0 only depend on local joint properties (not the rest of the mechanism) and can hence be evaluated offline and for each joint separately. This makes the presented algorithm suitable for software implementation. In order to obtain the full set of differential equations for (Q, v), the dynamic equations (16) can be augmented ˙ with the kinematic equations Q˙ = Q(Q, v) which relate the velocity coordinates to the time derivatives of the position coordinates and are determined on joint level by the relations Hi⋆ = Hi⋆ (Qi ) and Ti⋆,⋆ = Xi (Qi )vi . Example 3 (Euclidean joints): For Euclidean joints with configuration coordinates Q = q ∈ Rn and velocity coordinates v = q, ˙ the dynamic equations reduce to the standard equations (1) known from literature, i.e. S(Q, ϕ) = I, ˆ ˆ (Q) = N (q). C(Q, v) = C(q, q), ˙ and N Example 4 (Ball joint continued): We give an example of how some of the terms in Theorem 2 would look for a ball joint with coordinates (Q, v) as in Example 1. From the choice of coordinates (and coordinate mapping Φ), we can write for configurations Qt around fixed Q v˜ = Q˙ t QTt µ = Q ϕ˜˙ + µ = Q ϕ˜˙ +
¶ 1 ˙ ˙ (ϕ˜ϕ˜ + ϕ˜ϕ) ˜ + . . . (I − ϕ˜ + . . .) QT 2 ¶ 1 ˙ (ϕ˜ϕ˜ − ϕ˜˙ ϕ) ˜ + . . . QT 2
where dots indicate terms of second and higher order in ϕ. This relation can be written in vector form as µ ¶ 1 v = Q I + ϕ˜ + . . . ϕ˙ = S(Q, ϕ)ϕ˙ (18) 2 which gives the relation between v and ϕ. ˙ From this expression, we can compute for example ¯ µ ¶¯ ¯ 1 ∂(S T x) ¯¯ ∂ T ¯ (I − ϕ ˜ + . . .)Q x = ¯ ¯ ∂ϕ ϕ=0 ∂ϕ 2 ϕ=0 µ ¶¯ ¯ 1 ∂ T − ϕQ ˜ x + . . . ¯¯ = ∂ϕ 2 µ ¶¯ϕ=0 ¯ ∂ 1^ = (QT x)ϕ + . . . ¯¯ ∂ϕ 2 ϕ=0
1^ 1 T x) = QT x = (Q ˜Q 2 2 for any x ∈ R3 . This example is typical in the sense that, for joints with exponential Lie group coordinates, the final expressions to be used in the dynamic equations (evaluated at ϕ = 0) are generally very simple. C. Mechanisms with holonomic and nonholonomic joints Just as Definition 2 for nonholonomic joints is a restriction of Definition 1 for holonomic joints, we consider the dynamics of mechanisms with nonholonomic joints as a restriction
of the dynamics of holonomic joints. To be more precise, the dynamics of mechanism with velocity v is constrained to satisfy extra equations of the form AT (Q)v = 0. These constraints are enforced by constraint forces λ that enter the dynamic equations as extra terms A(Q)λ on the right-hand side of (16). The magnitude of λ is such that the constraints remain satisfied at all times. The following theorem presents how the dynamic equations for the restricted systems can be obtained. Theorem 3 (Dynamics for general joints): Given an nlink open rigid mechanism with general holonomic and nonholonomic joints, labeled as in Fig. 3, with joints described by global coordinates Q, local coordinates ϕ, and coordinate mapping Φ(Q, ϕ). Let S describe the relation between local and global velocity coordinates as v = S(Q, ϕ)ϕ˙ for fixed Q, and let S¯ describe the relation between constrained velocities ¯ v¯ and unconstrained velocities v as v = S(Q)¯ v (with ¯ S(Q) = I for holonomic joints). Let the mechanism have kinetic co-energy Uk = 12 v T M (Q)v, potential energy V (Q), and input torques τ collocated with v. The dynamics of the mechanism are given by ¯ (Q)v¯˙ + C(Q, ¯ ˆ (Q) = S¯T (Q)τ M v¯)¯ v + S¯T (Q)N
(19)
with ¯ (Q) := S¯T M S¯ M ¶¯ X µ ∂ S¯ ¡ ¢ ¯ ¯ ¯v ¯ C(Q, v¯) := S¯T M S −1 S¯ + S¯T Cˆ S¯ i ¯ ∂ϕ i ϕ=0 i
ˆ ˆ ¯v ) and N ˆ (Q) as in Theorem 2. where C(Q, v) = C(Q, S¯ Proof: Starting from the result of Theorem 2, i.e. before applying the constraint forces, the implicit dynamic equations of the mechanism are ˆ ˆ (Q) = τ + A(Q)λ M (Q)v˙ + C(Q, v)v + N
(20)
and the nonholonomic constraint equals AT (Q)v = 0. If ¯v (which can be done for all velocities we substitute v = S¯ satisfying the constraint, since S¯ defines a basis) and premultiply (20) by S¯T , we obtain ´ ³ ¯˙ v + S¯T Cˆ S¯ ¯v + S¯T N ˆ = S¯T τ + S¯T Aλ S¯T M S¯v¯˙ + S¯
¯ and the equation The last term equals zero by definition of S, can be written directly as in the theorem. An expression for λ can be obtained by pre-multiplying ¯v , which gives (20) with AT M −1 and substituting v = S¯ ¯˙ v ) + AT M −1 (Cˆ S¯ ¯v + N ˆ ) = AT M −1 (τ + Aλ) AT (S¯v¯˙ + S¯ ¡ ¢ ¯˙ v + AT M −1 (Cˆ S¯ ¯v + N ˆ − τ ) = AT M −1 A λ AT S¯
Since AT M −1 A is invertible, this expression uniquely determines λ from the state (Q, v¯) in an equation that complements (19). The columns of M −1 A form a basis for the constrained velocities that is M -orthogonal to the basis defined by the columns of S¯ (since S¯T M M −1 A = 0). This orthogonality in the proper metric M results in two independent sets of equations, one for the accelerations v¯˙ and one for the constraint forces λ.
3346
Example 5 (Planar joint continued): Consider again the nonholonomic planar joint as described in Example 2. If we consider Ψ1 as the inertial frame and body 2 has mass m and inertia J around the z-axis, the kinetic co-energy can be written in terms of the coordinates Q and v as J 0 0 1 1 Uk (Q, v) = v T M (Q)v = v T 0 m 0 v 2 2 0 0 m To find the mapping S, we compare the left-hand and righthand sides of the expression ] = T˜1,1 = H˙ 1 H 2 (Xv) 2 1 2 to find S(Q, ϕ) = I. The dynamic equations for the planar joint without nonholonomic constraints can thus be written in coordinates (Q, v) as τ1 J 0 0 v˙ 1 0 m 0 v˙ 2 + 0 + 0 = τ2 τ3 0 0 m v˙ 3 Using the definition of (the constant) S¯ from Example 2, the reduced equations in terms of v¯ become ¸· ¸ · ¸ · τ J 0 v¯˙ 1 +0+0= 1 τ2 0 m v¯˙ 2
The dynamics in this case are obviously not very interesting, but the example serves to illustrate the systematic procedure to obtain them, and the generality of using this particular joint as part of a larger mechanism (say, by putting the cart on a moving platform and adding a robotic manipulator on top of it). Furthermore, the kinematics, i.e. the relation between Q and v¯, also follows from the theory, in general as ^ ˙ = T˜1,1 (Q, v¯)H 1 (Q) = (X(Q) ¯ H˙ 21 (Q, Q) S(Q)¯ v )H21 (Q) 2 2 which for this example can be simplified to 0 −¯ v1 v¯2 0 0 Q˙ = Q v¯1 0 0 0
IV. C ONCLUSIONS AND F UTURE W ORK
This paper presents a generalization of classical modeling results to an extended class of rigid multibody systems. In addition to joints with Euclidean joint spaces (with configuration described by Rn ), general joints with configuration spaces different from Rn can be included, requiring only mild and local modifications to the classical Lagrangian results known from literature. The equations are globally valid for all configurations and do not require local configuration states. In addition, nonholonomic constraints can be added as a trivial extension, reducing the dimension of the velocity space, but again not drastically modifying the equations. The generalized class of joints includes all joints described by Lie groups, such as ball joints, ‘free motion’ joints (unconstrained 6D motion), and planar motion. However, any other general joint type can be included as well, as long as it can be formulated as in Definition 1.
The extra elements that appear in the dynamic equation have a block-diagonal form, which means they can be specified at joint level and do not depend on the rest of the mechanism. This makes the presented approach suitable for software implementation, as it allows many different joints to be specified as library elements with representations of the specific global coordinates for position and velocity, as well as the extra structure elements that appear in the equation: the matrix S(Q, ϕ), its inverse, and its partial derivatives with respect to each of the local coordinates ϕ, all evaluated at ϕ = 0, can be stored in the library element, and then used as systematic building blocks in the dynamic equations. The general form of the equations is currently being implemented in the modeling and simulation package 20sim [13]. As a possible extension for future work, the precise structure of the expressions for C¯ should be analyzed in more detail, in order to find fast and numerically robust ways to express these terms in simulation code. A future paper will also describe the presented results in port-Hamiltonian terms, i.e. using momentum instead of velocity variables. ACKNOWLEDGMENTS This work has been done in the context of the European sponsored project GeoPlex with reference code IST-200134166. Further information is available at the project website http://www.geoplex.cc. Vincent Duindam is sponsored by the Netherlands Organization for Scientific Research (NWO). R EFERENCES [1] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [2] J. E. Marsden and T. S. Ratiu, Introduction to Mechanics and Symmetry, 2nd ed., ser. Texts in Applied Mathematics. Springer-Verlag, 1999, vol. 17. [3] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control Systems, ser. Texts in Applied Mathematics. Springer, November 2004, vol. 49. [4] A. M. Bloch, P. S. Krishnaprasad, J. E. Marsden, and R. M. Murray, “Nonholonomic mechanical systems with symmetry,” Archive for Rational Mechanics and Analysis, vol. 136, pp. 21–99, 1996. [5] A. J. van der Schaft and B. M. Maschke, “On the Hamiltonian formulation of nonholonomic mechanical systems,” Reports on Mathematical Physics, vol. 34, pp. 225–233, 1994. [6] V. Duindam, G. Blankenstein, and S. Stramigioli, “Port-based modeling and analysis of snakeboard locomotion,” in Proceeding of the International Symposium on Mathematical Theory of Networks and Systems, July 2004. [7] T. R. Kane and C. F. Wang, “On the derivation of equations of motion,” Journal of the Society for Industrial and Applied Mathematics, vol. 13, no. 2, pp. 487–492, June 1965. [8] W. Blajer, “A geometrical interpretation and uniform matrix formulation of multibody system dynamics,” Zeitschrift f¨ur angewandte Mathematik und Mechanik, vol. 81, no. 4, pp. 247–259, 2001. [9] J. M. Cameron and W. J. Book, “Modeling mechanisms with nonholonomic joints using the Boltzmann-Hamel equations,” International Journal of Robotics Research, vol. 16, pp. 47–59, 1997. [10] V. Duindam, “Port-based modeling and control for efficient bipedal walking robots,” Ph.D. dissertation, University of Twente, March 2006. [Online]. Available: http://purl.org/utwente/50829 [11] S. Stramigioli, Modeling and IPC Control of Interactive Mechanical Systems – A Coordinate-free Approach. Springer-Verlag, 2001. [12] E. T. Whittaker, A Treatise on the Analytical Dynamics of Particles and Rigid Bodies, 4th ed. Cambridge University Press, 1998. [13] Control Lab Products, “20sim version 3.6,” 2005. [Online]. Available: http://www.20sim.com
3347
