1

Singularity-Free Dynamic Equations of Open-Chain Mechanisms with General Holonomic and Nonholonomic Joints Vincent Duindam, Member, IEEE, and Stefano Stramigioli, Senior Member, IEEE

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 that allows for a more 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. However, singularities are avoided through the use of global non-Euclidean configuration coordinates, together with mappings describing a local Euclidean structure around each configuration. The resulting equations are explicit (unconstrained) differential equations, both for holonomic and nonholonomic joints, that do not require a coordinate atlas and can be directly implemented in simulation software. Index Terms—robot dynamics, multibody systems, nonholonomic constraints, Lie groups, port-Hamiltonian systems.

I. I NTRODUCTION

F

OR 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 (at least as a starting point) as rigid multibody systems, meaning a collection of perfectly rigid bodies connected 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 for Rn . These coordinates, often denoted by a vector q, 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 in terms of the coordinates q and their time-derivatives by the well-known equation [1] M (q)¨ q + C(q, q) ˙ q˙ + N (q) = B(q)τ

(1)

Vincent Duindam is with the Department of Electrical Engineering and Computer Science, University of California at Berkeley, 253 Cory Hall, Berkeley, CA 94720-1770 USA (e-mail: [email protected]). Stefano Stramigioli is with the Department of Electrical Engineering, Math, and Computer Science, University of Twente, P.O. Box 217, 7500 AE Enschede, Netherlands (e-mail: [email protected]).

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 gravitational forces, τ the vector of actuation and other external torques, and B(q) the input mapping such that B(q)τ is collocated with q. ˙ Equivalently, one can represent the dynamics in port-Hamiltonian form as [2], [3]      " ∂H #  d q 0 0 I ∂q + = τ B(q) −I 0 ∂H dt p ∂p " # (2)   ∂H ∂q T y = 0 B (q) ∂H ∂p

in which p = M (q)q˙ is the momentum vector, H(q, p) the total energy (kinetic plus potential), I the identity matrix, and y the vector of velocities collocated with τ . The structure of the dynamic equations (1) and (2) requires that the configuration space can 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 the circle S1 and not R1 . However, for more general joints, such as ball joints (three degrees 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 the modeling methods for rigid-body mechanisms to a more general class of 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. We also show how to include 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 formulations (1) and (2). The main advantages of the presented globally valid explicit equations are 1) a reduced number of state variables and elimination of constraint equations (compared to implicit methods); 2) improved numerical stability (no singularities); 3) generally easier integration into model-based control methods such as MPC and computed torque (explicit differential equations without constraints).

2

The structure of this paper is as follows. Section II discusses related work and differences with the proposed approach. Section III 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 IV presents the Boltzmann-Hamel equations in Lagrangian and port-Hamiltonian form, as they are used in the derivation of the dynamics equations in Section V. Section VI concludes the paper and presents possible future extensions. II. R ELATED W ORK The derivations in this paper are partially based on the Boltzmann-Hamel equations [4], [5], which introduce the use of quasi-coordinates, i.e. velocity coordinates that are not simply the time-derivative of position coordinates. Quasicoordinates can be used to decompose a constrained dynamical system into unconstrained coordinates, representing the free dynamics, and constrained coordinates, which equal zero when the constraints are satisfied. This idea is used e.g. in the ‘Change of Basis’ technique used in the constrained force algorithm [6] or through the use of a projection operator [7]. The Boltzmann-Hamel equations themselves have also been applied to multibody systems, see for example [8]. However, these approaches all start from the assumption that the configuration space is isomorphic to Rn . Another aspect on which this paper builds is the use of Lie groups and exponential coordinates. Lie groups and algebras have formed the mathematical basis for the derivation of the dynamics of multibody systems in many ways, see for example [3], [9]–[11]. In its most direct application, exponential coordinates (coordinates generated by the Lie algebra) can be used as local Euclidean coordinates, and locally the dynamics can be expressed in these coordinates. The total configuration space can then be covered by an atlas of local exponential coordinate patches, and depending on the current configuration, the appropriate equations can be selected. Using more geometric techniques [12], [13], the dynamics of a system on a single Lie group can be expressed globally in terms of the group and algebra structures. Although these methods technically are also valid for combinations of Lie groups (which is what rigidbody mechanisms are), the formulation is quite different and generally more involved than the method proposed here. In this paper we only use the fact that exponential coordinates exist (and derive expressions based on their differential properties), we do not use the coordinates themselves as state variables. Implicit nonholonomic constraints have been converted to an explicit form in many ways, often involving a partial change of the velocity or momentum coordinates such that satisfying the constraint is equivalent to some of these coordinates being equal to zero. Results range from the classical GibbsAppel equations [14], [15], through Kane’s equations [16]– [18], to modern methods using Ehresmann connections [19] and Poisson structures [20], [21]. The approach presented in this paper follows a similar strategy, but combines it into a single framework used also for holonomic joints with a nonEuclidean configuration space. A final approach to deal with a non-Euclidean configuration space is to use implicit equations, i.e. to compute the required

