Inverse Kinematics

Geometric and Algebraic Approaches

Mechanism and Robot Kinematics
2015

A Simple Example

Revolute and
Prismatic
Joints
Combined

Finding :

y
= arctan( )
x

More Specifically:

(x , y)

y
= arctan 2( )
x

1
X

Finding S:

S = (x 2 + y 2 )

arctan2() specifies that its in the

first quadrant

Inverse Kinematics of a Two Link

Manipulator
(x , y)

Find:

l2

l1

Given: l1, l2 , x , y
1, 2

Redundancy:
A unique solution to this
problem does not exist. Notice, that
using the givens two solutions are
possible.
Sometimes no solution is possible.
(x , y)

The Geometric
Solution

l2

(x , y)

Using the Law of Cosines:

c 2 = a 2 + b 2 2ab cos C
( x 2 + y 2 ) = l1 + l2 2l1l2 cos(180 2 )
2

cos(180 2 ) = cos( 2 )
x 2 + y 2 l1 l2
cos( 2 ) =
2l1l2
2

l1

Using the Law of Cosines:

sin B sin C
=
b
c
sin 1 sin(180 2 )
sin( 2 )
=
=
l2
x 2 + y2
x 2 + y2
1 = 1 +
y
= arctan 2
x

x 2 + y 2 l12 l2 2

2 = arccos

2l1l2

Redundant since 2 could be in the

first or fourth quadrant.

Redundancy caused since 2 has two

possible values

l sin( )
y
2
1 = arcsin 2
+ arctan 2
x 2 + y2
x

The Algebraic Solution

2

l2

(x , y)

c1 = cos1
c1+ 2 = cos( 2 + 1 )
(1) x = l1 c1 + l2 c1+ 2

l1

(2) y = l1 s1 + l2 sin 1+ 2

(3) = 1 + 2

