Robotics 1

Inverse kinematicsProf. Alessandro De Luca

Robotics 1 1

Inverse kinematicswhat are we looking for?

Robotics 1 2

direct kinematics is always unique;how about inverse kinematics for this 6R robot?

°

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§ 𝑇 = 𝑅 𝑝

0' 1= )𝐴+(𝑞)

n a typical nonlinear problemn existence of a solution (workspace definition)n uniqueness/multiplicity of solutions (𝑟 ∈ ℝ1, 𝑞 ∈ ℝ+)n solution methods

§ 𝑟 = 𝑓4(𝑞), for a task function

Robotics 1 3

classical formulation:inverse kinematics for a given end-effector pose 𝑇

generalized formulation:inverse kinematics for a given value 𝑟 of task variables

Solvability and robot workspacefor 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 problemn if 𝑝 ∈ 𝑊𝑆7, there is a suitable 𝜙 (or 𝑅) for which a solution exists

n secondary (or dexterous) workspace 𝑊𝑆9: set of positions p 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

𝑊𝑆7 ⊂ ℝ;(≈ 𝑊𝑆9 for spherical wrist

without joint limits)

Robotics 1 5

section for aconstant angle 𝑞7

rotating the base joint angle 𝑞7

Workspace of a planar 2R arm

n if 𝑙7 ¹ 𝑙9n 𝑊𝑆7 = 𝑝 Î ℝ9: 𝑙7 − 𝑙9 ≤ 𝑝 ≤ 𝑙7 + 𝑙9 ⊂ ℝ9

n 𝑊𝑆9 = Æ

n if 𝑙7 = 𝑙9 = 𝑙n 𝑊𝑆7 = 𝑝 Î ℝ9: 𝑝 ≤ 2𝑙 ⊂ ℝ9n 𝑊𝑆9 = 𝑝 = 0 (all feasible orientations at the origin!... an infinite number)

Robotics 1 6

𝑥𝑞1

𝑞2

𝑃•

𝑙1

𝑙2

𝑦

𝑙7 + 𝑙9

𝑙7 − 𝑙9𝜕𝑊𝑆7outer and inner

boundaries

2 orientationsif 𝑝 Î int(𝑊𝑆7)= 𝑊𝑆7 − 𝜕𝑊𝑆7

1 orientationon 𝑊𝑆7

𝑂

𝑂 •

𝑝 = 𝑂𝑃

Wrist position and E-E poseinverse solutions for an articulated 6R robot

LEFT DOWN RIGHT DOWN

LEFT UP RIGHT UP

4 inverse solutionsout of singularities(for the position of

the wrist center only)

8 inverse solutions consideringthe complete E-E pose

(spherical wrist: 2 alternative solutions for the last 3 joints)

Unimation PUMA 560

Robotics 1 7

Counting/visualizing the 8 solutionsof the inverse kinematics for a Unimation Puma 560

Robotics 1 8

LEFT DOWN

LEFT UP

RIGHT DOWN

RIGHT UP

Inverse kinematic solutions of UR106-dof Universal Robot UR10, with non-spherical wrist

Robotics 1 9

video (slow motion)

desired pose

home configuration at start

[rad]

[m]p=−0.2373−0.08321.3224

R =3/2 0.5 0−0.5 3/2 00 0 1

𝑞 = 0 −𝜋/2 0 −𝜋/2 0 0 T

8 inverse kinematic solutions of UR10

Robotics 1 10

shoulderRightwristDownelbowUp

shoulderRightwristDown

elbowDown

shoulderRightwristUp

elbowUp

shoulderRightwristUp

elbowDown

shoulderLeftwristDown

elbowDown

shoulderLeftwristDownelbowUp

shoulderLeftwristUp

elbowDown

shoulderLeftwristUp

elbowUp

𝑞 =

1.0472−1.2833−0.7376−2.6915−1.57083.1416

𝑞 =

1.0472−1.99410.73762.8273−1.57083.1416

𝑞 =

1.0472−1.5894−0.52360.54221.57080

𝑞 =

1.0472−2.09440.52360

1.57080