constraint force [22] or solve a set of constrained equations at each time step. Because of its general applicability, this approach is used most often in physical simulation software packages, for example Modelica [23], or ODE (Open Dynamics Engine) [24] and its derivatives such as Webots [25]. III. PARAMETERIZATION OF 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 V. Section III-A gives a brief summary of the theory of homogeneous transformations and twists, which is then used in the definitions for general holonomic joints (Section III-B) and nonholonomic joints (Section III-C). Each definition is illustrated with examples of commonly encountered joints. A. Configuration and Velocities of Multibody Systems We first briefly summarize the theory and notation used to represent configurations and velocities of rigid bodies. We refer to [1] and [3] for a more extensive background; in this paper, we follow the notation of [3]. 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, it can be expressed by a 4 × 4 matrix Hba structured as   a Rb pab (3) Hba = 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 the 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 6 × 1 vector Tbc,a or 4 × 4 matrix T˜bc,a of the form  c,a    c,a ωb ω ˜b vbc,a c,a c,a ˜ Tb = c,a (4) or Tb := vb 0 0 with ωbc,a , vbc,a ∈ R3 the angular and linear velocity of the rigid body, respectively, and   0 −x3 x2 0 −x1  ∈ R3×3 x ˜ := (x∧) =  x3 (5) −x2 x1 0

for x ∈ R3 . 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 (3) as (6) T˜c,a = H c H˙ a H b b

a

b

c

with H˙ denoting the time-derivative of H (see [3] for details). The previous mathematical tools can be used to describe the configuration and velocity of any rigid body, possibly part of a mechanism. They form the basis of several algorithms that derive the dynamic equations of multibody systems, see for example [26], [27].

3

Φ(Q1 , ·) Q1

z

ϕ2

ϕ2

ϕ1

z

Ψ2

ϕ1

y

Ψ1 Ψ1

Ψ2 y

x x

ϕ2 ϕ1

Φ(Q2 , ·) Fig. 2. Two examples of generalized joints between two bodies with attached frames Ψ1 and Ψ2 : a ball joint (left) and a planar joint (right).

Q2 Fig. 1. The mapping Φ(Q, ϕ) provides a local Euclidean structure to a neighborhood of each each global coordinate Q ∈ Q.

