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
!  “given a desired end-effector pose (position +
orientation), find the values of the joint variables
that will realize it”
!  a synthesis problem, with input data in the form
R p p
!  T= = 0A (q)
n "  r = = fr(q), or for any
000 1 " other task vector
classical formulation: generalized formulation:
inverse kinematics for a given end-effector pose inverse kinematics for a given value of task variables

!  a typical nonlinear problem

!  existence of a solution (workspace definition)
!  uniqueness/multiplicity of solutions (r ! Rm, q !Rn)
!  solution methods
Robotics 1 3
 Solvability and robot workspace
(for tasks related to a desired end-effector Cartesian pose)

!  primary workspace WS1: set of all positions p that can be

reached with at least one orientation (" or R)
!  out of WS1 there is no solution to the problem
!  when p ! WS1, there is a suitable " (or R) for which a solution exists

!  secondary (or dexterous) workspace WS2: set of positions p

that can be reached with any orientation (among those
feasible for the robot direct kinematics)
!  when p ! WS2, there exists a solution for any feasible " (or R)

!  WS2 # WS1

Robotics 1 4
 Workspace of Fanuc R-2000i/165F
section for a
constant angle $1
WS1⊂R3
(! WS2 for spherical wrist
without joint limits)

rotating the
base joint angle $1
Robotics 1 5
 Workspace of planar 2R arm
2 orientations
l1+l2
y if p ! int(WS1)
•  p = WS1- (WS1
l2
q2
|l1-l2|
l1
q1 (WS1
inner and outer
x boundaries 1 orientation
!  if l1 % l2
!  WS1 = {p ! R2: |l1-l2| & !p!& l1+l2} ⊂ R2
!  WS2 = '

!  if l1 = l2 = "
!  WS1 = {p ! R2: !p!& 2"} ⊂ R2
!  WS2 = {p = 0} (infinite number of feasible orientations at the origin)

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 and visualizing the 8 solutions
to the inverse kinematics of 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& # 3 /2 0.5 0&
p = % "0.0832( R = % "0.5 3 /2 0(
% ( % (
$ 1.3224 ' $ 0 0 1'
[m]

!
configuration at start
! T
q = ( " /3 #2" /3 " /6 0 " /2 0)
[rad]

Robotics 1 9
 The 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 (
q =% ( q =% ( q =% ( q =% (
% "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 (
q =% ( q =% ( q =% ( q =% (
% 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
some examples

!  E-E positioning (m=2) of a planar 2R robot arm

!  2 regular solutions in int(WS1)
!  1 solution on (WS1
singular solutions
!  for l1 = l2: ) solutions in WS2
!  E-E positioning of an articulated elbow-type 3R robot arm
!  4 regular solutions in WS1 (with singular cases yet to be investigated ...)
!  spatial 6R robot arms
!  & 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 *i = 0 or ±+/2 (,i)
!  analysis based on algebraic transformations of robot kinematics
!  transcendental equations are transformed into a single polynomial
equation of one variable
!  seek for an equivalent polynomial equation of the least possible degree
Robotics 1 11
 A planar 3R arm
workspace and number/type of inverse solutions
y q3 l1 = l2 = l3 = ", n=3, m=2
•  p
l3
l2 WS1 = {p ! R2: !p!& 3"} ⊂ R2
q2
l1
WS2 = {p ! R2: !p!& "} ⊂ R2
q1 any planar orientation is feasible in WS2
x
1.  in WS1 : )1 regular solutions (except for 2. and 3.),
at which the E-E can take a continuum of
) orientations (but not all orientations in the plane!)
2.  if !p!= 3" : only 1 solution, singular
3.  if !p!= " : )1 solutions, 3 of which singular

4.  if !p!< " : )1 regular solutions (never singular)

Robotics 1 12
 Workspace of a planar 3R arm
general case: different link lengths

⇒ lmin = l3 = 0.3

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

!  if m = n
!  ! solutions
!  a finite number of solutions (regular/generic case)
!  “degenerate” solutions: infinite or finite set, but anyway
different in number from the generic case (singularity)
!  if m < n (robot is redundant for the kinematic task)
!  ! solutions
!  )n-m solutions (regular/generic case)
!  a finite or infinite number of singular solutions
!  use of the term singularity will become clearer when dealing
with differential kinematics
!  instantaneous velocity mapping from joint to task velocity
!  lack of full rank of the associated m"n Jacobian matrix J(q)
Robotics 1 14
 Dexter robot (8R arm)
!  m = 6 (position and orientation of E-E)
!  n = 8 (all revolute joints)
!  )2 inverse kinematic solutions (redundancy degree = n-m = 2)

video

exploring inverse kinematic solutions by a self-motion

Robotics 1 15
 Solution methods

ANALYTICAL solution NUMERICAL solution

(in closed form) (in iterative form)

"  preferred, if it can be found* "  certainly needed if n>m (redundant

"  use ad-hoc geometric inspection case), or at/close to singularities
"  algebraic methods (solution of "  slower, but easier to be set up
polynomial equations) "  in its basic form, it uses the
"  systematic ways for generating a (analytical) Jacobian matrix of the
reduced set of equations to be direct kinematics map
solved
(fr (q)
* sufficient conditions for 6-dof arms Jr(q) =
(q
•  3 consecutive rotational joint axes are
"  Newton method, Gradient method,
incident (e.g., spherical wrist), or
•  3 consecutive rotational joint axes are and so on…
parallel
D. Pieper, PhD thesis, Stanford University, 1968
Robotics 1 16
 Inverse kinematics of planar 2R arm
y
py •  p direct kinematics
l2
q2 px = l1 c1 + l2 c12
l1 py = l1 s1 + l2 s12
q1
data q1, q2 unknowns
px x

“squaring and summing” the equations of the direct kinematics

px2 + py2 - (l12 + l22) = 2 l1 l2 (c1 c12 + s1 s12) = 2 l1 l2 c2
in analytical form
and from this

c2 = (px2 + py2 - l12 - l22)/ 2 l1 l2, s2 = ±-1 - c22 q2 = ATAN2 {s2, c2}

must be in [-1,1] (else, point p 2 solutions

is outside robot workspace!)
Robotics 1 17
 Inverse kinematics of 2R arm (cont’d)
y
py •  p by geometric inspection
q2
q1 = * - .
.

q1 *
px x
2 solutions
q1 = ATAN2 {py, px} - ATAN2 {l2 s2 , l1 + l2 c2}
(one for each value of s2)
note: difference of ATAN2 needs
to be re-expressed in (-+ , +]!
q2’’
•  p
{q1,q2}UP/LEFT q2’ {q1,q2}DOWN/RIGHT

q1” q2’ e q2’’ have same absolute

value, but opposite signs
q1’
Robotics 1 18
 Algebraic solution for q1

another px = l1 c1 + l2 c12 = l1 c1 + l2 (c1 c2 - s1 s2)

linear in
solution
s1 and c1
method… py = l1 s1 + l2 s12 = l1 s1 + l2 (s1c2 + c1s2)

l1 + l2c2 - l2s2 c1 px
=
l2s2 l1 + l2c2 s1 py

except for l1=l2 and c2=-1

det = (l12 + l22 + 2 l1l2c2) > 0 being then q1 undefined
(singular case: )1 solutions)
q1 = ATAN2 {s1, c1} = ATAN2 {(py(l1+l2c2)-pxl2s2)/det, (px(l1+l2c2)+pyl2s2)/det}

notes: a) this method provides directly the result in (-+ , +]

b) when evaluating ATAN2, det > 0 can be eliminated
from the expressions of s1 and c1
Robotics 1 19
 Inverse kinematics of polar (RRP) arm
pz
Note: px = q3 c2 c1
q2 is NOT a q3
DH variable! py = q3 c2 s1
pz = d1 + q3 s2
q2
d1 px2 + py2 + (pz - d1)2 = q32
py
q3 = + -px2 + py2 + (pz - d1)2
px q1 our choice: take here only the positive value...

if q3 = 0, then q1 and q2 remain both undefined (stop); else

(if it stops,
q2 = ATAN2{(pz - d1)/q3, ± -(px + 2 py2)/q32
} a singular case:
)2 or )1
if px2+py2 = 0, then q1 remains undefined (stop); else solutions)
q1 = ATAN2{py/c2, px/c2} (2 regular solutions {q1,q2,q3})
Robotics 1 we have eliminated q3>0 from both arguments! 20
 Inverse kinematics of 3R elbow-type arm
{u, d} = elbow up, down
L2

q3
q2
L3
d1 pz
{f, b} = facing, backing
the point p=(px,py,pz) py

q1 symmetric structure without offsets

px e.g., first 3 joints of Mitsubishi PA10 robot

px = c1 (L2c2+ L3c23)
direct four regular inverse
py = s1 (L2c2+ L3c23)
kinematics kinematics solutions in WS1
pz = d1+ L2s2+ L3s23
Note: more details (e.g., full handling of
WS1={spherical shell centered at (0,0,d1), with outer singular cases) can be found in the solution
radius Rout=#L22+L32 and inner radius Rin=|L2-L3|} of the Robotics 1 written exam of 11.04.2017
Robotics 1 21
 Inverse kinematics of 3R elbow-type arm
L2
q3 px = c1 (L2c2+ L3c23)
q2 py = s1 (L2c2+ L3c23) direct
L3 kinematics
d1 pz pz = d1+ L2s2+ L3s23
py

q1
px

px2 + py2 + (pz-d1)2 = c12 (L2c2+ L3c23)2 + s12 (L2c2+ L3c23)2 + (L2s2+ L3s23)2
= ... = L22 + L32 + 2L2L3 (c2c23+s2s23) = L22 + L32 + 2L2L3 c3

c3 = (px2 + py2 + (pz-d1)2 - L22 - L32) / 2L2L3 ∈ [-1,1] (else, p is out of workspace!)
q3{+} = ATAN2{s3, c3}
±s3 = ±-1 - c3 two solutions
q3{-} = ATAN2{-s3, c3} = - q3{+}
Robotics 1 22
 Inverse kinematics of 3R elbow-type arm
L2
q3 px = c1 (L2c2+ L3c23)
q2 py = s1 (L2c2+ L3c23) direct
L3 kinematics
d1 pz pz = d1+ L2s2+ L3s23
py

q1
px (being px2 + py2 = (L2c2+ L3c23)2 > 0)

only when px2 + py2 > 0 c1 = px /±#px2 + py2

(else q1 is undefined —infinite solutions!) s1 = py /±#px2 + py2

q1{+} = ATAN2{py, px}

again, two solutions
q1{-} = ATAN2{-py, -px}

Robotics 1 23
 Inverse kinematics of 3R elbow-type arm
from the first two equations in the
L2
direct kinematics
q3 c1px + s1py = L2c2 + L3c23
q2
L3 = (L2+L3c3) c2 – L3s3 s2
d1 pz pz – d1 = L2s2+ L3s23
py = L3s3 c2 + (L2+L3c3) s2

q1 we can solve a linear system Ax = b

px in the algebraic unknowns x =(c2, s2)

#L 2 + L 3c3 " L 3s {+,"}

3
& # c2 & # c1{+,"}p x + s1{+,"}p y & four regular solutions for q2,
% ( %s ( = % ( depending on combinations
$ L 3s 3 L 2 + L 3c3 ' $ 2 ' $ p z " d1
{+,"}
'
of {+,-} from q1 and q3
coefficient matrix A known vector b

provided det A = px2 + py2 + (pz-d1)2 > 0

(else q2 is undefined —infinite solutions!) q2{{f,b},{u,d}}
= ATAN2{s2{{f,b},{u,d}}, c2{{f,b},{u,d}}}
Robotics 1 24
 Inverse kinematics
for robots with spherical wrist
z4 y6
j6 W x6
first 3 robot joints O6 = p
of any type (RRR, RRP, PPP, …)
z3 z5 z6 = a

z0 j5 d6
j4
find q1, …, q6 from the input data:
y0
•  p (origin O6)
x0 j1 •  R = [n s a] (orientation of RF6)

1.  W = p - d6 a / q1, q2, q3 (inverse “position” kinematics for main axes)

2.  R = 0R3(q1, q2, q3) 3R6(q4, q5, q6) / 3R6(q4, q5, q6) = 0R3T R / q4, q5, q6
(inverse “orientation”
given known, Euler ZYZ or ZXZ kinematics for the wrist)
after step 1 rotation matrix
Robotics 1 25
 6R example: Unimation PUMA 600

spherical n = 0x6(q)
wrist

s = 0y6(q)

a function of
q1, q2, q3 only!
a = 0z6(q)

p = 06(q)

8 different inverse solutions

here d6=0, that can be found in closed form
so that 06=W directly (see Paul, Shimano, Mayer; 1981)
Robotics 1 26
 Numerical solution of
inverse kinematics problems
!  use when a closed-form solution q to rd = fr(q) does not exist
or is “too hard” to be found
(fr
!  Jr(q) = (analytical Jacobian)
(q
!  Newton method (here for m=n)
!  rd = fr(q) = fr(qk) + Jr(qk) (q - qk) + o(!q - qk!2) 0 neglected

qk+1 = qk + Jr-1(qk) [rd - fr(qk)]

!  convergence if q0 (initial guess) is close enough to some q*: fr(q*) = rd

!  problems near singularities of the Jacobian matrix Jr(q)
!  in case of robot redundancy (m<n), use the pseudo-inverse Jr#(q)
!  has quadratic convergence rate when near to solution (fast!)

Robotics 1 27
 Operation of Newton method
!  in the scalar case, also known as “method of the tangent”
!  for a differentiable function f(x), find a root of f(x*)=0 by iterating as

f(x k )
x k+1 = xk "
f'(x k )

an approximating sequence

! {x , x , x , x , x ,...}
1 2 3 4 5
" x*

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

Robotics 1 28
 Numerical solution of
inverse kinematics problems (cont’d)

!  Gradient method (max descent)

!  minimize the error function
H(q) = $ !rd - fr(q)!2 = $ [rd - fr(q)]T [rd - fr(q)]
qk+1 = qk - * 1qH(qk)
from 1qH(q) = - JrT(q) [rd - fr(q)], we get

qk+1 = qk + * JrT(qk) [rd - fr(qk)]

!  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)
!  when the step size * is too small, convergence is extremely slow

Robotics 1 29
 Revisited as a “feedback” scheme
q(0)
.
rd + e q 2 q rd = cost
Jr T(q)
3
-
r (* = 1)
fr(q)

e = rd - fr(q) / 0 4 closed-loop equilibrium e=0 is asymptotically stable

V = $ eTe 5 0 Lyapunov candidate function
. . .
V=e e=e
T T d ((rd - fr(q)) = - e Jr q = - eT Jr JrTe & 0
T

. dt
V = 0 4 e ! Ker(JrT) in particular e = 0

asymptotic stability
Robotics 1 30
 Properties of Gradient method
!  computationally simpler: Jacobian transpose, rather than its
(pseudo)-inverse
!  direct use also for robots that are redundant for the task
!  may not converge to a solution, but it never diverges
!  the discrete-time evolution of the continuous scheme

qk+1 = qk + 6T JrT(qk) [rd - f(qk)] (* = 6T)

is equivalent to an iteration of the Gradient method
!  scheme can be accelerated by using a gain matrix K>0
.
q = JrT(q) K e
note: K can be used also to “escape” from being stuck in a stationary point,
by rotating the error e out of the kernel of JrT (if a singularity is encountered)

Robotics 1 31
 A case study
analytic expressions of Newton and gradient iterations

!  2R robot with l1 = l2 = 1, desired end-effector position rd = pd = (1,1)

!  direct kinematic function and error
" c1 + c12 % "1%
fr (q) = $ ' e = p d - fr (q) = $ ' - fr (q)
# s1 + s12 & #1&
!  Jacobian matrix
" fr (q) #-(s1 + s12 ) -s12 &
Jr (q) = =% (
"q
! $ c1 + c12 c12 '
!
!  Newton versus Gradient iteration
Jr -1(qk )
det Jr(q)
! 1 " c12 s12 % ek
$ '
s2 -(c
# 1 12+ c ) -(s + s )
12 & q=q k

k+1 k
1 "1 - (c1 + c12 )%
q =q + • $1 - (s + s )'
!# # 12 &
-(s1 + s12 ) c1 + c12 & 1 q=q k
" % (
$ -s 12 c12 ' q=q k
! !
!
Robotics 1
Jr T (qk ) 32
! !
 Error function
!  2R robot with l1=l2=1, desired end-effector position pd = (1,1)

e = pd - fr(q)

two local minima

plot of !e!2 as a function of q = (q1,q2)
(inverse kinematic solutions)
Robotics 1 33
 Error reduction by Gradient method
!  flow of iterations along the negative (or anti-) gradient
!  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

(q1,q2)’ =(0,!/2) (q1,q2)” =(!/2,-!/2) (q1,q2)max =(-3!/4,0) (q1,q2)saddle =(!/4,0)

Robotics 1 e ! Ker(JrT) ! 34
 Convergence analysis
when does the gradient method get stuck?

!  lack of convergence occurs when

!  the Jacobian matrix Jr(q) is singular (the robot is in a “singular configuration”)
!  AND the error is in the “null space” of JrT(q)
p
"1% #1 - 2 &
pd = $ ' pd e = pd " p = %
#1& (
$1 - 2 '
q2
T
" -(s1 + s12 ) c1 + c12 % "( 2 2%
J (q) = $ ' =$ '
!
q1
r
# -s12 c12 & q=q #( 2 2&
pd ! saddle

(q1,q2)saddle =(!/4,0) e ! Ker(JrT)

!

pd e 䌝 Ker(JrT) !!
e ! Ker(JrT) the algorithm will
proceed in this case,
moving out of
p the singularity
p (q1,q2)max =(-3!/4,0) (q1,q2) =(!/9,0)
Robotics 1 35
 Issues in implementation
!  initial guess q0
!  only one inverse solution is generated for each guess
!  multiple initializations for obtaining other solutions
!  optimal step size * in Gradient method
!  a constant step may work good initially, but not close to the
solution (or vice versa)
!  an adaptive one-dimensional line search (e.g., Armijo’s rule) could
be used to choose the best * at each iteration
!  stopping criteria
Cartesian error algorithm
(possibly, separate for !rd - f(qk)! %# increment !qk+1-qk! % #q
position and orientation)
!  understanding closeness to singularities
numerical conditioning
$min{J(qk)} & $0 of Jacobian matrix (SVD)
(or a simpler test on its determinant, for m=n)

Robotics 1 36
 Numerical tests on RRP robot
!  RRP/polar robot: desired E-E position rd = pd = (1, 1 ,1)
—see slide 20, with d1=0.5
!  the two (known) analytical solutions, with q3 & 0, are:
q* = (0.7854, 0.3398, 1.5)
q** = (q1*- +, + - q2*, q3*) = (-2.3562, 2.8018, 1.5)
!  norms # = 10-5 (max Cartesian error), #q =10-6 (min joint increment)
!  kmax=15 (max # iterations), |det(Jr)| % 10-4 (closeness to singularity)
!  numerical performance of Gradient (with different steps *) vs. Newton
!  test 1: q0 = (0, 0, 1) as initial guess

!  test 2: q0 = (-+/4, +/2, 1) —”singular” start, since c2=0 (see slide 20)
!  test 3: q0 = (0, +/2, 0) —”double singular” start, since also q3=0
!  solution and plots with Matlab code

Robotics 1 37
 Numerical test -1
!  test 1: q0 = (0, 0, 1) as initial guess; evolution of 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
ex
Cartesian errors
component-wise
very fast, converges
in 5 iterations ey

ez

Robotics 1 0.15䏙10-8 38
 Numerical test -1

!  test 1: q0 = (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 q* = (0.7854, 0.3398, 1.5)

Robotics 1 39
 Numerical test -2
!  test 2: q0 = (-+/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 e ∈ Ker(JrT)

Robotics 1 40
 Numerical test -3
!  test 3: q0 = (0, +/2, 0): “double” 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!
q*=(0.7854, 0.3398, 1.5) → “NaN” in Matlab
Cartesian errors

joint variables
䙷

Robotics 1 41
 Final remarks
!  an efficient iterative scheme can be devised by combining
!  initial iterations using Gradient (“sure but slow”, linear convergence rate)
!  switch then to Newton method (quadratic terminal convergence rate)
!  joint range limits are considered only at the end
!  check if the solution found is feasible, as for analytical methods
!  in alternative, an optimization criterion can be included in the search
!  driving iterations toward an inverse kinematic solution with nicer properties
!  if the problem has to be solved on-line
!  execute iterations and associate an actual robot motion: repeat steps at
times t0, t1=t0+T, ..., tk=tk-1+T (e.g., every T=40 ms)
!  the “good” choice for the initial guess q0 at tk is the solution of the previous
problem at tk-1 (provides continuity, needs only 1-2 Newton iterations)
!  crossing of singularities/handling of joint range limits need special care
!  Jacobian-based inversion schemes are used also for kinematic control,
along a continuous task trajectory rd(t)
Robotics 1 42