Elements of Kinematics and Dynamics of Industrial Robots

Transcription

1 Elements of Kinematics and Dynamics of Industrial Robots Claudio Melchiorri Dipartimento di Ingegneria dell Energia Elettrica e dell Informazione (DEI) Università di Bologna claudio.melchiorri@unibo.it Claudio Melchiorri Elements of Kinematics and Dynamics 1/ 106

2 Summary 1 Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators 2 Forward kinematics Inverse Kinematics 3 Claudio Melchiorri Elements of Kinematics and Dynamics 2/ 106

3 Summary 1 Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators 2 Forward kinematics Inverse Kinematics 3 Claudio Melchiorri Elements of Kinematics and Dynamics 2/ 106

4 Summary 1 Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators 2 Forward kinematics Inverse Kinematics 3 Claudio Melchiorri Elements of Kinematics and Dynamics 2/ 106

5 Degrees of Freedom of a Manipulator Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators A manipulator may be described as the interconnection of rigid bodies (links) through kinematic pairs (joints) A joint is an element that constrains one or more relative motion directions between two rigid bodies (links). In robotics, joints are of two types: rotoidal: rotation about a fixed axis prismatic: translation along a fixed axis Each joint is characterised by the number of independent motion directions allowed between the two consecutive links. Such a number defines the degrees of freedom of the joint rotoidal and prismatic joints have 1 degree of freedom (dof)! Claudio Melchiorri Elements of Kinematics and Dynamics 3/ 106

6 Degrees of Freedom of a Manipulator Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators If a joint has k degrees of freedom, then the relative configuration between two rigid bodies may be expressed as a function of k variables q 1, q 2,..., q k, called joint variables rotoidal joint: q is the amplitude of the rotation (θ) prismatic joint: q is the amplitude of the translation (d) Let us consider a manipulator with n+1 links (L 0,L 1,...,L n) interconnected by n joints (q 1,q 2,...q n), and indicate as k i the degrees of freedom of the i-th joint, i = 1,...,n we assume a serial kinematic chain! The manipulator configuration depends on N dof independent variables, where n N dof = N dof represents the number of degrees of freedom (dof) of the manipulator N dof also represents the number of actuators N dof = n for a manipulator with n+1 links and rotoidal/prismatic joints i=1 k i Claudio Melchiorri Elements of Kinematics and Dynamics 4/ 106

7 Joint Space and Work Space Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators Joint Space. Let us stack all the joint variables in a vector q = [q 1,q 2,...,q n] T, q Q R N dof. The set Q is called joint space, and contains all the possible values that the joint variables may assume. Note that for each q Q, there is a unique configuration of the mechanical structure. Work Space. The work space is a subset of the Euclidean space E in which the robot executes its tasks. It is the set of all the points (configurations) that the mechanical structure may assume, and in general is a 3D (2D) subset of E. Each point of the work space is indicated by a vector x of proper dimension, x R y, y = {3,6}. Configuration of a manipulator. It takes into account both the position and the orientation of a reference frame fixed to the manipulator extremity (end effector). Then (locally): x R 3 in a plane; x R 6 in space Classification of manipulators. If R n is the joint space and R m the work space: n = m: normal case n < m: defective manipulators n > m: redundant manipulators Claudio Melchiorri Elements of Kinematics and Dynamics 5/ 106

8 Types of robotic manipulators Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators The first three dof define the structure of the robot. By composing rotoidal and prismatic joints, it is possible to obtain different kinematic structures, with different properties that can be exploited in different types of tasks. Among the kinematic structures that have been developed, we have: Cartesian robots Anthropomorphic robots SCARA robots Cylindrical and spherical robots Spherical wrists Claudio Melchiorri Elements of Kinematics and Dynamics 6/ 106

9 Types of robotic manipulators Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators Cartesian Manipulator Claudio Melchiorri Elements of Kinematics and Dynamics 7/ 106

10 Types of robotic manipulators Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators Anthropomorphic Manipulator Claudio Melchiorri Elements of Kinematics and Dynamics 8/ 106

11 Types of robotic manipulators Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators SCARA Manipulator Claudio Melchiorri Elements of Kinematics and Dynamics 9/ 106

12 Types of robotic manipulators Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators Cylindrical Manipulator Spherical Manipulator Claudio Melchiorri Elements of Kinematics and Dynamics 10/ 106

13 Types of robotic manipulators Degrees of Freedom of a Manipulator Joint Space and Work Space Types of robotic manipulators spherical wrist Claudio Melchiorri Elements of Kinematics and Dynamics 11/ 106

14 Forward kinematics Inverse Kinematics Kinematic Model of Robot Manipulators Forward kinematics Inverse kinematics Differential kinematics and Statics (Jacobian) Claudio Melchiorri Elements of Kinematics and Dynamics 12/ 106

15 Kinematic model Forward kinematics Inverse Kinematics In describing the kinematics of a robot, there are two problems: Direct (forward) kinematic model: once the position, velocity, acceleration values are known in the joint space, determine the corresponding values in the work space (in a proper reference frame, e.g. Cartesian) x = f(q) q R n, x R m Inverse kinematic model: determine the inverse mapping of the kinematic variables from the work space to the joint space q = g(x) = f 1 (x) q R n, x R m It is possible to define different kinematic models for a given manipulator, although equivalent from a mathematical point of view. Claudio Melchiorri Elements of Kinematics and Dynamics 13/ 106

16 Kinematic model Example: a 2 dof planar robot Forward kinematics Inverse Kinematics Forward kinematic model: Inverse kinematic model: x = l 1 cosθ 1 +l 2 cos(θ 1 +θ 2 ) y = l 1 sinθ 1 +l 2 sin(θ 1 +θ 2 ) φ = θ 1 +θ 2 An easy problem... cosθ 2 = x2 0 + y2 0 l2 1 l2 2, sinθ 2 = ± (1 cos 2 θ 2 ) 2l 1 l 2 θ 2 = atan2(sinθ 2,cosθ 2 ) k 1 = l 1 + l 2 cosθ 2, k 2 = l 2 sinθ 2 sinθ 1 = y 0 k 1 x 0 k 2, cosθ 1 = y 0 k 1 sinθ 1 k k2 2 k 2 θ 1 = atan2(sinθ 1,cosθ 1 ) The solution is not so simple. Two possible solutions (sign of sinθ 2 ). Claudio Melchiorri Elements of Kinematics and Dynamics 14/ 106

17 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics The homogeneous transformation matrixs i T j are used to define the kinematic model. The configuration of a rigid body b i in 3D space may be described by the homogeneous transformation 0 T i between a reference frame FF i (fixed to the rigid body) and the base frame FF 0 ( ) 0 0 R 0 i p i ( ) T ( ) T i =, 0 R i = 0 1, R 0 i p i R R SO(3) is a (3 3) rotation matrix, 0 p i is a 3-dimensional position vector. Claudio Melchiorri Elements of Kinematics and Dynamics 15/ 106