B. Holonomic Joints Definition 1 (Globally parameterized 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: 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 the tangent space TQ Q at 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. Figure 1 illustrates the third property: every point of the allowed configuration space is assigned a unique coordinate Q, and the mapping Φ maps 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. For a joint with k 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. The coordinate mapping Φ can be chosen as the exponential coordinate mapping of the group, i.e. the mapping Φ(Q, ϕ) = Qe

Pk

i=1

bi ϕi

(7)

with bi the appropriate basis elements for the algebra. Indeed, Φ(Q, 0) = Qe0 = Q and the mapping maps local coordinates ϕ to a neighborhood of every Q ∈ Q. Example 1 (Ball joint): Consider the ball joint shown in Figure 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 H2 = H2 (Q) = ∈ R4×4 (8) 0 1 As the global velocity coordinates, we pick v ∈ R3 to be the angular velocity vector of frame Ψ2 relative to Ψ1 , expressed in Ψ1 . The relative twist between Ψ1 and Ψ2 then becomes   1 0 0  0 1 0      0 0 1 v 1 1,1   v 2  T2 = X(Q)v =  (9)   0 0 0 v 3  0 0 0 0 0 0 Finally, we can choose the coordinate exponential coordinates, i.e. as  1 Φ(Q, ϕ) = Qeϕ˜ = Q I + ϕ˜ + ϕ˜2 + 2

mapping Ψ using 1 3 ϕ˜ + . . . 6



(10)

with ϕ˜ = (ϕ∧) as in (5). Rodrigues’ formula [1] provides an explicit expression for this infinite series, but we only require the first few terms and hence do not need this expression. Example 2 (Planar joint): As a second example, consider the planar-motion joint shown in Figure 2. 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. 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 Q = Qrot Qtra = ∈ R2×3 (11) Q21 Q22 Q23 such that Qrot is a 2 × 2 rotation matrix and H21 (Q) equals   Q11 Q12 0 Q13 Q21 Q22 0 Q23   (12) H21 (Q) =   0 0 1 0  0 0 0 1

4

v1 Ψ2

Ψ1

(Q, v) and the coordinate mapping Φ of Example 2. 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. This constraint can be expressed simply as   AT (Q)v = 0 0 1 v = v3 = 0

v3

v2

Fig. 3. Example of a nonholonomic joint: a rolling-and-turning constraint between a body and the ground.

We choose the velocity coordinates v to describe the angular and linear velocities in body frame, i.e.   0 0 0  0   0 0    v1   1 0 0   T21,1 = X(Q)v =  (13)  Q23 Q11 Q12  v2   v3 −Q13 Q21 Q22  0 0 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. 

0

 ϕ  1

Φ(Q, ϕ) = Qe 0

−ϕ1 0 0



ϕ2  ϕ3   0

(14)

Note that an alternative (though less illustrative) choice of coordinates would have been the Euclidean set Q = (θz , dx , dy ) with rotation angle θz around z and translation distance (dx , dy ) along (x, y), ignoring the cyclic behavior (and hence multiple covering) of θz . C. Nonholonomic Joints Definition 2 (Globally parameterized 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 ¯ ¯ In addition to the A(Q) ∈ Rk×(k−k) of constant rank (k − k). properties given in Definition 1, a differentiable rank k¯ matrix ¯ ¯ ¯ S(Q) ∈ Rk×k must be chosen that satisfies 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 space. From the properties of A(Q), we can see that a suitable matrix S¯ always exists. The columns of S¯ form a basis for the space of allowed velocities, ¯v , or in other words, allowed velocities have the form v = S¯ ¯v = 0. automatically satisfying the constraint AT v = AT S¯ Example 3 (Planar joint continued): Consider the setup in Figure 3, which is an extension of Example 2 and Figure 2. The relative configuration of Ψ1 and Ψ2 is constrained to be a planar motion, and it can be parameterized by the coordinates

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 (15) 0 0

which has constant rank k¯ = 2 and clearly satisfies AT S¯ = 0. IV. T HE B OLTZMANN -H AMEL E QUATIONS IN L AGRANGIAN AND P ORT-H AMILTONIAN F ORM

In this section we briefly present a reformulation of the Boltzmann-Hamel equations [4]. 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). The following theorem shows how these equations can be formulated in Lagrangian and port-Hamiltonian form. Theorem 1: Given a mechanical system with position q, velocity v = S(q)q˙ (with S(q) invertible and continuously differentiable), inputs τ collocated with v, potential energy V (q) and kinetic co-energy1 Uk = 12 v T M (q)v. The dynamics can be written in (q, v) coordinates as !   X ∂L d ∂L −T ∂L −S + =τ (16) γk v k dt ∂v ∂q ∂v k

with L(q, v) := Uk (q, v) − V (q) = 21 v T M (q)v − V (q) and   X ∂Sjl ∂Sjm −1 −1 (γk )ij (q) := Sli Smk − (17) ∂qm ∂ql l,m

Equivalently, the dynamics are given in (q, p) coordinates as     " ∂H #   d q 0 S −1 0 ∂q = (18) ∂H + I τ −S −T J22 dt p ∂p with p := M (q)v, H(q, p) := 12 pT M −1 (q)p + V (q) and  T T  ∂ (S p) ∂(S T p) S −1 (19) J22 (q, p) := S −T − ∂q ∂q Eq. (18) can be augmented with the output equation y = ∂H ∂p . Proof: Given the definition of the function L(q, v) and the ¯ of the system coordinates v, we can write the Lagrangian L in (q, q) ˙ coordinates as 1 ¯ q) L(q, ˙ = q˙T S T (q)M (q)S(q)q˙ − V (q) 2

(20)

1 We distinguish between kinetic energy (when expressed in momentum coordinates) and kinetic co-energy (when expressed in velocity coordinates). See [28] for more information on this subtle distinction.

5

and its partial derivatives can be expressed as ¯ ∂L ∂ T (S q) ˙ ∂L ∂L = + ∂q ∂q ∂q ∂v ¯ ∂L ∂L = S T M S q˙ = S T ∂ q˙ ∂v

Ψi Qi

Q5

(taking B = S T since τ is collocated with v) gives   d ∂L ∂ T (S q) ˙ ∂L T ∂L S − − = ST τ dt ∂v ∂q ∂q ∂v     ∂L ∂L ∂L ∂ T (S q) ˙ T d T ˙ S − + S − = ST τ dt ∂v ∂q ∂q ∂v

(21)

∂I ∂(M −1 M ) ∂M −1 ∂M = = M + M −1 ∂qi ∂qi ∂qi ∂qi

we can write ∂( 1 pT M −1 p) ∂V ∂H = + 2 ∂qi ∂qi ∂qi −1 1 X ∂Mjk ∂V 1 X ∂Mjk ∂V + pj pk = − vj vk = ∂qi 2 ∂qi ∂qi 2 ∂qi j,k

∂( 1 v T M v) ∂L ∂V − 2 =− = ∂qi ∂qi ∂qi

Using this equation and p = ∂L ∂v , we can write (21) as   ∂H ∂ T (S q) ˙ p = ST τ S T p˙ + + S˙ T − ∂q ∂q   ∂(S T p) ∂ T (S T p) ∂H q˙ = S T τ + − S T p˙ + ∂q ∂q ∂q

Q4

Ψ4

Ψ2 Ψ3

Q2 Q3 Ψ0

Pre-multiplying (21) by S −T and expressing q˙ and S˙ T in terms of v gives (16), which are known as the Boltzmann-Hamel equations. Alternatively, since for any invertible matrix M and variable qi we have

j,k

Ψj

Ψ5

Substituting this into the classical Euler-Lagrange equations  ¯ ¯ ∂L d ∂L − = B(q)τ dt ∂ q˙ ∂q

0=

Qj

Ψ1 Q1

Fig. 4. Setup of a general open-chain rigid-body 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 .

definition of Φ) to obtain the dynamics expressed in global coordinates Q and v as desired. For nonholonomic joints, we perform the extra step of projecting the results onto the space of allowed velocities. Even though local coordinates ϕ appear in the derivations of the various equations, the final equations are all evaluated at ϕ = 0 and hence these final equations do not depend on local coordinates. The global coordinates Q and v are the only dynamic state variables and the equations are globally valid, without the need for coordinate transitions between various areas of the configuration space, as is required in methods that use an atlas of local coordinate patches. A. Mechanisms with only holonomic joints

