Artificial Intelligence & Neuro Cognitive Systems Fakultät für Informatik. Robot Dynamics. Dr.-Ing. John Nassour J.

Transcription

1 Artificial Intelligence & Neuro Cognitive Systems Fakultät für Informatik Robot Dynamics Dr.-Ing. John Nassour J.Nassour 1

2 Introduction Dynamics concerns the motion of bodies Includes Kinematics study of motion without reference to the force that cause it Kinetics relates these forces to the motion Dynamic behaviour of a robot: rate of change of arm configuration in relation to the torques exerted by the actuators J.Nassour 2

3 Forward vs. Inverse Forward dynamics Given vector of joint torques, work out the resulting manipulator motion. τ t = τ 1 τ n q(t) = Inverse Dynamics Given a vector of manipulator positions, velocities and acceleration. Find the required vector of joint torques. q t, q t, q(t) τ t q 1 q n J.Nassour 3

4 Torques The actuator has to balance torques from 4 different sources: Dynamic torques (caused by the motion) Inertial (promotional to joint acceleration, according to Newton s law) Centripetal (promotional to square of joint velocity, direction toward the centre of circular motion) Coriolis (vertical forces, interaction of two rotating links) Static torques (caused by friction) Gravity torques (caused by gravity) External torques (exerted on the end effector, caused by the task) J.Nassour 4

5 Centripetal Force A centripetal force is a force that makes a body follow a curved path. Its direction is always orthogonal to the velocity of the body and towards the fixed point of the instantaneous center of curvature of the path J.Nassour 5

6 Centrifugal Force J.Nassour 6

7 The Coriolis Effect J.Nassour 7

8 Manipulator Dynamics Mathematical equations describing the dynamic behaviour of the manipulator. Why needed? Simulation of the manipulator Design a suitable controller Evaluate the robot structure Joint torques Robot motion, i.e. acceleration, velocity, position J.Nassour 8

9 Approaches to Dynamic Modeling Newton-Euler Formulation Balance of forces/torques Dynamic equations in numeric/recursive form. Lagrange Formulation Energy-Based approach Dynamic equations in symbolic/closed form Other Formulations J.Nassour 9

10 Joint Space Dynamics M q q + V q, q + G q = Γ q: Generalized joint coordinates M q : Mass Matrix Kinetic Energy Matrix V q, q : Centrifugal and Coriolis forces G q : Gravity force Γ: Generalized forces J.Nassour 1

11 Newton-Euler Formulation In static equilibrium F i and N i are equal to. Newton: Linear motion m i v ci = F i Euler: Angular Moment = inertia. Acc. + Centrifugal and Coriolis forces. N i = I Ci ω i + ω i I Ci ω i By projecting on each axis we eliminate the internal forces acting on the links, we find therefor the torques applied on each joint axis. N i F i -n i-1 m i I Ci -n i+1 -f i+1 τ i = n i T. z i f i T. z i Revolute Prismatic -f i-1 Link i J.Nassour 11

12 Lagrange Formulation Link 2 Joint 3 Joint n-1 Link 1 Joint 2 Link n-1 Joint n Joint 1 Kinetic Energy of each link K i Potential energy P i The kinetic energy of the robot: Base K = i K i K = 1 2 qt. M. q By computing M, we can use it in: M q + V + G = τ J.Nassour 12

13 Lagrange Formulation d dt L q i L q i = τ i Lagrange function is defined L = K P K: Total kinetic energy of the robot. P: Total potential energy of the robot. q i : Joint variable of i-th joint. q i : first time derivative of q i τ i : Generalized force (torque) at i-th joint J.Nassour 13

14 Center of Mass COM is the point on a body that moves in the same way that a single particle subject to the same external force will move. i r cm = 1 M r. dm = 1 M 1 m i r i r cm is the location of the centre of mass. M is the total mass of the object r is the location of the reference frame. dm is the differential element of mass at point r J.Nassour 14

15 Inertia The tendency of a body to remain in a state of rest or uniform motion. Inertial frame: the frame in which this state is measured (a frame at rest or moving with constant velocity). Non-Inertial frame: a frame that is accelerating with respect to the inertial frame J.Nassour 15

