Artificial Intelligence & Neuro Cognitive Systems Fakultät für Informatik Robot Dynamics Dr.-Ing. John Nassour 25.1.218 J.Nassour 1
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. 25.1.218 J.Nassour 2
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 25.1.218 J.Nassour 3
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) 25.1.218 J.Nassour 4
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. 25.1.218 J.Nassour 5
Centrifugal Force 25.1.218 J.Nassour 6
The Coriolis Effect 25.1.218 J.Nassour 7
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 25.1.218 J.Nassour 8
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 25.1.218 J.Nassour 9
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 25.1.218 J.Nassour 1
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 25.1.218 J.Nassour 11
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 = τ 25.1.218 J.Nassour 12
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. 25.1.218 J.Nassour 13
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. 25.1.218 J.Nassour 14
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. 25.1.218 J.Nassour 15
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. 25.1.218 J.Nassour 16
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 1 4 4 = M r 2 2 + r 1 2 4 25.1.218 J.Nassour 17
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 25.1.218 J.Nassour 18
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 25.1.218 J.Nassour 19
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 3 3 + z2 l w ρ dz I xx = l3 h 3 + h3 l 3 w ρ 25.1.218 J.Nassour 2
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 2 25.1.218 J.Nassour 21
Example Find the inertia tensor of a rectangle rotating about a fixed point A. 25.1.218 J.Nassour 22
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 ) 25.1.218 J.Nassour 23
Example Find the inertia tensor of a rectangle rotating about the COM. 25.1.218 J.Nassour 24
Example Find the inertia tensor of a rectangle rotating about the COM. 25.1.218 J.Nassour 25
Example Find the inertia tensor of a rectangle rotating about the COM. 25.1.218 J.Nassour 26
Example Find the inertia tensor of a rectangle rotating about the COM. 25.1.218 J.Nassour 27
Example Find the inertia tensor of a rectangle rotating about the COM. 25.1.218 J.Nassour 28
Example Moving the axes of rotation to the centre of mass results in a diagonal inertia tensor. 25.1.218 J.Nassour 29
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). 25.1.218 J.Nassour 3
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) 25.1.218 J.Nassour 31
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 25.1.218 J.Nassour 32
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) 25.1.218 J.Nassour 33
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. 25.1.218 J.Nassour 34
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 25.1.218 J.Nassour 35
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 25.1.218 J.Nassour 36
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 τ 25.1.218 J.Nassour 37
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 25.1.218 J.Nassour 38
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? 25.1.218 J.Nassour 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? v Ci = J vi q, ω i = J ωi q 25.1.218 J.Nassour 4
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 25.1.218 J.Nassour 41
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 25.1.218 J.Nassour 42
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. 25.1.218 J.Nassour 43
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 25.1.218 J.Nassour 44
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 25.1.218 J.Nassour 45
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 25.1.218 J.Nassour 46
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 1. 25.1.218 J.Nassour 47
Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn 25.1.218 J.Nassour 48
Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn 25.1.218 J.Nassour 49
Mass Matrix M q = m 11 m 12 m 1n m 21 m 22 m 2n m n1 m n2 m nn 25.1.218 J.Nassour 5
Work out the mass matrix M. Example RP l 1 y I c2 m 2 m 1 I c1 x θ 1 d 2 25.1.218 J.Nassour 51
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 25.1.218 J.Nassour 52
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 2 25.1.218 J.Nassour 53
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 2 25.1.218 J.Nassour 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 ω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 zz2 25.1.218 J.Nassour 55 θ 1 d 2
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 1 2 + I zz1 + m 2 d 2 2 + I zz2 m 2 l 1 y I c2 m 2 m 1 I c1 x θ 1 d 2 25.1.218 J.Nassour 56
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 25.1.218 J.Nassour 57
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 = τ 25.1.218 J.Nassour 58
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 τ 2 25.1.218 J.Nassour 59
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 2 + + m ijk 25.1.218 J.Nassour 6 q k
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 111 + m 111 m 111 ) 1 2 (m 211 + m 211 m 112 ) 1 2 (m 122 + m 122 m 221 ) 1 2 (m 222 + m 222 m 222 ) q 1 2 q 2 2 + m 112 + m 121 m 121 m 212 + m 221 m 122 q 1 q 2 25.1.218 J.Nassour 61
Centrifugal and Coriolis Forces V q, q = 1 2 (m 111 + m 111 m 111 ) 1 2 (m 211 + m 211 m 112 ) 1 2 (m 122 + m 122 m 221 ) 1 2 (m 222 + m 222 m 222 ) q 1 2 q 2 2 + m 112 + m 121 m 121 m 212 + m 221 m q 1 q 2 122 Christoffel Symbols: b ijk = 1 2 (m ijk + m ikj m jki ) b 122 V q, q = b 111 q 1 b 211 b 222 2 q 2 2 + 2b 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 25.1.218 J.Nassour 62
Example RP Work out the Centrifugal and Coriolis Vector V. l 1 y I c2 m 2 m 1 I c1 x θ 1 d 2 25.1.218 J.Nassour 63
Example RP Work out the Centrifugal and Coriolis Vector V. C q b 122 V q, q = b 111 q 1 b 211 b 222 2 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 1 2 + I zz1 + m 2 d 2 2 + I zz2 m 2 V = 2m 2d 2 θ 1 d 2 + m 2 d 2 θ 1 2 d 2 2 25.1.218 J.Nassour 64
Dynamic Equations M q q + V q, q + G q = τ Gravity vector? 25.1.218 J.Nassour 65
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 25.1.218 J.Nassour 66
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 25.1.218 J.Nassour 67
Work out the gravity vector G. Example RP l 1 y I c2 m 2 g m 1 I c1 x θ 1 d 2 25.1.218 J.Nassour 68
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 1 25.1.218 J.Nassour 69
Work out the equation of motion. Example RP l 1 y I c2 m 2 g m 1 I c1 x θ 1 d 2 25.1.218 J.Nassour 7
Example RP Work out the equation of motion. g l 1 y I c2 m 2 m 1 l 2 1 + I zz1 + m 2 d 2 2 + 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 1 2 + I zz1 + m 2 d 2 2 + 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 θ 1 2 + m 2 gs 1 = f 2 25.1.218 J.Nassour 71
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 1 25.1.218 J.Nassour 72 y m 1 I c1 l I c2 m 2 q 2 x
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 1 25.1.218 J.Nassour 73 y m 1 I c1 l I c2 m 2 q 2 x
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 1 25.1.218 J.Nassour 74
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 25.1.218 J.Nassour 75
Example PR d Work out the Centrifugal and Coriolis Vector V. b 122 V q, q = b 111 q 1 b 211 b 222 2 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 2 25.1.218 J.Nassour 76
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 = τ 2 25.1.218 J.Nassour 77
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 2 25.1.218 J.Nassour 78
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 = τ 25.1.218 J.Nassour 79
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 12 25.1.218 J.Nassour 8
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 1 1 1 1, p C2 = l c2 c 12 + l 1 c 1 l c2 s 12 + l 1 s 1 x 25.1.218 J.Nassour 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 + 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 2 1 + 2m 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 zz2 25.1.218 J.Nassour 82
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 25.1.218 J.Nassour 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 = gm 2 l c2 c 12 + l 1 c 1 + gm 1 l c1 c 1 gm 2 l c2 c 12 25.1.218 J.Nassour 84
Example RR Work out the Centrifugal and Coriolis Vector V. b 122 V q, q = b 111 q 1 b 211 b 222 2 q 2 2 + 2b 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 2 2 25.1.218 J.Nassour 85
Work out the equation of motion. Example RR m 2 l 2 1 + 2m 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 1 25.1.218 J.Nassour 86 x m 1 I c1
Work out the equation of motion. Example RR m 2 l 2 1 + 2m 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 2 25.1.218 J.Nassour 87 x m 1 q 1
Work out the equation of motion. Example RR m 2 l 2 1 + 2m 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 2 25.1.218 J.Nassour 88 x m 1 q 1