Optimal dynamics of actuated kinematic chains. Part 1: Dynamic modelling and symbolic differentiations

European Journal of Mechanics A/Solids 24 (2005) 452–471

Optimal dynamics of actuated kinematic chains. Part 1: Dynamic modelling and symbolic differentiations Guy Bessonnet

∗

, Philippe Sardain

Laboratoire de Mécanique des Solides, UMR6610, Université de Poitiers, CNRS, SP2MI, Bd. M. & P. Curie, BP 30179, 86962 Futuroscope Chasseneuil cedex, France Received 9 October 2003; accepted 4 March 2005 Available online 12 April 2005

Abstract The problem of modelling dynamics of open-loop multibody systems is addressed in the present Part-1 of the paper with optimization of motion in mind. The optimization technique implemented in the complementary Part-2 is the Pontryagin Maximum Principle (PMP) which requires deriving equations of motion in state space form, and makes necessary to carry out higher order differentiation in order to formulate some optimality conditions. So as to fulfil these requirements, an algebraic differentiation technique is developed in the present paper, which results in formulating, in the same global computational scheme, Lagrangian and Hamiltonian equations of motion together with the Jacobian matrix of phase-velocities involved in the conditions for optimality stated by the PMP. All formulations required are formally exact and essentially non-redundant which will ensure safer and faster numerical processing. The structure of the final algorithm was used to develop a computerized symbolic formulation of the entire optimization problem. The file which results can be used directly by the numerical solver. Such a symbolic computation code has proved to be an essential tool to cope with the huge complexity of formulations involved in the statement of the optimization problem dealt with in part 2 of the paper.  2005 Elsevier SAS. All rights reserved. Keywords: Multibody systems; Hamiltonian dynamic model; Symbolic differentiations

1. Introduction Within the revival of classical mechanics arising through new fundamentals such as chaos theory, symplectic geometry, Lie algebra and optimization theory, the multibody system theory has enjoyed a renewal of interest which is flourishing for more than three decades. This now wide-ranging field of research in engineering sciences is mainly rooted in the early developments of robotics and deployable space structures. The need for dynamic analysis of multibody systems has been constantly increasing in a number of other technological fields such as mechanisms, heavy machinery, land vehicles, and, in other respects, articulated biosystems. Three main topics can be distinguished in this widespread field of research: dynamic analysis, dynamic modelling, and dynamic optimization of motions that we will term numerical dynamic synthesis. The present paper (Part 1) and its companion paper (Part 2) are related to and take from these three general subjects. However, dynamic synthesis is the central problem we * Corresponding author.

E-mail address: [email protected] (G. Bessonnet). 0997-7538/$ – see front matter  2005 Elsevier SAS. All rights reserved. doi:10.1016/j.euromechsol.2005.03.001

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

453

are concerned with. The reader is referred to the Part 2 of the paper for a detailed presentation of the general dynamic motionsynthesis problematic. Simply, let us point out here that motion optimization is especially useful in the field of robotics to carry out optimal trajectory planning. It could be helpful in simulating and designing controlled mechanical systems. Dynamic optimization presents a great interest too in legged-locomotion to generate reference gait patterns of walking machines such as human-like bipeds. The same approach may be developed to perform human movement synthesis such as walking and running, as well as acrobatic and athletic exercises. It could be helpful as well, for creating realistic movements in graphic animations. As the evolution of a mechanical system is governed by a set of differential equations, motion optimization falls naturally into the scope of optimal control theory which provides a powerful mathematical tool, the Pontryagin Maximum Principle (or PMP), to deal with optimal control problems. Our choice of implementing the PMP is thoroughly explained in Part 2 of the paper. Here, we only mention that this optimization tool calls for a specific requirement: the state equation which prescribes the evolution of the dynamic system must be solved in the phase velocities. In the field of dynamics, Hamilton’s equations naturally fulfil this condition. It is well known that Hamilton’s formalism is conceptually fertile, and has given rise to far-reaching mathematical developments in the field of purely dynamical systems (Arnold, 1989). However, unlike Newtonian and Lagrangian formulations, Hamilton’s equations have not been used very often for dynamic analysis of controlled mechanical systems. One can find earliest applications in Skowronski (1986) and Flashner and Skowronski (1989) for deriving control laws in robotics. The approach is essentially formal, not really ready for use. More recently, Borri et al. (1992) followed by Sika and Valasek (1997) developed a new formulation of motion equations of constrained mechanical systems based on a modified state space form using Hamilton’s canonic momenta. The result is an improved stabilisation technique designed for solving constrained forward dynamic problems. In the field of dynamic optimization, authors like Ailon and Langholz (1986) and Chen and Desrochers (1990) employed a Hamiltonian formulation with the aim of establishing existence results of the minimization control problem when considering multibody systems. Only a basic formulation is needed for the authors’ purpose. The first part of the paper is aimed at developing extensively the Hamiltonian formulation in order to deal efficiently with motion optimization using Pontryagin’s Maximum Principle. The algebraic structure of Hamilton’s equations makes it possible to clarify and simplify the formulations derived from the application of the PMP. However, the outstanding interest of the Hamiltonian model lies in the fact that it ensures a better convergence of the solving algorithms. Dynamic modelling of multibody systems leads to handling considerable amounts of symbols when formulating motion equations of non-planar mechanical systems. Tourassis and Neuman (1985) mentioned that writing a full Lagrangian dynamic model of a manipulator arm with “only” six internal degrees of freedom represents about forty type-written pages. However, we need much more than the formulation of a dynamic model. In fact, stating and solving an optimization problem requires generally the derivation of some Jacobians and gradients of cost functions and constraints. These formal operations may take on considerable computing proportions when they are derived from dynamic models of multibody systems. Such complementary formulations are an essential prerequisite to perform dynamic synthesis which requires a high degree of computational accuracy to cope with numerical instabilities intrinsically attached to the dynamic models of multibody systems. For this reason, the developments presented in this paper are not limited to the Hamiltonian formulation itself. They are extended to the formal differentiation of the Hamiltonian phase velocities, which results in a Jacobian matrix needed to formulate the optimality conditions stated by the PMP. These developments are based on two complementary approaches: (i) A modified global formulation of Hamilton’s equations, which exhibits a simple computational structure, quite easy to differentiate. (ii) An integral representation of dynamic coefficients of Lagrange’s equations developed in Fayet and Renaud (1989), Fayet and Pfister (1994) and Pfister (1997) which is perfectly appropriate to achieve the higher order differentiation completing the above formulations. The result is a comprehensive computational scheme which organizes in exact and compact form all the formulations derived from the Hamiltonian formalism (as well as the Lagrangian one), and that are required for stating and solving the optimal control problem we have in mind. A symbolic computing software has been created to generate the entire symbolic formulations of the optimal problem, ready to use by the numerical solver. This computing code has proved to be an essential tool to overcome the huge computational complexity of both the multibody system dynamics and the derived optimality conditions. The paper is organized as follows. A basic optimization problem is stated as a reference problem in Section 2. A Hamiltonian dynamic model is developed in Section 3 in a form suitable for dynamic optimization. A maximal set of independent Christoffel inertia-coefficients is identified in Section 4. The Jacobian matrix of Hamiltonian phase velocities is derived in Section 5 by differentiating the Christoffels with respect to the state variables. Differentiation is achieved using the concept of global inertia tensor initially developed in Fayet and Renaud (1989). Then, a symbolic computation algorithm, which generates the formulations of the entire optimization problem, is presented in Section 6. Concluding remarks are formulated in Section 7.

454

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

Fig. 1. Robotic manipulator described as an open kinematic chain made up of six links labelled from (L1 ) to (L6 ); the last three axis of rotation are concurrent. The six rotation coordinates: q1 , . . . , q6 , are defined using the Denavit–Hartenberg construction (see Appendix A1).

