Lagrangian Dynamics of Open Multibody Systems with Generalized Holonomic and Nonholonomic Joints

Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Roots and Systems San Diego, CA, USA, Oct 29 - Nov 2, 2007 ThB6.4 Lagrangian Dynamics of Open Multiody Systems with Generalized Holonomic and Nonholonomic Joints Vincent Duindam Dept. of EECS, University of California Berkeley, CA 94720, USA v.duindam@ieee.org Stefano Stramigioli Dept. of EEMCS, University of Twente P.O. Box 217, Enschede, Netherlands s.stramigioli@ieee.org Astract Standard methods to model multiody systems are aimed at systems with configuration spaces isomorphic to R n. This limitation leads to singularities and other artifacts in case the configuration space has a different topology, for example in the case of all 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 otained using classical methods, ut they do not suffer from singularities. Furthermore, the equations are explicit differential equations oth for holonomic and nonholonomic joints) and can e directly implemented in simulation software. I. INTRODUCTION For analysis, design, and control of rootic and other complex mechanical systems, it is often crucial to otain a good mathematical model of the dynamics of the mechanism. This model should reflect the essential properties of the mechanism, while still eing simple enough for simulation and controller design. For many applications, it is sufficient to model these mechanisms as rigid multiody systems, meaning interconnections of perfectly rigid odies y 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 R n. To represent the configuration of the mechanism, one can then choose coordinates q R n, 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 odies in the mechanism, the dynamics can then e descried y the well-known equation [1] Mq) q + Cq, q) q + Nq) = Bq)τ 1) where Mq) is the strictly positive-definite mass matrix of the system, Cq, q) q represents the Coriolis and centrifugal forces, Nq) the gravity forces, τ the vector of actuation torques, and Bq) the input mapping such that Bq)τ is collocated with q. To use 1), the configuration space is required to e adequately descried y R n. For most joint types, this is not a serious prolem, though strictly speaking the configuration space of a rotational joint is a circle and not R 1. However, for more general joints, such as all joints three degrees of freedom) and free motion joints unconstrained motion with six degrees of freedom), the required representation of the configuration space as R n 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 e 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, ut it includes the important suclass of all joints for which the configuration space is a Lie group, such as all 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 een 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 een 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 rackets [5], [6]. Both holonomic and nonholonomic constraints have een 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, ut are nevertheless restricted to joints with a Euclidean joint space. This paper, which is ased 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 e formulated in the required framework. Section III shows how the dynamics can e derived for multiody systems composed of such joints. Section IV concludes the paper. II. GENERAL HOLONOMIC AND NONHOLONOMIC JOINTS This section defines a general class of joints for which the dynamics can e derived y the method of Section III. Section II-A gives a rief summary of the theory of homogeneous transformations and twists, and then presents the definitions of general holonomic joints Section II-B) 1-4244-0912-8/07/$25.00 2007 IEEE. 3342

