Robotics 1

Inverse kinematics
Prof. Alessandro De Luca

Robotics 1 1
 Inverse kinematics
what are we looking for?

direct kinematics is always unique;

how about inverse kinematics for this 6R robot?
Robotics 1 2
 Inverse kinematics problem
n given a desired end-effector pose (position +
orientation), find the values of the joint variables 𝑞
that will realize it
n a synthesis problem, with input data in the form
𝑅 𝑝
§𝑇= ' = )𝐴+ (𝑞) § 𝑟 = 𝑓4 (𝑞), for a task function
0 1
classical formulation: generalized formulation:
inverse kinematics for a given end-effector pose 𝑇 inverse kinematics for a given value 𝑟 of task variables

n a typical nonlinear problem

n existence of a solution (workspace definition)
n uniqueness/multiplicity of solutions (𝑟 ∈ ℝ1 , 𝑞 ∈ ℝ+ )
n solution methods
Robotics 1 3
 Solvability and robot workspace
for tasks related to a desired end-effector Cartesian pose

n primary workspace 𝑊𝑆7 : set of all positions 𝑝 that can be

reached with at least one orientation (𝜙 or 𝑅 )
n out of WS1 there is no solution to the problem
n if 𝑝 ∈ 𝑊𝑆7, there is a suitable 𝜙 (or 𝑅) for which a solution exists

n secondary (or dexterous) workspace 𝑊𝑆9 : set of positions 𝑝

that can be reached with any orientation (among those
feasible for the robot direct kinematics)
n if 𝑝 ∈ 𝑊𝑆9, there exists a solution for any feasible 𝜙 (or 𝑅)

n 𝑊𝑆9 Í 𝑊𝑆7

Robotics 1 4
 Workspace of Fanuc R-2000i/165F
section for a
constant angle 𝑞7
𝑊𝑆7 ⊂ ℝ;
(≈ 𝑊𝑆9 for spherical wrist
without joint limits)

rotating the
base joint angle 𝑞7
Robotics 1 5
 Workspace of a planar 2R arm
2 orientations
𝑦 𝑝 = 𝑂𝑃 if 𝑝 Î int(𝑊𝑆7 )
•
𝑃
= 𝑊𝑆7 − 𝜕𝑊𝑆7
𝑙2
𝑞2
𝑙7 + 𝑙9 𝑂•
𝑙1
𝑞1 𝜕𝑊𝑆7
𝑙7 − 𝑙9

𝑂 𝑥 outer and inner

boundaries 1 orientation
n if 𝑙7 ¹ 𝑙9 on 𝜕𝑊𝑆7
n 𝑊𝑆7 = 𝑝 Î ℝ9: 𝑙7 − 𝑙9 ≤ 𝑝 ≤ 𝑙7 + 𝑙9 ⊂ ℝ9
n 𝑊𝑆9 = Æ
n if 𝑙7 = 𝑙9 = 𝑙
n 𝑊𝑆7 = 𝑝 Î ℝ9: 𝑝 ≤ 2𝑙 ⊂ ℝ9
n 𝑊𝑆9 = 𝑝 = 0 (all feasible orientations at the origin!... an infinite number)
Robotics 1 6
 Wrist position and E-E pose
inverse solutions for an articulated 6R robot
LEFT DOWN Unimation PUMA 560 RIGHT DOWN

4 inverse solutions
out of singularities
(for the position of
the wrist center only)

LEFT UP 8 inverse solutions considering RIGHT UP

the complete E-E pose
(spherical wrist: 2 alternative
solutions for the last 3 joints)

Robotics 1 7
 Counting/visualizing the 8 solutions
of the inverse kinematics for a Unimation Puma 560

RIGHT UP

RIGHT DOWN

LEFT UP

LEFT DOWN

Robotics 1 8
 Inverse kinematic solutions of UR10
6-dof Universal Robot UR10, with non-spherical wrist
video (slow motion)
desired pose
−0.2373
p = −0.0832 [m]
1.3224
3/2 0.5 0
R = −0.5 3/2 0
0 0 1
home configuration at start
𝑞= 0 −𝜋/2 0 −𝜋/2 T
0 0
[rad]

Robotics 1 9
 8 inverse kinematic solutions of UR10

shoulderRight shoulderRight shoulderRight shoulderRight

wristDown wristDown wristUp wristUp
elbowUp elbowDown elbowUp elbowDown
1.0472 1.0472 1.0472 1.0472
−1.2833 −1.9941 −1.5894 −2.0944
−0.7376 0.7376 −0.5236 0.5236
𝑞= 𝑞= 𝑞= 𝑞=
−2.6915 2.8273 0.5422 0
−1.5708 −1.5708 1.5708 1.5708
3.1416 3.1416 0 0

shoulderLeft shoulderLeft shoulderLeft shoulderLeft

wristDown wristDown wristUp wristUp
elbowDown elbowUp elbowDown elbowUp
2.7686 2.7686 2.7686 2.7686
−1.0472 −1.5522 −1.1475 −1.8583
−0.5236 0.5236 −0.7376 0.7376
𝑞= 𝑞= 𝑞= 𝑞=
3.1416 2.5994 0.3143 −0.4501
−1.5708 −1.5708 1.5708 1.5708
1.4202 1.4202 −1.7214 −1.7214

