Kinematic Model of Robot Manipulators

Transcription

1 Kinematic Model of Robot Manipulators Claudio Melchiorri Dipartimento di Ingegneria dell Energia Elettrica e dell Informazione (DEI) Università di Bologna claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Kinematic Model 1 / 164

2 Summary 1 Kinematic Model 2 Direct Kinematic Model 3 Inverse Kinematic Model 4 Differential Kinematics 5 Statics - Singularities - Inverse differential kinematics 6 Inverse kinematics algorithms 7 Measures of performance C. Melchiorri (DEIS) Kinematic Model 2 / 164

3 Kinematic Model Introduction Kinematic Model In robotics, there are two main kinematic problems: 1. Forward (direct) Kinematic Problem: once the joint position, velocity, acceleration are known, compute the corresponding variables of the end-effector in a given reference frame (e.g. a Cartesian frame). = Forward kinematic model: a function f defined between the joint space IR n and the work space IR m : x = f(q) x IR m, q IR n C. Melchiorri (DEIS) Kinematic Model 3 / 164

4 Kinematic Model Introduction Kinematic Model 2. Inverse Kinematic Problem: computation of the relevant variables (positions, velocities, accelerations) from the work space to the joint space. = Inverse Kinematic Model: function g = f 1 from IR m to IR n : q = g(x) = f 1 (x) q IR n, x IR m Some common (somehow arbitrary) definitions must be adopted for the same manipulator, different (although equivalent) kinematic models can be defined. C. Melchiorri (DEIS) Kinematic Model 4 / 164

5 Kinematic Model Introduction Kinematic Model Example: a 2 dof planar robot Forward kinematic model: Inverse kinematic model: x = l 1cosθ 1 +l 2cos(θ 1 +θ 2) y = l 1sinθ 1 +l 2sin(θ 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). C. Melchiorri (DEIS) Kinematic Model 5 / 164

6 Kinematic Model Introduction Kinematic Model Homogeneous Transformations are used for the definition of the kinematic model. A robotic manipulator is a mechanism composed by a chain of rigid bodies, the links, connected by joints. A reference frame is associated to each link, and homogeneous transformations are used to describe their relative position/orientation. C. Melchiorri (DEIS) Kinematic Model 6 / 164

7 Kinematic Model Introduction Kinematic Model A convention for the description of robots. Each link is numbered from 0 to n, in order to be univocally identified in the kinematic chain: L 0,L 1,...,L n. = Conventionally, L 0 is the base link, and L n is the final (distal) link. Each joint is numbered, from 1 to n, starting from the base joint: J 1,J 2,...,J n. = According to this convention, joint J i connects link L i 1 to link L i. A manipulator with n+1 links has n joints. C. Melchiorri (DEIS) Kinematic Model 7 / 164

8 Kinematic Model Introduction Kinematic Model The motion of the joints changes the end-effector position/orientation in the work space. The position and the orientation of the end-effector result to be a (non linear) function of the n joint variables q 1,q 2,...,q n, i.e. p = f(q 1,q 2,...,q n ) = f(q) q = [q 1 q 2...q n ] T is defined in the joint space IR n, p is defined in the work space IR m. Usually, p is composed by: some position components (e.g. x, y, z, wrt a Cartesian reference frame) some orientation components (e.g. Euler or RPY angles). C. Melchiorri (DEIS) Kinematic Model 8 / 164

9 Kinematic Model Denavit-Hartenberg Parameters Kinematic Model Need of defining a systematic and possibly unique method for the definition of the kinematic model of a robot manipulator: DENAVIT-HARTENBERG NOTATION A reference frame is assigned to each link, and homogeneous transformations matrices are used to describe the relative position/orientation of these frames. The reference frames are assigned according to a particular convention, and therefore the number of parameters needed to describe the pose of each link, and consequently of the robot, is minimized. C. Melchiorri (DEIS) Kinematic Model 9 / 164

10 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters Problem: How to assign frames to the links in order to minimize the number of parameters? Generally speaking, 6 parameters are necessary to describe the position and the orientation of a rigid body in the 3D space (a rigid body has 6 dof), and therefore 6 parameters are required to describe F b in F a. Under some hypotheses, only 4 parameters are required: the Denavit-Hartenberg Parameters. Given two reference frames F 0 and F 6 in the 3D space, 4 cases are possible: C. Melchiorri (DEIS) Kinematic Model 10 / 164

11 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters Most general case: Skew Axes. PROBLEM: Find a sequence of elementary homogeneous transformations relating two generic reference frames F 0 e F 6, with skew axes z 0 and z 6. SOLUTION: Infinite solutions are possible. It is desirable to define A sequence so that the kinematic model is defined univocally and using the minimum number of parameters. C. Melchiorri (DEIS) Kinematic Model 11 / 164

12 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure A common normal n exists among two skew z axes. Let us define: d the distance between the origin of F 0 and the intersection point of z 0 with n a the distance between z 0 and z 6 along n Apply the following sequence of translations/rotations: 1 Translate the origin of F 0 along z 0 for the quantity d: the frame F 1 is obtained 2 Rotate (ccw) F 1 about z 1 by the angle θ until x 1 is aligned with n:f 2 is obtained 3 Translate F 2 along x 2 (= n) for a: F 3 is obtained, with origin on the z 6 axis 4 Rotate (ccw) F 3 about x 3 by α, so that z 3 is aligned with z 6: F 4 is obtained 5 Translate F 4 along z 4 for the quantity b until F 6 : the frame F 5 is obtained 6 Rotate F 5 about z 5 by the angle φ: F 6 is reached C. Melchiorri (DEIS) Kinematic Model 12 / 164

13 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

14 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

15 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

16 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

17 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

18 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

19 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

20 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters - Procedure C. Melchiorri (DEIS) Kinematic Model 13 / 164

21 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters Six cyclic transformations have been employed to move from F 0 to F 6 : 3 translations and 3 rotations. There is a translation-rotation pattern: 0 T 6 = Tras(z 0,d)Rot(z 1,θ)Tras(x 2,a)Rot(x 3,α)Tras(z 4,b)Rot(z 5,φ) (1) The first 4 transformations are of particular interest: 2 couples of translations and rotations about two axes (note that z 0 = z 1 and x 2 = x 3 ): 0 H 4 = Tras(z 0,d)Rot(z 1,θ)Tras(x 2,a)Rot(x 3,α) C θ S θ C α S θ S α ac θ = S θ C θ C α C θ S α as θ 0 S α C α d C. Melchiorri (DEIS) Kinematic Model 14 / 164

22 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters Matrix 0 T 6 can be expressed in terms of H matrices by adding to (1) a null translation along x 6, obtaining the frame F 7 a null rotation about x 7, obtaining the frame F 8 Therefore we have 0 T 8 = Tras(z 0,d)Rot(z 1,θ)Tras(x 2,a)Rot(x 3,α) Tras(z 4,b)Rot(z 5,φ)Tras(x 6,0)Rot(x 7,0) that is expressed by cyclic transformations. C. Melchiorri (DEIS) Kinematic Model 15 / 164

23 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters If another frame F 12 is given, it is possible to move from F 6 to F 12 by means of a sequence similar to (1). Then, the transformation from F 0 to F 12 is 0 T 12 = 0 H 4 Tras(z 4,b)Rot(z 5,φ)Tras(z 6,d )Rot(z 7,θ )Tras(x 8,a )Rot(x 9,α ) Tras(z 10,b )Rot(z 11,φ )Tras(x 12,0)Rot(x 13,0) Since a translation and a rotation about the same axis may commute, i.e. Rot(z 5,φ)Tras(z 6,d ) = Tras(z 6,d )Rot(z 5,φ) we have that Tras(z 4,b)Rot(z 5,φ)Tras(z 6,d )Rot(z 7,θ ) = Tras(z 4,b)Tras(z 6,d )Rot(z 5,φ)Rot(z 7,θ ) = Tras(z 4,b +d )Rot(z 5,φ+θ ) C. Melchiorri (DEIS) Kinematic Model 16 / 164

24 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters In conclusion, the transformation between F 0 and F 12 is expressed by two DH transformations expressed by H matrices: the first one with parameters d,θ,a,α, the second one with parameters (b +d ),(φ+θ ),a,α (and a third one with parameters b,φ,0,0). C. Melchiorri (DEIS) Kinematic Model 17 / 164