Q 1 ϕ 2 ϕ 1 ϕ 2 ϕ 1 Q 2 ΦQ 1, ) ΦQ 2, ) Fig. 1. The mapping ΦQ, ϕ) assigns local Euclidean coordinates ϕ R k to a neighorhood of each gloal 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 riefly summarize the theory and notation used to represent configurations and velocities of rigid odies. We refer to [1] and [11] for a more extensive ackground; in this paper, we follow the notation of [11]. The configuration of a rigid ody in three-dimensional space, meaning its position and orientation relative to some reference frame, can e descried y an element of the Lie group SE3), the Special Euclidean group. Numerically, this configuration can e expressed y a 4 4 homogeneous matrix H a of the form H a = [ ] R a p a 0 1 where R a is a 3 3 orthogonal matrix with detra ) = 1 that descries the orientation of the ody, and p a is a 3 1 vector that descries the position of its origin. The indices a and refer to two right-handed coordinate frames Ψ a the reference frame) and Ψ rigidly attached to the ody). The velocity of a rigid ody, meaning oth its linear and angular velocity, can e concisely descried y an element of se3), the Lie algera of SE3). Numerically, it can e expressed as a vector T c,a or matrix [ T c,a ω c,a] = v c,a R 6 c,a or T := c,a T c,a [ ω ϕ 2 ϕ 1 2) of the form v c,a ] R 4 4 0 0 with ω c,a,v c,a R 3 the angular and linear velocity, respectively, of the rigid ody, and we define for every x R 3 x := x ) = 0 x 3 x 2 x 3 0 x 1 3) x 2 x 1 0 The twist T c,a denotes the relative velocity of frame Ψ with respect to Ψ a, expressed in coordinate frame Ψ c. It is related to the relative configuration 2) as T c,a = H c aḣa H c 4) with Ḣ denoting the time-derivative of H. B. Holonomic Joints Definition 1 Holonomic joint): A gloally parameterized holonomic joint is a smooth k-dimensional su-manifold Q SE3) that descries the permitted relative configurations Hj i of ody j relative to ody i, and satisfies the following properties: 1) Every joint configuration is uniquely descried y a coordinate matrix Q of aritrary ut constant dimensions), and Hj i = Hj i Q). We identify an astract element of Q with its coordinate representation Q and write Q Q; 2) Every allowed joint velocity an element of T Q Q) is uniquely descried y a vector v R k, and the joint twist can e expressed as T i,i j = XQ)v with XQ) R 6 k a matrix descriing the instantaneously allowed twists; 3) There exists a coordinate mapping Φ : Q R k Q, such that for each Q Q, ΦQ,0) = Q and ΦQ, ) defines a local diffeomorphism etween a neighorhood of 0 R k and a neighorhood of Q Q. Fig. 1 illustrates the third property: every point of the allowed configuration space is assigned a unique gloal) coordinate Q, and the mapping Φ assigns local) Euclidean coordinates ϕ to a neighorhood 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 e formulated in the form of the definition. If k is the numer of degrees of freedom, the gloal configuration coordinate can e chosen as Q R k, its velocity coordinate as v = Q R k, and the coordinate mapping as ΦQ,ϕ) = Q + ϕ with ϕ R k. Secondly, joints with a configuration space descried y a Lie group e.g. all joints or free motion joints) can also e 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 algera of the group. Finally, the coordinate mapping Φ can e chosen as the exponential coordinate mapping of the group, i.e. the mapping ΦQ,ϕ) = Qe P k i=1 iϕi 5) with i the appropriate asis elements for the algera. Indeed, ΦQ,0) = Qe 0 = Q and the mapping assigns local coordinates ϕ to a neighorhood of every Q Q. Example 1 Ball joint): Consider the all joint shown in Fig. 2. We choose the configuration matrix Q to e the 3 3 rotation matrix that descries the relative orientation of frames Ψ 1 and Ψ 2, such that their relative configuration satisfies [ ] H2 1 = H2Q) 1 Q 0 = R 4 4 6) 0 1 As the gloal velocity coordinates, we pick v R 3 1 to e the angular velocity vector of frame Ψ 2 relative to Ψ 1, 3343