Robotics 1 10
 Multiplicity of solutions
few examples

n E-E positioning (𝑚 = 2) of a planar 2R robot

n 2 regular solutions in int(𝑊𝑆7)
n 1 solution on 𝜕𝑊𝑆7
singular solutions
n for 𝑙7 = 𝑙9: ∞ solutions in 𝑊𝑆9
n E-E positioning (𝑚 = 3) of an elbow-type spatial 3R robot
n 4 regular solutions in 𝑊𝑆7 (with singular cases yet to be investigated ...)
n spatial 6R robot arms
n £ 16 distinct solutions, out of singularities: this “upper bound” of
solutions was shown to be attained by a particular instance of
“orthogonal” robot, i.e., with twist angles 𝛼\ = 0 or ±𝜋/2 (∀𝑖 )
n analysis based on algebraic transformations of robot kinematics
n transcendental equations are transformed into a single polynomial
equation in one variable (number of roots = degree of the polynomial)
n seek for a transformed polynomial equation of the least possible degree
Robotics 1 11
 A 6R robot with 16 IK solutions
all distinct and non-singular

an orthogonal manipulator with DH table for the desired end-effector pose

)𝑇
b =

there are 16 real solutions

𝑎7 = 0.3, 𝑎9 = 1, 𝑎` = 1.5, 𝑑; = 0.2 of the inverse kinematics!

base all non-singular

with non-spherical wrist

end-effector

Manseur and Doty:

International Journal of Robotics Research, 1989

solutions found using a fast

numerical inversion algorithm …
Robotics 1 12
 Algebraic transformations
whiteboard …

start with some trigonometric equation in the joint angle 𝜃 to be solved …

𝑎 sin 𝜃 + 𝑏 cos 𝜃 = 𝑐 (✻)

introduce the algebraic transformation (… and the related inverse formulas)

𝑢 = tan 𝜃⁄2
2𝑢 1 − 𝑢9
⇒ sin 𝜃 = cos 𝜃 = (⇒ sin9 𝜃 + cos 9 𝜃 = 1)
1 + 𝑢9 1 + 𝑢9
2 tan 𝜃 ⁄2 2𝑢
tan 𝜃 = tan 2 𝜃 ⁄2 = = (using the duplication formula)
1 − tan9 𝜃 ⁄2 1 − 𝑢9
substituting in (✻)
polynomial equation of second degree in 𝑢
2𝑢 1 − 𝑢9
𝑎 9
+𝑏 9
=𝑐 ⇒ 𝑏 + 𝑐 𝑢 9 − 2𝑎 𝑢 − 𝑏 − 𝑐 = 0
1+𝑢 1+𝑢

𝑎 ± 𝑎9 + 𝑏9 − 𝑐 9
⇒ 𝑢7,9 = ⇒ 𝜃7,9 = 2 arctan 𝑢7,9
𝑏+𝑐
only if argument is real, else no solution
Robotics 1 13
 A planar 3R arm
workspace and number/type of inverse solutions
𝑦 𝑞; 𝑝 𝑙7 = 𝑙9 = 𝑙; = 𝑙 𝑛 = 3, 𝑚 = 2
𝑝𝑦 •
𝑙3 𝑊𝑆7 = 𝑝 ∈ ℝ9: 𝑝 ≤ 3𝑙 ⊂ ℝ9
𝑙2 𝑞9
𝑊𝑆9 = 𝑝 ∈ ℝ9: 𝑝 ≤ 𝑙 ⊂ ℝ9
𝑙1
any planar orientation is feasible in 𝑊𝑆9
𝑞7 𝑥
𝑝𝑥
1. in int 𝑊𝑆7 , except for case 3: ∞7 regular solutions,
at which the E-E can take a continuum of ∞
orientations (but not all orientations in the plane!)
2. if 𝑝 = 3𝑙 : only 1 solution, singular
3. if 𝑝 = 𝑙 : ∞7 solutions, 3 of which singular

4. if 𝑝 < 𝑙 : ∞7 regular solutions (that are never singular)

Robotics 1 14
 Workspace of a planar 3R arm
with generic link lengths

⇒ lmin= l3 = 0.3

Exercise #3 in
classroom test
of 21 Nov 2014

⇒ 0.5
Rin = 0,
Robotics 1 15
 Multiplicity of solutions
summary of the general cases

n if 𝑚 = 𝑛
n ∄ solutions

n a finite number of solutions (regular/generic case)

n “degenerate” solutions: infinite or finite set, but anyway
different in number from the generic case (singularity)
n if 𝑚 < 𝑛 (robot is kinematically redundant for the task)
n ∄ solutions

n ∞+r1 solutions (regular/generic case)

n a finite or infinite number of singular solutions
n use of the term singularity will become clearer when dealing
with differential kinematics
n instantaneous velocity mapping from joint to task velocity
n lack of full rank of the associated 𝑚×𝑛 Jacobian matrix 𝐽(𝑞)
Robotics 1 16
 Dexter 8R robot arm
n 𝑚 = 6 (position and orientation of E-E)
n 𝑛 = 8 (all revolute joints)
n ∞9 inverse kinematic solutions (redundancy degree = 𝑛 − 𝑚 = 2)

video

exploring inverse kinematic solutions by a robot self-motion

Robotics 1 17
 Solution methods

ANALYTICAL solution NUMERICAL solution

(in closed form) (in iterative form)

§ preferred, if it can be found* § certainly needed if 𝑛 > 𝑚

§ use ad-hoc geometric inspection (redundant case) or at/close to
§ algebraic methods (solution of singularities
polynomial equations) § slower, but easier to be set up
§ systematic ways for generating a § in its basic form, it uses the
reduced set of equations to be (analytical) Jacobian matrix of the
solved direct kinematics map
* sufficient conditions for 6-dof arms 𝜕𝑓4 (𝑞)
𝐽4 𝑞 =
• 3 consecutive rotational joint axes are 𝜕𝑞
incident (e.g., spherical wrist), or
§ Newton method, Gradient method,
• 3 consecutive rotational joint axes are
and so on…
parallel
D. Pieper, PhD thesis, Stanford University, 1968

Robotics 1 18
 Inverse kinematics of planar 2R arm
𝑦
•
𝑃 direct kinematics
𝑝𝑦
𝑙2 𝑝v = 𝑙7𝑐7 + 𝑙9𝑐79
𝑞2
𝑝w = 𝑙7𝑠7 + 𝑙9𝑠79
𝑙1
𝑞1 𝑥 data 𝑞7, 𝑞9 unknowns
𝑝𝑥
“squaring and summing” the equations of the direct kinematics
𝑝v9 + 𝑝w9 − 𝑙79 + 𝑙99 = 2𝑙7𝑙9 𝑐7𝑐79 + 𝑠7𝑠79 = 2𝑙7𝑙9𝑐9
and from this
𝑐9 = 𝑝v9 + 𝑝w9 − 𝑙79 + 𝑙99 y2𝑙7𝑙9, 𝑠9 = ± 1 − 𝑐99 𝑞9 = atan2 𝑠9 , 𝑐9

must be in [−1,1] (else, point 𝑃 2 solutions

in analytical form
is outside robot workspace!)
Robotics 1 19
 Inverse kinematics of 2R arm (cont’d)
𝑦
𝑝𝑦 𝑙2 • 𝑃 by geometric inspection
𝑞9 𝑞7 = 𝛼 − 𝛽
b

𝑙1
𝑞7 a
𝑝𝑥 𝑥
2 solutions
(one for each value of 𝑠2)
𝑞7 = atan2 𝑝w , 𝑝v − atan2 𝑙9 𝑠9 , 𝑙7 + 𝑙9 𝑐9
note: difference of atan2’s needs
to be re-expressed in (−𝜋 , 𝜋]!
𝑞9~~
• p
{𝑞7, 𝑞9}UP/LEFT 𝑞9~ {𝑞7, 𝑞9}DOWN/RIGHT

𝑞7~~ 𝑞9~ and 𝑞9~~ have same absolute

value, but opposite signs
𝑞7~
Robotics 1 20
 Algebraic solution for 𝑞7
another 𝑝v = 𝑙7𝑐7 + 𝑙9𝑐79 = 𝑙7𝑐7 + 𝑙9 𝑐7𝑐9 − 𝑠7𝑠9 linear in
solution
𝑠7 and 𝑐7
method… 𝑝w = 𝑙7𝑠7 + 𝑙9𝑠79 = 𝑙7𝑠7 + 𝑙9 𝑠7𝑐9 + 𝑐7𝑠9
𝑙7 + 𝑙9𝑐9 −𝑙9𝑠 9 𝑐7 𝑝v
𝑠7 = 𝑝w
𝑙9𝑠9 𝑙7 + 𝑙9𝑐9
except if 𝑙7 = 𝑙9 and 𝑐9 = −1
det = 𝑙79 + 𝑙99 + 2𝑙7𝑙9𝑐9 > 0 being then 𝑞7 undefined
(singular case: ∞7 solutions)
𝑞7 = atan2 𝑠7, 𝑐7
= atan2 𝑝w 𝑙7 + 𝑙9𝑐9 − 𝑝v 𝑙9𝑠9 ⁄det , 𝑝v 𝑙7 + 𝑙9𝑐9 + 𝑝w 𝑙9𝑠9 ⁄det
notes: a) this method provides directly the result in (−𝜋, 𝜋]
b) when evaluating atan2, det > 0 can be in fact eliminated
from the expressions of 𝑠7 and 𝑐7 (not changing the result)
Robotics 1 21
 Inverse kinematics of polar (RRP) arm
𝑝𝑧
note: here 𝑝v = 𝑞;𝑐9𝑐7
𝑞9 is NOT a 𝑞3 𝑃 direct 𝑝w = 𝑞;𝑐9𝑠7
DH variable! kinematics
𝑝‚ = 𝑑7 + 𝑞;𝑠9
𝑞2
𝑝v9 + 𝑝w9 + 𝑝‚ − 𝑑7 9
= 𝑞;9
𝑑1
𝑝𝑦
𝑞; = + 𝑝v9 + 𝑝w9 + 𝑝‚ − 𝑑7 9

𝑝𝑥 𝑞1 our choice: take here only the positive value...

if 𝑞; = 0, then 𝑞7 and 𝑞9 remain both undefined (stop); else

(if we stop, it is
𝑞9 = atan2 𝑝‚ − 𝑑7 ⁄𝑞; , ± 𝑝v9 + 𝑝w9y𝑞; a singular case:
∞9 or ∞7
if 𝑝v9 + 𝑝w9 = 0, then 𝑞7 remains undefined (stop); else solutions)
𝑞7 = atan2 𝑝w ⁄𝑐9 , 𝑝v ⁄𝑐9 (2 regular solutions 𝑞7 , 𝑞9 , 𝑞; )
Robotics 1 eliminating 𝑞; > 0 from both arguments 22
 Inverse kinematics of 3R elbow-type arm
𝑢, 𝑑 : elbow up, down
𝐿2
𝑞3
𝑞2
𝐿3
𝑃
𝑑1 𝑝𝑧
𝑓, 𝑏 : facing, backing
point 𝑝 = 𝑝v , 𝑝w , 𝑝‚ 𝑝𝑦

𝑝v 𝑞1
symmetric structure without offsets
e.g., first 3 joints of Mitsubishi PA10 robot

𝑊𝑆7 = {spherical shell centered at (0,0, 𝑑7 ),

4 regular inverse
with outer radius 𝑅„…† = 𝐿9 + 𝐿;
kinematics solutions in 𝑊𝑆7
and inner radius 𝑅\+ = 𝐿9 − 𝐿; }
more details (e.g., full handling of singular cases)
can be found in the solution of Exercise #1
in written exam of 11 Apr 2017

Robotics 1 23
 Inverse kinematics of 3R elbow-type arm
step 1

𝐿2
𝑞3 𝑝v = 𝑐7 𝐿9𝑐9 + 𝐿;𝑐9;
𝑞2 direct
𝐿3 𝑝w = 𝑠7 𝐿9𝑐9 + 𝐿;𝑐9;
kinematics
𝑑1 𝑝𝑧 𝑝‚ = 𝑑7 + 𝐿9𝑠9 + 𝐿;𝑠9;
𝑝𝑦

𝑝v 𝑞1

𝑝v9 + 𝑝w9 + 𝑝‚ − 𝑑7 9
= 𝑐79 𝐿9 𝑐9 + 𝐿; 𝑐9; 9
+ 𝑠79 𝐿9 𝑐9 + 𝐿; 𝑐9; 9
+ 𝐿9 𝑠9 + 𝐿; 𝑠9; 9

= ⋯ = 𝐿99 + 𝐿9; + 2𝐿9 𝐿; 𝑐9 𝑐9; + 𝑠9 𝑠9; = 𝐿99 + 𝐿9; + 2𝐿9 𝐿; 𝑐;

𝑐; = 𝑝v9 + 𝑝w9 + 𝑝‚ − 𝑑7 9
− 𝐿99 − 𝐿9; ⁄2𝐿9 𝐿; ∈ −1, +1 (else, 𝑝 is out of workspace!)

𝑞;ˆ = atan2 𝑠;, 𝑐;

± 𝑠; = ± 1 − 𝑐;9 two solutions
𝑞;r = atan2 −𝑠;, 𝑐; = − 𝑞;ˆ
Robotics 1 24
 Inverse kinematics of 3R elbow-type arm
step 2

𝐿2
𝑞3 𝑝v = 𝑐7 𝐿9𝑐9 + 𝐿;𝑐9;
𝑞2 direct
𝐿3 𝑝w = 𝑠7 𝐿9𝑐9 + 𝐿;𝑐9;
kinematics
𝑑1
𝑝‚ = 𝑑7 + 𝐿9𝑠9 + 𝐿;𝑠9;
𝑝𝑧
𝑝𝑦

𝑝v 𝑞1 … being 𝑝v9 + 𝑝w9 = 𝐿9 𝑐9 + 𝐿; 𝑐9; 9

>0

𝑐7 = 𝑝v y± 𝑝v9 + 𝑝w9
only when 𝑝v9 + 𝑝w9 >0…
(else 𝑞1 is undefined —infinite solutions!) 𝑠7 = 𝑝w y± 𝑝v9 + 𝑝w9

𝑞7ˆ = atan2 𝑝w , 𝑝v
again, two solutions
𝑞7r = atan2 −𝑝w , −𝑝v
Robotics 1 25
 Inverse kinematics of 3R elbow-type arm
step 3

combine first the two equations of direct

𝐿2
kinematics and rearrange the last one
𝑞3 𝑐7 𝑝v + 𝑠7 𝑝w = 𝐿9 𝑐9 + 𝐿; 𝑐9;
𝑞2
𝐿3 = 𝐿9 + 𝐿; 𝑐; 𝑐9 − 𝐿; 𝑠; 𝑠9
𝑑1 𝑝𝑧 𝑝‚ − 𝑑7 = 𝐿9 𝑠9 + 𝐿; 𝑠9;
𝑝𝑦 = 𝐿; 𝑠; 𝑐9 + 𝐿9 + 𝐿; 𝑐; 𝑠9
define and solve a linear system 𝐴𝑥 = 𝑏
𝑝v 𝑞1 in the algebraic unknowns 𝑥 = (𝑐9 , 𝑠9 )

𝐿9 + 𝐿; 𝑐; −𝐿; 𝑠;ˆ,r 𝑐9 ˆ,r ˆ,r 4 regular solutions for 𝑞9 ,

𝑐7 𝑝v + 𝑠7 𝑝w
ˆ,r 𝑠9 = depending on the combinations
𝐿; 𝑠; 𝐿9 + 𝐿; 𝑐; 𝑝‚ − 𝑑7
of +, − from 𝑞7 and 𝑞;
coefficient matrix 𝐴 known vector 𝑏
Š,‹ , …,Œ
provided det 𝐴 = 𝑝v9 + 𝑝w9 + 𝑝‚ − 𝑑7 9
≠0 𝑞9
(else 𝑞9 is undefined —infinite solutions!) = atan2 𝑠9
Š,‹ , …,Œ
, 𝑐9
Š,‹ , …,Œ

Robotics 1 26
 Inverse kinematics
for robots with spherical wrist
last 3 joints RRR, with
𝑧4 𝑦6
axes intersecting in 𝑊
j6 𝑊 𝑥6
first 3 robot joints 𝑂b = 𝑝
of any type (RRR, RRP, PPP, …)
𝑧3 𝑧5 𝑧6 = 𝑎

𝑧0 j5 𝑑6
j4
find 𝑞7, ⋯ , 𝑞b from the input data
𝑦0
§ 𝑝 (origin 𝑂b)
𝑥0 j1 § 𝑅 = 𝑛 𝑠 𝑎 (orientation of 𝑅𝐹b)
1. 𝑊 = 𝑝 − 𝑑b𝑎 ⇒ 𝑞7, 𝑞9, 𝑞; (inverse “position” kinematics for main axes)
)
2. 𝑅 = 𝑅;(𝑞7,𝑞9,𝑞;) ;𝑅b(𝑞`,𝑞Ž,𝑞b) ⇒ ;𝑅b 𝑞`,𝑞Ž,𝑞b = )𝑅;' 𝑅 ⇒ 𝑞`, 𝑞Ž, 𝑞b
Euler 𝑍𝑌𝑍 or 𝑍𝑋𝑍 (inverse “orientation”
given known, two regular kinematics for the wrist)
after step 1 rotation matrix with
solutions
Robotics 1 𝑞` ,𝑞Ž ,𝑞b (𝜃` ,𝜃Ž ,𝜃b ) 27
 6R robot Unimation PUMA 600