25 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters In general, only frames F 0 and F 4 are of interest, and not the intermediate ones (F 1 -F 3 ). Therefore, F 4 will be indicated from now as F 1. The transformation 0 H 4 is then indicated as 0 H 1. 0 H 1 = Tras(z 0,d)Rot(z 1,θ)Tras(x 2,a)Rot(x 3,α) C θ S θ C α S θ S α ac θ = S θ C θ C α C θ S α as θ 0 S α C α d The frames associated to each link are used only for the definition of the kinematic model of the robot: usually their position/orientation may be freely assigned and do not depend by other constraints. Therefore, these frames are assigned in order to minimize the number of parameters required for the definition of the kinematic model. C. Melchiorri (DEIS) Kinematic Model 18 / 164

26 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters As a matter of fact, if F 0 and F 6 are two frames associated to two consecutive links, and the position and orientation of F 6 are not constrained by other considerations, it is possible to choose F 4 as the frame of the second link (NOT F 6 ), reducing in this manner to 4 the number of parameters: b and φ are not necessary. Then, the transformation between two consecutive links is 0 H 4. C. Melchiorri (DEIS) Kinematic Model 19 / 164

27 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters In conclusion: Although in general 6 parameters are necessary to specify the relative position and orientation of two frames F 0 and F 1, only 4 parameters are sufficient (d,θ,a,α) by assuming that: 1 The axis x 1 intersects z 0 2 The axis x 1 is perpendicular to z 0 These parameters are known as the Denavit-Hartenberg Parameters. C. Melchiorri (DEIS) Kinematic Model 20 / 164

28 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters Consider now a generic manipulator. L i 1, L i: consecutive links J i ed J i+1 i relative joints The motion axis of J i defines the direction of z i 1 (frame F i 1 ) associated to the proximal link z i (F i ) is aligned with the motion axis of the following joint The origin of F i is at the intersection of z i with the common normal a i between z i 1 and z i If a common normal does not exist (a i = 0), the origin of F i is placed on z i 1 If the two axes intersect, the origin is placed at the intersection If the two axes coincide, also the origins of F i 1 and F i coincide x i (F i ) is directed along the common normal y i is chosen in order to obtain a proper frame. C. Melchiorri (DEIS) Kinematic Model 21 / 164

29 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters 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 between the axis x i 1 and the common normal p i ˆo i about z i 1. C. Melchiorri (DEIS) Kinematic Model 22 / 164

30 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters The parameters a i and α i are constant and depend only on the link geometry: a i α i is the link length is the link twist angle between the joints axes. Considering the two other parameters, depending on the joint type one is constant and the other one may change in time: prismatic joint: d i is the joint variable and θ i is constant; rotational joint: θ i is the joint variable and d i is constant. C. Melchiorri (DEIS) Kinematic Model 23 / 164

31 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters The homogeneous transformation matrix relating the frames F i 1 and F i is i 1 H i = Trasl(z i 1,d i) Rot(z i 1,θ i) Trasl(x i,a i) Rot(x i,α i) = = d i C θi S θi 0 0 S θi C θi C θi S θi C αi S θi S αi a ic θi S θi C θi C αi C θi S αi a is θi 0 S αi C αi d i a i C αi S αi 0 0 S αi C αi known as Canonical Transformation. In literature, matrix i 1 H i is also indicated as i 1 A i. C. Melchiorri (DEIS) Kinematic Model 24 / 164

32 Kinematic Model Denavit-Hartenberg Parameters Denavit-Hartenberg Parameters Each matrix i 1 H i is a function of the i-th joint variable, d i or θ i depending on the joint type. For notational ease, the joint variable is generically indicated as q i, i.e.: q i = d i q i = θ i Therefore: i 1 H i = i 1 H i (q i ). for prismatic joints for rotational joints In case of a manipulator with n joints, the relationship between frame F 0 and frame F n is: 0 T n = 0 H 1 (q 1 ) 1 H 2 (q 2 )... n 1 H n (q n ) This equation expresses the position and orientation of the last link wrt the base frame, once the joint variables q 1,q 2,...,q n are known. This equation is the kinematic model of the manipulator. C. Melchiorri (DEIS) Kinematic Model 25 / 164

33 Kinematic Model Denavit-Hartenberg Parameters Reference Configuration of a Canonical Transformation A generic homogenous transformation 0 T n may be expressed as a function of n canonical transformations 0 T n = n i=1 i 1 H i If all the rotational joint variables are null, i.e. θ i = 0, and all the prismatic joints variables are at the minimum value, i.e. d j = min(d j ) (with θ j = 0), the so-called Reference Configuration for 0 T n is obtained. Note that for prismatic joints the value θ j may be imposed by the manipulator structure (and be not null). Also in these cases, it is arbitrarily considered null. A similar consideration holds also for rotational joints (θ i = 0): The reference configuration may be non physically reachable by the manipulator. C. Melchiorri (DEIS) Kinematic Model 26 / 164

34 Kinematic Model Denavit-Hartenberg Parameters Kinematic Model In the reference configuration, the matrices i 1 H i are: i 1 H i = i 1 H i θi =0 = i 1 H i = i 1 H i θi =0; d i =min(d i ) = a i 0 C αi S αi 0 0 S αi C αi d i a i 0 C αi S αi 0 0 S αi C αi min(d i) rotational joints prismatic joints The rotational part of these matrices indicates a rotation about the x i axis. Therefore, by composing all the i 1 H i, the x i axes results only translated (their orientation does not change). In this configuration, all the x i axes have the same direction (they are aligned). C. Melchiorri (DEIS) Kinematic Model 27 / 164

35 Direct Kinematic Model Procedure for assigning frames Kinematic Model of Robot Manipulators Direct Kinematic Model Claudio Melchiorri Dipartimento di Ingegneria dell Energia Elettrica e dell Informazione (DEI) Università di Bologna claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Kinematic Model 28 / 164

36 Direct Kinematic Model Procedure for assigning frames Kinematic Model A procedure to assign frames to the links of a manipulator Need of common conventions, in order to define univocally the kinematic equations. First step: definition of the base frame F 0. In this case it is usually posible to consider not only the kinematic configuration of the manipulator but also other considerations, related e.g. to the work space. However, according to the DH convention, usually F 0 is chosen so that z 0 coincides with the motion axis of J 1. F 0 =? F n =? Also F n is assigned considering not only the robot s kinematics, since a motion axis for the last link does not exist. = F 0 and F n are arbitrarily chosen! C. Melchiorri (DEIS) Kinematic Model 29 / 164

37 Direct Kinematic Model Procedure for assigning frames Kinematic Model The Denavit-Hartenberg convention does not define univocally the frames associated to the links. As a matter of fact, the frames may be assigned with some arbitrariness in the following cases: 1 F 0 : only the direction of z 0 may be univocally defined, while in general the origin o 0 and the orientation of x 0 and y 0 are not assigned; 2 F n : only x n is constrained to be perpendicular to z n 1 (i.e. to J n ). Since the joint n+1 does not exist, it is not possible to define the other elements; 3 parallel consecutive axes: it is not defined univocally the common normal line; 4 intersecting consecutive axes: the direction of x i is not defined; 5 prismatic joint: only z i is defined. In these cases, it is possible to exploit the arbitrariness in order to simplify the kinematic model, for example by posing the origin of consecutive frames in the same point, or aligning axes of consecutive frames, and so on. C. Melchiorri (DEIS) Kinematic Model 30 / 164

38 Direct Kinematic Model Procedure for assigning frames Procedure for assigning the frames The frames are assigned to the links with the following procedure: 1. The joints and links are numbered (joints from 1 to n; links from 0 to n). Links L i 1 and L i are adjacent (proximal and distal, respectively), connected by the joint J i (whose variable is q i ); 2. Definition of the axes z i, i = 0,...,n 1, aligned with the joint motion directions (rotation/translation); (N.B. z i is the motion direction of joint J i+1 : z 0 J 1 ; z 1 J 2 ;...) 3. Definition of F 0, with origin in any point of z 0, and axes x 0,y 0 properly chosen; C. Melchiorri (DEIS) Kinematic Model 31 / 164

