Ridig Body Motion Homogeneous Transformations

Ridig Body Motion Homogeneous Transformations Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna email: claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 1 / 80

Summary 1 Ridig Body Motion Translations 2 Homogeneous transformations C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 2 / 80

Ridig Body Motion Ridig Body Motion Homogeneous Transformations Ridig Body Motion Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna email: claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 3 / 80

Ridig Body Motion Rigid body motions - Homogeneous transformations Description of the manipulator kinematic properties: Description of the geometric characteristics of the robot s motion (position, velocity, acceleration), without considering the forces applied to it The solution of the kinematic problem is based on: definition of a reference frame associated to each link of the manipulator a procedure for the computation of the relative motion (position, velocity, acceleration) of these frames due to joints movements. It is necessary to introduce some conventions to describe the position/orientation of rigid bodies and their motion in the space: kinematic properties of a rigid body and how to describe them homogenous transformations description of position and velocity (force) vectors in different reference frames. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 4 / 80

Ridig Body Motion Rigid body and its representation A manipulator is composed by a series of rigid bodies, the links, connected by joints that allow a relative motion. RIGID BODY: idealization of a solid body of finite size in which deformation is neglected: the distance between any two given points of a rigid body remains constant in time regardless of external forces exerted on it. p(t) q(t) = d(p(t),q(t)) = cost C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 5 / 80

Ridig Body Motion Rigid body and its representation Some assumptions: The 3D operational space is represented by the vector space IR 3, In the 3D space, are defined the inner product n u T v = u i v i = u v cosθ i=1 u,v IR n and the Euclidean norm u = u T u = n i=1 u 2 i u IR n We often use Cartesian (right-handed) reference frames, with homogenous dimensions along the axes The base frame is an inertial frame. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 6 / 80

Ridig Body Motion Rigid body and its representation General definition of norm: u = u T W u being W a matrix: symmetric positive definite Often, W is a diagonal matrix. Since W = V T V, then: u = u T W u = (u T V T )(Vu) = x T x C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 7 / 80

Ridig Body Motion Rigid body and its representation In IR 3, a rigid body has 6 degrees of freedom (dof): 3 for the position, x,y,z; 3 for the orientation, α,β,γ. In general, a rigid body in IR n has n dof for the position n(n 1)/2 dof for the orientation C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 8 / 80

Ridig Body Motion Rigid body and its representation Roto-translation motion: most general motion of a rigid body in the space, composed by a rotation about an axis (instantaneous axis of rotation) and a translation along the same axis. Problem of describing the instantaneous position/orientation of a rigid body with respect to a fixed base frame Let 0 o 1 be the origin of the frame F 1 fixed to the rigid body, expressed in F 0. Each point of the body has coordinates 1 p = [ 1 p x 1 p y 1 p z ] T constant with respect to F 1. Since the body moves, the same point has coordinates 0 p, expressed in F 0, variable in time 0 p = [ 0 p x 0 p y 0 p z ] T C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 9 / 80

Ridig Body Motion Rigid body and its representation First problem: if point p is known in F 1, computetheequivalentrepresentation in F 0. 1 p = 0 p The problem is solved by using the Homogeneous Transformation Matrix 0 T 1 : [ ] n x s x a x o x 0 T 1 = 0 R 0 1 o 1 = n y s y a y o y 0 0 0 1 n z s z a z o z 0 0 0 1 defining the transformation (roto-translation) between F 1 and F 0. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 10 / 80

Ridig Body Motion Rigid body and its representation The problem is decomposed into two parts: 1 F 0 and F 1 share the same origin, and have a different orientation in space 2 F 0 and F 1 have parallel axes but a different origin (translation). C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 11 / 80

Ridig Body Motion Ridig Body Motion Homogeneous Transformations Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna email: claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 12 / 80

Ridig Body Motion Consider two reference frames F 0 and F 1 with the same origin, i.e. o 1 o 0. Given a vector 0 v in F 0, its components v x, v y, v z are the orthogonal projections of 0 v on the coordinate axes. v x = 0 v T i = 0 v cosα 1 v y = 0 v T j = 0 v cosα 2 v z = 0 v T k = 0 v cosα 3 i, j, k: unit vectors defining the directions of x 0, y 0, z 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 13 / 80

