Ho Chi Minh city University of Technology and Education

Faculty of Electrical & Electronics Engineering

Robotics and Intelligent Control Laboratory

Robotics

Presenter: Assoc. Prof. Dr. Duc Thien, Tran

1
 Tài liệu tham khảo

1. John J. Craig, Introduction to Robotics: Mechanics and Control, 2018.

2. F. Fahimi, Autonomous Robots: modeling, path planning, and control, 2009.
3. PGS. Nguyễn Trường Thịnh, Giáo trình Kỹ thuật Robot, NXB Đại học Quốc gia TP.HCM, 2014.
4. Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani and Giuseppe Oriolo, Robotics: Modelling,
Planning and Control, 2009.

2
 Contents

Topic 1: Introduction
Topic 2: Basic robotic concepts
Topic 3: Spatial Representations of Rigid Bodies
Topic 4: Forward Kinematics of Robot Manipulators
Topic 5: Inverse Kinematics of Robot Manipulators

3
 - Topic 5 -

Inverse Kinematics of Robot Manipulators

4
 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

5
5
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions

5. Numeric solutions

6. Numeric computation of the Jacobian

6
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}

7
Kinematics of Robot Manipulators

x = f (q)

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

Given q = ( q1 , , qn )

Find
0
Tn (q)or x = f (q) For example: x = ( x, y, z ,  ,  ,  )
8
 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 0 Tn (q) or x = f (q) for some point of the robot (e.g. end effector)

Find q = ( q1 , , qn )
9
 Kinematics of Robot Manipulators
Example: R-R Robot
1. Position of a 2 dof robot

ŷ
y  Cartesian Joint
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

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

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

kinematics  q  = ?  q = f −1 (x) = ? exist (a single) q?
 2   10
 Kinematics of Robot Manipulators

Example: Forward Kinematics End effector with respect to the base:

Position and
Orientation

where:

Stanford Manipulator
11
 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
12
 Inverse Kinematics

1. Find the joint angles

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

2. It is a synthesis problem
3. 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
4. It is a nonlinear problem:
• Is there a solution?
• Is there a unique solution, or multiple solutions? Answer: workspace,
• How to solve it? redundancy

13
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions

5. Numeric solutions

6. Numeric computation of the Jacobian

14
 Workspace

1. ABB’s IRB 120 Robot

15
[ABB IRB 120 Industrial Robot – Datasheet]
 Workspace

Example: R-R Robot

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

• Workspace (l1 > l2): Workspace: WS

WS = {p  2
:| l1 − l2 |‖ p‖  l1 + l2 }
with q1 ∈ [0, 2π], q2 ∈ [0, 2π]

Does the workspace change if joint limits are considered?

16
 Workspace

Example: R-R Robot Workspace

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

• If q1 ∈ [0, π], q2 ∈ [0, π], what is the workspace?

- The workspace (WS) is reduced when joint limits are added
- The analytic expression of the WS is more complicated

17
 Workspace

1. 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

2. Secondary Workspace (dexterous): WS2

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

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

3. Relation between WS1 y WS2: WS2 ⊆ WS1

18
 Workspace

1. Example: ABB’s IRB 120 robot

WS1 (=WS2 for spherical wrist)

It is used to evaluate the robot for a specific application

19
[ABB IRB 120 Industrial Robot – Datasheet]
 Workspace

1. Example: KUKA’s LBR iiwa robot

Side View Top View

20
[LBR iiwa 7 R800, LBR iiwa 14 R820 Specification Manual]
 Workspace

1. Example:
ABB’s IRB 360 robot

21
[IRB 360 Flex Picker Specifications]
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions

5. Numeric solutions

6. Numeric computation of the Jacobian

22
 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

23
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 , , qN }

q 24
 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

25
Soft robot

26
 Inverse Kinematics

Complexity
1. Equation are nonlinear
2. There can be:
• One solution
• Multiple solutions
• Infinite solutions (when there is redundancy)
• No admissible solution (outside the workspace)
3. 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

27
 Inverse Kinematics

