Recursive Robot Dynamics

205

Chapter

9 Recursive Robot Dynamics* This chapter introduces one of the orthogonal complement based methodology for the Why Recursive? automatic generation of dynamic algorithms, namely the inverse and forward dynamics. As Recursive algorithms are known to mentioned in Chapter 8, the inverse dynamics provide efficient and numerically is essential for control of robot manipulators, stable computer algorithms. whereas the forward dynamics is required for computer simulation and the real-time feedback control. Moreover, to solve the inverse or forward dynamics problems of a complex robotic system, a set of dynamic equations of motion or the dynamic model of the robot under study is needed, which are derived in Chapter 8 using Euler-Lagrange (EL) and Newton-Euler (NE) formulations. The resultant equations of motion are in the form of either the ordinary differential equations (ODE) or differential algebraic equations (DAE) (Shabana, 1994). The same set can be obtained by using alternate approaches, e.g. orthogonal complements of the velocity constraints (Huston and Passerello, 1974), Kane’s equations (Kane and Levinson, 1983), and others. Amongst these, the former approach has been adopted by many researchers for the automatic generation of the equations of motion for complex mechanical systems like the robots studied in this book. One such complement is the natural orthogonal complement (NOC), which * This chapter requires in-depth understanding of the Chapters 7–8. Hence, it can be taught to only postgraduate students, i.e., M. Tech./M.S./Ph. D. level students.

Print Area 6x8 Trim Area 7.25x9.5

206

Introduction to Robotics

was originally proposed by Angeles and Lee (1988) for serial robots, but generalised by Saha and Angeles (1991) to take into account the non-holonomic constraints of wheeled mobile robots as well. The NOC was later decoupled by Saha (1997, 1999, 2003), which was named as the Decoupled NOC (DeNOC), and shown to have the following advantages: • It allows one to obtain the recursive order (n)—n being the number of links in the robot—inverse and forward dynamics algorithms. The recursive forward dynamics algorithm was not possible with the original form of the NOC. • Each scalar element of the matrices and vectors associated with the equations of motion of the robot manipulator can be written analytically, which allows one to provide the physical interpretation like the articulated body inertia, etc., and helps a programmer to debug the computer algorithms. • Since the methodology is built upon basic mechanics and linear algebra theories, the concept can be even understood by the undergraduate students. The DeNOC based modelling has been successfully applied to (i) serial manipulators with fixed-base, as used in industrial robots (Saha, 1997; 1999; 2003); (ii) serial robots with free-base, a configuration for free-floating space robots (Saha, 1996); (iii) closed-loop parallel Stewart Platform type robots (Saha and Schiehlen, 2001; Khan et al., 2005), and Hexapod machine tools (Koteswara Rao et al., 2006); (iv) multi-closed-loop general mechanical systems (Chaudhary and Saha, 2006), and (v) serial flexible-link systems (Mohan and Saha, 2007a). As emphasised in the above papers and shown in Mohan and Saha (2007b), the DeNOC based formulation provides efficient and numerically stable algorithms.

9.1

DYNAMIC MODELLING

In this section, dynamic equations of motion of an n-link, n-degree-of-freedom (DOF) serial robot, as shown in Fig. 9.1(a), are derived using the Decoupled Natural Orthogonal Complement (DeNOC) matrices. First, the uncoupled Newton–Euler (NE) equations of motion, as introduced in Section 8.3, are written for all n links in a Modeling compact form. Next, the constraints between the links due to the joints, e.g. revolute, Modeling here means a way to be prismatic, etc. are expressed mathematically, able to understand the behaviour of a which brings the DeNOC matrix into picture. robot even without having a real one. The DeNOC relates the Cartesian velocities of all the links with the joint rates or velocities. Finally the pre-multiplication of the DeNOC matrix with the uncoupled NE equations yields a set of independent equations of motion, which are nothing but the Euler-Lagrange (EL) equations of motion as presented in Section 8.2. Hence, the EL equations are derived via DeNOC matrices without resorting to the complex partial derivatives given by eq. (8.24).

Recursive Robot Dynamics

Oi O3

Z

O2 #1 O1

Oi+1

#i

i

207

i+1

3

On

n

#2

#n

2 End-effector

Y

1

Frame, F X

Composite body, i

(a) Serial chain vi (velocity) Ci

ni ci O

fi

wi (ang. vel.) mi: mass Ii: inertia tensor

(b) The ith free-body

Fig. 9.1 A serial manipulator

9.1.1

Uncoupled Newton—Euler Equations

For the n-link n-degree-of-freedom (DOF) open-loop serial manipulator, Fig. 9.1(a), if mi is the mass of the ith link and Ii denotes the 3 ¥ 3 inertia tensor of the ith link about its mass center Ci as indicated in Fig. 9.1(b), then the Newton–Euler equations of motion for the ith link can be derived from its free-body diagram, and written in a reverse order as Euler’s Equation : Ii w& i + w i ¥ Iiw i = ni (9.1a) Newton’s Equation

:

mi &&c i = fi

(9.1b)

where w i and w& i are the 3-dimensional vectors of angular velocity and acceleration of the ith link respectively, whereas &&c i is the 3-dimensional vector of acceleration of the mass center Ci. Moreover, ni and fi are the 3-dimensional vectors of the resultant moment about Ci and the resultant force at Ci, respectively. Note here that no reference to the coordinate frame is made to express the vectors and matrices, as they can be represented in any frame of the analyst’s choice. Typically, they are expressed in the frame attached to the ith link, i.e., Frame (i + 1). However, during the derivations of the equations of motion in the subsequent sections and subsections

208

Introduction to Robotics

they will be avoided. Combining the eqs. (9.1a–b), the six scalar uncoupled NE equations of motion are written in a compact form as (9.2a) Mi t& i + Wi Mi Ei ti = wi where the 6 ¥ 6 mass matrix Mi, and the 6 ¥ 6 angular velocity matrix Wi, for the ith link are given by Mi ∫

LMI NO

OP Q

LM N

OP Q

O wi ¥ 1 O ; and Wi ∫ mi 1 O O

i

(9.2b)

