ROBOTICS

CHAPTER 5: INVERSE KINEMATICS

PhD. NGUYỄN HOÀNG GIÁP

CONTENTS
5.1 INVERSE KINEMATICS PROBLEM

5.2 INVERSE KINEMATICS SOLUTIONS

5.1 INVERSE KINEMATICS PROBLEM
❖ “Given a desired end-effector pose (position + orientation), find the values of the joint
variables that will realize it”
5.1 INVERSE KINEMATICS PROBLEM
❖ A synthesis problem, with input data in the form:

• Classical formulation: inverse kinematics for a given end-effector pose

 R p 0
T =  = An (q )
000 1 
• Generalized formulation: inverse kinematics for a given value of task variables

 p
r =   = f r (q )
 
• The equations to solve are in general nonlinear, and thus it is not always possible to find
a closed-form solution.
• Multiple solutions may exist.
• Infinite solutions may exist, e.g., in the case of a kinematically redundant manipulator.
• There might be no admissible solutions, in view of the manipulator kinematic structure.
5.1 INVERSE KINEMATICS PROBLEM
❖ Primary workspace WS1: set of all positions p that can be reached with at least one
orientation (𝜽 or R)
• Out of WS1 there is no solution to the problem
• When p ∈ WS1, there is a suitable 𝜽 (or R) for which a solution exists

❖ Secondary (or dexterous) workspace WS2: set of positions p that can be reached with
any orientation (among those feasible for the robot direct kinematics)
When p ∈ WS2, there exists a solution for any feasible 𝜽 (or R)

❖ WS2 ⊆ WS1
5.1 INVERSE KINEMATICS PROBLEM
❖ SOLUTION METHODS

ANALYTICAL solution (in closed form) NUMERICAL solution (in iterative form)
• Preferred, if it can be found* • Certainly needed if n>m (redundant
• Use ad-hoc geometric inspection case), or at/close to singularities
• Algebraic methods (solution of • Slower, but easier to be set up
polynomial equations) • In its basic form, it uses the (analytical)
• systematic ways for generating a reduced Jacobian matrix of the direct kinematics
set of equations to be solved map
𝜕𝑓𝑟 (𝑞)
* Sufficient conditions for 6-dof arms 𝐽𝑟 (𝑞) =
𝜕𝑞
• 3 consecutive rotational joint axes are
• Newton method, Gradient method, and so
incident (e.g., spherical wrist), or
on…
• 3 consecutive rotational joint axes are
parallel
5.2 INVERSE KINEMATICS SOLUTIONS: THREE-LINK PLANAR ARM

❖ ALGEBRAIC SOLUTION
c123 − s123 0 a1c1 + a2c12 + a3c123 
s c123 0 a1s1 + a2 s12 + a3 s123 
0
T3 =  123
𝜃3  0 0 1 0 
 
𝜑  0 0 0 1 

From Direct Kinematics:

𝜃2
𝛽  = 1 +  2 + 3 (1)
pWx = px − a3c = a1c1 + a2c12 (2)
𝛼 𝜃1
pWy = p y − a3 s = a1c1 + a2 s12 (3)

Link 𝑎𝑖 𝛼𝑖 𝑑𝑖 𝜃𝑖  pWx
2
+ pWy
2
= a12 + a22 + 2a1a2c2

1 𝑎1 0 0 𝜃1
2
pWx + pWy
2
− a12 − a22
 c2 = ; s2 =  1 − c22
2 𝑎2 0 0 𝜃2 2a1a2 c2
3 𝑎3 0 0 𝜃3   2 = ATAN 2( s2 , c2 )
5.2 INVERSE KINEMATICS SOLUTIONS: THREE-LINK PLANAR ARM

❖ ALGEBRAIC SOLUTION
From (2), (3):

 a1 + a2 c2 −a2 s2  c1   pWx 

 as    = 
 2 2 a + a 2 2   1
c s p
 Wy 
𝜃3 1

(a1 + a2 c2 ) pWy − a2 s2 pWx

 s1 =
𝜑
2
pWx + pWy2

(a1 + a2 c2 ) pWx + a2 s2 pWy

c1 = 2
pWx + pWy
2

𝜃2  1 = ATAN 2( s1 , c1 )
𝛽
3 =  − 1 −  2
𝛼 𝜃1 If 𝜑 is not specified, then the arm is redundant and
there exist infinite solutions to the inverse kinematics
problem.
5.2 INVERSE KINEMATICS SOLUTIONS: THREE-LINK PLANAR ARM

❖ GEOMETRIC SOLUTION

The application of the cosine theorem to the

triangle formed by links 𝑎1 , 𝑎2 and the
𝜃3 segment connecting points W and O gives

𝜑 2
pWx + pWy
2
= a12 + a22 − 2a1a2 cos( −  2 )
2
pWx + pWy
2
− a12 − a22
 c2 = ; s2 =  1 − c22
2a1a2 c2
𝜃2
  2 = ATAN 2( s2 , c2 )
𝛽

𝛼 𝜃1
5.2 INVERSE KINEMATICS SOLUTIONS: THREE-LINK PLANAR ARM

❖ GEOMETRIC SOLUTION

Notice that the determination of α depends

on the sign of 𝑝𝑊𝑥 and 𝑝𝑊𝑦
𝜃3
 = ATAN 2( pWy , pWx )