Solution Methods
1. Analytic Solution (exact)
It is preferred, when it can be found
Methods:
• Geometric ad-hoc approach
• Algebraic approach (solution of polynomial equations)
• Systematic reduction approach (obtain a reduced set of equations)
• Kinematic decoupling (Pieper): robots with 6 dof.
• When the last 3 axes are revolute, and they intersect each other (spherical wrist).
2. Numeric Solution (iterative)
• Needed when there is redundancy: n > m
• Easier to obtain (slower run time?)
• They use the Jacobian matrix of the forward kinematics
• Usual methods: Newton, gradient descent, etc.
28
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions

5. Numeric solutions

6. Numeric computation of the Jacobian

29
 Geometric Solutions

1. Applicable to robots with few dofs (3 or less)

2. In robots with more dof, it can be applied to the first 3 dofs (if the other dofs are only used for
orientation)
3. It is not a generic solution → it depends on the robot
4. Example:
Find the inverse kinematics for the position of the R-R robot using a geometric approach

ŷ
y

l2
l1 q2

q1
x x̂ 30
 Geometric Solutions

1. Example 1: Find the inverse kinematics for the position of the R-R robot using a geometric
approach ŷ
y
Solution
l l2
For q2 : q2
l1
- Using the law of cosines:  
q1
l 2 = l12 + l22 − 2l1l2 cos(180 − q2 ) x x̂
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 )
31
 Geometric Solutions
ye
xe ye
𝑧𝑒 l2
𝑞2 xe
l2
l1
y0
l1
𝑞1 x0
yW y0
xW x0
l0
zB
l0
xB 𝑥𝑒

32
 Geometric Solutions
Example 2: Find the inverse kinematics for the position of the R-R-R robot
using a geometric approach.
Apply the “law of cosines” to solve for 𝜃2 y0
(𝑥 − 𝑙3 𝑐123 )2 + 𝑦 − 𝑙3 𝑠123 2 = 𝑙12 + 𝑙22 − 2𝑙1 𝑙2 cos 180 − 𝜃2 (G1)
y
Where 𝑥1 = 𝑥 − 𝑙3 𝑐123 and 𝑦1 = 𝑦 − 𝑙3 𝑠123 .
l3
Because cos 180 − 𝜃2 = − cos −𝜃2 = − cos 𝜃2 ,
𝑥12 +𝑦12 −𝑙12 −𝑙22
cos 𝜃2 = 2𝑙1 𝑙2
(G2) y1
l2
sin 𝜃2 = ± 1 − cos2 𝜃2 (G3)
There are two solutions for 𝜃2 which can be positive or negative values l1
To solve for 𝜃1 y
𝛽 = 𝑎𝑡𝑎𝑛2 𝑦1 , 𝑥1 (G4)

By apply the law of cosines to find ψ: q1
x1 x x0
𝑥12 +𝑦12 +𝑙12 −𝑙22
cos 𝜓 = (G5)
2𝑙1 𝑥12 +𝑦12
33
 Geometric Solutions
The arccosine must be solved so 0 ≤ 𝜓 ≤ 180𝑜 in order that the
geometry which leads to (G6) will be preserved
y0
𝜃1 = 𝛽 ± 𝜓 (G6)
y
Where the plus sign is used if 𝜃2 < 0 and the minus sign if 𝜃2 >
l3
0.
To solve 𝜃3 y1
l2
𝜃3 = 𝜃 − 𝜃1 − 𝜃2

l1
y


q1
x1 x x0

34
 Algebraic Solutions

1. Solution using algebraic (and polynomial) equations

Example 1:
Find the inverse kinematics for the position of the RR robot using an algebraic approach
Solution - Forward kinematics: c12 − s12 0 l1c1 + l2 c12  cosy − siny 0 0
x2  ŷ
s  
0 l1s1 + l2 s12  siny cosy 0
0 y2  0 y
T2 = 
c12
T2 =  12
0 0
2
0 0 1 0   0 0 1 0 
    l2
0 0 0 1   0 0 0 1 
q2
l1
- For q2 :
From
0
x2 2 = l12 c12 + l2 2 c12 2 + 2l1c1l2 c12 q1
F.K.
y2 2 = l12 s12 + l2 2 s12 2 + 2l1s1l2 s12
0 0
x2 x̂

0
x2 2 + 0 y2 2 = l12 + l2 2 + 2l1l2 (c1c12 + s1s12 ) s22 + c22 = 1
0
x2 2 + 0 y2 2 = l12 + l2 2 + 2l1l2 c2 s2 =  1 − c22
x2 2 + 0 y2 2 − (l12 + l2 2 )
0
c2 = q2 = atan2( s2 , c2 )
2l1l2 35
 Algebraic Solutions

