Inverse Kinematics of Robot

Manipulators
 Objectives

• Understand basic concepts of inverse kinematics

• Understand the concept of workspace

• Compute inverse kinematics of simple robot

manipulators using analytic methods

• Compute the inverse kinematics of complex robot

manipulators using numeric methods

2
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions
5. Numeric solutions

- Inverse Kinematics of Robot Manipulators -

 Kinematics of Robot Manipulators

{n}

qi+1

Relation between joints (qi) and

the pose (position/orientation) of
some point (e.g.: frame {n})
qi
{0}
 Kinematics of Robot Manipulators

x  f (q)

Forward
Joint Kinematics Operational
space space
q  (q1 , , qn ) x  ( x, y , z ,  ,  ,  )

- Forward kinematics
Given a joint configuration, find the pose (position/orientation) of some part of
the robot (e.g. end effector)

Given q  (q1 , , qn )

or x  f (q) For example: x  ( x, y, z ,  ,  ,  )

0
Find Tn (q)
 Kinematics of Robot Manipulators

Joint Operational
space space
q  (q1 , , qn ) x  ( x, y , z ,  ,  ,  )
Inverse
kinematics

q  f 1 (x)
- Inverse kinematics
Find the joint configuration needed to achieve a certain position/orientation
(pose) for some part of the robot

Given Tn (q) or x  f (q)

0
for some point of the robot (e.g. end effector)

Find q  (q1 , , qn )
 Kinematics of Robot Manipulators
Example: R-R Robot

• Position of a 2 dof robot

ŷ
 Cartesian Joint
y variables variables
 x q 
l2 x  q   1
q2  y  q2 
l1 Considering
only position
q1
x x̂

Forward  x  l1 cos( q1 )  l2 cos(q1  q2 )  Given q, there

 y    l sin(q )  l sin(q  q )  x  f (q)
kinematics exists a single x
   1 1 2 1 2 

Inverse  q1  ?  Given x, does there

 q   ? 
1
kinematics q f (x)  ? exist (a single) q?
 2  
 Kinematics of Robot Manipulators
Example: Forward Kinematics

End effector with respect to the base:

Position and
Orientation

where:

Stanford Manipulator
 Kinematics of Robot Manipulators
Example: Inverse Kinematics

Compute the angles that achieve the following pose for

the end effector:
Numeric
matrix

Equivalently, solve the following system:

¡System of nonlinear
trigonometric equations!

Stanford Manipulator
 Inverse Kinematics

• Find the joint angles

q  f 1 (x)
Given a desired position and orientation for the end
effector, find the joint angles

• The input data (position and orientation) are of the form:

 R t x 
T   x   p
0 0 0 1  xr 
Classical formulation: Generalized formulation:
Inverse kinematics for the end effector Inverse kinematics for task variables

• It is a nonlinear problem:
- Is there a solution?
Answer: workspace,
- Is there a unique solution, or multiple solutions?
redundancy
- How to solve it?
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions
5. Numeric solutions

- Inverse Kinematics of Robot Manipulators -

 Workspace
Example: R-R Robot

ŷ
l1 > l2
y  p  x
 y
l2  
q2
l1
q1
x̂
x

• Workspace (l1 > l2): Workspace: WS

𝑊𝑆 = {𝐩 ∈ ℝ : |𝑙 − 𝑙 | ≤ ‖𝐩‖ ≤ 𝑙 + 𝑙 }

with q1 [0, 2π], q2 [0, 2π]

Does the workspace change if joint limits are considered?

 Workspace

• Primary Workspace (reachable): WS1

Positions that can be reached with at least one orientation
Each point can be reached
(orientation “does not matter”)

- Out of WS1 there is no solution to the problem

- For all p WS1 (using a proper orientation), there is at least one solution

• Secondary Workspace (dexterous): WS2

Positions can be reached with any orientation
Reach every point with all
possible orientations

- For all p WS2 there is (at least) one solution for every orientation

• Relation between : WS2 WS1

 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions
5. Numeric solutions

- Inverse Kinematics of Robot Manipulators -

Multiplicity of Solutions
Example: PUMA Robot

If only position is
considered, the 4
configurations set the end
effector at the same point

Multiplicity of solutions

PUMA 560
 Multiplicity of Solutions
What solution to choose?

pA Initial configuration for pA: qA

qA
pB
New configurations for pB: q1, q2
q2
Is it better to choose q1 or q2? q2
q1

Joint distance between configurations:

d1 ‖ q1  q A‖ Shortest distance
is preferred
d 2 ‖ q 2  q A‖

In general, if there are N possible configurations for pB, choose:

qb  arg min‖ q  q A‖ for q  {q1 , q 2 , , q N }

q
 Multiplicity of Solutions
Redundancy

- n joints: q  (q1 , , qn )

- Task space: x  ( x1 , x2 , , xm )
m: dimension of the task space

- Robot is redundant with respect to this

task if:
n>m
dof for position: 3
Robot dof: 4
• Example:
- A 6 dof robot is redundant if only position
(m = 3) is considered (no orientation)
- A 6 dof robot is not redundant if position
and orientation (m = 6) are considered
 Inverse Kinematics
Complexity

• Equations are nonlinear

• There can be:

- One solution
- Multiple solutions
- Infinite solutions (when there is redundancy)
- No admissible solution (outside the workspace)

• When is the existence of a solution guaranteed for the position?

- When the position (of the end effector) belongs to the reachable workspace

• When is the existence of a solution guaranteed for the pose?

- When the pose (of the end effector) belongs to the dexterous workspace
 Inverse Kinematics
Solution Methods
• Analytic Solution (exact)
- It is preferred, when it can be found
- Methods:
• Geometric approach
• Algebraic approach (solution of polynomial equations)

• Numeric Solution (iterative)

- Needed when there is redundancy: n > m
- They use the Jacobian matrix of the forward kinematics
- Usual methods: Newton, gradient descent, etc.
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions
5. Numeric solutions

- Inverse Kinematics of Robot Manipulators -

 Geometric Solutions

• Applicable to robots with few dofs (3 or less)

• In robots with more dof, it can be applied to the first 3 dofs (if the other
dofs are only used for orientation)
• It is not a generic solution → it depends on the robot

• Example:
Find the inverse kinematics for the position of the R-R robot using a
geometric approach

ŷ
y

l2
l1 q2

q1
x x̂
 Geometric Solutions

• Example: ŷ
Find the inverse kinematics for the position of
y
the R-R robot using a geometric approach
l l2
Solution q2
l1
For q2 :  
q1
- Using the law of cosines: x x̂
l 2  l12  l22  2l1l2 cos(180  q2 )
For q1 (using the geometry of the figure)
x 2  y 2  l12  l22  2l1l2 cos(q2 )
q1    
x  y  (l  l2 )
2 2 2 2
c2  1
  atan2( y, x)
2l1l2
- Using a trigonometric identity:
  atan2(l2 s2 , l1  l2 c2 )

s22  c22  1
Inverse kinematics:
s2   1  c 2
2 q1  atan2( y, x)  atan2(l2 s2 , l1  l2 c2 )
q2  atan2( s2 , c2 ) q2  atan2( s2 , c2 )
 Algebraic Solutions

• Solution using algebraic (and polynomial) equations

• Example:
Find the inverse kinematics for the position of the RR robot using an algebraic
approach
ŷ
Solution
- Forward kinematics:
y
x  l1 c1  l2 c12 y  l1 s1  l2 s12 l2
- For q2 : l1 q2

From x 2  l12 c12  l2 2 c12 2  2l1c1l2 c12 q1

F.K.
y 2  l12 s12  l2 2 s12 2  2l1s1l2 s12 x x̂

x 2  y 2  l12  l2 2  2l1l2 (c1c12  s1s12 ) s22  c22  1

