İTÜ-EEF Dept.

Of Control Engineering

Ch. 3: Forward and Inverse

Kinematics

KON 318E : Introduction to Robotics 1 1

 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : General IK Problem

• Inverse Kinematics problem is to find the joint veriables (q1, …qn) in terms of
end-effektors position end orientation (x,y,z,,,)

0 Forward
T n kinematics

Cartesian
Joint Space
Space
(q1,q2,…,qN)
(x,y,z,,,)

Inverse
kinematics
T n
0 1

(arm solution)

KON 318E : Introduction to Robotics 18

18
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : General IK Problem

• Given a homogeneous transformation :

 R o
H T   n
0
  SE 3
 0 1

H  Tn0 q1 ,..., qn   A1 q1    An qn 

Problem Definition : For a given position and orientation find q1, q2, …, qn

• For example, a given H ( for n=6):

 h11 h12 h13 h14 
h h22 h23 h24  given
given H  12 position (o)
orientation (R) h31 h32 h33 h34 
 
0 0 0 1
KON 318E : Introduction to Robotics 19
19
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : General IK Problem

• On the other hand:
 r11 (q) r12 (q) r13 (q) d x (q) 
r ( q ) r ( q ) r (q ) d ( q ) 
T60   21 22 23 y H q  q1 q2  qn 
r31 (q) r32 (q) r33 (q) d z (q) 
 
 0 0 0 1 
r11 (q )  h11
 
12 non-linear (trigonometric) equations :
r33 (q )  h33
: q1, q2, …qn are found
dx  h14
However this is too difficult
dy  h24
dz  h34
KON 318E : Introduction to Robotics 20
20
 İTÜ-EEF Dept. Of Control Engineering

Example: the Stanford manipulator

• For a given H: c1 c2 c4 c5c6  s4 s6   s2 s5c6   d 2 s4 c5c6  c4 s6   0
s1 c2 c4 c5c6  s4 s6   s2 s5c6   c1 s4 c5c6  c4 s6   0
0 1 0  0.154
0  s2 c4 c5c6  s4 s6   c2 s5c6  1
0 1 0.763 
H c1  c2 c4 c5 s6  s4 c6   s2 s5 s6   s1  s4 c5 s6  c4 c6   1
1 0 0 0 
   s1  c2 c4 c5 s6  s4 c6   s2 s5 s6   c1  s4 c5 s6  c4 s6   0
0 0 0 1 
s2 c4 c5 s6  s4 c6   c2 s5 s6  0
• Find 1, 2, d3, 4, 5, 6: c1 c2 c4 s5  s2 c5   s1s4 s5  0
s1 c2 c4 s5  s2 c5   c1s4 s5  1
 s2 c4 s5  c2 c5  0
c1s2 d 3  s1d 2  d 6 c1c2 c4 s5  c1c5 s2  s1s4 s5   0.154
s1s2 d 3  c1d 2  d 6 c1s4 s5  c2 c4 s1s5  c5 s1s2   0.763
c2 d 3  d 6 c2 c5  c4 s2 s5   0

• One solution: 1 = p/2, 2 = p/2, d3 = 0.5, 4 = p/2, 5 = 0, 6 = p/2 (How ?)

KON 318E : Introduction to Robotics 21

21
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : General IK Problem

• It is necessary to develop EFFICIENT and SYSTEMATIC
way to solve inverse kinematics problem

• Inverse kinematic may have non-unique solution

(multiple solutions)

• In Inverse Kinematic, we are interested by CLOSED

FORM SOLUTIONS (rather than numerical ones).
Because :
– Faster
– Easy to develop ELIMINATION Technique (in case of
multiple solutions)
KON 318E : Introduction to Robotics 22
22
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : Kinematic Decoupling

• IK problem is quite difficult for 6 DOF robots
• Thus, IK problem can be decoupled into 2 similar problems:
a) Inverse Position Kinematics This assumption is valid,
b) Inverse Orientation Kinematics if the wrist is sphrical

•  R60 q1 , q2  qn   R
o60  q1 , q2  qn   o
Origin of the tool frame

Spherical Wrist : z3, z4, z5 are intersect at oc (wrist center) .

So origins, o4, o5,(often o3) are located at oc

KON 318E : Introduction to Robotics 23

