Robotics 1 Position and orientation of rigid bodies Prof. Alessandro De Luca Robotics 1 1 Position and orientation right-handed orthogonal RFB rigid body Reference Frames A 3 • position: pAB (vector ! R ), B expressed in RF (use of coordinates I A RFA other than Cartesian is possible, e.g. pAB cylindrical or spherical) • orientation: orthonormal 3x3 matrix T -1 A B A (R = R " RB RA = I), with det = +1 A A A A RB = [ xB yB zB] • xA yA zA (xB yB zB) are unit vectors (with unitary norm) of frame RFA (RFB) A • components in RB are the direction cosines of the axes of RFB with respect to (w.r.t.) RFA Robotics 1 2 Rotation matrix T T T direction cosine of xA xB xA yB xA zB zB w.r.t. xA AR = y T x y T y y T z orthonormal, B A B A B A B T T T with det = +1 zA xB zA yB zA zB algebraic structure chain rule property of a group SO(3) (neutral element = I; k i k inverse element = RT) Ri ! Rj = Rj orientation of RFi orientation of RFj w.r.t. RFk w.r.t. RFk orientation of RFj w.r.t. RFi NOTE: in general, the product of rotation matrices does not commute! Robotics 1 3 Change of coordinates z0 0 px 0P = 0p = 1p 0x + 1p 0y + 1p 0z z1 • P y x 1 y 1 z 1 0p z 1 px y 1 0 0 0 1 = x1 y1 z1 py 1 pz RF1 y0 = 0R 1P RF0 1 x0 0 the rotation matrix R1 (i.e., the orientation of RF1 w.r.t. RF0) represents also the change of x1 coordinates of a vector from RF1 to RF0 Robotics 1 4 Ex: Orientation of frames in a plane (elementary rotation around z-axis) x = OB – xB = u cos # - v sin #$ y P • y = OC + Cy = u sin # + v cos #$ RFC z = w v # or… C u C 0 0 0 OP # RF0 xC yC zC O x B x cos # $-sin # $0 u u 0 OP y = sin # $ cos # 0 v = Rz(#) v z 0 0 1 w w R (-#) = R T(#) similarly: z z 1 0 0 cos # 0 sin # Rx(#) = 0 cos # - sin # Ry(#) = 0 1 0 0 sin # cos #$ - sin # 0 cos #$ Robotics 1 5 Ex: Rotation of a vector around z v’ x = |v| cos %$ y’ y = |v| sin %$ v y x’ = |v| cos (% + #) = |v| (cos % cos # - sin % sin #) # x’ = x cos # - y sin # %$ y’ = |v| sin (% + #) = |v| (sin % cos # + cos % sin #) x’ x O x’ = x sin # + y cos # z’ = z or… x’ cos # - sin # 0 x x …as y’ = sin # cos # 0 y = Rz(#) y before! z’ 0 0 1 z z Robotics 1 6 Equivalent interpretations of a rotation matrix the same rotation matrix, e.g., Rz(#), may represent: v’ RF C v • P RFC # # # RF RF0 0 the orientation of a rigid the change of coordinates the vector body with respect to a from RFC to RF0 rotation operator 0 C reference frame RF0 ex: P = Rz(#) P ex: v’ = Rz(#) v 0 0 0 ex: [ xc yc zc] = Rz(#) 0 the rotation matrix RC is an operator superposing frame RF0 to frame RFC Robotics 1 7 Composition of rotations 1 brings RF on RF 2 brings RF on RF brings RF0 on RF1 R2 1 2 R3 2 3 0 R1 • 3p p23 = 0 p = 0 01 p12 = 0 RF2 RF3 RF1 RF 0 a comment on computational complexity 63 products 0p = (0R 1R 2R ) 3p = 0R 3p 1 2 3 3 42 summations 0 0 1 2 3 27 products p = R1 ( R2 ( R3 p)) 18 summations 2p 1 Robotics 1 p 8 Axis/angle representation z 0 DATA r z • unit vector r (!r! = 1)$ r (positive if counterclockwise, as • P • # v seen from an “observer” placed like r) z1 y1 #$ DIRECT PROBLEM RF1 v’ ry find y0 RF0 0 0 0 R(#,r) = [ x1 y1 z1] rx RF is the result of rotating such that x0 1 RF by an angle around 0 # 0 1 0 0 the unit vector r P= R(#,r) P v’ = R(#,r) v x1 Robotics 1 9 Axis/angle: Direct problem z T 0 R(#,r) = C Rz(#) C 1 C concatenation of three rotations C-1 = CT r 3 C = n s r 2 z1 y1 Rz(#) RF1 after the first rotation y0 the z-axis coincides with r RF0 sequence of 3 rotations that n and s are orthogonal x 0 bring frame RF0 to superpose unit vectors such that with frame RF1 n " s = r, or nysz - synz = rx x1 nzsx - sznx = ry nxsy - sxny = rz Robotics 1 10 Axis/angle: Direct problem solution T R(#,r) = C Rz(#) C T c# - s# 0 n T R(#,r) = n s r s# c# 0 s 0 0 1 rT T T T T T = r r + (n n + s s ) c# + (s n - n s ) s# taking into account that T T T T C C = n n + s s + r r = I , and that 0 -r r z y skew-symmetric(r): T T s n - n s = 0 -r = S(r) x r " v = S(r)v = - S(v)r 0 depends only T T T R(#,r) = r r + (I - r r ) c# + S(r) s# = R (-#,r) = R(-#,-r) on r and # !! Robotics 1 11 Rodriguez formula v’ = R(#,r) v v’ = v cos # + (r " v) sin # + (1 - cos #)(rTv) r proof: T T R(#,r) v = (r r + (I - r r ) cos # + S(r) sin #)v T = r r v (1 - cos #) + v cos # + (r " v) sin # q.e.d. Robotics 1 14 Unit quaternion " to eliminate undetermined and singular cases arising in the axis/angle representation, one can use the unit quaternion representation Q = {-, .} = {cos(#/2), sin(#/2) r} a scalar 3-dim vector 2 2 " - + !.! = 1 (thus, “unit ...”) " (#, r) and (-#, -r) gives the same quaternion Q " the absence of rotation is associated to Q = {1, 0} " unit quaternions can be composed with special rules (in a similar way as in the product of rotation matrices) T Q 1*Q 2 = {-1-2 - .1 .2, -1.2 + -2.1 + .1".2} Robotics 1 20 Robotics 1 Minimal representations of orientation (Euler and roll-pitch-yaw angles) Homogeneous transformations Prof. Alessandro De Luca Robotics 1 1 “Minimal” representations rotation matrices: 9 elements inverse problem problem inverse - 3 orthogonality relationships - 3 unitary relationships = 3 independent variables direct problem problem direct sequence of 3 rotations around independent axes fixed (ai) or moving/current (a’i) axes 12 + 12 possible different sequences (e.g., XYX) actually, only 12 since {(a1 α1), (a2 α2), (a3 α3)} ≡ { (a’3 α3) , (a’2 α2), (a’1 α1)} Robotics 1 2 ZX’Z’’ Euler angles 1 z≡z’ z’ z’’ 2 y’’ RF” φ y’ RF’ θ y’ R (θ) = RF x’ θ 1 0 0 y 0 cos θ -sin θ x φ x’ x’≡x’’ 0 sin θ cos θ cos φ - sin φ 0 z’’ z’’’ Rz(φ) = sin φ cos φ 0 ≡ 3 y’’’ 0 0 1 y’’ ψ RF”’ cos ψ - sin ψ 0 R ( ) = z” ψ sin ψ cos ψ 0 x’’ x’’’ 0 0 1 ψ Robotics 1 3 ZX’Z’’ Euler angles direct problem: given φ , θ , ψ ; find R RZX’Z’’ (φ, θ, ψ) = RZ (φ) RX’ (θ) RZ’’ (ψ) order of definition cφ cψ - sφ cθ sψ - cφ sψ - sφ cθ cψ sφ sθ in concatenation = sφ cψ + cφ cθ sψ - sφ sψ + cφ cθ cψ - cφ sθ sθ sψ sθ cψ cθ given a vector v”’= (x”’,y”’,z”’) expressed in RF”’, its expression in the coordinates of RF is v = RZX’Z’’ (φ, θ, ψ) v”’ the orientation of RF”’ is the same that would be obtained with the sequence of rotations: ψ around z, θ around x (fixed), φ around z (fixed) Robotics 1 4 Roll-Pitch-Yaw angles ROLL PITCH 1 z’ 2 z z’’ z’ y’’ ψ T y’ C1RY(θ)C1 y’ θ with RY(θ) = θ y cos θ 0 sin θ y 0 1 0 x’ ψ x’’ - sin θ 0 cos θ x≡x’ YAW z 3 y’’’ 1 0 0 z’’ φ y’’ RX(ψ) = 0 cos ψ - sin ψ 0 sin ψ cos ψ T z’’’ C2RZ(φ)C2 cos φ - sin φ 0 with RZ(φ) = sin φ cos φ 0 0 0 1 x’’ φ x’’’ Robotics 1 6 Roll-Pitch-Yaw angles (fixed XYZ) direct problem: given ψ , θ , φ ; find R ⇐ note the order of products! RRPY (ψ, θ, φ) = RZ (φ) RY (θ) RX (ψ) order of definition cφ cθ cφ sθ sψ - sφ cψ cφ sθ cψ + sφ sψ = sφ cθ sφ sθ sψ + cφ cψ sφ sθ cψ - cφ sψ - sθ cθ sψ cθ cψ inverse problem: given R = {rij}; find ψ , θ , φ 2 2 2 2 2 r32 + r33 = c θ, r31 = -sθ ⇒ θ = ATAN2{-r31, ± √ r32 + r33 } 2 2 two symmetric values w.r.t. π/2 if r32 + r33 ≠ 0 (i.e., cθ ≠ 0) r32/cθ = sψ, r33/cθ = cψ ⇒ ψ = ATAN2{r32/cθ, r33/cθ} similarly... φ = ATAN2{ r21/cθ, r11/cθ } singularities for θ = ± π/2 Robotics 1 7 Homogeneous transformations P • Bp RF Ap B A ‘affine’ relationship pAB OB RFA A A A B p = pAB + RB p OA A A A B p RB pAB p linear A A B phom = = = TB phom relationship 1 0 0 0 1 1 vector in homogeneous 4x4 matrix of coordinates homogeneous transformation Robotics 1 9 Properties of T matrix describes the relation between reference frames (relative pose = position & orientation) transforms the representation of a position vector (applied vector from the origin of the frame) from a given frame to another frame it is a roto-translation operator on vectors in the three-dimensional space A -1 B it is always invertible ( TB) = TA A A B can be composed, i.e., TC = TB TC ← note: it does not commute! Robotics 1 10 Inverse of a homogeneous transformation A A A B B B B A A T A A T A p = pAB + RB p p = pBA + RA p = - RB pAB + RB p A A B B A T A T A RB pAB RA pBA RB - RB pAB = 0 0 0 1 0 0 0 1 0 0 0 1 A B A -1 TB TA ( TB) Robotics 1 11 Defining a robot task yE absolute definition of task task definition relative RFE to the robot end-effector zE RFB W W B E 2 • TT = TB TE TT 1 • RFT •3 known, once the robot direct kinematics of the is placed robot arm (function of q) RFW B W -1 W E -1 TE(q) = TB TT TT = cost Robotics 1 12 Final comments on T matrices they are the main tool for computing the direct kinematics of robot manipulators they are used in many application areas (in robotics and beyond) b in the positioning of a vision camera (matrix Tc with the extrinsic parameters of the camera posture) in computer graphics, for the real-time visualization of 3D solid objects when changing the observation point A A RB pAB A TB = αx αy αz σ all zero coefficients of scaling always unitary in robotics perspective coefficient in robotics deformation Robotics 1 13 Robotics 1 Direct kinematics Prof.