in which w i ¥ 1 is the 3 ¥ 3 cross-product tensor associated with the angular velocity w i ¥ 1)x ∫ w i ¥ x, for any Cartesian vector x. vector w i , which is defined as (w Moreover, 1 and O are the 3 ¥ 3 identity and zero matrices respectively. Furthermore, the 6-dimensional vectors, twist ti , and wrench wi , are as follows: ti ∫

LMw OP and w ∫ LMn OP N c& Q Nf Q i

i

(9.2c)

i

i

i

where, in contrast to the definition of twist given in Section 6.6, the linear velocity of the mass center of the ith link C is considered. In eq. (9.2a) the vector t& is the time i

i

derivative of the twist vector ti as defined in eq. (9.2c). Equation (9.2a) is now written for all n links, i.e. i = 1, º, n, as M &t + WMt = w

(9.3a)

where M and W are the 6n ¥ 6n generalised mass matrix, and the generalised matrix of the angular velocities respectively. They are given by: M ∫ diag. [M1, º, Mn], and W ∫ diag. [W1, º, Wn] (9.3b) Also, the 6n-dimensional vectors of generalised twist and wrench are given as t ∫ [t1T, º, t nT]T; and w ∫ [w1T, º, wnT] T (9.3c) Equations (9.3a–c) represent the 6n uncoupled scalar NE equations of motion for the n links in the serial robot manipulator under study.

9.1.2

Kinematic Constraints

Links of the robot manipulator, Fig. 9.1(a), are coupled by kinematic pairs or joints, which are either revolute or prismatic. From the rigid body motion of the two bodies, namely #i and #j, as shown in Fig. 9.2, the angular and linear velocities of the ith link, i.e. the twist of the ith body defined in eq. (9.2c), can be derived from the velocities of the jth link, and the joint motion of the ith joint. They are derived as follows: w = w + e q& (9.4a) i

j

i i

c& i = c& j + w j ¥ rj + w i ¥ di

(9.4b)

The above six scalar velocity constraint equations can also be written in a compact form as t = B t + p q& (9.4c) i

ij j

i i

where qi is the joint displacement, angular for a revolute joint and linear for a prismatic joint. Moreover, the 6 ¥ 6 matrix Bij and the 6-dimensional vector pi are

Recursive Robot Dynamics qi

ej

Cj cjk Ck

ei #i

di

rj

#j

209

Ci

cij cik

ci

cj

Inertial frame

#k O

Fig. 9.2 Three coupled bodies

functions of the positions of the mass centres of the two successive bodies, i.e, Ci and Cj of Fig. 9.2, and the axis of the joint joining them, namely ei. They are defined as Bij ∫

LM 1 OOP and p ∫ L e O MNe ¥ d PQ Nc ¥ 1 1 Q i

i

ij

i

(9.4d)

i

cij ¥ 1 being the cross-product tensor associated with the vector cij as shown in Fig. 9.2, which is defined as being similar to w i ¥ 1 of eq. (9.2b), i.e, (cij ¥ 1)x = cij ¥ x for any arbitrary 3-dimensional Cartesian vector x. The vector cij is given by cij = –di – rj. It is interesting to note here that the matrix Bij and the vector pi have the following interpretations: • For two rigidly connected moving bodies #i and #j, Bij propogates the twist of #j to #i. Hence, matrix Bij is termed here as the twist propagation matrix, which has the following properties: BijBjk = Bik ; Bii = 1; and Bij–1 = Bji (9.5a) • Vector pi , on the other hand, takes into account the motion of the ith joint. Hence, pi is termed as the joint-rate propagation vector, which is dependent on the type of joint. For example, the expression of pi in eq. (9.4d) is for a revolute joint as shown in Fig. 9.2, whereas for a prismatic joint, vector pi is given by pi ∫

LM 0 OP : For prismatic joints Ne Q

(9.5b)

i

where ei is the unit vector parallel to the axis of linear motion. Correspondingly, q& i of eq. (9.4c) would mean the linear joint rate. Other joints are not treated here because any other joint, e.g. spherical or screw, can be treated as combination of three revolute, or revolute and prismatic pairs, respectively. For i = 1, º, n, eq. (9.4c) is put in a compact form for all the n joints as t = N q& , where N ∫ Nl Nd

(9.6a)

210

Introduction to Robotics

where t containing the twists of all the links is the 6n-dimensional generalised twist defined in eq. (9.3c). In eq. (9.6a), it is expressed as a linear transformation of the ndimensional joint-rate vector, q& . The 6n ¥ 6n matrix, N , the 6n ¥ n matrix, N , and l

the n-dimensional vector, q& , are defined as follows:

LM 1 B N ∫M MM M NB

21

1

n1

O 1 M Bn2

OP PP PQ

LM MM MN

p1 L O 0 L O ; Nd ∫ O M M 0 L 1

0 L p2 L M 0

O L

d

LMq& OP OP q& P ; and q& ∫ M P MM M PP M P P p Q MNq& PQ 0 0

1

n

n

2

(9.6b)

The 6n ¥ n matrix N in eq. (9.6a) is nothing but the Natural Orthogonal Complement (NOC) matrix of the velocity constraints (Angeles and Lee, 1988), and its decoupled form Nl and Nd, are referred as the Decoupled NOC (DeNOC) matrices (Saha, 1997; 1999). The expressions of the DeNOC matrices allow one to develop recursive inverse and forward dynamics algorithms required in control and simulation of robot manipulators respectively.

9.1.3

Coupled Equations of Motion

The uncoupled NE equations of motion given by eq. (9.3a) are re-written as M &t + WMt = w E + wC (9.7) E C E C where w of eq. (9.3a) is substituted by, w ∫ w + w —w and w being the external and constraint wrenches respectively. The external wrench wE is contributed from the moments and forces due to the joint actuators, gravity, environmental effects, etc., whereas the constraint wrench wC is due to the presence of the joints that contains the reaction moments and forces at the joint interfaces. Since the constraint wrench wC does not do any useful work towards the motion of the robot links, the power consumed due to wC, i.e. P C ∫ tT wC vanishes. The sole purpose of wC is to maintain the relative configuration of the links without getting separated. Using the expression for the generalised twist t from eq. (9.6a), the vanishing power due to wC, P C is given by P C ∫ tT wC ∫ q& T NT wC ∫ q& T NTd NTl wC = 0 (9.8a) For the n-link, n-degree-of-freedom (DOF) serial robot, the n-dimensional jointrate vector q& is independent. Hence, to satisfy eq. (9.8a), the following condition must hold good: NT wC ∫ NTd NTl wC = 0 (9.8b) T Now, upon multiplication of the transpose of the NOC N to the uncoupled NE equations of motion i.e., eq. (9.7), the following set of independent dynamic equations of motion are obtained: && + h = t , where h ∫ C q& Iq (9.9a) where the result of eq. (9.8b), and the time derivative of the generalised twist t from && + N & q& , are used. Note that eq. (9.9a), in comparison to the eq. (9.6a) namely, &t = N q

Recursive Robot Dynamics

211

equations of motion derived in Chapter 8 namely the eq. (8.44a), does not contain the term g due to gravity. In fact t of eq. (9.9a) contains the effect of g . In a way t of eq. (9.9a) = t of eq. (8.44a) –gg of eq. (8.44a). Moreover, ~ I ∫ NTMN ∫ NTd M Nd the n ¥ n generalised inertia matrix (GIM) is symmetric and positive definite; ~ ~ ~ & + WMN) ∫ NT ( M C ∫ NT (M N l + M w + M e )Nd d the n ¥ n matrix of convective inertia (MCI) terms; ~ : the n-dimensional vector of convective inertia (VCI) terms; h ∫ C q& = NT w¢ d

~ E : the n-dimensional vector of generalised forces due to the t ∫ N w ∫ NTd w driving torques/forces, and those resulting from gravity, environment and dissipation. ~ ~ ~ ~ ~ Also, the 6n ¥ 6n matrices, M , M l , M w , M e and the 6n-dimensional vectors, w¢ ~ E , are given by and w T

E

~ ~ ~ ~ ~ T &l,M M ∫ NlT MNl ; M l ∫ NlT M N w ∫ M W, M e ∫ N1 WMN1 ~ ∫ N T (Mt¢ + WMt) ; t¢ ∫ ( N ~ E ∫ N T wE & l + N W)N q& ; and w w¢ l l d l

(9.9b)

& d = WN is used, and the matrices N , M, W and the vector wE are defined where N d l && = 0. It is previously. Note above that t¢ is nothing but the twist-rate vector while q

pointed out here that eq. (9.9a) is also derived in Chapter 8, i.e., eq. (8.44a), using the Euler-Lagrange equations of motion. In the latter case, one requires complex partial differentiations, whereas the former is derived from the Newton-Euler equations of motion, which are simpler to visualise in the three-dimensional Cartesian space, and simple linear algebra concepts.

9.2

ANALYTICAL EXPRESSIONS

It can be shown that using the concept of the DeNOC matrices each element of the GIM, MCI, VCI and the generalised forces can be expressed analytically, which allow one to extract many physical interpretations and suggest ways to simplify the expressions and the computational complexity. The analytical expressions are obtained next.

9.2.1

Generalised Inertia Matrix (GIM)

The expressions for the n ¥ n GIM— I is given after eq. (9.9a) as ~ I ∫ NTMN ∫ NdT M Nd (9.10a) ~ T where M ∫ Nl MNl , is the 6n ¥ 6n symmetric composite mass matrix, which is obtained as

212

Introduction to Robotics

LM M~ ~ M B M ~ ~ M ∫ MM B MM M MNM~ B

sym

1

2

21

3

31

~ M2 ~ M 3B 32

n1

M ~ M n B n2

n

~ M3 M O ~ ~ M n B n3 º M n

OP PP PP PQ

(9.10b)

~ ~ where “sym” denotes the symmetric elements of the matrix M . The 6 ¥ 6 matrix M i evaluated from i = n to i = 1 as ~ M i ∫ Mi +

n

 BTki M k Bki

(9.10c)

k = i +1

which requires order n2 computations, as there is summation over k = i + 1, º, n. A close look into the equation along with the first two properties of eq. (9.5a) however, reveals that the summation expression can be evaluated recursively for i = n, º, 1, as ~ ~ ~ (9.10d) M i ∫ Mi + BTi+1,i Mi +1 Bi+1 ,i where M n ∫ M n The eq. (9.10d) has the following physical interpretations: (1) It is the mass matrix of a body composed of links #n, º, #i, that are rigidly connected. This is referred to as the composite body i, as indicated in Fig. ~ 9.1(a), whose name can be justified from the 3 ¥ 3 block matrices of M i . In eq. (9.10d), if i = n,

LM N

In ~ M n = Mn ∫ O

O

OP Q

(9.11a)

mn 1

and for i = n – 1,

~ ~ M n -1 = M n–1 + BTn,n–1 M n Bn,n–1

(9.11b)

which can be rewritten as

~ ~ I - d n -1 ¥ 1 ~ M n -1 ∫ ~ n -1 ~ 1 d n -1 ¥ 1 m n -1

LM MN

OP PQ

(9.11c)

~ , the 3-dimensional vector ~ d n -1 , and the 3 ¥ 3 matrix where the scalar m n -1 ~ In -1, are given by ~ ~ ~ (9.12a) m n -1 ∫ m n–1 + mn , where mn = mn ~ ~ ~ d n -1 ∫ mn cn,n–1 + d n , where d n = 0 (9.12b) ~ ~ ~ ~ In -1 ∫ I n–1 + In – d n -1 ¥ (cn,n–1 ¥ 1), where In = I n (9.12c) ~ The matrix In -1, is the inertia tensor of the body composed of rigidly con-

nected links, #(n – 1) and #n with respect to the mass center of the (i – 1)st link,

Recursive Robot Dynamics

213

i.e. Ci–1, in which the third term is nothing but the one associated with the transfer of the definition of In from Cn to Cn–1, being similar to the parallel axis theorem given in the Subsection 8.1.3. Continuing with i = n – 2, º, 1, the ~ , 3-dimensional vector d~ and the 3 ¥ 3 matrix ~I , are calculated as scalar m i i i follows: ~ ~ =m + m m (9.13a) i +1 i i ~ ~ d i ∫ mi+1 ci+1,i + d i +1 (9.13b) ~ ~ ~ ~ Ii = Ii + Ii +1 – d i +1 ¥ (c i+1,i ¥ 1) – ci+1, i ¥ ( d i ¥ 1) (9.13c) ~ (2) The inertia effect of the composite body (i + 1), Ii +1 is taken into account with respect to Ci, and added with the inertia tensor of link i with respect to Ci, Ii, to ~ give the inertia of the composite body i, with respect to Ci i.e., Ii . Other terms, i.e. 21 and 22-blocks of eq. (9.11c) are similarly included to define the mass ~ matrix of composite body, i, or the composite mass matrix M i . Now, by using ~ the expression I ∫ NdT M Nd, the symmetric GIM, I is written as ~ º º º p1T M1p1 sym ~ T T ~ º º p 2 M 2 B21p1 p 2 M 2 p2 M ~ ~ ~ T T T I ∫ p 3 M 3 B31p1 p 3 M 3 B32 p 2 (9.14a) º p 3 M 3p3 M

LM MM MM M MNp M~ B T n

n

M T ~ p p M n1 1 n n Bn2 p2

M O ~ p Tn M n B n3 p3 º

OP PP PP M ~ p M p PQ T n

n n

where “sym” denotes the symmetric elements, and the (i, j) element of the GIM, denoted by iij, which is given analytically as ~ iij ∫ pTi M i Bij pj , for i = n, º, 1; j = i, º, 1 (9.14b) Note that the symmetric elements above the diagonal elements of the GIM in eq. (9.14a), can be expressed as ~ iji ∫ pTj B Tij M i pj , for i = n ,º, 1; j = i – 1 ,º, 1 (9.14c) In eq. (9.14a) iij = iji. Hence, one is required only to compute either (9.14b) or (9.14c) in dynamics algorithm. In the examples of this chapter, the lower triangular elements of the GIM are computed using the (9.14b).

9.2.2

Matrix of Convective Inertia (MCI) Terms

Analytical expressions of the matrix of convective inertia (MCI) terms are derived from the MCI definition given after eq. (9.9a), i.e., ~ ~ ~ & + WMN) ∫ NT ( M C ∫ NT (M N (9.15a) l + M w + M e )Nd d ~ ~ ~ where the 6n ¥ 6n matrices M l , M w and M e are reproduced from eq. (9.9b) as ~ ~ ~ ~ T T &l;M (9.15b) M l ∫ NTl M N w ∫ MW ∫ N l MNl W; and M e ∫ N l WMNl

214

Introduction to Robotics

~ Matrix M l , is then obtained as

LM 1 O ∫ M MM M MNO

~ Ml

B T21 º 1 º

B Tn1 M T O B n, n -1 º 1

M O

OP LM PP MM O PP MM OM QN

1

O º M2 º M O

O º

OP LM O PP MMB& M PM M Q MNB& O M O

n

º º

21

O O

n1

O O & º B n, n - 1

O M O O

OP PP PP Q

(9.15c)

& i+1,1 is given by where the 6 ¥ 6 matrix B B& i +1, i ∫

LM O N- (r& + d&

O i + 1) ¥ 1 O

i

OP Q

(9.15d)

~ in which r&i = w i ¥ ri, and d& i +1 = w i+1 ¥ d i+1. The expression of M i is then rewritten

as

LMB H~ MM H~~ ∫ MM HM MN H~ T 21

~ B T31H 32 º T ~ B 32 H 32 º ~ O H 32

21

21

~ Ml

31

OP MP MP P MP O PQ O

M

M ~ T M B n , n - 1H n, n - 1 ~ º H n, n -1

M ~ H n2

n1

~ B Tn1H n , n - 1

(9.15e)

~ ~ ~ ~ & ~ & T In the eq. (9.15e) Hij = M i B ij + B i+1,i H i +1, i , and for i = n, MJ& ∫ M n B nj = O. ~ Next, the 6n ¥ 6n matrix M w , as defined in eq. (9.15b), is formed as ~ º º sym W1 O º O M1 ~ ~ O W2 º M º M B 21M 2 M2 ~ Mw ∫ M M O M M M O M ~ ~ ~ B n1M n B n2 M n º M n O º º Wn

OP L PP MM PP MMN Q

LM MM MM N

OP PP PQ

which yields

LM M~ W ~ M B W ∫ M MM M MNM~ B W 1

~ Mw

2

n

1

~ B T21M 2 W2 ~ M 1W2

1

M O ~ M n B n 2 W2 º

1

21

n1

~ º B Tn1M n Wn ~ º B Tn2 M n Wn M ~ M n Wn

OP PP PP Q

(9.15f)

~ Finally, the matrix M e is obtained as ~ Me

LM 1 O ∫ M MM M MNO

B T21 M B Tn1 M M 1 M O B Tn , n - 1 O º 1

OP LW M PP MM O PP MM OM QN 1

1

O º W2 M 2 M M O O

º

OP LM 1 PP MMB M O P M W M Q NB O M

n

21

n

n1

O 1 M

OP PP P 1Q

º O º M O M

B n2 º

215

Recursive Robot Dynamics

which can be written in a compact form as ~ º º sym M ¢1 ~ ~ º M B 21M ¢2 M ¢2 ~ (9.15g) Me ∫ M M O M ~ ~ ~ B n1M ¢n B n 2 M ¢n º M ¢n ~ ~ where M¢i for i = n, º, 1, is calculated similarly to the matrix M i , in eq. (9.10d), as ~ ~ ~ (9.15h) M¢i = M¢i + BTi+1,i M i¢ + 1 Bi+1,i

OP PP PP Q

LM MM MM N

~ in which M i¢ = Wi Mi, for i = n, º, 1. The scalar elements of the MCI, C, i.e., cij, for i, j = n, º, 1 are then obtained explicitly from eqs. (9.15a) as ~ ~ ~ ci j ∫ pTi (BTji M j Wj + BTj+1,i H j +1, j + BTji M¢ j ) pj if i £ j (9.15i) ~ ~ ~ ci j ∫ pTi ( M j Bij Wj + Hij + M¢i ) pj otherwise.

Comparing the eqs. (9.14b) and (9.15i), it is observed that the elements of the GIM and MCI are expressed in a uniform manner, i.e., pTi (.)pj, where (.) denotes the matrix argument. In eq. (9.15i) the explicit analytical expression for the each element of the MCI is available, which is not advisable to be used for the calculation of vector h(∫ C q& ) in eq. (9.9a) but is suitable for the physical interpretations and debugging. The vector h is termed here as the vector of convective inertia (VCI) terms. Whereas the explicit evaluation of the MCI, C requires order (n2)—n being the degree of freedom of the robot—calculations, the VCI h can be computed recursively needing order n operations only. The order n algorithm for the VCI is given next. Now, as per as the physical interpretation of the MCI is concerned, note that for a planar robot with all revolute joints, the axes are all parallel and do not change their direction while the & d = WN = O. As a result the term robot is in motion. Hence, their time derivative N d

~ M w vanishes to provide simpler expressions.

9.2.3

Vector of Convective Inertia (VCI) Terms

The vector of convective inertia (VCI) terms h is given after eq. (9.9a) as

~ , where w¢ ~ = NT (Mt¢ + WMt) and t¢ = ( N & 1 + N W) q& (9.16) h ∫ C q& = NTd w¢ 1 1 && = 0, i.e., it contains the Note that t¢ is the generalised twist-rate vector while q centrifugal and Coriolis acceleration terms. Introducing the following notations M¢ = WM and w ¢ = Mt¢ + M¢t and substituting the expression for Nl from eq. (9.6b) into eq. (9.16), yields ~¢ w¢ w 1 BT º BT

LM ~ = MO w¢ MM M MNO

21

1 º

n1

M O B Tn, n -1 O 1

OP L O L O PP MMw¢ PP = MMw~ ¢ PP PP MMwM¢ PP MMw~M¢ PP QN Q N Q 1

1

2

2

n

n

(9.17a)

216

Introduction to Robotics

where w¢i = Mi t¢i + M¢i ti , for i = n, º, 1. The 6n-dimensional vector w¢ can be interpreted as the generalised wrench due to the convective inertia terms. Moreover, ~ , in eq. (9.17a) can be obtained recursively as the elements of the vector w¢ i ~ ~ = w¢ + BT ~ w¢ i i+1,i w¢i + 1 , where w¢n = w¢n i

(9.17b)

Furthermore, using the expression for Nd, the VCI of eq. (9.16) h is given by

LM p p h ∫M MM MNp

~¢ w 1 ~ w¢ 2 M T ~ n w ¢n T 1 T 2

OP PP PP Q

(9.18a)

in which each element hi is written as ~ , for i = n, º, 1 h = pT w¢ i

i

(9.18b)

i

The expressions in the eqs. (9.17b) and (9.18b) together provide an order n algorithm for the VCI.

9.2.4

Generalised Force

The expressions for the elements of the generalised forces t given after eq. (9.9a) are as follows: ~ E , where w ~ E = NT w E t = Nd w l

(9.19a)

Upon substitution of the expression for Nl from eq. (9.6b) and noting that, ~ E , is written as w ∫ [(w1E )T º(wnE)T ] T, the 6n-dimensional vector, w E

~E w

LM 1 O ∫ M MM M MNO

B T21 º 1 º

B Tn,1 M O B Tn , n - 1 O 1

OP LMw PP MMw PP MMwM QN

E 1 E 2 E n

OP LMw~ PP = MMw~ PP MMw~M Q N

E 1 E 2 E n

OP PP PP Q

(9.19b)

~ E ∫ w E + BT w ~ E and w ~ E ∫ w E . The generalised force, t , is then found where w n n i +1 i i i+1, i

as

LMp p t ∫ M MM MNp

T 1 T 2

~E w 1 ~E w 2

M T ~E n wn

OP PP PP Q

(9.20a)

From eq. (9.19a), each element of the n-dimensional vector t , i.e. ti is then obtained from ~ E , for i = n, º, 1 ti = p iT w i

(9.20b)

217

Recursive Robot Dynamics

Equations (9.9a), (9.14b), (9.15i) or (9.18b), and (9.20b) together provide the explicit expressions for the dynamic equations of motion for the n-link, n-degree of freedom serial robot.

Example 9.1

One-link Planar Arm

For the one-link arm shown in Fig. 9.3, the equation of motion is derived here using the concept of the DeNOC matrices. Using the eqs. (9.6a–b), the DeNOC matrices for the one-link arm is given by N = Nl Nd = p, where NI = 1, and Nd = p (9.21) Note above that subscript 1 to the 6dimensional joint-rate propagation vector p is not put, as there is only one joint. The inertia term, which is a scalar for the one-link arm, is then obtained from eq. (9.14b) as

Y1

O t C

q X1

mg

a Y2 X2

Fig. 9.3 One-link arm

~ I(∫ i11) = pT M p = eT Ie + m(e ¥ d)T (e ¥ d) ;

where

p ∫

LM e OP ; M~ ∫ M = LM I Ne ¥ d Q NO

OP Q

O m1

(9.22a)

In eq. (9.22a), the subscript 1 is also not used to the vector and matrix notations, as there is only one link and one joint. Now, the 3-dimensional vectors e and d, in the fixed frame, i.e. Frame 1, are obtained as

LM 1 acq N2

OP Q

T

1 (9.22b) asq 0 2 Moreover, the 3 ¥ 3 inertia matrix, I, for the one-link arm about its mass center C in the link-fixed frame, i.e., Frame 2, is written as

[e]1 ∫ [0 0

1]T; [d]1 ∫

OP PP Q

LM MM N

0 0 0 ma 2 0 1 0 [I]2 = 12 0 0 1

(9.22c)

Using the eq. (8.72) one can write

LM s q MM- sq cq N 0 2

2

ma [I]1 = Q[I]2 Q = 12 T

- s q cq 2

s q 0

OP 0P 1PQ 0

(9.22d)

218

Introduction to Robotics

where Q is the 3 ¥ 3 rotation matrix given by

LMcq Q = sq MM 0 N

OP 0 P 1 PQ

- sq

0

cq 0

(9.22e)

Substituting eqs. (9.22b-d) in eq. (9.22a) then yields 1 I = ma2 (9.22f) 3 Next, the VCI is obtained using eq. (9.15i), where for the one-link case B11 = ~ ~ 1, B T21 H 21 = O, as it does not arise, and M¢ ∫ M¢ = WM. Hence, C(∫ c11) = pT (MW + WM)p = q& eT [I(e ¥ e) + (e ¥ Ie)] = 0 (9.23a) where the expression of the 6 ¥ 6 matrix W used above is given by q& e ¥ 1 O W= (9.23b) O O Moreover, the 6-dimensional vector p and the 6 ¥ 6 matrix M are given in eq. (9.22a). Furthermore, the properties of the vector products are used in obtaining the result of the eq. (9.23a), i.e., e ¥ e = 0, and eT (e ¥ Ie) = 0. The latter result is due to the fact that vector e is orthogonal to the one that is obtained from its cross-product, namely e ¥ Ie. Finally, the generalised force is obtained from

LM N

t1 = NTwE = e T

OP Q

(e ¥ d) T

LMnOP = t – 1 mgasq Nf Q 2

(9.23c)

where the external moment due to the actuator, and external force due to gravity represented in Frame 1 are given as [n]1 ∫ [0 0 t]T, [f]1 ∫ [mg 0 0 ]T (9.23d) The subscript 1 is added to the generalised force on the left side of eq. (9.23c), i.e. t1. This is to distinguish the generalised force t1 from the actuator torque t on the eq. (9.23d). It is pointed out here that for the simple systems like the one in this example, it may be easier to obtain the dynamic equation of motion, eq. (9.23c), from the direct application of the expressions in terms of the NOC matrix that appear after eq. (9.9a).

Example 9.2

Two-link Planar Manipulator

A two-link planar manipulator with two revolute joints is shown in Fig. 9.4. The manipulator has two degrees-of-freedom, whose independent coordinates are q1, and q2. Correspondingly, the joint rates are q& 1 and q& 2 . The 2-dimensional joint-rate vector q& is then defined as (9.24) q& ∫ [q& , q& ]T 1

2

Recursive Robot Dynamics

Y1

219

(x, y) EE Y2

a2

C2 q2

d2

e2

C1

d1

R a1

e1

X2

X1

q1 R

Fig. 9.4 Two-link planar manipulator

The link lengths are a1 and a2, and masses are m1 and m2. The elements of the generalised inertia matrix (GIM) are then calculated as follows: For i, j = 2, the calculation of i22 is shown below from eq. (9.14b) as ~ i22 ∫ p T2 M 2 B22 p2 (9.25a) where

LM e OP ; B ∫ LM 1 OOP ; NO 1 Q Ne ¥ d Q LI O OP =M ∫ M N O m 1Q 2

p2 ∫

22

2

and

~ M2

2

2

2

(9.25b)

2

where vector [e2]2 ∫ [e2]1 ∫ [0, 0, 1]T is the unit vector along the axis of the second revolute joint, as indicated in Fig. 9.4. Vector d2 is shown in Fig. 9.4, whereas I2 is the inertia tensor about C2. Using the eq. (9.25b), and assuming the mass center lies at the center of the link length, i.e. d2 = a2 /2, the (2, 2)element of the GIM, i.e., i22, is then explicitly obtained, similarly to I (∫ i11) of Example 9.1 as i22 = [e2]T2 [I2]2 [e2]2 + m2[e2 ¥ d2]2T [e2 ¥ d2]2 1 1 1 = m a2 + m a2 = m a2 (9.25c) 12 2 2 4 2 2 3 2 2 For i = 2, j = 1, the element, i21, is given next as T ~ i21 = p T2 M 2 B21 p1, where p1 ∫ e1T , (e1 ¥ d1) T (9.26a) and the matrix B21 is given by B21 =

OO LM 1 N- (r + d ) ¥ 1 1 PQ 1

2

(9.26b)

220

Introduction to Robotics

where vector r1= a1 – d1. Hence, the (2, 1)-element of the GIM, i21, is calculated, similarly to i22, as i21 = eT2 I2 e1 + m2 (e2 ¥ d2)T [e1 ¥ (a1 + d2)] = e T2 I2 e2 + m2 d T2 a1 + m2 d T2 d2 1 1 1 = m a 2 + m a a cq + m a 2 12 2 2 2 2 1 2 2 4 2 2 1 1 = m2 a 22 + m2 a1 a2 cq2 (9.26c) 3 2 In eq. (9.26c), the vector product rule, namely, (a ¥ b)T (c ¥ d) = (aTc)(bTd) – T (a d)(bTc) is applied. Moreover, e1 = e2, e 2Te2 = 1 because e2 is a unit vector, e 2T d1 = 0 because e2 is orthogonal to d1. Furthermore, di = ai /2, ri + di = ai , for ~ 1 i = 1, 2, r1 + d1 = a1, and d T2 a1 = a2 a1 cq1. Next, matrix M1 is calculated as 2 ~ - d1 ¥ 1 I1 ~ ~ (9.27a) M1 = M1 + BT21 M 2 B21 = ~1 d1 ¥ 1 m 1 ~ ~ ~ are given below; where I1 , d 1, and m 1 ~ ~ ~ ~ = m + m (9.27b) I1 = I1 + I2 – m2 c21 ¥ ( d1 ¥ 1); d1 = m2c21; and m 1 2 1 ~ ~ ~ in which I2 ∫ I2, d 2 = 0, and m2 = m2 are used. Now, for i, j = 1, the (1, 1)element of the GIM, i11, is now found as ~ ~ d T d = 1 (m a 2 + m a 2 ) + m a 2 + m a a cq (9.28a) i11 = e T1 I1 e1 + m 2 2 2 1 2 1 2 2 1 1 1 3 1 1 For i, j = 2, 1, the MCI elements are given from eq. (9.15i) as ~ ~ ~ c22 = p T2 (B T22 M 2 W2 + B T32 H 32 + B T22 M¢2 )p2 = 0 (9.29a) ~ ~ ~ 1 c21 = p T2 ( M 1 B 21 W1 + H 21 + M¢2 )p1 = m2 a1 a2 sq2 q& 2 (9.29b) 2 ~ ~ ~ c12 = p T1 (B T21 M 2 W2 + B T31 H 32 + B T21 M¢2 )p2

OP Q

LM N

1 m a a sq (q& + q& 2 ) 2 2 1 2 2 1 ~ ~ ~ = p T1 (B T11 M1 W1 + B T31 H 22 + B T21 M¢1 )p1

=– c11

(9.29c)

1 m a a sq q& (9.29d) 2 2 1 2 2 2 ~ ~ In the eqs. (9.29a–c), H 32 ∫ O and M¢2 p2 = 0 were used. Moreover, for planar robots, it can be shown that the term Mi Wj , for i, j = 1, 2, vanishes. Finally, the elements of vector h can be shown to have the following expressions: ~ = 1 m a a sq q& 2 h2 = p T2 w¢ (9.30a) 2 2 2 1 2 2 1 ~ = – m a a sq 1 q& + q& q& h1 = p T1 w¢ (9.30b) 2 2 1 2 2 1 1 2 2

=–

FH

IK

Recursive Robot Dynamics

221

which are nothing but the result of the multiplication of the elements of the MCI, the eqs. (9.29a–d), with those of the joint rate vector q& , eq. (9.24).

9.3

RECURSIVE INVERSE DYNAMICS USING RIDIM†

Inverse dynamics is defined as “given a robot’s geometrical and inertial parameters, along with Why ‘Inverse?’ its joint motions, find the joint torques and forces.” In this section, it is emphasised that for Compared to forward dynamics, “rethe purpose of inverse dynamics analysis it is verse” or “inverse” calculations are not required to explicitly evaluate the matrices performed using dynamic equations and vectors appearing in eq. (9.9a), as the of motion. Hence, the term is used. required right hand side can be evaluated using a recursive order (n) algorithm (Saha, 1999), where n is the number of links in the robot manipulator. The recursive algorithm was later used to develop a general purpose C++ program, which was given an acronym RIDIM (Recursive Inverse Dynamics for Industrial Manipulator) (Marothiya and Saha, 2003).

9.3.1

Recursive Inverse Dynamics Algorithm

The recursive algorithm (Saha, 1999) implemented in RIDIM is presented here. It has two recursions, namely, forward and backward. They are given below: Forward Recursion a 1 = p1q& 1; a 2 = p2q& 2 + a 1;

b 1 = p1q&&1 + W 1 p1q& 1 & a b 2 = p2q&& 2 + W 2 p2q& 2 + B21b 1 + B 21 1

M a n = p nq& n + a n–1;

M & b n = pnq&& n + W n pn q& n + Bn,n–1 b n–1 + B n, n -1 a n–1

where the 6 ¥ 6 matrix, W i, is defined as Wi ∫

LMw ¥ 1 N O i

O wi ¥ 1

OP Q

(9.31)

Backward Recursion

†

g n = Mn b n + Wn Mna n; g n–1 = M n–1b n–1 + Wn–1 M n–1a n–1 + BTn,n–1g n;

tn = pTng n tn–1 = pTn–1g n–1

M g 1 = M1b 1 + W1M1a 1 + BT21 g 2;

M t1 = pT1g 1

RIDIM (Recursive Inverse Dynamics for Industrial Manipulators) is an in-house developed C++ program with MS-Windows interface.

222

Introduction to Robotics

where a i , b i , and g i are the 6-dimensional vectors with the dimensions of twist, twist rate, and the wrench. Note from the torque/force expressions above and that in eq. ~ E = 0. This implies that the required joint torques and ~ E , while w (9.20b), g ∫ w n +1

i

i

forces are due to system motion only without any external moments and forces on the end-effector. Moreover, if gravity is present it is taken into account by providing a negative acceleration due to the gravity, denoted by g to the first body, #1 (Kane and Levinson, 1983), i.e. an additional term is added to b 1 as b 1 = p1q&&1 + W 1 p1q& 1 + r , where r ∫ [0, –gT ] T

(9.32)

Now, for an all revolute-joint serial manipulator, the computational complexity of the above algorithm is shown in Table 9.1, which is compared with some of the other existing inverse dynamics algorithms. Even though the above algorithm is not the best but it is very simple, as is evident from the two-step algorithm where six dimensional vectors are treated similarly to those of the three dimensional vectors. That is, if one knows a 1 finding out a 2 is very similar to the evaluation of the linear velocity of body 2 from the known linear velocity of body 1. Table 9.1 Computational complexities for inverse dynamics

Algorithm Hollerbach (1980) Luh et al. (1980) Walker and Orin (1982) RIDIM (Saha, 1999) Khalil et al. (1986) Angeles et al. (1989) Balafoutis et al. (1988)

9.3.2

Multiplications/ Divisions (M)

Addition/ Subtraction (A)

412n–277 150n–48 137n–22 120n–44 105n–92 105n–109 93n–69

320n–201 131n+48 101n–11 97n–55 94n–86 90n–105 81n–65

n=6 2195M 852M 800M 676M 538M 521M 489M

1719A 834A 595A 527A 478A 435A 421M

RIDIM Program

RIDIM program has been developed in C++ for the inverse dynamics of a general nlink manipulator. In order to make the program user-friendly a user interface in MSWindows has also been developed, as is shown in Fig. 9.5. The program also has an inverse and forward kinematic analyses modules to generate data suitable for inverse Why ‘industrial’ dynamics and result verification, respectively. in RIDIM? Required inputs for the inverse dynamics The program can be used for robots analysis using RIDIM are as follows: For i = 1, for non-industrial applications also. º, n, However, majority of industrial robots are of serial type. Hence, the word is used.

1. Constant Denavit-Hartenberg (DH) parameters (Denavit and Hartenberg, 1955)

Recursive Robot Dynamics

223

ENGINEERING

Fig. 9.5 MS-Windows interface of RIDM

of the robot under study, which are defined in Chapter 5, namely, in Section 5.4. They are bi, ai, ai,, for a revolute joint, and qi, ai, ai, for a prismatic joint. The parameters bi, qi, ai, and ai, are referred to as the joint offset, joint angle, link length, and twist angle, respectively. 2. Time history of the variable DH parameter, i.e. qi, for a revolute pair, and bi, for a prismatic joint, and their first and second time derivatives, i.e. q& i , b&i , and q&& i , b&&i , respectively. 3. Mass of each body, mi. 4. Vector denoting the distance of the (i + 1)st joint from the ith mass center Ci in (i + 1)st frame, i.e. [ri]i+1. 5. Inertia tensor of the ith link about its mass center Ci in the (i + 1)st frame, [Ii]i+1. Note that RIDIM has following features: • It can handle manipulators with both revolute and prismatic pairs or joints; • Gravity is taken into account by providing negative acceleration due to the gravity denoted by g, to the first body, #1, as shown in eq. (9.32).

Example 9.3

Inverse Dynamics of Three-DOF Planar Manipulator

The three-link manipulator under study, as shown in Fig. 9.6, whose DH and inertial parameters are shown in Table 9.2. It is assumed that the manipulator moves in the X-Y plane, where the gravity is working in the negative Y direction. Let i and j be the two unit vectors parallel to the axes X and Y , respectively, and k = i ¥ j. The three joint torques, namely, t1, t2, and t3, were

224

Introduction to Robotics

a2 2 d2 q2

C2

a2 2 r2

Y

a1

g C3

q3

End effector

r13 q1

X

Fig. 9.6 Three-link manipulator

evaluated using RIDIM program to which smooth joint angles, as taken in Chapter 8, Subsection 8.5.1, and the corresponding rates and accelerations were taken as input. Table 9.2

The DH and inertia parameters of PUMA robot (a) DH parameters

Link

Joint

ai (m)

bi (m)

ai (rad)

qi (rad)

1 2 3

r r r

0.3 0.25 022

0 0 0

0 0 0

JV [0] JV [0] JV [0]

JV: Joint variable with initial values inside brackets, [ and ]; r: Revolute joint (b) Mass and inertia parameters

Link

mi (kg)

ri,x

1 2 3

0.5 0.4 0.3

0.15 0.125 0.11

ri,y ri,z Ii,xx Ii,xy Ii,xz Ii,yy Ii,yz 2 (m) (kg-m ) 0 0 0

0 0 0

0 0 0

0 0 0

0 0 0

0.00375 0.00208 0.00121

0 0 0

Ii,zz 0.00375 0.00208 0.00121

The joint angle is given by

LM F H N q (T ) - q ( 0 ) L = MN1 - cos FH 2Tp tIK OPQ ; T q (T ) - q ( 0 ) L 2 p = MN T sin FH 2Tp tIK OPQ T

qi = qi (0) + q& i q&& i

q i (T ) - q i ( 0) 2p T sin tt 2p T T

i

i

i

i

I OP KQ

(9.33a)

(9.33b)

Recursive Robot Dynamics

225

200

Joint angle (degree)

150

100

50

0

0

2

4

Fig. 9.7

t (sec)

6

8

10

Joint trajectory

where T = 10 sec, initial joint values, qi (0) = q& i (0) = q&& i (0) = 0, and the final joint values, q (T) = p, q& (0) = q&& (0) = 0, for i = 1, 2, 3, were taken. The joint i

i

i

trajectory is shown in Fig. 9.7, whereas the joint torques obtained from RIDIM are then plotted in Fig. 9.8. The terms, “tau_1”, “tau_2” and “tau_3” in Fig. 9.8, denote the joint torques, t1, t2, and t3, respectively. The plots were also verified with those obtained from the explicit expressions available in many textbooks on robotics, e.g. Angeles (2003). 5

tau_1

4

Torques (Nm)

3 2

tau_2

1

tau_3

0 –1 –2

0

2

4 6 Time (sec)

8

Fig. 9.8 Joint torques for the three-link manipulator

10

226

Introduction to Robotics

Example 9.4

PUMA Robot

For the PUMA robot shown in Fig. 9.9, the inverse dynamics results were obtained for its DH and inertial parameters shown in Table 9.3. Ze

End-effector

Xe 0.056 Z4, Z6

Z5

X5, X6 0.042

X3 Z3

Z1 X1, X2

X4

02

0.

0.432 Z2

0.149

Fig. 9.9 A PUMA robot

The trajectory for each joint is taken same as for the three-link manipulator, as defined in eqs. (9.33a-b) for the same T, and initial and final joint values. Corresponging joint torque plots are given in the Figs. 9.10(a–f). Table 9.3

The DH and inertia parameters of PUMA robot (a) DH parameters

Link

Joint

1 2 3 4 5 6

r r r r r r

ai (m) 0 0.432 0.02 0 0 0

bi (m)

ai (rad)

qi (rad)

0 0.149 0 0.432 0 0.05

–p/2 0 –p/2 –p/2 –p/2 0

JV [0] JV [0] JV [0] JV [0] JV [0] JV [0]

JV: Joint variable with initial values inside [ and ]; r: Revolute joint

227

Recursive Robot Dynamics (b) Mass and inertia parameters

Link 1 2 3 4 5 6

mi (kg)

ri,x

10.521 15.761 8.767 1.052 1.052 0.351

ri,y (m)

ri,z

0 0 0.054 0.292 0 0 0.02 0 – 0.197 0 –0.057 0 0 0 –0.007 0 0 0.019

Ii,xx 1.612 0.4898 3.3768 0.181 0.0735 0.0071

6

Torque (Nm)

Torque (Nm)

2 0 –2 –4 0

2

4 6 Time (sec) (a) Joint 1

8

1.612 8.0783 3.3768 0.1273 0.1273 0.0071

0

2

4 6 Time (sec) (b) Joint 2

0

2

0.15 0.1

10 0 –10

8

10

8

10

8

10

0.05 0

–0.05

–20 0

2

4

6

8

–0.1

10

Time (sec) (c) Joint 3

4

6

Time (sec) (d) Joint 4

0.15

0.02

0.1

0.1

Torque (Nm)

Torque (Nm)

0.5091 8.2672 0.3009 0.181 0.0735 0.0141

–50

20

0.05 0

0

–0.01

–0.05 –0.1

0 0 0 0 0 0

0

30

–30

0 0 0 0 0 0

Ii,zz

50

–100

10

Torque (Nm)

Torque (Nm)

0 0 0 0 0 0

Ii,yy Ii,yz (kg-m2)

100

4

–6

Ii,xy Ii,xz

–0.02 0

2

4 6 Time (sec) (e) Joint 5

8

10

–0.03

0

2

4 6 Time (sec) (f) Joint 6

Fig. 9.10 Joint torques for PUMA robot

228

Introduction to Robotics

Example 9.5

Stanford Arm

For the Stanford arm shown in Fig. 9.11, the DH and other parameters are shown in Table 9.4. Note that it differentiates from the PUMA robot in a way that it has a prismatic joint, in the joint location 3. The functions for the revolute joints were taken exactly same as in the PUMA robot of Example 9.4, with the following data: T = 10 sec, qi (0) = 0, for i π 2,3, q2 (0) = p/2; and qi (T) = p/3, for i π 3. Since the third joint is prismatic, the joint offset, denoted by b3 is a variable. Its variation is taken same as in eq. (9.33a), namely,

LM N

F H

bi (T ) - bi (0 ) 2p T tsin t T 2p T where b3(0) = 0, and b3(T) = 0.1 m.

bi = bi (0) +

I OP KQ

(9.34)

End-effector

Ze Z4, Z6 0.6

X3

Z1 X2

Xe X5, X6 Z5 X4 b3

Z3 Z2

0.1

0.1

X1

Fig. 9.11 The Stanford arm Table 9.4

The DH and inertia parameters of Stanford arm (a) DH parameters

Link

Joint

ai (m)

bi (m)

ai (rad)

qi (rad)

1 r 0 0.1 –p/2 JV [0] 2 r 0 0.1 –p/2 JV [0] 3 p 0 JV [0] 0 0 4 r 0 0.6 p/2 JV [0] 5 r 0 0 –p/2 JV [0] 6 r 0 0.0 0 JV [0] JV: Joint variable with initial values within [ and ]; r: Revolute joint; p: Prismatic joint

Recursive Robot Dynamics

229

(b) Mass and inertia parameters

Link

mi (kg)

ri,x

1 2 3 4 5 6

9 6 4 1 0.6 0.5

0 0 0 0 0 0

ri,y (m)

ri,z

–0.1 0 0 0 0 0 –0.1 0 0 –0.06 0 0.2

Ii,xx

Ii,xy

0.01 0.05 0.4 0.001 0.0005 0.003

0 0 0 0 0 0

Ii,xz Ii,yy 2 (kg-m ) 0 0 0 0 0 0

0.02 0.06 0.4 0.001 0.0005 0.001

Ii,yz

Ii,zz

0 0 0 0 0 0

0.01 0.01 0.01 0.0005 0.0002 0.002

The joint torques and force obtained from RIDIM are then plotted in Figs. 9.12(a–f), which exactly match with those reported in Cyril (1988) for the same arm.

9.4

RECURSIVE FORWARD DYNAMICS AND SIMULATION

Forward dynamics is defined as given the joint torques and forces, along with the robot’s physical parameters, to find the joint accelerations, i.e. solve for the joint && , from the dynamic equations of accelerations q motion eq. (9.9a). Simulation, on the other hand, involves forward dynamics, followed by the solution of the differential equations in joint accelerations to obtain joint velocities and positions, i.e. q& and q , respectively, for a given set of initial joint positions and rates of the manipulator under study, i.e. q& (0) and q (0).

9.4.1

Why ‘Forward?’ This step for simulation leads to the behaviour of a robot as if is existed and one is moving the robot. Hence, the word “forward” is used.

Recursive Forward Dynamics Algorithm

Conventionally, joint accelerations are solved from eq. (9.9a) using the Cholesky decomposition of the GIM I, as done by Walker and Orin (1982), Angeles (2003), and f \I” (Matlab, 2002), where f represents the others, or in the MATLAB command of “f vector of generalised forces due to the external moments and forces, gravity and Coriolis terms, etc. The above approach requires order (n3)—n being the number of links in the robot—computations, and produces non-smooth joint accelerations (Ascher et al., 1997). On the contrary, the dynamic formulation based on the Decoupled Natural Orthogonal Complement (DeNOC) matrices presented in this && from eq. (9.9a) recursively with order (n) chapter allows one to solve q computational complexity (Saha, 1999; 2003). Such recursive algorithms are known

Introduction to Robotics

0.15

18

0.1

17.5 17 16.5 16

Torque (Nm)

Torque (Nm)

230

0.05 0 –0.05 –0.1 –0.15

0

2

4 6 Time (sec) (a) Joint 1

8

10

–20 –25 –30

1.35 1.3 1.25 1.2 1.15 1.1 1.05 1 0.95 0.9 0.85

Torque (Nm)

0 –5 –10 –15

0

2

4 6 Time (sec) (c) Joint 3

8

10

0.1 0 –0.1 –0.2 –0.3 –0.4 –0.5 –0.6 –0.7 –0.8 –0.9

0

2

4 6 Time (sec) (b) Joint 2

8

10

0

2

4 6 Time (sec) (d) Joint 4

8

10

0

2

4 6 Time (sec) (f) Joint 6

8

10

0.0003 0.0002

Torque (Nm)

Torque (Nm)

Force (N)

5

15.5 15 14.5

0.0001 0 –0.0001 –0.0002

0

2

4 6 Time (sec) (e) Joint 5

8

10

–0.0003

Fig. 9.12 Joint torques and force for Stanford arm

&& , as reported in Ascher et al. (1997), Mohan and Saha to provide smooth profiles for q (2007), and others. In this subsection, a recursive order (n) forward dynamics algorithm is presented, which requires the following inputs: For i =1, º, n,

1. Inverse dynamics items, 1, and 3–5, as mentioned in Subsection 9.3.2. 2. Initial values for the variable DH parameters, i.e.qi, for a revolute joint, and bi, for a prismatic joint, and their first time derivative, i.e. q& and b& . i

3. Time history of the input joint forces/torques, i.e., ti.

i

Recursive Robot Dynamics

231

4. Each component of the vector, j ∫ t – C q& , obtained from eq. (9.9a), i.e. fi, which are to be calculated recursively using an inverse dynamics algorithm, && = 0. e.g. the one given in Subsection 9.3.1, while q The algorithm is based on the UDUT decomposition of the generalised inertia matrix, I of eq. (9.9a), i.e. I = UDUT, where U and D are the upper and diagonal matrices, respectively. Moreover, substituting for I, and, j ∫ t – C q& , in eq. (9.9a), one obtains && ∫ j UDUT q

(9.35)

A three step recursive scheme is then used to calculate the joint accelerations from the above equations, i.e. 1. Solution for j$ : The solution, j$ = U –1 j , is evaluated in terms of the scalar terms as f$ i = fi – pTi h i,i+1, for i = n, º, 1

(9.36a)

Note f$ n ∫ fn, and the 6-dimensional vector, h i,i+1, is obtained as h i,i+1 ∫ BTi+1,i h i+1 and h i+1 ∫ y i+1 f$ i +1 + h i+1,i+2

(9.36b)

in which h n,n+1 = 0. The new variable, y i+1, is the 6 ¥ 6 matrix which is evaluated using y$ $ p and m$ ∫ pT y $i y i = i , where y$ i ∫ M (9.36c) i i i i m$ i $ , called the “articulated body inertia” (Saha, In eq. (9.36c), the 6 ¥ 6 matrix, M i ~ 1997;1999) can be obtained recursively, similar to M i in eq. (9.10d), as ~ $ i+1 – y$ i +1 y T M i ∫ Mi + BTi+1,i Mi +1 B i+1,i , where M i+1 = M i+1 (9.36d) $ =M . for i = n – 1, º, 1, and M n n ~ ~ = j$ , involves the inverse of the diagonal 2. Solution for j : The solution, D j matrix, D of eq. (9.35), which is simple, namely, D–1 has only non-zero diagonal elements that are the reciprocal of the corresponding diagonal elements of ~ is obtained as follows: For i = 1, º, n, D. Vector j ~ f i = j$ i / m$ i

(9.37)

The scalar, m$ i , is defined in eq. (9.36c). ~ , is calculated, for i = 2, º, n, as && : In this step, = U –T j 3. Solution for q ~ ~ (9.38a) q&& = f – y$ T m i

i

i

i , i -1

232

Introduction to Robotics

~ ~ where q&& n = f 1, and the 6-dimensional vector, m i, i -1 , is obtained as ~ ~ ~ ∫ p q&& + m ~ ∫B and m m m i, i -1

i,i–1

i -1

i -1

i–1 i -1

i - 1, i - 2

(9.38b)

~ ∫0 in which m 10 The complexity of the proposed forward dynamics algorithm is compared in Table 9.5 with some other algorithms. The comparison clearly shows that the algorithm presented above is best. Moreover, using any other recursive algorithm the computational complexity for forward dynamic benefits over the conventional order (n3) algorithm when n ≥ 12, whereas the above algorithm benefits even for n ≥ 10. Table 9.5 Algorithm

Multiplications/ Divisions (M)

Saha (2003) Featherstone (1983) Valasek [14] Brandl et al. [14] Walker and Orin (1982)

9.4.2

Computational complexities for forward dynamics

191n–284 199n–198 226n – 343 250n – 222 n3/6+23

Addition/ Subtraction (A) 187n–325 174n–173 206n–345 220n – 198 n3/6+7n2+233n/3–46 n2/2+115n/3–47

n=6

n = 10

862M 797A 996M 871A 1013M 891A 1278M 1122A 633M 480A

1626M 1545A 1792M 1527A 1917M 1715A 2278M 2002A 1653M 1209A

Simulation

&& , followed by Simulation consists of forward dynamics to find the joint acceleration q its integration, i.e. to obtain q& and q , for a given set of initial conditions, i.e. q& (0) and q (0). As pointed out in Chapter 8 in Subsection 8.5.2, that except in extremely simple cases, the integration needs to be done numerically either using the well-established methods like Runge–Kutta–Fehlberg, Adams–Bashforth–Moulton and others (Shampine, 1994), or MATLAB command like “ode45” (Matlab, 2002). In this subsection, simulation results were generated using a computer program written in C++ for the implementation of the forward dynamics algorithm given in the Subsection 9.4.1 and the numerical integration scheme using Runge–Kutta–Fehlberg formula. An important aspect of smooth joint acceleration profiles available from a recursive forward dynamics algorithm, as mentioned in Subsection 9.4.1, is && is not smooth, convergence elaborated here. When the joint acceleration profile for q of its numerical integration to obtain the joint velocity and position, i.e. q& and q , is && profile obtained from a recursive forward slow. Alternatively, with smooth q dynamics algorithm convergence of the numerical integration results is faster. Hence, the overall CPU time required for forward dynamics and numerical integration together using the recursive algorithm may be smaller even for n = 6 compared to its order (n3) algorithm. This aspect is proven in Mohan and Saha (2007) through simulation.

Recursive Robot Dynamics

Example 9.6

233

Simulation of the Three-DOF Manipulator

The three-DOF manipulator shown in Fig. 9.6 is considered here, to let it fall under gravity from the horizontal initial configuration, i.e. q (0) = q& (0) = 0, for i

i

i = 1, 2, 3. Time step, DT, for numerical integration is taken as, DT = 0.01 sec. The results for the joint positions, namely, the variations of q1, q2, and q3 with time are shown in Fig. 9.13, where “th_1”, “th_2”, and “th_3” represent q1, q2, and q3, respectively. Note from Fig. 9.6 that due to gravity the first joint angle, q1, will increase initially in the negative direction. This is evident from Fig. 9.13. Moreover, the system under gravity behaves as a three-link pendulum, which is clear from all the joint angle variations of Fig. 9.13. 3 2

Angle (rad)

1 0 th_2

–1

th_3

–2

th_1

–3 –4

0

0.5

1

1.5 2 Time (sec)

2.5

3

3.5

Fig. 9.13 Joint angles for the three-DOF manipulator

Example 9.7

Simulation of the PUMA Robot

For the PUMA robot whose architecture shown in the Fig. 9.10, the free-fall simulation, i.e., the robot is let free to fall under gravity, is carried out with the initial conditions of q (0) = q& (0) = 0, for i = 1, º, 6. The time step, DT, taken i

i

for the numerical integration is taken as, DT = 0.01 sec. Variations of the joint

234

Introduction to Robotics

What is ‘Free?’

5 4 3 2 1 0 –1 –2

1.2 1 0.8 0.6 0.4 0.2 0 –0.2 –0.4 –0.6 –0.8

3

Angle (rad)

2.5 2 1.5 1 0.5 0

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (sec) (a) Joint 1

Angle (rad)

0.2 0 –0.2 –0.4 –0.6 –0.8 –1 –1.2

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (sec) (c) Joint 3

Angle (rad)

Angle (rad)

Angle (rad)

Angle (rad)

The joints are free from any applied torques.

positions vs. time are shown in the Figs. 9.14(a–f). It is clear from the Fig. 9.10 that due to the length, a3 = 0.02, joint 2 will rotate in the positive direction, which is evident from Fig. 9.14 (b).

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (sec) (e) Joint 5

3 2.5 2 1.5 1 0.5 0 –0.5

0.6 0.4 0.2 0 –0.2 –0.4 –0.6 –0.8 –1 –1.2 –1.4

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (sec) (b) Joint 2

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (sec) (d) Joint 4

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (sec) (f) Joint 6

Fig. 9.14 Joint angles for the PUMA robot

Example 9.8

Simulation of the Stanford Arm

Free-fall simulation of the Stanford arm, Fig. 9.11, is carried out here with the following values: q (0) = 0, for i π 2, 3, q (0) = p, b (0) = 0; q& (0) = 0, for i π 3, i

2

3

i

Angle (rad) Angle (rad)

0 –0.1 –0.2 –0.3 –0.4 –0.5

0 0.5

0 0.5

1

1.5 2 2.5 Time (sec) (a) Joint 1

3 3.5

3 3.5

2 1.5 1 0.5 0 –0.5

0 –0.5 –1 –1.5 –2 –2.5 –3

0 0.5

0 0.5

1

1.5 2 2.5 Time (sec) (b) Joint 2

(e) Joint 5

1 1.5 2 2.5 Time (sec)

3 3.5

3 3.5

70 60 50 40 30 20 10 0

10 8 6 4 2 0 –2

0 0.5

0 0.5

1

1.5 2 2.5 Time (sec) (c) Joint 3

(f) Joint 6

1 1.5 2 2.5 Time (sec)

3 3.5

3 3.5

Recursive Robot Dynamics

Fig. 9.15 Joint angles for the Stanford arm

Displacement (m) Angle (rad)

–0.6 –0.7

5 0 –5 –10 –15 –20 –25 –30 –35 –40 (d) Joint 4

1 1.5 2 2.5 Time (sec)

Angle (rad) Angle (rad)

235

236

Introduction to Robotics

and b&3 (0) = 0. Time step, DT, for numerical integration is taken same as before, i.e., DT = 0.01 sec. The joint position results are then shown in the Figs. 9.15(a– f). Since the initial configuration of the Stanford arm given in Table 9.4(a), the motion of joint 3 should increase sharply once the joint 2 turns more than p/2. This is true, as evident from the Figs. 9.15(c) and (b), respectively. In the Fig. 9.15(b), joint 2 turns by p/2 from its value of p/2 or 1.5708 rad to become zero. This happens a little after 0.5 sec, when the displacement of joint 3 starts picking up, as clear in the Fig. 9.15(c).

SUMMARY In this chapter, dynamic modelling of serial robots using the Decoupled Natural Orthogonal Complements (DeNOC) is presented. Recursive inverse and forward dynamics algorithms are presented for robot control and simulation, respectively. Computational complexities of both the algorithms are reported with illustrative results for planar and spatial robotic systems.

EXERCISES 9.1 What is inverse dynamics? 9.2 Why the concept of the DeNOC matrices is preferable over other dynamic modelling approaches? 9.3 What is the meaning of ‘orthogonal’ in DeNOC? 9.4 Find the expression of kinetic energy of one-link arm, eq. (8.54a), using the DeNOC matrices. 9.5 Derive the equation of motion of one-link arm, Fig. 9.3, using the matrix and vector expressions using the NOC matrix, N, appearing after eq. (9.9a). 9.6 Write the expression of the total kinetic energy for an n-link manipulator using the definition of the generalised twist, t, given in eq. (9.3c). 9.7 Redo Exercise 9.6 using the definition of the generalised joint-rate, q& of eq. (9.6b). 9.8 What are the features of RIDIM?

RIDIM BASED EXERCISES 9.9 Using the RIDIM program find the joint torques for the two-DOF planar manipulator shown in Fig. 9.4.

Recursive Robot Dynamics

237

9.10 Generate joint torques for the three-DOF manipulator of Fig. 9.6 for the joint trajectory given by the eqs. (9.33a–b) with the following inputs: q (0) = q& (0) = q&& (0) = 0; q (T) = p/2, q& (0) = q&& (0) = 0, for i = 1, 2, 3 i

i

i

i

i

i

(9.39)

MATLAB BASED EXERCISES 9.11 Generate the joint torque plots for Example 9.10 using the following explicit expressions: && + C q& = t g + t , where q ∫ [q q q ]T Iq 1 2 3

(9.40a)

where the 3 ¥ 3 GIM and MCI matrices, I and C, respectively, and the 3dimensional vector, t g, due to gravity, are given by i11 =

1 (m a 2 + m2 a 22 + m3 a 23 ) + m2 a 21 + m3 (a 21 + a 22 ) 3 1 1 + (m2 + 2m3)a1 a2 cq2 + m3 a3 (a2 cq 3 + a1 cq23)

i12 = i21 =

FH

IK

1 1 (m a 2 + m3 a 23 ) + m3 a 22 + m a + m3 a2 a1cq2 3 2 2 2 2 2

1 m a (2a cq + a1cq23) 2 3 3 2 3 1 1 = i31 = m3 a 23 + m3 a3 (a2 cq3 + a1cq23) 3 2 1 = (m2 a 22 + m3 a 23 ) + m3 (a 22 + a2 a3 cq3) 3 1 1 1 = i32 = m3 a 23 + m3 a2 a3 cq3; i33 = m3 a 23 3 2 3 1 = – [{m2 a1 a2 sq2 + m3 (2a1 a2 sq2 + a1 a3 sq23)}q& 2 2 + m (a a sq + a a sq )q& ]

+

i13 i22 i23 c11

3

c12

23

2 3

3

3

1 = – [{m2 a1 a2 sq2 + m3 (2a1 a2 sq2 + a1 a3 sq23)}q& 12 2 + m (a a sq + a a sq )q& ] 3

c13 = –

1 3

23

2 3

3

3

(9.40c)

1 m (a a sq + a2 a3sq3)q& 123 2 3 1 3 23

1 [{m2 a1 a2 sq2 + m3 (2a1a2 sq2 + a1 a3 sq23)}q& 1 – m3 a2 a3 sq3q& 3 ] 2 1 1 = – m3 a2 a3 sq3q& 3 ; c23 = – m3 a2 a3 sq3q& 123 2 2

c21 = c22

1 3

(9.40b)

238

Introduction to Robotics

1 1 m [(a a sq + a2 a3 sq3)q& 1 + a2 a3 sq3q& 2]; c32 = m3 a2 a3 sq3q& 12 2 3 1 3 23 2 =0

c31 = c33 and

1 g[–m1 a1 cq1 – 2m2 (a1 cq1 + a2 cq12) – 2m3 (a1 cq1 2 + a2 cq12 + a3 cq123)] 1 t g2 = g[–m2 a2 cq12 – 2m3 (a2 cq12 + a3 cq123); 2 1 t g3 = g[–m3 a3 cq123] (9.40d) 2 where q12 ∫ q1 + q2; q123 ∫ q12 + q3; q& 12 ∫ q& 1 + q& 2 ; q& 123 ∫ q& 12 + q& 3 . Find the joint actuator torques, vector t , for plotting. 9.12 Verify the simulation results of Example 9.11 using the expressions of the eq. (9.40) and ‘ode45’.

t g1 =