23
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : Kinematic Decoupling

• Tool Frame : Obtained by a translation of distance d6 along z5
from oc

Spherical
wrist

KON 318E : Introduction to Robotics 24

24
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : Kinematic Decoupling

• Now, origin of tool frame, o6, is a distance d6 translated along z5 (since z5
and z6 are collinear (=same axis)),
• Thus, the third column of R is the direction of z6 and z5 (wrt the
base frame) and we can write:

0 0 
o  o60  oco  d 6R 0 oco  o60  d 6 R 0 
 1 1 

• Calling o60 = [ox oy oz]T, oc0 = [xc yc zc]T

 xc   ox  d 6 r13 
Value of the first 3  y   o  d r 
joints  c  y 6 23 

 zc   oz  d 6 r33 

KON 318E : Introduction to Robotics 25

25
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics : Kinematic Decoupling

• [xc yc zc]T are determined from the first three joint variables (q1,q2,q3),
• Kinematic decoupling allows us to solve for the first three joint
angles decoupled from the final three.

• Thus we now have R30 ( Note that : R  R3 R6

0 3
)

• To solve for the final three joint angles:

R  R
3
6   0 1
3 R R  R0 T
3

Since the last three joints for a spherical wrist, we can use a set
of Euler Angles to solve for them

KON 318E : Introduction to Robotics 26

26
 İTÜ-EEF Dept. Of Control Engineering

Inverse position

• Now that we have [xc yc zc]T we need to find q1, q2, q3

– METHOD : Solve for qi by projecting onto the xi-1, yi-1 plane,

solve trig problem

– Solution technique will be explained on an elbow (RRR)

manipulator (eg. First 3 links of PUMA-like robot)

– to solve for 1 : (for an elbow manipulator) project the arm onto

the x0, y0 plane

KON 318E : Introduction to Robotics 27

27
 İTÜ-EEF Dept. Of Control Engineering

Background: two argument atan (atan2(x,y))

• We use atan2(·) instead of atan(·) to account for the full range of angular
solutions
– Called ‘four-quadrant’ arctan

  y
 atan   x  0, y  0
 x
p  atan   y  x  0, y  0
  x

atan 2 y, x     y
 p  atan   y  0, x  0
x

  atan  y  y  0, x  0
  x
 
 undefined y  0, x  0


KON 318E : Introduction to Robotics 28

28
 İTÜ-EEF Dept. Of Control Engineering

Example: RRR manipulator

1. To solve for 1, project the arm onto the x0, y0 plane
1  atan2xc , y c 

– Can also have: 1  p  atan2xc , y c 

• This will of course change the solutions for 2 and 3

KON 318E : Introduction to Robotics 29

29
İTÜ-EEF Dept. Of Control Engineering

singular configurations, offsets

• If xc=yc=0, 1 is undefined • If there is an offset, then we will have

– i.e. any value of 1 will work two solutions for 1: left arm and
right arm
– However, wrist centers cannot
intersect z0

KON 318E : Introduction to Robotics 30

30
İTÜ-EEF Dept. Of Control Engineering

KON 318E : Introduction to Robotics 3131

 İTÜ-EEF Dept. Of Control Engineering

Left arm and right arm solutions

• Left arm:

1   - 

 atan 2 xc , yc   atan 2 r 2  d 2 , d 
 atan 2 xc , yc   atan 2 xc  yc  d 2 , d 
2 2

 

KON 318E : Introduction to Robotics 33

33
 İTÜ-EEF Dept. Of Control Engineering

Left arm and right arm solutions

• Right arm:
1    
  atan 2 xc , yc 
 
  p  atan 2 r 2  d 2 , d  p  atan 2 xc 2  yc 2  d 2 , d 
 
 atan 2  xc  yc  d 2 , d 
2 2

 

adding p, transfers the angle

into 3rd quadrant

KON 318E : Introduction to Robotics 34

34
 İTÜ-EEF Dept. Of Control Engineering

Elbow up and Elbow down solutions

• There are in general two solutions for 1
• To find 2 and 3 , consider the following figure:

y1, y2

2
s
2  a3
r
3

a2
Joint 3

2 x1, x2

KON 318E : Introduction to Robotics 35

35
 İTÜ-EEF Dept. Of Control Engineering

