Inverse Kinematics
The Problem
Forward Kinematics:
Joint angles  End effector
Fall 2012
Inverse Kinematics:
End effector  Joint angles
2
Robotic applications: cutting/welding
Animation Applications (more)
Fall 2012
3
IK Solutions
Analytical solutions are desirable because of
their speed and exactness of solution.
For complex kinematics problems, analytical
solutions may not be possible
Use iterative methods

Fall 2012
Optimization methods (e.g., minimize the distance
between end effector and goal point)
4
Case Study: Two-Link Arm L
Forward
Kinematics
L1
x  L1 cos q 1  L 2 cos( q 1  q 2 )
y  L1 sin q 1  L 2 sin( q 1  q 2 )
Solving Inverse Kinematics
q1
2
q2
y
x
Analytic (closed form) solution
x  L1 cos q 1  2 L1 L 2 cos q 1 cos( q 1  q 2 )  L 2 cos (q 1  q 2 )
2
2
2
2
2
y  L1 sin q 1  2 L1 L 2 sin q 1 sin( q 1  q 2 )  L 2 sin (q 1  q 2 )
2
2
2
2
2
 cos q 1 cos q 1 cos q 2  sin q 1 sin q 2  

x  y  L  L  2 L1 L 2 
  sin q 1 sin q 1 cos q 2  cos q 1 sin q 2  
2
2
2
1
2
2
 L1  L 2  2 L1 L 2 cos q 2
2
2
2
2
2
2


x

y

L

L
1
1
2

q 2  cos 

2 L1 L 2
Fall 2012 

Case:3-link arm
5
2-link Arm (analytic solution2)
tan q 3 
tan q 4 
y
x
tan  a  b  
L 2 sin q 2
tan a  tan b
1  tan a tan b
L 2 cos q 2  L1
q1  q 3  q 4
tan q 1 
q 1  tan
y
x

1
1
L 2 sin q 2
L 2 cos q 2  L1
y
L 2 sin q 2
x L 2 cos q 2  L1

y  L 2 cos q 2  L1   x L 2 sin q 2
x  L 2 cos q 2  L1   y L 2 sin q 2
y  L 2 cos q 2  L1   x L 2 sin q 2
x  L 2 cos q 2  L1   y L 2 sin q 2
L2
q3
Fall 2012
q4 L1
q1
x
q2
6
y
Unreachable Targets
No joint angles can satisfy the target
q 2  cos
q 1  tan
Fall 2012
1
1
 x 2  y 2  L12  L22 




2 L1 L 2


y  L 2 cos q 2  L1   x L 2 sin q 2
x  L 2 cos q 2  L1   y L 2 sin q 2
If this value is
> 1 or < -1,
no solution exists
7
Statement of the IK Problem
Want
*
Error at ith iteration
i
i
Seek correction Dq to fix the error
q: joint angles
g=
Omit high order terms…
Fall 2012
Note: here
Dq are in
radians!
Jacobian matrix
xRn, qRm, Jnm
m: joint space dimension
n: space where end effector is in
8
Error at End Effector
Current
end effector
configurat
position
p c  x c
orientatio


n o c  u 1c


yc
ion
zc 
T


u 3c


u 2c
3 3
Errors :
E pos ( q )  p d  p c
E orient ( q ) 
 u
2
 u jd  1 
2
jc
j
Fall 2012
9
Jacobian Inverse
Forward
L2
L1
Kinematics
x  L1 cos q 1  L 2 cos( q 1  q 2 )
y  L1 sin q 1  L 2 sin( q 1  q 2 )
q1
q2
y
x
D x  JD q
 x q1
J  
y q1
 x  q 2    L1 sin q 1  L 2 sin q 1  q 2 
 
 y  q 2   L1 cos q 1  L 2 cos q 1  q 2 
J D q  D x , iterate until
 L 2 sin q 1  q 2 

L 2 cos q 1  q 2  
D q is small
Simple case: Cramer’s rule suffices
General case: pseudo inverse
Fall 2012
10
Jacobian inverse is
underdetermined
3-link 2D arm
Forward
Kinematics
x  L1 cos q 1  L 2 cos( q 1  q 2 )  L 3 cos( q 1  q 2  q 3 )
L3 q3
y  L1 sin q 1  L 2 sin( q 1  q 2 )  L 3 sin( q 1  q 2  q 3 )
L2
D x  JD q
 qx1
J   y
  q 1
x
q 2
y
q 2
x
q 3

y 
q 3 

  L1 sin q 1  L 2 sin q 1  q 2   L 3 sin q 1  q 2  q 3 

 L1 cos q 1  L 2 cos q 1  q 2   L 3 cos q 1  q 2  q 3 
Fall 2012
L1
q1
q2
x
 L 2 sin q 1  q 2   L 3 sin q 1  q 2  q 3 
L 2 cos q 1  q 2   L 3 cos q 1  q 2  q 3 
y
 L 3 sin q 1  q 2  q 3 

L 3 cos q 1  q 2  q 3  
11
Pseudo Inverse and Ax=b
Ax  b