16 Moment of Inertia Moment of inertia is the rotational inertia of a body with respect to the axis of rotation. Moment of inertia depends on the axis and on the manner in which the mass is distributed. It is equal to the sum of the product between mass of particles and the square of their distance to the axis of rotation. I = i 1 m i r i 2 Where m i is the mass of particle i, and r i is the distance to the particle i J.Nassour 16

17 Moment of Inertia For a rigid body with a continuous distribution of mass, summation becomes integral over the whole body: I = r 2 dm Example: Inertia of a cylinder (length l, mass M): Consider the cylinder made up of an infinite number of cylinders thickness dr, at radius r, of mass dm, and volume dv. dm = ρ. dv = 2πlρrdr I = r 2 dm = 2πl ρr 3 dr = 2πlρ r 2 4 r = M r r J.Nassour 17

18 Inertia Tensor A rigid body, free to move in space, has an infinite number of possible rotation axis. The inertia matrix ( Inertia tensor) is the integral over the volume of all the vectors r locating all the points dv on the rigid body and scaled by the density of all the masses dm of the rigid body. For a rigid body rotating about a fixed point, which is not the centre of the mass, the inertia tensor is: I = I xx I xy I xz I yx I yy I yz I zx I zy I zz J.Nassour 18

19 Inertia Tensor I = I xx I xy I xz I yx I yy I yz I zx I zy I zz Mass moments of inertia (diagonal terms): Mass products of inertia (offdiagonal terms): I xx = y 2 + z 2 ρ dv I xy = xy ρ dv I yy = x 2 + z 2 ρ dv I xz = xz ρ dv I zz = x 2 + y 2 ρ dv I yz = yz ρ dv J.Nassour 19

20 Example Find the inertia tensor of a rectangle rotating about a fixed point A. I xx = y 2 + z 2 ρ dv I xx = I xx = I xx = h l h l h[ y3 w y 2 + z 2 ρ dxdydz y 2 + z 2 w ρ dydz 3 + z2 y ] l w ρ dz I xx = l 3 z 3 + z3 l 3 h w ρ I xx = h l z2 l w ρ dz I xx = l3 h 3 + h3 l 3 w ρ J.Nassour 2

21 Example Find the inertia tensor of a rectangle rotating about a fixed point A. I xx = l3 h 3 + h3 l 3 w ρ Since the mass of the rectangle: m = wlh ρ I xx = m 3 l2 + h J.Nassour 21

22 Example Find the inertia tensor of a rectangle rotating about a fixed point A J.Nassour 22

23 Translation of Inertia Tensor Parallel axis theorem. Moments of inertia: A Izz = CM I zz + m(r x 2 + r y 2 ) Product of inertia: A Ixy = CM I xy + m(r x r y ) J.Nassour 23

24 Example Find the inertia tensor of a rectangle rotating about the COM J.Nassour 24

25 Example Find the inertia tensor of a rectangle rotating about the COM J.Nassour 25

26 Example Find the inertia tensor of a rectangle rotating about the COM J.Nassour 26

27 Example Find the inertia tensor of a rectangle rotating about the COM J.Nassour 27

28 Example Find the inertia tensor of a rectangle rotating about the COM J.Nassour 28

29 Example Moving the axes of rotation to the centre of mass results in a diagonal inertia tensor J.Nassour 29

30 Lagrange Formulation d dt L q L q = τ Lagrange function is defined L = K P K: Total kinetic energy of the robot. P: Total potential energy of the robot. q : Joint variable. q : first time derivative of q τ : Generalized force (torque) J.Nassour 3

31 Lagrange Formulation d dt L q L q = τ Lagrange function is defined L = K P Since P = P q d dt K q i K q + P q = τ Inertial forces Gravity vector (gradient of potential energy) J.Nassour 31

32 Lagrange Formulation d dt K q i K q = τ G q, P G q = q Inertial forces This equation can be written in the following from: Joint Space Dynamics M q q + V q, q + G q = Γ M q q + V q, q = Γ G q J.Nassour 32

33 Lagrange Formulation d dt K q i K q = τ G K = q i d K dt q i K = 1 2 qt M(q) q q i 1 2 qt M q q = M(q) q = d dt M q = M q + M q d dt K q i K q = M q + M q 1 2 M qt q q 1 M qt qn q = M q + V(q, q) J.Nassour 33