𝑞 =

2.7686−1.85830.7376−0.45011.5708−1.7214

𝑞 =

2.7686−1.1475−0.73760.31431.5708−1.7214

𝑞 =

2.7686−1.55220.52362.5994−1.57081.4202

𝑞 =

2.7686−1.0472−0.52363.1416−1.57081.4202

Multiplicity of solutionsfew examples

n E-E positioning (𝑚 = 2) of a planar 2R robotn 2 regular solutions in int(𝑊𝑆7)n 1 solution on 𝜕𝑊𝑆7n for 𝑙7 = 𝑙9: ∞ solutions in 𝑊𝑆9

n E-E positioning (𝑚 = 3) of an elbow-type spatial 3R robotn 4 regular solutions in 𝑊𝑆7 (with singular cases yet to be investigated ...)

n spatial 6R robot armsn £ 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 kinematicsn 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

singular solutions

Robotics 1 11

Algebraic transformationswhiteboard …

Robotics 1 12

start with some trigonometric equation in the joint angle 𝜃 to be solved …𝑎 sin 𝜃 + 𝑏 cos 𝜃 = 𝑐

introduce the algebraic transformation𝑢 = tan ⁄𝜃 2

cos 𝜃 =1 − 𝑢9

1 + 𝑢9sin 𝜃 =

2𝑢1 + 𝑢9 (⇒ sin9 𝜃 + cos9 𝜃 = 1)

tan 𝜃 = tan 2 ⁄𝜃 2 =2 tan ⁄𝜃 2

1 − tan9 ⁄𝜃 2 =2𝑢

1 − 𝑢9(using the duplication formula)

substituting in (✻)

(✻)

𝑎2𝑢

1 + 𝑢9 + 𝑏1 − 𝑢9

1 + 𝑢9 = 𝑐

⇒

⇒ 𝑏 + 𝑐 𝑢9− 2𝑎 𝑢 − 𝑏 − 𝑐 = 0

⇒ 𝑢7,9 =𝑎 ± 𝑎9 + 𝑏9 − 𝑐9

𝑏 + 𝑐 ⇒ 𝜃7,9 = 2 arctan 𝑢7,9only if argument is real, else no solution

polynomial equation of second degree in 𝑢

(… and the related inverse formulas)

1. in int 𝑊𝑆7 : ∞7 regular (except for 3.) 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, singular3. if 𝑝 = 𝑙 :∞7 solutions, 3 of which singular

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

A planar 3R armworkspace and number/type of inverse solutions

𝑙7 = 𝑙9 = 𝑙; = 𝑙

any planar orientation is feasible in 𝑊𝑆9

Robotics 1 13

𝑦

𝑥𝑞7

𝑞9

𝑙1

𝑙2𝑙3

𝑞; 𝑝•

𝑝𝑥

𝑝𝑦𝑛 = 3,𝑚 = 2

𝑊𝑆7 = 𝑝 ∈ ℝ9: 𝑝 ≤ 3𝑙 ⊂ ℝ9

𝑊𝑆9 = 𝑝 ∈ ℝ9: 𝑝 ≤ 𝑙 ⊂ ℝ9

Workspace of a planar 3R armwith generic link lengths

Robotics 1 14

lmin= l3 = 0.3⇒

⇒

Rin = 0,

0.5

Exercise #3 in classroom testof 21 Nov 2014

Multiplicity of solutionssummary of the general cases

n if 𝑚 = 𝑛n ∄ solutionsn 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 ∄ solutionsn ∞+o1 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 velocityn lack of full rank of the associated 𝑚×𝑛 Jacobian matrix 𝐽(𝑞)

Robotics 1 15

Dexter 8R robot armn 𝑚 = 6 (position and orientation of E-E)n 𝑛 = 8 (all revolute joints)n ∞9 inverse kinematic solutions (redundancy degree = 𝑛 −𝑚 = 2)

exploring inverse kinematic solutions by a robot self-motion

video

Robotics 1 16

Solution methods

ANALYTICAL solution(in closed form)

NUMERICAL solution(in iterative form)