(22)

Pre-multiplying (22) by S −T and substituting q˙ = S −1 ∂H ∂p gives the port-Hamiltonian form (18) of the Boltzmann-Hamel equations. Note that if the coordinates v are integrable, then the terms γk in (17) and J22 in (19) are identically zero. V. DYNAMICS OF G ENERAL M ULTIBODY S YSTEMS In this section, we discuss the procedure to derive the dynamics of rigid-body mechanisms with general joints as defined in Section III. We only consider systems without closed kinematic chains, so-called open-chain mechanisms. In order to be able to formulate the system in terms of Theorem 1, we derive an expression for the kinetic co-energy of a mechanism, expressed in coordinates Q, v, but locally parameterized by the coordinate mappings Φ for each joint. Then, we express the dynamics in local coordinates ϕ for configuration and v for velocity, and consider Q a parameter. After taking partial derivatives of the Lagrangian or Hamiltonian function, we then evaluate the results at ϕ = 0 (i.e. at configuration Q, by

We start by specifying an open-chain rigid-body mechanism as in Figure 4: n rigid bodies are connected in a tree structure by n ideal joints, each with global configuration coordinate Qi , velocity coordinate vi , and coordinate mapping Φi (Qi , ϕi ). Each joint has ki degrees of freedom, which means P the total n number of degrees of freedom for the mechanism is i=1 ki , which can be larger than the number of rigid bodies n. 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 (6) 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

(23)

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

6

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 [3]), we can derive an expression for the mass 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 as required. Theorem 2 (Mechanism dynamics for holonomic joints): Given an n-link open-chain rigid chain mechanism with general holonomic joints, labeled as in Figure 4, 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 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) = τ M (Q)v˙ + C(Q, v)v + N

(24)