Elbow up and Elbow down solutions

• Therefore there are in general two solutions for 1
• Finding 2 and 3 is identical to the planar two-link

p.s. : Law of Cosine

c 2  a 2  b 2  2ab. cos 
c
b   p 
 
c 2  a 2  b 2  2ab. cos 
a

KON 318E : Introduction to Robotics 36

36
 İTÜ-EEF Dept. Of Control Engineering

Elbow up and Elbow down solutions

• There are in general two solutions for 1
• To find 2 and 3 , consider the following figure:
• For 3

r 2  s 2  a22  a32  2a2 a3Cos 3 r 2  xc  yc  d 2

2 2

r 2  s 2  a22  a32 s  zc  d1
Cos 3 
2a2 a3
xc  yc   zc  d1   a22  a32
2 2 2
 D
2a2 a3

Sin 3   1  D 2

 
2 Solutions :
3  A tan 2 D,  1  D 2 •Elbow Down (with “+”)
•Elbow Up (with “-”)

KON 318E : Introduction to Robotics 37

37
 İTÜ-EEF Dept. Of Control Engineering

Elbow up and Elbow down solutions

• For 2 :

y1, y2

2
s
2  a3
r
 3

 a2
Joint 3

2 x1, x2

KON 318E : Introduction to Robotics 38

38
 İTÜ-EEF Dept. Of Control Engineering

Elbow up and Elbow down solutions

• For 2

 2   

2  A tan 2r , s   A tan 2a2  a3c3 , a3 s3 

 A tan 2 xc  yc  d1 , zc  d1   A tan 2a2  a3c3 , a3 s3 
2 2 2

 

KON 318E : Introduction to Robotics 39

39
 İTÜ-EEF Dept. Of Control Engineering

RRR: Four total solutions

• As a result , 4 solutions are optained in IK of a PUMA

like robot :
– Left Arm; Elbow Up

– Left Arm; Elbow Down

– Right Arm; Elbow Up

– Right Arm; Elbow Down

p.s. : we’ll see that thereare another 2 solutions for the wrist!

 TOTAL NUMBER of SOLUTIONS = 2x4=8

KON 318E : Introduction to Robotics 40

40
 İTÜ-EEF Dept. Of Control Engineering

RRR: Four total solutions

• In general, there will be a maximum of four solutions to the inverse position
kinematics of an elbow manipulator
– Ex: PUMA

(1) (3)

(2) (4)

KON 318E : Introduction to Robotics 41

41
 İTÜ-EEF Dept. Of Control Engineering

Example: RRP manipulator

• Spherical configuration
– Solve for 1 using same method as with RRR
1  atan2xc , y c 
– Again, if there is an offset, there
will be left-arm and right-arm solutions
– Solve for 2:
 2  atan2s, r 
r 2  xc  y c
2 2

s  zc  d1

– Solve for d3:

d3  r 2  s 2
 xc  y c  zc  d1 
2 2 2

KON 318E : Introduction to Robotics 42

42
 İTÜ-EEF Dept. Of Control Engineering

Inverse Orientation
• Geometric solutions of inverse position problem gives 1 , 2 and 3
(with respect to the position of the wrist center)
• Final 3 joint variables (4, 5 and 6 ) are found by solving inverse
orientation problem

Inverse Orientation Problem : Finding last 3 joint variables wrt o3x3y3z3

(i.e. R63 )

• For Spherical Wrist :

Inverse Orientation Problem  Finding Euler Angles

KON 318E : Introduction to Robotics 43

43
 İTÜ-EEF Dept. Of Control Engineering

Inverse orientation: spherical wrist

• In the previous section, it was shown that the forward kinematics of the
spherical wrist were identical to a ZYZ Euler angle transformation:

c 4c5c6  s4s6  c 4 c 5 s6  s 4 c 6 c 4 s5 c 4 s5 d 6 
s c c  c s  s 4 c 5 s6  c 4 c 6 s 4 s5 s4s5d 6 
T63  A4 A5 A6   4 5 6 4 6

  s5 c 6 s5 c 6 c5 c5d 6 
 
 0 0 0 1 

KON 318E : Introduction to Robotics 44

44
 İTÜ-EEF Dept. Of Control Engineering

Inverse orientation: spherical wrist

