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:
q0 xn
z0
qi+1 zn
qi
‘0’ y0 αi ‘n’ yn
a zi
x0 i
x i

di zi-1
qi
where 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. xn
q0
z
• Link twist αi is angle between zi-1 0
qi+1 zn
qi
and zi measured in plane normal ‘0’ y0 αi ‘n’ yn
ai zi
to xi (right-hand rule). x0 xi
• Link offset di is distance from origin di zi-1
qi
of frame i-1 to the intersection xi 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.

• Homogenous transformation matrix Tji expresses position and

orientation of ojxjyjzj with respect to oixiyizi:
i
Tj = Ai +1 Ai +2 K A j −1 A j
Introduction Robotics, lecture 3 of 7
 13/39

Forward kinematics (2/2)

• Forward kinematics of a serial manipulator with n joints can be
represented by homogenous transformation matrix Hn0 which
defines position and orientation of the end-effector’s (tip)
frame onxnynzn relative to the base coordinate frame o0x0y0z0:
H n0 (q) = Tn0 (q) = A1 (q1 ) ⋅ K ⋅ An (qn ),
0  Rn
0
(q ) x 0
n (q )

H n (q) =  
0
 3×1 1 
03×1 = [0 0 0];

q = [q1 L qn ]
T

Introduction Robotics, lecture 3 of 7

 14/39

Case-study: RRR robot manipulator

d3 x3
x2 y3
z3
y2 -q3
d2 a3
y1 z2
x1 elbow
α1 q2 a2

shoulder z1
z0
d1 α1 - twist angle
y0 ai - link lenghts
q1
waist di - link offsets
x0
Introduction Robotics, lecture 3 of 7 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:
T03 (q) = A1(q)A 2 (q)A 3 (q) =
 R 03 (q) x 03(q)
= 
 01×3 1 
where
q = [ q1 q2 q3 ]T x03(q) = [ x y z ]T

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 = cosq1[a3cos( q2 + q3 ) + a2cosq2 ] + ( d 2 + d3 )sinq1
,
y = sin q1[a3cos( q2 + q3 ) + a2cosq2 ] − ( d 2 + d3 )cosq1
, z = a3sin( q2 + q3 ) + a2sinq2 + d1

• Orientation of end-effector:
cosq1cos( q2 + q3 ) − cosq1sin( q2 + q3 ) sinq1 
R30 =  sinq1cos( q2 + q3 ) − sinq1sin( q2 + q3 ) − cosq1 
 
 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
of the base; T0n is product of homogenous transformation
matrices relating successive coordinate frames:

Introduction Robotics, lecture 3 of 7

 23/39

The general IK problem (2/2)

• Since the bottom rows of both T0n and H are equal to [0 0 0 1],
equation

gives rise to 4 trivial equations and 12 equations in n unknowns

q1,…,qn:

Here, Tij and Hij are nontrivial elements of T0n 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

• Rotational equations (correspond to R06):

• One solution: • Positional equations (correspond to o06):

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 Atan2(xc,yc) is defined for all (xc,yc)≠(0,0)

and equals the unique angle θ1 such that:
xc yc
cos θ1 = , sin θ1 = .
2 2 2 2
Introduction Robotics, lecture 3 of 7 xc + yc xc + yc
 32/39

Articulated manipulator: left arm configuration

Introduction Robotics, lecture 3 of 7

 33/39

Articulated manipulator: right arm configuration

*
Introduction Robotics, lecture 3 of 7 +π
 34/39

Articulated manipulator: IK solution for θ3

• 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=Atan2(r,s)
υ2=Atan2(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.

Introduction Robotics, lecture 3 of 7

 37/39

Articulated manipulator: inverse orientation problem

Introduction Robotics, lecture 3 of 7 • Equation to solve:

 38/39

Articulated manipulator: IK solutions for θ4 and θ5

• Equations given by the third column in :

• If not both right-hand sides of the first two equations are zero:
±

• If positive square root is chosen in solution for θ5:

Introduction Robotics, lecture 3 of 7

 39/39

Articulated manipulator: IK solutions for θ6

• The first two equations given by the last row in :
-s5c6 = s1r11 - c1r21
s5s6 = s1r12 - c1r22

• From these equations it follows:

• Analogous approach if negative square root is chosen in

solution for θ5.

Introduction Robotics, lecture 3 of 7