- For q1 (expanding terms from forward kinematics):

ŷ
0
x2 = l1c1 + l2 (c1c2 − s1s2 )
y
0
y2 = l1s1 + l2 ( s1c2 + c1s2 )
l2
Equations are linear in c1, s1: solve for them:
l1 q2
0
x2 = c1 (l1 + l2 c2 ) − s1 (l2 s2 ) q1
x x̂
0
y2 = s1 (l1 + l2 c 2 ) + c1 (l2 s2 )

In matrix form:
y2 (l1 + l2 c2 ) −0 x2l2 s2
0

l1 + l2 c2 −l2 s2  c1   0 x2  solving s1 =

 ls = 
det
 2 2 l1 + l2 c2   s1   0 y2  0
x2 (l1 + l2 c2 ) + 0 y2l2 s2
c1 =
det
q1 = atan2( s1 , c1 )
det = l12 + l22 + 2l1l2 c2 36
 Example 2

X4(EE)
Forward kinematic
𝑐123 −𝑠123 0 𝐿3 𝑐123 + 𝐿2 𝑐12 + 𝐿1 𝑐1
Z4(EE)
0 𝑇 = 𝑠123 𝑐123 0 𝐿3 𝑠123 + 𝐿2 𝑠12 + 𝐿1 𝑠1
(A.1)
4 0 0 1 0
X3
0 0 0 1
Z3
Y4(EE) The desired pose at end-effector with respect to the base
Y3
𝑐𝜙 −𝑠𝜙 0 𝑥
X2 0 𝑇 = 𝑠𝜙 𝑐𝜙 0 𝑦
Y2 4𝑑 (A.2)
Y0 0 0 1 0
Z2
0 0 0 1
Y1 X1 By equating (A.1) and (A.2), we have four nonlinear equations
Z0 Z1
X0 𝑐𝜙 = 𝑐123 (A.3)
𝑠𝜙 = 𝑠123 (A.4)
𝑥 = 𝐿1 𝑐1 + 𝐿2 𝑐12 + 𝐿3 𝑐123 (A.5)
𝑦 = 𝐿1 𝑠1 + 𝐿2 𝑠12 + 𝐿3 𝑠123 (A.6)
37
 Example 2

X4(EE)
Replacing (A.3) and (A.4) into (A.5) and (A.6), respectively, we have

Z4(EE)
𝑥 = 𝐿1 𝑐1 + 𝐿2 𝑐12 + 𝐿3 𝑐𝜙 (A.7)
𝑦 = 𝐿1 𝑠1 + 𝐿2 𝑠12 + 𝐿3 𝑠𝜙 (A.8)
X3
Let’s define 𝑥0 = 𝑥 − 𝐿3 𝑐𝜙, 𝑦0 = 𝑦 − 𝐿3 𝑠𝜙
Z3
Y4(EE)
Y3 The Eq (A.7) and (A.8) is rewritten as follows
X2 𝑥0 = 𝐿1 𝑐1 + 𝐿2 𝑐12 (A.9)
Y2
Y0 𝑦0 = 𝐿1 𝑠1 + 𝐿2 𝑠12 (A.10)
Z2
According example 1, we can specify the 𝜃1 and 𝜃2 as follows
Y1 X1
𝑥02 +𝑦02 −𝐿21 −𝐿22
𝜽𝟐 = 𝒂𝒕𝒂𝒏𝟐 𝒔𝟐 , 𝒄𝟐 where 𝑐2 = , 𝑠2 = ± 1 − 𝑐22
X0 2𝐿1 𝐿2
Z0 Z1
𝑦2 𝐿1 +𝐿2 𝑐2 −𝑥0 𝐿2 𝑠2 𝑥0 𝐿1 +𝐿2 𝑐2 +𝑦0 𝐿2 𝑠2
𝜽𝟏 = 𝐚𝐭𝐚𝐧(𝒔𝟏 , 𝒄𝟏 ) where 𝑠1 = , 𝑐1 = , 𝑑𝑒𝑡 =
𝑑𝑒𝑡 𝑑𝑒𝑡
𝐿21 + 𝐿22 + 2𝐿1 𝐿2 𝑐2
𝜽𝟑 = 𝝓 − 𝜽𝟏 − 𝜽𝟐
38
 Example 3