39 Direct Kinematic Model Procedure for assigning frames Procedure for assigning the frames Steps 4-6 are repeated for i = 1,...,n 1 4. Definition of F i. Three cases are possible: a) the axes of joints J i and J i+1 have a common normal: the origin of F i is placed at the intersection point of z i with the common normal between z i 1 and z i b) the axes of J i e J i+1 intersect: the origin of F i is placed at the intersection point between z i 1 and z i c) the axes of joints J i and J i+1 are coincident or parallel: if J i is rotational, the origin of F i is chosen so that d i = 0 if J i is prismatic, the origin of F i is placed at a joint limit 5. Definition of x i along the common normal between z i 1 and z i (if exists) with positive direction from J i to J i+1 ; if z i 1 intersects z i, then the following joints are considered; 6. Definition of y i in order to obtain a proper frame. C. Melchiorri (DEIS) Kinematic Model 32 / 164

40 Direct Kinematic Model Procedure for assigning frames Procedure for assigning the frames Finally: 7. Define o n coincident with o n 1 ; 8. Define x n perpendicular to z n 1 ; 9. If J n is rotational, choose z n parallel to z n 1 ; If J n is prismatic, it is possible to choose z n freely; 10. Define y n in order to obtain a proper frame. Note that: The position of o n and its orientation z n are arbitrary In this manner, the frame F n is different wrt the frame of the end-effector t T (with axes n, s, a). Therefore, it is in general necessary to define a constant homogeneous transform matrix to take into account this difference. C. Melchiorri (DEIS) Kinematic Model 33 / 164

41 Direct Kinematic Model Procedure for assigning frames Procedure for assigning the frames Once the n frames F i (i = 1,...,n) are defined, the corresponding 4n DH parameters d i, a i, α i, θ i can be easily determined, and therefore also the matrices i 1 H i can be computed. The kinematic model is then obtained. Then: a) Define the DH Parameters Table b) Compute the homogeneous transformation matrices i 1 H i, i = 1,...,n c) Compute the direct kinematic function 0 T n = 0 H 1 1 H 2... n 1 H n C. Melchiorri (DEIS) Kinematic Model 34 / 164

42 Direct Kinematic Model Examples Example Let us consider the following 3 dof manipulator: C. Melchiorri (DEIS) Kinematic Model 35 / 164

43 Direct Kinematic Model Examples Example Step 1: Assign numbers to joints and links C. Melchiorri (DEIS) Kinematic Model 36 / 164

44 Direct Kinematic Model Examples Example Step 2: Choice of the z i axes (joint rotation/translation motion axes) C. Melchiorri (DEIS) Kinematic Model 37 / 164

45 Direct Kinematic Model Examples Example Step 3: Choice of F 0 C. Melchiorri (DEIS) Kinematic Model 38 / 164

46 Direct Kinematic Model Examples Example Steps 4 - m: Definition of F 1... F n C. Melchiorri (DEIS) Kinematic Model 39 / 164

47 Direct Kinematic Model Examples Example Finally (optional): choice of the tool frame C. Melchiorri (DEIS) Kinematic Model 40 / 164

48 Direct Kinematic Model Examples Example Let s consider a 2 dof planar manipulator: Denavit-Hartenberg parameters d θ a α L1 0 θ 1 a 1 0 o L2 0 θ 2 a 2 0 o The i 1 H i matrices result: 0 H 1 = C 1 S 1 0 a 1C 1 S 1 C 1 0 a 1S H 2 = C 2 S 2 0 a 2C 2 S 2 C 2 0 a 2S C. Melchiorri (DEIS) Kinematic Model 41 / 164

49 Direct Kinematic Model Examples Example Then 0 T 2 = 0 H 1 1 H 2 = [ n s a p ] = C 12 S 12 0 a 1 C 1 +a 2 C 12 S 12 C 12 0 a 1 S 1 +a 2 S The vectors n,s,a express the orientation of the manipulator (rotation about z 0 ), while p defines the end effector position (plane x 0 y 0 ). C. Melchiorri (DEIS) Kinematic Model 42 / 164

50 Direct Kinematic Model Examples Example z i 1 axes aligned with the motion direction of J i Note that: d i = 0: distances among common normal lines a i: distances among the joint axes J i α i: angle between z i 1 and z i about x i With the DH convention, the origin of F 2 is coincident with F 1. In this case, one obtains: C 12 S 12 0 a 1C a 2 0 S T 2 = 12 C 12 0 a 1S Then T t = C. Melchiorri (DEIS) Kinematic Model 43 / 164

51 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator Table of the Denavit-Hartenberg parameters d θ a α L1 0 θ o L2 0 θ 2 a 2 0 o L3 0 θ 3 a 3 0 o Matrices i 1 H i 0 H 1= C 1 0 S 1 0 S 1 0 C ,1 H 2= C 2 S 2 0 a 2C 2 S 2 C 2 0 a 2S ,2 H 3= C 3 S 3 0 a 3C 3 S 3 C 3 0 a 3S C. Melchiorri (DEIS) Kinematic Model 44 / 164

52 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator Kinematic model: 0 T 3 = C 1 C 23 C 1 S 23 S 1 C 1 (a 2 C 2 +a 3 C 23 ) S 1 C 23 S 1 S 23 C 1 S 1 (a 2 C 2 +a 3 C 23 ) S 23 C 23 0 a 2 S 2 +a 3 S The orientation of z 3 depends only on the first joint J 1 ; p z does not depend on θ 1. C. Melchiorri (DEIS) Kinematic Model 45 / 164

53 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator Check if the model is correct With θ 1 = θ 2 = θ 3 = 0 o 0 T 3 = a 2 +a z 0 y 0 F 0 x 0 y 3 a 2 a 3 F 3 z 3 a 3 a 2 With θ 1 = θ 2 = θ 3 = 90 o 0 T 3 = a a x 3 F 3 y 3 z 3 z 0 y 0 F 0 x 0 C. Melchiorri (DEIS) Kinematic Model 46 / 164

54 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator Another choice for the last frame could be In this case, the last frame does not respect the DH convention: = x 3 does not intersect z 2! There are two possible manners to obtain the kinematic model. C. Melchiorri (DEIS) Kinematic Model 47 / 164

55 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator There are two possible manners to obtain the kinematic model: 1) Use the previous model and add a constant rotation, in this case T = and then 0 T 3,new = 0 T 3 T = C 1 S 23 S 1 C 1 C 23 C 1 (a 2 C 2 +a 3 C 23 ) S 1 C 23 C 1 S 1 C 23 S 1 (a 2 C 2 +a 3 C 23 ) C 23 0 S 23 a 2 S 2 +a 3 S The unit vector s depends only on the first joint. The position p z does not depend on θ 1. C. Melchiorri (DEIS) Kinematic Model 48 / 164

56 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator 2) Use the DH convention by adding suitable (fictitious) i 1 H i matrices. In this case, it is necessary to add a rotation of π/2 about z and a rotation of π/2 about x and therefore the DH angles θ = α = π/2. y 2 y 3 x 3 x 3 L 3 x 3 x 2 y 3 z 3 z 2 J 3 z 3 z 3 y 3 C. Melchiorri (DEIS) Kinematic Model 49 / 164

57 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator The new DH parameters table (the joint angle θ 3 and the new angle θ are defined about the same axis and then it is possible to simply add them together): d θ a α L1 0 θ o L2 0 θ 2 a 2 0 o L3 0 θ o a 3 90 o The i 1 H i matrices are 0 H 1= C 1 0 S 1 0 S 1 0 C ,1 H 2= C 2 S 2 0 a 2C 2 S 2 C 2 0 a 2S ,2 H 3= S 3 0 C 3 a 3C 3 C 3 0 S 3 a 3S C. Melchiorri (DEIS) Kinematic Model 50 / 164

58 Direct Kinematic Model Examples Example: 3 dof anthropomorphic manipulator The kinematic model results: 0 T 3,new = C 1 S 23 S 1 C 1 C 23 C 1 (a 2 C 2 +a 3 C 23 ) S 1 C 23 C 1 S 1 C 23 S 1 (a 2 C 2 +a 3 C 23 ) C 23 0 S 23 a 2 S 2 +a 3 S The unit vector s depends only on the first joint. The position p z does not depend on θ 1. C. Melchiorri (DEIS) Kinematic Model 51 / 164