34 Lagrange Formulation d dt K q i K q = M q + If q =, V q, q = M q 1 2 If M = CONSTANT,, V q, q = M qt q q 1 M qt qn q = M q + V(q, q) Centrifugal and Coriolis forces We need to compute M from the kinetic energy equations then we have the dynamics of the robot J.Nassour 34

35 Dynamic Equations d dt K q i K q = M q + M q 1 2 M qt q q 1 M qt qn q = M q + V(q, q) M q q + V q, q + G q = τ K = 1 2 qt M q q M q M q V q, q J.Nassour 35

36 Dynamic Equations Total kinetic energy K = K Link i = 1 2 qt M q ω i v Ci Link 2 Joint 3 Joint i Link i Link 1 Joint 2 Joint i+1 Joint 1 Base J.Nassour 36

37 Kinetic Energy Work done by external forces to bring the system from rest to its current state. v K = 1 2 mv2 m F ω I C is a matrix ω is a vector K = 1 2 ωt I C ω I C τ J.Nassour 37

38 Dynamic Equations The kinetic energy for link i: K i = 1 2 (m iv T Ci v Ci + ω T i I Ci ω i ) ω i v Ci Total kinetic energy: n Link 2 Joint 3 Joint i Link i K = i=1 K i Link 1 Joint 2 Joint i+1 Joint 1 Base J.Nassour 38

39 Dynamic Equations Kinetic energy in quadratic form of generalized velocities: K = 1 2 qt M q Then we can write: 1 2 qt M n q = 1 2 i=1 (m i v T Ci v Ci + ω T i I Ci ω i ) How extract the mass matrix M? J.Nassour 39

40 Dynamic Equations Kinetic energy in quadratic form of generalized velocities: K = 1 2 qt M q Then we can write: 1 2 qt M n q = 1 2 i=1 (m i v T Ci v Ci + ω T i I Ci ω i ) How extract the mass matrix M? v Ci = J vi q, ω i = J ωi q J.Nassour 4

41 Dynamic Equations v Ci = J vi q ω i = J ωi q ω i v Ci Link 2 Joint 3 Joint i Link i Link 1 Joint 2 p Ci Joint i+1 Joint 1 Base J.Nassour 41

42 Dynamic Equations 1 2 qt M n q = 1 2 i=1 (m i v T Ci v Ci + ω T i I Ci ω i ) 1 2 qt M v Ci = J vi q, ω i = J ωi q n q = 1 2 i=1 1 2 qt M q = 1 2 qt [ (m i q T J T vi J vi q + n i=1 q T J T ωi I Ci J ωi q) (m i J T vi J vi + J T ωi I Ci J ωi )] q J.Nassour 42

43 Dynamic Equations M = n i=1 (m i J vi T J vi + J ωi T I Ci J ωi ) The mass matrix is the sum of Jacobians transpose Jacobians scaled by the mass properties (m i, I Ci ). If the robot has 1 DOF, M = With multiple links, the Jacobian matrices of all links contribute in the mass matrix. Each link has an impact on the total mass matrix J.Nassour 43

44 M = n i=1 Dynamic Equations (m i J vi T J vi + J ωi T I Ci J ωi ) J vi = p C i q 1 p Ci q 2 p C i qi ω i v Ci Link 2 Joint 3 Joint i Link i Link 1 Joint 2 p Ci Joint i+1 Joint 1 Base J.Nassour 44

45 M = n i=1 Dynamic Equations (m i J vi T J vi + J ωi T I Ci J ωi ) J ωi = ρ 1 z ρ 2 z 1 ρ i z i 1 ω i v Ci Link 2 Joint 3 Joint i Link i Link 1 Joint 2 p Ci Joint i+1 Joint 1 Base J.Nassour 45

46 Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn m 11 represents the inertia of the arm perceived at joint 1. m 11 = f q 2:n m 22 = f q 3:n m nn = constant J.Nassour 46

47 Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn m 12 represents the coupling between the acceleration of joint 2 on joint J.Nassour 47

48 Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn J.Nassour 48

49 Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn J.Nassour 49

50 Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn J.Nassour 5