For a 3-dof robot, its end effector pose with respect to its base is given by

where a3 and d1 are constants. The desired pose for the end effector is:

Find the inverse kinematics (𝜃1 , 𝜃2 , 𝑞2 ) 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.
39
 Example 3

- Finding 𝜃1 :

- Finding 𝜃3 :

- Finding 𝜃2 :

40
 Algebraic solution by reduction to
polynomial

Using a single variable to simplify the triangle functions:

𝜃
𝑢 = tan
2
1 − 𝑢2
cos 𝜃 =
1 + 𝑢2
2𝑢
sin 𝜃 =
1 + 𝑢2
This is a very important geometric substitution used often in solving kinematic equations.

41
 Example

Solve the below equation to find 𝜃:

𝑎𝑐𝑜𝑠𝜃 + 𝑏𝑠𝑖𝑛𝜃 = 𝑐 (E.1)
𝜃 1−𝑢2 2𝑢
Let’s define 𝑢 = tan , cos 𝜃 = , sin 𝜃 =
2 1+𝑢2 1+𝑢2

Substituting into (E.1) and multiplying through by 1 + 𝑢2 , we have

𝑎 1 − 𝑢2 + 2𝑏𝑢 = 𝑐(1 + 𝑢2 ) (E.2)
Collecting powers of u yields
𝑎 + 𝑐 𝑢2 − 2𝑏𝑢 + 𝑐 − 𝑎 = 0 (E.3)
Which is solved by the quadratic formula:
𝑏± 𝑏2 +𝑎2 −𝑐 2
𝑢= (E.4)
𝑎+𝑐
𝑏± 𝑏2 +𝑎2 −𝑐 2
So, 𝜃 = 2𝑎𝑡𝑎𝑛
𝑎+𝑐

42
Inverse kinematic Examples

43
 Bài tập

• Tính 0 𝑇𝑒 dựa vào phương pháp DH cải tiến

với mô hình robot trong Figure 1.
• Xác định ma trận 0𝑇 dựa vào Figure 2.
𝑒𝑑𝑒𝑠𝑖𝑟𝑒𝑑 xe

• Tính giá trị 𝑞1 , 𝑞2 và 𝑞3 sao cho robot có tư thế ye

sau dựa trên kết quả 0 𝑇𝑒𝑑𝑒𝑠𝑖𝑟𝑒𝑑 . l3 q3

ye
l3
l2 xe

l2
q2
l1
y0 y0 l1
q1 x0 x0 300

Figure 1 Hệ cánh tay robot Figure 2 Tư thế của cánh

3 bậc tự do tay robot
44
 Bài tập

• Tính 𝑤 𝑇𝑒 dựa vào phương pháp

DH cải tiến với mô hình robot trong
Figure 1.
xe
• Xác định ma trận 𝑤𝑇 dựa
𝑒𝑑𝑒𝑠𝑖𝑟𝑒𝑑 ye
vào Figure 2.
l3 q3
• Tính giá trị 𝑞1 , 𝑞2 và 𝑞3 sao cho
l3
robot có tư thế sau dựa trên kết quả
𝑤
𝑇𝑒𝑑𝑒𝑠𝑖𝑟𝑒𝑑 . l2 300

l2
q2
l1
yw y0 yw y0 l1
q1 xw x0 300
xw x0

l0 l0

Figure 1 Hệ cánh tay robot Figure 2 Tư thế của cánh

3 bậc tự do tay robot
45
 Outline

1. Introduction

2. Workspace (and the existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions

5. Numeric solutions

6. Numeric computation of the Jacobian

46
Numeric solutions

47
 Numeric Solutions

1. 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
2. Idea: x = f (q) x − f (q) = 0
Forward Find q (using iterations) such that
kinematics the difference is zero

f (q) n m
J (q) = J (q) 
3. They use the Jacobian matrix: q

4. Usual solution methods:

• Newton’s method
Notes: - In robotics, the Jacobian matrix is known as the analytic Jacobian
• Gradient descent method - n: size of q (number of joints)
- m: size of x (size of the task space)

48
 Numeric Solutions

a) Newton’s Method
1. Problem: given a (constant) xd, find q such that xd: desired value for x
f : forward kinematics function
x d − f (q) = 0
2. 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: x d − ( f (q k ) + J (q k )(q − q k ) ) = 0 Jacobian matrix

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 )
3. Final solution (q = qk+1):