59 Direct Kinematic Model Examples Example: 3 dof spherical manipulator Denavit-Hartenberg parameters: d θ a α L1 0 θ o L2 d 2 θ o L3 d o The Denavit-Hartenberg matrices are: C 1 0 S H 1= S 1 0 C , 1 H 2= C 2 0 S 2 0 S 2 0 C d , 2 H 3= d C. Melchiorri (DEIS) Kinematic Model 52 / 164

60 Direct Kinematic Model Examples Example: 3 dof spherical manipulator The kinematic model results: 0 T 3 = [ n s a p ] = C 1 C 2 S 1 C 1 S 2 d 2 S 1 +d 3 C 1 S 2 C 2 S 1 C 1 S 1 S 2 d 2 C 1 +d 3 S 1 S 2 S 2 0 C 2 d 3 C The third joint J 3 does not affect the orientation, s depends only on J 1. C. Melchiorri (DEIS) Kinematic Model 53 / 164

61 Direct Kinematic Model Examples Example: 3 dof spherical manipulator z 3 If θ 1 = θ 2 = 0 o, d 3 = d 0 T 3 = d d z 0 y 0 x 0 F 0 d 2 x 3 d F 3 y 3 If θ 1 = θ 2 = 90 o, d 3 = d 0 T 3 = d d z 0 d 2 y 0 x 0 F 0 d x 3 F 3 y 3 z 3 C. Melchiorri (DEIS) Kinematic Model 54 / 164

62 Direct Kinematic Model Examples Example: 3 dof spherical wrist Then 3 H 4= C 4 0 S 4 0 S 4 0 C ,4 H 5= Denavit-Hartenberg parameters d θ a α L4 0 θ o L5 0 θ o L6 d 6 θ o Note the numbers starting from 4... C 5 0 S 5 0 S 5 0 C ,5 H 6= C 6 S S 6 C d C. Melchiorri (DEIS) Kinematic Model 55 / 164

63 Direct Kinematic Model Examples Example: 3 dof spherical wrist The kinematic model is: 3 T 6 = C 4 C 5 C 6 S 4 S 6 S 4 C 6 C 4 C 5 S 6 C 4 S 5 d 6 C 4 S 5 S 4 C 5 C 6 +C 4 S 6 C 4 C 6 S 4 C 5 S 6 S 4 S 5 d 6 S 4 S 5 S 5 C 6 S 5 S 6 C 5 d 6 C In this case, the rotation matrix has the same expression as an Euler ZYZ rotation matrix. R Euler (φ,θ,ψ) = Rot(z 0,φ)Rot(y 1,θ)Rot(z 2,ψ) = C φc θ C ψ S φ S ψ C φ C θ S ψ S φ C ψ C φ S θ S φ C θ C ψ +C φ S ψ S φ C θ S ψ +C φ C ψ S φ S θ S θ C ψ S θ S ψ C θ It means that the manipulator s joints θ 4,θ 5 and θ 6 are equivalent to the Euler ZYZ angles. C. Melchiorri (DEIS) Kinematic Model 56 / 164

64 Direct Kinematic Model Examples Example: 3 dof spherical wrist If θ 1 = θ 2 = θ 3 = 0 o 3 T 6 = d If θ 1 = θ 2 = θ 3 = 90 o 3 T 6 = d C. Melchiorri (DEIS) Kinematic Model 57 / 164

65 Direct Kinematic Model Examples Example: Stanford manipulator By composing the 3 dof spherical manipulator with the spherical wrist, the so-called Stanford manipulator is obtained, a 6 dof robot. Since the frames have been defined in a consistent manner, the kinematic model is simply obtained by multiplying the matrices 0 T 3 of the arm and 3 T 6 of the wrist. Then 0 T 6 = 0 T 3 3 T 6 = [ n s a p ] C. Melchiorri (DEIS) Kinematic Model 58 / 164

66 Direct Kinematic Model Examples Example: Stanford manipulator where n = o = S 1(S 4C 5C 6 +C 4S 6)+C 1(C 2(C 4C 5C 6 S 4S 6) S 2S 5C 6) C 1(S 4C 5C 6 +C 4S 6)+S 1( S 2S 5C 6 +C 2(C 4C 5C 6 S 4S 6)) C 2S 5C 6 S 2(C 4C 5C 6 S 4S 6) S 1(C 4C 6 S 4C 5S 6)+C 1( C 2(S 4C 6 +C 4C 5S 6)+S 2S 5S 6) C 1(C 4C 6 S 4C 5S 6)+S 1(S 2S 5S 6 C 2(S 4C 6 +C 4C 5S 6)) C 2S 5S 6 +S 2(S 4C 6 +C 4C 5S 6) p = a = S 1S 4S 5 +C 1(S 2C 5 +C 2C 4S 5) C 1S 4S 5 +S 1(S 2C 5 +C 2C 4S 5) C 2C 5 S 2C 4S 5 d 2S 1 +d 3C 1S 2 +d 6(C 1S 2C 5 +C 1C 2C 4S 5 S 1S 4S 5) d 2C 1 +d 3S 1S 2 +d 6(S 1S 2C 5 +S 1C 2C 4S 5 +C 1S 4S 5) d 3C 2 +d 6(C 2C 5 S 2C 4S 5) C. Melchiorri (DEIS) Kinematic Model 59 / 164

67 Direct Kinematic Model Examples Example: PUMA 260 Joint variables θ i are defined about the z i 1 axes; a 2 is the distance between z 1 and z 2 (in this case parallel), d 3 is the offset between the origins of F 2 and F 3, and d 4 is the offset between the origins of F 3 and F 4. Frames F 4 and F 5 coincides. The α i angles are either 0 o or ±90 o. d θ a α L1 0 θ o L2 0 θ 2 a 2 0 o L3 d 3 θ o L4 d 4 θ o L5 0 θ o L6 d 6 θ o C. Melchiorri (DEIS) Kinematic Model 60 / 164

68 Direct Kinematic Model Examples Example: PUMA 260 Canonical transformation matrices: 0 H 1= 3 H 4= C 1 0 S 1 0 S 1 0 C C 4 0 S 4 0 S 4 0 C d ,1 H 2=,4 H 5= C 2 S 2 0 a 2C 2 S 2 C 2 0 a 2S C 5 0 S 5 0 S 5 0 C ,2 H 3=,5 H 6= C 3 0 S 3 0 S 3 0 C d C 6 S S 6 C d C. Melchiorri (DEIS) Kinematic Model 61 / 164

69 Direct Kinematic Model Examples Example: PUMA T 3 = C 1 C 2 C 3 C 1 S 2 S 3 S 1 C 1 C 3 S 2 +C 1 C 2 S 3 a 2 C 1 C 2 d 3 S 1 C 2 C 3 S 1 S 1 S 2 S 3 C 1 C 3 S 1 S 2 +C 2 S 1 S 3 C 1 d 3 +a 2 C 2 S 1 C 3 S 2 +C 2 S 3 0 (C 2 C 3 )+S 2 S 3 a 2 S T 6 = C 4 C 5 C 6 S 4 S 6 (C 6 S 4 ) C 4 C 5 S 6 C 4 S 5 C 4 d 6 S 5 C 5 C 6 S 4 +C 4 S 6 C 4 C 6 C 5 S 4 S 6 S 4 S 5 d 6 S 4 S 5 (C 6 S 5 ) S 5 S 6 C 5 d 4 +C 5 d T 6 = 0 T 6 = 0 T 3 3 T 6 = [ n s a p ] C. Melchiorri (DEIS) Kinematic Model 62 / 164

70 Direct Kinematic Model Examples Example: PUMA 260 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 )) ] [ ] S1 (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 ) ] C. Melchiorri (DEIS) Kinematic Model 63 / 164

71 Direct Kinematic Model Examples Example: planar 4 dof manipulator (redundant) DH parameters d θ a α L1 0 θ 1 a 1 0 o L2 0 θ 2 a 2 0 o L3 0 θ 3 a 3 0 o L4 0 θ 4 a 4 0 o All the i 1 H i matrices have the same structure C i S i 0 a i C i i 1 H i = S i C i 0 a i S i C. Melchiorri (DEIS) Kinematic Model 64 / 164

72 Direct Kinematic Model Examples Example: planar 4 dof manipulator (redundant) Then 0 T 4 = 0 H 1 1 H 2 2 H 3 3 H 4 = = [ n s a p C 1234 S a 1 C 1 +a 2 C 12 +a 3 C 123 +a 4 C 1234 S 1234 C a 1 S 1 +a 2 S 12 +a 3 S 123 +a 4 S The vectors n, s, a define the end-effector orientation (rotation about z), while p defines its position (on the x y plane, p z = 0). = The procedure can be applied also to redundant manipulators. ] C. Melchiorri (DEIS) Kinematic Model 65 / 164