• The inverse orientation problem reduces to finding a set of Euler

angles (4, 5, 6) that satisfy:

c4 c5c6  s4 s6  c4 c5 s6  s4 c6 c4 s5   r11 r12 r13 

R63   s4 c5c6  c4 s6  s4 c5 s6  c4 c6 s4 s5   r21 r22 r23 
  s5c6 s5 s6 c5  r31 r32 r33 

• In the solution, 2 cases are possible:

1. Both r13 and r23 are not zero (i.e. 5 ≠ 0)… nonsingular
2. 5 = 0, thus r13 = r23 = 0… singular

KON 318E : Introduction to Robotics 45

45
 İTÜ-EEF Dept. Of Control Engineering

Inverse orientation: spherical wrist

• Nonsingular case:
– If 5 ≠ 0, then r33 ≠ ±1 and:

c4 c5c6  s4 s6  c4 c5 s6  s4 c6 c4 s5   r11 r12 r13 

R63   s4 c5c6  c4 s6  s4 c5 s6  c4 c6 s4 s5   r21 r22 r23 
  s5c6 s5 s6 c5  r31 r32 r33 

c5  r33 , s5   1  r33
2

2 different solutions for 

 5  atan 2 r33 , 1  r33  2

 

KON 318E : Introduction to Robotics 46

46
 İTÜ-EEF Dept. Of Control Engineering

Inverse orientation: spherical wrist

• Thus there are two values for 5.

c4 c5c6  s4 s6  c4 c5 s6  s4 c6 c4 s5   r11 r12 r13 

R63   s4 c5c6  c4 s6  s4 c5 s6  c4 c6 s4 s5   r21 r22 r23 
  s5c6 s5 s6 c5  r31 r32 r33 

Using the first value for 5 (s5 > 0): Using the second value for 5 (s5 < 0):

 4  atan2r13 , r23   4  atan2 r13 ,r23 

 6  atan2 r31, r32   6  atan2r31,r32 

Thus for the nonsingular case, there are two solutions for the inverse
orientation kinematics
KON 318E : Introduction to Robotics 47
47
 İTÜ-EEF Dept. Of Control Engineering

Inverse orientation: spherical wrist

• In the singular case :

5 = 0 thus s5 = 0  r13 = r23 = r31 = r32 = 0

• Therefore, R63 has the form:

c 4c 6  s4s6  c 4 s6  s 4 c 6 0 c 46  s46 0 r11 r12 0

R63  s4c 6  c 4s6  s 4 s6  c 4 c 6 0  s46 c 46 0  r21 r22 0
 0 0 1  0 0 1  0 0 1

• So we can find the sum 4 + 6 as follows:

 4  6  atan2r11, r21   atan2r11,r12 

• Since we can only find the sum, there is an infinite number of solutions
(singular configuration)

KON 318E : Introduction to Robotics 48

48
 İTÜ-EEF Dept. Of Control Engineering

Inverse Kinematics: general procedure

1. Find q1, q2, q3 with respect to the wrist center is:

0 inverse position

oco  o  d 6R 0
kinematics

 1

2. Using q1, q2, q3, determine R30

3. Find Euler angles corresponding to the rotation matrix:

inverse orientation

R  R
3
6   0 1
3 R R  R0 T
3
kinematics

KON 318E : Introduction to Robotics 49

49
 İTÜ-EEF Dept. Of Control Engineering

Example: RRR arm with spherical wrist

• For the DH parameters below, we can derive R30 from the forward
kinematics:
c1c 23  c1s23 s1 
R30  s1c 23  s1s23  c1 
 s23 c 23 0 

• We know that R63 is given as follows:

c 4c5c 6  s4s6  c 4c5s6  s 4c 6 c 4 s5 
R63  s4c5c 6  c 4s6  s 4c 5s6  c 4c 6 s4s5 
  s5c 6 s5 c 6 c5 
• To solve the inverse orientation kinematics:
link ai i di i
R  R
3
6  R
0 T
3
1 0 90 d1 1
– For a given desired R 2 a2 0 0 2
3 a3 0 0 3
KON 318E : Introduction to Robotics 50
50
 İTÜ-EEF Dept. Of Control Engineering

Example: RRR arm with spherical wrist

