Ho Chi Minh city University of Technology and Education

Faculty of Electrical & Electronics Engineering

Robotics and Intelligent Control Laboratory

Robotics

Presenter: 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
 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

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 ,  ,  ,  )

Given q = ( q1 , , qn )

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

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

 Workspace

1. ABB’s IRB 120 Robot

[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?

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

 Workspace

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

 Workspace

1. Example: KUKA’s LBR iiwa robot

Side View Top View

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

 Workspace

1. Example:
ABB’s IRB 360 robot

[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

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

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

 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̂
 Geometric Solutions

1. Example: 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 )
 Algebraic Solutions

1. Solution using algebraic (and polynomial) equations

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

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

1. 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 (𝜃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.
 Algebraic Solutions

1. Example 2
- Finding 𝜃1 :

- Finding 𝜃3 :

- Finding 𝜃2 :
 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.

35
 For example

Convert the transcendental equation

acos 𝜃 + 𝑏𝑠𝑖𝑛 𝜃 = 𝑐
Into a polynomial in the tangent of the half angle, and solve for 𝜃
Substituting 𝜃 by 𝑢 and multiplying through by 1 + 𝑢2 , we have
𝑎 1 − 𝑢2 + 2𝑏𝑢 = 𝑐 1 + 𝑢2
Collecting powers of u yields
𝑎 + 𝑐 𝑢2 − 2𝑏𝑢 + 𝑐 − 𝑎 = 0
Which is solved by quadratic formula:
𝑏 ± 𝑏2 + 𝑎2 − 𝑐 2
𝑢=
𝑎+𝑐
So,
𝑏 ± 𝑏 2 + 𝑎2 − 𝑐 2
𝜃 = 2 tan−1
𝑎+𝑐
36
 Exercise

Given a 3-DOF manipulator in Figure 1. (x,y,q)

Please calculate its inverse kinematics by
1. Geometric approach. l3
2. Algebraic method.

q3
l2
y0

q1 q2

x0
37
 Geometric
Apply the “law of cosines” to solve for 𝜃2
(𝑥 − 𝑙3 𝑐123 )2 + 𝑦 − 𝑙3 𝑠123 2
= 𝑙12 + 𝑙22 − 2𝑙1 𝑙2 cos 180 + 𝜃2 (G1)
y0
Where 𝑥1 = 𝑥 − 𝑙3 𝑐123 and 𝑦1 = 𝑦 − 𝑙3 𝑠123 .
Because cos 180 + 𝜃2 = − cos 𝜃2 , y

𝑥12 +𝑦12 −𝑙12 −𝑙22 l3

cos 𝜃2 = 2𝑙1 𝑙2
(G2)

sin 𝜃2 = ± 1 − cos 2 𝜃2 (G3) y1

l2
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
38
 Geometric
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

39
 MATLAB Program

Forward kinematics Inversed kinematics

- Create MATLAB Function - Create a MATLAB Function.
- Create a Matlab Simulink file. - Create a MATLAB Simulink file.

40
 Forward kinematics

Standard D-H Approach 1. MATLAB function

𝑐𝜃123 −𝑠𝜃123 0 𝐿3 𝑐𝜃123 + 𝐿2 𝑐𝜃12 + 𝐿1 𝑐𝜃1
0 𝑠𝜃123 𝑐𝜃123 0 𝐿3 𝑠𝜃123 + 𝐿2 𝑠𝜃12 + 𝐿1 𝑠𝜃1 2. MATLAB Simulink
3𝑇 =
0 0 1 0
0 0 0 1

42
 Inverse kinematics

Results: 1. MATLAB function

2. MATLAB Simulink

43
 Inverse kinematics

function [theta1,theta2,theta3] = function [theta1,theta2,theta3] =

IK1(x,y,theta,l1,l2,l3) IK2(x,y,theta,l1,l2,l3)
nx=x-l3*cosd(theta); nx=x-l3*cosd(theta);

ny=y-l3*sind(theta); ny=y-l3*sind(theta);

theta2=acosd((nx^2+ny^2-l1^2-l2^2)/(2*l1*l2)); theta2=acosd((nx^2+ny^2-l1^2-l2^2)/(2*l1*l2));

theta1=atan2d(ny,nx)-acosd((nx^2+ny^2+l1^2- theta1=atan2d(ny,nx)+acosd((nx^2+ny^2+l1^2-
l2^2)/(2*l1*(sqrt(nx^2+ny^2)))); l2^2)/(2*l1*(sqrt(nx^2+ny^2))));

theta3=theta-theta1-theta2; theta3=theta-theta1-theta2;

end end

44
Check inversed and forward kinematics

45
 Specify the workspace

clc; clear all; close all;

1 = 100; l2 = 200; l3 = 150;
tt=0;
for the1 = -90:90
for the2 = -90:90
for the3 =-90:90
if (the1 + the2 + the3) == 0 %% Điều
kiện của khâu cuối
tt=tt+1;
x = % động học thuận 1,
y = % động học thuận 2,
emtry(:,tt) = [x;y];
end
end
end
end
figure(1);
plot(emtry(1,:),emtry(2,:),'.b','MarkerSize',2);

46
 Specify the workspace

clc; clear all; close all;

l1 = 100; l2 = 200; l3 = 150;
tt=0;
for the1 = -90:90
for the2 = -90:90
tt=tt+1;
x = % động học thuận 1,
y = % động học thuận 2,
emtry(:,tt) = [x;y];
end
end
figure(1);
plot(emtry(1,:),emtry(2,:),'.b','MarkerSi
ze',2);

47
 Specify the workspace

clc; clear all; close all; if (the2>=0)

l1 = 100; l2 = 200; l3 = 150; ii=ii+1;
tt=0;ii=0;jj=0; emtry1(:,ii)=[x,y];
for the1 = -90:90
end
for the2 = -90:90
for the3 =-90:90 if (the2<0)
if (the1 + the2 + the3) == 0 %% Điều kiện của khâu jj=jj+1;
cuối emtry2(:,jj)=[x,y];
tt=tt+1; end
x= end
l1*cosd(the1)+l2*cosd(the1+the2)+l3*cosd(the1+the2+the3); end
% động học thuận 1,
end
y=
l1*sind(the1)+l2*sind(the1+the2)+l3*sind(the1+the2+the3);%
end
động học thuận 2, figure(1);
emtry(:,tt) = [x;y]; plot(emtry(1,:),emtry(2,:),'.b','MarkerSize',2);
[the1_IK1,the2_IK1,the3_IK1]=IK1(x,y,0,l1,l2,l3); figure(2);
[the1_IK2,the2_IK2,the3_IK2]=IK2(x,y,0,l1,l2,l3); plot(emtry1(1,:),emtry1(2,:),'.b','MarkerSize',2);
figure(3);
plot(emtry2(1,:),emtry2(2,:),'.b','MarkerSize',2);

48
49
 VI. Kết quả
6.2 Kiểm chứng động học nghịch

Video kiểm chứng động học nghịch

50
 VI. Kết quả
6.3 Kiểm chứng quy hoạch quỹ đạo

Video quy hoạch quỹ đạo: LINE

51
 VI. Kết quả
6.3 Kiểm chứng quy hoạch quỹ đạo

Video quy hoạch quỹ đạo: TAM GIÁC

52
 VI. Kết quả
6.3 Kiểm chứng quy hoạch quỹ đạo

Video quy hoạch quỹ đạo: HÌNH CHỮ NHẬT

53
 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

Numeric solutions

55
 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)
 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 ) )
 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)
 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 
 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)
 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
 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]
 (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

 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
 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 
 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)
 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
 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

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

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

74
 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)
Thank you for your listening

78