2. Stating a basic motion optimization problem We need to identify the basic content of the optimization problem to be dealt with in order to clearly define the formulations we intend to develop in the present Part 1 of the paper. As a simple reference example, let us consider the serial robotic manipulator shown in Fig. 1. We denote q = (q1 , . . . , qnq ) the vector of joint coordinates describing the rotations of its six rotational joints (nq = 6), all of them being actuated. This mechanical system has six internal degrees of freedom (d.o.f.). A usual task to be performed consists in moving an object from an initial location to a final position. In practice, this simple pick-and-place task can be repetitive and performed at fast rate. The question which naturally arises is: how to ensure soft and efficient functioning while improving the manipulator working performances? A common answer is: by controlling all active joints so as to achieve the best dynamic coordination of joint motions, while minimizing the overall motion time together with the actuating inputs and/or the energy consumption. This optimization problem is typically an optimal control problem whose basic statements are: (i) A state equation describing the dynamic evolution of the multibody system over a time interval such that
 t ∈ [t0 , t1 ], x˙ (t) = f x(t), u(t)

(1)

where x is the nx -vector of state variables, while u is the nu -vector of actuating inputs, i.e. the control variables (nu = nq for the manipulator shown in Fig. 1). Most often, the state variables in x are the (2nq )-vector of phase coordinates: x := ˙ q˙ := dq/dt. (q, q), (ii) A performance criterion to be minimized t1 Min J (u),

J (u) =



x(t), u(t) dt

(2)

t0

which can combine, for instance, travelling time, energy consumption, integral quadratic norm of actuating torques. As we intend to implement the Pontryagin Maximum Principle (Pontryagin et al., 1962) for solving optimisation problems such as (1) and (2), we anticipate the statement of two main conditions for optimality with which the present Part 1 of the paper is concerned. Defining the Hamiltonian function H ∗ such that y ∈ 2n ,

H ∗ (x, u, y) := yT f(x, u) −
(x, u),

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

455

the PMP states that (see, e.g., Lewis and Syrmos, 1995) if (x, u) is an optimal control process (i.e. a solution of (1) minimizing (2)), then, there exists a costate time vector-function y(t) ∈ 2nq verifying the costate vector-equation (or adjoint system) y˙ T = −∂H ∗ /∂x

t ∈ [t0 , t1 ],

(3)

and the maximality condition for the Hamiltonian  ∂H ∗ /∂u = 0, if u ∈ nu ,

 t ∈ [t0 , t1 ], H ∗ x(t), u(t), y(t) = Max H ∗ x(t), v, y(t) , v∈U

if u ∈ U ⊂ nu .

(4)

In the second relationship in (4), U stands for a bounded subset of nu . Prior to any numerical processing of this resulting problem, a preliminary task to be achieved is the explicit formulation of conditions (1), (3) and (4). In particular, differentiations of H ∗ , with respect to x and u, must be thoroughly carried out. Obviously, deriving ∂H ∗ /∂x and ∂H ∗ /∂u needs to compute the Jacobian matrices ∂f/∂x, ∂f/∂u, and the gradients ∂
/∂x, ∂
/∂u. In a wide range of cases, explicit formulations of
, ∂
/∂x, ∂f/∂u and ∂
/∂u are straightforward. However, dealing with the function f and its Jacobian matrix ∂f/∂x requires handling huge amounts of arithmetic operations when coping with complex articulated systems. In such cases, a dynamic model organized according to an appropriate structure with as few computational redundancies as possible is needed for developing the formulations of f and ∂f/∂x. The fulfilment of such requirements is the main objective of the present paper.

3. Hamiltonian versus Lagrangian dynamic model Dynamics-based control of powered multilink systems requires direct assessment of joint coordinates and velocities. Thus, the Lagrangian formulation which is based on the introduction of generalized coordinates and their time derivatives is fully appropriate to define the actuating inputs required to control the motion. But, before controlling the motion, it must be properly organized. Since we are interested in generating optimal movements, a set of first order differential equations is required as indicated in (1) to describe the state evolution of the mechanical system. It is well known that the Hamiltonian formulation meets intrinsically this requirement. Furthermore, its formal structure offers significant advantages for deriving all formulations needed for dealing with the optimization problem. 3.1. Lagrangian state equation The mechanical system, denoted by S, is assumed to be moving with respect to a fixed base. Using the notations introduced ˙ the kinetic energy T of S is obtained as the quadratic form in Section 2 above, for q and q, 1 ˙ = q˙ T M(q)q˙ T (q, q) 2

(5)

where M is the symmetric, positive-definite mass matrix of S. Let V (q) be the potential energy of the conservative force fields including gravity and possibly elastic forces from compensating devices. Then, Lagrange equations of motion can be formulated as   ∂T ∂V d ∂T − =− + Qi , i = 1, . . . , nq . (6) dt ∂q˙i ∂qi ∂qi In (6), the generalized forces Qi can be expressed as
 ˙ + Bτ i , Qi = D(q, q) where D stands for the vector of generalized dissipative forces Di due to joint friction and damping, while τ stands for the vector of joint actuating torques τi . B is a constant (nq × nq )-matrix which results from the choice of generalized coordinates. Taking (5) into account, Lagrange equations (6) can be rewritten in the classical more explicit form   i
nq , Mij q¨j + Γi,j k q˙j q˙k = −V,i + Di + (Bτ )i , (7) j

j,k

where the Christoffel symbols Γi,j k are defined as the well known linear combinations   ∂Mj k 1 ∂Mij ∂Mki + − Γi,j k = 2 ∂qk ∂qj ∂qi

(8)

456

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

depending on the q-derivatives of the inertia matrix (see, e.g., Lur’e, 1968). Now, setting v = (v1 , . . . , vnq )T , u := τ ,  Ci (q, v) := Γi,j k (q)vj vk , C = (C1 , . . . , Cnq )T ,

vi := q˙i ,

V,q = (V,1 , . . . , V,nq )T ,

j,k

the set of second-order equations (7) can be equivalently written as the two first-order nq -vector differential equations solved for the Lagrangian phase velocities q˙ and v˙  q˙ = v,

(9) v˙ = M−1 (q) −C(q, v) − ∂V (q)/∂q + D(q, v) + M−1 (q)Bu. At this point, we can consider the Lagrangian phase coordinates as the state vector of the controlled mechanical system, namely x := (q, v). Then, the double vector equation (9) gets the form of the generic state equation (1). However, as shown in the section below, this Lagrangian formulation is not really appropriate for deriving optimality conditions stated by the PMP. 3.2. Hamiltonian state equation Define the Lagrangian of the mechanical system: ˙ := T (q, q) ˙ − V (q). L(q, q)

(10)

We recall that the Hamiltonian formalism is based on the introduction of the conjugate momentum p = (p1 , . . . , pnq )T ,

pi = ∂L/∂q˙i , i = 1, . . . , nq ,

(11)

˙ namely together with the Hamiltonian given as the Legendre transformation of q˙ → L(q, q), ˙ H (q, p) := pT q˙ − L(q, q).

(12)

In terms of canonical variables q and p, Hamilton’s equations are derived from (6), through (11) and (12), as the well known set of first-order vector-equations  q˙ = ∂H /∂p, (13) p˙ = −∂H /∂q + D + Bu. Using the expression (5) for T , in (12) through (10), the basic equations (13) can be reformulated more explicitly as  q˙ = M−1 (q)p, p˙ = − 12 pT (∂M−1 /∂q)p − ∂V /∂q + D + Bu.

(14)

It should be noted that the second equation in (14) is expressed on the basis of the q-derivative of the inverse mass matrix. This underlies intricate formulations when considering multibody systems whose mass matrix may have quite involved dependency on the qi s. The result could be an excessive computational cost of the formulation (14), and even more for its Jacobian. For this reason, a transformation of the second equation in (14) is necessary. First, let us define the vector-function Fq as  q q q −1 Mij pj , i
nq , (15) Fq = (F1 , . . . , Fnq )T , Fi = j

and introduce the short notations for any differentiable function Φ(q): Φ,q := ∂Φ/∂q,

Φ,i := ∂Φ/∂qi .

Next, given that −1 −1 M−1 ,i = −M M,i M ,

one gets the identity q T q pT M−1 ,i p ≡ −(F ) M,i F .

(16)

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

457

As a result, the problem of computing the derivatives M−1 ,i is eliminated. Consequently, a low algorithmic cost for computing Hamilton’s equations (14) is as follows  (a) Fq (p, q) := M−1 (q)p,     (b) Fp (q, p) := 1 (Fq )T M Fq − V + D, ,q ,q 2 (17) q (q, p),  ˙ (c) q = F    (d) p˙ = Fp (q, p) + Bu, p

