1/39 Introduction Robotics dr Dragan Kosti ć WTB Dynamics and Control September-October 2009 Introduction Robotics, lecture 3 of 7 2/39 Outline • Recapitulation • Forward kinematics • Inverse kinematics problem Introduction Robotics, lecture 3 of 7 3/39 Recapitulation Introduction Robotics, lecture 3 of 7 4/39 Robot manipulators • Kinematic chain is series of links and joints. SCARA geometry • Types of joints: – rotary (revolute, θ) – prismatic (translational, d). Introduction Robotics, lecture 3 of 7 schematic representations of robot joints 5/39 Common geometries of robot manipulators Introduction Robotics, lecture 3 of 7 6/39 Forward kinematics problem • Determine position and orientation of the end-effector as function of displacements in robot joints. Introduction Robotics, lecture 3 of 7 7/39 DH convention for homogenous transformations (1/2) • An arbitrary homogeneous transformation is based on 6 independent variables: 3 for rotation + 3 for translation. • DH convention reduces 6 to 4, by specific choice of the coordinate frames. • In DH convention, each homogeneous transformation has the form: Introduction Robotics, lecture 3 of 7 8/39 DH convention for homogenous transformations (2/2) • Position and orientation of coordinate frame i with respect to frame i-1 is specified by homogenous transformation matrix: q xn z 0 0 q i+1 zn qi ‘0’ n y0 αi ‘ ’ yn zi ai x0 xi di z i-1 q where i xi-1 Introduction Robotics, lecture 3 of 7 9/39 Physical meaning of DH parameters • Link length ai is distance from zi-1 to zi measured along xi. q xn z 0 • Link twist α is angle between z 0 q i i-1 i+1 zn qi and z measured in plane normal ‘0’ n i y0 αi ‘ ’ yn zi ai to x (right -hand rule). x0 i xi • Link offset di is distance from origin di z i-1 q of frame i-1 to the intersection x i i xi-1 with zi-1, measured along zi-1. • Joint angle θi is angle from xi-1 to xi measured in plane normal to zi-1 (right-hand rule). Introduction Robotics, lecture 3 of 7 10/39 DH convention to assign coordinate frames 1. Assign zi to be the axis of actuation for joint i+1 (unless otherwise stated zn coincides with zn-1). 2. Choose x0 and y0 so that the base frame is right-handed. 3. Iterative procedure for choosing oixiyizi depending on oi-1xi-1yi-1zi-1 (i=1, 2, …, n-1): a) zi−1 and zi are not coplanar; there is an unique shortest line segment from zi−1 to zi, perpendicular to both; this line segment defines xi and the point where the line intersects zi is the origin oi; choose yi to form a right-handed frame, b) zi−1 is parallel to zi; there are infinitely many common normals; choose xi as the normal passes through oi−1 ; choose oi as the point at which this normal intersects zi; choose yi to form a right-handed frame, c) zi−1 intersects zi; axis xi is chosen normal to the plane formed by zi and zi−1 ; it’s positive direction is arbitrary; the most natural choice of oi is the intersection of zi and zi−1 , however, any point along the zi suffices; choose yi to form a right-handed frame. Introduction Robotics, lecture 3 of 7 11/39 Forward Kinematics Introduction Robotics, lecture 3 of 7 12/39 Forward kinematics (1/2) • Homogenous transformation matrix relating the frame oixiyizi to oi-1xi-1yi-1zi-1: Ai specifies position and orientation of oixiyizi w.r.t. oi-1xi-1yi-1zi-1. i • Homogenous transformation matrix Tj expresses position and orientation of ojxjyjzj with respect to oixiyizi: i K T j = Ai+1Ai+2 Aj−1Aj Introduction Robotics, lecture 3 of 7 13/39 Forward kinematics (2/2) • Forward kinematics of a serial manipulator with n joints can be 0 represented by homogenous transformation matrix Hn which defines position and orientation of the end-effector’s (tip) frame onxnynzn relative to the base coordinate frame o0x0y0z0: 0 0 K H n (q) = Tn (q) = A1(q1) ⋅ ⋅ An (qn ), 0 0 0 Rn (q) xn (q) H n (q) = 03×1 1 03×1 = [0 0 0]; L T q = [q1 qn ] Introduction Robotics, lecture 3 of 7 14/39 Case-study: RRR robot manipulator d3 x3 x2 y3 z3 y 2 -q 3 d a3 y 2 1 z2 x1 elbow α1 q 2 a2 z1 z0 shoulder d1 α1 - twist angle y 0 ai - link lenghts q1 waist di - link offsets Introduction Robotics, lecture 3 of 7 x0 qi - displacements 15/39 DH parameters of RRR robot manipulator Introduction Robotics, lecture 3 of 7 16/39 Forward kinematics of RRR robot manipulator (1/2) • Coordinate frame o3x3y3z3 is related with the base frame o0x0y0z0 via homogenous transformation matrix: 0 T3(q) = A1(q)A 2(q)A 3(q) = R0(q) x0(q) = 3 3 01×3 1 where T 0 T q = [q1 q2 q3] x3(q) = [x y z] 01×3 = [0 0 0] Introduction Robotics, lecture 3 of 7 17/39 Forward kinematics of RRR robot manipulator (2/2) • Position of end-effector: x = cos q1[a3cos( q2 + q3) + a2cos q2 ]+ (d2 + d3)sin q1 , y = sin q1[a3cos( q2 + q3) + a2cos q2 ]−(d2 + d3)cos q1 , z = a3sin( q2 + q3) + a2sin q2 + d1 • Orientation of end-effector: cos q1cos( q2 + q3) − cos q1sin( q2 + q3) sin q1 0 R3 = sin q1cos( q2 + q3) − sin q1sin( q2 + q3) − cos q1 sin( q2 + q3) cos( q2 + q3) 0 Introduction Robotics, lecture 3 of 7 18/39 Inverse Kinematics Introduction Robotics, lecture 3 of 7 19/39 Inverse kinematics problem • Inverse kinematics (IK): determine displacements in robot joints that correspond to given position and orientation of the end-effector. Introduction Robotics, lecture 3 of 7 20/39 Illustration: IK for planar RR manipulator (1/2) • Elbow down IK solution Introduction Robotics, lecture 3 of 7 21/39 Illustration: IK for planar RR manipulator (2/2) • Elbow up IK solution Introduction Robotics, lecture 3 of 7 22/39 The general IK problem (1/2) • Given a homogenous transformation matrix H∈SE (3) find (multiple) solution(s) q1,…,qn to equation • Here, H represents the desired position and orientation of the tip coordinate frame onxnynzn relative to coordinate frame o0x0y0z0 0 of the base; T n is product of homogenous transformation matrices relating successive coordinate frames: Introduction Robotics, lecture 3 of 7 23/39 The general IK problem (2/2) 0 • Since the bottom rows of both T n and H are equal to [0 0 0 1], equation gives rise to 4 trivial equations and 12 equations in n unknowns q1,…,qn: 0 Here, Tij and Hij are nontrivial elements of T n and H. Introduction Robotics, lecture 3 of 7 24/39 Illustration: Stanford manipulator Introduction Robotics, lecture 3 of 7 25/39 FK of Stanford manipulator Introduction Robotics, lecture 3 of 7 26/39 Example of IK solution for Stanford manipulator 0 • Rotational equations (correspond to R 6): 0 • One solution: • Positional equations (correspond to o 6): Introduction Robotics, lecture 3 of 7 27/39 Nature of IK solutions • FK problem has always unique solution whereas IK problem may or may not have a solution; if IK solution exists, it may or may not be unique; solving IK equations, in general, is much too difficult. • It is preferable to find IK solutions in closed-form: – faster computation (e.g. at sampling time of 1 [ms]), – if multiple IK solutions exist, then closed-form allows us to develop rules for choosing a particular solution among several. • Existence of IK solutions depends on mathematical as well as engineering considerations. • We assume that the given position and orientation is such that at least one IK solution exists. Introduction Robotics, lecture 3 of 7 28/39 Kinematic decoupling (1/3) • General IK problem is difficult BUT for manipulators having 6 joints with the last 3 joint axes intersecting at one point, it is possible to decouple the general IK problem into two simpler problems: inverse position kinematics and inverse orientation kinematics. • IK problem: for given R and o solve 9 rotational and 3 positional equations: Introduction Robotics, lecture 3 of 7 29/39 Kinematic decoupling (2/3) • Spherical wrist as paradigm. • Let oc be the intersection of the last 3 joint axes; as z3, z4, and z5 intersect at oc, the origins o4 and o5 will always be at oc; the motion of joints 4, 5 and 6 will not change the position of oc; only motions of joints 1, 2 and 3 can influence position of oc. Introduction Robotics, lecture 3 of 7 30/39 Kinematic decoupling (3/3) ⇒ q1, q2, q3 ⇒ ⇒ Introduction Robotics, lecture 3 of 7 q4, q5, q6 31/39 Articulated manipulator: inverse position problem • Inverse tangent function Atan 2( xc,yc) is defined for all (xc,yc)≠(0,0) and equals the unique angle θ such that: 1 x y cos θ = c , sin θ = c . 1 2 2 1 2 2 Introduction Robotics, lecture 3 of 7 xc + yc xc + yc 32/39 Articulated manipulator: left arm configuration r * * Introduction Robotics, lecture 3 of 7 33/39 Articulated manipulator: right arm configuration * r * Introduction Robotics, lecture 3 of 7 +π 34/39 Articulated manipulator: IK solution for θθθ3 r * • Law of cosines: Introduction Robotics, lecture 3 of 7 + elbow down; - elbow up 35/39 Articulated manipulator: IK solution for θθθ2 υυυ2 θ2=υ1- υ2 υυυ 1 υ1=Atan 2( r,s) υ2=Atan 2( a2+a3cos θ3,a3sin θ3) Introduction Robotics, lecture 3 of 7 36/39 Four IK solutions θθθ1-θθθ3 for articulated manipulator • PUMA robot as an example of the articulated geometry.