spherical 𝑛 = 0𝑥6(𝑞)
wrist

𝑠 = 0𝑦6(𝑞)

a function of
𝑞7 , 𝑞9 , 𝑞; only!
𝑎 = 0𝑧b (𝑞)

𝑝 = 𝑂6(𝑞)

8 different (regular) inverse solutions

here 𝑑b = 0, that can be found in closed form
Robotics 1 so that 𝑂b = 𝑊 directly 28
 Finding nice kinematic relations
whiteboard …

§ the most complex inverse kinematics that can be solved in principle in closed
form (i.e., analytically) is that of a 6R serial manipulator, with arbitrary DH table
§ ways to systematically generate equations from the direct kinematics that could be
easier to solve ⇒ some scalar equations may contain perhaps a single unknown variable!
) ) 7
𝑇b = 𝐴7 𝜃7 𝐴9 𝜃9 ⋯ Ž𝐴b 𝜃b = 𝑈)
method used for the
Unimation PUMA 600 in (*)
) r7 )
𝐴7 𝑇b = 𝑈7 (= 7𝐴9 ⋯ Ž𝐴b ) )
𝑇b Ž𝐴r7 ) `
b = 𝑉Ž (= 𝐴7 ⋯ 𝐴Ž )
7 r7 ) r7 )
𝐴9 𝐴7 𝑇b = 𝑈9 = 9𝐴; ⋯ Ž𝐴b or also ...
)
𝑇b Ž𝐴r7
b
` r7
𝐴Ž = 𝑉` = )𝐴7 ⋯ ;𝐴`
⋯ ⋯
` r7
𝐴Ž ⋯ 7𝐴r7
9
) r7 )
𝐴7 𝑇b = 𝑈Ž (= Ž𝐴b ) )
𝑇b 𝐴b 𝐴Ž ⋯ 7𝐴r7
Ž r7 ` r7 )
9 = 𝑉7 (= 𝐴7 )