• Euler angle solutions can be applied. Taking the third column of (R30)TR

c 4s5  c1c23 r13  s1c23 r23  s23 r33

s4s5  c1s23 r13  s1s23 r23  c23 r33
c5  s1r13  c1r23

• Again, if 5 ≠ 0, we can solve for 5:

 5  atan2 s1r13  c1r23 , 1  s1r13  c1r23  
2

 

• Finally, we can solve for the two remaining angles as follows:

 4  atan2c1c23 r13  s1c23r23  s23 r33 ,c1s23 r13  s1s23 r23  c23 r33 
 6  atan2 s1r11  c1r21, s1r12  c1r22 
• For the singular configuration (5 = 0), we can only find 4 + 6 thus it is
common to arbitrarily set 4 and solve for 6

KON 318E : Introduction to Robotics 51

51
 İTÜ-EEF Dept. Of Control Engineering

Example: elbow manipulator with spherical

wrist
• Derive complete inverse kinematics solution

link ai i di i
1 0 90 d1 1
2 a2 0 0 2
3 a3 0 0 3
4 0 -90 0 4
5 0 0 0 5
• we are given H = T60 such that: 6 0 0 d6 3
o x  r11 r12 r13 
o  oy , R  r21 r22
  r23 
oz  r31 r32 r33 

KON 318E : Introduction to Robotics 52

52
 İTÜ-EEF Dept. Of Control Engineering

Example: elbow manipulator with spherical

wrist
• First, we find the wrist center:
 xc  o x  d 6 r13 
 y   o  d r 
 c  y 6 23 

 zc  oz  d 6 r33 

• Inverse position kinematics:

1  atan2xc , y c 
 2  atan2 xc  y c  d 2 , zc  d1   atan2a2  a3c3 , a3s3 
2 2

 

 3  atan2 D, 1  D 2 
• Where d is the shoulder offset (if any) and D is given by:

xc  y c  d 2  zc  d1   a2  a3
2 2 2 2 2

D
2a2a3

KON 318E : Introduction to Robotics 53

53
 İTÜ-EEF Dept. Of Control Engineering

Example: elbow manipulator with spherical

wrist
• Inverse orientation kinematics:
– Now that we know 1, 2, 3, we know R30. need to find R36:

R  R
3
6  R
0 T
3

• Solve for 4, 5, 6, Euler angles:

 4  atan 2c1c23r13  s1c23r23  s23r33 ,  c1s23r13  s1s23r23  c23r33 

 5  atan 2 s1r13  c1r23 ,  1  s1r13  c1r23 2 
 
 6  atan 2 s1r11  c1r21 , s1r12  c1r22 

KON 318E : Introduction to Robotics 54

54
 İTÜ-EEF Dept. Of Control Engineering

Example: inverse kinematics of SCARA

manipulator
• We are given T40: R o 
T40   
 0 1
c12c 4  s12s4 s12c 4  c12s4 0 a1c1  a2c12 
s c  c s  c12c 4  s12s4 0 a1s1  a2s12 
  12 4 12 4

 0 0  1  d3  d 4 
 
 0 0 0 1 

link ai i di i
1 a1 0 0 1
2 a2 180 0 2
3 0 0 d3 0
4 0 0 d4 4

KON 318E : Introduction to Robotics 55

55
 İTÜ-EEF Dept. Of Control Engineering

Example: inverse kinematics of SCARA

manipulator
• Thus, given the form of T40, R must have the following form:
c  s
0
R  sc 0
 0
0 1
• Where  is defined as:   1   2   4  atan2r11, r12 
• To solve for 1 and 2 we project the manipulator onto the x0-y0 plane:
o x  oy  a1  a2
2 2 2 2

c2 
2a1a2
• This gives two solutions for 2:  2  atan2 c2 , 1  c2 2 
 
• Once 2 is known, we can solve for 1:
1  atan2ox , oy  - atan2a1  a2c2, a2s2 
• 4 is now give as:  4  1   2  atan2r11, r12 
• Finally, it is easy to see that d3 = oz + d4

KON 318E : Introduction to Robotics 56

56
İTÜ-EEF Dept. Of Control Engineering

End of Forward and Inverse Kinematic

Chapter

Questions ??

KON 318E : Introduction to Robotics 60

60