𝜑 To compute 𝛽, applying again the cosine
theorem yields

𝜃2
c 2
pWx + pWy
2
= a1 + a2c2
𝛽  p2 + p2 + a2 − a2 
  = cos −1  Wx Wy 1 2

 2a1 pWx2
+ p 2 
𝛼 𝜃1  Wy 
1 =    3 =  − 1 −  2
5.2 INVERSE KINEMATICS SOLUTION OF MANUPULATOR WITH SPHERICAL WRIST

Problem: find the joint variables 𝑞1 , 𝑞2 ,…, 𝑞6 from

the input data:
• 𝑃𝑒 (origin 𝑂6 )
• 𝑅 = 𝑛𝑒 𝑠𝑒 𝑎𝑒 (orientation of 𝑅𝐹6 )

Solution:
• Compute wrist position 𝑃𝑊= 𝑃𝑒 − 𝑑6 𝑎𝑒
• Solve inverse kinematics for 𝑞1 , 𝑞2 , 𝑞3
• Compute R3 ( q1 , q2 , q3 )
0

• Compute R6 ( q4 , q5 , q6 ) = R3 R
3 0 T

• Solve inverse kinematics for the spherical wrist ( q4 , q5 , q6 )

5.2 INVERSE KINEMATICS SOLUTIONS: ARTICALUATED ARM

c1c23 −c1s23 s1 c1 (a2 c2 + a3c23 ) 

s c − s1s23 −c1 s1 (a2c2 + a3c23 ) 
0
T3 = A1 A2 A3 =  1 23
0 1 2

 s23 c23 0 a2 s2 + a3 s23 

 
 0 0 0 1 
From Direct Kinematics:
pWx = c1 (a2c2 + a3c23 )
pWy = s1 (a2c2 + a3c23 )
pWz = a2 s2 + a3 s23

 pWx
2
+ pWy
2
+ pWz
2
= a22 + a32 + 2a2 a3c3
Link 𝑎𝑖 𝛼𝑖 𝑑𝑖 𝜃𝑖
1 0 𝜋/2 0 𝜃1
2
pWx + pWy
2
+ pWz
2
− a22 − a32
 c3 =
2 𝑎2 0 0 𝜃2 2a2 a3
3 𝑎3 0 0 𝜃3
 3 = ATAN 2( s3 , c3 )

( Condition : a2 − a3  2
pWx + pWy
2
+ pWz
2
 a2 + a3 )
5.2 INVERSE KINEMATICS SOLUTIONS: ARTICALUATED ARM

From Direct Kinematics:

+ pWy = ( a2c2 + a3c23 )

2 2 2
pWx
 a2 c2 + a3c23 =  pWx
2
+ pWy
2

  pWx
2
+ pWy2
( a2 + a3c3 ) + pWz a3 s3
c2 =
 a22 + a32 + 2a2 a3c3

 2
pWx + pWy2
a3 s3 + pWz ( a2 + a3c3 )
 s2 =
 a2
2
+ a 3 + 2a2 a3c3
2

  2 = ATAN 2( s2 , c2 )
5.2 INVERSE KINEMATICS SOLUTIONS: ARTICALUATED ARM

From Direct Kinematics:

pWx = c1 pWx

2
+ pWy
2

pWy =  s1 pWx
2
+ pWy
2

 1 = ATAN 2 ( pWy , pWx )

or 1 = ATAN 2 ( − pWy , − pWx )

• There exist four solutions according to the values of 𝜃1 , 𝜃2 , 𝜃3

• It is possible to find the solutions only if at least 𝑝𝑊𝑥 ≠ 0 𝑜𝑟 𝑝𝑊𝑦 ≠ 0
• In case 𝑝𝑊𝑥 = 𝑝𝑊𝑦 = 0, an infinity of solutions is obtained => the arm in such
configuration is kinematically singular
5.2 INVERSE KINEMATICS SOLUTIONS: SPHERICAL WRIST

Problem: find the joint variables 𝜃4 , 𝜃5 , 𝜃6

3
corresponding to a given end-effector orientation T6

 3 nx 3
sx 3
ax 
3 
3
T6 =  n y 3
sy 3
ay 
 3 nz 3
sz 3
az 

Link 𝑎𝑖 𝛼𝑖 𝑑𝑖 𝜃𝑖
 4 = ATAN 2 ( 3 a y , 3 ax )
4 0 −𝜋/2 0 𝜃4 
  
 5 = ATAN 2  ( 3 ax ) + ( 3 a y ) , 3 az  , for 5  (0,  )
2 2
5 0 𝜋/2 0 𝜃5
  
6 0 0 𝑑6 𝜃6  = ATAN 2 ( 3 s , − 3 n )
 6 z z

 c4 c5c6 − s4 s6 −c4 c5 s6 − s4c6 c4 s5 c4 s5 d 6 

s c c + c s   4 = ATAN 2 ( − 3 a y , − 3 ax )
− s4c5 s6 + c4c6 s4 s5 s4 s5 d 6  
3
T6 =  4 5 6 4 6   
 ( x ) ( y ) , az  , for 5  (− , 0)
2 2 3
 − s5c6 s5 s6 c5 c5 d 6  and 
 5 = ATAN 2 − 3
a + 3
a
    
 0 0 0 1   = ATAN 2 ( − 3 s , 3 n )
 6 z z