Robotics

Lecture 8

Inverse Kinematics

Emam Fathy

Department of Electrical and Control Engineering

email: emfmz@aast.edu

http://www.aast.edu/cv.php?disp_unit=346&ser=68525
1
Inverse Kinematics
Decoupling Technique

2
 Decoupling Technique
 inverse kinematics is searching for the elements
of the joint variables vector q

q =[q1 q2 q3 · · · qn]T

 Transformation 0Tn is given as a function of the

joint variables q1 ,q2 ,q3 ,· · · ,qn.
0T = 0T1(q1) 1T2(q2) 2T3(q3) 3T4(q4) · · · n−1Tn(qn)
n

3
 The result of forward kinematics of a 6 DOF robot is a 4 × 4
transformation matrix:

0T = 0T1 1T2 2T3 3T44T55T6

6

r11 r12 r13 r14

= rr21 rr22 rr23 rr24
31 32 33 34
𝟎 𝟎 𝟎 𝟏

 Where 12 elements are trigonometric functions of six

unknown joint variables.
 However, because the upper left 3×3 submatrix is a
rotation matrix, only three elements of them are
independent (because of the orthogonality condition).
 Six equations out of the 12 equations are independent.
 Trigonometric functions provide multiple solutions. 4
 It is possible to decouple the inverse kinematics
problem into inverse position and inverse
orientation kinematics.
 Now we have two independent problems, each with
only three unknown parameters.
 Then the overall transformation matrix of a robot
can be decomposed to a translation and a rotation.
0R 0d
0T = 6 6
6
0 1
I 0d 0R 0
= D6 R6=
0 0 6 6
0 1 0 1
 0D6 translation matrix indicates the position of the
end-effector to the base frame.
 0R6 rotation matrix indicates the orientation of the
end-effector to the base frame. 5
 Most robots have a spherical wrist made of three
revolute joints (0d6=0d3)with intersecting and
orthogonal axes at the wrist point.
 we can decouple the kinematics of the wrist and
manipulator by decomposing the overall forward
kinematics transformation matrix 0T6 into the wrist
orientation and wrist position

0R 0d 3R 0
0T =0T3 3T6= 3 3 6
6
0 1 0 1
 where the wrist orientation matrix is:

r11 r12 r13

3R
6 = 0R3T 0R6 = 0R3T r21 r22 r23
r31 r32 r33 6
 And the wrist position vector is:

r14
0d = r
6 24
r34

7
Examples

8
 Example: RRR arm with spherical wrist

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

• We know that R63 is given as follows:

c 4c5c6  s4s6  c 4c5s6  s4c6 c 4s5 
R63  s4c5c6  c 4s6  s4c5s6  c 4c6 s4s5 
  s5c6 s5 c 6 c5 

• To solve the inverse orientation kinematics:

R  R
3
6  R
0 T
3
link ai ai di qi
1 0 90 d1 q1
– For a given desired R
2 a2 0 0 q2
3 a3 0 0 q3
 Example: RRR arm with spherical wrist

• Euler angle solutions can be applied. Taking the third column

of (R30)TR c s c c r s c r s r
4 5 1 23 13 1 23 23 23 33

s4s5  c1s23 r13  s1s23 r23  c23 r33

c5  s1r13  c1r23
• Again, if q5 ≠ 0, we can solve for q5:
q5  atan2 s1r13  c1r23 , 1  s1r13  c1r23  
2

 

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

q 4  atan2c1c23 r13  s1c23 r23  s23 r33 ,c1s23 r13  s1s23 r23  c23 r33 
q 6  atan2 s1r11  c1r21, s1r12  c1r22 

• For the singular configuration (q5 = 0), we can only find q4 + q6

thus it is common to arbitrarily set q4 and solve for q6
 Example: elbow manipulator with spherical wrist

• Derive complete inverse kinematics solution

