Dr.

Omar Abdelaziz - Robotics– ME404

Differential Kinematics (Velocity Relationships ) Jacobian

Inverse Kinematics(Numerical method)

1
 Dr. Omar Abdelaziz - Robotics– ME404

0 −𝜔𝑧 𝜔𝑦
Differential Kinematics (Velocity Relationships ) Jacobian ෝ = 𝜔𝑧
𝑠𝑘𝑒𝑤 𝜔 ≡ 𝜔
−𝜔𝑦
0
𝜔𝑥
−𝜔𝑥
0

Inverse Kinematics(Numerical method)

𝑠𝑘𝑒𝑤 𝜔 = 𝑹ሶ 𝑅 −1 = 𝑹ሶ 𝑅 𝑇
Jacobian
∆𝑞1→𝑛
𝑣𝑥 Taking discrete
𝑱 𝑞1ሶ Taking discrete ∆𝑥 ∆𝑞1
𝑣𝑦 rate of change
𝑞2ሶ rate of change ∆𝑦 ∆𝑞2
𝑣 𝐽𝑝
𝑉 = 𝜔𝑧 = 𝑠𝑘𝑒𝑤 𝜔 = ∆𝑹 𝑅𝑇
𝑥 𝐽𝑟
𝑞3ሶ 𝑉 ≡ ∆𝑋 = ∆𝑧 = 𝑱 ∆𝑞3
⋮ ?
𝜔𝑦 ⋮
𝑞𝑛ሶ ? 𝜔 = 𝑣𝑒𝑐(∆𝑹 𝑅𝑇 )
𝜔𝑧 ∆𝑞𝑛
?

2
 Dr. Omar Abdelaziz - Robotics– ME404

0 −𝜔𝑧 𝜔𝑦
Differential Kinematics (Velocity Relationships ) Jacobian ෝ = 𝜔𝑧
𝑠𝑘𝑒𝑤 𝜔 ≡ 𝜔
−𝜔𝑦
0
𝜔𝑥
−𝜔𝑥
0

Inverse Kinematics(Numerical method)

𝑠𝑘𝑒𝑤 𝜔 = 𝑹ሶ 𝑅 −1 = 𝑹ሶ 𝑅 𝑇
Jacobian
𝑣𝑥 Taking discrete
𝑞1ሶ Taking discrete
𝑣𝑦 ∆𝑥 rate of change
𝑞2ሶ rate of change
𝑣 𝐽𝑝 ∆𝑦
𝑉 = 𝜔𝑧 = 𝑞3ሶ 𝑉 ≡ ∆𝑋 = = 𝑱∆𝑞1→𝑛 𝑠𝑘𝑒𝑤 𝜔 = ∆𝑹 𝑅𝑇
𝑥 𝐽𝑟 ∆𝑧
𝜔𝑦 ⋮ 𝑣𝑒𝑐 ∆𝑹 𝑅𝑇
𝜔𝑧 𝑞𝑛ሶ 𝜔 = 𝑣𝑒𝑐(∆𝑹 𝑅𝑇 )

For solving IK →
given are desired position 𝑃𝑑 = 𝑥𝑑 𝑦𝑑 𝑧𝑑 𝑇 , and desired orientation matrix 𝑹𝒅 ,
Known are current position 𝑃0 = 𝑥0 𝑦0 𝑧0 𝑇 , and current orientation matrix 𝑹𝟎 , and current joint angles 𝒒𝟎
Required: desired joint angles 𝒒𝒅
Solution By taking ∆ as the change from current to desired pose
𝑥𝑑 − 𝑥0
∆𝑥 = 𝑥𝑑 − 𝑥0 ∆𝑹 = 𝑹𝒅 − 𝑹𝟎 𝑦𝑑 − 𝑦0
∆𝑦 = 𝑦𝑑 − 𝑦0 ∆𝑞 = 𝒒𝒅 − 𝒒𝟎 𝑧𝑑 − 𝑧0 = 𝑱 𝒒𝒅 − 𝒒𝟎
𝑣𝑒𝑐 𝑹𝒅 − 𝑹𝟎 𝑹𝟎 𝑇 3
∆𝑧 = 𝑧𝑑 − 𝑧0
 Dr. Omar Abdelaziz - Robotics– ME404