p

p

where Fp = (F1 , . . . , Fnq )T , and Fi = 1/2(Fq )T M,i Fq − V,i + Di , i = 1, . . . , ,nq . Now, using the notations x = (x1 , . . . , x2nq )T := (q1 , . . . , qnq , p1 , . . . , pnq )T , q

q

p

p

F = (F1 , . . . , F2nq )T := (F1 , . . . , Fnq , F1 , . . . , Fnq )T ,

(18)

both vector-equations (c) and (d) in (17) can be written as the single generic 2nq -order state vector-equation x˙ = F(x) + B∗ u := f(x, u),

(19)

where B* is a constant matrix such as   0nq ×nq B∗ = . Bnq ×nq As far as we are aware, the formulation (17) was not developed in the field of multibody system dynamics. Authors like Skowronski (1986), Flashner and Skowronski (1989) put forward the Hamiltonian state–space formulation to deal with dynamics-based control of mechanical systems. But, they only used formally the non-developed equations (13). Also mention that Chen and Desrochers (1990) made use of the formulation (14) to optimize the motions of a SCARA type manipulator with two degrees of freedom. The simple structure of Eqs. (17) is quite suitable for developing optimality conditions resulting from the optimization problem summarized in Section 2. Particularly, an interesting feature of (17) versus its Lagrangian counterpart (9) lies in the fact that the factor M−1 is shifted from the control variable term in (9) to the velocity equation in (14) or (17). As a first consequence, the formulation of the optimal control u, which results from condition (4), is noticeably simplified in the Hamiltonian case. Furthermore, in most cases, the time function t → u(t) exhibits some numerical stiffness such as non-differentiability, sharp variations and discontinuities. In the Lagrangian model, the factorization of u by M−1 results in injecting u in the adjoint system. Therefore, the latter could suffer from stiff numerical conditioning conveyed by u. The Hamiltonian model avoids this drawback. However, as shown in Bessonnet (1992), the main numerical interest of the Hamiltonian approach lies in the fact that the conjugate momenta pi , which are linear combinations of the q˙i s weighted by the inertia coefficients of the mass matrix, have smoother variations than the q˙i s themselves. The result is a better numerical conditioning of the entire optimization problem. As a consequence, the choice of the Hamiltonian formulation versus the Lagrangian one, noticeably strengthens the numerical robustness of algorithms used to solve the optimization problem. A further advantage of the formulation (17) lies in its formal structure which makes an exact differentiation easy to achieve, as shown in the next section. 3.3. The Jacobian matrix of Hamiltonian phase velocities As mentioned in Section 2, the Jacobian matrix f,x of the phase velocities in the state equation (1), or (19) just above, is needed to generate the adjoint system (3). This differentiation must be carried out carefully in order to avoid useless redundancies. In fact, both vector-functions Fq and Fp in (17) need to be differentiated with respect to the qi , pi canonic coordinates, or equivalently the xi , xnq +i state variables as defined in (18). In order to simplify the notations, we will use equivalently the following differentiation symbols φ,i :=

∂φ ∂φ = ; ∂qi ∂xi

φ,nq +j :=

∂φ ∂φ = , ∂pj ∂xnq +j

where φ stands for any differentiable function. Considering the expression p = 1 (Fq )T M,i Fq − V,i F i 2

(20)

458

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

and taking into account symmetries in both functions Fq and Fp , their state partial derivatives with respect to the xi s, can be formulated as  nq −1 q q (a) Fi,j = − k,l Mik Mkl,j Fl ,      −1   (b) F q i,nq +j = Mij , 1
i
nq , 1
j
nq , (21) p  p + Di,j ,  (c) Fi,j = F  i,j    q  (d) F p = −F + D , i,nq +j

j,i

i,nq +j

where p = 1 (Fq )T M,ij Fq + (Fq )T M,i Fq − V,ij . p = F F i,j j,i ,j 2

(22)

Proof of results (21) and (22) can be readily established. q Combining the xj -derivative of (17a): F,j = (M−1 ),j p with (16), one gets F,j = −M−1 M,j Fq q

(23)

which is a matrix representation for (21a). The formulation (21b) which is the result of differentiating a linear vector-function in p is straightforward. The symmetry property of  Fp in (22) is obvious when considering its first and third terms, given that the mass matrix M and the potential V are assumed to be twofold continuously differentiable. The symmetry with respect to subscripts i and j of the middle term can be established using the formula (23), which gives (Fq )T M,i F,j = −(Fq )T M,i M−1 M,j Fq . q

Then, invoking the symmetry of the bilinear form defined by M−1 in the second hand member, and using again (23) with subscript j switched for i, one gets the result expected: q

(Fq )T M,i F,j = (Fq )T M,j F,i . The fourth derivative (d) in (21) results from (17) as the expression:  q p q Fi,n +j = Fk,n +j Mkl,i Fl + Di,nq +j , q

q

k,l −1 by (21b). Then, comparing with (21a), we get (21d). where we have Fk,n +j = Mkj q Since the Hamiltonian state x is written as x := (qT , pT )T according to (18), the Jacobian matrix of the function F in (19) exhibits the general structure   q M−1 F . (24) F,x = p ,q q F,q + D,q −(F,q )T + D,p q

p The symmetry of the sub-matrix  F,q is noteworthy because, due to its dependency on the second-order derivatives of M in (22), this matrix represents by far the main part of the calculations required for completing the computation of F,x . Thanks to this property, the computational saving quickly increases with the number nq of d.o.f. It is well known that the computational cost of the Lagrangian formulation such as (7) or (9) is O(n3q ) (see e.g. Garcia de Jalon and Bayo, 1994). This estimation holds for the Hamiltonian model which is simply derived from Lagrange’s one. Therefore, when deriving the Jacobian matrix from each model, the cost becomes O(n4q ). This total cost can be accurately evaluated in terms of arithmetic operations to carry out in order to compute F and F,x (Bessonnet, 1992). For a 6 d.o.f. universal manipulator, and a 12 d.o.f. anthropomorphic mechanical biped, it amounts to 1167 and 14 610 multiplications respectively for the Hamiltonian model. The corresponding cost of the Lagrangian model derived from (9) amounts to 1775 and 18 067 multiplications respectively. Thus, the computational burden of the Lagrangian model is greater than the Hamiltonian one. It must be added that, when considering real multibody systems, the above numbers drop significantly due to the fact that the successive axes of rotation are orthogonal or parallel. In order to complete the above formulations, the most involved computational developments are remaining to be done, namely the computation of the mass matrix M together with its first and second derivatives with respect to q, i.e. M,q and M,q2 . This is the subject of the next sections.

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

459

Fig. 2. ith distal subsystem Si of an nq -link open kinematic chain.

4. Correlating M,q and M,q2 with the Christoffel coefficients The first and second order state-derivatives of the terms Mij of the mass matrix can be expressed as linear combinations of the Christoffel coefficients Γi,j k and their q-derivatives, respectively. Thus, to begin with, we need to formulate both the Mij s and the Γi,j k s appropriately. The approach presented is an extension of the method introduced in Fayet and Renaud (1989) and Fayet and Pfister (1994) which is based on the notion of global inertia tensors. Our purpose is to generalize the Fayet technique in order to obtain the q-derivatives of the Γi,j k s in an exact, purely algebraic computational process with as few computational redundancies as possible. But, before developing such formulations (Section 5), the correlations linking the state derivatives of the mass matrix to the Christoffel coefficients must be precisely analyzed. 4.1. Basic statements and formulations Let us consider any open kinematic chain S as shown in Fig. 2 (see also Fig. 1 in Section 2), and introduce the assumptions and notations All links Li , i
nq , are assumed to be rigid bodies. Each joint Ji is revolute or prismatic. The supporting base B0 is attached to an inertial orthonormal frame Fr0 := (O;X0 , Y0 , Z0 ). A local frame Fri := (Oi ; Xi , Yi , Zi ) is attached to Li according to the recursive construction of Denavit–Hartenberg, the unit vector Zi being directed along the ith joint axis (see Part A1 of the Appendix, and Fig. 1 in Section 2). (v) The geometrical configuration of the mechanical system S is described by a set of nq generalized coordinates qi , i
nq , which, in complex three-dimensional cases, are the relative joint coordinates defined by the Denavit-Hartenberg construction (Fig. 1, Section 2). Si := {L1 , . . . , Li−1 ] (vi) The set Si := {Li , . . . , Lnq } defines the ith distal subsystem (Fig. 2). Its complementary part in S is   (S = Si ∪ Si ).