(*) Paul, Shimano, and Mayer: IEEE Transactions on Systems, Man, and Cybernetics, 1981

§ generating from the direct kinematics a reduced set of equations to be solved (setting
w.l.o.g. 𝑑7 = 𝑑b = 0) ⇒ 4 compact scalar equations in the 4 unknowns 𝜃9 , … , 𝜃Ž

) 𝑛 𝑠 𝑎 𝑝 )
𝑎‚ = 𝑎 ' 𝜃 𝑧 𝑝 9 = 𝑝' 𝜃 𝑝(𝜃) solved analytically
𝑇b = = 𝐴b 𝜃
0 0 0 1 '
𝑝‚ = 𝑝 𝜃 𝑧 𝑝' 𝑎 = 𝑝' 𝜃 𝑎 𝜃 or numerically …

'
𝑧= 0 0 1 … then solve easily for the remaining 𝜃7 and 𝜃b

Manseur and Doty: International Journal of Robotics Research, 1988

Robotics 1 29
 Numerical solution of
inverse kinematics problems
n use when a closed-form solution 𝑞 to 𝑟Œ = 𝑓4 (𝑞) does not
exist or is “too hard” to be found
•Š– (—)
n all methods are iterative and need the matrix 𝐽4 𝑞 =
•—
(analytical Jacobian)
n Newton method (here only for 𝑚 = 𝑛, at the 𝑘th iteration)
¬ neglected
n 𝑟Œ = 𝑓4 𝑞 = 𝑓4 𝑞 ™ + 𝐽4 𝑞 ™ 𝑞 − 𝑞™ + 𝑜 𝑞 − 𝑞™ in Taylor expansion