q k +1 = q k + J −1 (q k ) ( x d − f (q k ) )
49
 Numeric Solutions

a) Newton’s Method
1. 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
2. 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) 50
 Numeric Solutions

a) Newton’s Method
1. 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 c + c 
- Forward kinematics: f (q) =  1 12  y
 s1 + s12 
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 
51
 Numeric Solutions

a) Newton’s Method
1. 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)
52
 Numeric Solutions

a) Newton’s Method
function [q1,q2,i]=NewtonIK(x,y,q01,q02)
1. Example: RR robot epsilon = 1e-3;
max_iter = 100; %Maximum number of iterations
xd=[x,y]';
q=[q01,q02]';
%Iterations: Newton's method
for i=1:max_iter
q1 = q(1); q2 =q(2);
J =[-sin(q1)-sin(q1+q2), -sin(q1+q2);...
cos(q1)+cos(q1+q2), cos(q1+q2)];
f = [cos(q1)+cos(q1+q2), sin(q1)+sin(q1+q2)]';
e = xd-f;
q = q + inv(J)*e;
%End condition
if (norm(e) < epsilon)
break
end
end
end

53
 Numeric Solutions

a) Newton’s Method
1. Example: RR robot
Note that the result depends on the initial value.
Results: 0.2278  1.3430 
q=  q= 
1.1157   −1.1152 

Final configuration when the initial Final configuration when the initial
value was q=[0.5; 0.5] value was q = [1; -1]
54
 (Generic Gradient Descent)

1. Objective:
• Minimize the generic function g (q)
min g (q)
q
2. 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 55

 Numeric Solutions

b) Gradient Descent Method

1. Forward kinematics: x d = f (q) or equivalently x d − f (q) = 0
2. Procedure: error
1
Define a scalar error function: g (q) = ‖ x d − f (q)‖ 2
g: n
→
2
Objective: minimize the error: min g (q)
q
• Compute the gradient of g(q):
T
1
g (q) = ( x d − f (q) ) ( x d − f (q) )
T  f (q) 
g (q) = −   ( x d − f (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 ) )
3. Pros: computationally simpler (transpose instead of inverse)
4. Cons: there can be a slow convergence

56
 Numeric Solutions

b) Gradient Descent Method

1. 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 c + c 
- Forward kinematics f (q) =  1 12  y
 s1 + s12 
l2
- Jacobian matrix: q2
l1
 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:
- Transpose of the Jacobian:
1.2 
 −( s + s ) c1 + c12  xd =  
J T (q) =  1 12 1.2 
 − s12 c12 
57
 Numeric Solutions

b) Gradient Descent Method

1. 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:

qk +1 = qk +  J T (qk )(xd − 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)
58
 Numeric Solutions

Comparison
1. Newton’s method
• Quadratic convergence rate (it is fast)
• Problems near singularities (singularities can be computed using the singular values of J)
2. Gradient descent method
• Linear convergence rate (it is slow)
• It does not have singularity problems
• The step size (α) must be carefully chosen (but there exist adaptive methods such as line
search)
3. Efficient algorithm:
• Start with the gradient descent method (safe, but slow)
• Then switch to Newton’s method

59
 Outline

1. Introduction

2. Workspace (and existence of solutions)

3. Multiplicity of solutions

4. Analytic solutions

5. Numeric solutions

6. Numeric Computation of the Jacobian

60
 Numeric Computation of the Jacobian

1. For complex robots:

• Very tedious to manually compute the Jacobian
2. Alternative:
• Compute the Jacobian numerically: numeric differentiation
3. The Jacobian (considering only position) of an n dof robot is:

Each column is the

variation with respect
to a given angle:
position
x p = ( x, y , z )
Approximation of the derivative of the position xp with respect to joint qi x p = f (q1 , , qn )

x p x p f (q1 , , qi + qi , , qn ) − f (q1 , , qn )

 = Increment only for
qi qi qi joint qi

61
 Numeric Computation of the Jacobian

1. Example
Consider the following forward kinematics (R-R Robot with l1 = l2 = 1):
 f x (q)  cos(q1 ) + cos(q1 + q2 ) 