A panacea for Ax=b

x  A b  V U b
T
Full rank: A-1 exist; A+ is the same as A-1
Underdetermined case: many solutions; will
find the one with the smallest magnitude |x|
Overdetermined case: find the solution that
minimize the error r=||Ax–b||, the least
square solution
Fall 2012
12
About Pseudo Inverse A+
.
A
.
.
.
.

. 2  3
A: row space → column space
A+: column space → row space
A+
A
R3
Fall 2012
R2
13
Using GSL
(Gnu Scientific Library)
Full rank
Full row rank
Full column rank
1

2
1

0
1

1

 0
2

1
Fall 2012
3   x1   4 
    
2  x2  4
1
1
 x1 
0   2
  x2    
1
2
 x 3 
0
 1 
  x1   
1   2
 x
 
 2
  1
1 
2   x1   8 
    
1   x 2  3
14
Forward Kinematics
L2
Using Coordinate Transformation
 x  cq1
  
y  sq
   1
 1   0
 sq1
cq1
0
0  1

0 0

1   0
0
1
0
x  R1 q 1 T1  L1  R 2 q 2 T 2  L 2  p
Fall 2012
L1   c q 2

0 sq 2

1   0
L1
 sq 2
cq 2
0
q1
0  1

0 0

1   0
x
0
1
0
q2
y
End effector at
the local origin
L2  0 
 
0 0
 
1   1 
Rotate(z,q1)
Translate(L1,0)
Rotate(z,q2)
Translate(L2,0)
15
Computing Jacobian
wi: unit vector of
Fall 2012 rotation axis
reference
17
x  L1 cos q 1  L 2 cos( q 1  q 2 )
Example
J  J 1
y  L1 sin q 1  L 2 sin( q 1  q 2 )
J2
i

J 1  w 1  r1    0
x

j
0
y
k
  y 
1 

x


0 

i

J 2  w 2  r2   
0
 L 2 cos q 1  q 2 

  L 2 sin q 1  q 2 
 



L
cos
q

q
1
2
 2

Fall 2012
L1
j
0
L 2 sin q 1  q 2 
k

1
0 
q1
r1
L2
q1
y
x
L2
L1
q2
r2
q2
x
18
y
About Joint Space Redundancy
Joint Space
m
End Effector
n
If m > n
Redundant
manipulator
Exploiting
redundancy
D q  J D x  I  J J  
D z




secondary goals
Dz  R
m
Fall 2012
19
Example
D z  q 1  q max , 0 , 0  : Joint angle limit q max
T
Secondary

goal correction

:

D q 3 1  I 3  3  J 3  2 J 2  3 D z
qmax
Fall 2012
20
IK Solution (CCD)
CCD: cyclic coordinate descent; initially from C.
Welman (1993)
From the most distal joint, solve a series of onedimensional minimization analytically to satisfy the
goal (one joint at a time)
Fall 2012
21
CCD-1 (Cyclic Coordinate Descent)
Fall 2012
22
CCD-2
Fall 2012
23
CCD-3
Implementing joint limits in
CCD is straight forward:
simply clamp the joint angle
Fall 2012
24
CCD (2-link arm)
Pcurrent
L2
L1
P1
f
f  sign  cos
q2
Pdest
1
 p1 p c
p1 p d


 p p
p1 p d
 1 c
sign : determined




by p 1 p c  p 1 p d
q1
Pc
L2
P0
L1 f
q1
Fall 2012
q2
Pd
f  sign  cos
1
 p0 pc
p0 pd


 p p
p0 pd
 0 c
sign : determined




by p 0 p c  p 0 p d
25
IK Solutions (DLS)
For 3-link 2D arm:
m = 2, n = 3
DLS (damped least square)
Position of k end effectors
target of end effectors
error of end effectors
Joint angles
Jacobian matrix (mn)
Fall 2012
26
DLS (Damped Least Square)
Joint angle to correct error
Minimize damped least square
Rewrite error as
Normal equation for
least square problem
Simplifying
and get
Fall 2012
27
[Details]
JD q  e  R
Dq  R
m
 JD q  e   J 
 e 


x 
D q    
  

  D q    I 
 0 
n
 JD q  e 
mn
x

R

 Dq 
JD q  e
2
 Dq
2
minimize
 x
2
x
2
 J
 
 I
Ax
Summary
minimize
b  A xˆ
by solving
A A xˆ  A b
T
Fall 2012

e
 D q   

0
T
2
b
2
Normal Equation
 J

 I



T
 J

 I

 J 
 D q  


 I 
T
e
 