(i) (ii) (iii) (iv)

The basic formulations we need now are the integral representations of the inertia coefficients (terms Mij of the mass matrix M, and Christoffel coefficients Γi,j k ) defining the Lagrange equations of motion (Fayet and Renaud, 1989; Fayet and Pfister, 1994):  OP,i ·OP,j dm, (25a) Mij = P∈S



Γi,j k =

OP,i ·OP,j k dm,

(25b)

P∈S

where the short notations OP,i := ∂OP/∂qi and OP,ij := ∂2 OP/∂qi ∂qj are used for any subscripts i and j . The dot symbol ‘·’ stands for the scalar dot-product. It can be readily established that the qi -derivatives in (25a), (25b) may be expressed in terms of cross-products through the following representations (Fayet and Renaud, 1989)  σ¯ i Zi × Oi P + σi Zi , P ∈ Si , (26a) OP,i = 0, P ∈ S¯i ,

460

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

 i
j,

OP,ij =

σ¯ i Zi × OP,j , 0,

P ∈ Sj , P ∈ S¯j ,

(26b)

where the factors σi and σ¯ i are Boolean variables such that σ¯ i = 1 − σi , σi being equal to either zero or one, according to the ith joint is either revolute or prismatic (see part A1 of the Appendix). It should be noted that (OOi ),j = 0 is taken into account in (26a). Obviously, OP,j i = OP,ij in (26b). Using (26a) and (26b), expressions (25a) and (25b) become  (σ¯ i Zi × Oi P + σi Zi ) · (σ¯ j Zj × Oj P + σj Zj ) dm, (27a) Mij = P∈Smax(i,j )

j
k,



Γi,j k = σ¯ j

(OP,i , Zj , OP,k ) dm.

(27b)

P∈Smax(i,k)

In (27b), the expression in parentheses stands for a triple scalar product. Now, realizing that relationships (25a) and (25b) show symmetries with respect to indices i, j , and j, k, respectively, that is: Mij = Mj i ,

Γi,j k = Γi,kj ,

(28a)

while (27b) reveals the following skew-symmetry property with respect to the indices i and k: Γi,j k = −Γk,j i ,

(28b)

the development of formulations (27a) and (27b) can be restricted to indices such that  (σ¯ i Zi × Oi P + σi Zi ) · (σ¯ j Zj × Oj P + σj Zj ) dm, i
j, Mij = P∈Sj

i < k, j
k,

(29a)



Γi,j k = σ¯ j

(OP,i , Zj , OP,k ) dm,

(29b)

P∈Sk

In (29a), due to the fact that the vectors Oi P and Oj P depend only on qk for k > i and k > j  i, respectively, Mij does not depend on the i first joint coordinates, that is i
j,

Mij = Mij (qi+1 , . . . , qnq ).

This well known property is mentioned, for instance, in Tourassis and Neumann (1985). As a direct consequence, the first-order derivatives of the mass matrix M take the form M  ··· M M ··· M 2
k
nq ,

   M,k =   

11,k

···

1(k−1),k

···

M(k−1)(k−1),k

1k,k

·

M(k−1)k,k 0

··· ··· ··· ···

Symmetric

1nq ,k

···   M(k−1)nq ,k  , 0   ··· 0

(30)

where the matrix terms are formally defined as: (M,k )ij := Mij,k = ∂Mij /∂qk . Similarly, if we set (M,kl )ij := Mij,kl = ∂2 Mij /∂qk ∂ql , the second-order derivatives of M can be defined by the set of subscripts i, j, k and l such that 2
k
l
n,

M,kl = M,lk = (Mij,kl )i
j
n .

(31)

A first impression about the computational complexity of the problem to be dealt with is given by the number of independent scalar derivatives to be calculated in relation to the number of degrees of freedom of the mechanical system. The structure of matrices (30) shows that a maximal set of linearly independent terms Mij,k is defined by subscripts such that i
j and i < k, or else i < k
j,

i
j < k.

(32)

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

461

In the same way, independent second order derivatives Mij,kl are defined by subscripts satisfying i
j, i < k and k
l, or equivalently i < k
l < j,

i < k
j
l,

i
j < k
l.

(33)

Applying the formula (A2.1) of the Appendix to the inequalities (32) and (33) gives the result N1 = (nq − 1)nq (nq + 1)/3, N2 = (nq − 1)nq (nq + 1)(3nq + 2)/24,

(34)

where N1 and N2 are the maximum number of derivatives Mij,k and Mij,kl , respectively, to be computed. It should be noticed that these numbers amount to 70 and 175 for a six d.o.f. open kinematic chain (572 and 2717 for a 12 d.o.f. human-like bipedal locomotion system). Then, introducing the set of matrices Γ k defined through (29b) as  (OP,i , Zk , OP,j ) dm (35) 1
k
nq , Γijk := Γi,kj = σ¯ k P∈Sj

one can derive from (25a) a double set of matrices with single and double differentiation subscripts k and l such that 1
k
nq ,

M,k = Γ k + (Γ k )T ,

1
k
l
nq ,

(36)

M,kl = Γ k,l + (Γ k,l )T ,

(37)

where the terms of the matrices Γ ,kl are defined by setting (Γ,kl )ij := ∂Γi,kj /∂ql . At this point, let us simply observe that the second-order derivatives of M in (37) are reduced to the determination of the firstorder derivatives of the matrices Γ k . This is the new computation to be carried out to complete the Hamiltonian formulation. It will be achieved in Section 5. In addition, it should be noticed that the superscript k in Γijk is also the subscript of Zk in the integral representation of Γijk in (35), Zk being the unit vector on the kth joint axis. k 4.2. Defining a set of linearly independent derivatives Γij,l

Computation of matrices M,k and M,kl in (36) and (37) requires the identification of appropriate independent terms in Γ k and Γ ,kl . Linearly independent coefficients Γijk in (35) are defined through the limitations set on i, j, k in (29b). Therefore, such coefficients are represented by the set of subscripts i < k
j,

k
i < j,

(38)

which represents the same number N1 (in (34)) of terms that there are independent coefficients Mij,k identified by (32). Then, independent terms in M,k can be expressed as linear combinations of their Γ k counterparts, by the simplified relationships  i   (a) i = j < k, Mii,k = 2Γik ,      (b) i < j < k, Mij,k = Γ j + Γ i , ik jk (39) k    (c) i < j = k, Mik,k = Γik ,     (d) i < k < j, M =Γk −Γi . ij,k

ij

kj

These ones result in the reciprocal formulations  k =M  (a) i < k = j, Γik ik,k ,     1 k   (b) i < k < j, Γij = 2 (Mik,j + Mij,k ),  (c) i = k < j,       (d) k < i < j,

k = 1M Γkj 2 kk,j ,

Γijk = 12 (Mki,j − Mkj,i ).

(40)

462

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

Differentiating both sets of relationships (39) and (40) with respect to appropriate state variables ql , will make it possible to k . First, (39) gives independent second order derivatives in q for subscripts l identify a suitable set of independent terms Γij,l l satisfying l  k, that is  i , (a) i = j < k
l, Mii,kl = 2Γik,l      j   Mij,kl = Γik,l + Γjik,l ,  (b) i < j < k
l, (41) k , (c) i < j = k
l, Mik,kl = Γik,l          (d1 ) i < k < j
l , Mij,kl = Γ k − Γ i . ij,l kj,l (d2 ) i < k
l < j It can be easily verified, using the formula (A2.1), that the total number of terms Mij,kl in (41) is equal to N2 as defined in (34). k as follows Now, differentiating relationships (40) yields the non-zero derivatives Γij,l (a) i < k = j

 (a1 ) i < l < k = j , (a2 ) i < k = j
l

k =M Γik,l ik,kl ;

(42a)

k = 1 (M Γij,l ik,j l + Mij,kl ); 2

