Movie Segment

Catching Flying Balls and

Preparing Coffee: Humanoid
Rollin’Justin Performs Dynamic
and Sensitive Tasks.
Berthold Bäuml, Florian Schmidt, Thomas Wimböck, Oliver
Birbach, Alexander Dietrich, Matthias Fuchs, Werner Friedl,
Udo Frese, Christoph Borst, Markus Grebenstein, Oliver
Eiberger, and Gerd Hirzinger.

ICRA Video Proceedings, 2009.

Direct Kinematics

(forward kinematics)
f
•x
q•

Joint Space Task Space

(dimensions n) (dimensions m)
LMq OP LM x OP
Inverse Kinematics
1 1

MMq. PP
2 MM x. PP
2

qM P xM P
MM . PP MM . PP
MNq. PQ x  f ( q) MNx. PQ
m
n

Joint Coordinates Inverse Kinematics

Revolute Joints i
3
Prismatic Joints di l3
qi   i i   i d i
y0 
LM x OP
RS
l2 
2 x  M y P  f (q)
0 revolute joint
i 
l1
MN PQ
T
1 prismatic joint
1 x
0

LM x OP LM l c  l c OP
i  1 i x  MyP  M l s  l s P
1 1 2 12
b
cos  1   2  c12 g
MN PQ MN     PQ
1 1 2 12
Direct Kinematics
Given q  ( q 1 q2 ... q n ) T 1 2 3

0
n T  0nT ( q) or x  f ( q) (Geometric Model) Given q a unique x

1
 Inverse Kinematics Inverse Kinematics
{W} Finding
{B} 1
 f (X)
or

B
W T  06T ( 1 ,  2 ,  3 ,  4 ,  5 ,  6 ) Solving

or X 
LM X OP  f ()
P
0
T ( 1 ,  2 ,  3 ,  4 ,  5 ,  6 )  WB T
NX Q
R
6

Inverse Problem ( 126 unknowns

equations
)
Given (W T or X) find 
B

Existence of Solutions

y0
l2
l1 (x0,y0); 0
l1- l2 l2
x0
l1
Fc  s123 0 l1 c1 I
 l2 c12 F
c 0  s 0 0 x0I
Gs JJ GG y J
123

 l2 s12 s 0 c 0
T G J
c123 0 l1 s1 0

0 123 0
3
GG 0 0 1 0 JJ 0 GG 0 1 0J
J
H0 0 0 1 K 0 H 0 0 1K
solution if
(l1  l 2 ) 2  x 02  y 02  (l1  l2 ) 2

Joint Limits 0   1  180 o Workspace

0   2  180 o
• Reachable Workspace
• Dextrous Workspace

2
 Dextrous Workspace Multiplicity of Solutions
A
l2 B
B1

l1 B2
Selection of a solution
Criterion: Joint distance

C1   ( B1)   ( A )
C2   ( B 2 )   ( A )

Weighted Joint distance

moving smaller joints

Number of Solutions Main Results

It depends on General 6R open-chain 16 solutions
• Number of Joints General 5RP open-chain 16 solutions
• Link Parameters General 4R2P open-chain 8 solutions
e.g. 6-revolute-joint manipulator General 3R3P open-chain 2 solutions
if all a i  0 Number solutions < 16
Special conditions in the structure [such
if a1 = a3 = a5 = 0 Number solutions < 4 as intersecting or parallel axes] cause
• Range of Motion the general number of solutions to reduce.
There exist open-chain manipulators with
General Mechanism with 6 d.o.f. 16, 14, 12, 10, 8, 6, 4, 2 solutions.
Number of solutions < 16

For a given set of 6 lengths of the legs PUMA 560

General in-parallel structure has
40 configurations
By specializing structure the number of
configurations can be reduced

__Solutions
  
  
 

3
Stanford Scheinman Arm Stanford Scheinman Arm

Stanford Scheinman Arm Solvability

A manipulator is solvable if ALL the

sets of solutions can be determined.
6 d.o.f. open-chain mechanisms are
“now” solvable.
(the general solution is a numerical one)

Closed Form Solutions Closed Form Solutions 0

6 T
{6}
Analytical Solutions - Exist for a large B
class of mechanisms. W T
{0} P0

Sufficient Condition
3 intersecting neighboring axes
(most industrial robots) 0
6 T ( 1 ,  2 , . . . ,  6 )  WB T
Solutions: • Algebraic
• Geometric

4
 Geometric Solutions Algebraic Solutions 0
T  WB T
C 0  3 3
y0
2 First  Fc  s123 0 l1 c1  l2 c12 I c 0 F  s 0 0 x0 I
l2 x0 ,y0) 2 GG s JJ GG JJ
123

D (x0,y0) 0 l1 s1  l2 s12 s 0 c 0
2 l1  l22  2 l1 l2 cos  2  x 02  y 02
{0} 123 c123

0 y0