73 Inverse Kinematic Model Kinematic Model of Robot Manipulators Inverse Kinematic Model Claudio Melchiorri Dipartimento di Ingegneria dell Energia Elettrica e dell Informazione (DEI) Università di Bologna claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Kinematic Model 66 / 164

74 Inverse Kinematic Model Introduction Inverse Kinematic Model Direct Kinematic Model: The direct kinematic model consists in a function f(q) mapping the joint position variables q IR n to the position/orientation of the end effector. The definition of f(q) is conceptually simple, and a general approach for its computation has been defined. C. Melchiorri (DEIS) Kinematic Model 67 / 164

75 Inverse Kinematic Model Introduction Inverse Kinematic Model Inverse Kinematic Model: The inverse kinematics consists in finding a function g(x) mapping the position/orientation of the end-effector to the corresponding joint variables q: the problem is not simple! A general approach for the solution of this problem does not exist On the other hand, for the most common kinematic structures, a scheme for obtaining the solution has been found. Unfortunately The solution is not unique. In general we have: No solution (e.g. starting with a position x not in the workspace); A finite set of solutions (one or more); Infinite solutions. We seek for closed form solutions, and not based on numerical techniques: The analytic solution is more efficient from the computational point of view; If the solutions are known analytically, it is possible to select one of them on the basis of proper criteria. C. Melchiorri (DEIS) Kinematic Model 68 / 164

76 Inverse Kinematic Model Introduction Inverse Kinematic Model In order to obtain a closed form solution to the inverse kinematic problem, two approaches are possible: An algebraic approach, i.e. elaborations of the kinematic equations until a suitable set of (simple) equations is obtained for the solution A geometric approach based, when possible, on geometrical considerations, dependent on the kinematic structure of the manipulator and that may help in the solution. C. Melchiorri (DEIS) Kinematic Model 69 / 164

77 Inverse Kinematic Model Algebraic Approach Algebraic Approach For a 6 dof manipulator, the kinematic model is described by the equation 0 T 6 = 0 H 1(q 1) 1 H 2(q 2)... 5 H 6(q 6) equivalent to 12 equations in the 6 unknowns q i, i = 1,...,6. Example: spherical manipulator (only 3 dof) T= = C 1C 2 S 1 C 1S 2 d 2S 1 + d 3C 1S 2 C 2S 1 C 1 S 1S 2 d 2C 1 + d 3S 1S 2 S 2 0 C 2 d 3C Since both the numerical values of 0 T 6 and the structure of the i 1 H i matrices are known, by suitable pre- / post-multiplications it is possible to obtain [ 0 H 1(q 1)... i 1 H i(q i)] 1 0 T 6 [ j H j+1(q j+1)... 5 H 6(q 6)] 1 = i H i+1(q i+1)... j 1 H j(q j), i < j obtaining 12 new equations for each couple (i,j), i < j. By selecting the most simple equations among all those obtained, it might be possible to obtain a solution to the problem. C. Melchiorri (DEIS) Kinematic Model 70 / 164

78 Inverse Kinematic Model Geometric Approach Geometric Approach General considerations that may help in finding solutions with algebraic techniques do not exist, being these strictly related to the mathematical expression of the direct kinematic model. On the other hand, it is often possible to exploit considerations related to the geometrical structure of the manipulator. PIEPER APPROACH Many industrial manipulators have a kinematically decoupled structure, for which it is possible to decompose the problem in two (simpler) sub-problems: 1 Inverse kinematics for the position (x,y,z) q 1,q 2,q 3 2 Inverse kinematics for the orientation R q 4,q 5,q 6. C. Melchiorri (DEIS) Kinematic Model 71 / 164

79 Inverse Kinematic Model Geometric Approach Geometric Approach PIEPER APPROACH: Given a 6 dof manipulator, sufficient condition to find a closed form solution for the IK problem is that the kinematic structure presents: or three consecutive rotational joints with axes intersecting in a single point three consecutive rotational joints with parallel axes. C. Melchiorri (DEIS) Kinematic Model 72 / 164

80 Inverse Kinematic Model Geometric Approach Geometric Approach In many 6 dof industrial manipulators, the first 3 dof are usually devoted to position the wrist, that has 3 additional dof give the correct orientation to the end-effector. In these cases, it is quite simple to decompose the IK problem in the two sub-problems (position and orientation). C. Melchiorri (DEIS) Kinematic Model 73 / 164

81 Inverse Kinematic Model Geometric Approach Geometric Approach In case of a manipulator with a spherical wrist, a natural choice is to decompose the problem in 1 IK for the point p p (center of the spherical wrist) 2 solution of the orientation IK problem Therefore: 1 The point p p is computed since 0 T 6 is known (submatrices R and p): p p = p d 6 a p p depends only on the joint variables q 1,q 2,q 3 ; 2 The IK problem is solved for q 1,q 2,q 3 ; 3 The rotation matrix 0 R 3 related to the first three joints is computed; 4 The matrix 3 R 6 = 0 R T 3 R is computed; 5 The IK problem for the rotational part is solved (Euler). C. Melchiorri (DEIS) Kinematic Model 74 / 164

82 Inverse Kinematic Model Examples Solution of the spherical manipulator Direct kinematic model: [ ] 0 n s a p T 3 = = C 1 C 2 S 1 C 1 S 2 d 2 S 1 +d 3 C 1 S 2 C 2 S 1 C 1 S 1 S 2 d 2 C 1 +d 3 S 1 S 2 S 2 0 C 2 d 3 C If the whole matrix 0 T 3 is known, it is possible to compute: = θ 1 = atan2( s x,s y ) θ 2 = atan2( n z,a z ) d 3 = p z /cosθ 2 Unfortunately, according to the Pieper approach only p is known! C. Melchiorri (DEIS) Kinematic Model 75 / 164

83 Inverse Kinematic Model Examples Solution of the spherical manipulator We known only the position vector p. From 0 T 3 = 0 H 1 1 H 2 2 H 3 wehave ( 0 H 1) 1 0 T 3 = C 1 S S 1 C n x s x a x p x n y s y a y p y n z s z a z p z = C 2 0 S 2 d 3S 2 S 2 0 C 2 d 3C d = 1 H 2 2 H 3 C. Melchiorri (DEIS) Kinematic Model 76 / 164

84 Inverse Kinematic Model Examples Solution of the spherical manipulator By equating the position vectors, 1 p p = p xc 1 +p y S 1 p z p x S 1 +p y C 1 = d 3S 2 d 3 C 2 d 2 The vector 1 p p depends only on θ 2 and d 3! Let s define a = tanθ 1 /2, then C 1 = 1 a2 1+a 2 S 1 = 2a 1+a 2 By substitution in the last element of 1 p p (d 2 +p y)a 2 +2p xa+d 2 p y = 0 = a = px ± p 2 x +p 2 y d 2 2 d 2 +p y Two possible solutions! ((p 2 x +p2 y d2 2 ) > 0??) Then θ 1 = 2 atan2( p x ± px 2 +py 2 d2 2, d 2 +p y ) C. Melchiorri (DEIS) Kinematic Model 77 / 164

85 Inverse Kinematic Model Examples Solution of the spherical manipulator Vector 1 p p is defined as 1 p p = From the first two elements p xc 1 +p y S 1 p z p x S 1 +p y C 1 = d 3S 2 d 3 C 2 d 2 from which p x C 1 +p y S 1 p z = d 3S 2 d 3 C 2 θ 2 = atan2(p x C 1 +p y S 1, p z ) Finally, if the first two elements are squared and added together d 3 = (p x C 1 +p y S 1 ) 2 +pz 2 C. Melchiorri (DEIS) Kinematic Model 78 / 164

86 Inverse Kinematic Model Examples Solution of the spherical manipulator Note that two possible solutions exist considering the position of the end-effector (wrist) only. If also the orientation is considered, the solution (if exists) is unique. We have seen that the relation (px 2 +p2 y d2 2 ) > 0 must hold: C. Melchiorri (DEIS) Kinematic Model 79 / 164