𝑞™ˆ7 = 𝑞™ + 𝐽4r7(𝑞™ ) 𝑟Œ − 𝑓4 𝑞™
n convergence for 𝑞) (initial guess) close enough to some 𝑞∗: 𝑓4 (𝑞∗) = 𝑟Œ
n problems near singularities of the Jacobian matrix 𝐽4 𝑞
n in case of robot redundancy (𝑚 < 𝑛), use the pseudo-inverse 𝐽4#(𝑞)
n has quadratic convergence rate when near to a solution (fast!)

Robotics 1 30
 Operation of Newton method
n in the scalar case, also known as “method of the tangent”
n for a differentiable function 𝑓(𝑥), find a root 𝑥 ∗ of 𝑓 𝑥 ∗ = 0 by
iterating as
𝑓(𝑥™ )
𝑥™ˆ7 = 𝑥™ − ~
𝑓 𝑥™

an approximating sequence

𝑥7, 𝑥9, 𝑥;, 𝑥`, 𝑥Ž, ⋯ ⟶ 𝑥 ∗

function
tangent

animation from
http://en.wikipedia.org/wiki/File:NewtonIteration_Ani.gif

Robotics 1 31
 Numerical solution of
inverse kinematics problems (cont’d)
n Gradient method (max descent)
n minimize the error function
7 9 7 '
𝐻 𝑞 = 𝑟Œ − 𝑓4 (𝑞) = 𝑟Œ − 𝑓4 (𝑞) 𝑟Œ − 𝑓4 (𝑞)
9 9

𝑞™ˆ7 = 𝑞™ − 𝛼 ∇— 𝐻(𝑞™ )
from
'
' '
∇— 𝐻(𝑞) = 𝜕𝐻(𝑞)⁄𝜕𝑞 =− 𝑟Œ − 𝑓4 𝑞 𝜕𝑓4 (𝑞)⁄𝜕𝑞 = −𝐽4' (𝑞) 𝑟Œ − 𝑓4 (𝑞)
we get
𝑞™ˆ7 = 𝑞™ + 𝛼 𝐽4' (𝑞™ ) 𝑟Œ − 𝑓4 (𝑞™ )
n the scalar step size 𝛼 > 0 should be chosen so as to guarantee
a decrease of the error function at each iteration: too large
values for 𝛼 may lead the method to “miss” the minimum
n when the step size is too small, convergence is extremely slow
Robotics 1 32
 Revisited as a feedback scheme
𝑞(0)
𝑟Œ + 𝑒 𝑞̇ ó
𝑞 𝑟Œ = cost
𝐽4' (𝑞) õ
-
𝑟 (𝛼 = 1)
𝑓4 (𝑞)

𝑒 = 𝑟Œ − 𝑓4 𝑞 ® 0 ⟺ closed-loop equilibrium 𝑒 = 0
is asymptotically stable
7 '
𝑉= ³0
9
𝑒 𝑒 is a Lyapunov candidate function
𝑑
𝑉̇ = 𝑒 𝑒̇ = 𝑒
' '
𝑟Œ − 𝑓4 (𝑞) = −𝑒 ' 𝐽4 𝑞 𝑞̇ = −𝑒 ' 𝐽4 𝑞 𝐽4' 𝑞 𝑒 ≤ 0
𝑑𝑡
𝑉̇ = 0 ⟺ 𝑒 ∈ 𝒩 𝐽4' (𝑞) in particular, 𝑒 = 0

null space asymptotic stability

Robotics 1 33
 Properties of Gradient method
n computationally simpler: use the Jacobian transpose, rather
than its (pseudo)-inverse
n same use also for robots that are redundant (𝑛 > 𝑚) for the task
n may not converge to a solution, but it never diverges
n the discrete-time evolution of the continuous scheme
𝑞™ˆ7 = 𝑞™ + ∆𝑇 𝐽4' (𝑞™ ) 𝑟Œ − 𝑓4 (𝑞™ ) , 𝛼 = Δ𝑇
is equivalent to an iteration of the Gradient method
n the scheme can be accelerated by using a gain matrix 𝐾 > 0
𝑞̇ = 𝐽4' 𝑞 𝐾𝑒 = 𝐽4' 𝑞 𝐾 𝑟Œ − 𝑓4 (𝑞)
note: 𝐾 ⟶ 𝐾 + 𝐾¦ , with 𝐾¦ skew-symmetric, can be used also to “escape”
from being stuck in a stationary point of 𝑉 = §¨ 𝑒 ' 𝐾𝑒, by rotating the error
𝐾𝑒 out of the null space of 𝐽4' (when a singularity is encountered)
Robotics 1 34
 A case study
analytic expressions of Newton and gradient iterations

n 2R robot with 𝑙7 = 𝑙9 = 1, desired end-effector position 𝑟Œ = 𝑝Œ = (1,1)

n direct kinematic function and error
𝑐 + 𝑐79 1
𝑓4 𝑞 = 7 𝑒 = 𝑝Œ − 𝑓4 𝑞 = − 𝑓4 (𝑞)
𝑠7 + 𝑠79 1
n Jacobian matrix
𝜕𝑓4 (𝑞) − 𝑠7 + 𝑠79 −𝑠79
𝐽4 𝑞 = =
𝜕𝑞 𝑐7 + 𝑐79 𝑐79
n Newton versus Gradient iteration
𝐽4r7 (𝑞 ™ )
det 𝐽4 (𝑞)
1 𝑐79 𝑠79 𝑒™
𝑠9 − 𝑐7 + 𝑐79 − 𝑠7 + 𝑠79 |—¬— - 1 − 𝑐7 + 𝑐79
™ˆ7 ™
𝑞 =𝑞 + ×
− 𝑠7 + 𝑠79 𝑐7 + 𝑐79 1 − 𝑠7 + 𝑠79 |—¬— -
𝛼
−𝑠79 𝑐79 |—¬— -

𝐽4' (𝑞 ™ )
Robotics 1 35
 Error function
n 2R robot with 𝑙7 = 𝑙9 = 1 and desired end-effector position 𝑝Œ = (1,1)

𝑒 = 𝑝Œ − 𝑓4 (𝑞)

two local minima

plot of 𝑒 9 as a function of 𝑞 = (𝑞7, 𝑞9)
(inverse kinematic solutions)
Robotics 1 36
 Configuration space of 2R robot
whiteboard …

n can we represent the correct ‘‘distance’’ between two configurations 𝑞 ~ and 𝑞 ~~

of this robot on a (square) region in ℝ9 ?
𝑞9
𝜋
𝑞~ 𝑞~ 𝑞~
join the join the
two sides two sides
𝑞7 &
close or far? 𝑞7 = −𝜋 𝑞9 = −𝜋
~~ ~~ ~~
𝑞 𝑞 and 𝑞7 = 𝜋 𝑞 and 𝑞9 = 𝜋
−𝜋
−𝜋 𝜋
n configuration space is a torus 𝑆𝑂 1 × 𝑆𝑂 1 , i.e., the surface of a ‘‘donut’’
𝑞7

(0,0)

𝑞9

n the right metric is a geodesic on the torus …

Robotics 1 37
 Error reduction by Gradient method
n flow of iterations along the negative (or anti-) gradient
n two possible cases: convergence or stuck (at zero gradient)

start

one solution

.
local maximum
.
saddle point
(stop if this is the initial guess) (stop after some iterations)

another start...

...the other solution

(𝑞7 , 𝑞9 )~ = (0, 𝜋/2) (𝑞7 , 𝑞9 )~~ = (𝜋/2, −𝜋/2) (𝑞7 , 𝑞9 )1®v = (−3𝜋/4,0) (𝑞7 , 𝑞9 )¦®ŒŒ¯° = (𝜋/4,0)

Robotics 1 𝑒 ∈ 𝒩(𝐽4' (𝑞)) ! 38

 Convergence analysis
when does the gradient method get stuck?

n lack of convergence occurs when

n the Jacobian matrix 𝐽4 (𝑞) is not full rank (the robot is in a “singular configuration”)
n AND the error 𝑒 is in the null space of 𝐽4' (𝑞)
𝑝
1 𝑒 = 𝑝Œ − 𝑝 = 1 − 2
𝑝Œ = 𝑝Œ 1− 2
1
𝑞2 − 𝑠7 + 𝑠79 𝑐7 + 𝑐79
𝐽4' 𝑞 =
−𝑠79 𝑐79 |—¬—²³´´µ¶

𝑞1 𝑝Œ = − 2 2
− 2 2

(𝑞7 , 𝑞9 )¦®ŒŒ¯° = (𝜋/4,0) 𝑒 ∈ 𝒩(𝐽4' (𝑞))

𝑝Œ 𝑒 ∉ 𝒩(𝐽4' (𝑞)) !!
𝑒 ∈ 𝒩(𝐽4' (𝑞)) the algorithm will
proceed in this case,
moving out of
𝑝 the singularity
𝑝 (𝑞7 , 𝑞9 )1®v = (−3𝜋/4,0) (𝑞7 , 𝑞9 ) = (𝜋/9,0)
Robotics 1 39
 Issues in implementation
n initial guess 𝑞)
n only one inverse solution is generated for each guess
n multiple initializations for obtaining other solutions
n optimal step size 𝛼 > 0 in Gradient method
n a constant step may work good initially, but not close to the
solution (or vice versa)
n an adaptive one-dimensional line search (e.g., Armijo’s rule) could
be used to choose the best 𝛼 at each iteration
n stopping criteria
Cartesian error algorithm
(possibly, separate for 𝑟Œ − 𝑓4 𝑞 ™ ≤𝜀 𝑞 ™ˆ7 − 𝑞 ™ ≤ 𝜀—
increment
position and orientation)

n understanding closeness to singularities

™
good numerical conditioning
𝜎1\+ 𝐽4 𝑞 ≥ 𝜎) of Jacobian matrix (SVD)
(or a simpler test on its determinant, for 𝑚 = 𝑛 )
Robotics 1 40
 Numerical tests on RRP robot
n RRP/polar robot: desired E-E position 𝑟Œ = 𝑝Œ = 1, 1, 1
—see slide #22, with 𝑑7 = 0.5
n the two (known) analytical solutions, with 𝑞; ≥ 0, are
𝑞 ∗ = (0.7854, 0.3398, 1.5)
𝑞 ∗∗ = (𝑞7∗ − 𝜋, 𝜋 − 𝑞9∗ , 𝑞;∗ ) = (−2.3562, 2.8018, 1.5)
n norms 𝜀 = 10rŽ (max Cartesian error), 𝜀— = 10rb (min joint increment)
n 𝑘1®v = 15 (max # iterations), det 𝐽4 (𝑞) ≤ 10r` (singularity closeness)