51 Work out the mass matrix M. Example RP l 1 y I c2 m 2 m 1 I c1 x θ 1 d J.Nassour 51

52 Work out the mass matrix M. Example RP M = n (m i J vi T J vi + J ωi T I Ci J ωi ) l 1 y I c2 m 2 i=1 m 1 J vi = p C i q 1 p Ci q 2 p C i qi θ 1 I c1 x d 2 J ωi = ρ 1 z ρ 2 z 1 ρ i z i 1 I = I xx I xy I xz I yx I yy I yz I zx I zy I zz J.Nassour 52

53 Work out the mass matrix M. n M = (m i J T vi J vi + J T ωi I Ci J ωi ) i=1 Example RP M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 In frame (o,x,y,z ) p C1 = J v1 = l 1 c 1 l 1 s 1, p C2 = l 1 s 1 l 1 c 1 d 2 c 1 d 2 s 1 l 1 y m 1 I c2 m 2 J v2 = d 2 s 1 c 1 d 2 c 1 s 1 θ 1 I c1 x d J.Nassour 53

54 Example RP Work out the mass matrix M. M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 J v1 = l 1 s 1 l 1 c 1 J v2 = d 2 s 1 c 1 d 2 c 1 s 1 m 1 J v1 T J v1 = m 1l 1 2 l 1 y I c2 m 2 m 2 J v2 T J v2 = m 2d 2 2 m 2 I c1 m 1 x θ 1 d J.Nassour 54

55 Example RP Work out the mass matrix M. M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 J ωi = ρ 1 z ρ 2 z 1 ρ i z i 1 J ω1 = 1 J ω2 = 1 l 1 y I c2 m 2 J ω1 T I C1 J ω1 = I zz1 I c1 m 1 x J ω2 T I C2 J ω2 = I zz J.Nassour 55 θ 1 d 2

56 Example RP Work out the mass matrix M. M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 2 M = m 2 1l 1 + m 2d 2 + I zz1 m 2 + I zz2 M = m 1l I zz1 + m 2 d I zz2 m 2 l 1 y I c2 m 2 m 1 I c1 x θ 1 d J.Nassour 56

57 Dynamic Equations d dt K q i K q = M q + M q 1 2 M qt q q 1 M qt qn q = M q + V(q, q) M q q + V q, q + G q = τ K = 1 2 qt M q q M q M q V q, q J.Nassour 57

58 Centrifugal and Coriolis Forces V q, q = M q 1 2 M qt q q 1 M qt qn q M = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn M q q + V q, q + G q = τ J.Nassour 58

59 Centrifugal and Coriolis Forces V q, q = M q 1 2 M qt q q 1 M qt qn q For robot with 2 DOF M q q + V q, q + G q = τ m 11 m 12 m 12 m 22 q 1 q 2 + v 1 v 2 + g 1 g 2 = τ 1 τ J.Nassour 59

60 Centrifugal and Coriolis Forces V q, q = M q 1 2 V q, q = m 11 m 12 M qt q q 1 M qt qn q m 12 m 22 q 1 2 q T m 111 m 121 m 121 m 221 q q T m 112 m 122 m 122 m 222 q m ijk = m ij qk m ij = m ij1 q 1 + m ij2 q m ijk J.Nassour 6 q k

61 Centrifugal and Coriolis Forces V q, q = m 11 m 12 m 12 m 22 q 1 2 q T m 111 m 121 m 121 m 221 q q T m 112 m 122 m 122 m 222 q V q, q = 1 2 (m m 111 m 111 ) 1 2 (m m 211 m 112 ) 1 2 (m m 122 m 221 ) 1 2 (m m 222 m 222 ) q 1 2 q m m 121 m 121 m m 221 m 122 q 1 q J.Nassour 61

62 Centrifugal and Coriolis Forces V q, q = 1 2 (m m 111 m 111 ) 1 2 (m m 211 m 112 ) 1 2 (m m 122 m 221 ) 1 2 (m m 222 m 222 ) q 1 2 q m m 121 m 121 m m 221 m q 1 q Christoffel Symbols: b ijk = 1 2 (m ijk + m ikj m jki ) b 122 V q, q = b 111 q 1 b 211 b q b 112 2b 212 q 1 q 2 Terms of the type q 2 i are called centrifugal. Terms of the type q k are called coriolis. q i C q B q J.Nassour 62