87 Inverse Kinematic Model Examples Solution of the spherical manipulator Numerical example: Given a spherical manipulator with d 2 = 0.8 m in the pose θ 1 = 20 o, θ 2 = 30 o, d 3 = 0.5 m we have 0 T 3 = The solution based on the whole matrix 0 T 3 is: θ 1 = 20 o, θ 2 = 30 o, d 3 = 0.5. The solution based on the vector p gives: a) θ 1 = 20 o, θ 2 = 30 o, d 3 = 0.5 (with the + sign). b) θ 1 = 14.7 o, θ 2 = 30 o, d 3 = 0.5 (with the - sign). C. Melchiorri (DEIS) Kinematic Model 80 / 164

88 Inverse Kinematic Model Examples Solution of the spherical manipulator The solution based on the vector p gives: a) θ 1 = 20 o, θ 2 = 30 o, d 3 = 0.5 (with the + sign). b) θ 1 = 14.7 o, θ 2 = 30 o, d 3 = 0.5 (with the - sign). C. Melchiorri (DEIS) Kinematic Model 81 / 164

89 Inverse Kinematic Model Examples Solution of the 3 dof anthropomorphic manipulator From the kinematic structure, one obtains θ 1 = atan2(p y, p x ) Concerning θ 2 and θ 3, note that the arm moves in a plane defined by θ 1 only. One obtains C 3 = p2 x +py 2 +pz 2 a2 2 a3 2 S 3 = ± 1 C3 2 2a 2a 3 θ 3 = atan2(s 3, C 3) Moreover, by geometrical arguments, it is possible to state that: θ 2 = atan2(p z, px 2 +py 2 ) atan2(a 3S 3, a 2 +a 3C 3) C. Melchiorri (DEIS) Kinematic Model 82 / 164

90 Inverse Kinematic Model Examples Solution of the 3 dof anthropomorphic manipulator Note that also the solution is valid θ 1 = π+atan2(p y, p x ), θ 2 = π θ 2 Then, FOUR possible solutions exist: shoulder right - elbow up; shoulder left - elbow up; Same position, but different orientation! shoulder right - elbow down; shoulder left - elbow down; Note that the conditions p x 0, p y 0 must hold (singular configuration). C. Melchiorri (DEIS) Kinematic Model 83 / 164

91 Inverse Kinematic Model Examples Solution of the spherical wrist Let us consider the matrix 3 R 6 = n x s x a x n y s y a y n z s z a z From the direct kinematic equations one obtains C 4C 5C 6 S 4S 6 S 4C 6 C 4C 5S 6 C 4S 5 3 R 6 = S 4C 5C 6 +C 4S 6 C 4C 6 S 4C 5S 6 S 4S 5 S 5C 6 S 5S 6 C 5 C. Melchiorri (DEIS) Kinematic Model 84 / 164

92 Inverse Kinematic Model Examples Solution of the spherical wrist 3 R 6 = [ C4C 5C 6 S 4S 6 S 4C 6 C 4C 5S 6 C 4S 5 S 4C 5C 6 +C 4S 6 C 4C 6 S 4C 5S 6 S 4S 5 S 5C 6 S 5S 6 C 5 The solution is then computed as (ZYZ Euler angles): θ 5 [0,π]: θ 5 [ π,0]: θ 4 = atan2(a y, a x) = atan2( ax 2 +ay, 2 a z) θ 5 θ 6 = atan2(s z, n z) θ 4 = atan2( a y, a x) = atan2( ax 2 +ay, 2 a z) θ 5 θ 6 = atan2( s z, n z) ] C. Melchiorri (DEIS) Kinematic Model 85 / 164

93 Inverse Kinematic Model Examples Solution of the spherical wrist Numerical example: Let us consider a spherical wrist in the pose Then θ 4 = 10 o θ 5 = 20 o, θ 6 = 30 o 3 R 6 = [ ] Therefore, if θ 5 [0, π] θ 4 = 10 o θ 5 = 20 o, θ 6 = 30 o θ 5 [ π, 0] θ 4 = 170 o θ 5 = 20 o, θ 6 = 30 o Note that the PUMA has an anthropomorphic structure (4 solutions) and a spherical wrist (2 solutions): = 8 different configurations are possible! C. Melchiorri (DEIS) Kinematic Model 86 / 164

94 Differential Kinematics Kinematic Model of Robot Manipulators Differential Kinematics Claudio Melchiorri Dipartimento di Ingegneria dell Energia Elettrica e dell Informazione (DEI) Università di Bologna claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Kinematic Model 87 / 164

95 Differential Kinematics Introduction Differential Kinematics: the Jacobian matrix In robotics it is of interest to define, besides the mapping between the joint and workspace position/orientation (i.e. the kinematic equations), also The relationship between the joints and end-effector velocities: [ ] v q ω The relationship between the force applied on the environment by the manipulator and the corresponding joint torques [ ] f τ n These two relationships are based on a linear operator, a matrix J, called the Jacobian of the manipulator. C. Melchiorri (DEIS) Kinematic Model 88 / 164

96 Differential Kinematics Introduction The Jacobian matrix In robotics, the Jacobian is used for several purposes: To define the relationship between joint and workspace velocities To define the relationship between forces/torques between the spaces To study the singular configurations To define numerical procedures for the solution of the IK problem To study the manipulability properties. C. Melchiorri (DEIS) Kinematic Model 89 / 164

97 Differential Kinematics The Jacobian Matrix Velocity domain The translational and rotational velocities are considered separately. Let us consider two frames F 0 (base frame) and F 1 (integral with the rigid body). The translational velocity of point p of the rigid body, with respect to F 0, is defined as the derivative (with respect to time) of p, denoted as ṗ: ṗ = d p dt C. Melchiorri (DEIS) Kinematic Model 90 / 164

98 Differential Kinematics The Jacobian Matrix Velocity domain With respect to the rotational velocity, two different definitions are possible: 1 A triple γ IR 3 giving the orientation of F 1 with respect to F 0 (Euler, RPY,... angles) is adopted, and its derivative is used to define the rotational velocity γ γ = dγ dt 2 An angular velocity vector ω is defined, giving the rotational velocity of a third frame F 2 with origin coincident with F 0 and axes parallel to F 1. The velocity vector ω is placed in the origin, and its direction coincides with the instantaneous rotation axis of the rigid body. C. Melchiorri (DEIS) Kinematic Model 91 / 164

99 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian The two descriptions lead to different results concerning the expression of the Jacobian matrix, in particular in the part relative to the rotational velocity. One obtains respectively the: Two problems: Analytic Jacobian J A Geometric Jacobian J G 1) Integration of the rotational velocity γdt = γ: orientation of the rigid body ωdt =?? C. Melchiorri (DEIS) Kinematic Model 92 / 164

100 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian Example: Let s consider a rigid body and the following rotational velocities Case a) ω = [π/2, 0, 0] T 0 t 1 ω = [0, π/2, 0] T 1 < t 2 Case b) ω = [0, π/2, 0] T 0 t 1 ω = [π/2, 0, 0] T 1 < t 2 By integrating the velocities in the two cases, one obtains: 2 0 ωdt = [π/2, π/2, 0] T C. Melchiorri (DEIS) Kinematic Model 93 / 164

101 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian Case a) ω = [π/2, 0, 0] T 0 t 1 ω = [0, π/2, 0] T 1 < t 2 Case b) ω = [0, π/2, 0] T 0 t 1 ω = [π/2, 0, 0] T 1 < t ωdt = [π/2, π/2, 0] T On the other hand, the rotation matrices in the two cases are: R a = R b = The integration of ω does not have a clear physical interpretation. C. Melchiorri (DEIS) Kinematic Model 94 / 164

102 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian 2) On the other hand: ω represents the velocity components about the three axes of F 0, the elements of γ are defined with respect to frame that: a) is not Cartesian (its axes are not orthogonal each other); b) varies in time according to γ. C. Melchiorri (DEIS) Kinematic Model 95 / 164

103 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian The two expressions of the Jacobian matrix physically define the same phenomenon (velocity of the manipulator), and therefore a relationship between them must exist. For example, if the Euler angles φ,θ,ψ are used for the triple γ, it is possible to show that ω = 0 sinφ cosφsinθ 0 cosφ sinφsinθ 1 0 cos θ γ = T(γ) γ Note that matrix T(γ) is singular when sinθ = 0. In this case, some rotational velocities may be expressed by ω and not by γ, e.g. ω = [cosφ, sinφ, 0] T. These cases are called representation singularities of γ. C. Melchiorri (DEIS) Kinematic Model 96 / 164