n numerical performance of Gradient (with different steps 𝛼) vs. Newton

n test 1: 𝑞 ) = (0, 0, 1) as initial guess
n test 2: 𝑞 ) = (−𝜋/4, 𝜋/2, 1) — ‘‘singular” start, since 𝑐9 = 0 (see slide #22)
n test 3: 𝑞 ) = (0, 𝜋/2, 0) — ‘‘doubly singular” start, since also 𝑞; = 0
n solution and plots with MATLAB code

Robotics 1 41
 Numerical test - 1
n test 1: 𝑞 ) = (0, 0, 1) as initial guess; evolution of the error norm

Gradient: 𝛼 = 0.5 Gradient: 𝛼 = 1 Gradient: 𝛼 = 0.7

too large, oscillates

around solution good, converges
in 11 iterations
slow, 15 (max)
iterations

0.57⋅10-5
Newton
𝑒𝑥
Cartesian errors
component-wise
very fast, converges
in 5 iterations 𝑒𝑦

𝑒𝑧
Robotics 1 0.15⋅10-8 42
 Numerical test - 1
n test 1: 𝑞 ) = (0, 0, 1) as initial guess; evolution of joint variables

Gradient: 𝛼=1 Gradient: 𝛼 = 0.7 Newton

not converging converges in converges in
to a solution 11 iterations 5 iterations