with −T ∂V ˆ (25) N (Q) := S ∂ϕ ϕ=0   X ∂Mij 1 −1 ∂Mjl vl Cˆij (Q, v) := S −1 − Ski ∂ϕk kl 2 ∂ϕk ϕ=0 k,l    X  ∂Sjm ∂Sjs −1 −1 + Smi − Ssk Mkl vl (26) ∂ϕs ∂ϕm ϕ=0 k,l,m,s

Equivalently, for p := M (Q)v the dynamics equations are   ∂H ∂H + J22 p˙ = −S −T +τ (27) ∂ϕ ∂p ϕ=0 with H(Q, p) = 21 pT M −1 (Q)p + V (Q) and   T T ∂ (S p) ∂(S T p) S −1 − J22 (Q, ϕ, p) := S −T ∂ϕ ∂ϕ

(28)

Partial derivatives of M (Q), H(Q, p), and V (Q) with respect to ϕ are to be obtained by first substituting the coordinate mappings Φ(Q, ϕ) for Q and then taking the partial derivative. Proof: The theorem follows more or less directly by applying the Boltzmann-Hamel equations of Theorem 1 to the rigid-body mechanism expressed in (ϕ, v) or (ϕ, p) coordinates around a certain point Q and evaluating at ϕ = 0. More precisely, if we parameterize the configuration around a certain fixed point Q as Φ(Q, ϕ), we can write the Lagrangian L and Hamiltonian H of the mechanism as 1 T v M (Φ(Q, ϕ))v − V (Φ(Q, ϕ)) 2 1 H(ϕ, p) = pT M −1 (Φ(Q, ϕ))p + V (Φ(Q, ϕ)) 2 L(ϕ, v) =

(29) (30)

We can then apply Theorem 1 and calculate !   X ∂M d ∂L = ϕ˙ k v + M v˙ dt ∂v ∂ϕk k

∂L 1 ∂M ∂V = vT v− ∂ϕk 2 ∂ϕk ∂ϕk

ˆ) Combining all elements (and using the definitions of Cˆ and N results in expression (24). The port-Hamiltonian formulation (27) is obtained directly from (18) by replacing q with ϕ and evaluating at ϕ = 0. It is important to note that only the global coordinates Q appear in the expression for the dynamics and are states of the dynamical system; the local coordinates ϕ are evaluated at zero. This means that (24) 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) or (Q, p), the dynamic equations (24) and (27) are augmented with the kinematic equations ˙ ˙ Q˙ = Q(Q, v) = Q(Q, M −1 p)

(31)

which relate the velocity coordinates to the time derivatives of the position coordinates and are determined at joint level by the relations Hi⋆ = Hi⋆ (Qi ) and Ti⋆,⋆ = Xi (Qi )vi . For a general joint, the elements of Q may be dependent — indeed, the very motivation for the presented method is to allow for such joints. Numerical integration of (31) is hence not trivial and, for finitely small step-sizes, can result in the computed solution matrix Q not satisfying these dependencies. Such issues can be avoided by projecting the computed matrix Q onto the allowed configuration space Q [29] or by using specific structure-preserving integration methods [30]. The integration of (31) for a particular joint does not depend on the mechanism in which the joint is used, and hence it should be dealt with at the joint level. In a software implementation with a library of different joint types, each library element can describe a particular joint type and additionally specify an accurate and efficient method to numerically integrate (31) for that joint. For the overall result to be accurate, it is important that this local integration at the joint level preserves the required structure of Q. Example 4 (Euclidean joints): For Euclidean joints with configuration coordinates Q = q ∈ Rn and velocity coordinates v = q, ˙ the dynamics equations reduce to the standard equations (1) and (2) known from the literature, ˆ ˆ (Q) = N (q), and i.e. S(Q, ϕ) ≡ I, C(Q, v) = C(q, q), ˙ N J22 ≡ 0. Example 5 (Ball joint continued): We give an example of how the term J22 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

7

configurations Φ(Q, ϕ) around fixed Q ˙ v˜ = Φ(Q, ϕ)ΦT (Q, ϕ)   1 ˙ ˙ ˙ = Q ϕ˜ + (ϕ˜ϕ˜ + ϕ˜ϕ) ˜ + . . . (I − ϕ˜ + . . .) QT 2   1 ˜ + . . . QT = Q ϕ˜˙ + (ϕ˜ϕ˜˙ − ϕ˜˙ ϕ) 2 where dots indicate terms of second and higher order in ϕ. This relation can be written in vector form as   1 (32) v = Q I + ϕ˜ + . . . ϕ˙ = S(Q, ϕ)ϕ˙ 2