(1) 2 + (2) 2 = x 2 + y 2 =

) (

= l1 c1 + l2 (c1+ 2 ) 2 + 2l1l2 c1 (c1+ 2 ) + l1 s1 + l2 (sin1+ 2 ) 2 + 2l1l2 s1 (sin1+ 2 )

2

= l1 + l2 + 2l1l2 (c1 (c1+ 2 ) + s1 (sin1+ 2 ) )

2

= l1 + l2 + 2l1l2 c 2
2

Only
Unknown

x 2 + y 2 l12 l2 2

2 = arccos

2
l
l
1 2

Note :
cos(a+b) = (cos a)(cos b) + (sin a)(sin b)

sin(a+b) = (cos a)(sin b) + (cos b)(sin a)

x = l1 c1 + l2 c1+ 2
= l1 c1 + l2 c1c 2 l2 s1s2
= c1 (l1 + l2 c 2 ) s1 (l2 s2 )
y = l1 s1 + l2 sin 1+ 2
= l1 s1 + l2 s1c 2 + l2 s 2 c1
= c1 (l2 s 2 ) + s1 (l1 + l2 c 2 )
c1 =

x + s1 (l 2 s 2 )
(l1 + l 2 c 2 )

x + s1 (l 2 s 2 )
y=
(l 2 s 2 ) + s1 (l1 + l 2 c 2 )
(l1 + l 2 c 2 )
=

s1 =

Note :
cos(a+b) = (cos a)(cos b) + (sin a)(sin b)

sin(a+b) = (cos a)(sin b) + (cos b)(sin a)

We know what 2 is from the
previous slide. We need to solve
for 1 . Now we have two
equations and two unknowns (sin
1 and cos 1 )
Substituting for c1 and
simplifying many times

1
x l 2 s 2 + s1 (l12 + l 2 2 + 2l1l 2 c 2 )
(l1 + l 2 c 2 )
y(l1 + l 2 c 2 ) x l 2 s 2
x 2 + y2

Notice this is the law of

cosines and can be replaced
by x2+ y2

y(l1 + l 2 c 2 ) x l 2 s 2
1 = arcsin
x 2 + y2

Inverse Kinematics
given the pose of the end effector, find the
joint variables that produce the end effector
pose
for a 6-joint robot, given
0
0

R
o
0
6
6
T 6=

0 1

find

q1 , q2 , q3 , q4 , q5 , q6
7

RPP + Spherical Wrist

RPP + Spherical Wrist

solving for the joint variables directly is hard
r11 r12 r13 d x
r r r d
y
22
23
0
0 3 21
T 6 = T 3T 6 =
r31 r32 r33 d z

0 0 0 1
r11 = c1c4c5c6 c1s4 s6 + s1s5c6

d z = s4 s5 d 6 + d1 + d 2
9

Kinematic Decoupling
for 6-joint robots where the last 3 joints
intersecting at a point (e.g., last 3 joints are
spherical wrist) there is a simpler way to solve
the inverse kinematics problem
1. use the intersection point (wrist center) to solve
for the first 3 joint variables

inverse position kinematics

2. use the end-effector pose to solve for the last 3

joint variables

inverse orientation kinematics

10

Spherical Wrist
pc
p

d6

11

RPP Cylindrical Manipulator

d3*
d2*

pc

d1
yc
xc

3/10/2015

12

RRP Spherical Manipulator

3/10/2015

13

Spherical Wrist

Link

ai

di

-90

4*

90

5*

d6

6*

* joint variable

14

Spherical Wrist
c4c5c6 s4 s6 c4c5 s6 s4c6 c4 s5 c4 s5 d 6
s c c + c s s c s + c c s s s s d
4 5 6
4 6
4 5
4 5 6
T 36 = T 34T 54T 56 = 4 5 6 4 6
s5c6
s5 s6
c5
c5 d 6

0
0
0
1

15

Spherical Wrist
pc
p

d6

0
pc0 = p60 d 6 R 06 0
1
16

Inverse Kinematics Recap

1. Solve for the first 3 joint variables q1, q2, q3
such that the wrist center pc has coordinates
0
pc0 = p60 d 6 R 06 0
1

2. Using the results from Step 1, compute

3. Solve for the wrist joint variables q4, q5, q6
corresponding to the rotation matrix

R 30

( )R

R = R
3
6

0 T
3

0
6
17

Spherical Wrist
for the spherical wrist
c4c5c6 s4 s6 c4c5 s6 s4c6 c4 s5 c4 s5 d 6
s c c + c s s c s + c c s s s s d
4 5 6
4 6
4 5
4 5 6
T 36 = T 34T 54T 56 = 4 5 6 4 6
s5c6
s5 s6
c5
c5 d 6

0
0
0
1

if s5 0

( 1 r , r )
= atan2 ( 1 r , r )

5pos = atan2
5neg

2
33

33

2
33

33

18

Spherical Wrist
c4c5c6 s4 s6 c4c5 s6 s4c6 c4 s5 c4 s5 d 6
s c c + c s s c s + c c s s s s d
4 5 6
4 6
4 5
4 5 6
T 36 = T 34T 54T 56 = 4 5 6 4 6
s5c6
s5 s6
c5
c5 d 6

0
0
0
1

for 5pos , s5 > 0

4 = atan2 (r23 , r13 )

6 = atan2 (r32 , r31 )
19

Spherical Wrist
c4c5c6 s4 s6 c4c5 s6 s4c6 c4 s5 c4 s5 d 6
s c c + c s s c s + c c s s s s d
4 5 6
4 6
4 5
4 5 6
T 36 = T 34T 54T 56 = 4 5 6 4 6
s5c6
s5 s6
c5
c5 d 6

0
0
0
1

for 5neg , s5 < 0

4 = atan2 ( r23 , r13 )

6 = atan2 ( r32 , r31 )
20

Spherical Wrist
c4c5c6 s4 s6 c4c5 s6 s4c6 c4 s5 c4 s5 d 6
s c c + c s s c s + c c s s s s d
4 5 6
4 6
4 5
4 5 6
T 36 = T 34T 54T 56 = 4 5 6 4 6
s5c6
s5 s6
c5
c5 d 6

0
0
0
1

if 5 = 0

c4c6 s4 s6 c4 s6 s4c6
s c + c s s s + c c
4 6
4 6
= 4 6 4 6

0
0

0
0

0 0
0 0
1 d6

0 1
21

Spherical Wrist
c4c6 s4 s6 c4 s6 s4c6
s c + c s s s + c c
4 6
4 6
= 4 6 4 6

0
0

0
0

0
0
1 d6

0 1

c4 + 6 s 4 + 6
s
c
4
6
4+ 6
+
=
0
0

0
0

only the sum 4+6

can be determined

0
0 0
1 d6

0 1

0
0

22