Foundations of Robotics: 2018-I

- Topic 4 -
Inverse Kinematics of Robot
Manipulators
Prof. Oscar E. Ramos, Ph.D.

- Week 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

2
 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

- Inverse Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 3
 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}

4
 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 )
0
Find Tn (q) or x  f (q) For example: x  ( x, y, z ,  ,  ,  )

5
 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
0
GIven Tn (q) or x  f (q) for some point of the robot (e.g. end effector)

Find q  (q1 , , qn )

6
 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

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

End effector with respect to the base:

Position and
Orientation

where:

Stanford Manipulator
8
 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
9
 Inverse Kinematics

• Find the joint angles

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

• It is a synthesis problem
• 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?

10
 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

- Inverse Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 11
 Workspace

• ABB’s IRB 120 Robot

[ABB IRB 120 Industrial Robot – Datasheet] 12

 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?

13
 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

14
 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 poing with all
possible orientations

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

• Relation between WS1 y WS2: WS2 ⊆ WS1

15
 Workspace

• Example: ABB’s IRB 120 robot

WS1 (=WS2 for spherical wrist)

It is used to evaluate the robot for a specific application

[ABB IRB 120 Industrial Robot – Datasheet] 16

 Workspace

• Example: KUKA’s LBR iiwa robot

Side View Top View

[LBR iiwa 7 R800, LBR iiwa 14 R820 Specification Manual] 17

 Workspace

• Example:
ABB’s IRB 360 robot

[IRB 360 Flex Picker Specifications] 18

 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

- Inverse Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 19
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 20
 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
21
 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

22
 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

23
 Inverse Kinematics
Solution Methods
• 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)

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

24
 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

- Inverse Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 25
 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̂
26
 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    
2 2 2 2
x  y  (l  l2 )1
c2    atan2( y, x)
2l1l2
- Using a trigonometric identity:
  atan2(l2 s2 , l1  l2 c2 )

s22  c22  1
Inverse kinematics:
2
s2   1  c 2 q1  atan2( y, x)  atan2(l2 s2 , l1  l2 c2 )
q2  atan2( s2 , c2 ) q2  atan2( s2 , c2 )
27
 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
28
 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  s1s2 ) 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  l2 c2 )  xl2 s2
s1 
l1  l2 c2 l2 s2  c1   x  solving det
 ls 
 2 2 l1  l2 c2   s1   y  x(l  l c )  yl2 s2
c1  1 2 2
det
q1  atan2( s1 , c1 )
det  l12  l22  2l1l2 c2
29
 Algebraic Solutions

• Example 2
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 ( , , ) 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.

30
 Algebraic Solutions

• Example 2
- Finding :

- Finding :

- Finding :

31
 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

- Inverse Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 32
 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)  J (q)   nm
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)
33
 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 ) 

34
 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)

35
 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 )  s  s  y
 1 12 
l2
- Jacobian matrix:
l1 q2
 f x f x 

f (q)  q1 q2   ( s1  s12 )  s12  q1
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 

36
 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 )(xd  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)

37
 Numeric Solutions
a) Newton’s Method

• Example: RR robot
import numpy as np
cos=np.cos; sin=np.sin

xd = np.array([1.2, 1.2]) # Desired value in the Cartesian space

q = np.array([0.5, 0.5]) # Initial value in the joint space
epsilon = 1e-3
max_iter = 100 # Maximum number of iterations

# Iterations: Newton's method

for i in range(max_iter):
q1 = q[0]; q2 = q[1];
J = np.array([
[-sin(q1)-sin(q1+q2), -sin(q1+q2)],
[ cos(q1)+cos(q1+q2), cos(q1+q2)]])
f = np.array([cos(q1)+cos(q1+q2), sin(q1)+sin(q1+q2)])
e = xd-f
q = q + np.dot(np.linalg.inv(J), e)
# End condition
if (np.linalg.norm(e) < epsilon):
break
print(q)
38
 Numeric Solutions
a) Newton’s Method

• Example: RR robot
- Note that the result depends on the initial value.
- Results:
0.2278  1.3430 
q  q 
1.1157   1.1152 
1.4

1.2

0.8

0.6

0.4

0.2

0
0 0.2 0.4 0.6 0.8 1 1.2

Final configuration when the Final configuration when the

initial value was q=[0.5; 0.5] initial value was q = [1; -1]

39
 (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

40
 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 g : n  
2
- Objective: minimize the error: min g (q )
q
- Compute the gradient of g(q):
T
1 T  f (q) 
g (q)   x d  f (q)   x d  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 ) 

• Pros: computationally simpler (transpose instead of inverse)

• Cons: there can be a slow convergence
41
 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 )  s  s  y
 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 
42
 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)

43
 Numeric Solutions
b) Gradient Descent Method

• Example: RR robot
import numpy as np
cos=np.cos; sin=np.sin