18 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics Given two rigid bodies b i and b j with the corresponding matrixs 0 T i and 0 T j, their relative configuration is given by: ( ) 1 j T i = 0 T 0 j T i Claudio Melchiorri Elements of Kinematics and Dynamics 16/ 106

19 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics Homogeneous transformation matrixs i T j are used to define the kinematic model. A manipulator is a mechanical system made of a chain of rigid bodies, the links, connected by joints. A reference frame is properly associated to each link, and two consecutive reference frames are related by the homogeneous transformation matrix i 1 T i, that results to be function of the joint variable q i. Links: L 0,L 1,...,L n Joints: J 1,J 2,...,J n Frames: F 0,F 1,...,F n Transformation matrixs: 0 T 1, 1 T 2,..., n 2 T n 1, n 1 T n Kinematic model: 0 T n = 0 T 1 1 T 2... n 1 T n Claudio Melchiorri Elements of Kinematics and Dynamics 17/ 106

20 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics A notation to define the kinematic model (Denavit-Hartenberg). Each link is numbered, from 0 to n, so that it is uniquely identified in the mechanical chain: L 0, L 1,..., L n. The base link is conventionally identified as L 0, the distal link as L n. A manipulator with n+1 links has n joints, each of them allowing the relative motion between two consecutive links. Also joints are numbered, from 1 to n, starting from the base: J 1, J 2,..., J n. According to this definition, J i connects the links L i 1 and L i. Frames F i are associated to each joint, according to the specific Denavit-Hartenberg procedure, and matrixs i 1 T i are computed. Links: L 0,L 1,...,L n Joints: J 1,J 2,...,J n Frames: F 0,F 1,...,F n Transformation matrixs: 0 T 1, 1 T 2,..., n 2 T n 1, n 1 T n Kinematic model: 0 T n = 0 T 1 1 T 2... n 1 T n Claudio Melchiorri Elements of Kinematics and Dynamics 18/ 106

21 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics The motion of the joints changes the position and the orientation in space of the extremity of the manipulator. Then, the position and the orientation are (in general) non linear functions of the n joint variables q 1, q 2,..., q n, that is: p = f(q 1,q 2,...,q n) = f(q) where q = (q 1,q 2,...,q n) T is defined in the joint space R n p is defined in the work space R m Vector p usually takes into account: some components for the position (e.g. the position x, y, z, with respect to a Cartesian base frame) some components for orientation (e.g. Euler or RPY angles). The Denavit-Hartenberg notation gives a systematic procedure to define the kinematic model of a robot manipulator, and requires 4 parameters only (not 6) for each link: d i,θ i,a i,α i. Two constraints: 1) axis x i+1 intersects, and 2) it is perpendicular to z i. Claudio Melchiorri Elements of Kinematics and Dynamics 19/ 106

22 Kinematic model Denavit-Hartenberg Parameters Forward kinematics Inverse Kinematics Conclusion: the position and the orientation of two consecutive frames, and therefore of the related links, may be defined by the four Denavit-Hartenberg parameters: a i = length of the common normal between the axes of two consecutive joints α i = ccw angle between z i 1 the axis of joint i, and z i, axis of joint i +1 d i = distance between the origin o i 1 of F i 1 and the point p i, θ i = ccw angle about z i 1 between the axis x i 1 and the common normal x i between z i 1 and z i. i 1 T i = C θi S θi C αi S θi S αi ac θi S θi C θi C αi C θi S αi as θi 0 S αi C αi d i Claudio Melchiorri Elements of Kinematics and Dynamics 20/ 106

23 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics Given two consecutive links L i 1 and L i connected by joint J i, matrix i 1 T i is function of the joint variable q i, that is of a rotation θ i if the joint is rotational or of a translation d i if the joint is prismatic i 1 T i = i 1 T i(q i) In case of a manipulator with n joints, the relationship between frames F 0 and F n is given by 0 T n = 0 T 1(q 1) 1 T 2(q 2) n 1 T n(q n) Once the joint variables q 1, q 2,..., q n are known, this equation defines position and orientation of the last link (frame) with respect to the base reference frame This equation is the kinematic model of the manipulator It is always possible to derive the forward kinematic model (for any serialchain manipulator). Claudio Melchiorri Elements of Kinematics and Dynamics 21/ 106

24 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics Example. Let us consider a planar, 2 dof robot matrixs i 1 T i are defined as (C i = cosθ i, S i = sinθ i): C 1 S 1 0 a 1C 1 0 T 1(θ 1) = S 1 C 1 0 a 1S T 2(θ 2) = C 2 S 2 0 a 2C 2 S 2 C 2 0 a 2S Claudio Melchiorri Elements of Kinematics and Dynamics 22/ 106

25 Kinematic model Direct kinematic model Forward kinematics Inverse Kinematics Then (C ij = cos(θ i +θ j), S ij = sin(θ i +θ j)): ( ) 0 T 2 = 0 T 1 n s a p 1 T 2 = C 12 S 12 0 a 1C 1 +a 2C 12 = S 12 C 12 0 a 1S 1 +a 2S Vectors n, s, a define the orientation of the extremity of the manipulator (rotation about z), while p defines its position (plane x y) Claudio Melchiorri Elements of Kinematics and Dynamics 23/ 106

26 Forward kinematics Inverse Kinematics Example: PUMA 260 Homogeneous transformation matrixs: 0 T1 = C 1 0 S 1 0 S 1 0 C T 2 C 2 S 2 0 a 2 C 2 S 2 C 2 0 a 2 S T 3 = C 3 0 S 3 0 S 3 0 C d T4 = C 4 0 S 4 0 S 4 0 C d T 5 = C 5 0 S 5 0 S 5 0 C T 6 = C 6 S S 6 C d Claudio Melchiorri Elements of Kinematics and Dynamics 24/ 106