0
28
DLS (cont)
(continued)
Shown next page
Therefore
Instead of computing
inverse, solve
How to
choose ?
Fall 2012
For 3-link 2D arm:
m = 2, n = 3
2x2
Affect convergence rate
29
Damping Effects
Fall 2012
30
Properties of Pseudo Inverse A+
(From Wikipedia)
Fall 2012
31
Challenging IK Cases:
Multiple Targets
Fall 2012
32
Types of IK
Fall 2012
33
Types of IK (cont)
Fall 2012
34
Inverse Kinetics (Boulic96)
The constraint on the position of the center of mass is
treated as any other task, and solved at the differential
level with a special-purpose Jacobian matrix that relates
differential changes of the joint coordinates to
differential changes of the Cartesian coordinates of the
center of mass.
Fall 2012
35
Kinematic chain
Fall 2012
36
Fall 2012
37
Fall 2012
38
Support Materials
3-link 2D arm
Jacobian inverse is underdetermined
L3 q3
Forward
Kinematics
L2
x  L1 cos q 1  L 2 cos( q 1  q 2 )  L 3 cos( q 1  q 2  q 3 )
y  L1 sin q 1  L 2 sin( q 1  q 2 )  L 3 sin( q 1  q 2  q 3 )
x  J q
 qx1
J   y
  q 1
x
q 2
y
q 2
x
q 3

y 
q 3 

  L1 sin q 1  L 2 sin q 1  q 2   L 3 sin q 1  q 2  q 3 
 
 L1 cos q 1  L 2 cos q 1  q 2   L 3 cos q 1  q 2  q 3 
Fall 2012
L1
q1
q2
y
x
 L 2 sin q 1  q 2   L 3 sin q 1  q 2  q 3 
L 2 cos q 1  q 2   L 3 cos q 1  q 2  q 3 
 L 3 sin q 1  q 2  q 3 

L 3 cos q 1  q 2  q 3  
40
Recall least square problem
Projection onto a Space
Ax=b may not have
solution (if b is not
in C(A))
Solve A xˆ  p
instead where p is
the projection of b
onto C(A)
b
a2
 |

A  a1

 |
e=b–p
p
a1
Summary
minimize
b  A xˆ
by solving
A A xˆ  A b 41
T
Fall 2012
| 

a2

| 
T
2
Projection (cont)
p  xˆ1 a 1  xˆ 2 a 2  A xˆ
b  A xˆ  a 1
b  A xˆ  a 2
 a 1T b  A xˆ   0
 a 1T 
  T  b  A xˆ  
 T
 a 2 b  A xˆ   0
a2 
A A xˆ  A b
T
Known as the “normal equation”
T

xˆ  A A
T

1
T
A b

p  A xˆ  A A A
Fall 2012
0 
 
0 
T

1
A b  Pb
T
P: projection matrix
42
Extra material
Jacobian transpose
Constraint dynamics
IK General (Jacobian Transpose)
pc
L2
L1
Force applied at end effector

F  fx
fy
q2
y
my
mz

T
x 6  1  f ( q n 1 )
pd
of virtual work
  J n  6 F 6 1 , J 6  n 
T
x
mx
Forward Kinematics
From principle
q1
fz
Aristotole
dynamics
f
q
 n 1  m q n 1 and let m  1
T
q  J F
Fall 2012
44
Jacobian Transpose
Forward
L2
Kinematics
x  L1 cos q 1  L 2 cos( q 1  q 2 )
L1
y  L1 sin q 1  L 2 sin( q 1  q 2 )
x  J q
 x q1
J  
y q1
Fall 2012
 x  q 2    L1 sin q 1  L 2 sin q 1  q 2 
 
 y  q 2   L1 cos q 1  L 2 cos q 1  q 2 
q1
q2
y
x
 L 2 sin q 1  q 2 

L 2 cos q 1  q 2  
45
Fall 2012
46
Fall 2012
47
Fall 2012
48
Fall 2012
49
Differences
Jacobian
inverse
Jacobian t ranspose
x  f (q )
x  f (q )
x 2 1  J 2  3 q 31
D x 2 1  J 2  3 D q 3 1

q  J x
Dq  J Dx
Fall 2012
T
50
P2
[Constraint Dynamics]
Eqn of motion :
L1
q  WQ
Q  fa  fc
2
C ( q )  J ( q ) q  0
 Cx 1
J    C1
2
  x1
Combining
 fc   0
work
of virtual
Principle
fc  J 
T
JWJ
T
   JWf
Fall 2012
2
q1
2
 x 2  x1  2   y 2 
C (q )  0
 fa
P1
q2
x1  y 1  L1  0
Constraint
JW
L2
y1   L 2  0
2
C1
C1
 y1
x 2
C 2
C 2
 y1
x 2
2 x1

 
 2  x1  x 2 
2
C1

C 2 
y 2 

y 2
2 y1
0
2  y1  y 2 
2  x 2  x1 


2  y 2  y 1 
0
a
51
Fall 2012
52
A
A
B
L2
L1
q1
q2
F2
g
x  BT x
A
B
T : T taking frameA to
frameB
F1
g  F B1 T
FB
g
F2
g  F21 T
F1
FB
T
F1
FB
T  Rot ( z , q 1 )
F2
F1
T  Rot ( z , q 2 ) trans (  L1 , 0 , 0 )
F
F
FB
g
F1 FB
All joints rotate w.r.t. local z
CCD step 1: Find 2g; project og to XY plane (as og’); rotate OE to og’
CCD step 2: Find 1g; project og to XY plane (as og’); rotate OE to og’
Fall 2012
53