Ridig Body Motion If 0 v indicates an axis of F 1, e.g. 0 i 1, then 0 i 1 = [ 0 i x 0 i j 0 i z ] T, where 0 i x = 0 i T 1 i = cosα 1 0 i y = 0 i T 1 j = cosα 2 0 i z = 0 i T 1 k = cosα 3 This is a well known result: 0 i 1 = [cosα 1, cosα 2, cosα 3 ] T the components of a unit vector with respect to a reference frame are its direction cosines. A similar results holds for the other directions 0 j 1 and 0 k 1. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 14 / 80

Ridig Body Motion Once the direction cosines of the three axes of F 1 with respect to F 0 are known, the matrix R may be defined: 0 i T 1 i 0 j T 1 i 0 k T 1 i R = 0 i T 1 j 0 j T 1 j 0 k T 1 j 0 i T 1 k 0 j T 1 k 0 k T 1 k 0 i 1, 0 j 1, 0 k 1 : axes of F 1 expressed in F 0 i, j, k: axes of F 0. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 15 / 80

Ridig Body Motion EXAMPLE By projecting the unit vectors i 1,j 1,k 1 on i 0,j 0,k 0, the components of the principal axes of F 1 in F 0 are obtained: i 1 = [0, 1/ 2, 1/ 2] T j 1 = [0, 1/ 2, 1/ 2] T k 1 = [1, 0, 0] T C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 16 / 80

Ridig Body Motion The rotation matrix between F 0 and F 1 is obtained from these three vectors: R = 0 0 1 1 2 2 1 0 1 2 2 1 0 In general x 0 y 0 z 0 x 1 y 1 z 1 r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 17 / 80

Ridig Body Motion Usually, in robotics the symbols n,s,a, are used to indicate the axes x 1,y 1,z 1, then 0 R 1 = [ 0 n 0 s 0 a] = n x s x a x n y s y a y n z s z a z defining the relative orientation between F 0 and F 1. Symbols n,s,a refer to a frame fixed on the endeffector (e.g. gripper) with z axis (a) along the approach direction y axis (s) in the sliding plane of the fingers x axis (n) in the normal direction with respect to y,z. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 18 / 80