§ preferred, if it can be found*

§ use ad-hoc geometric inspection§ algebraic methods (solution of

polynomial equations)§ systematic ways for generating a

reduced set of equations to be solved

* sufficient conditions for 6-dof arms• 3 consecutive rotational joint axes are

incident (e.g., spherical wrist), or• 3 consecutive rotational joint axes are

parallel

§ certainly needed if 𝑛 > 𝑚(redundant case) or at/close to singularities

§ slower, but easier to be set up§ in its basic form, it uses the

(analytical) Jacobian matrix of the direct kinematics map

§ Newton method, Gradient method, and so on…

Robotics 1 17D. Pieper, PhD thesis, Stanford University, 1968

𝐽4 𝑞 =𝜕𝑓4(𝑞)𝜕𝑞

Inverse kinematics of planar 2R arm

direct kinematics

𝑝s = 𝑙7𝑐7 + 𝑙9𝑐79

𝑝t = 𝑙7𝑠7 + 𝑙9𝑠79

data 𝑞7, 𝑞9 unknowns

“squaring and summing” the equations of the direct kinematics𝑝s9 + 𝑝t9 − 𝑙79 + 𝑙99 = 2𝑙7𝑙9 𝑐7𝑐79 + 𝑠7𝑠79 = 2𝑙7𝑙9𝑐9

and from this𝑐9 = v𝑝s9 + 𝑝t9 − 𝑙79 + 𝑙99 2𝑙7𝑙9, 𝑠9 = ± 1 − 𝑐99

2 solutions in analytical formmust be in [−1,1] (else, point 𝑃is outside robot workspace!)

Robotics 1 18

𝑞1

𝑞2

𝑃•

𝑙1

𝑙2

𝑦

𝑥𝑝𝑥

𝑝𝑦

𝑞9 = atan2 𝑠9, 𝑐9

Inverse kinematics of 2R arm (cont’d)

𝑞7 = atan2 𝑝t, 𝑝s − atan2 𝑙9𝑠9, 𝑙7 + 𝑙9𝑐9

a

b

by geometric inspection𝑞7 = 𝛼 − 𝛽

2 solutions (one for each value of 𝑠2)

note: difference of atan2’s needsto be re-expressed in (−𝜋 , 𝜋]!