63 Example RP Work out the Centrifugal and Coriolis Vector V. l 1 y I c2 m 2 m 1 I c1 x θ 1 d J.Nassour 63

64 Example RP Work out the Centrifugal and Coriolis Vector V. C q b 122 V q, q = b 111 q 1 b 211 b q 2 2 B q + 2b 112 2b 212 q 1 q 2 l 1 y I c2 m 2 b ijk = 1 2 (m ijk + m ikj m jki ) m ijk = m ij q k θ 1 I c1 m 1 x d 2 B = 2b 112 C = b 122 b 211 = 2m 2d 2 = m 2 d 2 M = m 1l I zz1 + m 2 d I zz2 m 2 V = 2m 2d 2 θ 1 d 2 + m 2 d 2 θ 1 2 d J.Nassour 64

65 Dynamic Equations M q q + V q, q + G q = τ Gravity vector? J.Nassour 65

66 Gravity vector Link i G q = P q p Ci h i g Potential energy: P i = m i g h i + P G k q = P q k = n i (m i g T p c i q k ) P i = m i g T. p ci P = P i G = J v1 T J v2 T J vn T m 1 g m 2 g m n g i J.Nassour 66

67 Gravity vector Link 2 Joint i Link i Link 1 m 2 g m i g m 1 g Base G = J v1 T m n g J v2 T J vn T m 1 g m 2 g m n g J.Nassour 67

68 Work out the gravity vector G. Example RP l 1 y I c2 m 2 g m 1 I c1 x θ 1 d J.Nassour 68

69 Example RP Work out the gravity vector G. G = J v1 T J v2 T G = J v1 T m 1 g J v2 T m 2 g J vn T m 1 g m 2 g m n g l 1 y g m 1 I c2 m 2 In frame (o,x,y,z ): The gravity vector is: g = g θ 1 I c1 x d 2 G = l 1 s 1 l 1 c 1 T m 1 g d 2 s 1 c 1 d 2 c 1 s 1 T m 2 g = m 1l 1 + m 2 d 2 gc 1 m 2 gs J.Nassour 69

70 Work out the equation of motion. Example RP l 1 y I c2 m 2 g m 1 I c1 x θ 1 d J.Nassour 7

71 Example RP Work out the equation of motion. g l 1 y I c2 m 2 m 1 l I zz1 + m 2 d I zz2 θ 1 m 2 d 2 m 1 + 2m 2d 2 θ 1 d 2 + m 2 d 2 θ 1 2 d 2 2 θ 1 I c1 x d 2 + m 1l 1 + m 2 d 2 gc 1 m 2 gs 1 = τ 1 f 2 m 1 l I zz1 + m 2 d I zz2 θ 1 + 2m 2 d 2 θ 1 d 2 + m 1 l 1 + m 2 d 2 gc 1 = τ 1 m 2 d 2 m 2 d 2 θ m 2 gs 1 = f J.Nassour 71

72 Example PR Work out the mass matrix M. n M = (m i J T vi J vi + J T ωi I Ci J ωi ) i=1 M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 d In frame (o,x,y,z ) q 1 l p C1 =, p C2 = 1 J v1 = 1 ds 2 J v2 = dc 2 q 1 + dc 2 ds 2 q J.Nassour 72 y m 1 I c1 l I c2 m 2 q 2 x

73 Example PR Work out the mass matrix M. n M = (m i J T vi J vi + J T ωi I Ci J ωi ) i=1 M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 d In frame (o,x,y,z ) q 1 l p C1 =, p C2 = J ω1 = J ω2 = 1 q 1 + dc 2 ds 2 q J.Nassour 73 y m 1 I c1 l I c2 m 2 q 2 x

74 Example PR Work out the mass matrix M. M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 d M = m 1 + m 2 m 2 ds 2 m 2 ds 2 I zz2 + m 2 d 2 y m 2 I c2 q 2 m 1 I c1 x l q J.Nassour 74