Rotation matrix: From the conditions: Ridig Body Motion R = [n s a] = n x s x a x n y s y a y n z s z a z { n T a = s T a = s T n = 0 n = s = a = 1 it follows that R is an orthonormal matrix, i.e. R R T = R T R = I 3 I 3 : 3 3 identity matrix A rotation matrix is always invertible. By pre-multiplying by R 1 we have i.e. R 1 = R T 0 R 1 1 = 1 R 0 = 0 R T 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 19 / 80

Elementary rotations Ridig Body Motion Consider two frames F 0 and F 1 with coincident origins. of θ about the x 0, y 0, and z 0 axes C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 20 / 80

Elementary rotations Ridig Body Motion In the first case, F 1 is obtained with a rotation of an angle θ about the x 0 axis of F 0. From we have R = 0 R 1 = Rot(x,θ) = 0 i T 1 i 0 j T 1 i 0 k T 1 i 0 i T 1 j 0 j T 1 j 0 k T 1 j 0 i T 1 k 0 j T 1 k 0 k T 1 k 1 0 0 0 cosθ sinθ 0 sinθ cosθ C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 21 / 80

Elementary rotations Ridig Body Motion Similarly, considering rotations about y 0 and z 0 : cosθ 0 sinθ 0 R 1 = Rot(y,θ) = 0 1 0 sinθ 0 cosθ cosθ sinθ 0 0 R 1 = Rot(z,θ) = sinθ cosθ 0 0 0 1 Rotation matrices R relate different reference frames. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 22 / 80

Ridig Body Motion Another interpretation for rotation matrices. Let us consider a rotation of point 0 p 1 = [7,3,2] T by 90 o about z 0. The matrix expressing the rotation is cos90 o sin90 o 0 R 1 = Rot(z,90 o ) = sin90 o cos90 o 0 0 0 1 0 1 0 = 1 0 0 0 0 1 Therefore: 0 p 2 = 3 7 2 = 0 1 0 1 0 0 0 0 1 7 3 2 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 23 / 80

Ridig Body Motion Consider now a second rotation of 90 o about y 0 : 0 p 3 = 2 7 3 = R 2 0 p 2 = 0 0 1 0 1 0 1 0 0 3 7 2 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 24 / 80

Ridig Body Motion By combining the two rotations one obtains R = R 2 R 1 = 0 0 1 0 1 0 0 1 0 1 0 0 = 0 0 1 1 0 0 1 0 0 0 0 1 0 1 0 from which 0 p 3 = 2 7 = R 0 p 1 = 0 0 1 1 0 0 7 3 3 0 1 0 2 Rotation matrices rotates vectors with respect to a fixed reference frame. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 25 / 80

Ridig Body Motion Axis/angle rotations Rotation θ about a generic unit vector w = [w x w y w z ] T. Therotationoftheangleθ aboutw isequivalent to the following procedure: Aligne w with z 0 Rotate by θ about w z 0 Restore w in its original position. Each rotation is performed with respect to F 0, then: Rot(w,θ) = Rot(z 0,α)Rot(y 0,β)Rot(z 0,θ)Rot(y 0, β)rot(z 0, α) C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 26 / 80

Axis/angle rotations Ridig Body Motion Moreover, since w = 1, we have: w y sinα = wx 2 +w2 y sinβ = wx 2 +w2 y cosα = cosβ = w z w x w 2 x +w2 y The matrix R representing the rotation is therefore given by R(w,θ) = w xw x V θ +C θ w y w x V θ w z S θ w z w x V θ +w y S θ w x w y V θ +w z S θ w y w y V θ +C θ w z w y V θ w x S θ w x w z V θ w y S θ w y w z V θ +w x S θ w z w z V θ +C θ being C θ = cosθ,s θ = sinθ, e V θ = versθ = 1 cosθ. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 27 / 80

Proprieties of rotations Ridig Body Motion 1. Not all the orthogonal matrices (R T R = I) for which the following conditions { n T a = s T a = s T n = 0 n = s = a = 1 are satisfied represent rotations. For example, matrix S = 1 0 0 0 1 0 0 0 1 does not represent a rotation, but rather a specular transformation. It is not possible, starting from F 0, to obtain frame F 1 with a rotation. F 1 may be obtained only by means of a specular reflection. This is not physically feasible for a rigid body. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 28 / 80

Ridig Body Motion Proprieties of rotations If matrix R represents a rigid body rotation, then det(r) = 1 Because of their properties, the rotation matrices in IR 3 belong to a special set, the Special Orthogonal group of order 3, i.e. So(3). More in general, the set of n n matrices R satisfying the two conditions { RR T = R T R = I det(r) = +1 is called So(n): Special Orthogonal group in IR n = Special: det(r) = +1 = Orthogonal: RR T = R T R = I So(n) = {R IR n n : RR T = R T R = I, det(r) = +1} C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 29 / 80

Proprieties of rotations Ridig Body Motion 2. The equations 0 R 1 = [ 0 n 0 s 0 a ] = n x s x a x n y s y a y n z s z a z 0 R 1 1 = 1 R 0 = 0 R T 1 allow to consider the relative rotation of two frames, and to transform in F 0 vectors defined in F 1. The expression of 1 p in F 0 is given by: n x s x a x 0 p = 0 R 1 1 p = n y s y a y 1 p n z s z a z C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 30 / 80

Proprieties of rotations Ridig Body Motion The composition of more rotations is expressed by a simple matrix multiplication: Given n+1 reference frames F 0,..., F n with coincident origins and relative orientation expressed by i 1 R i, i = 1,...n, and given the vector n p in F n, then A note about computational complexity: 0 p = 0 R 1 1 R 2... n 1 R n n p { 63 products 0 p = ( 0 R 1 1 R 2 2 R 3 ) 3 p = 0 R 3 3 p 42 summations { 0 p = ( 0 R 1 ( 1 R 2 ( 2 R 3 27 products 3 p) )) }{{} 18 summations 2 p } {{ } 1 p 3. From 0 p = 0 R 1 1 p it follows that a rotation applied to vector 1 p is a linear function: 0 p = r( 1 p) = 0 R 1 1 p. Given two vectors p,q and two scalar quantities a,b, we have r(ap+bq) = ar(p)+br(q) C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 31 / 80

Proprieties of rotations Ridig Body Motion 4. do not change the amplitude of a vector: As a matter of fact: Ra = a Ra = a T R T Ra = a T a = a 5. The inner product, and then the angle between two vectors, is invariant with respect to rotations: a T b = (Ra) T (Rb) As a matter of fact: (Ra) T (Rb) = a T R T Rb = a T b = a b cosθ 6. Since R is an orthogonal matrix, the following property holds R(a b) = Ra Rb C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 32 / 80

Proprieties of rotations Ridig Body Motion 7. In general, the product of rotation matrices does not commute: R a R b R b R a Except the trivial case of the identity matrix (i.e. when R = I 3 ), rotations commute only if the rotation axis is the same! Consider the two rotations by a 90 o angle about the x 0 and y 0 axes: R a = Rot(x,90 0 ) = 1 0 0 0 0 1 R b = Rot(y,90 o ) = 0 0 1 0 1 0 0 1 0 1 0 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 33 / 80

Proprieties of rotations Ridig Body Motion 1) 2) Case 1) R b followed by R a: 0 1 0 R = R b R a = 0 0 1 1 0 0 Case 2) R a followed by R b : 0 0 1 R = R ar b = 1 0 0 0 1 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 34 / 80

Ridig Body Motion Proprieties of rotations 8. It may be of interest to define a sequence of rotations with respect to F 0, and not with respect to the current frame F i as assumed until now. Consider two rotations R 1 = Rot(y,φ) and R 2 = Rot(z,θ) about the axes y 0 and z 0 of F 0. What is the result of applying first R 1 and then R 2? Consider the vector 0 p in F 0. After the first rotation R 1, the new expression of the vector (still wrt F 0 ) is 0 p 1 = R 1 0 p Since also the second rotation is about an axis of F 0, we have 0 p 2 = R 2 0 p 1 = R 2 R 1 0 p More in general, given n consecutive rotations R i,i = 1,...,n defined with respect to the same reference frame F 0, then 0 p n = R n R n 1...R 1 0 p C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 35 / 80

Proprieties of rotations Ridig Body Motion Then, there are two different possibilities to define a sequence of consecutive rotations: 1 If each rotation is expressed wrt the current frame F n, F n 1,..., F 0, then the equivalent rotation matrix 0 R n is obtained by post-multiplication of the matrices i 1 R i. 0 p = 0 R 1 1 R 2... n 1 R n n p 2 If matrices R i,i = 1,...,n describe rotations about an axis of the base frame F 0, the equivalent matrix is obtained by pre-multiplication of the matrices. 0 p n = R n R n 1... R 1 0 p C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 36 / 80

Ridig Body Motion Interpretations of a rotation matrix In summary, a rotation matrix 0 R 1 has three equivalent interpretations: 1. 0 R 1 describes the mutual orientation of two reference frames F 0 and F 1 ; the columns of 0 R 1 are the direction cosines of the axes of F 1 expressed in F 0 2. 0 R 1 defines the coordinate transformation between the coordinates of a point expressed in F 0 and in F 1 (common origin) 3. 0 R 1 rotates a vector 0 v a to 0 v b in a given reference frame F 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 37 / 80

Ridig Body Motion Representations of rotations A rotation is described with a 3 3 matrix with 9 elements: R = n x s x a x n y s y a y n z s z a z On theotherhand, arigidbodyinir 3 has 3 rotational dof Three parameters should be sufficient to describe its orientation A 3 3 matrix, although computationally efficient, is redundant. Among the 9 elements of R one can define the following relations: { n T a = s T a = s T n = 0 n = s = a = 1 Note that it is sufficient to know 6 elements of R to define completely the matrix. If only 5 (or less) elements are known, R cannot be determined univocally. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 38 / 80

Ridig Body Motion Representations of rotations Theoretically, only 3 parameters are sufficient to describe the orientation of a rigid body in the 3D space. There are representations based on 3 parameters only (minimal representations), more compact than rotation matrices, although computationally less convenient. Among these representations, we have: Euler angles: three consecutive rotations about axes z,y,z Roll, Pitch and Yaw angles: three consecutive rotations about axes z 0,y 0,x 0 Axis/Angle representation: a unitary rotation axis r and the angle θ C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 39 / 80

Ridig Body Motion Euler angles Euler angles (φ, θ, ψ) represents three rotations, applied sequentially about the axes z 0,y 1,z 2 of the current frame F 0, F 1, F 2. Consider a base frame F 0. By applying the three rotations we have - A frame F 1 obtained with the rotation φ about z 0 - A frame F 2 obtained from F 1 with the rotation θ about y 1 - A frame F 3 obtaine from F 2 with the rotation ψ about z 2 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 40 / 80

Euler angles Ridig Body Motion By composing the three rotations, the total rotation from F 0 to F 3 is 0 R 3 = 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 θ C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 41 / 80

Euler angles Ridig Body Motion Rotation matrix corresponding to the Euler angles: R Euler (φ,θ,ψ) = 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 θ Inverse problem: compute the Euler angles corresponding to a given rotation matrix R: R = r 11 r 12 r 13 r 21 r 22 r 23 (φ, θ, ψ)? r 31 r 32 r 33 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 42 / 80

Ridig Body Motion Atan2 function arctan( y x ) = arctan( y x ), gives results in two quadrants (in [ π/2, +π/2]) atan2 is the arctangent with output values in the four quadrants: two input arguments gives values in [ π, +π] undefined only for (0, 0) uses the sign of both arguments to define the output quadrant based on arctan function with output values in [ π/2, +π/2] available in main languages (C++, Matlab,...) atan2(y,x) = arctan(y/x) x > 0 π +arctan(y/x) y 0,x < 0 π+arctan(y/x) y < 0,x < 0 π/2 y > 0,x = 0 π/2 y < 0,x = 0 undefined y = 0,x = 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 43 / 80

Euler angles Ridig Body Motion Two cases are possible: 1 r13 2 +r2 23 0 sinθ 0. By assuming 0 < θ < π (sinθ > 0), one obtains: φ = atan2(r 23,r 13 ); θ = atan2( r13 2 +r2 23,r 33); ψ = atan2(r 32, r 31 ) or, with π < θ < 0, (sinθ < 0): φ = atan2( r 23, r 13 ); θ = atan2( r13 2 +r2 23,r 33); ψ = atan2( r 32,r 31 ) Two possible sets of solutions depending on the sign of sinθ. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 44 / 80

Euler angles Ridig Body Motion 2. r13 2 +r2 23 = 0 (θ = 0,π and cosθ = ±1). By choosing θ = 0 (cosθ = 1) one obtains { θ = 0 φ+ψ = atan2(r 21,r 11 ) = atan2( r 12,r 11 ); On the other hand, if θ = π (cosθ = 1) { θ = π φ ψ = atan2( r 21, r 11 ) = atan2( r 12, r 11 ); In both cases, infinite solutions are obtained (only the sum or difference of φ and θ is known). Being θ = 0,π, the rotations by the angles φ and ψ occur about parallel (the same) axes, i.e. the z axis. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 45 / 80

Ridig Body Motion Roll, Pitch, Yaw Consider three consecutive rotations about the axes of the base frame F 0 : A rotation ψ about x 0, (yaw), A rotation θ about y 0, (pitch) A rotation φ about z 0, (roll). C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 46 / 80

Roll, Pitch, Yaw Ridig Body Motion By properly composing the three rotations: 0 R 3 = R RPY (φ,θ,ψ) = Rot(z 0,φ)Rot(y 0,θ)Rot(x 0,ψ) = C φc θ S φ C ψ +C φ S θ S ψ S φ S ψ +C φ S θ C ψ S φ C θ C φ C ψ +S φ S θ S ψ C φ S ψ +S φ S θ C ψ S θ C θ S ψ C θ C ψ C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 47 / 80

Roll, Pitch, Yaw Ridig Body Motion Rotation matrix corresponding to the RPY angles: R RPY (φ,θ,ψ) = C φc θ S φ C ψ +C φ S θ S ψ S φ S ψ +C φ S θ C ψ S φ C θ C φ C ψ +S φ S θ S ψ C φ S ψ +S φ S θ C ψ S θ C θ S ψ C θ C ψ Inverse problem: compute the RPY angles corresponding to a given rotation matrix R: R = r 11 r 12 r 13 r 21 r 22 r 23 (φ, θ, ψ) (?) r 31 r 32 r 33 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 48 / 80

Roll, Pitch, Yaw Ridig Body Motion Two cases are possible: 1 r11 2 +r2 21 0 cosθ 0. By choosing θ [ π/2,π/2], one obtains: φ = atan2(r 21,r 11 ); θ = atan2( r 31, r32 2 +r2 33 ); ψ = atan2(r 32,r 33 ); Otherwise, if θ [π/2,3π/2]: φ = atan2( r 21, r 11 ); θ = atan2( r 31, r32 2 +r2 33 ); ψ = atan2( r 32, r 33 ); C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 49 / 80

Roll, Pitch, Yaw Ridig Body Motion 2. r11 2 +r2 21 = 0 cosθ = 0: θ = ±π/2 and infinite solutions are possible (sum or difference of ψ and φ). It may be convenient to (arbitrarily) assign a value (e.g. ±90 o ) to one of the two angles (φ or ψ) a ±90 o and then compute the remaining one: { θ = ±π/2; φ ψ = atan2(r 23,r 13 ) = atan2( r 12,r 22 ); C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 50 / 80

Ridig Body Motion Angle/Axis representation It is possible to describe any rotation in 3D by means of the rotation angle θ and the corresponding rotation axis w R = wx(1 C 2 θ )+C θ w xw y(1 C θ ) w zs θ w xw z(1 C θ )+w ys θ w xw y(1 C θ )+w zs θ wy(1 C 2 θ )+C θ w yw z(1 C θ ) w xs θ w xw z(1 C θ ) w ys θ w yw z(1 C θ )+w xs θ wz(1 C 2 θ )+C θ 4 parameters: w x,w y,w z, θ with the condition: w 2 x +w 2 y +w 2 z = 1 = 3 dof C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 51 / 80

Ridig Body Motion Angle/Axis representation Inverse problem: compute the axis w and the angle θ corresponding to a given rotation matrix R: θ = acos r 11 +r 22 +r 33 1 2 1 w = r 32 r 23 r 13 r 31 2sinθ r 21 r 12 The trace of a rotation matrix depends only on the (cosine of) rotation angle. This representation suffers of some drawbacks: It is not unique: Rot(w,θ) = Rot( w, θ) (we can arbitrarily assume 0 θ π) If θ = 0 then Rot(w,0) = I 3 and w is indefinite There are numerical problems if θ 0: in this case sinθ 0 and problems may arise in computing w C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 52 / 80

Ridig Body Motion Example Compute the rotation matrix corresponding to the RPY angles 0 o,45 o,90 o, i.e.: 1 A rotation of 90 o about x 0, (yaw) 2 A rotation of 45 o about y 0, (pitch) 3 A rotation of 0 o about z 0, (roll) RPY rotations: roll = 0 o, pitch = 45 o, yaw = 90 o One obtains: R = 1 2 1 2 0 0 0 1 1 2 1 2 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 53 / 80

Example Ridig Body Motion R = 1 2 1 2 0 0 0 1 1 2 1 2 0 RPY rotations: roll = 0 o, pitch = 45 o, yaw = 90 o The Euler angles corresponding to this rotation matrix are φ = 90 o ; θ = 90 o ; ψ = 45 o Considering the Angle/Axis representation, one obtains: rotation of the angle θ = 98.42 o about the axis w = [0.863, 0.357, 0.357] T C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 54 / 80

Ridig Body Motion Ridig Body Motion Homogeneous Transformations Translations Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna email: claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 55 / 80

Translations Ridig Body Motion Translations between two coordinate frames can be expressed in matrix form: 0 p = R 1 p This is not possible for translations! It is not possible to define a 3 3 matrix P so that a translation can be expressed as a matrix/vector multiplication 0 p = P 1 p = not possible!! A translation of a vector 0 p by 0 o corresponds to a vectorial summation Then 0 q = 0 p + 0 o 0 q x = 0 p x+ 0 o x 0 q y = 0 p y+ 0 o y 0 q z = 0 p z+ 0 o z C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 56 / 80

Translations Ridig Body Motion Translations Then, a translation is expressed as a function t(p) = p+o In general: t(ap+bq) = ap+bq+o at(p)+bt(q) Then, translations are not linear transformations! The most general transformation between two coordinate frames cannot be represented by a 3 3 matrix. The composition of a rotation and a translation is obtained from 0 q = 0 p+ 0 o by considering vector p defined in F 1, rotated and translated with respect to F 0. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 57 / 80

Translations Ridig Body Motion Translations Since vectors can be added only if they are defined with respect to the same coordinate system: 0 p = 0 R 1 1 p+ 0 o 1 being 0 o 1 the translation from F 0 to F 1 and 0 R 1 the mutual rotation. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 58 / 80

Ridig Body Motion Ridig Body Motion Homogeneous Transformations Homogeneous transformations Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna email: claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 59 / 80

Homogeneous transformations Homogeneous transformations It is of interest to put in matrix form the equation 0 p = 0 R 1 1 p+ 0 o 1 since, in case of successive transformations, one could obtain expressions similar to 0 p = 0 R 1 1 R 2... n 1 R n n p For this purpose, it is possible to add to matrix R the vector 0 o as fourth column; in this manner a 3 4 matrix is obtained M = [n s a 0 o 1 ] = [R 0 o 1 ] A square matrix is obtained by adding to M the row [0 0 0 1]. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 60 / 80

Homogeneous transformations Homogeneous transformations The Homogeneous Transformation Matrix 0 T 1 is obtained [ ] 0 T 1 = 0 R 0 1 o 1 = 0 0 0 1 n x s x a x o x n y s y a y o y n z s z a z o z 0 0 0 1 This matrix represents the transformation (rotation-translation) between F 0 and F 1. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 61 / 80

Homogeneous transformations Homogeneous transformations By defining the homogeneous 4-dimensional (in IR 4 ) vector: one obtains [ 0 p 1 ] [p T 1] T = [p x p y p z 1] T = 0 T 1 [ 1 p 1 ] = [ 0 R 1 1 p+ 0 o 1 1 ] The subspace IR 3 defined by the first three components represents the transformation 0 p = 0 R 1 1 p+ 0 o 1 on vectors in IR 3, and physically represents the rigid body motion from F 1 to F 0 ; the fourth component is not affected by the matrix multiplication. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 62 / 80

Homogeneous transformations Homogeneous transformations Given the homogeneous transformation matrices 0 T 1, from F 1 to F 0, and 1 T 2, from F 2 to F 1, the composition 0 T 2 = 0 T 1 1 T 2 can be applied to vectors [ 2 p T 1] T defined in F 2, and the result is 0 T 1 ( 1 T 2 [ 2 p 1 ] [ 1 ] ) = 0 R T 2 2 p+ 1 o 2 1 1 [ 0 R = 1 1 R 2 2 p+ 0 R 1 1 o 2 + 0 o 1 ] C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 63 / 80

Homogeneous transformations Homogeneous transformations This is equivalent to the product of the homogeneous matrix [ 0 T 2 = 0 R 1 1 R 2 0 R 1 1 o 2 + 0 o 0 0 0 1 ] = 0 T 1 1 T 2 with the vector [ 2 p T 1] T. In general we have 0 T n = 0 T 1 1 T 2... n 1 T n C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 64 / 80

Homogeneous transformations Homogeneous transformation of coordinates The coordinate transformation bewteen two reference frames F 0 and F 1 may be expressed by a 4 4 matrix 0 T 1 : 0 p = 0 T 1 1 p Of particular interest are the elementary transformations, i.e. simple rotations or translations along the coordinate axes. All the coordinate transformations may be obtained by combinations of these elementary transformations. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 65 / 80

Homogeneous transformations Elementary rotations and translations The homogeneous transformation matrices corresponding to rotations of an angle θ about the axes x,y,z of F 0 are: 1 0 0 0 0 C Rot(x, θ)= θ S θ 0 0 S θ C θ 0,Rot(y,θ)= 0 0 0 1 being C θ = cosθ,s θ = sinθ. C θ 0 S θ 0 0 1 0 0 S θ 0 C θ 0 0 0 0 1,Rot(z,θ)= C θ S θ 0 0 S θ C θ 0 0 0 0 1 0 0 0 0 1 The homogeneous transformation T corresponding to the translation of vector v = [v x v y v z ] T is 1 0 0 v x T = Trasl(v) = 0 1 0 v y 0 0 1 v z 0 0 0 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 66 / 80

Example Homogeneous transformations The frame F 1, with respect to F 0, is translated of 1 along x 0 and of 3 along y 0, moreover, it is rotated by 30 o about z 0. The transformation matrix from F 1 to F 0 is 0 T 1 = 0.866 0.500 0 1 0.500 0.866 0 3 0 0 1 0 0 0 0 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 67 / 80

Example Homogeneous transformations Consider a point defined in F 1 by 1 p = Its coordinates in F 0 are 0 p = 0 T 1 1 p = 2 1 0 0.866 0.500 0 1 0.500 0.866 0 3 0 0 1 0 0 0 0 1 2 1 0 1 = 2.232 4.866 0 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 68 / 80

Example Homogeneous transformations Consider the point in F 0 0 p a = 2 1 0 Let us apply to 0 p a translation of 1 along x 0, of 3 along y 0 ; then rotate the vector by 30 o about z 0. The result is obtained by multiplying vector 0 p by the matrix 0.866 0.500 0 1 T = 0.500 0.866 0 3 0 0 1 0 0 0 0 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 69 / 80

Example Homogeneous transformations Then 0 p b = T 0 p a = T = 0.866 0.500 0 1 0.500 0.866 0 3 0 0 1 0 0 0 0 1 2 1 0 1 = 2.232 4.866 0 1 The same numerical result is obtained, although the physical interpretation is different. C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 70 / 80

Homogeneous transformations Interpretations of a homogeneous transformation matrix Similarly to rotation matrices, also an homogeneous transformation matrix 0 T 1 [ 0 ] 0 R T 1 = 0 1 v 0 0 0 1 has three possible physical interpretations: 1 Description of F 1 in F 0 : in particular 0 v represents the origin of F 1 with respect to F 0, and the elements of 0 R 1 give the direction of the axes of F 1 2 Coordinate transformation of vectors between F 1 and F 0, 1 p 0 p; 3 Translates and rotates a generic vector 0 p a to 0 p b in a given reference frame F 0 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 71 / 80

Inverse transformation Homogeneous transformations Once the position/orientation of F 1 with respect to F 0 are known, defined by the homogeneous transformation matrix 0 T 1, it is simple to compute the inverse transformation 1 T 0 = ( 0 T 1 ) 1, defining the position/orientation of F 0 with respect to F 1. From 0 T 1 1 T 0 = I 4 it follows that [ 1 T 0 = 0 R T 1 0 R T 1 v 0 0 0 1 ] = n x n y n z v T n s x s y s z v T s a x a y a z v T a 0 0 0 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 72 / 80

Inverse transformation Homogeneous transformations As a matter of fact, if then [ R v 0 0 0 1 and therefore [ 1 T 0 = ] M x 0 0 0 1 ][ ] M x = 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 I 3 = RM 0 = Rx+v = M = RT x = R T v C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 73 / 80

Example Homogeneous transformations Given T = 0 0 1 2 1 0 0 1 0 1 0 0 0 0 0 1 compute its inverse transformation. Solution: [ T 1 = R T R T v 0 0 0 1 ] = n x n y n z v T n s x s y s z v T s a x a y a z v T a 0 0 0 1 = 0 1 0 1 0 0 1 0 1 0 0 2 0 0 0 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 74 / 80

Homogeneous transformations Homogeneous transformations The term homogeneous derives from projective geometry. Equations describing lines and planes in projective geometry in IR 3 are homogeneous in the four variables x 1,x 2,x 3,x 4 in IR 4. In IR 3 these equations, affine transformations, are non homogeneous in x 1,x 2,x 3, (lines or planes not passing through the origin present a constant term - non function of x 1,x 2,x 3 - in their expression). In general, computations of an affine transformations in IR n 1 may be expressed as homogeneous linear transformations in IR n : 0 p = 0 R 1 1 p+ 0 o 1 affine transformation [ 0 ] [ 0 p p 0 ][ R = = 0 1 o 1 ] 1 p homogeneous transformation 1 0 1 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 75 / 80

Homogeneous transformations Homogeneous transformations The most general expression for an homogeneous transformation is [ ] [ D3 3 p T = 3 1 Deformation Translation = s Perspective Scale f 1 3 ] Note the terms D, f 1 3 and s. These quantities, in robotics, are alway assumed as: A rotation matrix (D = R), The null vector [0 0 0] (f 1 3 = [0 0 0]), The unit gain (s = 1) In other cases (e.g. computer graphics), these quantities are used to obtain deformations, perspective distortions, change of scaling factors, and so on (in general: effects non applicable to rigid bodies). C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 76 / 80

Example Homogeneous transformations Consider the transformation matrix T = a 0 0 0 0 b 0 0 0 0 c 0 0 0 0 1 This transformation applies a different gain along the three reference axes x,y,z. A deformation of the body is obtained. Point p = [1,1,1] T is transformed to p 1 = T p = a 0 0 0 0 b 0 0 0 0 c 0 0 0 0 1 1 1 1 1 = With this matrix, a cube is transformed in a parallelepiped. a b c 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 77 / 80

Homogeneous transformations Example Similarly, the transformation T = 1 0 0 0 0 1 0 0 0 0 1 0 0 1 f 0 1 performs a perspective transformation along y. The coordinates x,y,z of a point p are transformed in 80 x = y = x 1 y/f y 1 y/f 70 60 50 40 f = 20 z = z 1 y/f 30 20 10 x y z 0 0 10 20 30 40 50 60 70 80 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 78 / 80

Homogeneous transformations Equations with homogeneous transformations Usually, in robotics it is necessary to specify the position/orientation of an object with respect to different reference frames (e.g. wrt to the end-effector, to the base frame, to other machines/tools,...). The following equation must be verified if the robot has to grasp the object: B B T 6 E = OG C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 79 / 80

Homogeneous transformations Equations with homogeneous transformations B B T 6 E = OG Usually, matrices B, O, G, E are known (and constant). Therefore, the equation can be solved in terms of B T 6 B T 6 = B 1 OGE 1 The robot configuration is then obtained. Otherwise, the object position O (if not known) can be computed as O = B B T 6 EG 1 C. Melchiorri (DEIS) Ridig Body Motion Homogeneous Transformations 80 / 80