which gives the relation between v and ϕ. ˙ This in turn can be used to compute   ∂ 1 ∂(S T p) T = (I − ϕ ˜ + . . .)Q p ∂ϕ ϕ=0 ∂ϕ 2 ϕ=0   ∂ 1 T = − ϕQ ˜ p + . . . ∂ϕ 2   ϕ=0 ∂ 1^ = (QT p)ϕ + . . . ∂ϕ 2 ϕ=0

1 1^ T p) = QT p ˜Q = (Q 2 2 with again x e := (x∧) ∈ R3×3 , from which we find the (rather simple) expression for J22 as   T T ∂ (S p) ∂(S T p) −1 −T S − J22 (Q, ϕ, p) =S ∂ϕ ∂ϕ ϕ=0 ϕ=0   1 T 1 T p = Q − Q p˜Q − Q p˜Q QT = −˜ 2 2 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. B. 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 a mechanism with velocity v (or momentum p) are constrained to satisfy extra equations of the form AT (Q)v = AT (Q)M −1 (Q)p = 0

(33)

These constraints are enforced by constraint forces λ that enter the dynamics equations as extra terms A(Q)λ on the righthand sides of equations (24) and (27). The magnitude of λ is such that (33) remains satisfied at all times. The following theorem presents how the dynamics equations for the restricted systems can be obtained. Theorem 3 (Mechanism dynamics for general joints): Given an n-link open-chain rigid-body mechanism with general holonomic and nonholonomic joints, labeled as in Figure 4, 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 the reduced velocity coordinates v¯ and ¯ ¯ full velocity coordinates v as v = S(Q)¯ v (with S(Q) ≡ I for holonomic joints). Let the mechanism 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)v¯˙ + C(Q, ¯ ˆ (Q) = S¯T (Q)τ M v¯)¯ v + S¯T (Q)N (34) with ¯ (Q) := S¯T M S¯ M  X  ∂ S¯
¯ ¯v + S¯T Cˆ S¯ C(Q, v¯) := S¯T M S −1 S¯ i ∂ϕ i ϕ=0 i

ˆ ˆ ¯v ) and N ˆ (Q) as in Theorem 2. where C(Q, v) = C(Q, S¯ T ¯ ¯ Equivalently, for p¯ = S p = M v¯, the dynamics are  ¯ ¯  ∂H ∂H ¯ ˙p¯ = −S¯T S −T + J22 + S¯T τ (35) ∂ϕ ∂ p¯ ϕ=0

¯ −1 p¯ + V (Q) and ¯ with H(Q, p¯) := 12 p¯T M ∂(S¯T p) −1 ¯ ¯T −T ∂ T (S¯T p) S S−S S (36) J¯22 := S¯T J22 S¯ + ∂ϕ ∂ϕ ¯ −1 p¯. where p is to be evaluated as p = M −1 S¯M 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 (37)

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 (37) by S¯T , we obtain   ¯˙ v + S¯T Cˆ S¯ ¯v + S¯T N ˆ = S¯T τ + S¯T Aλ (38) 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. From the definitions ¯ as in the theorem, it can be shown that of p¯ and H ¯ ¯ ∂H ∂H ∂H ∂ T (S¯T p) ∂ S¯ = + pT v¯ = + v¯ ∂ϕi ∂ϕi ∂ϕi ∂ϕi ∂ϕi Using this equality, we can express the time-derivative of p¯ as follows. p¯˙ = S¯˙ T p + S¯T p˙

∂H ∂H + S¯T J22 + S¯T τ + S¯T Aλ = S¯˙ T p − S¯T S −T ∂ϕ ∂p ¯ ∂(S¯T p) −1 ¯ ∂H ∂ T (S¯T p) = S S¯ v − S¯T S −T − S¯T S −T v¯ ∂ϕ ∂ϕ ∂ϕ ¯ ∂H + S¯T τ + S¯T J22 S¯ ∂ p¯ which can be written as in the theorem. An expression for λ can be obtained by pre-multiplying (37) ¯v , which gives 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¯

8

Since AT M −1 A is invertible, this expression uniquely determines λ from the state (Q, v¯) in an equation that complements (34). 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 λ, a result equivalent to [6]. Example 6 (Planar joint continued): Consider again the planar joint of Example 2 with nonholonomic constraints as described in Example 3 and shown in Figure 3. 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 T 1 T 0 m 0v Uk (Q, v) = v M (Q)v = v 2 2 0 0 m To find the mapping S, we compare the two expressions for T21,1 (one in coordinates v, one in coordinates ϕ): ˙   0 −v1 0 Q23 v1 + Q11 v2 − Q12 v3  0 0 −Q1 3v1 + Q21 v2 + Q22 v3  ] = v 1  T˜21,1 = (Xv)  0 0 0 0 0 0 0 0   0 −ϕ˙ 1 0 ϕ˙ 2 ϕ˙ 1 0 0 ϕ˙ 3   H2 T˜21,1 = H˙ 21 H12 = H21  0 0 0 0 1 0 0 0 0