both to the same solution 𝑞 ∗ = (0.7854, 0.3398, 1.5)

Robotics 1 43
 Numerical test - 2
n test 2: 𝑞 ) = (−𝜋/4, 𝜋/2, 1): singular start
with check of
Newton singularity:
blocked at start
error norms

without check:
it diverges!
Gradient
𝛼 = 0.7

!!
starts toward
solution, but
joint variables

slowly stops
(in singularity):
when Cartesian error
vector 𝑒 ∈ 𝒩 𝐽4' (𝑞)

Robotics 1 44
 Numerical test - 3
n test 3: 𝑞 ) = (−𝜋/4, 𝜋/2, 1): doubly singular start
Gradient (with 𝛼 = 0.7) Newton
① starts toward solution is either
error norm

② exits the double singularity blocked at start

③ slowly converges in 19 or (w/o check)
0.96⋅10-5 iterations to the solution explodes!
𝑞 ∗ = (0.7854, 0.3398, 1.5) ⇒ “NaN” in MATLAB

𝑒𝑥
Cartesian errors

joint variables
𝑒𝑦 ➂

➀
𝑒𝑧

Robotics 1 45
 Final remarks
n an efficient iterative scheme can be devised by combining
n initial iterations using Gradient (“sure but slow”, linear convergence rate)
n switch then to Newton method (quadratic terminal convergence rate)
n joint range limits are considered only at the end
n check if the solution found is feasible, as for analytical methods
n or, an optimization criterion and/or constraints included in the search
n drive iterations toward an inverse kinematic solution with nicer properties
n if the problem has to be solved on-line
n execute iterations and associate an actual robot motion: repeat steps at
times 𝑡) , 𝑡7 = 𝑡) + 𝑇 , ..., 𝑡™ = 𝑡™r7 + 𝑇 (e.g., every 𝑇 = 40 ms)
n a “good” choice for the initial guess 𝑞 ) at 𝑡™ is the solution of the previous
problem at 𝑡™r7 (provides continuity, requires only 1-2 Newton iterations)
n crossing of singularities and handling of joint range limits need special care
n Jacobian-based inversion schemes are used also for kinematic control,
moving along/tracking a continuous task trajectory 𝑟Œ (𝑡)
Robotics 1 46