x 2  y 2  l12  l2 2  2l1l2 c2 s2   1  c22
x 2  y 2  (l12  l2 2 )
c2  q2  atan2( s2 , c2 )
2l1l2
 Algebraic Solutions

• Example:
Find the inverse kinematics for the position of the RR robot using an algebraic
approach
Solution
- For q1 (expanding terms from forward kinematics): ŷ
x  l1c1  l2 (c1c2  s1 s2 ) y
y  l1s1  l2 ( s1c2  c1s2 )
l2
Equations are linear in c1, s1: solve for them: q2
l1
x  c1 (l1  l2 c2 )  s1 (l2 s2 )
y  s1 (l1  l2 c1 )  c1 (l2 s2 ) q1
x x̂
In matrix form:
y (l1  l2c2 )  xl2 s2
s1 
l1  l2c2 l2 s2  c1   x  solving det
 ls     
 2 2 l1  l2c2   s1   y  x(l  l c )  yl2 s2
c1  1 2 2
det
q1  atan2( s1 , c1 )
det  l12  l22  2l1l2 c2
 Algebraic Solutions
• Example 2
For a 3-dof robot, its end effector pose with respect to its base is given by

where l1, l2 and l3 are constants. The desired pose for the end effector is:

Find the inverse kinematics ( ) of this robot as a function of the

elements of Tdes.
Solution
The procedure consists in obtaining relations between both matrices, and
applying some algebra, so that the value for each joint can be solved.
 Algebraic Solutions

• Example 2
- Finding :

- Finding :

- Finding
 Algebraic Solutions
Problem : The world coordinates for a robot in above example are x3 = 300 mm,y3
= 400 mm, and  = 30° and given that the links have values L1 = 350 mm,L2 = 250
mm and L3 = 50 mm, determine the joint angles 1 , 2 and 3.
Given that L1 = 350 mm, L2 = 250 mm, L3 = 50 mm.
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions
5. Numeric solutions

- Inverse Kinematics of Robot Manipulators -

 Numeric Solutions

• They are mainly used when:

- There is no analytic solution
- There are infinite solutions (e.g. redundant robots)
- It is “too complicated” to find a solution
• Idea:
x  f (q) x  f (q )  0
Forward Find q (using iterations) such that
kinematics the difference is zero

f (q)
• They use the Jacobian matrix: J (q)  𝐽(𝐪) ∈ ℝ ×

q
• Usual solution methods:
- Newton’s method
- Gradient descent method

Notes: - In robotics, the Jacobian matrix is known as the analytic Jacobian

- n: size of q (number of joints)
- m: size of x (size of the task space)
 Numeric Solutions
a) Newton’s Method

• Problem: given a (constant) xd, find q such that

xd: desired value for x
x d  f (q)  0 f : forward kinematics function
• Procedure for the solution
- First order Taylor approximation for f(q)
f (q k )
f (q)  f (q k )  J (q k )(q  q k ) J (q k ) 
q
- Replacing: Jacobian matrix
x d   f (q k )  J (q k )(q  q k )   0

x d  f (q k )  J (q k )(q  q k )

- Assuming J is square and invertible (n = m):

J 1 (q k )  x d  f (q k )   (q  q k )
• Final solution (q = qk+1):

q k 1  q k  J 1 (q k )  x d  f (q k ) 
 Numeric Solutions
a) Newton’s Method

• Algorithm:
- Start with an initial q0 (usually the current configuration)
- Iteratively update using:
q k 1  q k  J 1 (q k )  x d  f (q k )  k = 0,1,2,3,…
- Stop when:
‖ x d  f (q k )‖   or ‖ q k 1  q k‖    : small value
Small Cartesian error Small joint increment
• Comments:
- Convergence if we start with q0 (initial value) close to the solution
- When there is redundancy (m < n):
J is not square (and there is no inverse)! → we use the pseudo-inverse
- Disadvantages:
• Computation time of the inverse (or pseudo-inverse)
• Problems near singularities of J (Jacobian matrix)
- Advantage: it is fast (quadratic convergence)
 Numeric Solutions