to find S(Q, ϕ) ≡ I. The dynamics 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 v˙ 3 0 0 m

Using the definition of (the constant) S¯ from Example 3, the reduced equations in terms of v¯ become      τ1 J 0 v¯˙ 1 + 0 + 0 = 0 m v¯˙ 2 τ2 The dynamics in this case are not very interesting, but the example serves to illustrate the systematic procedure of obtaining them, and the generality of using this particular joint as part of a larger mechanism, as illustrated in the next example. Furthermore, the kinematics, i.e. the relation between Q and v¯, also follows from the theory, in general as ^ ˙ = T˜1,1 (Q, v¯)H21 (Q) = (X(Q) ¯ H˙ 21 (Q, Q) S(Q)¯ v )H21 (Q) 2 which for this example can be simplified to   0 −¯ v1 v¯2 0 0 Q˙ = Q v¯1 0 0 0

Example 7 (General mechanism): As a final example, we sketch how the proposed algorithm would work for the general mechanism in Figure 5. The mechanism is comprised of an inertial reference frame Ψ0 , a rotating plane (with attached

Ψ3

Ψ2 Ψ1

Ψ0

Fig. 5. Mechanism with three general joints: one rotation, one nonholonomic planar joint, and one ball joint.

frame Ψ1 ), a moving cart (frame Ψ2 ), and a final end effector (frame Ψ3 ), connected to the cart through a ball joint. The total dynamics of this system are obtained by combining the expressions of the individual joints, as computed in the previous examples. In the interest of conciseness, we do not show the complete expressions here, but only sketch the structure of the equations and the approach to obtain them. The global configuration is described by the collection of joint configuration matrices Qi , i.e. Q = {Q1 , Q2 , Q3 } ∈ R × R2×3 × R3×3

(39)

where we picked the rotation angle for Q1 , the matrix (11) for Q2 , and the matrix Q from (8) for Q3 . The local configuration coordinates ϕ ∈ R7 are the concatenation of the local joint coordinates ϕi used in the previous examples. The global velocity coordinates v ∈ R7 (before applying the nonholonomic constraints) are the concatenation of the global joint velocity coordinates vi , and are related to the local joint configuration coordinates through the matrix S(Q, ϕ) ∈ R7×7 as    1 0 0 ϕ˙ 1  ϕ˙ 2  0 (40) v = S(Q, ϕ)ϕ˙ = 0 I ϕ˙ 3 0 0 S3 (Q3 , ϕ3 )

with S3 (Q3 , ϕ3 ) equal to the expression for S(Q, ϕ) in (32). The global velocity coordinates v¯ ∈ R6 (after applying the nonholonomic constraint to the cart) are obtained using (15) ¯ for the second joint, and are related to v by S(Q) ∈ R7×6 as   1  0  0   1 0   ¯    0 1 0 (41) v = S(Q, ϕ)¯ v = 0  v¯   0 0 0 0 I Given these expressions for the global and local coordinates and their relations, as obtained simply by concatenating the results of the previous examples, the dynamics in v¯ or

9