Ψ i Q j Ψ 2 Ψ 1 z Ψ 1 x y v 1 Ψ 2 v 2 v 3 Ψ 5 Q 2 Q 5 Q i Q 4 Ψ 2 Ψ 3 Ψ 4 Ψ j Fig. 2. Two examples of generalized joints etween two odies with attached frames Ψ 1 and Ψ 2 : a all joint left) and a planar nonholonomic joint right). expressed in Ψ 1. The relative twist etween Ψ 1 and Ψ 2 then ecomes T 1 0 0 0 0 0 v 1 T 1,1 2 = XQ)v = 7) 0 1 0 0 0 0 0 0 1 0 0 0 v 2 v 3 Finally, we can choose the coordinate mapping Ψ using exponential coordinates, i.e. as ΦQ,ϕ) = Qe ϕ = Q I + ϕ + 1 2 ϕ2 + 1 ) 6 ϕ3 +... 8) with ϕ := ϕ ) R 3 3 as efore. C. Nonholonomic Joints Definition 2 Nonholonomic joint): A gloally parameterized nonholonomic joint is a gloally parameterized holonomic joint together with a restriction on the allowed instantaneous velocities, given y an equation A T Q)v = 0 for some differentiale matrix AQ) R k k k) of constant rank k k). In addition to the properties given in Definition 1, there must exist a differentiale rank k matrix SQ) R k k such that A T Q) SQ) = 0 for all Q Q. Following the definition, a nonholonomic joint is asically a constrained holonomic joint, implicitly restricting the allowed velocities to a k-dimensional linear suspace. The columns of the matrix S form a asis for the space of allowed velocities, or in other words, allowed velocities have the form v = S v, automatically satisfying the constraint A T v = A T 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 e only translation in the horizontal plane and rotation around the vertical axis. In addition, the instantaneous relative velocity is constrained to e a comination of forward rolling and pure rotation around the vertical axis: no sideways velocity in the y-direction of Ψ 2 is allowed. As a gloal coordinate matrix, we choose the comination of a 2 2 rotation matrix and a 2 1 translation vector, i.e. a matrix of the form Q = [ [ ] ] Q11 Q Q rot Q tra = 12 Q 13 R 2 3 9) Q 21 Q 22 Q 23 Q 3 Ψ 0 Ψ 1 Q 1 Fig. 3. Setup of a general open rigid mechanism with rigid links interconnected y general joints. Frame Ψ 0 denotes the inertial reference frame, and each ody i with frame Ψ i ) connects to the parent ody in the tree y a joint with configuration coordinate Q i. such that Q rot is a 2 2 rotation matrix and H2Q) 1 equals Q 11 Q 12 0 Q 13 H2Q) 1 = Q 21 Q 22 0 Q 23 0 0 1 0 10) 0 0 0 1 We choose the velocity coordinates v to descrie the angular and linear velocities in ody frame, i.e. T 0 0 1 Q 23 Q 13 0 v 1 T 1,1 2 = XQ)v = 0 0 0 Q 11 Q 21 0 0 0 0 Q 12 Q 22 0 v 2 v 3 such that v 1 descries the angular velocity around the z-axis of Ψ 2, and v 2,v 3 ) the linear velocity in the x,y) direction of Ψ 2. Finally, we use exponential coordinates for SE2) as the coordinate mapping ΦQ, ϕ), i.e. ΦQ,ϕ) = Qe 2 6 4 0 ϕ 1 ϕ 2 ϕ 1 0 ϕ 3 0 0 0 The nonholonomic constraint can e expressed simply as 3 7 5 A T Q)v = [ 0 0 1 ] v = v 3 = 0 11) due to the specific choice of coordinates for v. In this case, we can choose the matrix SQ) as 1 0 SQ) = 0 1 0 0 which has constant rank k = 2 and satisfies A T S = 0. III. DYNAMICS OF OPEN MULTIBODY SYSTEMS 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 lael the n joints and n links as in Fig. 3. Each joint has a gloal configuration coordinate Q i, velocity coordinate v i R ki, and coordinate mapping Φ i Q i,ϕ i ). 3344