link ai ai di qi
1 0 90 d1 q1
2 a2 0 0 q2
3 a3 0 0 q3
4 0 -90 0 q4
5 0 0 0 q5
• we are given H = T6 such that:
0
6 0 0 d6 q3

o x  r11 r12 r13 

o  oy , R  r21 r22 r23 
oz  r31 r32 r33 
 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:
q1  atan2xc , y c 
q 2  atan2 xc  y c  d 2 , zc  d1   atan2a2  a3c3 , a3s3 
2 2

 

q 3  atan2 D, 1  D 2 
• Where d is the shoulder offset (if any) and D is given by:
xc  y c  d  zc  d1   a2  a3
2 2 2 2 2 2

D
2a2a3
 Example: elbow manipulator with spherical wrist

• Inverse orientation kinematics:

– Now that we know q1, q2, q3, we know R30. need to find R36:
R  R
3
6   0 T
3 R

• Solve for q4, q5, q6, Euler angles:

q 4  atan2c1c23 r13  s1c23 r23  s23 r33 ,c1s23 r13  s1s23 r23  c23 r33 
q 5  atan2 s1r13  c1r23 , 1  s1r13  c1r23  
2

 
q 6  atan2 s1r11  c1r21, s1r12  c1r22 
14
 kinematic decoupling
Now, origin of tool frame, o6, is a distance d6 translated along z5 (since
z5 and z6 are collinear)
– Thus, the third column of R is the direction of z6 (w/ respect to the
base frame) and we can write: 0
o  o60  oco  d 6R 0
 1
– Rearranging:
0
oco  o  d 6R 0
 1

– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T

 xc   ox  d 6 r13 
 yc   o y  d 6 r23 
 z  o  d r 
 c  z 6 33 
 kinematic decoupling

• Since [xc yc zc]T are determined from the first three joint angles, our
forward kinematics expression now allows us to solve for the first three
joint angles decoupled from the final three.
– Thus we now have R30
– Note that:
R  R30 R63

– To solve for the final three joint angles:

 
R63  R30
1
 T
R  R30 R

– Since the last three joints for a

spherical wrist, we can use a set of
Euler angles to solve for them
 Inverse position kinematics

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

– Solve for qi by projecting onto the xi-1, yi-1 plane, solve trig problem
– Two examples
• elbow (RRR) manipulator: 4 solutions (left-arm elbow-up, left-arm elbow-down,
right-arm elbow-up, right-arm elbow-down)
• spherical (RRP) manipulator: 2 solutions (left-arm, right-arm)
 Inverse orientation kinematics
• Now that we can solve for the position of the wrist center (given kinematic
decoupling), we can use the desired orientation of the end effector to solve for the
last three joint angles

– Finding a set of Euler angles corresponding to a

desired rotation matrix R
– We want the final three joint angles that give the
orientation of the tool frame with respect to o3
(i.e. R63)
 Inverse orientation: spherical wrist
• Previously, we said that the forward kinematics of the spherical wrist
were identical to a ZYZ Euler angle transformation:
c 4c5c 6  s4s6  c 4 c 5 s6  s 4 c 6 c 4 s5 c 4 s5 d 6 
s c c  c s  s 4c 5s6  c 4c 6 s 4 s5 s4s5d 6 
T6  A4 A5 A6   4 5 6
3 4 6

  s5 c 6 s5 c 6 c5 c5d 6 
 
 0 0 0 1 
 Inverse orientation: spherical wrist
• The inverse orientation problem reduces to finding a set of Euler angles
(q4, q5, q6) that satisfy:
c 4c5c 6  s4s6  c 4c5s6  s 4c 6 c 4 s5 
R  s4c5c 6  c 4s6
3
6  s 4c 5s6  c 4c 6 s4s5 
  s5c 6 s5 c 6 c5 
• to solve this, take two cases:
1. Both r13 and r23 are not zero (i.e. q5 ≠ 0)… nonsingular
2. q5 = 0, thus r13 = r23 = 0… singular
• Nonsingular case
– If q5 ≠ 0, then r33 ≠ ±1 and:

c5  r33 , s5   1  r33
2

q 5  atan2 r33 , 1  r33 

2

 
 Inverse orientation: spherical wrist
• Thus there are two values for q5. Using the first (s5 > 0):
q 4  atan2r13 , r23 
q 6  atan2 r31, r32 
• Using the second value for q5 (s5 < 0):
q 4  atan2 r13 ,r23 
q 6  atan2r31,r32 
• Thus for the nonsingular case, there are two solutions for the inverse
orientation kinematics
 Inverse orientation: spherical wrist
• In the singular case, q5 = 0 thus s5 = 0 and 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
R  s4c 6  c 4s6
3
6  s 4s6  c 4c 6 0  s46 c 46 0  r21 r22 0
 0 0 1  0 0 1  0 0 1

• So we can find the sum q4 + q6 as follows:

q 4  q 6  atan2r11, r21   atan2r11,r12 
• Since we can only find the sum, there is an infinite number of solutions
(singular configuration)
 Inverse Kinematics: general procedure
1. Find q1, q2, q3 such that the position of the wrist center is:

0
inverse position
oco  o  d 6R 0 kinematics
2. Using q1, q2, q3, determine R30  1
3. Find Euler angles corresponding to the rotation matrix:

 
R63  R30
1
 T
R  R30 R
inverse orientation
kinematics
 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 ai di qi
R63  R30 R   T

– For a given desired R 1 0 90 d1 q1

2 a2 0 0 q2
3 a3 0 0 q3
 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 q5 ≠ 0, we can solve for q5:

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

 
• Finally, we can solve for the two remaining angles as follows:
q 4  atan2c1c23 r13  s1c23 r23  s23 r33 ,c1s23 r13  s1s23 r23  c23 r33 
q 6  atan2 s1r11  c1r21, s1r12  c1r22 
• For the singular configuration (q5 = 0), we can only find q4 + q6 thus it is
common to arbitrarily set q4 and solve for q6
 Example: elbow manipulator with spherical wrist

• Derive complete inverse kinematics solution

link ai ai di qi
1 0 90 d1 q1
2 a2 0 0 q2
3 a3 0 0 q3
4 0 -90 0 q4
5 0 0 0 q5
6 0 0 d6 q3
• we are given H = T60 such that: o x  r11 r12 r13 
o  oy , R  r21 r22 r23 
oz  r31 r32 r33 
 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:

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

 

q 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
 Example: elbow manipulator with spherical wrist

• Inverse orientation kinematics:

– Now that we know q1, q2, q3, we know R30. need to find R36:
 
R63  R30 R
T

• Solve for q4, q5, q6, Euler angles:

q 4  atan2c1c23 r13  s1c23 r23  s23 r33 ,c1s23 r13  s1s23 r23  c23 r33 
q 5  atan2 s1r13  c1r23 , 1  s1r13  c1r23  
2

 
q 6  atan2 s1r11  c1r21, s1r12  c1r22 
29
 Example: inverse kinematics of SCARA manipulator

R o 
• We are given T40: T40   
 0 1
c12c 4  s12s4 s12c 4  c12s 4 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 ai di qi
1 a1 0 0 q1
2 a2 180 0 q2
3 0 0 d3 0
4 0 0 d4 q4
 Example: inverse kinematics of SCARA manipulator

• Thus, given the form of T40, R must have the following form:
ca  sa 0
R  sa ca 0
 0 0 1

• Where a is defined as: a  q1  q 2  q 4  atan 2r11, r12 

• To solve for q1 and q2 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 q2: q 2  atan2 c2 , 1  c22 
 
• Once q2 is known, we can solve for q1:
q1  atan2ox , oy  - atan2a1  a2c2, a2s2 
• q4 is now give as: q 4  q1  q 2  atan 2r11, r12 
• Finally, it is trivial to see that d3 = oz + d4
End of Lec

32