75 Example PR d Work out the gravity vector G. G = J v1 T J v2 T G = J v1 T m 1 g J v2 T m 2 g J vn T m 1 g m 2 g m n g y m 1 I c1 I c2 m 2 q 2 In frame (o,x,y,z ): The gravity vector is: g = g l x q 1 G = 1 T m 1 g 1 ds 2 dc 2 T m 2 g = dc 2 m 2 g J.Nassour 75

76 Example PR d Work out the Centrifugal and Coriolis Vector V. b 122 V q, q = b 111 q 1 b 211 b q 2 2 b ijk = 1 2 (m ijk + m ikj m jki ) + 2b 112 2b 212 y q 1 q 2 m 1 I c1 I c2 m 2 q 2 m ijk = m ij q k l x B = C = V = m 2dc 2 q 1 q 2 + m 2dc 2 q 1 2 q 2 2 q 1 M = m 1 + m 2 m 2 ds 2 m 2 ds 2 I zz2 + m 2 d J.Nassour 76

77 Example PR d Work out the equation of motion. y m 2 m 1 + m 2 m 2 ds 2 q 1 m 2 ds 2 I zz2 + m 2 d 2 q 2 m 1 I c1 I c2 q 2 + q 1 q 2 + m 2dc 2 q 1 2 q 2 2 l x + dc 2 m 2 g = f 1 τ2 q 1 m 1 + m 2 q 1 m 2 ds 2 q 2 m 2 dc 2 q 2 2 = f 1 m 2 ds 2 q 1 + I zz2 + m 2 d 2 q 2 + dc 2 m 2 g = τ J.Nassour 77

78 Work out the equation of motion. Example PP M = m 1 + m 2 m 2 m 1 + m 2 q 1 + g m 1 + m 2 = f 1 m 2 q 2 = f J.Nassour 78

79 Example RR g Work out the equation of motion. y l 2 m 2 l 1 l c2 I c2 q 2 l c1 I c1 m 1 q 1 x M q q + V q, q + G q = τ J.Nassour 79

80 Example RR g Work out the mass matrix M. n M = (m i J T vi J vi + J T ωi I Ci J ωi ) y l 1 l c2 l 2 m 2 i=1 M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + l c1 I c2 q 2 m 2 J T v2 J v2 + J T ω2 I C2 J ω2 In frame (o,x,y,z ) m 1 I c1 q 1 p C1 = J v1 = l c1 c 1 l c1 s 1, p C2 = l c1 s 1 l c1 c 1 l c2 c 12 + l 1 c 1 l c2 s 12 + l 1 s 1 x J v2 = l c2 s 12 l 1 s 1 l c2 s 12 l c2 c 12 + l 1 c 1 l c2 c J.Nassour 8

81 Example RR g Work out the mass matrix M. n M = (m i J T vi J vi + J T ωi I Ci J ωi ) y l 1 l c2 l 2 m 2 i=1 M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + l c1 I c2 q 2 m 2 J T v2 J v2 + J T ω2 I C2 J ω2 In frame (o,x,y,z ) m 1 I c1 q 1 p C1 = J ω1 = J ω2 = l c1 c 1 l c1 s , p C2 = l c2 c 12 + l 1 c 1 l c2 s 12 + l 1 s 1 x J.Nassour 81

82 Example RR g Work out the mass matrix M. n M = (m i J T vi J vi + J T ωi I Ci J ωi ) y l 1 l c2 l 2 m 2 i=1 M = m 1 J v1 T J v1 + J ω1 T I C1 J ω1 + m 2 J v2 T J v2 + J ω2 T I C2 J ω2 l c1 I c2 q 2 I c1 m 1 q 1 x M = m 2l m 2 l 1 l c2 c 2 + m 1 l 2 c1 + m 2 l 2 c2 + I zz1 m 2 I 2 c2 + l 1 m 2 I c2 c 2 + I zz2 m 2 I c2 2 + l 1 m 2 l c2 c 2 + I zz2 m 2 I c2 2 + I zz J.Nassour 82

83 Example RR g Work out the gravity vector G. G = J v1 T J v2 T G = J v1 T m 1 g J v2 T m 2 g J vn T m 1 g m 2 g m n g y l c1 l 1 I c1 l c2 l 2 I c2 m 2 q 2 In frame (o,x,y,z ): The gravity vector is: g = g m 1 q 1 x G = l c1 s 1 l c1 c 1 T m 1 g l c2 s 12 l 1 s 1 l c2 s 12 l c2 c 12 + l 1 c 1 l c2 c 12 T m 2 g J.Nassour 83

