Carnegie Mellon An Introduction to Robot Kinematics © Howie Choset, 2007 Carnegie Mellon An Example - The PUMA 560 2 3 4 1 There are two more joints on the end effector (the gripper) The PUMA 560 has SIX revolute joints A revolute joint has ONE degree of freedom ( 1 DOF) that is defined by its angle Carnegie Mellon Other basic joints Revolute Joint 1 DOF ( Variable - ) Prismatic Joint 1 DOF (linear) (Variables - d) Spherical Joint 3 DOF ( Variables - 1, 2, 3) Carnegie Mellon We are interested in two kinematics topics Forward Kinematics (angles to position) What you are given: The length of each link The angle of each joint What you can find: The position of any point (i.e. it’s (x, y, z) coordinates Inverse Kinematics (position to angles) What you are given: The length of each link The position of some point on the robot What you can find: The angles of each joint needed to obtain that position Carnegie Mellon Quick Math Review Dot Product: ax a Geometric Representation: y b A B A B cosθ A x b θ y Matrix Representation: B ax bx A B axbx ayby ay by Unit Vector Vector in the direction of a chosen vector but whose magnitude is 1. B u B B uB Carnegie Mellon Quick Matrix Review Matrix Multiplication: An (m x n) matrix A and an (n x p) matrix B, can be multiplied since the number of columns of A is equal to the number of rows of B. Non-Commutative Multiplication AB is NOT equal to BA a b e f ae bg af bh c d g h ce dg cf dh Matrix Addition: a b e f a e b f c d g h c g d h Carnegie Mellon Basic Transformations Moving Between Coordinate Frames Translation Along the X-Axis Y O (VN,VO) VO P N X VN Px Px = distance between the XY and NO coordinate planes V X V N P Notation: XY NO P x V Y V O V V 0 Carnegie Mellon Writing V XY in terms of VNO Y O VO P N X VN P VN V XY X P V NO O V Carnegie Mellon O Translation along the X-Axis and Y-Axis Y VO N VN XY Px X P PY N XY NO PX V V P V O PY V Carnegie Mellon Using Basis Vectors Basis vectors are unit vectors that point along a coordinate axis O n Unit vector along the N-Axis o Unit vector along the N-Axis VO V NO Magnitude of the VNO vector o n VN N N NO NO NO NO V V cosθ V cosθ V n V O NO NO NO V V sinθ V cos(90 θ) V o Carnegie Mellon Y Rotation (around the Z-Axis) Z X Y V VY X VX = Angle of rotation between the XY and NO coordinate axis X N XY V NO V V Y V O V V Carnegie Mellon Y x Unit vector along X-Axis V Can be considered with respect to the XY coordinates or NO coordinates V VXY VNO VY X VX VX VXY cosα VNO cosα VNO x X N O (Substituting for VNO using the N and O V (V n V o) x components of the vector) VX VN (xn) VO (xo) VN (cosθ) VO (cos(θ 90)) VN (cosθ) VO (sinθ) Carnegie Mellon Similarly…. VY VNO sinα VNO cos(90α) VNO y VY (VN n VO o) y VY VN (y n) VO (y o) VN (cos(90 θ)) VO (cosθ) VN (sinθ) VO (cosθ) So…. VX VN (cosθ) VO (sinθ) VY VN (sinθ) VO (cosθ) Written in Matrix Form X N XY V cosθ X sinθV Rotation Matrix about the z-axis V XY V Y V O V sinθ Ycos θ V V Carnegie Mellon Y1 (VN,VO) Y0 VNO VXY 1 P X Translation along P followed by rotation by X0 VX P cosθ sinθVN VXY x Y O V Py sinθ cosθ V (Note : Px, Py are relative to the original coordinate frame. Translation followed by rotation is different than rotation followed by translation.) In other words, knowing the coordinates of a point (VN,VO) in some coordinate frame (NO) you can find the position of that point relative to your original coordinate frame (X0Y0). Carnegie Mellon HOMOGENEOUS REPRESENTATION Putting it all into a Matrix What we found by doing a translation and a rotation X N V Px cosθ sinθ 0V Padding with 0’s and 1’s V Y P sinθ cosθ 0 VO y 1 1 0 0 1 1 X N XY V Px cosθ sinθV V XV cos θ sin θ P VN Y P x O V y sinθ cosθ V VY sinθ cosθ P VO Simplifying into a matrix form y 1 0 0 1 1 cosθ sinθ Px H sinθ cosθ Py Homogenous Matrix for a Translation in XY plane, followed by a Rotation around 0 0 1 the z-axis Carnegie Mellon Rotation Matrices in 3D – OK,lets return from homogenous repn cosθ sinθ 0 Rotation around the Z-Axis R sinθ cosθ 0 z 0 0 1 cosθ 0 sinθ R 0 1 0 Rotation around the Y-Axis y sinθ 0 cosθ 1 0 0 R 0 cosθ sinθ Rotation around the X-Axis z 0 sinθ cosθ Carnegie Mellon Homogeneous Matrices in 3D H is a 4x4 matrix that can describe a translation, rotation, or both in one matrix O 1 0 0 Px Y 0 1 0 Py N P H 0 0 1 Pz X A 0 0 0 1 Translation without rotation Z n o a 0 Y x x x n o a 0 O y y y N H nz oz az 0 X 0 0 0 1 Rotation part: Z Rotation without translation Could be rotation around z-axis, A x-axis, y-axis or a combination of the three. Carnegie Mellon Homogeneous Continued…. V N VO The (n,o,a) position of a point relative to the V XY H current coordinate frame you are in. V A 1 N nx ox ax Px V O X N O A n o a P V V n V o V a V P V XY y y y y x x x x A nz oz az Pz V 0 0 0 1 1 The rotation and translation part can be combined into a single homogeneous matrix IF and ONLY IF both are relative to the same coordinate frame. Carnegie Mellon Finding the Homogeneous Matrix EX. Y W N O W A W T P X Z I X W W Point relative to the Point relative to the Point relative to the J W Y W I-J-K frame N-O-A frame X-Y-Z frame K Z W W I N I N W ni oi ai Pi W W Pi ni oi ai W J O J O W n j o j a j Pj W W Pj n j oj aj W K A K A W P n o a W W nk ok ak Pk W k k k k 1 0 0 0 1 1 Carnegie Mellon Y T P X W N O Z W W A WX i j k T WI WX T i j k WI x x x x x x x x Y J W iy jy k y Ty W WY T i j k WJ y y y y Z K W iz jz k z Tz W WZ T i j k Wk z z z z 1 0 0 0 1 1 WI J W K W X N W ix jx k x Tx ni oi ai Pi W Y O W i j k T n o a P W Substituting for y y y y j j j j Z A W iz jz k z Tz nk ok ak Pk W 1 0 0 0 1 0 0 0 1 1 Carnegie Mellon X N W W i j k T n o a P x x x x i i i i WY WO iy jy k y Ty n j o j a j Pj H H WZ W A iz jz k z Tz nk ok ak Pk 1 1 0 0 0 1 0 0 0 1 Product of the two matrices Notice that H can also be written as: 1 0 0 Tx ix jx k x 01 0 0 Pi ni oi ai 0 0 1 0 T i j k 00 1 0 P n o a 0 H y y y y j j j j 0 0 1 Tz iz jz k z 00 0 1 Pk nk ok ak 0 0 0 0 1 0 0 0 10 0 0 1 0 0 0 1 H = (Translation relative to the XYZ frame) * (Rotation relative to the XYZ frame) * (Translation relative to the IJK frame) * (Rotation relative to the IJK frame) Carnegie Mellon The Homogeneous Matrix is a concatenation of numerous translations and rotations Y T W N O P W X A W Z One more variation on finding H: H = (Rotate so that the X-axis is aligned with T) * ( Translate along the new t-axis by || T || (magnitude of T)) * ( Rotate so that the t-axis is aligned with P) * ( Translate along the p-axis by || P || ) * ( Rotate so that the p-axis is aligned with the O-axis) This method might seem a bit confusing, but it’s actually an easier way to solve our problem given the information we have.