xd = np.array([1.2, 1.2]) # Desired value in the Cartesian space

q = np.array([0.5, 0.5]) # Initial value in the joint space
epsilon = 1e-3
max_iter = 1000 # Maximum number of iterations
alpha = 0.5

# Iterations: Gradient descent

for i in range(max_iter):
q1 = q[0]; q2 = q[1];
J = np.array([[-sin(q1)-sin(q1+q2), -sin(q1+q2)],
[ cos(q1)+cos(q1+q2), cos(q1+q2)]])
f = np.array([cos(q1)+cos(q1+q2), sin(q1)+sin(q1+q2)])
e = xd-f
q = q + alpha*np.dot(J.T, e)
# End condition
if (np.linalg.norm(e) < epsilon):
break
print(q)
44
 Numeric Solutions
Comparison

• Newton’s method
- Quadratic convergence rate (it is fast)
- Problems near singularities (singularities can be computed using the singular
values of J)

• 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)

• Efficient algorithm:
- Start with the gradient descent method (safe, but slow)
- Then switch to Newton’s method

45
 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

- Inverse Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 46
 Numeric Computation of the Jacobian

• For complex robots:

- Very tedious to manually compute the Jacobian
• Alternative:
- Compute the Jacobian numerically: numeric differentiation

• 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 )
x p  f (q1 , , qn )

- Approximation of the derivative of the position xp with respect to joint qi

Increment only
x p x p f (q1 , , qi  qi , , qn )  f (q1 , , qn )
  for joint qi
qi qi qi
47
 Numeric Computation of the Jacobian

• 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  q )
2 

Compute the Jacobian numerically when the joint values are q1 = 0.5, q2 = 1.0
Solution
- The Jacobian is:  f x f x 
 q q2 
1
J (q)   
 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
48
 Numeric Computation of the Jacobian

• 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  q )
2 

Compute the Jacobian numerically when the joint values are q1 = 0.5, q2 = 1.0
Solution
- The Jacobian is:  f x f x 
 q q2 
1
J (q)   
 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
49
 Numeric Computation of the Jacobian

• 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

- Jacobian using the numeric computation: approach (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)
50
 Numeric Computation of the Jacobian

• Example
Computationally (using Python):

- Forward kinematics function

def fkine(q):
x = np.cos(q[0]) + np.cos(q[0]+q[1]);
y = np.sin(q[0]) + np.sin(q[0]+q[1]);
return np.array([x,y])

- Computation of the Jacobian

q1 = 0.5; q2 = 1.0
delta = 0.001

JT = 1/delta*np.array([
fkine([q1+delta, q2]) - fkine([q1,q2]),
fkine([q1, q2+delta]) - fkine([q1,q2])])
J = JT.transpose()
print(J)

51
 Conclusions

• Inverse kinematics finds joint configurations given a

desired position and orientation (pose)

• 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)

• Analytic solutions are more complex to find, less

systematic and applicable mainly to simple cases

• Numeric solutions are more generic, and they can be

applied to all the cases

52
 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)

53
 Appendix 1: Newton’s Method

• Objective:
- Find the point where the function is zero
- Equivalently: find the intersection of the curve y = f(x) with the x axis, that is:
f(x) = 0

Idea

Imagen de: https://en.wikipedia.org/wiki/Newton%27s_method

• How? Using iterations

54
 Appendix 1: Newton’s Method

• For scalars (1-D): f ( x)

- Tangent line at x0
f ( x)  f ( x0 )  f '( x0 )( x  x0 ) f ( x0 )

- We want the intersection with 0: f(x) = 0

 f ( x0 )  f '( x0 )( x  x0 )
f ( x0 )
x  x0  x0
f ( x0 ) x
- In general:
f ( xk ) Apply
xk 1  xk  iteratively
x0 , x1 , x2 , x3 ,
f ( xk )

- Alternatively: Taylor expansion

f ( x)  f ( x0 )  f '( x0 )( x  x0 )  o ‖x  x0‖2 

High-order terms

Discarding the high-order terms, we obtain the line equation

55
 Appendix 1: Newton’s Method

• Example:
Find the intersection with zero for f(x) = 3 + (x-2)3 using the Newton’s method
and 10 iterations
f ( xk ) 3  ( xk  2)3
xk 1  xk  xk 1  xk 
f ( xk ) 3( xk  2) 2

# Initial value (any value)

xo = 0;
# Iterations
for i in range(10):
xo = xo - (3+(xo-2)**3)/(3*(xo-2)**2)
print('Intersection at: ', str(xo))

# Plot
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt

x = np.arange(0,1,0.01)
y = 3 + (x-2)**3 Using Python, the
plt.plot(x,y); plt.plot(xo,0,'o'); intersection is at: 0.55775
plt.grid()
56