Introduction to ROBOTICS Robot Kinematics II Dr. Jizhong Xiao Department of Electrical Engineering City College of New York jxiao@ccny.cuny.edu The City College of New York 1 Outline • Review – Manipulator Specifications • Precision, Repeatability – Homogeneous Matrix • Denavit-Hartenberg (D-H) Representation • Kinematics Equations • Inverse Kinematics The City College of New York 2 Review • Manipulator, Robot arms, Industrial robot – A chain of rigid bodies (links) connected by joints (revolute or prismatic) • Manipulator Specification – DOF, Redundant Robot – Workspace, Payload How accurately a specified point can be reached – Precision accurately the same position can be reached – Repeatability How if the motion is repeated many times The City College of New York 3 Review • Manipulators: Cartesian: PPP Cylindrical: RPP Spherical: RRP Hand coordinate: Articulated: RRR SCARA: RRP n: normal vector; s: sliding vector; (Selective Compliance Assembly Robot Arm) a: approach vector, normal to the tool mounting plate The City College of New York 4 Review • Basic Rotation Matrix px i x i u Pxyz p y jy i u p z k z i u Pxyz RPuvw i x jv j y jv k z jv i x k w pu jy k w pv RPuvw k z k w pw z w P Puvw QPxyz Q R1 RT u v y x The City College of New York 5 Basic Rotation Matrices – Rotation about x-axis with 1 0 Rot( x, ) 0 C 0 S 0 S C – Rotation about y-axis with C Rot( y, ) 0 S – Rotation about z-axis with Pxyz RPuvw C Rot( z , ) S 0 The City College of New York S 1 0 0 C 0 S C 0 0 0 1 6 Review • Coordinate transformation from {B} to {A} A P A r RB B r P Ar o' A A o' r RB r B r P 1 1 1 013 • Homogeneous transformation matrix A P RB TB 013 A A r R33 1 0 A o' P31 1 Scaling The City College of New York Rotation matrix Position vector 7 Review • Homogeneous Transformation – Special cases 1. Translation I 33 A TB 013 A o' r 1 A RB A TB 013 031 1 2. Rotation The City College of New York 8 Review • Composite Homogeneous Transformation Matrix • Rules: – Transformation (rotation/translation) w.r.t. (X,Y,Z) (OLD FRAME), using pre-multiplication – Transformation (rotation/translation) w.r.t. (U,V,W) (NEW FRAME), using postmultiplication The City College of New York 9 Review • Homogeneous Representation – A point in R 3 space px p P y Homogeneous coordinate of P w.r.t. OXYZ pz P( px , z 1 a – A frame in R space 3 nx n s a P n y F 0 0 0 1 nz 0 sx sy sz 0 ax ay az 0 px p y pz 1 The City College of New York py , pz ) s n y x 10 Review • Orientation Representation (Euler Angles) – Description of Yaw, Pitch, Roll • A rotation of about the OX axis ( Rx, ) -- yaw • A rotation of about the OY axis ( Ry , ) -- pitch yaw • A rotation of about the OZ axis ( Rz , ) -- roll Z roll Y pitch X The City College of New York 11 Quiz 1 • How to get the resultant rotation matrix for YPR? T Rz , Ry , Rx, C S 0 0 S C 0 0 0 0 1 0 0 C 0 0 0 S 1 0 Z X 0 S 1 0 0 C 0 0 0 0 0 1 1 0 0 C 0 S 0 0 0 S C 0 0 0 0 1 The City College of New York Y 12 Quiz 2 • Geometric Interpretation? R33 T 0 P31 1 Orientation of OUVW coordinate frame w.r.t. OXYZ frame Position of the origin of OUVW coordinate frame w.r.t. OXYZ frame • Inverse Homogeneous Matrix? T T Inverse of the rotation submatrix R R P is equivalent to its transpose T 1 Position of the origin of OXYZ 1 reference frame w.r.t. OUVW frame 0 T R 1 T T 0 R T P R P R T R 0 I 44 1 0 1 0 1 The City College of New York 13 Kinematics Model • Forward (direct) Kinematics q (q1 , q2 ,qn ) Joint variables Direct Kinematics z Position and Orientation of the end-effector y x Inverse Kinematics Y ( x, y, z, , , ) • Inverse Kinematics The City College of New York 14 Robot Links and Joints The City College of New York 15 Denavit-Hartenberg Convention • Number the joints from 1 to n starting with the base and ending with the end-effector. • Establish the base coordinate system. Establish a right-handed orthonormal coordinate system ( X 0 , Y0 , Z0 ) at the supporting base with Z 0 axis lying along the axis of motion of joint 1. • Establish joint axis. Align the Zi with the axis of motion (rotary or sliding) of joint i+1. • Establish the origin of the ith coordinate system. Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis. • Establish Xi axis. Establish X i (Zi1 Zi ) / Zi1 Zi or along the common normal between the Zi-1 & Zi axes when they are parallel. • Establish Yi axis. Assign Yi (Zi X i ) / Zi X i to complete the right-handed coordinate system. • Find the link and joint parameters The City College of New York 16 Example I • 3 Revolute Joints Z3 end-effector frame Z1 Z0 Y0 O3 Y1 Link 1 Joint 1 Joint 3 O0 X0 Joint 2 Link 2 X3 d2 O1 X1 O2 X2 Y2 a0 a1 The City College of New York 17 Link Coordinate Frames • Assign Link Coordinate Frames: – To describe the geometry of robot motion, we assign a Cartesian coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows: • establish a right-handed orthonormal coordinate frame O0 at the supporting base with Z0 lying along joint 1 motion axis. • the Zi axis is directed along the axis of motion of joint (i + 1), that is, link (i + 1) rotates about or translates along Zi; Z3 Z1 Z0 Y0 O3 Y1 Link 1 Joint 1 Joint 3 O0 X0 Joint 2 Link 2 X3 d2 O1 X1 O2 X2 Y2 a0 a1 The City College of New York 18 Link Coordinate Frames – Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis. – the Xi axis lies along the common normal from the Zi-1 axis to the Zi axis X i (Zi1 Zi ) / Zi1 Zi , (if Zi-1 is parallel to Zi, then Xi is specified arbitrarily, subject only Z3 to Xi being perpendicular to Zi); Z1 Z0 Y0 Joint 3 O3 Y1 X3 d2 Joint 1 O0 X0 Joint 2 O1 X1 O2 X2 Y2 a0 a1 The City College of New York 19 Link Coordinate Frames – Assign Yi (Zi X i ) / Zi X i to complete the righthanded coordinate system. • The hand coordinate frame is specified by the geometry On of the end-effector. Normally, establish Zn along the direction of Zn-1 axis and pointing away from the robot; establish Xn such that it is normal to both Zn-1 and Zn axes. Assign Yn to complete the right-handed coordinate system. Z3 Z1 Z0 Y0 Joint 3 O3 Y1 X3 d2 Joint 1 O0 X0 Joint 2 O1 X1 O2 X2 Y2 a0 a1 The City College of New York 20 Link and Joint Parameters • Joint angle i : the angle of rotation from the Xi-1 axis to the Xi axis about the Zi-1 axis. It is the joint variable if joint i is rotary. • Joint distance di : the distance from the origin of the (i-1) coordinate system to the intersection of the Zi-1 axis and the Xi axis along the Zi-1 axis. It is the joint variable if joint i is prismatic. • Link length a i : the distance from the intersection of the Zi-1 axis and the Xi axis to the origin of the ith coordinate system along the Xi axis. • Link twist angle i : the angle of rotation from the Zi-1 axis to the Zi axis about the Xi axis. The City College of New York 21 Example I Z3 Z1 Z0 Y0 Joint 3 O3 Y1 X3 d2 Joint 1 O0 X0 Joint 2 O1 X1 O2 X2 Y2 a0 a1 i : rotation angle from Zi-1 to Zi about Xi a i : distance from intersection of Zi-1 & Xi D-H Link Parameter Table Joint i i ai di i 1 0 a0 0 1 2 -90 a1 0 2 3 0 0 d2 3 to origin of i coordinate along Xi di : distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1 i : rotation angle from Xi-1 to Xi about Zi-1 The City College of New York 22 Example II: PUMA 260 1 2 Z1 3 O1 Y1 Z0 1. Number the joints 2. Establish base frame 3. Establish joint axis Zi 4. Locate origin, (intersect. of Zi & Zi-1) OR (intersect of common normal & Zi ) X1 Z 2 Z6 5. O2 Y3 Z4 Z X 2 5 6 Y6 O3 Y2 5 O6 Y5 X 3 Y4 t O5 O X 5 X 6 4 Z3 X4 Establish Xi,Yi X i (Zi 1 Zi ) / Zi 1 Zi Yi (Zi X i ) / Zi X i 4 PUMA 260 The City College of New York 23 Link Parameters 1 J 2 1 Z1 2 3 O1 3 X1 Z 2 Z6 Y1 O2 Y3 Z4 Z X 2 5 6 Y6 O3 Y2 5 O6 Z0 Y5 X 3 Y4 O5 O X 5 X 6 4 Z3 X4 Joint distance 4 4 5 6 i 1 2 3 4 5 6 i a i di -90 0 13 0 8 0 90 0 -90 0 -l 8 90 0 0 0 0 t i : angle from Xi-1 to Xi about Zi-1 i : angle from Zi-1 to Zi about Xi a i : distance from intersection of Zi-1 & Xi to Oi along Xi di : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1 The City College of New York 24 Transformation between i-1 and i • Four successive elementary transformations are required to relate the i-th coordinate frame to the (i-1)-th coordinate frame: – Rotate about the Z i-1 axis an angle of i to align the X i-1 axis with the X i axis. – Translate along the Z i-1 axis a distance of di, to bring Xi-1 and Xi axes into coincidence. – Translate along the Xi axis a distance of ai to bring the two origins Oi-1 and Oi as well as the X axis into coincidence. – Rotate about the Xi axis an angle of αi ( in the righthanded sense), to bring the two coordinates into coincidence. The City College of New York 25 Transformation between i-1 and i • D-H transformation matrix for adjacent coordinate frames, i and i-1. – The position and orientation of the i-th frame coordinate can be expressed in the (i-1)th frame by the following homogeneous transformation matrix: Source coordinate Ti i 1 T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R ( xi , i ) Reference Coordinate C i S i 0 0 C i S i S i S i C i C i S i C i S i 0 C i 0 The City College of New York ai C i ai S i di 1 26 Kinematic Equations q (q1 , q2 ,qn ) • Forward Kinematics – Given joint variables – End-effector position & orientation Y ( x, y, z, , , ) • Homogeneous matrix T0n – specifies the location of the ith coordinate frame w.r.t. the base coordinate system – chain product of successive coordinate transformation matrices of Ti i 1 T T T T n 0 Orientation matrix R0n 0 1 2 0 1 n n 1 Position vector P0n n s a P0n 1 0 0 0 1 The City College of New York 27 Kinematics Equations • Other representations – reference from, tool frame 0 Treftool Bref T0n H ntool – Yaw-Pitch-Roll representation for orientation T Rz , Ry , Rx, C S 0 0 S C 0 0 0 0 1 0 0 C 0 0 0 S 1 0 0 S 1 0 0 C 0 0 The City College of New York 0 0 0 1 1 0 0 C 0 S 0 0 0 S C 0 0 0 0 1 28 Representing forward kinematics • Forward kinematics px 1 p y 2 pz 3 4 5 6 • Transformation Matrix nx n y T nz 0 The City College of New York sx sy sz 0 ax ay az 0 px p y pz 1 29 Representing forward kinematics • Yaw-Pitch-Roll representation for orientation CC CSS SC SC SSS CC T0n S CS 0 0 nx n T0n y nz 0 Problem? sx sy sz 0 ax ay az 0 px p y pz 1 CSC SS SSC CS CC 0 sin (nz ) 1 px p y pz 1 az cos ( ) cos nx 1 cos ( ) cos 1 Solution is inconsistent and ill-conditioned!! The City College of New York 30 atan2(y,x) y x 0 90 for x and y for x and y 90 180 a tan 2( y, x) 180 90 for x and y 90 0 for x and y The City College of New York 31 Yaw-Pitch-Roll Representation T Rz , Ry , Rx, C S 0 0 S C 0 0 nx n y nz 0 sx sy sz 0 0 0 1 0 0 0 0 1 ax ay az 0 0 0 0 1 C 0 S 0 0 S 1 0 0 C 0 0 0 0 0 1 The City College of New York 1 0 0 C 0 S 0 0 0 S C 0 0 0 0 1 32 Yaw-Pitch-Roll Representation 1 z , R T Ry, Rx, C S 0 0 C 0 S 0 S C 0 0 0 0 1 0 0 0 0 1 nx n y nz 0 0 S 1 0 0 C 0 0 0 0 0 1 1 0 0 C 0 S 0 0 sx sy sz 0 ax ay az 0 0 0 0 1 0 S C 0 0 0 0 1 The City College of New York (Equation A) 33 Yaw-Pitch-Roll Representation • Compare LHS and RHS of Equation A, we have: sin nx cos ny 0 a tan2(ny , nx ) cos nx sin n y cos nz sin a tan2(nz , cos nz sin ny ) sin s x cos s y cos sin ax cos a y sin a tan2(sin ax cos ay , sin sx cos s y ) The City College of New York 34 Kinematic Model • Steps to derive kinematics model: – Assign D-H coordinates frames – Find link parameters – Transformation matrices of adjacent joints – Calculate Kinematics Matrix – When necessary, Euler angle representation The City College of New York 35 Example Z3 Z1 Z0 Y0 Joint 3 O3 Y1 X3 d2 Joint 1 O0 X0 Joint 2 O1 X1 O2 X2 Y2 a0 a1 Joint i i ai di i 1 0 a0 0 1 2 -90 a1 0 2 3 0 0 d2 3 The City College of New York 36 Example Joint i i ai di i 1 0 a0 0 1 2 -90 a1 0 2 3 0 0 d2 3 Ti i 1 C i S i 0 0 C i S i C i C i S i 0 S i S i S i C i C i 0 T03 (T 01)(T 21)(T 23) ai C i ai S i di 1 cosθ1 sinθ 1 T 01 0 0 cosθ 2 2 sinθ 2 T 1 0 0 cosθ 3 sin 3 T 23 0 0 The City College of New York sinθ1 cosθ1 0 0 0 a0 cos1 0 a0 sin 1 1 0 0 1 0 sin 2 0 cos 2 1 0 0 0 sinθ 3 cos 3 0 0 a1 cos 2 a1 sin 2 0 1 0 0 0 0 1 d2 0 1 37 Example: Puma 560 The City College of New York 38 Example: Puma 560 The City College of New York 39 Link Coordinate Parameters PUMA 560 robot arm link coordinate parameters Joint i i i 1 1 -90 2 2 0 431.8 149.09 3 3 90 -20.32 0 4 5 4 5 -90 90 0 0 433.07 0 6 6 0 0 56.25 ai(mm) di(mm) 0 The City College of New York 0 40 Example: Puma 560 The City College of New York 41 Example: Puma 560 The City College of New York 42 Inverse Kinematics • Given a desired position (P) & orientation (R) of the endz effector q (q1 , q2 ,qn ) • Find the joint variables which can bring the robot the desired configuration The City College of New York y x 43 Inverse Kinematics • More difficult – Systematic closed-form solution in general is not available – Solution not unique (x , y) • Redundant robot • Elbow-up/elbow-down configuration – Robot dependent The City College of New York 44 Inverse Kinematics • Transformation Matrix nx n T y nz 0 sx sy sz 0 ax ay az 0 px p y T01T12T23T34T45T56 pz 1 1 2 3 4 5 6 Special cases make the closed-form arm solution possible: 1. Three adjacent joint axes intersecting (PUMA, Stanford) 2. Three adjacent joint axes parallel to one another (MINIMOVER) The City College of New York 45 Thank you! Homework 2 posted on the web. Next class: Inverse Kinematics, Jocobian Matrix, Trajectory planningz y z z y x z x y x y x The City College of New York 46