(42b)

(b) i < k < j

(b1 ) i < l < k < j  (b2 ) i < k
l < j , (b3 ) i < k < j
l

(c) i = k < j

 (c1 ) (i = k < l < j ) , (c2 ) i = k < j
l

k = 1M Γkj,l kk,j l ; 2

(42c)

(d) k < i < j

(d1 ) k < l < i < j  (d2 ) k < i
l < j , (d3 ) k < i < j
l

k = 1 (M Γij,l ki,j l − Mkj,il ). 2

(42d)

k from (42a–d), able to generate the terms M At this point, we want to select independent Γij,l ij,kl in (41). Firstly, let us remark that (a2 ) in (42a), and (c2 ) in (42c) are the inverse formulations of (c) and (a) in (41). Secondly, in (41b), the permutation j k with i < k < j
l, inequalities representing the terms in (42b)-(b ). The (i, j, k, l) → (k, i, j, l) changes Γik,l into Γij,l 3 k with k < i < j
l, inequalities which define the case (42d)-(d ). Hence, second term Γjik,l in (41b) can be changed into Γij,l 3 k of cases (42b)-(b ), (42d)-(d ). In the same way, one can verify that the terms Mij,kl in (41b) can be computed with terms Γij,l 3 3 Mij,kl in (41d1 ) (resp. (41d2 )) are linear combinations of terms from (42b)-(b3 )-(d3 ) (resp. (42b)-(b2 )-(d2 )). k in (41) requires only the terms of cases (42)-(a ,b ,b ,c ,d ,d ). However, the Therefore, the formulations of terms Γij,l 2 2 3 2 2 3 last ones are not independent, for their number is greater than N2 . One can see directly that cases (a2 ,b2 ,b3 ,c2 ) are globally independent. As we did above, it is easy to show that, by permuting subscripts i, j, k, l, the terms in (d3 ) are not linear combinations of terms from (a2 ,b2 ,b3 ,c2 ), while the terms in (d2 ) can be expressed as such combinations. Let us verify the latter case:

– For i < l in (d2 ), the successive permutations – in (b2 ), (i, k, l, j ) → (k, i, l, j ) i = 1 (M Γkj,l ki,j l + Mkj,il ); 2 – in (b3 ), (i, k, j, l) → (k, i, l, j ) i = 1 (M Γkl,j ki,lj + Mkl,ij ); 2 – in (d3 ), (k, i, j, l) → (k, i, l, j ) k = 1 (M Γil,j ki,lj − Mkl,ij ) 2

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

463

result in the combination k < i < l < j,

k =Γk +Γi −Γi . Γij,l il,j kl,j kj,l

(43)

– For i = l, permuting, in the same way, indices of terms in (a2 ) and (b2 ), one obtains k < i = l < j,

k =Γi −Γi . Γij,i ki,j kj,i

(44)

Consequently, terms in (d2 ) appear as the linear combinations (43) and (44) of terms from (a2 ,b2 ,b3 ,c2 ,d3 ). k of the last five cases are independent and generate directly, and indirectly by using the combiTo sum up, the terms Γij,l nations (43) and (44), the set of independent Mij,kl as defined by (41). The above five subsets can be grouped together in the following set of inequalities  (a) i < k
j
l, j
l, (b) k
i < j
l, (45) l < j,

(c) i < k
l < j.

k to be formulated This is the main result of this section: inequalities in (45) define a maximal set of linearly independent Γij,l in order to generate the second-order derivatives of the mass matrix M through the relationships (41). In the next section, these k . inequalities will play a key role in organizing the explicit formulations of the derivatives Γij,l Let us add that, using the formula (A2.1) of the Appendix, the numbers of terms defined by subsets (a), (b), (c) in (45) are, successively

Na = Nb = (nq − 1)nq (nq + 1)(nq + 2)/24, Nc = (nq − 2)(nq − 1)nq (nq + 1)/24.

(46)

As expected, their sum is equal to N2 (cf. (34)).

5. Reducing second-order differentiations to algebraic operations k with respect to any configuration parameter q (k
l
n ). They The objective is to compute the partial derivatives Γij,l q l can be derived directly from the formula (35), as the integral 

k = σ¯ Γij,l (47) (OP,il , Zk , OP,j ) − (OP,j l , Zk , OP,i ) dm. k P∈Smax(j,l)

Two cases must be distinguished according to the set of integration Smax(j,l) is either Sl (j
l) or Sj (l < j ). Therefore, the formula (47) requires computing the four following integrals of triple scalar products  1   (48a) Aij kl := σ¯ k P∈Sl (OP,il , Zk , OP,j ) dm, i < k
j,  (i) j
l, 1 := σ¯ k
i < j, (OP , Z , OP ) dm, (48b) Bij k P∈Sl ,j l k ,i kl (ii)

l < j, i < k
l,

  2 Aij kl := σ¯ k P∈Sj (OP,il , Zk , OP,j ) dm,  2 := σ¯ Bij k P∈Sj (OP,j l , Zk , OP,i ) dm, kl

(49a) (49b)

where the subscripts i, j, k, l are restricted by the limitations (45). 1 = A1 . But, this equality is formal and does not present From (48a) and (48b), it can be straightforwardly seen that Bij kl j ikl a real practical interest, because it would need to extend the computation of the terms A1ij kl to indices satisfying i > j , which 1 for indices defined in (48). Thus, all the terms in (48a) and (48b) are different and need amounts to computing the terms Bij kl to be computed. In (49a) and (49b), permuting indices i and j , as above, is not possible since the set of integration is Sj in both cases. Furthermore, the set of terms A2ij kl in (49a) cannot be compared with its counterpart in (48a) because both integration 2 1 sets are different. But, it can be easily seen that Bij kl is equal to Bilkj . Moreover, in this case, the set of indices in (49b) is included in the set of indices in (48b) after having permuted l and j . Therefore we can set

i < k
l < j,

2 = B1 . Bij kl ilkj

(50)

464

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

5.1. Integrating the triple scalar products To begin with, we give special attention to the computation of the first integral labelled A1ij kl in (48a). The triple scalar product can be written as (OP,il , Zk , OP,j ) = Zk · (OP,j × OP,il ). Using the formulations (26a) and (26b), the first and second order derivatives of OP become explicitly  OP,j = σ¯ j (Zj × Oj P) + σj Zj , j
l, ∀P ∈ Sl , OP,il = σ¯ i σ¯ l Zi × (Zl × Ol P) + σ¯ i σl Zi × Zl .

(51)

(52)

Then, the right-hand member in (51) takes the form 

   (OP,il , Zk , OP,j ) = σ¯ i σ¯ j σ¯ l Zk · (Zj × Oj P) × Zi × (Zl × Ol P) + σ¯ i σ¯ j σl Zk · (Zj × Oj P) × (Zi × Zl ) 

   + σ¯ i σj σ¯ l Zk · Zj × Zi × (Zl × Ol P) + σ¯ i σj σl Zk · Zj × (Zi × Zl ) .

(53)

Next, using the key formula (A2.2) of the Appendix, the expression in brackets in the first term of the right hand member of (53), turns into ∧ ∧ 




Zl (Ol P ⊗ Oj P) Zj Zi . (Zj × Oj P) × Zi × (Zl × Ol P) = − Zj · (Oj P Ol P)Zl 1 + 

(54)

In this way, the vectors Oj P and Ol P are grouped together so as to introduce, by integrating (53), the global inertia tensors we k . intend to use in order to achieve the computation of Γij,l When expression (53) is integrated in (48a), while accounting for (54), the following global inertia tensors appear as defined in (A3.4), (A3.8) and (A3.11) of the Appendix:  Oj P dm = Π j l , P∈Sl



OPl ⊗ OPj dm = LT jl,

(55)

P∈Sl



∧

∧

Oj P Ol P dm = −Jj l . P∈Sl

The global result which holds for the subscripts defined in (45a, b), can be organized as follows   A1ij kl = σ¯ i σ¯ j σ¯ k σ¯ l (Zi · Zk )(Zj · Jj l Zl ) − (Zi × Zj ) · Lj l (Zk × Zl )   + σ¯ i σ¯ j σ¯ k σl −(Zj · Zk )Π j l · (Zi × Zl ) − [Zi , Zj , Zl ](Π j l · Zk )   + σ¯ i σj σ¯ k σ¯ l −(Zi · Zl )Π ll · (Zk × Zj ) − [Zj , Zk , Zl ](Π ll · Zi )   + σ¯ i σj σ¯ k σl Ml (Zj · Zl )(Zi · Zk ) − (Zi · Zj )(Zk · Zl ) .