0 −𝜔𝑧 𝜔𝑦
Differential Kinematics (Velocity Relationships ) Jacobian ෝ = 𝜔𝑧
𝑠𝑘𝑒𝑤 𝜔 ≡ 𝜔
−𝜔𝑦
0
𝜔𝑥
−𝜔𝑥
0

Inverse Kinematics(Numerical method)

𝑠𝑘𝑒𝑤 𝜔 = 𝑹ሶ 𝑅 −1 = 𝑹ሶ 𝑅 𝑇
Jacobian
𝑣𝑥 Taking discrete
𝑞1ሶ Taking discrete
𝑣𝑦 ∆𝑥 rate of change
𝑞2ሶ rate of change
𝑣 𝐽𝑝 ∆𝑦
𝑉 = 𝜔𝑧 = 𝑞3ሶ 𝑉 ≡ ∆𝑋 = = 𝑱∆𝑞1→𝑛 𝑠𝑘𝑒𝑤 𝜔 = ∆𝑹 𝑅𝑇
𝑥 𝐽𝑟 ∆𝑧
𝜔𝑦 ⋮ 𝑣𝑒𝑐 ∆𝑹 𝑅𝑇
𝜔𝑧 𝑞𝑛ሶ 𝜔 = 𝑣𝑒𝑐(∆𝑹 𝑅𝑇 )

For solving IK →
given are desired position 𝑃𝑑 = 𝑥𝑑 𝑦𝑑 𝑧𝑑 𝑇 , and desired orientation matrix 𝑹𝒅 ,
Known are current position 𝑃0 = 𝑥0 𝑦0 𝑧0 𝑇 , and current orientation matrix 𝑹𝟎 , and current joint angles 𝒒𝟎
Required: desired joint angles 𝒒𝒅
Solution By taking ∆ as the change from current to desired pose 𝑹𝒅 𝑹𝟎 𝑻 − 𝑰
𝑥𝑑 − 𝑥0
∆𝑥 = 𝑥𝑑 − 𝑥0 ∆𝑹 = 𝑹𝒅 − 𝑹𝟎 𝑦𝑑 − 𝑦0
∆𝑦 = 𝑦𝑑 − 𝑦0 ∆𝑞 = 𝒒𝒅 − 𝒒𝟎 𝑧𝑑 − 𝑧0 = 𝑱 𝒒𝒅 − 𝒒𝟎
𝑣𝑒𝑐 𝑹𝒅 − 𝑹𝟎 𝑹𝟎 𝑇
∆𝑧 = 𝑧𝑑 − 𝑧0
 Dr. Omar Abdelaziz - Robotics– ME404

0 −𝜔𝑧 𝜔𝑦
Differential Kinematics (Velocity Relationships ) Jacobian ෝ = 𝜔𝑧
𝑠𝑘𝑒𝑤 𝜔 ≡ 𝜔
−𝜔𝑦
0
𝜔𝑥
−𝜔𝑥
0

Inverse Kinematics(Numerical method)

𝑠𝑘𝑒𝑤 𝜔 = 𝑹ሶ 𝑅 −1 = 𝑹ሶ 𝑅 𝑇
Jacobian
𝑣𝑥 Taking discrete
𝑞1ሶ Taking discrete
𝑣𝑦 ∆𝑥 rate of change
𝑞2ሶ rate of change
𝑣 𝐽𝑝 ∆𝑦
𝑉 = 𝜔𝑧 = 𝑞3ሶ 𝑉 ≡ ∆𝑋 = = 𝑱∆𝑞1→𝑛 𝑠𝑘𝑒𝑤 𝜔 = ∆𝑹 𝑅𝑇
𝑥 𝐽𝑟 ∆𝑧
𝜔𝑦 ⋮ 𝑣𝑒𝑐 ∆𝑹 𝑅𝑇
𝜔𝑧 𝑞𝑛ሶ 𝜔 = 𝑣𝑒𝑐(∆𝑹 𝑅𝑇 )

