Introduction to Robotics
MEC281
Inverse Kinematics
2-Link Planar Manipulator
Geometric (Graphical) method
2
• 𝑃2 = 𝑥𝑒 + 𝑦𝑒 2 (1)
• 𝜃2 = 180 − 𝛼 (2)
• 𝑃2 = 𝐿1 2 + 𝐿2 2 − 2𝐿1 𝐿2 cos 𝛼
−1 𝐿1 2 +𝐿2 2 −𝑃2
• 𝛼= cos (3)
2𝐿1 𝐿2
• 𝜃1 = γ − 𝛽 (4)
𝑦𝑒
• γ= tan−1 (5)
𝑥𝑒
• 𝐿2 2 = 𝐿1 + 𝑃2 − 2𝐿1 𝑃 cos 𝛽
2
−1 𝐿1 2 +𝑃2 −𝐿2 2
• 𝛽= cos (6)
2𝐿1 𝑃
2-Link Planar Manipulator
Geometric (Graphical) method
−1 𝐿1 2 +𝐿2 2 − 𝑥𝑒 2 +𝑦𝑒 2
• 𝜃2 = 180 − cos
2𝐿1 𝐿2
2
𝑦 𝐿1 2 + 𝑥𝑒 2 +𝑦𝑒 2 −𝐿2 2
• 𝜃1 = tan−1 𝑒 − cos −1
𝑥𝑒 2𝐿1 𝑥𝑒 2 +𝑦𝑒 2
Jacobians
(Velocity)
Jacobian
• Errors: effect of errors in the joint angles
• Determining singularities
• Solve the inverse kinematics
• Joints and End-effector Velocities
• Linearization at an operating point
Jacobian
• 𝑃𝑥𝑦𝑧 = 01𝑇𝑖 𝑃𝑢𝑣𝑤 (this formula isn’t correct)
For Puma Example
• 𝑃𝑥𝑦𝑧 = 01𝑇1 θ1 1 2 3 4 5
1𝑇2 θ2 1𝑇3 θ3 1𝑇4 θ4 1𝑇5 θ5 1𝑇6 θ6 𝑃𝑢𝑣𝑤
𝑥 𝑢
• 𝑦 = 01𝑇1 θ1 11𝑇2 θ2 21𝑇3 3 4 5
θ3 1𝑇4 θ4 1𝑇5 θ5 1𝑇6 θ6 𝑣
𝑧 𝑤
• 𝑥 = 𝐾1 (θ1 , … . . , θ6 )
• 𝑦 = 𝐾2 (θ1 , … . . , θ6 )
• 𝑧 = 𝐾3 (θ1 , … . . , θ6 )
Jacobian
• If we make small changes in the joint angles, we get
Taylor expansion (first-order approximation)
𝜕𝐾1 𝜕𝐾1 𝜕𝐾1
• δ𝑥 = δθ1 + δθ1 + ⋯ + δθ6
𝜕𝜃1 𝜕𝜃2 𝜕𝜃6
𝜕𝐾2 𝜕𝐾2 𝜕𝐾2
• δ𝑦 = δθ1 + δθ1 + ⋯ + δθ6
𝜕𝜃1 𝜕𝜃2 𝜕𝜃6
𝜕𝐾3 𝜕𝐾3 𝜕𝐾3
• δ𝑧 = δθ1 + δθ1 + ⋯ + δθ6
𝜕𝜃1 𝜕𝜃2 𝜕𝜃6
• Δ𝑥 = 𝐽Δθ
• Δθ = 𝐽−1 Δ𝑥
Jacobian
• Δ𝑥 = 𝐽Δθ
• J is the Jacobian matrix of partial derivatives
𝜕𝐾1 𝜕𝐾1 𝜕𝐾1
…
𝜕𝜃1 𝜕𝜃2 𝜕𝜃6
𝜕𝐾2 𝜕𝐾2 𝜕𝐾2
•𝐽= 𝜕𝜃1 𝜕𝜃2
…
𝜕𝜃6
𝜕𝐾3 𝜕𝐾3 𝜕𝐾3
…
𝜕𝜃1 𝜕𝜃2 𝜕𝜃6
3-Link Planar Manipulator
• 𝑥 = 𝑙1 cos θ1 + 𝑙2 cos θ1 + θ2 + 𝑙3 cos θ1 + θ2 + θ3
• 𝑦 = 𝑙1 sin θ1 + 𝑙2 sin θ1 + θ2 + 𝑙3 sin θ1 + θ2 + θ3
The Jacobian is
𝜕𝑥 𝜕𝑥 𝜕𝑥
𝜕𝜃1 𝜕𝜃2 𝜕𝜃3
𝐽=
𝜕𝑦 𝜕𝑦 𝜕𝑦
𝜕𝜃1 𝜕𝜃2 𝜕𝜃3
3-Link Planar Manipulator
• 𝑥 = 𝑙1 cos θ1 + 𝑙2 cos θ1 + θ2 + 𝑙3 cos θ1 + θ2 + θ3
• 𝑦 = 𝑙1 sin θ1 + 𝑙2 sin θ1 + θ2 + 𝑙3 sin θ1 + θ2 + θ3
𝜕𝑥
• = −𝑙1 sin θ1 − 𝑙2 sin θ1 + θ2 − 𝑙3 sin θ1 + θ2 + θ3
𝜕𝜃1
𝜕𝑥
• = −𝑙2 sin θ1 + θ2 − 𝑙3 sin θ1 + θ2 + θ3
𝜕𝜃2
𝜕𝑥
• = −𝑙3 sin θ1 + θ2 + θ3
𝜕𝜃3
𝜕𝑦
• = 𝑙1 cos θ1 + 𝑙2 cos θ1 + θ2 + 𝑙3 cos θ1 + θ2 + θ3
𝜕𝜃1
𝜕𝑦
• = 𝑙2 cos θ1 + θ2 + 𝑙3 cos θ1 + θ2 + θ3
𝜕𝜃2
𝜕𝑦
• = 𝑙3 cos θ1 + θ2 + θ3
𝜕𝜃3
Effect of Errors in The Joint Angles.
• Example 1 (3-Link Planar Manipulator)
Assume Home position is 𝜃1 = 𝜃2 = 𝜃3 =0
Find the errors in Δ𝑥 due to an error in θ2 by δθ2 = 0.1 radian
• Δ𝑥 = 𝐽Δθ
0 0 0
• 𝐽 0,0,0 =
𝑙1 + 𝑙2 + 𝑙3 𝑙2 + 𝑙3 𝑙3
δθ1 = 0
δ𝑥 0 0 0
• = δθ2 = 0.1
δ𝑦 𝑙1 + 𝑙2 + 𝑙3 𝑙2 + 𝑙3 𝑙3
δθ3 = 0
δ𝑥 0
• = 1
𝑙2 + 𝑙3
δ𝑦 10
3-Link Planar Manipulator
• Example 2 (3-Link Planar Manipulator)
Assume Home position is 𝜃1 = 𝜃3 = 0 & 𝜃2 = π/2
Find the errors in Δ𝑥 due to an error in θ2 by δθ2 = 0.1 radian
• Δ𝑥 = 𝐽Δθ
δθ1 = 0
δ𝑥 −𝑙2 − 𝑙3 −𝑙2 − 𝑙3 −𝑙3
• 𝐽 0, π/2,0 = = δθ2 = 0.1
δ𝑦 𝑙1 0 0
δθ3 = 0
1
δ𝑥 − 𝑙 + 𝑙3
• = 10 2
δ𝑦 0
Singularity
• We define Singularity as a point where the robot loses an
instantaneous DOF
• The robot’s end-effector can't move in one direction
• A point ϕ in the joint space of a robot is a singular point if and only if
the Jacobian J(ϕ) has less than maximal rank.
• If there are linear dependencies among the columns of the Jacobian
Singularity
• In example 1
δθ1 = 0
δ𝑥 0 0 0
• 𝐽 0,0,0 = = δθ2 = 0.1
δ𝑦 𝑙1 + 𝑙2 + 𝑙3 𝑙2 + 𝑙3 𝑙3
δθ3 = 0
• There is a row of zeros ---> zero determinant and rank of one
• Thus, the home position is singular
Singularity
• In example 2
δθ1 = 0
δ𝑥 −𝑙2 − 𝑙3 −𝑙2 − 𝑙3 −𝑙3
• 𝐽 0, π/2,0 = = δθ2 = 0.1
δ𝑦 𝑙1 0 0
δθ3 = 0
• is a non-zero determinant ---> the rank is two
• Thus, the point is non-singular
Determining singularities
• Det(J)=0
• The Jacobian of 3-R wrist
0 − sin θ1 cos θ1 sin θ2
• 𝐽 = 0 cos θ1 sin θ1 sin θ2
1 0 cos θ2
• det 𝐽 = 0 = − sin2 θ1 sin θ2 − cos2 θ1 sin θ2
• det 𝐽 = 0 = − sin θ2
Thus, singularity happens at θ2 = 0 or θ2 = 180°
Inverse of non-square matrix
Moore-Penrose Inverse or (Pseudoinverse)
Such that
• For A:mxn | m>n we have left inverse
• 𝐴−1
𝑙𝑒𝑓𝑡 = 𝐴𝑇 𝐴 −1 𝐴𝑇 ---------> 𝐴−1 𝐴 = 𝐼
𝑛
• For A:mxn | m<n we have right inverse
• 𝐴−1
𝑟𝑖𝑔ℎ𝑡 = 𝐴𝑇 𝐴𝐴𝑇 −1 ---------> 𝐴𝐴−1 = 𝐼
𝑚
• Sometime ill-condition occurs
• Inverse of a singular matrix -------> determinant=0
Numerical method
• One of the uses of the Jacobian is to solve the inverse
kinematics
• Newton-Raphson Method
𝑓 𝑥𝑖
• 𝑥 𝑖+1 = 𝑥 𝑖 − 𝑑
𝑑𝑥
𝑓 𝑥𝑖
Newton-Raphson Method
(Proof)
Numerical Method
f(θ)=0
• 𝑓1 = 𝐾1 θ1 , … . . , θ6 − 𝑥𝑑 = 0 𝑥𝑑 : desired position
• 𝑓2 = 𝐾2 θ1 , … . . , θ6 − 𝑦𝑑 = 0 𝐾1 : forward kinematics of end-effector
• 𝑓3 = 𝐾3 θ1 , … . . , θ6 − 𝑧𝑑 = 0
Taylor theorem
• f θ+ℎ ≈f θ +J θ ℎ h: is the error
We can approximate the error h to be
• ℎ ≈ 𝐽−1 θ f θ + ℎ
• θ is unknown, thus we can’t calculate 𝐽−1 θ
• So approximate 𝐽−1 θ to be 𝐽−1 θ + ℎ ((which is a guess))
Numerical Method
By setting
• ℎ𝑖 = θ𝑖 − θ𝑖+1
Then the Newton-Raphson formula become
• θ𝑖+1 = θ𝑖 − 𝐽−1 θ𝑖 f θ𝑖
• However, inverting matrices is very slow
Numerical Method
• θ𝑖+1 = θ𝑖 − 𝐽−1 θ𝑖 f θ𝑖
• θ𝑖+1 − θ𝑖 = −𝐽−1 θ𝑖 f θ𝑖
• −ℎ𝑖 = −𝐽−1 θ𝑖 f θ𝑖
• ℎ𝑖 = 𝐽−1 θ𝑖 f θ𝑖
• A quicker method is to solve the linear equation
• J θ𝑖 ℎ𝑖 = 𝑓 θ𝑖 using Gauss elimination
Newton-Raphson Method
(Solution Steps)
Numerical Method
1. Derive the end-effector position from forward kinematics ---> 𝑥 = , 𝑦 = , ∅ =
2. Derive Jacobian Matrix
Given Desired position xd & yd & ∅d
3. Derive f by subtracting the desire position from end-effector position (point 1-point 3)(f=x-xd)
Guess any initial angles 𝛉(𝟎) (preferred end-effector current position)
4. Calculate the Jacobian matrix (from point 2)
5. Calculate the function f (from point 3)
6. Calculate the error 𝒉 = 𝑱−𝟏 ∗ 𝐟
if f is not a small number (tends to 0) then ---> next iteration
7. 𝜽(𝟏) = 𝜽(𝟎) − 𝒉 𝟎
Newton-Raphson Method
(Example)
Numerical Method
• Consider 3-link planar manipulator
𝐿1 = 2 𝐿2 = 1 𝐿3 = 1
• current position at 𝜃1 = 𝜋/3 𝜃2 = 𝜋/6 𝜃3 = 𝜋/6
• Assume ∅ = 𝜃1 + 𝜃2 + 𝜃3
1) Find (the end-effector position from forward kinematics)
• 𝑥 = 𝑙1 cos θ1 + 𝑙2 cos θ1 + θ2 + 𝑙3 cos θ1 + θ2 + θ3
𝑥 = 2 cos 𝜋/3 + cos 𝜋/3 + 𝜋/6 + cos 𝜋/3 + 𝜋/6 + 𝜋/6 = 0.5
• 𝑦 = 𝑙1 sin θ1 + 𝑙2 sin θ1 + θ2 + 𝑙3 sin θ1 + θ2 + θ3
𝑦 = 2 sin 𝜋/3 + 𝑙2 sin 𝜋/3 + 𝜋/6 + 𝑙3 sin 𝜋/3 + 𝜋/6 + 𝜋/6 = 3.5981
• ∅ = 𝜃1 + 𝜃2 + 𝜃3
𝜋 𝜋 𝜋 𝜋
∅= + + =2
3 6 6 3
Numerical Method
𝜋
• 𝑥 = 0.5 y = 3.5981 ∅=2
3
Now suppose we want to move the end-effector to the position
𝜋
• 𝑥 = 0.5 y = 3 ∅=2
3
2) Then find the function f by subtract the desire position from end-effector position (xyz)
• 𝑓1 = 2 cos θ1 + cos θ1 + θ2 + cos θ1 + θ2 + θ3 − 0.5
• 𝑓2 = 2 sin θ1 + sin θ1 + θ2 + sin θ1 + θ2 + θ3 − 3
𝜋
• 𝑓3 = 𝜃1 + 𝜃2 + 𝜃3 − 2
3
3) Calculate the Jacobian matrix
𝜕𝑓1 𝜕𝑓1 𝜕𝑓1
𝜕𝜃1 𝜕𝜃2 𝜕𝜃3
𝜕𝑓2 𝜕𝑓2 𝜕𝑓2
• 𝐽 𝜃1 , 𝜃2 , 𝜃3 = 𝜕𝜃1 𝜕𝜃2 𝜕𝜃3
𝜕𝑓3 𝜕𝑓3 𝜕𝑓3
𝜕𝜃1 𝜕𝜃2 𝜕𝜃3
−2 sin θ1 − sin θ1 + θ2 − sin θ1 + θ2 + θ3 − sin θ1 + θ2 − sin θ1 + θ2 + θ3 − sin θ1 + θ2 + θ3
• 𝐽 𝜃1 , 𝜃2 , 𝜃3 = 2 cos θ1 + cos θ1 + θ2 + cos θ1 + θ2 + θ3 cos θ1 + θ2 + cos θ1 + θ2 + θ3 cos θ1 + θ2 + θ3
1 1 1
Numerical Method
−2 sin θ1 − sin θ1 + θ2 − sin θ1 + θ2 + θ3 − sin θ1 + θ2 − sin θ1 + θ2 + θ3 − sin θ1 + θ2 + θ3
• 𝐽 𝜃1 , 𝜃2 , 𝜃3 = 2 cos θ1 + cos θ1 + θ2 + cos θ1 + θ2 + θ3 cos θ1 + θ2 + cos θ1 + θ2 + θ3 cos θ1 + θ2 + θ3
1 1 1
4) Calculate the Jacobian matrix assuming a starting value of 𝜃 (preferred end-
effector current position)
5) Calculate the function f using the same starting value
(0) 𝜋 𝜋 𝜋 𝑇
• As our initial guess use the starting point 𝜃 = , ,
3 6 6
−3.5981 −1.866 −0.866 0
(0) (0)
• 𝐽 𝜃 = 0.5 −0.5 −0.5 ,𝑓 𝜃 = 0.5981
1 1 1 0
Numerical Method
6) Calculate the approximation error 𝒉(𝟎)
• We find the first approximation to the error by solving
(0)
−3.5981 −1.866 −0.866 ℎ1 0
(0)
• 0.5 −0.5 −0.5 ℎ2 = 0.5981
1 1 1 (0) 0
ℎ3
(0)
ℎ1 0.5981
• ℎ2(0) = −1.6340
(0) 1.0359
ℎ3
7) Find the next iteration 𝜃 (1) , 𝐣 𝜽 𝟏 , 𝑓 𝜃 (1) & 𝐡(1)
Numerical Method
• And thus
0.4491
• 𝜃 (1) = 𝜃 (0) − ℎ 0 = 2.1576 (Gauss Eliminating Method)
−0.5123
2.2441 −1.3758 −0.866
• 𝐽 𝜃 (1) = 0.4413 −1.4603 −0.5
1 1 1
−0.059
• 𝑓 𝜃 (1) = −0.756
0
−0.1606
• ℎ(1) = 0.5493
−0.3887 0.6097 0.0367
• And therefore 𝜃 (2) = 1.6083 ----> 𝑓 𝜃 (2) = −0.191
−0.1236 0
• The values of the function 𝑓 𝜃 (2) is getting closer to zero
Numerical Method
0.6789 0.0608
• 𝜃 (3) = 1.4106 ----> 𝑓 𝜃 (3) = −0.0096
0.0049 0.0000
0.6984 0.0001
• 𝜃 (3) = 1.4329 ----> 𝑓 𝜃 (3) = −0.0010
−0.0369 0.0000
• We can continue to any desired accuracy
Assignment
• Compare the numerical method with the closed form method
(graphical method)
Linear Velocities
• Δ𝑥 = 𝐽Δθ
• 𝑥ሶ = 𝐽θሶ
Trajectory Following
• Linear Approximation
• Polynomial Approximation
Linearization of non-linear Differential
Equations
• Taylor-based linearization at an operating point
• Jacobian