𝑞9{ and 𝑞9{{ have same absolute value, but opposite signs

Robotics 1 19

• p

𝑞7{

𝑞9{

𝑞9{{

𝑞7{{

{𝑞7, 𝑞9}UP/LEFT {𝑞7, 𝑞9}DOWN/RIGHT

𝑦

𝑥𝑝𝑥

𝑝𝑦 • 𝑃

𝑞9

𝑙1

𝑙2

𝑞7

𝑞7 = atan2 𝑠7, 𝑐7= atan2 ⁄𝑝t 𝑙7 + 𝑙9𝑐9 − 𝑝s𝑙9𝑠9 det , ⁄𝑝s 𝑙7 + 𝑙9𝑐9 + 𝑝t𝑙9𝑠9 det

Algebraic solution for 𝑞7

det = 𝑙79 + 𝑙99 + 2𝑙7𝑙9𝑐9 > 0except if 𝑙7 = 𝑙9 and 𝑐9 = −1

being then 𝑞7 undefined(singular case: ∞7 solutions)

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)

linear in𝑠7 and 𝑐7

another solution method…

Robotics 1 20

𝑝s = 𝑙7𝑐7 + 𝑙9𝑐79 = 𝑙7𝑐7 + 𝑙9 𝑐7𝑐9 − 𝑠7𝑠9

𝑝t = 𝑙7𝑠7 + 𝑙9𝑠79 = 𝑙7𝑠7 + 𝑙9 𝑠7𝑐9 + 𝑐7𝑠9

𝑙7 + 𝑙9𝑐9 −𝑙9𝑠9

𝑙9𝑠9 𝑙7 + 𝑙9𝑐9

𝑐7𝑠7 =

𝑝s𝑝t

Robotics 1 21

Inverse kinematics of polar (RRP) arm

𝑝𝑥

𝑝𝑦

𝑝𝑧

𝑞1

𝑞2

𝑞3

𝑑1

𝑞7 = atan2 ⁄𝑝t 𝑐9 , ⁄𝑝s 𝑐9

if 𝑝s9 + 𝑝t9 = 0, then 𝑞7 remains undefined (stop); else (2 regular solutions 𝑞7, 𝑞9, 𝑞; )

𝑞9 = atan2 ⁄𝑝� − 𝑑7 𝑞; , v± 𝑝s9 + 𝑝t9 𝑞;

if 𝑞; = 0, then 𝑞7 and 𝑞9 remain both undefined (stop); else (if we stop, it isa singular case:

∞9 or ∞7

solutions)

eliminating 𝑞; > 0 from both arguments

𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 = 𝑞;9

𝑞; = + 𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9

our choice: take here only the positive value...

note: here𝑞9 is NOT aDH variable!

𝑝s = 𝑞;𝑐9𝑐7𝑝t = 𝑞;𝑐9𝑠7𝑝� = 𝑑7 + 𝑞;𝑠9

directkinematics

𝑃

Robotics 1 22

Inverse kinematics of 3R elbow-type arm

𝑝s

𝑝𝑦𝑝𝑧

𝑞1

𝑞2𝑞3

𝑑1

𝐿2

𝐿3

symmetric structure without offsetse.g., first 3 joints of Mitsubishi PA10 robot

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

𝑓, 𝑏 : facing, backingpoint 𝑝 = 𝑝s, 𝑝t, 𝑝�

𝑢, 𝑑 : elbow up, down

4 regular inverse kinematics solutions in 𝑊𝑆7

𝑊𝑆7 = {spherical shell centered at (0,0, 𝑑7), with outer radius 𝑅��� = 𝐿9 + 𝐿;and inner radius 𝑅\+ = 𝐿9 − 𝐿; }

𝑃

Robotics 1 23

Inverse kinematics of 3R elbow-type armstep 1

∈ −1,+1 (else, 𝑝 is out of workspace!)

± 𝑠;= ± 1 − 𝑐;9

directkinematics

𝑝s

𝑝𝑦

𝑝𝑧

𝑞1

𝑞2𝑞3

𝑑1

𝐿2

𝐿3

𝑝s = 𝑐7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝t = 𝑠7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝� = 𝑑7 + 𝐿9𝑠9 + 𝐿;𝑠9;

𝑝s9 + 𝑝t9 + 𝑝� − 𝑑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𝐿;𝑐;𝑐; = ⁄𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 − 𝐿99 − 𝐿;9 2𝐿9𝐿;

𝑞;� = atan2 𝑠;, 𝑐;two solutions

𝑞;o = atan2 −𝑠;, 𝑐; = − 𝑞;

�

Robotics 1 24

only when 𝑝s9 + 𝑝t9 > 0 …

… being 𝑝s9 + 𝑝t9 = 𝐿9𝑐9 + 𝐿;𝑐9; 9 > 0𝑝s

𝑝𝑦

𝑝𝑧

𝑞1

𝑞2𝑞3

𝑑1

𝐿2

𝐿3

again, two solutions𝑞7� = atan2 𝑝t, 𝑝s

𝑞7o = atan2 −𝑝t, −𝑝s

directkinematics

𝑝s = 𝑐7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝t = 𝑠7 𝐿9𝑐9 + 𝐿;𝑐9;𝑝� = 𝑑7 + 𝐿9𝑠9 + 𝐿;𝑠9;

𝑐7 = v𝑝s ± 𝑝s9 + 𝑝t9

𝑠7 = v𝑝t ± 𝑝s9 + 𝑝t9(else 𝑞1 is undefined —infinite solutions!)

Inverse kinematics of 3R elbow-type armstep 2

Robotics 1 25

define and solve a linear system 𝐴𝑥 = 𝑏in the algebraic unknowns 𝑥 = (𝑐9, 𝑠9)

combine first the two equations of direct kinematics and rearrange the last one

coefficient matrix 𝐴 known vector 𝑏

4 regular solutions for 𝑞9,depending on the combinations of +,− from 𝑞7 and 𝑞;

provided det 𝐴 = 𝑝s9 + 𝑝t9 + 𝑝� − 𝑑7 9 ≠ 0

𝑝s

𝑝𝑦

𝑝𝑧

𝑞1

𝑞2𝑞3

𝑑1

𝐿2

𝐿3

Inverse kinematics of 3R elbow-type armstep 3

𝑐7𝑝s + 𝑠7𝑝t = 𝐿9𝑐9 + 𝐿;𝑐9;

𝑝� − 𝑑7 = 𝐿9𝑠9 + 𝐿;𝑠9;

= 𝐿9 + 𝐿;𝑐; 𝑐9 − 𝐿;𝑠;𝑠9

= 𝐿;𝑠;𝑐9 + 𝐿9 + 𝐿;𝑐; 𝑠9

𝐿9 + 𝐿;𝑐; −𝐿;𝑠;�,o

𝐿;𝑠;�,o 𝐿9 + 𝐿;𝑐;

𝑐9𝑠9 = 𝑐7

�,o 𝑝s + 𝑠7�,o 𝑝t

𝑝� − 𝑑7

(else 𝑞9 is undefined —infinite solutions!)𝑞9

�,� , �,�

= atan2 𝑠9�,� , �,� , 𝑐9

�,� , �,�

Inverse kinematics for robots with spherical wrist

𝑥0

𝑦0

𝑧0

𝑥6𝑦6

𝑧6 = 𝑎first 3 robot jointsof any type (RRR, RRP, PPP, …)

find 𝑞7,⋯ , 𝑞� from the input data§ 𝑝 (origin 𝑂�)§ 𝑅 = 𝑛 𝑠 𝑎 (orientation of 𝑅𝐹�)

𝑧3 𝑧5

j4j5

j6𝑂� = 𝑝

𝑊

𝑑6

j1

1. 𝑊 = 𝑝 − 𝑑�𝑎 ⇒ 𝑞7, 𝑞9, 𝑞; (inverse “position” kinematics for main axes)

2. 𝑅 = )𝑅;(𝑞7,𝑞9,𝑞;) ;𝑅�(𝑞�,𝑞�,𝑞�)

given Euler 𝑍𝑌𝑍 or 𝑍𝑋𝑍rotation matrix with𝑞�,𝑞�,𝑞� (𝜃�,𝜃�,𝜃�)

known,after step 1

⇒ ;𝑅� 𝑞�,𝑞�,𝑞� = )𝑅;'𝑅

Robotics 1 26

𝑧4

⇒ 𝑞�, 𝑞�, 𝑞�

last 3 joints RRR, withaxes intersecting in 𝑊

(inverse “orientation”kinematics for the wrist)two regular

solutions

6R robot Unimation PUMA 600

8 different (regular) inverse solutions that can be found in closed form

sphericalwrist

Robotics 1 27

𝑎 = 0𝑧6(𝑞)

𝑛 = 0𝑥6(𝑞)

𝑠 = 0𝑦6(𝑞)

𝑝 = 𝑂6(𝑞)

here 𝑑� = 0, so that 𝑂� = 𝑊 directly

a function of𝑞7, 𝑞9, 𝑞; only!

Finding nice kinematic relationswhiteboard …

Robotics 1 28

§ the most complex inverse kinematics that could 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!

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

Manseur and Doty: International Journal of Robotics Research, 1988

)𝑇� = )𝐴7 𝜃7 7𝐴9 𝜃9 ⋯ �𝐴� 𝜃� = 𝑈)

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

method used for theUnimation PUMA 600 in (*)

)𝐴7o7 )𝑇� = 𝑈7 (= 7𝐴9 ⋯ �𝐴�)7𝐴9o7 )𝐴7o7 )𝑇� = 𝑈9 = 9𝐴; ⋯ �𝐴�

⋯�𝐴�o7 ⋯

7𝐴9o7 )𝐴7o7 )𝑇� = 𝑈� (= �𝐴�)

)𝑇� �𝐴�o7 = 𝑉� (= 7𝐴9 ⋯ �𝐴�))𝑇� �𝐴�o7 �𝐴�o7 = 𝑉� = 7𝐴9 ⋯ ;𝐴�

⋯)𝑇� �𝐴�o7 �𝐴�o7 ⋯

7𝐴9o7 = 𝑉7 (= )𝐴7)

or also ...

)𝑇� =𝑛 𝑠 𝑎 𝑝0 0 0 1 = )𝐴� 𝜃

𝑎� = 𝑎' 𝜃 𝑧𝑝� = 𝑝' 𝜃 𝑧

𝑝 9 = 𝑝' 𝜃 𝑝(𝜃)𝑝'𝑎 = 𝑝' 𝜃 𝑎 𝜃

… then solve easily for the remaining 𝜃7 and 𝜃�

solved analyticallyor numerically …

𝑧 = 0 0 1 '

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 𝑞 = ���(�)��

n Newton method (here only for 𝑚 = 𝑛, at the 𝑘th iteration)n 𝑟� = 𝑓4 𝑞 = 𝑓4 𝑞� + 𝐽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!)