A. The Boltzmann-Hamel Equations The derivation of the dynamic equations is ased on the Boltzmann-Hamel equations [12] as formulated in Theorem 1 elow. These equations descrie the dynamics of a system in quasi-coordinates, i.e. velocity coordinates v that are not the time-derivative of position coordinates q ut are related as v = Sq) q for some diffeomorphism Sq). Theorem 1 Boltzmann-Hamel): Given a mechanical system with configuration q, velocity v = Sq) q with Sq) invertile and continuously differentiale), inputs torques τ collocated with v, potential energy V q) and kinetic coenergy U k = 1 2 vt Mq)v. The dynamics can e written in q, v) coordinates as d dt L v ) T L S q + ) γ k v k k L v = τ 12) with Lq,v) := U k q,v) V q) = 1 2 vt Mq)v V q) and γ k ) ij q) := li Slj mk S ) lm 13) q m q j l,m Proof: Given the definition of the function Lq, v) and the coordinates v, we can write the Lagrangian L of the system in q, q) coordinates as Lq, q) = 1 2 qt S T q)mq)sq) q + V q) 14) Expressing its partial derivatives in terms of the partial derivatives of L, and sustituting into the classical Euler- Lagrange equations d dt ) L L q q = Bq)τ 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 e expressed as T, i = X i Q i )v i, where denotes the index of the parent link in the tree. Furthermore, we can use the coordinate mapping Φ i Q i,ϕ i ) to represent the relative configuration of a joint as Hi Q i,ϕ i ). For fixed Q i, the time-derivative Ḣ i ecomes a function Ḣ i Q i,ϕ i, ϕ i ) that is linear in ϕ i. Comining this with the fact that 4) gives a and Ḣ i, we can conclude that there exists a linear relation etween v and ϕ, parameterized y Q i and Φ i. We express this relation as linear relation etween T, i v i = S i Q i,ϕ i ) ϕ i 15) and refer to the examples further on for typical expressions for S i Q i,ϕ i ) for common joints. For joints with a Euclidean joint space, we have v i = ϕ i and hence S i Q i,ϕ i ) = I. For a general mechanism with n joints, we loosely denote y Q the collection of all gloal joint coordinates Q i, y ϕ the vector stacking of all local coordinates ϕ i around a certain configuration Q, y v the vector stacking of all joint velocity coordinates v i, and y SQ,ϕ) the lock-diagonal matrix relating v to ϕ. Following standard procedures see e.g. [1] or [11]), we can derive an expression for the inertia matrix MQ) of the mechanism and write the kinetic co-energy as U k Q,v) = 1 2 vt MQ)v. The potential energy due to gravity can e derived y expressing the heights h i of the centers of mass of all rigid odies in terms of Q and writing V Q) = n i=1 m igh i Q), with m i the mass of ody i and g the gravitational constant. Additional potential energies can e added if required. Theorem 2 Dynamics for holonomic joints): Given an n-link open rigid mechanism with general holonomic joints, laeled as in Fig. 3, with joints descried y gloal coordinates Q, local coordinates ϕ, coordinate mapping ΦQ, ϕ), gloal velocity coordinates v, and the matrix S relating local and gloal velocity coordinates as v = SQ,ϕ) ϕ for fixed Q. Let it have kinetic co-energy U k = 1 2 vt MQ)v, potential energy V Q), and input torques τ collocated with v. The dynamics of the mechanism are given y with MQ) v + ĈQ,v)v + ˆNQ) = τ 16) T V ˆNQ) := S ϕ Ĉ ij Q,v) := k,l + mi k,l,m,s Mij kl 1 ϕ k 2 S 1 ki Smj ϕ s ) M jl v l ϕ k S ) ms sk kl) ϕ M v l j Partial derivatives of MQ) and V Q) with respect to ϕ should e computed y first sustituting the coordinate mappings ΦQ,ϕ) for Q and then taking the partial derivative, = MΦQ,ϕ)) ϕ k. so as MQ) ϕ k Proof: The theorem follows more or less directly y 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 Q t around a certain fixed point Q as Q t = ΦQ,ϕ), we can write the Lagrangian in coordinates ϕ, v), parameterized y Q, as L Q ϕ,v) = 1 2 vt MΦQ,ϕ))v V ΦQ,ϕ)) 17) We can then apply Theorem 1 and calculate ) ) d LQ M = ϕ k v + M v dt v ϕ k k L Q = 1 M vt v V ϕ k 2 ϕ k ϕ k Comining all elements and using the definitions of Ĉ and ˆN) results in expression 16). It is important to note that only the gloal 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 gloally valid equation for all coordinates Q, and does not require different expressions across the configuration manifold that depend on its exact covering y a coordinate atlas. Furthermore, the expressions evaluated at ϕ = 0 only depend on local joint properties not the rest of the mechanism) and can hence e evaluated offline and for each joint separately. This makes the presented algorithm suitale for software implementation. In order to otain the full set of differential equations for Q, v), the dynamic equations 16) can e augmented with the kinematic equations Q = QQ,v) which relate the velocity coordinates to the time derivatives of the position coordinates and are determined on joint level y the relations Hi = H i Q i) and T, i = X i Q i )v i. Example 3 Euclidean joints): For Euclidean joints with configuration coordinates Q = q R n and velocity coordinates v = q, the dynamic equations reduce to the standard equations 1) known from literature, i.e. SQ, ϕ) = I, ĈQ,v) = Cq, q), and ˆNQ) = Nq). Example 4 Ball joint continued): We give an example of how some of the terms in Theorem 2 would look for a all joint with coordinates Q, v) as in Example 1. From the choice of coordinates and coordinate mapping Φ), we can write for configurations Q t around fixed Q ṽ = Q t Q T t = Q ϕ + 1 ) 2 ϕ ϕ + ϕ ϕ) +... I ϕ +...) Q T = Q ϕ + 1 ) 2 ϕ ϕ ϕ ϕ) +... Q T where dots indicate terms of second and higher order in ϕ. This relation can e written in vector form as v = Q I + 12 ) ϕ +... ϕ = SQ,ϕ) ϕ 18) which gives the relation etween v and ϕ. From this expression, we can compute for example S T x) ϕ = I 1 x) ϕ 2 ϕ +...)QT 1 2 ϕqt x +...) = ϕ = ϕ 1 Q 2 T x)ϕ +...) = 1 2 Q T x) = 1 2 QT xq for any x R 3. This example is typical in the sense that, for joints with exponential Lie group coordinates, the final expressions to e 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 e more precise, the dynamics of mechanism with velocity v is constrained to satisfy extra equations of the form A T Q)v = 0. These constraints are enforced y constraint forces λ that enter the dynamic equations as extra terms AQ)λ 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 e otained. Theorem 3 Dynamics for general joints): Given an n- link open rigid mechanism with general holonomic and nonholonomic joints, laeled as in Fig. 3, with joints descried y gloal coordinates Q, local coordinates ϕ, and coordinate mapping ΦQ, ϕ). Let S descrie the relation etween local and gloal velocity coordinates as v = SQ,ϕ) ϕ for fixed Q, and let S descrie the relation etween constrained velocities v and unconstrained velocities v as v = SQ) v with SQ) = I for holonomic joints). Let the mechanism have kinetic co-energy U k = 1 2 vt MQ)v, potential energy V Q), and input torques τ collocated with v. The dynamics of the mechanism are given y with MQ) v + CQ, v) v + S T Q) ˆNQ) = S T Q)τ 19) MQ) := S T M S CQ, v) := S T M i S )i) S 1 S v + ϕ i S T Ĉ S where ĈQ,v) = ĈQ, S v) and ˆNQ) as in Theorem 2. Proof: Starting from the result of Theorem 2, i.e. efore applying the constraint forces, the implicit dynamic equations of the mechanism are MQ) v + ĈQ,v)v + ˆNQ) = τ + AQ)λ 20) and the nonholonomic constraint equals A T Q)v = 0. If we sustitute v = S v which can e done for all velocities satisfying the constraint, since S defines a asis) and premultiply 20) y S T, we otain S T M S v + S v ) + S T Ĉ S v + S T ˆN = ST τ + S T Aλ The last term equals zero y definition of S, and the equation can e written directly as in the theorem. An expression for λ can e otained y pre-multiplying 20) with A T M 1 and sustituting v = S v, which gives A T S v + S v) + A T M 1 Ĉ S v + ˆN) = A T M 1 τ + Aλ) A T S v + A T M 1 Ĉ S v + ˆN τ) = A T M 1 A ) λ Since A T M 1 A is invertile, this expression uniquely determines λ from the state Q, v) in an equation that complements 19). The columns of M 1 A form a asis for the constrained velocities that is M-orthogonal to the asis defined y the columns of S since ST MM 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 descried in Example 2. If we consider Ψ 1 as the inertial frame and ody 2 has mass m and inertia J around the z-axis, the kinetic co-energy can e written in terms of the coordinates Q and v as U k Q,v) = 1 2 vt MQ)v = 1 J 0 0 2 vt 0 m 0 v 0 0 m To find the mapping S, we compare the left-hand and righthand sides of the expression Xv) = T 1,1 2 = Ḣ1 2H 2 1 to find SQ,ϕ) = I. The dynamic equations for the planar joint without nonholonomic constraints can thus e written in coordinates Q,v) as J 0 0 v 1 τ 1 0 m 0 v 2 + 0 + 0 = τ 2 0 0 m v 3 τ 3 Using the definition of the constant) S from Example 2, the reduced equations in terms of v ecome [ ] [ ] J 0 ][ v 1 τ1 + 0 + 0 = 0 m v 2 The dynamics in this case are oviously not very interesting, ut the example serves to illustrate the systematic procedure to otain them, and the generality of using this particular joint as part of a larger mechanism say, y putting the cart on a moving platform and adding a rootic manipulator on top of it). Furthermore, the kinematics, i.e. the relation etween Q and v, also follows from the theory, in general as Ḣ 1 2Q, Q) = T 1,1 2 Q, v)h 1 2Q) = XQ) SQ) v)h 1 2 Q) which for this example can e simplified to 0 v 1 v 2 Q = Q v 1 0 0 0 0 0 IV. CONCLUSIONS AND FUTURE WORK This paper presents a generalization of classical modeling results to an extended class of rigid multiody systems. In addition to joints with Euclidean joint spaces with configuration descried y R n ), general joints with configuration spaces different from R n can e included, requiring only mild and local modifications to the classical Lagrangian results known from literature. The equations are gloally valid for all configurations and do not require local configuration states. In addition, nonholonomic constraints can e added as a trivial extension, reducing the dimension of the velocity space, ut again not drastically modifying the equations. The generalized class of joints includes all joints descried y Lie groups, such as all joints, free motion joints unconstrained 6D motion), and planar motion. However, any other general joint type can e included as well, as long as it can e formulated as in Definition 1. τ 2 The extra elements that appear in the dynamic equation have a lock-diagonal form, which means they can e specified at joint level and do not depend on the rest of the mechanism. This makes the presented approach suitale for software implementation, as it allows many different joints to e specified as lirary elements with representations of the specific gloal coordinates for position and velocity, as well as the extra structure elements that appear in the equation: the matrix SQ, ϕ), its inverse, and its partial derivatives with respect to each of the local coordinates ϕ, all evaluated at ϕ = 0, can e stored in the lirary element, and then used as systematic uilding locks in the dynamic equations. The general form of the equations is currently eing implemented in the modeling and simulation package 20sim [13]. As a possile extension for future work, the precise structure of the expressions for C should e analyzed in more detail, in order to find fast and numerically roust ways to express these terms in simulation code. A future paper will also descrie the presented results in port-hamiltonian terms, i.e. using momentum instead of velocity variales. ACKNOWLEDGMENTS This work has een done in the context of the European sponsored project GeoPlex with reference code IST-2001-34166. Further information is availale at the project wesite http://www.geoplex.cc. Vincent Duindam is sponsored y the Netherlands Organization for Scientific Research NWO). REFERENCES [1] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Rootic 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, Novemer 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-ased modeling and analysis of snakeoard 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 multiody system dynamics, Zeitschrift für 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 Rootics Research, vol. 16, pp. 47 59, 1997. [10] V. Duindam, Port-ased modeling and control for efficient ipedal walking roots, Ph.D. dissertation, University of Twente, March 2006. [Online]. Availale: 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. Camridge University Press, 1998. [13] Control La Products, 20sim version 3.6, 2005. [Online]. Availale: http://www.20sim.com 3347