f (q) =  = 
 f y (q )   sen( q1 ) + s en ( q1 + q2 
)
Compute the Jacobian numerically when the joint values are q1 = 0.5, q2 = 1.0
Solution
 f x f x 
- The Jacobian is:  q q2 
J (q) =  
1

 f y f y 
 
 q1 q2 

- Derivatives with respect to q1 (first column):

f x f x (q1 + q1 , q2 ) − f x (q1 , q2 ) =  cos(q1 + q1 ) + cos((q1 + q1 ) + q2 )  − ( cos( q1 ) + cos( q1 + q2 ) )
=
q1 q1 q1

f y
=
f y (q1 + q1 , q2 ) − f y (q1 , q2 )
=
sen(q1 + q1 ) + sen((q1 + q1 ) + q2 ) − ( sen(q1 ) + sen(q1 + q2 ) )
q1 q1 q1
62
 Numeric Computation of the Jacobian

1. Example
Consider the following forward kinematics (R-R Robot with l1 = l2 = 1):
 f x (q)  cos(q1 ) + cos(q1 + q2 ) 
f (q) =  = 
 y  sen(q1 ) + sen(q1 + q2 ) 
f (q )
Compute the Jacobian numerically when the joint values are q1 = 0.5, q2 = 1.0
Solution  f x f x 
- The Jacobian is:  q q2 
J (q) =  
1

 f y f y 
 
 q1 q2 

- Derivatives with respect to q2 (second column):

f x f x (q1 , q2 + q2 ) − f x (q1 , q2 ) =  cos(q1 ) + cos(q1 + (q2 + q2 ))  − ( cos(q1 ) + cos( q1 + q2 ) )
=
q2 q2 q2

f y
=
f y (q1 , q2 + q2 ) − f y (q1 , q2 )
=
sen(q1 ) + sen(q1 + (q2 + q2 )) − ( sen(q1 ) + sen(q1 + q2 ) )
q2 q2 q1
63
 Numeric Computation of the Jacobian

1. Example
- Using Δq1 = Δq2 = 0.001:

f x ( cos(0.5 + 0.001) + cos(0.5 + 0.001 + 1.0) ) − ( cos(0.5) + cos(0.5 + 1.0) )

= = −1.4774
q1 0.001

f y
=
( sen(0.5 + 0.001) + sen(0.5 + 0.001 + 1.0) ) − ( sen(0.5) + sen(0.5 + 1.0) ) = 0.9476
q1 0.001

f x ( cos(0.5) + cos(0.5 + 1.0 + 0.001) ) − ( cos(0.5) + cos(0.5 + 1.0) )

= = −0.9975
q2 0.001

f y
=
( sen(0.5) + sen(0.5 + 1.0 + 0.001) ) − ( sen(0.5) + sen(0.5 + 1.0) ) = 0.0702
q2 0.001

Jacobian using the analytical approach

- Jacobian using the numeric computation: (for comparison)

 −1.4774 −0.9975  −1.4769 −0.9975

J (q) =   J (q) =  
 0.9476 0.0702   0.9483 0.0707 
Similar values (within rounding errors) 64
 Conclusions

1. Inverse kinematics finds joint configurations given a desired position and orientation (pose)
2. In inverse kinematics, there can be a single solution, many solutions, infinite solutions, or no
solution (the existence of solutions is defined by the workspace)
3. Analytic solutions are more complex to find, less systematic and applicable mainly to simple
cases
4. Numeric solutions are more generic, and they can be applied to all the cases

65
65
 Exercises

John J. Craig, Introduction to Robotics: Mechanics and Control (3rd Edition),

Chapter 4: Inverse manipulator kinematics
• 4.2, 4.4
• MATLAB Exercise 4 (a, b)

66
 References

• B. Siciliano, L. Sciavicco, L. Villani, y G. Oriolo. Robotics: modelling, planning and control. Springer
Science & Business Media, 2010 (Chapter 2.12)
• M.W. Spong, S. Hutchinson, y M. Vidyasagar. Robot Modeling and Control. John Wiley & Sons,
2006 (Chapter 1.4, 3.3)
• K. Lynch and F. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University
Press, 2017 (Chapter 6.1-6.2)
• John J. Craig, Introduction to Robotics Mechanics and Control, Pearson Prentice Hall.

67
Thank you for your listening

70