27 Example: PUMA 260. Matrix 0 T 6 = Forward kinematics Inverse Kinematics [ n s a p ], with n = S 1 (C 5 C 6 S 4 + C 4 S 6 ) + C 1 (C 2 ( (C 6 S 3 S 5 ) + C 3 (C 4 C 5 C 6 S 4 S 6 )) S 2 (C 3 C 6 S 5 + S 3 (C 4 C 5 C 6 S 4 S 6 ))) (C 1 (C 5 C 6 S 4 + C 4 S 6 )) + S 1 (C 2 ( (C 6 S 3 S 5 ) + C 3 (C 4 C 5 C 6 S 4 S 6 )) S 2 (C 3 C 6 S 5 + S 3 (C 4 C 5 C 6 S 4 S 6 ))) S 2 ( (C 6 S 3 S 5 ) + C 3 (C 4 C 5 C 6 S 4 S 6 )) + C 2 (C 3 C 6 S 5 + S 3 (C 4 C 5 C 6 S 4 S 6 )) S 1 (C 4 C 6 C 5 S 4 S 6 )+C 1 (C 2 (S 3 S 5 S 6 + C 3 ( (C 6 S 4 ) C 4 C 5 S 6 )) S 2 ( (C 3 S 5 S 6 ) + S 3 ( (C 6 S 4 ) C 4 C 5 S 6 ))) s= (C 1 (C 4 C 6 C 5 S 4 S 6 ))+S 1 (C 2 (S 3 S 5 S 6 + C 3 ( (C 6 S 4 ) C 4 C 5 S 6 )) S 2 ( (C 3 S 5 S 6 )+S 3 ( (C 6 S 4 ) C 4 C 5 S 6 ))) S 2 (S 3 S 5 S 6 + C 3 ( (C 6 S 4 ) C 4 C 5 S 6 )) + C 2 ( (C 3 S 5 S 6 )+S 3 ( (C 6 S 4 ) C 4 C 5 S 6 )) a = S 1 S 4 S 5 + C 1 (C 2 (C 5 S 3 + C 3 C 4 S 5 ) S 2 ( (C 3 C 5 ) + C 4 S 3 S 5 )) (C 1 S 4 S 5 ) + S 1 (C 2 (C 5 S 3 + C 3 C 4 S 5 ) S 2 ( (C 3 C 5 ) + C 4 S 3 S 5 )) S 2 (C 5 S 3 + C 3 C 4 S 5 ) + C 2 ( (C 3 C 5 ) + C 4 S 3 S 5 ) p = S 1 ( d 3 + d 6 S 4 S 5 ) + C 1 (a 2 C 2 + C 2 ((d 4 + C 5 d 6 )S 3 + C 3 C 4 d 6 S 5 ) S 2 ( (C 3 (d 4 + C 5 d 6 )) + C 4 d 6 S 3 S 5 )) (C 1 ( d 3 + d 6 S 4 S 5 )) + S 1 (a 2 C 2 + C 2 ((d 4 + C 5 d 6 )S 3 + C 3 C 4 d 6 S 5 ) S 2 ( (C 3 (d 4 + C 5 d 6 )) + C 4 d 6 S 3 S 5 )) a 2 S 2 + S 2 ((d 4 + C 5 d 6 )S 3 + C 3 C 4 d 6 S 5 ) + C 2 ( (C 3 (d 4 + C 5 d 6 )) + C 4 d 6 S 3 S 5 ) Claudio Melchiorri Elements of Kinematics and Dynamics 25/ 106

28 Kinematic model Inverse Kinematics Forward kinematics Inverse Kinematics It is always possible to compute the forward kinematic model with a standard procedure (DH). Viceversa, a standard procedure to obtain the inverse kinematic model q = f 1 (x) = g(x) does not exist! Moreover, we may have: no solution (if x does not belong to the workspace) a finite set of solutions (in general more than one) infinite solutions. We look for closed-form solutions, and not for numerical procedures: an analytical solution is better from the computational point of view given the analytical expressions, it is easy to select a particular solution in the set of solutions. Claudio Melchiorri Elements of Kinematics and Dynamics 26/ 106

29 Kinematic model Inverse Kinematics Forward kinematics Inverse Kinematics Example: an anthropomorphic arm has 4 possible different configurations to reach a given point in space! Claudio Melchiorri Elements of Kinematics and Dynamics 27/ 106

30 Kinematic model Inverse Kinematics Forward kinematics Inverse Kinematics A closed form solution to the inverse kinematic problem may be obtained, in case it exists, by trying one, or both, of these approaches: ALGEBRAIC approach, that is an elaboration of the kinematic equations in order to obtain a set of simple equations that can be solved in the unknown joint variables q GEOMETRIC approach, based, when possible, on considerations dependent on the geometric structure of the manipulator, that may help in solving the problem. Claudio Melchiorri Elements of Kinematics and Dynamics 28/ 106

31 Kinematic model Inverse Kinematics Forward kinematics Inverse Kinematics Algebraic Approach. The kinematic model of a 6 dof robot manipulator is given by the matrix equation 0 T 6 = 0 T 1(q 1) 1 T 2(q 2) 5T 6(q 6) that is equivalent to 12 scalar equations in the 6 unknowns q i, i = 1,...,6 Since the values of elements in 0 T 6 and the structure of matrixs i 1 T i are known, pre- and post-multiplying we get: [ ] 1 [ 0 T 1(q 1) i 1 T i(q i) 0 T j 6 T j+1(q j+1) 5T ] 1 6(q 6) = = i T i+1(q i+1) j 1 T j(q j) that is, other 12 scalar equations for each pair (i,j), i < j By selecting the most simple equations among those defined above, it might be possible to find 6 equations that can be solved. this method is closely related to the expression of the direct kinematic model! Claudio Melchiorri Elements of Kinematics and Dynamics 29/ 106

32 Kinematic model Inverse Kinematics Forward kinematics Inverse Kinematics Pieper approach While the algebraic approach is not based on any general consideration that helps in finding a solution, the geometric approach may give useful indications for solving the inverse kinematics. For several kinematic structures of industrial manipulators, it is possible to apply the kinematic decoupling principle, that allows to decompose the overall problem into two, simpler, sub-problems: 1. solution of the inverse kinematic problem for the position 2. solution of the inverse kinematic problem for the orientation of the end-effector Pieper Approach: Sufficient condition to solve the inverse kinematic problem for a manipulator with 6 degrees of freedom is that its geometric structure has: three consecutive rotational joints whose axes intersect in a point (spherical wrist) or three consecutive rotational joints with parallel axes Claudio Melchiorri Elements of Kinematics and Dynamics 30/ 106

33 Kinematic model Inverse Kinematics Forward kinematics Inverse Kinematics Spherical wrist (NB: the type/position of the first three joints q 1, q 2 e q 3 does not matter!) The end effector position/orientation is given in ( ) R p 0 T 6 = the vector p p can be computed from 0 T 6 (matrixs R, p): p p = p d 6z 6 being d 6 a constant parameter, and z 6 the last column of R p p is function of q 1, q 2 and q 3 only! given p p, the inverse kinematics is solved for q 1, q 2 and q 3 the rotation matrix 0 R 3 (function of the first three joints) is computed since 0 R 3 3 R 6 = R, we have 3 R 6 = 0 R 1 3 R = 0 R3 T R given 3 R 6, the IK for the orientation is solved (Euler ang.): q 4, q 5, q 6 Claudio Melchiorri Elements of Kinematics and Dynamics 31/ 106