p¯ coordinates can be obtained directly from the theorems. Since S and S¯ have a block diagonal structure, their partial derivatives only depend on the partial derivatives of the block elements with respect to the local coordinates related to that specific block element. These partial derivatives can hence be computed beforehand on a joint-by-joint basis, without the need to know the mechanism in which the joints are being used. VI. 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 and portHamiltonian results known from the 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 and momentum 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 terms that appear in the dynamic equations 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, ϕ) and its partial derivatives 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 [31]. As a possible extension for future work, the precise structure ¯ H of the expressions for C¯ and ∂∂ϕ should be analyzed in more detail, in order to find fast and numerically robust ways to express these terms in simulation code. This may also reveal possible numerical differences between the expressions in coordinates v¯ or p¯, i.e. in Lagrangian and port-Hamiltonian form. The difference at this point is mostly a matter of taste; the formulations are mathematically equivalent. 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] B. M. Maschke and A. J. van der Schaft, “Port-controlled Hamiltonian systems: Modelling origins and system-theoretic properties,” in IFAC Symposium on Nonlinear Control Systems, June 1992, pp. 282–288. [3] S. Stramigioli, Modeling and IPC Control of Interactive Mechanical Systems – A Coordinate-free Approach. Springer-Verlag, 2001. [4] E. T. Whittaker, A Treatise on the Analytical Dynamics of Particles and Rigid Bodies, 4th ed. Cambridge University Press, 1998. [5] J. G. Papastavridis, Analytical Mechanics: A Comprehensive Treatise on the Dynamics of Constrained Systems; For Engineers, Physicists, and Mathematicians. Oxford University Press, 2002. [6] R. Featherstone and A. Fijany, “A technique for analyzing constrained rigid-body systems, and its application to the constraint force algorithm,” IEEE Transactions on Robotics and Automation, vol. 15, no. 6, pp. 1140–1144, December 1999. [7] F. Aghili and J. C. Piedboeuf, “Simuation of motion of constrained multibody systems based on projection operator,” Multibody System Dynamics, vol. 10, pp. 3–16, 2003. [8] 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. [9] F. C. Park, J. E. Bobrow, and S. R. Ploen, “A Lie group formulation of robot dynamics,” International Journal of Robotics Research, vol. 14, no. 6, pp. 609–618, 1995. [10] A. M¨uller and P. Maißer, “A Lie-group formulation of kinematics and dynamics of constrained MBS and its applications,” Multibody System Dynamics, vol. 9, pp. 311–352, 2003. [11] J. M. Selig, Geometric Fundamentals of Robotics, 2nd ed. SpringerVerlag, 2005. [12] J. E. Marsden and T. S. Ratiu, Introduction to Mechanics and Symmetry, 2nd ed., ser. Texts in Applied Mathematics. Springer-Verlag, 1999, vol. 17. [13] 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. [14] J. W. Gibbs, “On the fundamental formulae of dynamics,” American Journal of Mathematics, vol. 2, pp. 49–64, 1879. [15] A. D. Lewis, “The geometry of the Gibbs-Appel equations and Gauss’s principle of least constraint,” Reports on Mathematical Physics, vol. 38, no. 1, pp. 11–28, 1996. [16] 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. [17] M. Lesser, “A geometrical interpretation of Kane’s equations,” Proceedings: Mathematical and Physical Sciences, vol. 436, no. 1896, pp. 69– 87, January 1992. [18] 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. [19] 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. [20] 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. [21] 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. [22] S. W. J. Welch, M. Hong, J. A. Trapp, and M. H. Choic, “Parameterfree second-order numerical scheme for constrained multibody dynamic system,” Journal of Guidance, Control, and Dynamics, vol. 30, no. 5, pp. 1494–1503, 2007. [23] M. Otter, “The new Modelica multibody library,” in Proceedings of the Third International Modelica Conference, P. Fritzson, Ed., November 2003. [24] R. Smith, “Open dynamics engine (ODE),” 2006. [Online]. Available: http://www.ode.org [25] Cyberbotics, “Webots 5,” 2006. [Online]. Available: http://www.cyberbotics.com [26] R. Featherstone, Robot Dynamics Algorithms, ser. The International Series in Engineering and Computer Science. Springer-Verlag, June 1987, vol. 22.

10

[27] J. Park, “Principle of dynamical balance for multibody systems,” Multibody System Dynamics, vol. 14, no. 3-4, pp. 269–299, November 2005. [28] D. C. Karnopp, D. L. Margolis, and R. C. Rosenberg, System Dynamics: Modeling and Simulation of Mechatronic Systems, 4th ed. WileyInterscience, 2006. [29] R. I. McLachlan and G. R. W. Quispel, “Geometric integrators for ODEs,” Journal of Physics A: Mathematical and General, vol. 39, no. 19, pp. 5251–5285, May 2006. [30] H. Munthe-Kaas, “Runge-Kutta methods on Lie groups,” BIT Numerical Mathematics, vol. 38, no. 1, pp. 92–111, 1998. [31] Control Lab Products, “20sim version 3.6,” 2005. [Online]. Available: http://www.20sim.com