For solving IK →
given are desired position 𝑃𝑑 = 𝑥𝑑 𝑦𝑑 𝑧𝑑 𝑇 , and desired orientation matrix 𝑹𝒅 ,
Known are current position 𝑃0 = 𝑥0 𝑦0 𝑧0 𝑇 , and current orientation matrix 𝑹𝟎 , and current joint angles 𝒒𝟎
Required: desired joint angles 𝒒𝒅 pseudo
Solution By taking ∆ as the change from current to desired pose inverse
𝑥𝑑 − 𝑥0 Multiply both sides by 𝑥𝑑 − 𝑥0
∆𝑥 = 𝑥𝑑 − 𝑥0 ∆𝑹 = 𝑹𝒅 − 𝑹𝟎 𝑱+ and Rearrange equ.
𝑦𝑑 − 𝑦0 𝑦𝑑 − 𝑦0
𝑧𝑑 − 𝑧0 = 𝑱 𝒒𝒅 − 𝒒𝟎 𝒒𝒅 = 𝒒𝟎 + 𝑱+ 𝑧𝑑 − 𝑧0
∆𝑦 = 𝑦𝑑 − 𝑦0 ∆𝑞 = 𝒒𝒅 − 𝒒𝟎
𝑣𝑒𝑐 𝑹𝒅 𝑹𝟎 𝑻 − 𝑰 𝑣𝑒𝑐 𝑹𝒅 𝑹𝟎 𝑻 − 𝑰
5
∆𝑧 = 𝑧𝑑 − 𝑧0
 Dr. Omar Abdelaziz - Robotics– ME404

Differential Kinematics (Velocity Relationships ) Jacobian

Inverse Kinematics(Numerical method) Set initial joints values 𝒒𝟎 , get
Implementation 𝑥0 , 𝑦0 , 𝑧0 , 𝑹𝟎 from FK model

𝑇 𝑥0 , 𝑦0 , 𝑧0 , 𝑹𝟎 , 𝒒𝟎
given are desired position 𝑃𝑑 = 𝑥𝑑 𝑦𝑑 𝑧𝑑
, and desired orientation matrix 𝑹𝒅 ,
calculate Jacobian matrix 𝑱 and ∆𝑋

pseudo 𝒒𝒅 = 𝒒𝟎 + 𝑱+ ∆𝑋
inverse 𝑥𝑑 − 𝑥0
𝑦𝑑 − 𝑦0 𝑒: should be very
+ small close to zero
𝒒𝒅 = 𝒒𝟎 + 𝑱 𝑧𝑑 − 𝑧0 No
Set 𝒒𝟎 = 𝒒𝒅
𝑣𝑒𝑐 𝑹𝒅 𝑹𝟎 𝑻 − 𝑰 Then , get 𝑥0 , 𝑦0 , 𝑧0 , 𝑹𝟎 If ( ∆𝑋 < 𝑒) ∆𝑋 : norm
from FK using new 𝒒𝟎

yes

∆𝑋 System converge ;
Optimum values = 𝒒𝒅

6
 Dr. Omar Abdelaziz - Robotics– ME404

Differential Kinematics (Velocity Relationships ) Jacobian

Inverse Kinematics(Numerical method) Set initial joints values 𝒒𝟎 , get
Implementation 𝑥0 , 𝑦0 , 𝑧0 , 𝑹𝟎 from FK model