34 Kinematic model Forward kinematics Inverse Kinematics In robotics, besides the relationship between joint positions and end-effector position/orientation, it is of interest to define also the relationships between: end-effector velocity and joint velocities: ( ) v ω force applied on the environment by the manipulator and corresponding joint torques: ( ) f τ n These two relationships are based on a matrix, defining therefore a linear mapping between joint- and work-space, known as the Jacobian of the manipulator. The Jacobian matrix, very important in robotics, is used also for: studying the singularities of the mechanical structure; defining numerical algorithms to solve the inverse kinematics; studying the manipulability properties. q Claudio Melchiorri Elements of Kinematics and Dynamics 32/ 106

35 Kinematic model Forward kinematics Inverse Kinematics The six-dimensional velocity vector (twist) in the work-space is defined with three linear and three rotational components: ( ) v ẋ = ω Two different (although equivalent) expressions of the Jacobian matrix can be defined: the analytic expression, used when the rotational component in ẋ is defined as γ, the time-derivative of a triple of orientation variables the geometric expression, used when the rotational component in ẋ is defined as ω, the rotational velocity vector It is possible to relate the two expressions by means of a proper matrix T(γ) [ ] I3 0 J G (q) = T(γ)J A (q) = J A (q) 0 ˆT(γ) Claudio Melchiorri Elements of Kinematics and Dynamics 33/ 106

36 Kinematic model Forward kinematics Inverse Kinematics Analytic expression The analytic expression of the Jacobian is obtained by differentiating the six-dimensional vector x = f(q) = [p T, γ T ] T giving the position and orientation of the end-effector in the work-space with respect to the base frame FF0; usually, γ containes the Euler or the RPY angles. By differentiating f(q) we get: where the m n matrix dx = f (q)dq = J(q)dq ( ẋ = J(q) q ) q J(q) = f 1 q 1 f 1. f m q 1 f 1 q n q f m q 2 f m q n J(q) R m n is called the Jacobian matrix, or simply the Jacobian, of the manipulator Claudio Melchiorri Elements of Kinematics and Dynamics 34/ 106