84 Example RR g Work out the gravity vector G. G = J v1 T J v2 T G = J v1 T m 1 g J v2 T m 2 g J vn T m 1 g m 2 g m n g y l c1 l 1 I c1 l c2 l 2 I c2 m 2 q 2 In frame (o,x,y,z ): The gravity vector is: g = g m 1 q 1 x G = gm 2 l c2 c 12 + l 1 c 1 + gm 1 l c1 c 1 gm 2 l c2 c J.Nassour 84

85 Example RR Work out the Centrifugal and Coriolis Vector V. b 122 V q, q = b 111 q 1 b 211 b q b 112 2b 212 q 1 q 2 g l 2 b ijk = 1 2 (m ijk + m ikj m jki ) y m 2 m ijk = m ij q k l 1 l c2 I c2 q 2 l c1 B = 2l 1l c2 m 2 s 2 l 1 l c2 m 2 s 2 C = l 1 l c2 m 2 s 2 m 1 I c1 q 1 x V q, q = 2l 1l c2 m 2 s 2 q 1 q 2 + l 1 l c2 m 2 s 2 l 1 l c2 m 2 s 2 q 1 2 q J.Nassour 85

86 Work out the equation of motion. Example RR m 2 l m 2 l 1 l c2 c 2 + m 1 l 2 c1 + m 2 l 2 c2 + I zz1 m 2 I 2 c2 + l 1 m 2 l c2 c 2 + I zz2 q 1 m 2 I 2 c2 + l 1 m 2 I c2 c 2 + I zz2 m 2 I 2 c2 + I zz2 q 2 + 2l 1l c2 m 2 s 2 q 1 q 2 + l 1 l c2 m 2 s 2 l 1 l c2 m 2 s 2 q 1 2 q 2 2 g + gm 2 l c2 c 12 + l 1 c 1 + gm 1 l c1 c 1 gm 2 l c2 c 12 = τ 1 τ 2 l 2 y m 2 l 1 l c2 I c2 q 2 l c1 q J.Nassour 86 x m 1 I c1

87 Work out the equation of motion. Example RR m 2 l m 2 l 1 l c2 c 2 + m 1 l 2 c1 + m 2 l 2 c2 + I zz1 m 2 I 2 c2 + l 1 m 2 l c2 c 2 + I zz2 q 1 m 2 I 2 c2 + l 1 m 2 I c2 c 2 + I zz2 m 2 I 2 c2 + I zz2 q 2 + 2l 1l c2 m 2 s 2 q 1 q 2 + l 1 l c2 m 2 s 2 l 1 l c2 m 2 s 2 q 1 2 q 2 2 g + gm 2 l c2 c 12 + l 1 c 1 + gm 1 l c1 c 1 gm 2 l c2 c 12 = τ 1 τ 2 l 2 The centrifugal terms on one joint are proportional to the square of the velocity of the other joint, and are at a maximum when the two links are perpendicular. y l c1 l 1 I c1 l c2 I c2 m 2 q J.Nassour 87 x m 1 q 1

88 Work out the equation of motion. Example RR m 2 l m 2 l 1 l c2 c 2 + m 1 l 2 c1 + m 2 l 2 c2 + I zz1 m 2 I 2 c2 + l 1 m 2 l c2 c 2 + I zz2 q 1 m 2 I 2 c2 + l 1 m 2 I c2 c 2 + I zz2 m 2 I 2 c2 + I zz2 q 2 + 2l 1l c2 m 2 s 2 q 1 q 2 + l 1 l c2 m 2 s 2 l 1 l c2 m 2 s 2 q 1 2 q 2 2 g + gm 2 l c2 c 12 + l 1 c 1 + gm 1 l c1 c 1 gm 2 l c2 c 12 = τ 1 τ 2 l 2 The Coriolis term is proportional to the product of the two joint velocities, and is also at a maximum when the two links are perpendicular. y l c1 l 1 I c1 l c2 I c2 m 2 q J.Nassour 88 x m 1 q 1