¬ neglected

Numerical solution of inverse kinematics problems

Robotics 1 29

𝑞��7 = 𝑞� + 𝐽4o7(𝑞�) 𝑟� − 𝑓4 𝑞�

(analytical Jacobian)

Operation of Newton methodn in the scalar case, also known as “method of the tangent”n for a differentiable function 𝑓(𝑥), find a root 𝑥∗ of 𝑓 𝑥∗ = 0 by

iterating as

Robotics 1 30

animation fromhttp://en.wikipedia.org/wiki/File:NewtonIteration_Ani.gif

an approximating sequence

𝑥��7 = 𝑥� −𝑓(𝑥�)𝑓{ 𝑥�

𝑥7, 𝑥9, 𝑥;, 𝑥�, 𝑥�,⋯ ⟶ 𝑥∗

n Gradient method (max descent)n minimize the error function

𝐻 𝑞 = 79 𝑟� − 𝑓4(𝑞) 9 = 7

9 𝑟� − 𝑓4(𝑞) ' 𝑟� − 𝑓4(𝑞)

𝑞��7 = 𝑞� − 𝛼 ∇�𝐻(𝑞�)

Numerical solution of inverse kinematics problems (cont’d)

Robotics 1 31

from∇�𝐻(𝑞) = ⁄𝜕𝐻(𝑞) 𝜕𝑞 ' = − 𝑟� − 𝑓4 𝑞

' ⁄𝜕𝑓4(𝑞) 𝜕𝑞'= −𝐽4'(𝑞) 𝑟� − 𝑓4(𝑞)

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

we get

Revisited as a feedback scheme

𝐽4'(𝑞) óõ

𝑓4(𝑞)

+

-

𝑒𝑟� 𝑞�̇�𝑞(0)

𝑟� = cost

𝑒 = 𝑟� − 𝑓4 𝑞 ® 0 ⟺ closed-loop equilibrium 𝑒 = 0is asymptotically stable

𝑉 = 79𝑒'𝑒 ³ 0 is a Lyapunov candidate function

in particular, 𝑒 = 0

asymptotic stability

(𝛼 = 1)

Robotics 1 32

𝑟

�̇� = 𝑒'�̇� = 𝑒'𝑑𝑑𝑡

𝑟� − 𝑓4(𝑞) = −𝑒'𝐽4 𝑞 �̇� = −𝑒'𝐽4 𝑞 𝐽4' 𝑞 𝑒 ≤ 0

�̇� = 0 ⟺ 𝑒 ∈ 𝒩 𝐽4'(𝑞)

null space

Properties of Gradient methodn computationally simpler: use the Jacobian transpose, rather

than its (pseudo)-inversen same use also for robots that are redundant (𝑛 > 𝑚) for the taskn may not converge to a solution, but it never divergesn the discrete-time evolution of the continuous scheme

is equivalent to an iteration of the Gradient methodn the scheme can be accelerated by using a gain matrix 𝐾 > 0

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 33

𝛼 = Δ𝑇𝑞��7 = 𝑞� + ∆𝑇 𝐽4'(𝑞�) 𝑟� − 𝑓4(𝑞�) ,

�̇� = 𝐽4' 𝑞 𝐾𝑒 = 𝐽4' 𝑞 𝐾 𝑟� − 𝑓4(𝑞)

𝑞��7 = 𝑞� +

1𝑠9

𝑐79 𝑠79− 𝑐7 + 𝑐79 − 𝑠7 + 𝑠79 |�¬�­

𝛼 − 𝑠7 + 𝑠79 𝑐7 + 𝑐79−𝑠79 𝑐79 |�¬�­

× 1 − 𝑐7 + 𝑐791 − 𝑠7 + 𝑠79 |�¬�­

A case studyanalytic 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

n Jacobian matrix

n Newton versus Gradient iteration

Robotics 1 34

𝑓4 𝑞 = 𝑐7 + 𝑐79𝑠7 + 𝑠79

𝑒 = 𝑝� − 𝑓4 𝑞 = 11 − 𝑓4(𝑞)

𝐽4 𝑞 =𝜕𝑓4(𝑞)𝜕𝑞 = − 𝑠7 + 𝑠79 −𝑠79

𝑐7 + 𝑐79 𝑐79

𝑒�det 𝐽4(𝑞)

𝐽4o7(𝑞�)

𝐽4'(𝑞�)

Error functionn 2R robot with 𝑙7 = 𝑙9 = 1 and desired end-effector position 𝑝� = (1,1)

Robotics 1 35

plot of 𝑒 9 as a function of 𝑞 = (𝑞7, 𝑞9)

𝑒 = 𝑝� − 𝑓4(𝑞)

two local minima(inverse kinematic solutions)

Configuration space of 2R robotwhiteboard …

n can we represent the correct ‘‘distance’’ between two configurations 𝑞{ and 𝑞{{of this robot on a (square) region in ℝ9?

Robotics 1 36

n configuration space is a torus 𝑆𝑂 1 × 𝑆𝑂 1 , i.e., the surface of a ‘‘donut’’

n the right metric is a geodesic on the torus …

𝑞7

𝑞9

−𝜋 𝜋−𝜋

𝜋𝑞{

𝑞{{

𝑞{

𝑞{{

𝑞{

𝑞{{close or far?

join thetwo sides𝑞7 = −𝜋

and 𝑞7 = 𝜋

join thetwo sides𝑞9 = −𝜋

and 𝑞9 = 𝜋

𝑞7

𝑞9

(0,0)

&

Error reduction by Gradient methodn flow of iterations along the negative (or anti-) gradientn two possible cases: convergence

Robotics 1 37

start

one solution

local maximum(stop if this is the initial guess)

. .

another start...

...the other solution

saddle point(stop after some iterations)

(𝑞7, 𝑞9){ = (0, 𝜋/2) (𝑞7, 𝑞9){{ = (𝜋/2,−𝜋/2) (𝑞7, 𝑞9)1®s = (−3𝜋/4,0) (𝑞7, 𝑞9)¦®��¯° = (𝜋/4,0)

𝑒 ∈ 𝒩(𝐽4'(𝑞)) !

or stuck (at zero gradient)

Convergence analysiswhen does the gradient method get stuck?

n lack of convergence occurs whenn 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'(𝑞)

Robotics 1 38

(𝑞7, 𝑞9)1®s = (−3𝜋/4,0)

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

𝑝�

𝑝

𝑒 ∉ 𝒩(𝐽4'(𝑞)) !!

𝑝�

𝑝

𝑒 ∈ 𝒩(𝐽4'(𝑞))𝑝�

𝑝

(𝑞7, 𝑞9) = (𝜋/9,0)

the algorithm will proceed in this case,

moving out ofthe singularity

𝑞1

𝑞2

𝑝� =11

𝑒 = 𝑝� − 𝑝 =1 − 21 − 2

𝐽4' 𝑞 = − 𝑠7 + 𝑠79 𝑐7 + 𝑐79−𝑠79 𝑐79 |�¬�²³´´µ¶

= − 2 2− 2 2

Issues in implementationn initial guess 𝑞)

n only one inverse solution is generated for each guessn multiple initializations for obtaining other solutions

n optimal step size 𝛼 > 0 in Gradient methodn 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 iterationn stopping criteria

n understanding closeness to singularities

Robotics 1 39

Cartesian error(possibly, separate for

position and orientation)

algorithmincrement

𝑟� − 𝑓4 𝑞� ≤ 𝜀 𝑞��7 − 𝑞� ≤ 𝜀�

good numerical conditioning of Jacobian matrix (SVD)

(or a simpler test on its determinant, for 𝑚 = 𝑛)𝜎1\+ 𝐽4 𝑞� ≥ 𝜎)

Numerical tests on RRP robot

n RRP/polar robot: desired E-E position 𝑟� = 𝑝� = 1, 1, 1—see slide 21, 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 𝜀 = 10o� (max Cartesian error), 𝜀� = 10o� (min joint increment)n 𝑘1®s = 15 (max # iterations), det 𝐽4(𝑞) ≤ 10o� (singularity closeness)

n numerical performance of Gradient (with different steps 𝛼) vs. Newtonn test 1: 𝑞) = (0, 0, 1) as initial guessn test 2: 𝑞) = (−𝜋/4, 𝜋/2, 1)— ‘‘singular” start, since 𝑐9 = 0 (see slide 21)n test 3: 𝑞) = (0, 𝜋/2, 0)— ‘‘doubly singular” start, since also 𝑞; = 0n solution and plots with Matlab code

Robotics 1 40

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

Robotics 1 41

Gradient: 𝛼 = 0.5

slow, 15 (max)iterations

Gradient: 𝛼 = 1

too large, oscillatesaround solution

Newton

very fast, convergesin 5 iterations

0.15⋅10-8

Gradient: 𝛼 = 0.7

good, convergesin 11 iterations

0.57⋅10-5

Cartesian errorscomponent-wise

𝑒𝑥

𝑒𝑦

𝑒𝑧

Numerical test - 1

n test 1: 𝑞) = (0, 0, 1) as initial guess; evolution of joint variables

Robotics 1 42

Gradient: 𝛼 = 1 Gradient: 𝛼 = 0.7not convergingto a solution

converges in 11 iterations

Newtonconverges in 5 iterations

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

Numerical test - 2n test 2: 𝑞) = (−𝜋/4, 𝜋/2, 1): singular start

Robotics 1 43

Gradient𝛼 = 0.7

with check of singularity:

blocked at start

without check:it diverges!

Newton

erro

r nor

ms

starts towardsolution, butslowly stops

(in singularity):when Cartesian errorvector 𝑒 ∈ 𝒩 𝐽4'(𝑞) jo

int v

aria

bles

!!

Numerical test - 3n test 3: 𝑞) = (−𝜋/4, 𝜋/2, 1): doubly singular start

Robotics 1 44

Newtonis either

blocked at startor (w/o check)

explodes!⇒ “NaN” in Matlaber

ror n

orm

Gradient (with 𝛼 = 0.7)① starts toward solution② exits the double singularity③ slowly converges in 19

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

join

t var

iabl

es

Carte

sian

erro

rs

➀

➁

➂

0.96⋅10-5

𝑒𝑥

𝑒𝑦

𝑒𝑧

Final remarksn 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 endn check if the solution found is feasible, as for analytical methods

n in alternative, an optimization criterion can be included in the search n drives iterations toward an inverse kinematic solution with nicer properties

n if the problem has to be solved on-linen execute iterations and associate an actual robot motion: repeat steps at

times 𝑡), 𝑡7 = 𝑡) + 𝑇, ..., 𝑡� = 𝑡�o7 + 𝑇 (e.g., every 𝑇 = 40 ms) n a “good” choice for the initial guess 𝑞) at 𝑡� is the solution of the previous

problem at 𝑡�o7 (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 a continuous task trajectory 𝑟�(𝑡)

Robotics 1 45