Lecture Notes on Dynamics

Introduction to Robotics, H. Harry Asada 1 Chapter 7 Dynamics In this chapter, we analyze the dynamic behavior of robot mechanisms. The dynamic behavior is described in terms of the time rate of change of the robot configuration in relation to the joint torques exerted by the actuators. This relationship can be expressed by a set of differential equations, called equations of motion, that govern the dynamic response of the robot linkage to input joint torques. In the next chapter, we will design a control system on the basis of these equations of motion. Two methods can be used in order to obtain the equations of motion: the Newton-Euler formulation, and the Lagrangian formulation. The Newton-Euler formulation is derived by the direct interpretation of Newton's Second Law of Motion, which describes dynamic systems in terms of force and momentum. The equations incorporate all the forces and moments acting on the individual robot links, including the coupling forces and moments between the links. The equations obtained from the Newton-Euler method include the constraint forces acting between adjacent links. Thus, additional arithmetic operations are required to eliminate these terms and obtain explicit relations between the joint torques and the resultant motion in terms of joint displacements. In the Lagrangian formulation, on the other hand, the system's dynamic behavior is described in terms of work and energy using generalized coordinates. This approach is the extension of the indirect method discussed in the previous chapter to dynamics. Therefore, all the workless forces and constraint forces are automatically eliminated in this method. The resultant equations are generally compact and provide a closed-form expression in terms of joint torques and joint displacements. Furthermore, the derivation is simpler and more systematic than in the Newton-Euler method. The robot’s equations of motion are basically a description of the relationship between the input joint torques and the output motion, i.e. the motion of the robot linkage. As in kinematics and in statics, we need to solve the inverse problem of finding the necessary input torques to obtain a desired output motion. This inverse dynamics problem is discussed in the last section of this chapter. Efficient algorithms have been developed that allow the dynamic computations to be carried out on-line in real time. 7.1 Newton-Euler Formulation of Equations of Motion 7.1.1. Basic Dynamic Equations In this section we derive the equations of motion for an individual link based on the direct method, i.e. Newton-Euler Formulation. The motion of a rigid body can be decomposed into the translational motion with respect to an arbitrary point fixed to the rigid body, and the rotational motion of the rigid body about that point. The dynamic equations of a rigid body can also be represented by two equations: one describes the translational motion of the centroid (or center of mass), while the other describes the rotational motion about the centroid. The former is Newton's equation of motion for a mass particle, and the latter is called Euler's equation of motion. We begin by considering the free body diagram of an individual link. Figure 7.1.1 shows all the forces and moments acting on link i. The figure is the same as Figure 6.1.1, which describes the static balance of forces, except for the inertial force and moment that arise from the dynamic motion of the link. Let v ci be the linear velocity of the centroid of link i with reference Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 2 to the base coordinate frame O-xyz, which is an inertial reference frame. The inertial force is then given by − m i v ci , where mi is the mass of the link and v ci is the time derivative of v ci . Based on D’Alembert’s principle, the equation of motion is then obtained by adding the inertial force to the static balance of forces in eq.(6.1.1) so that f i −1,i − f i ,i +1 + mi g − mi v ci = 0, i = 1," , n (7.1.1) where, as in Chapter 6, f i −1,i and − f i ,i +1 are the coupling forces applied to link i by links i-1 and i+1, respectively, and g is the acceleration of gravity. − f i , i +1 ωi f i−1, i − N i , i+1 v ci N i−1, i Link i ri, C i Ci ri−1, i Oi Oi-1 z m ig O Joint i+1 Joint i y x Figure 7.1.1 Free body diagram of link i in motion Rotational motions are described by Euler's equations. In the same way as for translational motions, adding “inertial torques” to the static balance of moments yields the dynamic equations. We begin by describing the mass properties of a single rigid body with respect to rotations about the centroid. The mass properties are represented by an inertia tensor, or an inertia matrix, which is a 3 x 3 symmetric matrix defined by 2 2 ⎛ ⎜ ∫body {( y − yc ) + ( z − zc ) }ρ dV ⎜ I = ⎜ − ∫ ( x − xc )( y − yc ) ρ dV body ⎜ − ⎜ ∫body ( z − zc )( x − xc ) ρ dV ⎝ −∫ body ∫ body ( x − xc )( y − yc ) ρ dV {( z − zc ) 2 + ( x − xc ) 2 }ρ dV −∫ body ( y − yc )( z − zc ) ρ dV ⎞ ⎟ ⎟ − ∫ ( y − yc )( z − zc ) ρ dV ⎟ body ⎟ 2 2 ∫body {( x − xc ) + ( y − yc ) }ρ dV ⎟⎠ (7.1.2) −∫ body ( z − zc )( x − xc ) ρ dV where ρ is the mass density, xc , yc , zc are the coordinates of the centroid of the rigid body, and each integral is taken over the entire volume V of the rigid body. Note that the inertia matrix varies with the orientation of the rigid body. Although the inherent mass property of the rigid Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 3 body does not change when viewed from a frame fixed to the body, its matrix representation when viewed from a fixed frame, i.e. inertial reference frame, changes as the body rotates. The inertial torque acting on link i is given by the time rate of change of the angular momentum of the link at that instant. Let ω i be the angular velocity vector and I i be the centroidal inertia tensor of link i, then the angular momentum is given by I i ω i . Since the inertia tensor varies as the orientation of the link changes, the time derivative of the angular momentum  i , but also a term resulting from changes in the includes not only the angular acceleration term I i ω inertia tensor viewed from a fixed frame. This latter term is known as the gyroscopic torque and is given by ωi ×(I i ωi ) . Adding these terms to the original balance of moments (4-2) yields  i − ω i × (I i ω i ) = 0, i = 1,", n N i −1,i − N i ,i +1 − (ri −1,i + ri ,Ci ) × fi −1,i + (−ri ,Ci ) × (−fi ,i +1 ) − I i ω (7.1.3) using the notations of Figure 7.1.1. Equations (2) and (3) govern the dynamic behavior of an individual link. The complete set of equations for the whole robot is obtained by evaluating both equations for all the links, i = 1, · ,n. 7.1.2. Closed-Form Dynamic Equations The Newton-Euler equations we have derived are not in an appropriate form for use in dynamic analysis and control design. They do not explicitly describe the input-output relationship, unlike the relationships we obtained in the kinematic and static analyses. In this section, we modify the Newton-Euler equations so that explicit input-output relations can be obtained. The Newton-Euler equations involve coupling forces and moments f i−1, i and N i−1, i . As shown in eqs.(6.2.1) and (6.2.2), the joint torque τi, which is the input to the robot linkage, is included in the coupling force or moment. However, τi is not explicitly involved in the Newton-Euler equations. Furthermore, the coupling force and moment also include workless constraint forces, which act internally so that individual link motions conform to the geometric constraints imposed by the mechanical structure. To derive explicit input-output dynamic relations, we need to separate the input joint torques from the constraint forces and moments. The Newton-Euler equations are described in terms of centroid velocities and accelerations of individual arm links. Individual link motions, however, are not independent, but are coupled through the linkage. They must satisfy certain kinematic relationships to conform to the geometric constraints. Thus, individual centroid position variables are not appropriate for output variables since they are not independent. The appropriate form of the dynamic equations therefore consists of equations described in terms of all independent position variables and input forces, i.e., joint torques, that are explicitly involved in the dynamic equations. Dynamic equations in such an explicit input- output form are referred to as closed-form dynamic equations. As discussed in the previous chapter, joint displacements q are a complete and independent set of generalized coordinates that locate the whole robot mechanism, and joint torques τ are a set of independent inputs that are separated from constraint forces and moments. Hence, dynamic equations in terms of joint displacements q and joint torques τ are closed-form dynamic equations. Example 7.1 Figure 7.1.1 shows the two dof planar manipulator that we discussed in the previous chapter. Let us obtain the Newton-Euler equations of motion for the two individual links, and then derive closed-form dynamic equations in terms of joint displacements θ1 andθ 2 , and joint torques τ1 and τ2. Since the link mechanism is planar, we represent the velocity of the centroid of Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 4 each link by a 2-dimensional vector vi and the angular velocity by a scalar velocity ωi . We assume that the centroid of link i is located on the center line passing through adjacent joints at a distance A ci from joint i, as shown in the figure. The axis of rotation does not vary for the planar linkage. The inertia tensor in this case is reduced to a scalar moment of inertia denoted by Ii. From eqs. (1) and (3), the Newton-Euler equations for link 1 are given by f 0, 1 − f1, 2 + m1g − m1 v c1 = 0, N 0,1 − N1, 2 + r1,c1 × f1, 2 − r0,c1 × f 0,1 − I1ω1 = 0 (7.1.4) Note that all vectors are 2 x 1, so that moment N i-1,i and the other vector products are scalar quantities. Similarly, for link 2, f1, 2 + m2 g − m2 v c 2 = 0, N 1, 2 − r1,c 2 × f1,2 − I 2ω 2 = 0 (7.1.5) y A2 Vc2 A c2 A1 I 2 , m2 θ2 , τ 2 f1,2 A c1 O I1 , m1 x θ1 , τ 1 Link 0 Figure 7.1.2 Mass properties of two dof planar robot To obtain closed-form dynamic equations, we first eliminate the constraint forces and separate them from the joint torques, so as to explicitly involve the joint torques in the dynamic equations. For the planar manipulator, the joint torques τ1 and τ2 are equal to the coupling moments: N i −1,i = τ i , i = 1, 2 (7.1.6) Substituting eq.(6) into eq.(5) and eliminating f1,2 we obtain Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 5 τ 2 − r1,c 2 × m2 v c 2 + r1,c 2 × m2 g − I 2ω 2 = 0 (7.1.7) Similarly, eliminating f0,1 yields, τ 1 − τ 2 − r0,c1 × m1 v c1 − r0,1 × m2 v c 2 + r0,c1 × m1g + r0,1 × m2 g − I1ω1 = 0 (7.1.8) Next, we rewrite v ci , ωi , and ri ,i +1 using joint displacements θ1 and θ 2 , which are independent variables. Note that ω2 is the angular velocity relative to the base coordinate frame, while θ 2 is measured relative to link 1. Then, we have ω1 = θ1 , ω2 = θ1 + θ2 (7.1.9) The linear velocities can be written as ⎛ − A θ sin θ1 ⎞ ⎟ v c1 = ⎜⎜ c1 1  cosθ ⎟ A θ 1 ⎠ ⎝ c1 1 ⎛ − {A 1 sin θ1 + A c 2 sin(θ1 + θ 2 )}θ1 − A c 2 sin(θ1 + θ 2 )}θ2 ⎞ ⎟ v c 2 = ⎜⎜   ⎟ ⎝ {A 1 cosθ1 + A c 2 cos(θ1 + θ 2 )}θ1 + A c 2 cos(θ1 + θ 2 )}θ 2 ⎠ (7.1.10) Substituting eqs. (9) and (10) along with their time derivatives into eqs. (7) and (8), we obtain the closed-form dynamic equations in terms of θ1 andθ 2 : τ 1 = H11θ1 + H12θ2 − hθ22 − 2hθ1θ2 + G1 τ 2 = H 22θ2 + H 21θ1 + hθ12 + G2 (7.1.11-b) H 11 = m1A 2c1 + I1 + m2 (A 12 + A 2c 2 + 2A 1A c 2 cos θ 2 ) + I 2 (7.1.12-a) H 22 = m A + I 2 (7.1.12-b) H 12 = m2 (A 2c 2 + A 1A c 2 cos θ 2 ) + I 2 (7.1.12-c) h = m2A 1A c 2 sin θ 2 G1 = m1A c1 g cosθ1 + m2 g{A c 2 cos(θ1 + θ 2 ) + A 1 cos θ1} G2 = m2 gA c 2 cos(θ1 + θ 2 ) (7.1.12-d) (7.1.11-a) where 2 2 c2 (7.1.12-e) (7.1.12-f) The scalar g represents the acceleration of gravity along the negative y-axis. More generally, the closed-form dynamic equations of an n-degree-of-freedom robot can be given in the form n n n j +1 j =1 k =1 τ i = ∑ H ij qj + ∑∑ hijk q j qk + Gi , Department of Mechanical Engineering i = 1,", n (7.1.13) Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 6 where coefficients Hij , hijk, and Gi are functions of joint displacements q1 , q2 ,", qn . When external forces act on the robot system, the left-hand side must be modified accordingly. 7.1.3. Physical Interpretation of the Dynamic Equations In this section, we interpret the physical meaning of each term involved in the closedform dynamic equations for the two-dof planar robot. The last term in each of eqs. (11-a, b), Gi , accounts for the effect of gravity. Indeed, the terms G1 and G2, given by (12-e, f), represent the moments created by the masses m1 and m2 about their individual joint axes. The moments are dependent upon the arm configuration. When the arm is fully extended along the x-axis, the gravity moments become maximums. Next, we investigate the first terms in the dynamic equations. When the second joint is immobilized, i.e. θ2 = 0 and θ2 = 0 , the first dynamic equation reduces to τ 1 = H11θ1 , where the gravity term is neglected. From this expression it follows that the coefficient H11 accounts for the moment of inertia seen by the first joint when the second joint is immobilized. The coefficient H11 given by eq. (12-a) is interpreted as the total moment of inertia of both links reflected to the first joint axis. The first two terms, m1A c1 + I1 , in eq. (12-a), represent the moment of inertia of link 1 with respect to joint 1, while the other terms are the contribution from link 2. The inertia of the second link depends upon the distance L between the centroid of link 2 and the first joint axis, as illustrated in Figure 7.1.3. The distance L is a function of the joint angle θ 2 and is given by 2 L2 = A 1 + A c 2 + 2A 1A c 2 cosθ 2 2 2 (7.1.14) Using the parallel axes theorem of moment of inertia (Goldstein, 1981), the inertia of link 2 with respect to joint 1 is m2L2+I2 , which is consistent with the last two terms in eq. (12-a). Note that the inertia varies with the arm configuration. The inertia is maximum when the arm is fully extended ( θ 2 = 0 ), and minimum when the arm is completely contracted ( θ 2 = π ). L’ θ2 ' L θ2 θ1 O A c2 A1 Figure 7.1.3 Varying inertia depending on the arm configuration Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 7 Let us now examine the second terms on the right hand side of eq. (11). Consider the instant when θ1 = θ2 = 0 and θ1 = 0 , then the first equation reduces to τ 1 = H12θ2 , where the gravity term is again neglected. From this expression it follows that the second term accounts for the effect of the second link motion upon the first joint. When the second link is accelerated, the reaction force and torque induced by the second link act upon the first link. This is clear in the original Newton-Euler equations (4), where the coupling force -fl,2 and moment -N1,2 from link 2 are involved in the dynamic equation for link 1. The coupling force and moment cause a torque τ int about the first joint axis given by τ int = − N1, 2 − r0,1 × f1, 2 = − I 2ω 2 − r0,c 2 × m2 v c 2 (7.1.15) 2 = −{I 2 + m2 (A c 2 + A 1A c 2 cos θ 2 )}θ2 where N1,2 and fl,2 are evaluated using eq. (5) for θ1 = θ2 = 0 and θ1 = 0 . This agrees with the second term in eq. (11-a). Thus, the second term accounts for the interaction between the two joints. The third terms in eq. (11) are proportional to the square of the joint velocities. We consider the instant when θ2 = 0 and θ1 = θ2 = 0 , as shown in Figure 7.1.4-(a). In this case, a centrifugal force acts upon the second link. Let fcent be the centrifugal force. Its magnitude is given by 2 f cent = m2 L θ1 (7.1.16) where L is the distance between the centroid C2 and the first joint O. The centrifugal force acts in the direction of position vector rO ,C 2 . This centrifugal force causes a moment τcent about the second joint. Using eq. (16), the moment τcent is computed as τ cent = r1,c 2 × fcent = −m2A 1A c 2θ1 2 (7.1.17) This agrees with the third term hθ12 in eq. (11-b). Thus we conclude that the third term is caused by the centrifugal effect on the second joint due to the motion of the first joint. Similarly, rotating the second joint at a constant velocity causes a torque of − hθ22 due to the centrifugal effect upon the first joint. fCor fcent A c 2 θ2 C2 r1,c 2 r0, c 2 y O1 O θ1 θ1 (a) O θ2 yb Ob x θ1 + θ 2 xb (b) Figure 7.1.4 Centrifugal (a) and Coriolis (b) effects Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 8 Finally we discuss the fourth term of eq. (11-a), which is proportional to the product of the joint velocities. Consider the instant when the two joints rotate at velocities θ1 and θ2 at the same time. Let Ob-xbyb be the coordinate frame attached to the tip of link 1, as shown in Figure 7.1.4-(b). Note that the frame Ob-xbyb is parallel to the base coordinate frame at the instant shown. However, the frame rotates at the angular velocity θ1 together with link 1. The mass centroid of link 2 moves at a velocity of A c 2θ2 relative to link 1, i.e. the moving coordinate frame Ob-xbyb. When a mass particle m moves at a velocity of vb relative to a moving coordinate frame rotating at an angular velocity ω, the mass particle has the so-called Coriolis force given by − 2m (ω × v b ) . Let fCor be the force acting on link 2 due to the Coriolis effect. The Coriolis force is given by ⎛ 2m A θ θ cos(θ1 + θ 2 ) ⎞ ⎟ fCor = ⎜⎜ 2 c 2 1 2  θ sin(θ + θ ) ⎟ 2 A m θ 1 2 ⎠ ⎝ 2 c2 1 2 (7.1.18) This Coriolis force causes a moment τ C or about the first joint, which is given by τ Cor = r0,c 2 × fCor = 2 m2 A1 A c 2 θ1 θ2 sin θ 2 (7.1.19) The right-hand side of the above equation agrees with the fourth term in eq. (11-a). Since the Coriolis force given by eq. (18) acts in parallel with link 2, the force does not create a moment about the second joint in this particular case. Thus, the dynamic equations of a robot arm are characterized by a configurationdependent inertia, gravity torques, and interaction torques caused by the accelerations of the other joints and the existence of centrifugal and Coriolis effects. 7.2. Lagrangian Formulation of Robot Dynamics 7.2.1. Lagrangian Dynamics In the Newton-Euler formulation, the equations of motion are derived from Newton's Second Law, which relates force and momentum, as well as torque and angular momentum. The resulting equations involve constraint forces, which must be eliminated in order to obtain closedform dynamic equations. In the Newton-Euler formulation, the equations are not expressed in terms of independent variables, and do not include input joint torques explicitly. Arithmetic operations are needed to derive the closed-form dynamic equations. This represents a complex procedure that requires physical intuition, as discussed in the previous section. An alternative to the Newton-Euler formulation of manipulator dynamics is the Lagrangian formulation, which describes the behavior of a dynamic system in terms of work and energy stored in the system rather than of forces and moments of the individual members involved. The constraint forces involved in the system are automatically eliminated in the formulation of Lagrangian dynamic equations. The closed-form dynamic equations can be derived systematically in any coordinate system. Let q1 ,", qn be generalized coordinates that completely locate a dynamic system. Let T and U be the total kinetic energy and potential energy stored in the dynamic system. We define the Lagrangian L by Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 9 L(qi , qi ) = T (qi , qi ) − U (qi ) (7.2.1) Note that the potential energy is a function of generalized coordinates qi and that the kinetic energy is that of generalized velocities qi as well as generalized coordinates qi. Using the Lagrangian, equations of motion of the dynamic system are given by d ∂L ∂L − = Qi , dt ∂qi ∂qi i = 1," , n (7.2.2) where Qi is the generalized force corresponding to the generalized coordinate qi. Considering the virtual work done by non-conservative forces can identify the generalized forces acting on the system. 7.2.2 Planar Robot Dynamics Before discussing general robot dynamics in three-dimensional space, we consider the 2 dof planar robot, for which we have derived the equations of motion based on Newton-Euler Formulation. Figure 7.2.1 shows the same robot mechanism with a few new variables needed for the Lagrangian Formulation. y Vc2 A c2 A1 ω2 I 2 , m2 θ2 , τ 2 Vc1 ω1 A c1 I1 , m1 O θ1 , τ 1 x Figure 7.2.1 Two dof robot The total kinetic energy stored in the two links moving at linear velocity v ci and angular velocity ωi at the centroids, as shown in the figure, is given by 2 1 1 2 2 T = ∑ ( mi v ci + I i ωi ) 2 i =1 2 (7.2.3) where v ci represents the magnitude of the velocity vector. Note that the linear velocities and the angular velocities are not independent variables, but are functions of joint angles and joint Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 10 angular velocities, i.e. the generalized coordinates and the generalized velocities that locate the dynamic state of the system uniquely. We need to rewrite the above kinetic energy so that it is with respect to θ i and θi . The angular velocities are given by ω1 = θ1 , ω2 = θ1 + θ2 (7.2.4) The linear velocity of the first link is simply 2 2 2 v c1 = A c1 θ1 (7.2.5) However, the centroidal linear velocity of the second link vc2 needs more computation. Treating the centroid C2 as an endpoint and applying the formula for computing the endpoint velocity yield the centroidal velocity. Let J c 2 be the 2x2 Jacobian matrix relating the centroidal velocity vector to joint velocities. Then, v c 2 = J c 2q = q T J c 2 J c 2q 2 ( where q = θ1 T= 2 T (7.2.6) θ2 ) . Substituting eqs.(4~6) to eq.(3) yields T ( 1 1 1 H11θ12 + H12θ1θ2 + H 22θ22 = θ1 θ2 2 2 2 ) ⎛⎜⎜ HH T ⎝ 11 12 H12 ⎞⎛ θ1 ⎞ ⎟⎜ ⎟ H 22 ⎟⎠⎜⎝θ2 ⎟⎠ (7.2.7) where coefficients Hij are the same as the ones in eq.(7.1.12). H 11 = m1A 2c1 + I1 + m2 (A 12 + A 2c 2 + 2A 1A c 2 cos θ 2 ) + I 2 = H11 (θ 2 ) (7.1.12-a) H 22 = m A + I 2 (7.1.12-b) H 12 = m2 (A 2c 2 + A 1A c 2 cos θ 2 ) + I 2 = H12 (θ 2 ) (7.1.12-c) 2 2 c2 Note that coefficients H11 and H12 are functions of θ 2 . The potential energy stored in the two links is given by U = m1 gA c1 sin θ1 + m2 g{A 1 sin θ1 + A c 2 sin(θ1 + θ 2 )} (7.2.8) Now we are ready to obtain Lagrange’s equations of motion by differentiating the above kinetic energy and potential energy. For the first joint, ∂L ∂U =− = −[m1A c1 g cos θ1 + m2 g{A c 2 cos(θ1 + θ 2 ) + A 1 cos θ1}] = −G1 ∂q1 ∂q1 ∂L = H11θ1 + H12θ2 ∂q1 d ∂L ∂H ∂H 2 = H11θ1 + H12θ2 + 11 θ2θ1 + 12 θ2 dt ∂q1 ∂θ 2 ∂θ 2 Department of Mechanical Engineering (7.2.9) (7.2.10) Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 11 Substituting the above two equations into eq.(2) yields the same result as eq.(7.1.11-a). The equation of motion for the second joint can be obtained in the same manner, which is identical to eq.(7.1.11-b). Thus, the same equations of motion have been obtained based on Lagrangian Formulation. Note that the Lagrangian Formulation is simpler and more systematic than the Newton-Euler Formulation. To formulate kinetic energy, velocities must be obtained, but accelerations are not needed. Remember that the acceleration computation was complex in the Newton-Euler Formulation, as discussed in the previous section. This acceleration computation is automatically dealt with in the computation of Lagrange’s equations of motion. The difference between the two methods is more significant when the degrees of freedom increase, since many workless constraint forces and moments are present and the acceleration computation becomes more complex in Newton-Euler Formulation. 7.2.3 Inertia Matrix In this section we will extend Lagrange’s equations of motion obtained for the two d.o.f. planar robot to the ones for a general n d.o.f. robot. Central to Lagrangian formulation is the derivation of the total kinetic energy stored in all of the rigid bodies involved in a robotic system. Examining kinetic energy will provide useful physical insights of robot dynamic. Such physical insights based on Lagrangian formulation will supplement the ones we have obtained based on Newton-Euler formulation. As seen in eq.(3) for the planar robot, the kinetic energy stored in an individual arm link consists of two terms; one is kinetic energy attributed to the translational motion of mass mi and the other is due to rotation about the centroid. For a general three-dimensional rigid body, this can be written as Ti = 1 1 T T mi v ci v ci + ω i I i ω i , 2 2 i = 1,", n (7.2.11) where ω i and Ii are, respectively, the 3x1 angular velocity vector and the 3x3 inertia matrix of the i-th link viewed from the base coordinate frame, i.e. inertial reference. The total kinetic energy stored in the whole robot linkage is then given by n T = ∑ Ti (7.2.12) i =1 since energy is additive. The expression for the kinetic energy is written in terms of the velocity and angular velocity of each link member, which are not independent variables, as mentioned in the previous section. Let us now rewrite the above equations in terms of an independent and complete set of generalized coordinates, namely joint coordinates q = [q1, .. ,qn]T. For the planar robot example, we used the Jacobian matrix relating the centroid velocity to joint velocities for rewriting the expression. We can use the same method for rewriting the centroidal velocity and angular velocity for three-dimensional multi-body systems. v ci = J iLq ωi = J iAq Department of Mechanical Engineering (7.2.13) Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 12 where JLi and JAi are, respectively, the 3 x n Jacobian matrices relating the centroid linear velocity and the angular velocity of the i-th link to joint velocities. Note that the linear and angular velocities of the i-th link are dependent only on the first i joint velocities, and hence the last n-i columns of these Jacobian matrices are zero vectors. Substituting eq.(13) into eqs.(11) and (12) yields T= T T 1 n 1 (mi q T J iL J iL q + q T J iA I i J iAq ) = q T Hq ∑ 2 i=1 2 (7.2.14) where H is a n x n matrix given by n H = ∑ (mi J iL J iL + J iA I i J iA ) T T (7.2.15) i =1 The matrix H incorporates all the mass properties of the whole robot mechanism, as reflected to the joint axes, and is referred to as the Multi-Body Inertia Matrix. Note the difference between the multi-body inertia matrix and the 3 x 3 inertia matrices of the individual links. The former is an aggregate inertia matrix including the latter as components. The multi-body inertia matrix, however, has properties similar to those of individual inertia matrices. As shown in eq. (15), the multi-body inertia matrix is a symmetric matrix, as is the individual inertia matrix defined by eq. (7.1.2). The quadratic form associated with the multi-body inertia matrix represents kinetic energy, so does the individual inertia matrix. Kinetic energy is always strictly positive unless the system is at rest. The multi-body inertia matrix of eq. (15) is positive definite, as are the individual inertia matrices. Note, however, that the multi-body inertia matrix involves Jacobian matrices, which vary with linkage configuration. Therefore the multi-body inertia matrix is configuration-dependent and represents the instantaneous composite mass properties of the whole linkage at the current linkage configuration. To manifest the configuration-dependent nature of the multi-body inertia matrix, we write it as H(q), a function of joint coordinates q. Using the components of the multi-body inertia matrix H={Hij}, we can write the total kinetic energy in scalar quadratic form: T= 1 n n ∑∑ H ij qi q j 2 i=1 j =1 (7.2.16) Most of the terms involved in Lagrange’s equations of motion can be obtained directly by differentiating the above kinetic energy. From the first term in eq.(2), n n dH d ∂T d n = (∑ H ij q j ) = ∑ H ij q j + ∑ ij q j dt ∂qi dt j =1 j =1 j =1 dt The first term of the last expression, n ∑H j =1 diagonal terms n ∑H i≠ j ij ij (7.2.17) i as well as offq j , comprises the diagonal term H ii q qj , representing the dynamic interactions among the multiple joints due to accelerations, as discussed in the previous section. It is important to note that a pair of joints, i and j, have the same coefficient of the dynamic interaction, Hij=Hji , since the multi-body inertia matrix H is symmetric. In vector-matrix form these terms can be written collectively as Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 13 ⎛ H11 ⎜ i>⎜ #  = Hq j>⎜ # ⎜ ⎜H ⎝ n1 " " % H ji " H ij % " ⎛ q1 ⎞ H1n ⎞ ⎜ # ⎟ ⎟⎜ ⎟ # ⎟ ⎜ qi ⎟ # ⎟ ⎜⎜ q j ⎟⎟ ⎟ H nn ⎟⎠ ⎜ # ⎟ ⎜ q ⎟ ⎝ n⎠ (7.2.18) It is clear that the interactive inertial torque H ij q j caused by the j-th joint acceleration upon the ith joint has the same coefficient as that of H ji qi caused by joint i upon joint j. This property is called Maxwell’s Reciprocity Relation. The second term of eq.(17) is non-zero in general, since the multi-body inertia matrix is configuration-dependent, being a function of joint coordinates. Applying the chain rule, dH ij dt n ∂H ∂H ij dqk = ∑ ij q k dt k =1 ∂qk k =1 ∂qk n =∑ (7.2.19) The second term in eq.(2), Lagrange’s equation of motion, also yields the partial derivatives of Hij. From eq.(16), ⎞ 1 n n ∂H jk ∂T ∂ ⎛1 n n ⎜ ∑∑ H jk q j qk ⎟ = ∑∑ = q j qk ⎟ 2 ∂qi ∂qi ⎜⎝ 2 j =1 k =1 j =1 k =1 ∂qi ⎠ (7.2.20) Substituting eq.(19) into the second term of eq.(17) and combining the resultant term with eq.(20), let us write these nonlinear terms as n n hi = ∑∑ Cijk q j qk (7.2.21) j =1 k =1 where coefficients Cijk is given by Cijk = ∂H ij ∂qk − 1 ∂H jk 2 ∂qi (7.2.22) This coefficient Cijk is called Christoffel’s Three-Index Symbol. Note that eq.(21) is nonlinear, having products of joint velocities. Eq.(21) can be divided into the terms proportional to square joint velocities, i.e. j=k, and the ones for j ≠ k : the former represents centrifugal torques and the latter Coriolis torques. n n hi = ∑ Cijj q j + ∑ Cijk q j qk = (Centrifugal) + (Coriolis) 2 j =1 Department of Mechanical Engineering (7.2.23) k≠ j Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 14 These centrifugal and Coriolis terms are present only when the multi-body inertia matrix is configuration dependent. In other words, the centrifugal and Coriolis torques are interpreted as nonlinear effects due to the configuration-dependent nature of the multi-body inertia matrix in Lagrangian formulation. 7.2.4 Generalized Forces Forces acting on a system of rigid bodies can be represented as conservative forces and non-conservative forces. The former is given by partial derivatives of potential energy U in Lagrange’s equations of motion. If gravity is the only conservative force, the total potential energy stored in n links is given by n U = −∑ mi g T r0, ci (7.2.24) i =1 where r0 , ci is the position vector of the centroid Ci that is dependent on joint coordinates. Substituting this potential energy into Lagrange’s equations of motion yields the following gravity torque seen by the i-th joint: Gi = n n ∂r ∂U = −∑ m j gT 0, cj = −∑ m j gT J Lj , i ∂qi ∂qi j =1 j =1 (7.2.25) L where J j, i is the i-th column vector of the 3 x 1 Jacobian matrix relating the linear centroid velocity of the j-th link to joint velocities. Non-conservative forces acting on the robot mechanism are represented by generalized forces Qi in Lagrangian formulation. Let δWork be virtual work done by all the non-conservative forces acting on the system. Generalized forces Qi associated with generalized coordinates qi, e.g. joint coordinates, are defined by n δWork = ∑ Qiδqi (7.2.26) i =1 If the virtual work is given by the inner product of joint torques and virtual joint displacements, τ 1δq1 +" + τ nδqn , the joint torque itself is the generalized force corresponding to the joint coordinate. However, generalized forces are often different from joint torques. Care must be taken for finding correct generalized forces. Let us work out the following example. Example 7.2 Consider the same 2 d.o.f. planar robot as Example 7.1. Instead of using joint angles θ1 and θ 2 as generalized coordinates, let us use the absolute angles, φ1 and φ2 , measured from the positive x-axis. See the figure below. Changing generalized coordinates entails changes to generalized forces. Let us find the generalized forces for the new coordinates. Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 15 y τ2 θ2 , τ 2 δφ2 φ2 τ1 φ1 O θ1 , τ 1 −τ 2 δφ1 x Figure 7.2.2 Absolute joint angles φ1 and φ2 and disjointed links As shown in the figure, joint torque τ 2 acts on the second link, whose virtual displacement is δφ2 , while joint torque τ 1 and the reaction torque − τ 2 act on the first link for virtual displacement δφ1 . Therefore the virtual work is δWork = (τ 1 − τ 2 )δφ1 + τ 2δφ2 (7.2.27) Comparing this equation with eq.(26) where generalized coordinates are φ1 = q1 , φ2 = q2 , we can conclude that the generalized forces are: Q1 = τ 1 − τ 2 , Q2 = τ 2 (7.2.28) The two sets of generalized coordinates θ1 and θ 2 vs. φ1 and φ2 are related as φ1 = θ1 , φ2 = θ1 + θ 2 (7.2.29) Substituting eq.(29) into eq.(27) yields δWork = (τ 1 − τ 2 )δθ1 + τ 2δ (θ1 + θ 2 ) = τ 1δθ1 + τ 2δθ2 (7.2.30) This confirms that the generalized forces associated with the original generalized coordinates, i.e. joint coordinates, are τ 1 and τ 2 . Non-conservative forces acting on a robot mechanism include not only these joint torques but also any other external force Fext . If an external force acts at the endpoint, the generalized forces Q=(Q1,…, Qn)T associated with generalized coordinates q are, in vector form, given by δWork = τT δq + Fext T δp = ( τ + J T Fext )T δq = QT δq Q = τ + J T Fext Department of Mechanical Engineering (7.2.31) Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 16 When the external force acts at position r, the above Jacobian must be replaced by Jr = dr dq (7.2.32) Note that, since generalized coordinates q can uniquely locate the system, the position vector r must be written as a function of q alone. Department of Mechanical Engineering Massachusetts Institute of Technology