(56)

Achieving similar computations for the integrals (48b) and (49a) gives immediately the two complementary results: 1 = σ¯ σ¯ σ¯ σ¯ (Z · Z )(Z · J Z ) − (Z × Z ) · L (Z × Z ) Bij i j k l j k i il l j i il k l kl   + σ¯ i σ¯ j σ¯ k σl −(Zi · Zk )Π il · (Zj × Zl ) − [Zj , Zi , Zl ](Π il · Zk )   + σi σ¯ j σ¯ k σ¯ l −(Zj · Zl )Π ll · (Zk × Zi ) − [Zi , Zk , Zl ](Π ll · Zj )   + σi σ¯ j σ¯ k σl Ml (Zi · Zl )(Zj · Zk ) − (Zj · Zi )(Zk · Zl ) ,   A2ij kl = σ¯ i σ¯ j σ¯ k σ¯ l (Zk · Zi )(Zl · Jlj Zj ) − (Zk × Zl ) · Llj (Zi × Zj )   + σ¯ i σj σ¯ k σ¯ l −(Zl · Zi )Π lj · (Zk × Zj ) − [Zk , Zl , Zj ](Π lj · Zi )   + σ¯ i σ¯ j σ¯ k σl −(Zk · Zj )Π jj · (Zi × Zl ) − [Zl , Zi , Zj ](Π jj · Zk )   + σ¯ i σj σ¯ k σl Mj (Zl · Zj )(Zk · Zi ) − (Zk · Zl )(Zi · Zj ) .

(57)

(58)

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

465

It should be noticed that for each set of subscripts (i, j, k, l) satisfying (45a), (45b) and (45c), one at the most among the four Boolean coefficients in (56), (57) and (58) is not equal to zero. In fact, the only term designated by this eventually non-zero coefficient must be computed. Moreover, one can observe that if the kth joint is translational (i.e. if σ¯ k = 0), then A1ij kl = 1 = A2 = 0 for any value of i, j and l. Bij kl ij kl k in (47) may be summarized as follows Finally, the formulation of the derivatives Γij,l  i < k
j, k = A1 − B 1 , • j
l, Γij,l ij kl ij kl k
i < j,

• l < j, i < k
l,

k = A2 − B 1 . Γij,l ij kl ilkj

(59) (60)

5.2. Application to kinematic chains with rotational joints Most of controlled multibody systems such as industrial manipulators, walking machines, excavators and articulated lifting devices have only rotational joints. In such cases, the single non-zero Boolean coefficient is σ¯ i σ¯ j σ¯ k σ¯ l . Consequently, the only expression to be taken into account in (56), (57) and (58) is in the first bracket of each formulation. Also, it should be noticed that these expressions include terms of the type Zα · Jαβ Zβ which represent the inertia coefficients Mαβ of the mass matrix M, in accordance with the formula (A3.14) of the Appendix. Thus, expressions (56), (57) and (58) become simply A1ij kl = (Zi · Zk )Mj l − (Zi × Zj ) · Lj l (Zk × Zl ), 1 = (Z · Z )M + (Z × Z ) · L (Z × Z ), Bij j k il i j il k l kl

(61)

A2ij kl = (Zi · Zk )Mj l − (Zk × Zl ) · Llj (Zi × Zj ). At this stage, taking into account the formulations (61) and the formulas (A3.15) and (A3.17) of the Appendix, we are able to k in terms of global inertia tensor L and N , as follows formulate concisely all coefficients Mij , Γijk and Γij,l ij ij 1. i
j
nq Mij = −Zj · (Lij Zi ) + (Zi · Zj ) tr(Lij );

(62)

2. i < j, k
j
nq Γijk = −(Zi × Zj ) · Lij Zk − (Zj · Zk )(Zi · Nij );

(63)

3. Max(j, l)
nq • j
l (i < k
j, k
i < j ) 1 = (Z · Z )M + (Z × Z ) · L (Z × Z ), Bij j k il i j il k l kl k = (Z · Z )M − (Z × Z ) · L (Z × Z ) − B 1 ; Γij,l i k jl i j jl k l ij kl

(64a)

• l < j (i < k
l) k = (Z · Z )M − (Z × Z ) · L (Z × Z ) − B 1 . Γij,l i k jl k l lj i j ilkj

(64b)

This unified, intrinsic formulation embodies numerous scalar dot-products and cross-products of unit vectors on rotation axes. In fully actuated multibody systems of serial type or having a kinematic tree-structure, joint axes are successively parallel or orthogonal. This fact, together with the special algebraic structure of above formulations, make it possible to eliminate directly k will take many terms, especially in the relationships (64a) and (64b). On the other hand, computations of coefficients Γij,l advantage of pre-calculated factors in (62) and (63), such as dot-products, cross-products, inertia tensors and mass-matrix coefficients. Nevertheless, remaining algebraic operations in (64a), (64b) represent a significant amount of further computation to be carried out. In order to develop final formulations, expressions (62) to (64b) must be computed in terms of components of vectors and tensors projected onto local frames as defined, for instance, by the Denavit–Hartenberg construction (part A1 of the Appendix). These complementary developments are detailed in the part A4 of the Appendix. Finally, the above formulations must be completed by the computation of the global inertia tensors Lij . These calculations are carried out in the following subsection. Let us point out that the main results of this section are the general formulations (56), (57) and (58), together with their special form (64a), (64b) which express quite concisely, and exactly, the partial derivatives of the Christoffel coefficients with respect to the configuration parameters.

466

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

6. Recursions and symbolic computation algorithm Computing the tensors Lij must be achieved using the backward recursion formulas (A3.5) and (A3.9) given in the Appendix. Of course, in the recursive construction which results, all vectors and tensors must be represented in matrix form with components related to some local bases. Low cost computational formulations are obtained using matrix representations on the k defined by (62), (63), (64a) and (64b), and in tensors local bases numbered by the higher subscript in coefficients Mij , Γijk , Γij,l Lij defined by (A3.8). In the following, second-order inertia tensors will be represented in matrix form by the same symbol as the tensor underlined twice, with the projection basis (or frame) number added as a superscript. In a similar way, first-order inertia tensors and column-matrix representation of vectors will use the original symbol underlined once, with the projection basis number added as a superscript. As indicated by the basic formulations given in parts A1 and A3 of the Appendix, the primary data required to compute the tensors Lij are i Ri+1 , rotation matrix of link Li+1 versus Li , i = 1, . . . , nq − 1, Λi,i+1 := Oi Oi+1 ,  (65)  Mi , mass of distal subsystem Si , π ∗i , first-order augmented inertia tensor of link Li , i = 1, . . . , nq ,  K∗ , second-order augmented inertia tensor of link L . i i In accordance with the recursion formulas (A3.5) and (A3.9), a recursive computation scheme for the Lij s can be organized as follows – Firstly, compute Λi,i+1 following the forward recursion (the matrix j Rj −1 is defined in (A1.2) of the Appendix) 1
i < j
nq ,

j

Λ i,i+1 = j R

j −1

j −1

Λ i,i+1 ;

– Secondly, compute Lij for 1
i < j
n following the double backward recursion  n  Π nqq nq = π ∗n , q j = nq ,  Lnq = K∗ , nq nq nq  j j +1 Π = jR Π ,   j +1 j +1,j +1  j +1,j +1    j j  ∗   Π jj = Π j +1,j +1 + π j ,    j  j +1 j j j j  j RT L = jR L + Λ j,j +1 (Π j +1,j +1 )T + Π j +1,j +1 (Λ j,j +1 )T + K∗ , jj j j +1 j +1 j +1,j +1 nq > j > 1,  j    i = j, H jj = 0,    j   j j T   H = Hj   jj i+1,j + Λ i,i+1 (Π jj ) ,   j > i > 1,    Lj = Lj + Hj . ij jj ij

(66)

(67)

