Inverse Kinematics
9-22-14
Homework #4 – Solve 4.9 (Use the method described in the presentation slides),
4.11, 4.18, 19, 4.24, Due 10-1-14
Inverse manipulator kinematics:
Find the joint variables Q  q1 , q2 , ..., qn  ( q i =  i for revolute, or = d i for prismatic), given
T
0
N
P  [ px , py , pz ]T and the joint parameters transformation matrices i 1iT , i  1...N .
 S i
 C i
 S C
i
 i i 1 C i C i 1
i 1T 
 S i S i 1 C i S i 1

0
0

0
 S i 1
C i 1
0
ai 1 
 S i 1d i 
C i 1d i 

1

(3.6)
Adept V+ command:
SOLVE.ANGLES V[] ; Finds the joint rotation angles for a given position vector
V=[x, y, z, y, p, r].
Closed Form Solutions
R-R-R Robot Example (Similar to SCARA without the Z component)
Ky
θ2
L2
L1
ϕ
β
θ1
A. Algebraic Solution
DH 
i  i 1
1 0
ai 1
0
2
3
L1
L2
0
0
c123  s123
s
0
 123 c123
3T 
 0
0

0
0

i
1
0 2
0 3
di
0
0 L1c1  L2 c12 
0 L1s1  L2 s12 
1
C i 1d i 

0
1

Kx
 r11 r12
r
0
 21 r22
T

N
r31 r32

0 0
r13
r23
r33
0
px 
p y 
, where rij’s are unknown and Pi’s are known.
pz 

1
Equating the sums of the squares of the (1,4) and (2,4) elements
Px2  Py2  L21  L22  2L1 L2c2
which is solved for c2 since Px and Py are given, which in turn yields s2 via c22  s22  1 , and
finally θ2=tan-1(s2/c2).
B. Geometric Solution
Px2  Py2  L21  L22  2L1 L2 cos(180   2 )  L21  L22  2L1 L2 cos  2 .
from which θ2 is found. With β = tan-1(Py/Px),
 P 2  P 2  L2  L2 
x
y
1
2

 2 L1 Px2  Py2 


1    cos  1 
θ3 is arbitrarily set for an object pick up.
Multiple Solutions – Due to more than one way of reaching the target position by the arm links.
Choices – Left arm vs. right arm. Above shoulder vs. below shoulder. Above object vs. below object.
No Solutions – When. two or more links line up through a complete stretching or folding back, one or
more degrees of freedom are lost. In these cases, the determinants of the rotation matrices become
zero and no solutions may be found.
PUMA 560 Example
If the rotation matrix 60 R is known along with the position vector 60 P and the link length and link offset
values, the inverse problem is solving the following six non-linear equations from Textbook (3.14):
r11 = c1[c23 (c4c5c6  s4 s6 )  s23s5c6 ]  s1 ( s4c5c6  c4 s6 )
r22 = s1[c23 (c4c5 s6  s4c6 )  s23s5 s6 ]  c1 (c4c6  s4c5 s6 )
r33 = s23c4 s5  c23c5
Px = c1[a2c2  a3c23  d 4 s23 ]  d3s1
Py = s1[a2c2  a3c23  d 4 s23 ]  d3c1
Pz =  a3s23  a2 s2  d 4c23
(3.14)