37 Kinematic model Forward kinematics Inverse Kinematics Example. Let us consider the 2 dof planar manipulator The forward kinematics is given by: p x = a 1cosθ 1 +a 2cos(θ 1 +θ 2) p y = a 1sinθ 1 +a 2sin(θ 1 +θ 2) γ = θ 1 +θ 2 Therefore: ṗ x ṗ y = γ a 1sinθ 1 a 2sin(θ 1 +θ 2) a 2sin(θ 1 +θ 2) a 1cosθ 1 +a 2cos(θ 1 +θ 2) a 2cos(θ 1 +θ 2) 1 1 } {{ } J(q) ( θ1 θ 2 ) }{{} q Claudio Melchiorri Elements of Kinematics and Dynamics 35/ 106

38 Kinematic model Forward kinematics Inverse Kinematics Geometric expression The geometric expression of the Jacobian is obtained by considering as rotational components of the velocity vector ẋ the rotational vector ω, and not the derivative of a triple of angles γ. ẋ = [ ṗ γ ] ẋ = [ ṗ ω Note that, from a geometric point of view, the expression ẋ = J(q) q implies that the vector ẋ is a linear combination of the elements in q, where the weights are the columns J i of matrix J(q) (weight of the joint velocity q i on ẋ). ] v = J v1 q 1 +J v2 q J vn q n ω = J ω1 q 1 +J ω2 q J ωn q n Claudio Melchiorri Elements of Kinematics and Dynamics 36/ 106

39 Kinematic model Forward kinematics Inverse Kinematics It is possible to proof that the generic i-th column of the geometric Jacobian is defined as [ Jvi J ωi [ Jvi J ωi ] ] = = [ 0 z i 1 ( 0 p n 0 p i 1) [ ] 0 z i z i 1 ] rotational joint prismatic joint It is simple to obtain these expressions once the forward kinematics of the manipulator is known, 0 T n = 0 T 1(q 1) 1 T 2(q 2) n 1 T n(q n), since the vectors 0 p i and 0 z i are obtained from matrices 0 T i. From a computational point of view, there is just a minor increase with respect to the computation of the forward kinematic model! Claudio Melchiorri Elements of Kinematics and Dynamics 37/ 106

40 Kinematic model Forward kinematics Inverse Kinematics Force domain By exploiting the virtual work principle, from the equation ẋ = J(q) q, that gives the mapping in terms of velocities between the joint- and work-space, it is possible to obtain an analogous equation in the force domain. Since the work, that is the product of an applied external force with the displacement, is invariant with respect to the frame where it is computed, we have w T dx = τ T dq w = ( f T n T) T is a 6-dimensional vector (wrench) collecting the linear forces f and the torques n applied to the manipulator τ is the n-dimensional vector of the joint forces/torques Claudio Melchiorri Elements of Kinematics and Dynamics 38/ 106

41 Kinematic model Forward kinematics Inverse Kinematics From w T dx = τ T dq, since we easily obtain dx = J(q)dq τ = J T (q)w This equation gives the relationship between the joint torque vector τ and the force/torque vector w applied by the manipulator in the work-space. Claudio Melchiorri Elements of Kinematics and Dynamics 39/ 106

42 Kinematic model Forward kinematics Inverse Kinematics Singularities: The equation ẋ = J(q) q may be interpreted, from an infinitesimal point of view, as: dx = J(q)dq that is as a relationship between infinitesimal displacements in R n and R 6. In general, rankj(q) = min(6,n), but: there are joint configurations q in which J(q) looses rank (rankj(q) < min(6,n)) in these configurations, there are directions in R 6 without a corresponding direction in R n (the robot cannot move along these directions) In singular configurations: some motion directions in R 6 cannot be achieved limited velocities of the manipulator end-effector may correspond to infinite velocities in the joint space usually, they correspond to points at the border of the work-space, that is to points of maximum extension of the manipulator a well defined solution to the inverse kinematic problem does not exist: either no solution or infinite solutions Claudio Melchiorri Elements of Kinematics and Dynamics 40/ 106

43 Kinematic model Forward kinematics Inverse Kinematics Example. Let us consider the Jacobian matrix ( ) 1 1 J(q) =, with rankj(q) = For any value of dq 1 and dq 2 we have: { dx1 = dq 1 +dq 2 dx 2 = 0 Any vector dx with dx 2 0 is not physically achievable, dq 1,dq 2 Because of the duality of velocities and forces, we have ( ) ( )( ) τ1 τ = = J T 1 0 fx f = = 1 0 τ 2 force f y does not affect joint torques τ 1,τ 2 f y ( fx f x ) Claudio Melchiorri Elements of Kinematics and Dynamics 41/ 106

44 Kinematic model Forward kinematics Inverse Kinematics ( a1s 1 a 2S 12 a 2S 12 J(q) = a 1C 1 +a 2C 12 a 2C 12 ) If θ 1 = θ 2 = 0, we have J(q) = ( N.B.: det{j(q)} = a 1a 2sin(θ 2) ), a 1 = a 2 = 1 Then { dx = 0 dy = 2dq 1 +dq 2 e { τ1 = 2f y τ 2 = f y no (instantaneous) motion is possible along x forces along x do not generate joint torques (can be applied without any effort at joints) Claudio Melchiorri Elements of Kinematics and Dynamics 42/ 106

45 Kinematic model Forward kinematics Inverse Kinematics Inverse differential kinematics The forward mapping between joint-a and work-space is given by ẋ = J(q) q, J(q) R m n The inverse mapping is then given as the solution of a linear (matrix) algebraic equation. m = n: if the manipulator is not in a singular configuration, it is possible to use the inverse of the Jacobian: q = J 1 (q)ẋ m n: the Moore-Penrose pseudoinverse of the Jacobian is used: q = J + (q)ẋ J + = J T( JJ T) 1 if m < n (right pseudoinverse) JJ + = I m J + = ( J T J ) 1 J T if m > n (left pseudoinverse) J + J = I n I p: (p p) identity matrix Claudio Melchiorri Elements of Kinematics and Dynamics 43/ 106

46 Kinematic model Forward kinematics Inverse Kinematics n m rankj = p < min(m,n) The solution given by the pseudoinverse q s = J + ẋ is such that (ẋ s = J q s): { ẋ ẋs the norm of the error is minimum q s the norm of the solution is minimum Claudio Melchiorri Elements of Kinematics and Dynamics 44/ 106

47 Kinematic model Forward kinematics Inverse Kinematics Solution of ẋ = J q if m < n rank(j) = min(m,n) = m R(J) = IR ẋ q tale che ẋ = J q (more than one exists!) q = J + ẋ N(J) such that q N(J) ẋ = J q = 0 q = J + ẋ+q N ẋ = J (J + ẋ+ q N ) = ẋ, q N N(J) q = J + ẋ +(I J + J)y general expression of the solution (I J + J) is a base of N(J) q = J + ẋ has minimum norm Claudio Melchiorri Elements of Kinematics and Dynamics 45/ 106

48 Kinematic model Forward kinematics Inverse Kinematics The Jacobian may be used to solve the inverse kinematic problem if a closed form solution is not available A method could be to compute q k+1 = q k +J 1 (q k )v k dt where a numerical integration of the position is performed Unfortunately, this method is subject to numerical drifts and initialization problems, and therefore it is difficult to obtain the desired solution An alternative method is based on a feedback loop, by defining an error in the work-space. Given e = x d x By differentiating, we obtain ė = ẋ d J(q) q we have to define q in such a way that e converges to 0 (e 0 when t ) Claudio Melchiorri Elements of Kinematics and Dynamics 46/ 106

49 Kinematic model Forward kinematics Inverse Kinematics Algorithm 1: If q = J 1 (q)(ẋ d +Ke) with K = K T > 0, then ė = Ke Inverse kinematic algorithm based on the Jacobian (pseudo-)inverse Claudio Melchiorri Elements of Kinematics and Dynamics 47/ 106

50 Kinematic model Forward kinematics Inverse Kinematics Algorithm 2: Given ẋ d = 0, if q = J T (q)ke with K = K T > 0, then V(e) = 1 2 et Ke V(e) = e T Kė = e T KJ(q)J T (q)ke 0 Inverse kinematic algorithm based on the Jacobian transpose q = 0 with e 0 if Ke kerj T (q) Claudio Melchiorri Elements of Kinematics and Dynamics 48/ 106

51 Kinematic model Forward kinematics Inverse Kinematics Manipulability measures The Jacobian may be used to evaluate the achievable performances of a manipulator, in terms of velocities and applicable forces, or the predisposition of a manipulator to execute a given task manipulability ellipsoids Claudio Melchiorri Elements of Kinematics and Dynamics 49/ 106

52 Forward kinematics Inverse Kinematics Velocity manipulability ellipsoids Velocity. Given a sphere with unit radius in the joint velocity space, q T q = 1, that may be considered as a cost (energy in input to the manipulator), we want to obtain its equivalent expression in the work-space From ẋ = J(q) q we obtain: ẋ T( JJ T) +ẋ = 1 that is an ellipsoid in the work-space R m : the directions of the principal axes are given by the eigenvectors of JJ T the length of the principal axes are given by the singular values of J, σ i = λ i (JJ T ) Claudio Melchiorri Elements of Kinematics and Dynamics 50/ 106

53 Kinematic model Forward kinematics Inverse Kinematics Force manipulability ellipsoids Force. Let us consider now the equation τ T τ = 1, from which we obtain: w T JJ T w = 1 This equation describes an ellipsoids in the R m force space direction of the principal axes given by the eigenvectors of JJ T length of the principal axes given by the inverse of the singular values of J This result confirms the duality of the velocity/force spaces: along the directions in which high velocity performances are obtained, low force performances are achieved, and viceversa. Considerations: actuation requires a large amplification, and results better along directions where the eigenvectors (eigenvalues) are larger control requires a small amplification, and results better along directions where the eigenvectors (eigenvalues) are smaller (better sensitivity ) the optimal actuation direction in the velocity (force) domain is also the best control direction in the force (velocity) domain Claudio Melchiorri Elements of Kinematics and Dynamics 51/ 106

54 Kinematic model Forward kinematics Inverse Kinematics Velocity Ellipsoids Force Ellipsoids [m] 0 [m] [m] [m] Claudio Melchiorri Elements of Kinematics and Dynamics 52/ 106

55 Dynamic Model of Robot Manipulators Euler-Lagrange model Newton- Euler model Claudio Melchiorri Elements of Kinematics and Dynamics 53/ 106

56 Dynamics of a robot manipulator: analysis of the relationship between the applied forces/torques and the resulting motion of a robot (ẍ = f/m) For the dynamics, it is possible to define two models : Direct model: given the forces/torques applied at joints, the joint position and velocity, compute the acceleration and then q = q = f(q, q,τ) qdt q = Inverse model: given the desired joint acceleration, velocity and position, compute the corresponding force/torque to be applied qdt τ = f 1 ( q, q,q) = g( q, q,q) In deriving the dynamic model of a robot, we refer to a chain of rigid bodies mutually, and ideally, connected among them. Claudio Melchiorri Elements of Kinematics and Dynamics 54/ 106

57 The dynamic model of a robot is used for several purposes: simulation: testing motion behaviour without resorting to physical experiments analysis and synthesis of control laws analysis of the structural properties of a manipulator in the design phase. Two techniques are available for the computation of the dynamic model: Euler-Lagrange approach. First approach to be developed. The dynamic model obtained in this manner is simpler and more intuitive, and also more suitable to understand the effects of changes in the mechanical parameters. The links are considered altogether, and the model is obtained analytically. Drawbacks: the model is obtained starting from the kinetic and potential energies (non intuitive); the model is not computationally efficient. Newton-Euler approach. Based on a computationally efficient recursive technique that exploits the serial structure of an industrial manipulator. On the other hand, the mathematical model is not expressed in closed form. Obviously, the two techniques are equivalent (provide the same results). Claudio Melchiorri Elements of Kinematics and Dynamics 55/ 106

58 Euler-Lagrange model From physics, we know that it is possible to define: 1 The Kinetic Energy function K(q, q) 2 The Potential Energy function P(q) and therefore the Lagrangian function L(q, q) = K(q, q) P(q) The Euler-Lagrange equations for a system described by n Lagrangian coordinates q i are ψ i = d ( ) L L i = 1,...,n dt q i q i being ψ i the non-conservative (external or dissipative) generalized forces performing work on q i. In robotics, we consider: τ i joint actuator torque [ J T ] F c term due to external (contact) forces i d ii q i joint friction torque Therefore: ψ i = τ i + [ ] J T F c d i ii q i. Claudio Melchiorri Elements of Kinematics and Dynamics 56/ 106

59 Euler-Lagrange model Since the potential energy does not depend on the velocity, the Euler-Lagrange equations can be rewritten as ψ i = d ( ) K K + P i = 1,...,n (1) dt q i q i q i This formulation is more convenient since in robotics it is possible to compute quite easily the terms K and P from the geometric properties of the manipulator. Then, by applying (1), the dynamic model is obtained. Note that K = n K i P = i=1 n i=1 P i Claudio Melchiorri Elements of Kinematics and Dynamics 57/ 106

60 Euler-Lagrange model Computation of the Kinetic Energy. For a rigid body B: The mass can be computed by m = ρ(x,y,z)dx dy dz B where ρ(x,y,z) is the mass density (assumed constant): ρ(x,y,z) = ρ. The center of mass (CoM) is p C = 1 p(x,y,z)ρdx dy dz = 1 p(x,y,z)dm m B m B The kinetic energy results as K = 1 v T (x,y,z)v(x,y,z)ρdx dy dz 2 B = 1 v T (x,y,z)v(x,y,z)dm 2 B Claudio Melchiorri Elements of Kinematics and Dynamics 58/ 106

61 Euler-Lagrange model Let assume that v C and ω, i.e. the translational and rotational velocities of the center of mass, are known with respect to an inertial frame F 0. The velocity of a generic point p of the body is v = v C +ω (p p C ) = v C +ω r (2) Note that the velocity expressed in a frame F (e.g. fixed to the rigid body) may be computed by introducing the rotation matrix R between F and F 0 R T v = R T (v C +ω r) = R T v C +(R T ω) (R T r) Therefore v = v C +ω r Claudio Melchiorri Elements of Kinematics and Dynamics 59/ 106

62 Euler-Lagrange model Since the product ω r in (2) can be expressed as S(ω) r, we have: K = 1 v T (x,y,z)v(x,y,z)dm 2 B = 1 (v C +Sr) T (v C +Sr) dm 2 B = 1 v 2 Cv T C dm+ 1 r T S T Sr dm + v B 2 CSr T dm B B = 1 vcv T C dm+ 1 r T S T Sr dm 2 2 B B As a matter of fact, from the definition of CoM ( B rdm = B (p C p)dm = 0): vcsr T dm = vcs T r dm = 0 B B Claudio Melchiorri Elements of Kinematics and Dynamics 60/ 106

63 Euler-Lagrange model In conclusion K = 1 2 B v T Cv C dm+ 1 2 B r T S T Sr dm The first term depends on the linear velocity v C of the center of mass 1 v T 2 Cv C dm = 1 2 m vt Cv C B For the second term, considering that a T b = Tr(a b T ) and the particular structure of matrix S, we have 1 r T S T Sr dm = 1 Tr(Sr r T S T ) dm = Tr(S rr T dm S T ) B B = 1 2 Tr(S J ST ) = 1 2 ωt I ω I : body inertia matrix Also this term depends on the velocity of the center of mass (in this case ω). B Claudio Melchiorri Elements of Kinematics and Dynamics 61/ 106

64 Euler-Lagrange model Matrix J, Euler matrix, and matrix I, body inertia matrix, are symmetric, and have the following general expressions: J = r 2 x dm r xr y dm r xr z dm rxr y dm r 2 y dm r yr z dm rxr z dm r yr z dm r 2 z dm (r 2 y +r 2 z)dm r xr y dm r xr z dm I = r xr y dm r xr z dm (r 2 x +rz)dm 2 r yr z dm r yr z dm (r 2 x +ry)dm 2 The elements of both matrices J and I depend on vector r, i.e. on the position of the generic point of the i-th link with respect to its center of mass, defined in the base frame F 0. Since the position of the i-th link depends on the configuration of the manipulator, matrices J and I are in general functions of the joint variables q! Claudio Melchiorri Elements of Kinematics and Dynamics 62/ 106

65 Euler-Lagrange model In conclusion, the kinetic energy of a rigid body is defined as (Konig Theorem) K = 1 2 m vt Cv C ωt Iω (3) Both terms depend only on the velocity of the rigid body. The first term, being related to the magnitude of a vector ( v C = v T C v C), is invariant with respect to the reference frame used to express the velocity: v T C v C = (Rv C ) T (Rv C ) = v T C (RT R)v C, R This property holds also for the second term: the product ω T Iω is invariant with respect to the reference frame. As a matter of fact, the body inertia matrix is transformed according to the following relation: I = R I R T Then: ω T Iω = ω T I ω = (R ω) T (RIR T )(Rω) = ω T (R T R)I(R T R)ω. Therefore, being (3) invariant with respect to the reference frame, F can be chosen in order to simplify the computations required for the evaluation of K. Claudio Melchiorri Elements of Kinematics and Dynamics 63/ 106

66 Euler-Lagrange model Since the kinetic energy K i of the generic i-th link is invariant with respect to the reference frame (used to express v C,ω,I), it is convenient to chose a frame F i fixed to the link itself, with origin in the center of mass. In this manner matrix I is constant and simple to be computed on the basis of the geometric properties of the link. On the other hand, normally the rotational velocity ω is defined in the base frame F 0, and therefore it is needed to transform it according to R T ω, being R the rotation matrix between F i and F 0 (known from the kinematic model of the manipulator). Claudio Melchiorri Elements of Kinematics and Dynamics 64/ 106

67 Euler-Lagrange model In conclusion, the kinetic energy of a manipulator can be determined when, for each link, the following quantities are known: the link mass m i; the inertia matrix I i, computed wrt a frame F i fixed to the center of mass in which it has a constant expression Ĩi; the linear velocity v Ci of the center of mass, and the rotational velocity ω i of the link (both expressed in F 0 ); the rotation matrix R i between the frame fixed to the link and F 0. The kinetic energy K i of the i-th link has the form: K i = 1 2 mivt Civ Ci ωt i R i Ĩ ir T i ω i It is now necessary to compute the linear and rotational velocities (v Ci and ω i) as functions of the Lagrangian coordinates (i.e. the joint variables q). Claudio Melchiorri Elements of Kinematics and Dynamics 65/ 106

68 Euler-Lagrange model The end-effector velocity may be computed as a function of the joint velocities q 1,..., q n through the Jacobian matrix J. The same methodology can be used to compute the velocity of a generic point of the manipulator, and in particular the velocity v Ci = ṗ Ci of the center of mass p Ci, that results function of the joint velocities q 1,..., q i only: ṗ Ci = J i v1 q 1 +J i v2 q J i vi q i = J i v q ω i where = J i ω1 q 1 +J i ω2 q J i ωi q i = J i ω q J i v = [ J i v1... J i vi ] J i ω = [ J i ω1... J i ωi ] with (j = 1,...,i) [ ] J i vj J i = [ ωj ] J i vj J i = ωj [ ] zj 1 (p Ci p j 1 ) z j 1 [ ] zj 1 0 rotational joint linear joint being p j 1 the position of the origin of the frame associated to the j-th link. Claudio Melchiorri Elements of Kinematics and Dynamics 66/ 106

69 Euler-Lagrange model In conclusion, for a n dof manipulator we have: K = 1 2 n m ivciv T Ci i=1 = 1 2 qt n i=1 = 1 2 qt M(q) q = 1 2 n i=1 n ω T i R i Ĩ ir T i ω i=1 [ ] m ij i v T (q)j i v(q)+j i ω T (q)r i Ĩ ir T i J i ω(q) n M ij(q) q i q j j=1 where because of its definition M(q) is a n n, symmetric and positive definite matrix, function of the manipulator configuration q. Matrix M(q) is called the Inertia Matrix of the manipulator. q Claudio Melchiorri Elements of Kinematics and Dynamics 67/ 106

70 Euler-Lagrange model Computation of the Potential Energy. For rigid bodies, the only potential energy taken into account in the dynamics is due to the gravitational field g. For the generic i-th link P i = g T pdm = g T pdm = g T p Ci m i L i L i The potential energy does not depend on the joint velocities q, and may be expressed as a function of the position of the centers of mass. For the whole manipulator: P = n g T p Ci m i i=1 In case of flexible link, one should consider also terms due to elastic forces. Important result: K e P are computed (once m i and Ĩ are known) with a procedure similar to the one adopted for the forward kinematic model: K matrices J i e R i, P p Ci position of the centers of mass Claudio Melchiorri Elements of Kinematics and Dynamics 68/ 106

71 Euler-Lagrange model Once K and P are known, it is possible to compute the dynamic model of the manipulator. The dynamics is expressed by ψ k = d ( ) L L k = 1,...,n dt q k q k The Lagrangian function is L = K P = 1 n n n M ij q i q j g T p Ci m i 2 i=1 j=1 i=1 Then and Moreover d L = dt q k n n M kj q j + j=1 L = K = q k q k j=1 L q k = 1 2 d M kj dt n n i=1 j=1 n M kj q j j=1 n n n q j = M kj q j + j=1 M ij q k q i q j P q k i=1 j=1 M kj q i q i q j Claudio Melchiorri Elements of Kinematics and Dynamics 69/ 106

72 Euler-Lagrange model The Lagrangian equations have the following formulation n n n [ Mkj M kj q j + 1 ] M ij q i q j + P = ψ k q i 2 q k q k j=1 i=1 j=1 By defining the term h kji (q) as and g k (q) as h kji (q) = M kj(q) 1 M ij (q) q i 2 q k g k (q) = P(q) q k the following equations are finally obtained n M kj (q) q j + j=1 n i=1 n h kji (q) q i q j +g k (q) = ψ k j=1 k = 1,...,n k = 1,...,n Claudio Melchiorri Elements of Kinematics and Dynamics 70/ 106

73 Euler-Lagrange model The elements M kj (q), h ijk (q), g k (q) are function of the joint position only, and therefore their computation is relatively simple once the manipulator s configuration is known. They have the following physical meaning: For the acceleration terms: M kk is the moment of inertia about the k-th joint axis, in a given configuration and considering blocked all the other joints M kk is function of the joint positions q k+1,...,q n only (NOT of q 1,...,q k ) M kj is the inertia coupling, accounting the effect of acceleration of joint j on joint k For the quadratic velocity terms: h kjj q 2 j represents the centrifugal effect induced on joint k by the velocity of joint j (notice that h kkk = M kk q k = 0) h kji q i q j represents the Coriolis effect induced on joint k by the velocities of joints i and j For the configuration-dependent terms: g k represents the torque generated on joint k by the gravity force acting on the manipulator in the current configuration. Claudio Melchiorri Elements of Kinematics and Dynamics 71/ 106

74 Euler-Lagrange model Recalling that the non-conservative forces ψ k are in general composed by τ k [ J T ] F c k d kk q k joint actuator torque external (contact) forces joint friction torque the Lagrangian equations n n n M kj (q) q j + h kji (q) q i q j +g k (q) = ψ k j=1 i=1 j=1 can be written in matrix form as k = 1,...,n M(q) q+c(q, q) q+d q+g(q) = τ +J T (q)f c This matrix equation is known as the dynamic model of the manipulator. Claudio Melchiorri Elements of Kinematics and Dynamics 72/ 106

75 Euler-Lagrange model - Some considerations The product C(q, q) q = n i=1 n j=1 h kji(q) q i q j is a (n 1) vector v whose elements are quadratic functions of the joint velocities q j. The k-th element v k of this vector is: v k = where the elements C kj are computed as n C kj q j j=1 C kj = n c ijk q i i=1 with c ijk = 1 2 [ Mkj + M ] ki Mij q i q j q k Christoffel Symbols (4) Claudio Melchiorri Elements of Kinematics and Dynamics 73/ 106

76 Euler-Lagrange model - Some considerations The elements of matrix C(q, q) are computed as follows. From n n n n [ Mkj h kji q i q j = 1 ] M ij q i q j q i=1 j=1 i=1 j=1 i 2 q k by exchanging the sum (i, j) and exploiting the symmetry one obtains n n M kj = 1 n n [ Mkj + M ] ki q i 2 q i q j i=1 j=1 i=1 j=1 and then n n [ Mkj 1 ] M ij n n [ 1 Mkj = + M ki M ] n n ij = c ijk q i=1 j=1 i 2 q k 2 q i=1 j=1 i q j q k i=1 j=1 [ where c ijk = 1 Mkj + M ki M ] ij are the so-called Christoffel Symbols. 2 q i q j q k Since matrix M(q) is symmetric, for a given k then c ijk = c jik. In conclusion, the elements of matrix C(q, q) are computed as n [C(q, q)] k,j = c ijk q i (5) i=1 Claudio Melchiorri Elements of Kinematics and Dynamics 74/ 106

77 Euler-Lagrange model - Some considerations This is not the only possible expression for matrix C(q, q). In general, any matrix such that n n n c ij q j = h ijk q k q j j=1 j=1 k=1 can be considered. The choice (4) is preferred since in this case the following property is verified. Property. Matrix N(q, q), defined as N(q, q) = Ṁ(q, q) 2C(q, q) (6) in which the elements of C(q, q) are defined as c ijk = 1 2 [ Mkj + M ] ki Mij q i q j q k results skew-symmetric, i.e. n kj = n jk, n kk = 0. [C(q, q)] k,j = n c ijk q i i=1 Claudio Melchiorri Elements of Kinematics and Dynamics 75/ 106

78 Euler-Lagrange model - Some considerations In fact, by considering the generic element n kj, one obtains n kj = d M kj 2[C] kj dt n [ Mkj = ( M kj + M ] ki Mij ) q i q i q i q j q k i=1 n [ Mij = M ] ki q i q k q j i=1 from which it follows (if indices k and j are exchanged, because of the symmetry of M(q)) that n kj = n jk. Since matrix N is skew-symmetrix, then x T N(q, q) x = 0, x Claudio Melchiorri Elements of Kinematics and Dynamics 76/ 106

79 Euler-Lagrange model - Some considerations The condition x T N(q, q) x = 0, holds since N(q, q) is skew-symmetric, due to the particular choice of the elements of matrix C(q, q). On the other hand, the condition (with x = q) q T N(q, q) q = 0 holds for any choice of matrix C(q, q) (from the energy conservation principle). The evolution over time of the kinetic energy K must be equal to the work generated by the forces acting at joints: ( ) q T M q = q T (τ D q g(q) J T F) d K dt = 1 d 2 dt The first element is (from the dynamic model M q = C q D q g(q)+τ J T F): 1 d ( ) q T M q = 1 2 dt 2 qt Ṁ q+ q T M q = 1 2 qt Ṁ q+ q T ( C q D q g(q)+τ J T F) x Claudio Melchiorri Elements of Kinematics and Dynamics 77/ 106

80 Euler-Lagrange model - Some considerations Then 1 2 qt Ṁ q+ q T ( C q D q g(q)+τ J T F) = q T (τ D q g(q) J T F) from which 1 2 qt Ṁ q = q T C q = q T (Ṁ 2C) q = 0 This equation holds q and without any assumption on matrix C( q,q) (that is, it holds even if C is not based on the Cristoffel symbols). Claudio Melchiorri Elements of Kinematics and Dynamics 78/ 106

81 Euler-Lagrange model - Some considerations In deriving the dynamic model, the actuation system has not been taken into account. This is normally composed by: motors reduction gears trasmission system. The actuation system has several effects on the dynamics: if motors are installed on the links, then masses and inertia are changed it introduces its own dynamics (electromechanical, pneumatic, hydraulic,...) that may be non negligible (e.g. in case of lightweight manipulators) it introduces additional nonlinear effects such as backslash, friction, elasticity,... Notice that these effects could be considered by introducing suitable terms in the dynamic model derived on the basis of the Euler-Lagrangian formulation. Claudio Melchiorri Elements of Kinematics and Dynamics 79/ 106

82 Example - 1 of a pendulum (one dof manipulator). Consider θ joint variable, τ joint torque, m mass, L distance between center of mass and joint, d viscous friction coefficient, I inertia seen at the rotation axis. Kinetic energy: K = 1 2 I θ 2 Potenzial energy: Lagrangian function L: P = mgl(1 cosθ) L = 1 2 I θ 2 mgl(1 cosθ) Claudio Melchiorri Elements of Kinematics and Dynamics 80/ 106

83 Example - 1 Lagrangian function: from which L = I θ, θ L = 1 2 I θ 2 mgl(1 cosθ) d dt L = I θ, θ L θ = mglsinθ The generalized Lagrangian force in this case must account for the torque applied to the joint and for the friction effect: From the general expression ψ = d dt ψ = τ d θ ( L θ ) L θ we have the following second order differential equation I θ +d θ+mglsinθ = τ Claudio Melchiorri Elements of Kinematics and Dynamics 81/ 106

84 Example - 2 of a 2 dof manipulator. Consider: θ i i-th joint variable; m i i-th link mass ; Ĩ i i-th link inertia, about an axis through the CoM and parallel to z; a i i-th link length; a Ci distance between joint i and the CoM of the i-th link; τ i torque on joint i; g gravity force along y; P i, K i potential and kinetic energy of the i-th link. Claudio Melchiorri Elements of Kinematics and Dynamics 82/ 106

85 Example - 2 The dynamic model M(q) q+c(q, q) q+d q+g(q) = τ results M 11 θ1 +M 12 θ2 +c 121 θ1 θ2 +c 211 θ2 θ1 +c 221 θ2 2 +g 1 = τ 1 or M 21 θ 1 +M 22 θ 2 +c 112 θ 2 1 +g 2 = τ 2 [m 1 a 2 C1 +m 2(a 2 1 +a2 C2 +2a 1a C2 C 2 )+Ĩ 1 +Ĩ 2 ] θ 1 +[m 2 (a 2 C2 +a 1a C2 C 2 )+Ĩ 2 ] θ 2 m 2 a 1 a C2 S 2 θ 2 2 2m 2a 1 a C2 S 2 θ 1 θ 2 +(m 1 a C1 +m 2 a 1 )gc 1 +m 2 ga C2 C 12 = τ 1 [m 2 (a 2 C2 +a 1a C2 C 2 )+Ĩ2] θ 1 +[m 2 a 2 C2 +Ĩ2] θ 2 +m 2 a 1 a C2 S 2 θ2 1 +m 2 ga C2 C 12 = τ 2 Claudio Melchiorri Elements of Kinematics and Dynamics 83/ 106

86 Properties of the Euler-Lagrangian dynamic model The Euler-Lagrange dynamic model is characterized by some structural properties, concerning in particular: 1 The inertia matrix M(q); 2 The vector c(q, q) = C(q, q)q; 3 The vectors g(q) and D q; 4 Linearity with respect to the geometric/mechanical parameters. It is necessary the definition of proper vector norms in IR n n 1-norm: x 1 = x i i=1 ( n ) 1/2 2-norm: x 2 = xi 2 (Euclidean norm) i=1 ( n ) 1/p p-norm: x p = x i p -norm: i=1 x = max 1 i n x i Claudio Melchiorri Elements of Kinematics and Dynamics 84/ 106