In this way, an algorithm leading from the data (65) to the complete computation of a full set of independent coefficients k can be organized following the main steps: Mij , Γijk , Γij,l j

j

Step 1 – Compute Zi , Λi,i+1 , (Zi × Zj )k for 1
i < j
k
nq . Step 2 – Compute Lij for 1
i < j
nq , using the recursions (66) and (67). k according to the computational scheme (62), (63), (64a), (64b) and using the Step 3 – Compute all coefficients Mij , Γijk , Γij,l formulas (A4.2) to (A4.5) of the Appendix. We used this global algorithm to create a symbolic computing code developed using the software Mathematica. The programme is organized in three parts. The first generates the inertia coefficients and their partial derivatives as defined by the above computing steps. The second generates the phase velocities defined by the functions Fq and Fp in (17) (Section 3) and their Jacobian matrices in the state variable in (21). The mass matrix is inverted numerically using a Gauss elimination technique. The third part derives the conditions for optimality stated by the PMP and expands a two-point boundary-value problem ready for use by a numerical solver (see Part 2 of the paper, Section 4).

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

467

Conclusive numerical tests were achieved for verifying the programme. Especially, results of motion synthesis were compared when using parallel formulations of the problem, generated by the symbolic computing code, and fully developed by hand. Of course, these comparisons were carried out considering simple mechanical systems such as a 5-link planar system and a 3-link three-dimensional robot arm without wrist. Nevertheless, the computational complexity of these problems is quite significant.

7. Conclusion Dealing with motion optimization of actuated multibody systems is primarily based on selecting a suitable optimization technique. Pontryagin’s maximum principle is theoretically perfectly relevant to carry out dynamics-based optimization. For being computationally efficient, it needs to be associated with an appropriate formulation of motion equations. We showed that the Hamiltonian dynamic model is formally quite suitable for deriving necessary conditions for optimality stated by the PMP. We organized the canonic equations under a feasible form in order to be applied to non-trivial mechanical systems. In the same way, the Jacobian matrix of Hamiltonian phase velocities was developed in compact algebraic form which reveals useful symmetrical properties. This approach is a first step toward reducing the computational complexity of the optimization problem which is intended to be dealt with in the companion paper. A second major step has consisted in developing, in the same computational algebraic scheme, using the notion of global inertia tensors, the dynamic coefficients and their time-derivatives that complete the Hamiltonian dynamic model and its derived Jacobian matrix. On this basis, we have developed a symbolic computing code which generates the Hamiltonian state equation and the costate equation stated by the PMP, both ready for use by the numerical solver described in Part 2 of the paper. It must be emphasized that these exact and compact symbolic formulations ensure both numerical accuracy and computing efficiency of the solving algorithms. Nevertheless, as pointed out in the companion paper, the final numerical solving code we employed requires the Jacobian matrix of the costate equation, that is to say the Hessians of phase velocities. This higher order differentiation, which remains to be done, represents an ultimate improvement which could help to enhance significantly the numerical robustness of the solving algorithm.

Appendix A1. Denavit–Hartenberg construction We consider a serial kinematic chain with successive links connected by n rotational or translational joints denoted Ji , i
n. The construction of Denavit and Hartenberg (1955) makes it possible to define a set of configuration coordinates using the same recursive geometrical scheme for all joints. First, an axial direction is defined on each joint axis by introducing a unit vector Zi for the ith axis (see Fig. 1 in Section 2). Then, considering two successive joints Ji−1 and Ji , Denavit–Hartenberg construction is based on introducing the common perpendicular Oi−1 O i to the joint axes (Fig. A1.1). Using the notations of Khalil and Kleinfinger (1986), let us define the unit vector Xi−1 on the segment Oi−1 O i as: Xi−1 = Oi−1 O i / Oi−1 O i . Then, defining ri = Oi−1 O i ,

λi = O i Oi · Zi , αi = (Zi−1 , Zi )Xi−1 , angle oriented by Xi−1 , ϕi = (Xi−1 , Xi )Zi ,

angle oriented by Zi ,

the joint coordinate qi describing the motion of the link Li (or frame Fri := (Oi ; Xi , Yi , Zi ), Yi = Zi × Xi , attached to Li ) with respect to the link Li−1 (or frame Fri−1 := (Oi−1 ; Xi−1 , Yi−1 , Zi−1 )) is commonly expressed using the Boolean operation: qi = σ¯ i ϕi + σi λi

(σ¯ i = 1 − σi )

(A1.1)

where σi = 0 or 1, depending on whether the joint is rotational or translational, respectively. Thus, if Ji is rotational, the joint movement is described by the angle of rotation ϕi about the axis (Oi ; Zi ), λi being a constant. Conversely, if Ji is translational, the joint movement is described by the translation coordinate λi along the axis (Oi ; Zi ), ϕi being a constant. The orientation of the frame Fri with respect to the frame Fri−1 is then defined by the transformation orthogonal matrix  Cos ϕ  − Sin ϕ 0 i−1 R = i

which satisfies i R

i

i

Cos αi Sin ϕi Sin αi Sin ϕi i−1

Cos αi Cos ϕi Sin αi Cos ϕi

− Sin αi Cos αi

≡ (i−1 R )−1 = (i−1 R )T . i

i

(A1.2)

468

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

Fig. A1.1. Construction of Denavit–Hartenberg.

A2. Two useful results A2.1 Proposition 1. Let n1 , . . . , np be p increasing integers, considered as subscripts of any mathematical symbols, such that 1
n1 and np
n, n being a given integer. Let q (0
q < p) be the number of strict inequalities. Then, the number N of distinct indexations is N (p, q) = (n − q)(n − q + 1) · · · (n − q + p − 1)/p!.

(A2.1)

Proof. Let us consider, at first, the particular case there are no strict inequalities (i.e. q = 0). Then, the number of distinct indexations is the number of combinations with repetitions of p elements chosen among n, namely (see any textbook dealing with combinatorial analysis)   (n + p − 1)! n+p−1 , N= ≡ p (n − 1)!p! formulation which gives (A2.1) for q = 0. In the general case q = 0, one can consider that the q strict inequalities eliminate q possibilities of combinations among n. Therefore, the number of distinct indexations is equal to the number of combinations with repetitions of p elements chosen among n − q, hence   (n − q + p − 1)! n−q +p−1 . N= ≡ p (n − q − 1)!p! After simplification, one gets (A2.1).

2

A2.2 Let us introduce the basic notations – V ∈ 3 ,  V = Skew(V), skew-symmetrical tensor defined by V, V, column-matrix and matrix representations for V and  V, respectively, over any given projection basis, – V,  3  – U ∈  , VU = V × U, cross product of V and U. We need the result stated in the following proposition. Proposition 2. For any vectors X, Y, Z, U and V belonging to 3 , the following relationship holds
 

 (X × U) × Y × (Z × V) = − X · ( U V)Z 1 +  Z(V ⊗ U) X Y. Proof. Using the formula: X × (Y × Z) = (X · Z)Y − (X · Y)Z, the left hand member in (A2.2) becomes




(X × U) × Y × (Z × V) = (X × U) · (Z × V) Y − (X × U) · Y (Z × V). Now, using the identities (for any vectors X and Y) X · Y ≡ XT Y,

X ⊗ Y ≡ XYT ,

X×Y≡ XY

(A2.2)

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

469

we have V U X ≡ −XT  U V Z, (X × U) · (Z × V) = −ZT 