𝑇 𝑥0 , 𝑦0 , 𝑧0 , 𝑹𝟎 , 𝒒𝟎
given are desired position 𝑃𝑑 = 𝑥𝑑 𝑦𝑑 𝑧𝑑
, and desired orientation matrix 𝑹𝒅 ,
calculate Jacobian matrix 𝑱 and ∆𝑋
This numerical method is known as
∆𝑋 Newton Raphson iterative method
pseudo 𝒒𝒅 = 𝒒𝟎 + 𝑱+ ∆𝑋
inverse 𝑥𝑑 − 𝑥0
𝑦𝑑 − 𝑦0 𝑒: should be very
+ small close to zero
𝒒𝒅 = 𝒒𝟎 + 𝑱 𝑧𝑑 − 𝑧0 No
Set 𝒒𝟎 = 𝒒𝒅
𝑣𝑒𝑐 𝑹𝒅 𝑹𝟎 𝑻 − 𝑰 Then , get 𝑥0 , 𝑦0 , 𝑧0 , 𝑹𝟎 If ( ∆𝑋 < 𝑒) ∆𝑋 : norm
from FK using new 𝒒𝟎

yes
∆𝑥
∆𝑦 System converge ;
∆𝑧 Optimum values = 𝒒𝒅
𝒒𝒅 = 𝒒𝟎 + 𝑱+ ∆𝜃𝑥
∆𝜃𝑦
∆𝜃𝑧 norm ∶ ∆𝑋 = ∆𝑥 2 + ∆𝑦 2 + ∆𝑧 2 + ∆𝜃𝑥 2 + ∆𝜃𝑦 2 + ∆𝜃𝑧 2
7
 Dr. Omar Abdelaziz - Robotics– ME404

Differential Kinematics (Velocity Relationships ) Jacobian

Inverse Kinematics(Numerical method)
Implementation
pseudo
𝑇 inverse 𝑥𝑑 − 𝑥0
given are desired position 𝑃𝑑 = 𝑥𝑑 𝑦𝑑 𝑧𝑑
𝑦𝑑 − 𝑦0
, and desired orientation matrix 𝑹𝒅 , 𝒒𝒅 = 𝒒𝟎 + 𝑱+ 𝑧𝑑 − 𝑧0
𝑣𝑒𝑐 𝑹𝒅 𝑹𝟎 𝑻 − 𝑰

Adding learning rate (convergence rate)𝜶

∆𝑋
𝒒𝒅 = 𝒒𝟎 + 𝜶 𝑱+ ∆𝑋

8
 Dr. Omar Abdelaziz - Robotics– ME404

Differential Kinematics (Velocity Relationships ) Jacobian

Inverse Kinematics(Numerical method)
Alternative methods to pseudo inverse

• pseudo inverse( 𝑱+ ) Note : 𝑱+ ≡ 𝑱𝑇 (𝐽𝑱𝑇 )−1

𝒒𝒅 = 𝒒𝟎 + 𝑱+ ∆𝑋
often performs poorly because of instability near singularities.

+
• Weighted pseudo inverse (𝐽𝑊 )

• Damping least square (𝐽𝐷𝐿𝑆 )

• Selective Damping least square (𝐽𝑆𝐷𝐿𝑆 )

9
 Dr. Omar Abdelaziz - Robotics– ME404

Differential Kinematics (Velocity Relationships ) Jacobian

Inverse Kinematics(Numerical method)
Alternative methods to pseudo inverse

• Damping least square (𝐽𝐷𝐿𝑆 )

𝜇: is the damping factor.
𝐽𝐷𝐿𝑆 ≡ 𝑱𝑇 (𝐽𝑱𝑇 + 𝜇2 Ι6 )−1
- is (6 × 6)diagonal matrix . For each of the 6 task space directions ∆𝑋
(∆𝑥, ∆𝑦, ∆𝑧, 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ).
- Is single value if the damping factor is same for all the task space
direction ∆𝑋
Ι6 : is (6 × 6)Identity matrix.

IK is calculated as follow

𝒒𝒅 = 𝒒𝟎 + 𝐽𝐷𝐿𝑆 ∆𝑋

The damped least squares method most commonly used for IK to avoids many of the
pseudoinverse method’s problems with singularities and can give a numerically stable method.
(It is also called the Levenberg-Marquardt method)
10