a) Newton’s Method

• Example: RR robot
Compute the joint values needed for the end effector to be at x = 1.2, y = 1.2
using Newton’s method. Assume that l1 = l2 = 1.
Solution ŷ
c1  c12 
- Forward kinematics: f (q)    y
s  s
 1 12 
l2
- Jacobian matrix:
l1 q2
 f x f x 
 q2   ( s1  s12 ) q1
f (q)  q1    s12 
J (q)  
q  f y f y   c  c c12  x x̂
   1 12
 q1 q2 
Desired position:
- Inverse of the Jacobian:
1.2
1  c12 s12  xd   
J 1 (q)   (c  c )  ( s  s )  1.2
s2  1 12 1 12 
 Numeric Solutions
a) Newton’s Method

• Example: RR robot
Compute the joint values needed for the end effector to be at x = 1.2, y = 1.2
using Newton’s method. Assume that l1 = l2 = 1.
Solution
- Expression for Newton’s method:
q k 1  q k  J 1 (q k )(x d  f (q))
- Replacing the previous values:

1  c12 s12   1.2  c1  c12  

q k 1  qk    (c  c ) ( s  s )   1.2    s  s  
 s2  1 12 1 12      1 12  
Current error

- It is iteratively applied
For example, use q1 = 0.5 y q2 = 0.5 as initial values (note: we cannot start with q2 = 0
since J-1 would not exist)
 (Generic Gradient Descent)

• Objective:
- Minimize the generic function g (q)
min g (q)
q
• Idea:
- Start with an initial value q0
- “Move” in the negative direction of the gradient ( g), iteratively

q k 1  q k  g (q k ) 𝛼 = size of the step

- The step size α > 0 must guarantee a maximum descent of g(q) in every
iteration:
• α very high: it can lead to divergence (the minimum is not found)
• α very small: it generates a slow convergence

Recall that the gradient indicates the direction of maximum variation

 Numeric Solutions
b) Gradient Descent Method

• Forward kinematics: x d  f (q) or equivalently x d  f (q )  0

error
• Procedure:
1
- Define a scalar error function: g (q)  ‖ x d  f (q )‖
2

2
- Objective: minimize the error: min g (q )
q
- Compute the gradient of g(q):
T
1  f (q) 
g (q)   x d  f (q)   x d  f (q)   x d  f (q) 
T
g (q)    
2  q 
- Apply gradient descent J (q)

q k 1  q k  g (q k )

q k 1  q k   J T (q k )  x d  f (q k ) 

• Pros: computationally simpler (transpose instead of inverse)

• Cons: there can be a slow convergence
 Numeric Solutions
b) Gradient Descent Method

• Example: RR robot
Compute the joint values for the end effector to be at x = 1.2, y = 1.2 using the
gradient descent method. Assume that l1 = l2 = 1.
Solution ŷ
c1  c12 
- Forward kinematics f (q)    y
s  s
 1 12 
l2
- Jacobian matrix: q2
l1
 f x f x 
 q1
f (q)  q1 q2   ( s1  s12 )  s12 
J (q)    
f y   c  c x x̂
q  f y c12 
   1 12
 q1 q2 
Desired position:
- Transpose of the Jacobian:
1.2
 ( s  s ) c1  c12  xd   
J T (q)   1 12 1.2
  s12 c12 
 Numeric Solutions
b) Gradient Descent Method

• Example: RR robot
Compute the joint values for the end effector to be at x = 1.2, y = 1.2 using the
gradient descent method. Assume that l1 = l2 = 1.
Solution
- Expression for gradient descent:
q k 1  q k   J T (q k )(x d  f (q))
- Replacing the previous values:

 ( s  s ) (c1  c12 )   1.2 c1  c12  

q k 1  qk    1 12   1.2   s  s  
  s12 c12      1 12  
Current error

- It is iteratively applied.
For example, use q1 = 0.5 y q2 = 0.5 as initial values (in this case we can start with
q2 = 0)