T ˆ ⊗ U) X Y ≡ −Z(V (X × U) · Y ( Z × V ) = − ZVU  X Y, which establishes the relationship (A2.2).

2

This result is helpful in dealing with the derivatives of Christoffel coefficients in Section 5. A3. Global inertia tensors Notions, definitions and results presented below are essentially related to Fayet and Renaud (1989). The reader is also referred to Fayet and Pfister (1994) and Bessonnet (1992). A3.1 Inertia tensors and global inertia tensors Some notations (cf. Subsection 4.1): mk = mass(Lk ),

Mi = mass(Li ) =

nq 

mk ,

i < j,

Λij := Oi Oj .

k=i

First order inertia tensors  Oi P dm = mi Oi Gi ; Gi , center of mass of link Li , π i :=

(A3.2)

P∈Li

π ∗i := π i + Mi+1 Λi,i+1 , first order inertia tensor of the ‘augmented body’,  Oi P dm = Π jj + Mj Λij . i
j, Π ij ≡

(A3.3) (A3.4)

P∈Lj

Backward recursion formula  j = nq , Π nq nq = π nq , j < nq ,

(A3.5)

Π jj = Π j +1,j +1 + π ∗j .

Second order inertia tensors  Oi P ⊗ Oi P dm, Ki :=

(A3.6)

P∈Li

K∗i := Ki + Mi+1 Λi,i+1 ⊗ Λi,i+1 ,  Oi P ⊗ Oj P dm. Lij :=

inertia tensor of the ‘augmented body’,

(A3.7) (A3.8)

P∈Lj

Backward recursion formulas  Lij = Ljj + Λij ⊗ Π j , i < j, Ljj = Lj +1,j +1 + Λi,i+1 ⊗ Π j +1,j +1 + Π j +1,j +1 ⊗ Λi,i+1 + K∗j . Other global inertia tensors  1 Oi P × Oj P dm = ε(Lij + LT Nij := ij ), 2

(A3.9)

(A3.10)

P∈Lj

where ε is the Ricci tensor.  ∧ ∧ Oi P Oj P dm = −LT Jij := − ij + tr(Lij )1. P∈Lj

(A3.11)

470

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

A3.2. Algebraic representation of the mass-matrix coefficients and Christoffel coefficients (from Fayet and Renaud, 1989) Developing the formula (27) in Section 4, and using the representations (A3.4) and (A3.11) one obtains i
j,

Mij = σ¯ i σ¯ j Zi · (Jij Zj ) + σ¯ i σj (Zi , Π ij , Zj ) + σi σ¯ j (Zi , Zj , Π ij ) + σi σj Mj (Zi · Zj ).

(A3.12)

In the same way, taking into account (26a), the integral (28) becomes Γijk = − σ¯ i σ¯ j σ¯ k Zi · ( Zj Lij + Lij ⊗ Zj )Zk + σ¯ i σj σ¯ k (Zi × Π ij , Zk , Zj )

j
k, i < k,

+ σi σ¯ j σ¯ k (Zi , Zj , Zk × Π kk ) + σi σj σ¯ k Mk (Zi , Zj , Zk ).

(A3.13)

If all joints of the kinematic chain are rotational, (A3.12) and (A3.13) become respectively i
j,

Mij = Zi · (Jij Zj ),

(A3.14)

Γijk = −Zi · ( Zi Lij + Lij ⊗ Zj )Zk .

j
k, i < k,

(A3.15)

In the same way as in (A3.15), Mij coefficients can be expressed in terms of global inertia tensors Lij using the formula (A3.11), namely Mij = Zj · (Lij Zi ) + (Zi · Zj ) tr(Lij ).

(A3.16)

Also, let us mention that (A3.15) can be reformulated as (cf. Bessonnet, 1992) Γijk = −(Zi × Zj ) · Lij Zk − (Zj · Zk )(Zi · Nij ).

j
k, i < k,

(A3.17)

The latter result is useful for eliminating the zero terms produced by orthogonal or parallel vectors Zi , Zj , Zk of rotation axes. A4. Projected formulations The following matrix representations of vectors and tensors (see convention for notations in Section 6) – Zi over the basis Bj , j

j

j

j

Z i = (xi , yi , zi )T ;

i < j,

– Zi × Zj over Bk , k , y k , z k )T ; Zkij = (xij ij ij

i < j
k, – Lij over Bj ,

j L ij =

i
j,

 AL

ij I Lij H Lij

(ELij )  (DLij ) (CLij )

F Lij BLij GLij

(A4.1)

give the expressions for – Mij (mass matrix coefficients, Section 5.4), using Bj as a computing basis in (58) j

j

j

Mij = −xi H Lij − yi GLij + Zi (ALij + BLij );

(A4.2)

– Γijk , using Bj as a computing basis in (59) j

j

j

j

j

j

j

j

j j

Γijk = xi (xk I Lij + yk BLij + zk GLij ) − yi (xk ALij + yk F Lij + zk H Lij ) + zi zk (I Lij − F Lij );

(A4.3)

k , using B as a computing basis for the case (i) in (60a) – Γij,l l j

1 = −z M + x l (AL y l − F L x l ) + y l (I L y l − BL x l ) + zl (H L y l − GL x l ), Bij il k il k il k il k il k il k kl ij ij ij k il j

k = z M + x l (AL y l − F L x l ) + y l (I L y l − BL x l ) + zl (H L y l − GL x l ) − B 1 ; Γij,l jl k jl k jl k jl k jl k jl k ij kl ij ij ij i jl

(A4.4a) (A4.4b)

G. Bessonnet, P. Sardain / European Journal of Mechanics A/Solids 24 (2005) 452–471

471

k , using B as a computing basis in (60b) – Γij,l j j

j

j

k = zk M − zl M + x (x F L − y AL ) Γij,l lj lj i lj k ij kl i i j

j

j

j

j

j

1 . + ykl (xi BLlj − yi I Llj ) + zkl (xi GLlj − yi H Llj ) − Bilkj

(A4.5)

Let us mention that the terms in parenthesis in the third column of the matrix (A4.1) do not appear in any expressions containing other Lij terms, which introduces a noteworthy simplification in computing the Lij tensors. This fact was reported in Fayet and Renaud (1989) when considering the expressions (A4.2), (A4.3) derived from (58), (59) in Subsection 5.4.

References Ailon, A., Langholz, G., 1986. Further study on time optimal control of multi-link mechanical systems. J. Franklin Inst. 321 (3), 167–177. Arnold, V.I., 1989. Mathematical Methods of Classical Mechanics. Springer, Berlin. Bessonnet, G., 1992. Optimisation dynamique des mouvements point à point de robots manipulateurs. Thesis, University of Poitiers, France. Borri, M., Bottasso, C., Mantegazza, P., 1992. A modified phase space formulation for constrained mechanical systems – differential approach. Eur. J. Mech. A Solids 11, 701–727. Chen, Y., Desrochers, A.A., 1990. A proof of the structure of the minimum-time control law of robotic manipulators using a Hamiltonian formulation. IEEE Trans. Robotics and Automation 6, 388–393. Denavit, J., Hartenberg, R.S., 1955. A kinematic notation for lower-pair mechanisms based on matrices. J. Appl. Mech. 77, 215–221. Fayet, M., Pfister, F., 1994. Analysis of multibody systems with indirect coordinates and global inertia tensors. Eur. J. Mech. A Solids 13, 431–457. Fayet, M., Renaud, M., 1989. Quasi-minimal computation under an explicit form of the inverse dynamic model of a robot manipulator. Mech. Mach. Theory 24, 165–174. Flashner, H., Skowronski, J.M., 1989. Model tracking control of Hamiltonian systems. ASME J. Dyn. Syst. Meas. and Control 111, 656–660. Garcia de Jalon, J., Bayo, E., 1994. Kinematic and Dynamic Simulation of Multibody Systems – The Real Time Challenge. Springer, Berlin. Khalil, W., Kleinfinger, J.K., 1986. A new geometric notation for open and closed loop robots. In: Proc. IEEE Conf. Rob. and Automation, San Francisco, pp. 1174–1179. Lewis, F.L., Syrmos, V.L., 1995. Optimal Control. Wiley, New York. Lur’e, L., 1968. Mécanique analytique. Masson, Paris. Pfister, F., 1997. Linearized dynamics – a tensorial treatment based on the raw form of Lagrange’s equations. Eur. J. Mech. A Solids 16, 529–555. Pontryagin, L., Boltiansky, V., Gamkrelitze, A., Mishchenko, E., 1962. The Mathematical Theory of Optimal Processes. Wiley, New York. Sika, Z., Valasek, M., 1997. Parallelization of multibody formalism for rigid bodies using natural coordinates and modified state space. Eur. J. Mech. A Solids 16, 325–339. Skowronski, J.M., 1986. Control Dynamics of Robotic Manipulators. Academic Press, New York. Tourassis, V.D., Neuman, C.P., 1985. The inertial characteristics of dynamic robot models. Mech. Mach. Theory 20, 41–52.