104 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian Definition of matrix T(γ): φ : [ω x, ω y, ω z] T = φ[ ] ω z = φ θ : [ω x, ω y, ω z] T = θ [ Sφ C φ 0 ] { ωx = S φ θ ω y = C φ θ ψ : [ω x, ω y, ω z] T = ψ [ Cφ S θ S φ S θ C θ ] ω z = C θ ψ ω x = S θ C φ ψ ω y = S θ S φ ψ C. Melchiorri (DEIS) Kinematic Model 97 / 164

105 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian If sinθ = 0, then the components perpendicular to z of the velocity expressed by γ are linearly dependent (ω 2 x +ω 2 y = θ 2 ), while physically this constraint may not exist! From: one obtains: 0 S φ 0 0 C φ φ θ ψ ω = = 0 sinφ cosφsinθ 0 cosφ sinφsinθ 1 0 cos θ S φ θ C φ θ φ+ ψ = ω x ω y ω z γ { ω 2 = x +ωy 2 = θ 2 ω z = φ+ ψ C. Melchiorri (DEIS) Kinematic Model 98 / 164

106 Differential Kinematics The Jacobian Matrix Analytical and Geometrical expressions of the Jacobian In general, given a triple of angles γ, a transformation matrix T(γ) exists such that ω = T(γ) γ Once matrix T(γ) is known, it is possible to relate the analytical and geometrical expressions of the Jacobian matrix: [ ] [ ][ ] v I 0 ṗ = ω 0 T(γ) γ Then J G = [ I 0 0 T(γ) ] J A = T A (γ)j A C. Melchiorri (DEIS) Kinematic Model 99 / 164

107 Differential Kinematics Analytical Jacobian Analitycal Jacobian The analytical expression of the Jacobian is obtained by differentiating a vector x = f(q) IR 6, that defines the position and orientation (according to some convention) of the manipulator in F 0. By differentiating f(q), one obtains where the m n matrix J(q) = f(q) q = dx = f(q) q dq f 1 q 1 f 1... f m q 1 = J(q)dq q 2... f m q 2... f 1 q n f m q n is called Jacobian matrix or JACOBIAN of the manipulator. J(q) IR m n C. Melchiorri (DEIS) Kinematic Model 100 / 164

108 Differential Kinematics Analytical Jacobian Analitycal Jacobian If the infinitesimal period of time dt is considered, on obtains that is d x dt = J(q) d q dt [ ] ẋ = = J(q) q v γ relating the velocity vector ẋ expressed in F 0 and the joint velocity vector q. The elements J i,j of the Jacobian are non linear functions of q(t): therefore these elements change in time according to the value of the joint variables. The Jacobian dimensions depend on the number n of joints and on the dimension m of the considered operative space (J(q) IR m n ). C. Melchiorri (DEIS) Kinematic Model 101 / 164

109 Differential Kinematics Analytical Jacobian Example: 2 dof manipulator d θ a α L1 0 θ 1 a 1 0 o L2 0 θ 2 a 2 0 o The end-effector position is p x = a 1 C 1 +a 2 C 12 p y = a 1 S 1 +a 2 S 12 p z = 0 If γ is composed by the Euler angles φ,θ,ψ defined about axes z 0,y 1,z 2, and considering that the z axes of the base frame and of the end effector are parallel, the total rotation is equivalent to a single rotation about z 0 and therefore: φ θ ψ = θ 1 +θ C. Melchiorri (DEIS) Kinematic Model 102 / 164

110 Differential Kinematics Analytical Jacobian Example: 2 dof manipulator Euler angles: φ θ ψ = θ 1 +θ By differentiation of the expressions of p and γ one obtains a 1 S 1 a 2 S 12 a 2 S 12 [ ] a 1 C 1 +a 2 C 12 a 2 C 12 ṗ = 0 0 γ 1 1 q = J(q) q C. Melchiorri (DEIS) Kinematic Model 103 / 164

111 Differential Kinematics Geometric Jacobian Geometric Expression of the Jacobian The geometric expression of the Jacobian is obtained considering the rotational velocity vector ω. Each column of the Jacobian matrix defines the effect of the i-th joint on the end-effector velocity. It is divided in two terms. The first term considers the effect of q i on the linear velocity v, while the second one on the [ rotational ] velocity ω, i.e. [ ] v Jv1 J = J q = J = v2... J vn ω J ω1 J ω2... J ωn Therefore v = J v1 q 1 +J v2 q J vn q n ω = J ω1 q 1 +J ω2 q J ωn q n The analytic and geometric Jacobian differ for the rotational part. In order to obtain the geometric Jacobian, a general method based on the geometrical structure of the manipulator is adopted. C. Melchiorri (DEIS) Kinematic Model 104 / 164

112 Differential Kinematics Geometric Jacobian Derivative of a Rotation Matrix Let s consider a rotation matrix R = R(t) and R(t)R T (t) = I. Let s derive the equation: R(t)R T (t) = I = Ṙ(t)RT (t)+r(t)ṙt (t) = 0 A 3 3 (skew-symmetric) matrix S(t) is obtained As a matter of fact Then S(t) = Ṙ(t)RT (t) S(t)+S T (t) = 0 = S = Ṙ(t) = S(t)R(t) 0 ω z ω y ω z 0 ω x ω y ω x 0 This means that the derivative of a rotation matrix is expressed as a function of the matrix itself. C. Melchiorri (DEIS) Kinematic Model 105 / 164

113 Differential Kinematics Geometric Jacobian Derivative of a Rotation Matrix Physical interpretation: Matrix S(t) is expressed as a function of a vector ω(t) = [ω x,ω y,ω z ] T representing the angular velocity of R(t). Consider a constant vector p and the vector p(t) = R(t)p. The time derivative of p(t) is ṗ(t) = Ṙ(t)p which can be written as ṗ(t) = S(t)R(t)p = ω (R(t) p ) This last results, i.e. ṗ(t) = ω (R(t) p ), is well known from the classic mechanics of rigid bodies. C. Melchiorri (DEIS) Kinematic Model 106 / 164

114 Differential Kinematics Geometric Jacobian Derivative of a Rotation Matrix Moreover R S(ω) R T = S(R ω) i.e. the matrix form of S(ω) in a frame rotated by R is the same as the skew-symmetric matrix S(R ω) of the vector ω rotated by R. Consider two frames F and F, which differ by the rotation R (ω = R ω). Then S(ω ) operates on vectors in F and S(ω) on vectors in F. Consider a vector v a in F and assume that some operations must be performed on that vector in F (then using S). It is necessary to: 1 Transform the vectors from F to F (by R T ) 2 Use S(ω) 3 Transform back the result to F (by R) That is: equivalent to the transformation using S(ω ): v b = R S(ω) RT v a v b = S(ω ) v a C. Melchiorri (DEIS) Kinematic Model 107 / 164

115 Differential Kinematics Geometric Jacobian Example Consider the elementary rotation about z cosθ sinθ 0 Rot(z,θ) = sinθ cosθ If θ is a function of time θsinθ θcosθ 0 S(t)= θcosθ θsinθ cosθ sinθ 0 sinθ cosθ = 0 θ 0 θ = S(ω(t)) Then ω = 0 0 θ i.e. a rotational velocity about z. C. Melchiorri (DEIS) Kinematic Model 108 / 164

116 Differential Kinematics Geometric Jacobian Geometric Jacobian The end-effector velocity is a linear composition of the joint velocities v = J v1 q 1 +J v2 q J vn q n ω = J ω1 q 1 +J ω2 q J ωn q n Each column of the Jacobian matrix defines the effect of the i-th joint on the end-effector velocity. C. Melchiorri (DEIS) Kinematic Model 109 / 164

117 Differential Kinematics Geometric Jacobian Geometric Jacobian It is possible to show (see Appendix) that the i-th column of the Jacobian can be computed as [ ] [ 0 ] Jvi z = i 1 ( 0 p n 0 p i 1 ) J 0 revolute joint ωi z i 1 [ ] [ 0 ] Jvi z = i 1 prismatic joint J ωi 0 where 0 z i 1 and 0 r i 1,n = 0 p n 0 p i 1 depend on the joint variables q 1,q 2,...,q n. In particular: 0 p n is the end-effector position, defined in the first three elements of the last column of 0 T n = 0 H 1 (q 1 )... n 1 H n (q n ); 0 p i 1 is the position of F i 1, defined in the first three elements of the last column of 0 T i 1 = 0 H 1 (q 1 )... i 2 H i 1 (q i 1 ); 0 z i 1 is the third column of 0 R i 1 : 0 R i 1 = 0 R 1 (q 1 ) 1 R 2 (q 2 )... i 2 R i 1 (q i 1 ) C. Melchiorri (DEIS) Kinematic Model 110 / 164

118 Differential Kinematics Examples Example: 2 dof manipulator The Jacobian is computed as [ ] z0 (p J = 2 p 0 ) z 1 (p 2 p 1 ) z 0 z 1 The origins of the frames are p 0 = p 1 = a 1C 1 a 1 S 1 0 p 2 = a 1C 1 +a 2 C 12 a 1 S 1 +a 2 S 12 0 and the rotational axes are z 0 = z 1 = C. Melchiorri (DEIS) Kinematic Model 111 / 164

119 Differential Kinematics Examples Example: 2 dof manipulator Then z 0 (p 2 p 0) = = z 1 (p 2 p 1) = 0 0 a 1S 1 +a 2S a 1C 1 a 2C 12 a 1S 1 a 2S 12 a 1C 1 +a 2C 12 0 a 1S 1 a 2S 12 a 1C 1 +a 2C a 2S a 2C 12 a 2S 12 a 2C = a 2S 12 a 2C In conclusion: J(q) = a 1S 1 a 2S 12 a 2S 12 a 1C 1 +a 2C 12 a 2C C. Melchiorri (DEIS) Kinematic Model 112 / 164

120 Differential Kinematics Examples Example: 2 dof manipulator Jacobian: J(q) = a 1S 1 a 2S 12 a 2S 12 a 1C 1 +a 2C 12 a 2C If the orientation is not of interest, only the first two rows may be considered [ ] a1 S J(q) = 1 a 2 S 12 a 2 S 12 a 1 C 1 +a 2 C 12 a 2 C 12 C. Melchiorri (DEIS) Kinematic Model 113 / 164

121 Differential Kinematics Examples Example: 3dof anthropomorphic manipulator and the kinematic model 0 T 3 = The canonical transformation matrices are C 1 0 S H 1= S 1 0 C H 2= H 3 = C 3 S 3 0 a 3C 3 S 3 C 3 0 a 3S C 1 C 23 C 1 S 23 S 1 C 1 (a 2 C 2 +a 3 C 23 ) S 1 C 23 S 1 S 23 C 1 S 1 (a 2 C 2 +a 3 C 23 ) S 23 C 23 0 a 2 S 2 +a 3 S C 2 S 2 0 a 2C 2 S 2 C 2 0 a 2S C. Melchiorri (DEIS) Kinematic Model 114 / 164

122 Differential Kinematics Examples Example: 3dof anthropomorphic manipulator The Jacobian results [ ] z0 (p J = 3 p 0 ) z 1 (p 3 p 1 ) z 2 (p 3 p 2 ) z 0 z 1 z 2 where p 0 = p 1 = p 2 = a 2C 1 C 2 a 2 S 1 S 2 a 2 S 2 p 3 = C 1(a 2 C 2 +a 3 C 23 ) S 1 (a 2 C 2 +a 3 C 23 ) a 2 S 2 +a 3 S 23 The rotational axes are z 0 = z 1 = z 2 = S 1 C 1 0 C. Melchiorri (DEIS) Kinematic Model 115 / 164

123 Differential Kinematics Examples Example: 3dof anthropomorphic manipulator Therefore J = S 1 (a 2 C 2 +a 3 C 23 ) C 1 (a 2 S 2 +a 3 S 23 ) a 3 C 1 S 23 C 1 (a 2 C 2 +a 3 C 23 ) S 1 (a 2 S 2 +a 3 S 23 ) a 3 S 1 S 23 0 a 2 C 2 +a 3 C 23 a 3 C 23 0 S 1 S 1 0 C 1 C Only three rows are linearly independent (3 dof). - Note that it is not possible to achieve all the rotational velocities ω in IR 3. - Moreover S 1 ω y = C 1 ω x (ω x = S 1 θ2 +S 1 θ3, ω y = C 1 θ2 C 1 θ3, ). By considering the linear velocity only, one obtains: J = S 1(a 2 C 2 +a 3 C 23 ) C 1 (a 2 S 2 +a 3 S 23 ) a 3 C 1 S 23 C 1 (a 2 C 2 +a 3 C 23 ) S 1 (a 2 S 2 +a 3 S 23 ) a 3 S 1 S 23 0 a 2 C 2 +a 3 C 23 a 3 C 23 C. Melchiorri (DEIS) Kinematic Model 116 / 164

124 Differential Kinematics Examples Example: 3dof anthropomorphic manipulator Note that: θ 1 does not affect v z (nor ω x, ω y ) ω z depends only by θ 1 S 1 ω y = C 1 ω x : ω x and ω y are not independent the first three rows may also be obtained by derivation of 0 p 3 In the linear velocity case (i.e. the first three rows only) Therefore det(j) = 0 in two cases: { 0 S 3 = 0 = θ 3 = π det(j) = a 2 a 3 S 3 (a 2 C 2 +a 3 C 23 ) (a 2 C 2 +a 3 C 23 ) = 0 i.e. when the wrist is on the z axis (p x = p y = 0): shoulder singularity C. Melchiorri (DEIS) Kinematic Model 117 / 164

125 Differential Kinematics Examples Example 3 dof spherical manipulator Kinematic model: 0 T 3 = Canonical transformation matrices C 1 0 S H 1= S 1 0 C ,1 H 2= H 3 = d C 1 C 2 S 1 C 1 S 2 d 2 S 1 +d 3 C 1 S 2 C 2 S 1 C 1 S 1 S 2 d 2 C 1 +d 3 S 1 S 2 S 2 0 C 2 C 2 d C 2 0 S 2 0 S 2 0 C d C. Melchiorri (DEIS) Kinematic Model 118 / 164

126 Differential Kinematics Examples Example 3 dof spherical manipulator The Jacobian is with and J = z 0 = p 0 = p 1 = [ z0 (p 3 p 0) z 1 (p 3 p 1) z 2 z 0 z z 1 = p 2 = S 1 C 1 0 d 2S 1 d 2C 1 0 z 2 = p 3 = ] C 1S 2 S 1S 2 C 2 d 2S 1 +d 3C 1S 2 d 2C 1 +d 3S 1S 2 C 2d 3 C. Melchiorri (DEIS) Kinematic Model 119 / 164

127 Differential Kinematics Examples Example 3 dof spherical manipulator Then J = d 2C 1 d 3S 1S 2 d 3C 1C 2 C 1S 2 d 2S 1 +d 3C 1S 2 d 3S 1C 2 S 1S 2 0 d 3S 2 C 2 0 S C Note that: q 3 does not affect ω; ω z depends only on q 1 ; S 1 ω y = C 1 ω x. C. Melchiorri (DEIS) Kinematic Model 120 / 164

128 Differential Kinematics Examples Example: 3 dof spherical wrist J = d 6S 4S 5 d 6C 4C 5 0 d 6C 4S 5 d 6C 5S d 6S S 4 C 4S 5 0 C 4 S 4S C 5 By choosing d 6 = 0, i.e. the origin of F 6 is in the intersection point of the three joint axes, then J = S 4 C 4S 5 0 C 4 S 4S C 5 With this expression, however, the linear velocity of the end-effector is not computed. det(j) = 0 = S 5 = 0, i.e. θ 5 = 0, π. In this case it is not possible to determine individually θ 4 and θ 6. C. Melchiorri (DEIS) Kinematic Model 121 / 164

129 Differential Kinematics Examples Example: PUMA 260 Only revolute joints are present: [ z0 (p J = 6 p 0 ) z 1 (p 6 p 1 ) z 2 (p 6 p 2 ) z 0 z 1 z 2 ] z 3 (p 6 p 3 ) z 4 (p 6 p 4 ) z 5 (p 6 p 5 ) z 3 z 4 z 5 C. Melchiorri (DEIS) Kinematic Model 122 / 164