ECE5463: Introduction to Robotics Lecture Note 11: Dynamics of a Single Rigid Body Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 1 / 24 Outline Kinetic Energy of a Rigid Body • Rotational Inertia Matrix • Newton Euler Equation • Twist-Wrench Formulation of Rigid-Body Dynamics • Outline Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 2 / 24 Robot Dynamic Model Can be Complicated • Dynamic model of PUMA 560 Arm: sin(q2 q2 93). And when a product of several trigoDometric + + 12 = +I16 * (C223 * C5 - 5’223 * C4 * 5’5) + 121 * SC23 * CC4 operations on the same joint variable appears we write GG2 to +I20 * ((I+ CC4) * SC23 * SS5 - (1 - 2 * SS23) * C4 * SC5) I3 = mean cos(q2) * cos(q2) and GS4 to mean cos(q4) * sin(q4). These +I22 * ((1- 2 * 5’5’23) * C5 - 2 * SC23 * C4 * S5)) final abbreviations, GS4 etc. , are considered to be factorizations; +Ilo* (1- 2 * ss23) + (1 - 2 ss2) ; and the cost of computing these terms is included in the totals N -2.76 * SC2 + 7.44XlO-’ * C223 + 0.60 * SC23 reported above. - 2.13~10-~* (1 - 2 * SS23) . The position of zero joint angles and coordinate frame at- Is = bl13 = 2 {I5 * C2 * C23 + I7 * SC23 - I12 * C2 * 523 tachments to the PUMA arm are shown in Figure 2 above. The I, = +Ils*(2*sc23*c5+(1-2*ss23)*c4*s5) modified Denavit-Hartenberg parameters, assigned according to +I16 * C2 * (C23* C5 - S23* C4 * 5’5) + 121 * SC23 * CC4 the method presented in [Craig 851, are listed in Table AI. +I20 * ((1 + CC4) * 5c23 * 555 - (1- 2 * 5523) * C4 * 5c5) +I22 ((1 - 2 * 5523) * C5 - 2 * 5c23 * C4 * 55)} I7 = +I10 * (1- 2 * SS23) ; Table Al. Modified Denavit - Hartenberg Parameters X 7.44~10~’* C2 * C23 + 0.60 * SC23 + 2.20~10-~* C2 * S23 - 2.13~10-~* (1 - 2 * SS23) . i Qi-1 8; ai- 1 d; bllr = 2*{-115*sc23*54*s5-116*c2*c23*s4*s5 (degrees) (meters) (meters) +I18 * C4 * S5 - 120 * (SS23 * SS5 * SC4 - SC23 * 54 * SC5) -122 * CC23 * S4 * S5 - I21 * SS23 * SC4} ; 1 0 ¶1 0 0 x -2.50~10-~* SC23 * S4 * S5 + 8.60~10-~* C4 * 55 2 -90 92 0 .2435 - 2.48xlO-’ * C2 * C23 * S4 * S5 . ,4318 6115 = 2 * {I20 * (SC5 * (CC4 * (1 - CC23) - CC23) -SC23 * C4 * (1- 2 * SS5)) - 115 (SS23 * S5 - SC23* 6‘4 * C5) *'Ti -116*C2*(S23*S5-C23*C4*C5)+118*S4*C5 +I22 * (CC23 * C4 * C5 - SC23 * S5)} 90 P0 N -2.50~10-~* (SS23 * S5 - SC23 * C4 * C5) i- -. - 2.48~lo-’ * C2 * (S23* 55 - C23 * C4 * C5) + 8.EOxlO-4 * 54 * C5. The equations of the PUMA model constants are presented inTable A2; these const,ants appear in the dynamic equations b116 = 0. of Tables A4 through A7. Z,,;and mi refer to the second mo- blzs = 2*{-Ib*s23+Ils*C23+116*s23*S4*s5 ment of link i about the z axis of frame i and the mass of link +I18 * (C23* C4 * S5 + S23 * C5) + 119 * C23 * SC4 i respectively. The terms a; and di are the Denavit-Barterberg +I20 * S4 * (C23 * C4 * CC5 - 523 * SC5) parameters. Terms of the form rzi are the offsets to the center +Iz2 * G23 * S4 * S5) ; of gravity of link i in the ith coordinate frame. In Table A3 the x 2,67xlO-’ * S23 - 7.58x10-’ * C23 . values of the model constants are listed. The terms I,,,; are the 6124 = -118 * 2 * S23 * 5’4 * S5+ 119 * 523 * (1- (2 * SS4)) motor and drive train contribution to inertia at joint i. +120*S23*(1-2*SS4*CC5)-114*S23; %Ow The equations for the elements of the kinetic energy matrix, Part 11. Gravitional Constants b125 = I1,*C23*S4+Il8*2*(s23*C4*C5+c23*s5) A(q), arepresented in Table A4. A(p) is symmetric, so only 81 = -g*((m~+m4+m5+m~)*az+m2*rz2); +120*s4*(C23*(1-2*SS5)-s23*C4*2*SC5); equations for elementson and above the matrix diagonal are pre- gz = g*(m.s*ry3-(m4+m5+m6)*d4-m4*r,4); X0. sented. g3 = g * m2 * ry2 ; The equations for the elements of the Coriolis matrix, B(q), 6126 = -Izs*(S23*C5+C23*C4*S5); xO. 84 = -g*(m4+m5+rn6)*as; are presented in Table A5. The Coriolis terms have been left in 613, = 6124 - 6135 = blzs * 6156 = b126 * the form of a three dimensional array, with a convention for the g.5 = -g * m.5 * rZ6; bl45 = 2 * {I15 * S23 * C4 * C5 + 116 * C2 c C4 * C5 indices that matches that of the Cristoffel symbols. Element +Ils*c23*s4*c5+122*C23*c4*C5}+I17*s23*C4 Table AS. Computed Values for the Constants Appearing multiples qk and ql to give a contribution to the torque at joint i. -I*o * (s23* c4 * (I - 2 * SS5) + 2 * C23 :: SC5) ; in the Equations of Forces of Motion. b424 = 0 . The Coriolis matrix may also be written as a 6 x 15 array, where XO. the 15 columnscorrespond to the 15 possiblecombinations of (Inertial constants have units of kilogram meters-squared) 6146 = I23 * 5’23 * S4 * 55 ; N 0 . joint velocities. The equations for the elements of the centrifugal 1.43 f 0.05 matrix, C(q),are presented in Table A6. And the equations for 1.38 f 0.05 b156 = -123 * (6’23 * 5’5 + ,523 * C4 * c5) ; X 0. the terms of the gravity vector, g(p), are presented in Table A7. 3.72~10-I f 0.31~10-~ b212 = 0 . b2ls 0. A load can be represented in this model by attaching it to 2.98xlO-' f 0.29~10-~ the gfh link. In the model the 6'h link is assumed to havea 2.38~lO-~f 1.20~10-~ bzlr = Ilt * S23 + 119 * S23* (1- (2* SS4)) +2*{-Il5*C23*C4*S5+116*S2*C4*S5 center of gravity on the axis of rotation, and tohave Zzze = &e; -1.42~10-~f 0.70~10~~ +I20 * (523 * (CC5 * CC4 - 0.5) + C23 * C4 * SC5) these rest,rictions extend to a load represented by changing the -3.79~10-' f 0.90~10-~ 1.25~10-~f 0.30~10-~ +I22 * S23 * G4 t S5) ; 6'h link parameters. A moregeneral, though computationclly 6.42xlO-' f 3.00~10-~ X 1.64X10-s * 5’23 - 2.50~10-~* C23 * C4 * S5 + more expensive, method of incorporating a load in the dynamic 3.00~10-~f 14.0~10-~ 2.48~10-’* S2 C4 * S5 + 0.30~10-’ * 523 * (1 - (2* ss4)). calculation is presented in [Izaguirre and Paul 19851. -I.OOX~O-~ f 6.00~10-~ bzla =2*{-115*C23*S4*C5+122*S23*S4+C5 ~.OOXIO-J+ 2.00~10-5 Table A2. Expressions for the Constants Appearing in the +116*S2*S4*C5}-I17*C23*s4 Equations of Forces of Motion. +rzo~(C23~s4~(1-2~ss5)-2~s23~sC4~SC5); X -2.50~10-’ * C23 * S4 * C5 + 2.48~10-~* 52 * 54 * c5 - 6.42x10-’ * C23 * 54 . Part I. Inertial Constants I1 = IZzl+ ml * rYl2+ mz * d2' + (m4 + m5 + m,) * (Gravitationalconstants have units of newton meters) b216 = -6126 * = gz +mz * rzz2 + (ms + m4 + m5 + m6) * (d2+ dS)' gl -37.2 f 00.5 = -8.44 f 0.20 b223 =2*{-112*S3+I~*C3+116*(C3*C5-53*C4*S5))j = 1.02 0.50 = 2.49~10-' 0.25XIO-' +IZZZ+ Iyys + 2 * m2 * d2 * rz2 + m2 * rYz2+ mS * rrlz gs f g* f a 2.20~10-~* S3 + 7.44x10-’ * C3 . +2 * ms * (d2 + d3) * rzS+ I,,, + Iyy5+ Irz6 ; gs = -2.82~10-' f 0.56X10-2 517 516 518 Kinetic Energy Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 3 / 24 Kinetic Energy • Consider a point mass ¯m with s -frame coordinate p(t), its kinetic energy is given by f g 1 = ¯m p_ 2 K 2 k k • Note: m denotes moment (vector) and ¯m denotes mass (scalar). • Question: given a moving rigid body with configuration T (t) = (R(t); p(t)), how to compute its kinetic energy? - Rigid body consists of infinitely many \particles" with different fsg-frame velocities 1 X 2 K ≈ ¯mikp_ik 2 i - Velocities of particles p_i are caused by the rigid body velocity (twist) - The overall kinetic energy should depend on the rigid body velocity as well as the geometry and mass distribution of the body Kinetic Energy Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 4 / 24 Recall: Rigid Body Velocity Given rigid body T (t) = (R(t); p(t)): • Spatial twist: • Body twist: Kinetic Energy Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 24 Recall: Rigid Body Velocity • Consider a particle i on the body with b -frame coordinate r and s -frame f g i f g coordinate pi - Velocity of particle i: - Acceleration of particle i: - Velocity of the origin of fbg: Kinetic Energy Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 6 / 24 Rigid Body Kinetic Energy • Kinetic Energy: Given a rigid body T (t) = (R(t); p(t)) with body twist b = (!b; vb). Suppose the b -frame origin coincides with the center of massV of the body. Then itsf kineticg energy is given by: 1 1 = ¯m v 2 + !T ! ; with = ρ(r)[r]T [r]dV K 2 k bk 2 b Ib b Ib ZB where is the rotational inertia matrix in body frame Ib Derivation: Divide the body into small point masses, where point i has mass ¯mi, fbg-frame coordinate ri, and fsg-frame coordinate pi Kinetic Energy Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 7 / 24 Derivation of Kinetic Energy (Continued) • Kinetic Energy Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 8 / 24 Outline Kinetic Energy of a Rigid Body • Rotational Inertia Matrix • Newton Euler Equation • Twist-Wrench Formulation of Rigid-Body Dynamics • Rotational Inertia Matrix Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 24 Rotational Inertia Matrix in Body Frame ρ(r)[r]T [r]dV Ib , ZB • Individual entries of : Ib 2 3 Ixx Ixy Ixz Ib = 4 Iyx Iyy Iyz 5 Izx Izy Izz where Z Z 2 2 2 2 Ixx = (y + z )ρ(x; y; z)dV; Iyy = (x + z )ρ(x; y; z)dV B B Z Z 2 2 Izz = (x + y )ρ(x; y; z)dV; Ixy = Iyx = − xyρ(x; y; z)dV B B Z Z Ixz = Izx = − xzρ(x; y; z)dV Iyz = Izy = − yzρ(x; y; z)dV B B • If the body has a uniform density, then b is determined exclusively by the shape of the rigid body I Rotational Inertia Matrix Lecture 11 (ECE5463 Sp18) Wei Zhang(OSU) 10 / 24 Principal Axes of Inertia Let v1; v2; v3 and λ1; λ2; λ3 be the eigenvectors and eigenvalues of b, respectively.