l l2  GG 0 0 1 0 JJ0 GG 0 1 0 JJ
 1 2 H0 0 0 1 K0 H 0 0 1 K
1  B E ( x 2  y 02 )  (l12  l22 ) l1
cos  2  0 cos( 1   2   3 )  cos  0 UV  
A 2l1 l2 1 2 3  0
x0
  2 and -  2
sin( 1   2   3 )  sin  0 W 1

For 1 and 2: l1 c1  l 2 c12  x 0

 l22  l12  ( x 02  y 02 )  2 l1 x 02  y 02 cos  l1 s1  l 2 s12  y 0
Solution if (x0, y0) is in the workspace
x  y l l
2 2 2 2
y0
cos   0 0 1 2
and tan   1  cos  2 
( x 02  y 02 )  (l12  l22 )
1
2l1 x  y 2
0
2
0
x0 2l1l2

 1      3   0  ( 1   2 )   2  A tan 2(  1  cos 2  2 , cos  2 )

Algebraic Solutions Pieper’s Solution

l1 c1  l2 c12  x 0
{3}
 3 l1 s1  l2 s12  y 0
{2}
For 1: {4}
{0} (x0,y0)
UV
(l1  l2 c 2 )c1  (l2 s 2 )s1  x 0 {0} {1}
(l  l c )s  (l s )c  y W
l2 
2 1 2 2 1 2 2 1 0
l1 k c k s  x U
1 => k s  k c  y V P 1T( 1 ) .1 P
1 1 2 1 0 0 0

W P T( 2 ) . 2 P
LMc g  s g OP
1

LM g OP
1 1 2 1 0 1
r  k12  k 22
     
FG k
1  r.cos  IJ 1
2
1 1 1 2
( k1 , k 2 )
Hk K
PM
MM g PPP
 r.sin  s g c g
P  M P ; g  g (c , s , f )
tan   k 2 / k1 2 g 2 0 1 1 1 2

MMg PP
1
x 0  r.cos( 1   )
i i 2 2 i
3 3
=> y0  r.sin( 1   )
=>  1  A tan 2 ( y 0 , x 0 )  A tan 2 ( k 2 , k1 )
N1Q N 1 Q

Pieper’s Solution Pieper’s Solution

{3} {3}
{2} {4} {2} {4}
{0} {1} {0} {1}

0
P  P0
LM a OP 3 LM f ( )OP
1 3
LMc g  s g OP LMx OP For 1: c1 g1  s1 g 2  x 0 UV
1 1 1 2 0
1
PM
MM c . d PPP PM
MM f ( )PPP
 s . d f ( )
3 3 4 2
P T( 3 ) . P
2
3
3 2 2 3
MMs g  c g PP  MM y PP
1 1 1 2 0
s1 g1  c1 g2  y 0 W
N 1 Q
3 4

N 1 Q
3 3
N g Q Nz Q3 0
if g1 and g 2 are known

 1 = Atan2(y 0 , x 0 )  Atan2(g 2 , g1 )

5
 Pieper’s Solution Pieper’s Solution
{3} {3}
{2} {4} {2} {4}
{0} {1} {0} {1}
LMc g  s g OP Lx O
MMs g  c g PP  MMM y PPP
1 1 1 2 0

1 1 1 2 0 ( k1 c 2  k 2 s 2 )2 a1  k 3  r02
N g Q Nz Q
3 0 ( k1 s 2  k 2 c 2 )s 1  k 4  z 0
For 2:
RSg2
1  g 22  g32  x 02  y 02  z 02  r02 For 3:
T g3  z 0 ( r02  k 3 ) 2 . s 2  1  ( Z 0  k 4 ) 2 .4. a12  4. a12 . s 2  1 ( k12  k 22 )
( k1 c 2  k 2 s 2 )2 a1  k 3  r02
g i  g i (c 2 , s 2 , f1 , f 2 , f 3 ) (k s  k c )s  k  z
1 2 2 2 1 4 0
k i  k i ( f i ( c 3 , s 3 ))
k i  k i ( f1 , f2 , f3 )   2 if ki are known

Transcendental Equations For and 

Reduction to Polynomial
R|cos  1  u 2
0
6 R (  )  R0
 R( )  10 R( 1 ). 21 R( 2 ). 32 R( 3 ). 43 R( 4 ). 54 R( 5 ). 65 R( 6 )
u  tan  S
0
1 u 2
6
2 |Tsin   1 2uu
R( 4 )  43R| 4  0 . RZ ( 4 )
2 3
4
For 3: k i  k i ( u, u 2 )
[ 40 R| 4  0 ( 1 ,  2 ,  3 )].[ R Z ( 4 ). 64 R( 5 ,  6 )]  R0
A. u  B. u  C. u  D. u  E  0
4 3 2
is known
R
3 R( 4 ,  5 ,  6 )  R0
with u  tan
2
Euler Angle Solution