Movie Segment

Locomotion Gates with Polypod,

Mark Yim, Stanford University,
ICRA 1994 video proceedings

Differential Motion

{n}
{n}

{0}

Instantaneous

Kinematics

Forward Kinematics

Instantaneous Kinematics

+ x + x

Relationship: x

 x

Jacobian
Differential Motion
Linear & Angular Motion
Velocity Propagation
Explicit Form
Static Forces

Linear Velocity
Angular Velocity

Joint Coordinates
revolute
coordinate i : i
d i prismatic
Joint coordinate-i: qi = i i + i d i

0 revolute
1 prismatic

with

i =

and

i = 1 i

Joint Coordinate Vector:

q = (q1q2 ....qn )T

Jacobians: Direct Differentiation

x 1

F x I F f (q ) I
G J G J
x = f (q); G x J = G f (q) J
GG # JJ GG # JJ
H x K H f (q)K
f
f
LM f
=
q +"+
q
q
q
M q
x = M #
MM f
f
f
=
q +"+
q
q
q
N q
1

x m

#
"

OP
P
# P. q
f P
q PQ
f1
q n

"

x( m1) = J ( mn ) (q ) q( n1)

where

J ij (q) =

fi (q)
q j

Jacobian
x( m1) = J ( mn ) (q ) q( n1)

x( m1) = J ( mn ) (q ) q( n1)

Example
l1

Stanford Scheinman Arm

x = l1 c1 + l2 c12

(x,y)

l2

y = l1 s1 + l2 s12

x = (l1 s1 + l2 s12 ) 1 l2 s12 2

y = (l1 c1 + l2 c12 ) 1 + l2 c12 2

LMx OP = LM y l s OPFG IJ
Ny Q N x l c QH K
F x x I
x = J ( )
G JJ = y
JG

GH y y JK x
x = J ( )
X =

2 12

2 12

l2 s12
l2c12

d2
{0} z
0
1

y0
x0

d2

d3

{0} z
0
1

4 5

x6

y6

d3
y0
x0

4 5

z6

i-1

ai-1

di

1
2
3
4
5
6

0
-90
90
0
-90
90

0
0
0
0
0
0

0
d2
d3
0
0
0

x6

y6
z6

1
2
0
4
5
6

C 1 S 2 d 3 S1 d 2
S1 S 2 d 3 + C1 d 2

xP

r1
x = r2 =

r3

C2 d3
C 1 [ C 2 ( C 4 C 5 C 6 S 4 S 6 ) S 2 S 5 C 6 ] S1 ( S 4 C 5 C 6 + C 4 S 6 )
S1 [ C 2 ( C 4 C 5 C 6 S 4 S 6 ) S 2 S 5 C 6 ] + C 1 ( S 4 C 5 C 6 + C 4 S 6 )
S 2 ( C 4 C 5 C 6 S 4 S 6 ) C 2 S5 C 6
C 1 [ C 2 ( C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] S1 ( S 4 C 5 S 6 + C 4 C 6 )
S1 [ C 2 ( C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] + C 1 ( S 4 C 5 S 6 + C 4 C 6 )
S2 (C 4 C 5 S6 + S4 C 6 ) + C 2 S5 S6

C1 ( C 2 C 4 S 5 + S 2 C 5 ) S1 S 4 S 5
S1 ( C 2 C 4 S 5 + S 2 C 5 ) + C 1 S 4 S 5
S2 C 4 S5 + C 2 C 5

Stanford Scheinman Arm

LM
MM
N

c1 s 2 d 3 s1 d 2
x p = s1 s2 d 3 + c1 d 2
c2 d 3

Position

x p

F xI LM y
= G y J = M x
GH zJK M 0
N

Orientation: Direction Cosines

OP
PP
Q

LM q OP
q
0OM P
q P
M
0 PP M P

q
0 PQ M P
MMq PP
Nq Q
1

c1 c2 d 3
s1 c 2 d 3

c1 s2
s1 s2

0 0
0 0

s2 d 3

c2

0 0

F r I
= G r J
GH r JK

x R

x p(31) = J xp (36) (q) q(6x1)

F r
GG q
r
=G
GG q
GH qr

( 9 x1)

3
1

xR

x R = J X R ( q )q

LMr (q)OP
= Mr ( q )P
MNr (q)PQ
2

r1
q 6
r2
"
q 6
r3
"
q 6
"

I
JJ
JJ
JJ
K

F q I
GG q JJ
GG # JJ
H q K
1
2

( 6 x1)

(9 x6)

Linear Velocity V

C 1 [ C 2 ( C 4 C 5 C 6 S 4 S 6 ) S 2 S 5 C 6 ] S1 ( S 4 C 5 C 6 + C 4 S 6 )

Representations

S1 [ C 2 ( C 4 C 5 C 6 S 4 S 6 ) S 2 S 5 C 6 ] + C 1 ( S 4 C 5 C 6 + C 4 S 6 )

S2 (C 4 C 5 C 6 S4 S6 ) C 2 S5 C 6

xR =

C 1 [ C 2 ( C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] S1 ( S 4 C 5 S 6 + C 4 C 6 )
S1 [ C 2 ( C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] + C 1 ( S 4 C 5 S 6 + C 4 C 6 )
S2 (C 4 C 5 S6 + S 4 C 6 ) + C 2 S5 S6

S1 ( C 2 C 4 S 5 + S 2 C 5 ) + C 1 S 4 S 5

F r I
= G r J
GH r JK

1
1

x R

F r
GG q
r
=G
GG q
GH qr

S2 C 4 S5 + C 2 C 5

( 9 x1)

3
1

Jacobian for X
x P = J X P ( q )q
x R = J X R ( q )q

r1
q 6
r2
"
q 6
r3
"
q 6
"

I
JJ
JJ
JJ
K

Cartesian
Spherical
Cylindrical
.

x O
L
X=M P
Nx Q

C 1 ( C 2 C 4 S 5 + S 2 C 5 ) S1 S 4 S 5

F q I
GG q JJ
GG # JJ
H q K

Euler Angles
Direction Cosines
Euler Parameters

1
2

( 6 x1)

(9 x6)

FG IJ FG
H K H

IJ
K

J XP (q)
x P
=
q
J XR (q)
x R

Cartesian & Direction Cosines

X (12 x1) = J X ( q ) (12 x 6 ) q ( 6 x1)

The Jacobian is dependent on the representation

Basic Jacobian
linear velocity

{0}

FG v IJ
H K

angular velocity

= J 0 (q) ( 6 xn ) q ( nx1)
(6 x1)

x P = E P ( x P )v
x R = E R ( x R )

Examples

*
*

xR

xP

Jacobian for X

F s . c
F I
G s
= G J ; E ( x ) = G c
GG s
GH JK
H s
F xI
F 1 0 0I
G
J
= y ; E ( x ) = G 0 1 0J
GH z JK
GH 0 0 1JK
R

c . c
s
s
c

I
1
J
0J
J
0J
K

Given a representation

x = J x ( q ) q

J x (q) = E ( x) J 0 (q)
Basic Jacobian

= J 0 ( q ) q

Jv

= J q

= J v q
= J q

x P = E P . v x P = ( E P . J v )q
x R = E R . x R = ( E R . J )q

RS J
TJ

XP
XR

v
= J 0 (q ) q
w

Jacobian and Basic Jacobian

v

x
x = P
xR

0 Jv

ER J w

J
J = XP
J XR

EP
=
0

J (q)

E( X )

v

w

J 0 (q ) q

J 0 (q)

With Cartesian Coordinates

= EP . Jv
= E R . J

I 0
EP = I 3 ; J XP = J v ; and E =

0 ER

Position Representations
Cartesian Coordinates ( x, y , z )

Spherical Coordinates ( , , )
Using

EP ( X ) = I 3

( x y z )T = ( cos sin sin sin cos )T

Cylindrical Coordinates ( , , z )

cos sin
sin sin
cos

EP ( X ) = sin
cos
0
( sin )
( sin )

cos cos
sin cos
sin

Using ( x

y z )T = ( cos sin z )T

cos

EP ( X ) = sin

cos
0

0
1
sin

Jacobian for X

Euler Angles
s .c
s

x R = ; E R ( x R ) = c

s
s

c .c
s
s

c
s

Singularity of the representation

for = k

Jacobian

x
x = P
xR

x = J x ( q ) q

J x (q) = E ( x) J 0 (q)
Basic Jacobian

v
= J 0 (q ) q
w

Linear & Angular Velocities

linear velocity

{0}

angular velocity

FG v IJ
H K

Given a representation

{0}

= J (q) ( 6 xn ) q ( nx1)

linear velocity
angular velocity

( 6 x1)

Linear Velocity
vP/ A

Pure Translation

{B}
B

{A}

vP/ A

{A}

vP/ A
{B}

vP/ A

v A/ B

{C}
C

vP/ A

Pure Translation
{A}

vP/ B

Rotational Motion
vP/ A
Axis of rotation

v A/ B

{B}

fixed points on the rigid body

rigid body

v P/ B = v A/ B + v P/ A

Rotational Motion

Rotational Motion

Angular Velocity

Angular Velocity

vR

Rotational Motion

Rotational Motion

Angular Velocity
vP
vR

Angular Velocity

vP = ?

vP
vR

fixed point

Rotational Motion

Rotational Motion

Angular Velocity

Angular Velocity
Psin

vP
vR

||||
||Psin||
and
vP _|
vP _| P

vP

vR

vP is proportional to:

fixed point

fixed point

vP = P

Cross Product Operator

ax
bx

a = ay , b = by
az
bz

Cross Product Operator

 : a skew-symmetric matrix

matrices

vectors

a a

LM
MM
N

c = ab

Simultaneous linear and angular motion

vP/ B

{B}

P=
vP =

LM
MM
N

LM 0
MM
N

z
0

z
y

OP
PP
Q
y

OP LP O
PP MM PP
Q MN P PQ
x

x . Py
0

P
vP =

Spatial Mechanisms
{2}
{1}

v B/ A

OP
PP
Q

x
Px
= y ; P = Py
z
Pz

: a skew-symmetric matrix

0 az ay bx

= az
c = ab
0 ax by
ay ax
0 bz

{A}

P
vP = P vP =

c = a b c = ab

{n}
{0}

v P / A = v B / A + v P / B + PB
A

v P / A = A v B / A + BA R. B v P / B + A B BA R. B PB

Propagation of velocities
x

v : linear velocity
: angular velocity

x = J ( ).

Velocity propagation

Velocity propagation
{i+1} zi+1

{i} zi

+1

i
vi

Joint 1
v1 and 1 in frame {1}
Joint i+1
i +1
i +1 = i +i1R. i i +  i +1 . i +1 Z i +1

i+1
vi+1

Pi+1

i +1

v i +1 = v i + i Pi +1 + d i +1 . Z i +1

Linear

n n

i +1 = i + i +1

Angular

i +1

= 

v i +1 = i +i1R.( i v i + i i i Pi +1 ) + di +1 . i +1 Z i +1

FG v IJ = FG R 0 IJ.FG v IJ
H K H 0 RK H K
0

i +1

. Z i +1

P3

{0}

 3

l2
P2

l1

 1

 2

P1

LM 0
= 0 + M
MN 0

v P2

v P3

LM0
= 0
MM1
N

v P1 = 0

v P2 = v P1 + 1 P2

v P3 = v P2 + 2 P3

LM(l s + l s
= l c +l c
MM 0
N
0 0 O L O
M P
0 0 P. M P
P
1 1 QP MN PQ
1 1

2 12

OP L
PP MM
Q MN

OP
PP
Q

LM
MM
N

) l2 s12
l2 c12
0

OP
PP
Q

v P3

LM0 1 0OP
+ 1 0 0 .( + 
MM0 0 0PP
N
Q
LMl . s OP
+ l . c .( +  )
MM 0 PP
N Q
1

). 0 P3

12

12

LMl . c
MMl .0s
N
2

12

12

OP
PP
Q

3 = ( 1 +  2 +  3 ). 0 Z 0

OP LM OP
P0P MM PP
QN Q
0
0 .

1
2

Jv

Movie Segment

LMl . s OP
= l . c .
MM 0 PP
N Q
LMl . s OP
= l . c .
MM 0 PP
N Q
1

v P3 = 0 v P2 + 0 2 0 P3

0 l1 . c1
l1 . s1
0 . l1 . s1 = l1 . c1 . 1
0
0
0

2 12

1 1

1 =  1 . 0 Z1

 1
0
0

v i +1 = v i + i Pi +1

{n}

0
n

l3

{n-1}

0
n

Example

n
and v n


FG v IJ = J. FG IJ
H K GGH JJK
1
2

Beach Volleyball, Toshiba,

ICRA 1999 video proceedings

The Jacobian (EXPLICIT FORM)

i

Vj

The Jacobian (EXPLICIT FORM)

i

Vj
Prismatic

Revolute

Vj

i Pin

Linear Vel:
Angular Vel:

none

Effector Linear Velocity

Prismatic Joint

Vi = Z i qi

i Pin

Vj
Effector

Prismatic

Revolute

Vj

i Pin

Linear Vel:
Angular Vel:

none

Effector Linear Velocity

Vi = Zi qi

i =1

Effector Angular Velocity

i Pin

Pi n

Vj

v = [i Vi + i ( i Pin )]

Vj

v = [i Z i + i ( Z i Pin )] qi
i =1

Vi = Zi qi

Effector Angular Velocity

= i i

i = Zi qi

i =1

(
i =1

Z i ) q i

The Jacobian

v = [1 Z1 + 1 ( Z1 P1n )]q1 + "

+ [n1 Z n1 + n1 (Z n1 P( n1) n )]qn1 + n Z n qn

q1
q
v = [1 Z1 + 1 (Z1 P1n ) 2 Z2 + 2 (Z2 P2n ) "] 2
#

qn
v = J v q

=1 Z1q1 +2 Z2q2 +"+n Znqn q1

q
2
#

q n

= [1 Z1 2 Z 2 " n Z n ]

= J q

Pi n
Effector

i = Z i qi

The Jacobian (EXPLICIT FORM)

Revolute Joint

Matrix J v

F xI
v = G y J = x
GH zJK

i = Zi qi

J
J = v
Jw

(direct differentiation)

x P
x
x
. q1 + P . q 2 +"+ P . q n
q1
q 2
q n

x xP
xP
Jv = P
"

qn
q1 q2

Jacobian in a Frame

J in Frame {0}

Vector Representation

J=

F x
GG q
H . Z

x P
q 2

P
1

2 . Z 2

"
"

I
JJ
.Z K
x P
q n
n

In {0}

F x
J = G q
GH . Z
0

0 xP
q 2

2 . 0 Z 2

I
JJ
. Z K

0 xP
q n

"
"

F
GG
H

0
Zi = R Zi ; Zi = Z = 0
1
0
i

I
JJ
.( R. Z )K

0
0
( xP ) "
( xP )
q 2
q n

(0 xP )
J = q1
1 .( 10 R. Z )

2 .( 20 R. Z ) "

0
n

The Jacobian

Stanford Scheinman Arm

Vj

Pi n

J
J = v
Jw

v = J v q

= J q

Jv = [1 Z1 + 1 (Z1 P1n ) 2 Z2 + 2 (Z2 P2n ) "]

J = [1 Z1 2 Z 2

d2

d3
Z0

Y0

" n Z n ]

Z2

d2

X6

d3

Z0 Z1

Y6

X0

X1 X

Z6

X
X5 6

X2

Z5
Z3 Z4
Z6 X3 X

F
J = GZ P
GH Z
1

13

Z2 P23

Z3

Z2

Z4

Z5

Z6

I
JJ
K

i-1

ai-1

di

1
2
3
4
5
6

0
-90
90
0
-90
90

0
0
0
0
0
0

0
d2
d3
0
0
0

1
2
0
4
5
6

10

Stanford Scheinman Arm

2

d2
{0} z
1 0

d3
y0
x0

4 5

x6

y6
z6

i-1

ai-1

di

1
2
3
4
5
6

0
-90
90
0
-90
90

0
0
0
0
0
0

0
d2
d3
0
0
0

LMc
s
T=M
MM 0
N0

s1
c1
0
0

1
2
0
4
5
6

0
1

1
2

i-1
i

T=

ci
si ci-1
si si-1
0

-si
ci ci-1
ci si-1
0
0

Forward Kinematics:

3
4

LMc
s
T=M
MM 0
N0

s4

c4

O
0 0P
P
1 0P
0 1 PQ
LM c
0
T=M
MMs
N0

0
-si-1
ci-1
0

T =

ai-1
-s i-1 di
ci-1 di
1

T ...

LMc
s
T=M
MM 0
N0

4
5

OP
P
0P
1 PQ

s5
0

0 0
1 0

c5
0

0
0

0
2

5
6

OP
P
0P
1 PQ

s6
0

0 0
1 0

c6
0

0
0

c1c2c4 s1s4 c1c2 s4 s1c4 c1s2 c1d3s2 s1d2

s c c + c s s c s + c c s s s d s + c d
1 2 4
1 4
1 2
1 3 2
1 2
0
12 4 14
4T =
s2c4

s2 s4
c2
d3c2

0
0
0
1

X X c1 c2 s 4 s1 c 4 c1 d 3 s 2 s1 d 2

LM
X
T=M
MM X
N0
LM X X
X X
T=M
MM X X
N0 0
0
5

0
6

s1 c2 s 4 + c1 c 4

s2 s 4

O
s d s +c d P
PP
dc
PQ
1
1

3 2

s2

1 d2

c2
0

0
0

s1

0 0

c1

0 0

0
0

1
0

1 2

c1 s2

1 2

c1

c1 d 2

c2
0

0
0

0
1

s1
c1
0
0

c1 s 2
s1 s2
c2
0

1 2

LMX X cc c s ss s +cs s
X X sc c s +cs s +ss c
T =M
MMX X s c s +cc
0
N0 0
1 2 4 5

14 5

12 5

1 2 4 5

14 5

12 5

2 4 5

5 2

cds
1 3 2 sd
1 2
sds
1 3 2 +cd
1 2
dc
3 2

c1 d 3 s 2 s1 d 2
s1 d 3 s 2 + c1 d 2

s 2 c 4 s5 + c5 c2

d3 c2

OP
PP
PQ

d2

Z0

OP
P
0 P
1 PQ
0

0
0

OP
PP
PQ

c1 d 3 s 2 s1 d 2
s1 d 3 s2 + c1 d 2
d3 c2
1

OP
PP
PQ

d3
Y0

X6

Y6

X0
Z6

C 1 S 2 d 3 S1 d 2
S1 S 2 d 3 + C 1 d 2

3 2

c1c 2 c 4 s 5 s1 s 4 s 5 + c1 s 2 s 5
s1c 2 c 4 s 5 + c1 s 4 s 5 + s1 s 2 c 5

1
0

s1 s 2

LMc c
sc
T=M
MM s
N0

0 1 d 3

s1 d 2

0
6

s1

1 2

0
3

LM1
0
T=M
MM0
N0

OP
P
0P
1 PQ

LMc c
sc
T=M
MM s
N0

OP
P
0P
1 PQ
0

LMc
0
T=M
MMs
N0

OP
PP
PQ

0
1

LM
0
T=M
MMs
N0

0
0
0
1

2
3

N-1

0 0

0
0
1
0
c2

OP
PP
PQ

xP

r1
x = r2 =

r3

C2 d3
C 1 [ C 2 ( C 4 C 5 C 6 S 4 S 6 ) S 2 S 5 C 6 ] S1 ( S 4 C 5 C 6 + C 4 S 6 )
S1 [ C 2 ( C 4 C 5 C 6 S 4 S 6 ) S 2 S 5 C 6 ] + C 1 ( S 4 C 5 C 6 + C 4 S 6 )

S2 (C 4 C 5 C 6 S4 S6 ) C 2 S5 C 6
C 1 [ C 2 ( C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] S1 ( S 4 C 5 S 6 + C 4 C 6 )
S1 [ C 2 ( C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] + C 1 ( S 4 C 5 S 6 + C 4 C 6 )
S2 (C 4 C 5 S6 + S 4 C 6 ) + C 2 S5 S6

C1 ( C 2 C 4 S 5 + S 2 C 5 ) S1 S 4 S 5
S1 ( C 2 C 4 S 5 + S 2 C 5 ) + C 1 S 4 S 5
S2 C 4 S5 + C 2 C 5

11

Stanford Scheinman Arm Jacobian

0 xP

0
J = q1
0Z
1

LMc d
MMs d
MM
MM
N
1

0 xP
q2
0

0 xP
q3

0
0

Z2

Z4

s1 s2 d 3
+ c1 s 2 d 3

c1 c 2 d 3
s1 c 2 d 3

c1 s2
s1 s 2

0
0

0
0

0
0
0
1

s2 d 3
s1
c1
0

c2
0
0
0

0
c1 s2
s1 s 2
c2

0
c1 c 2 s 4 s1 c 4
s1 c 2 s 4 + c1 c 4
s2 s4

0
Z6

0
0

Z5

OP
P
0
PP
c c c s s s s +c s c P
s c c s +c s s +s s c P
PQ
s c s + c c
0
0

1 2 4 5

1 4 5

1 2 5

1 2 4 5

1 4 5

1 2 5

2 4 5

5 2

Movie Segment
Automatic Parallel Parking, INRIA,
ICRA 1999 video proceedings

Kinematic Singularity
The Effector Locality loses the ability
to move in a direction or to rotate about
a direction - singular direction

J = ( J1 J 2 " J n )

det ( J ) = 0

( )

det i J = det

Kinematic Singularity
B

BR
J = A
0

0 A
J
B
R
A

det[ B J ] det[ A J ]

( )

det i J = det

( J)
j

( J)
j

Singular Configurations

det[ J ( q )] = 0

Singular Configurations

det[ J (q)] = S1 (q)S2 (q)...Ss (q) = 0

S1 (q) = 0
S2 (q) = 0
#
S s (q) = 0

12

Example (Kinematic Singularities)

(x,y)

y
{1}

l1S1 + l 2 S12
J =
l1C1 + l 2C12

det ( J ) = l1l 2 S 2

l2 2
{0}
y
{1}

l 2 S12

l 2C12

At Singularity

q1

Small Displacements

q1

1
l1

1

l1

(x,y)

x(1)

q2

l2 2
{0}
y
{1}

y(1)

q1

q1

x
X =
y
q2

q2

q1

Kinematic Singularities (reduced matrix)

l1

q2

q, X

x(1) 1 y(1)
+
l1 2
l1
( l1 + l 2 ) x(1) 1 + y(1)
q2 =
l1l 2
l1
2

y(1)

{1}

l2

q1 =

q1

0
J =
l1 + l 2

+ l2 ) 1 + l2 2

{1}

q = J 1X

1
l
1 2
1
J (1)
l1 + l 2

l1l 2 2

small 2

q2

q1

LM x = 0
MM y = (l
N

x
X =
y

J =10 R 0 J

C1 S1 l 2 S 2 l 2 S 2
J =

S1 C1 l1 + l 2C 2 l 2C 2

q, X
q2

{1}

l1

Singularity at q2 = k

Small Displacements

(x,y)

y = l 1S1 + l 2 S12

l1
1

(x,y)

x = l1C1 + l 2C12

l2 2
{0}

Example (Kinematic Singularities)

(l1 + l 2 ) x(1)
l2

y(1)

q2

y(1)

l1

l1
1

det ( J ) = l1l 2 S 2

l1S1 + l 2 S12

l1C1 + l 2C12
J =
0

l1S1 + l 2 S12
J =
l1C1 + l 2C12

l 2 S12

l 2C12

0
0

l 2 S12

l 2C12

Singularity at q2 = k

13

 l1s1 l2s12 l3s123 l2s12 l3s123 l3s123

JE = l1c1 +l2c12 +l3c123 l2c12 +l3c123 l3c123

E

1
1
1

Stanford Scheinman Arm

l3

l2

LMl s l s l s
MM l c + l c0 + l c
=M
MM 0
MN 01
1 1

l1

2 12

1 1

JE

l2 s12 l3s123 l3s123

3 123

2 12

3 123

l2c12 + l3c123

l3c123

0
1

0
1

OP
PP
PP
PP
Q

Stanford Scheinman Arm Jacobian

2

d2
{0} z
1 0

d3
y0
x0

4 5

x6

y6
z6

i-1
1

T=

ci
si ci-1
si si-1
0

Forward Kinematics:

-si
ci ci-1
ci si-1
0
0
N

T =

i-1

ai-1

di

1
2
3
4
5
6

0
-90
90
0
-90
90

0
0
0
0
0
0

0
d2
d3
0
0
0

0
-si-1
ci-1
0

1
2
0
4
5
6

ai-1
-s i-1 di
ci-1 di
1

T ...

N-1
N

0 xP

0
J = q1
0Z
1

LMc d
MMs d
MM
MM
N
1

0 xP
q2
0

0 xP
q3

0
0

Z2

Z4

s1 s2 d 3
+ c1 s 2 d 3

c1 c 2 d 3
s1 c 2 d 3

c1 s2
s1 s 2

0
0

0
0

0
0
0
1

s2 d 3
s1
c1
0

c2
0
0
0

0
c1 s2
s1 s 2
c2

0
c1 c 2 s 4 s1 c 4
s1 c 2 s 4 + c1 c 4
s2 s4

0
Z6

0
Z5

OP
P
0
PP
c c c s s s s +c s c P
s c c s +c s s +s s c P
PQ
s c s + c c
0
0

1 2 4 5

1 4 5

1 2 5

1 2 4 5

1 4 5

1 2 5

2 4 5

5 2

Stanford Scheinman Arm Jacobian

Jacobian at the End-Effector

{e}

5 = k
0
0
c1d2 s1s2d3 c1c2d3 c1s2 0
s d + c s d s c d s s 0
0
0
1 2 12 3 12 3 12

s2d3 c2
0
0
0
0
J =

s
c
s

c
c
s

s
c
c
0
0
1
1 2
1 2 4
1 4
1s2

c1
0
0 s1s2 s1c2s4 + c1c4 s1s2

s2s4
c2
1
0
0 c2

{n}

vn

ve

Pne

v e = v n + n Pne

RSv = v P
T =
e

ne

14

RS v = v P
T =
FG v IJ = FG P IJ FG v IJ
H K H K H K
e

ne

Cross Product Operator (in diff. frames)

n
{n} vn

ne

P

J e q = ne J n q

Je

F I
= G J J
H K
Pne

FR
J =G
H0
i
j

0R
J e = n
0

l2
l1

LMl s l s
MM l c +0l c
=M
MM 0
MN 10
1 1

1 1

JW

0
0
0
1

P = n0 R n P

I
JK

l3

0 j
J
i
R
j

FI
GH 0

0 Pne
I

I
JK

Jn

0
n

RT

Wrist Point
x = l1 c1 + l2 c12
y = l1 s1 + l2 s12

l2

End-Effector Point
x = l1 c1 + l2 c12 + l3 c123

l1

y = l1 s1 + l2 s12 + l3 s123

E
l3

OP
P
0P
0 PP
0P
P
1Q
0
0

; JE

FI
=G
H0

0 PWE
I

I
JK

y = l1 s1 + l2 s12

End-Effector Point
x = l1 c1 + l2 c12 + l3 c123

l1

1 1

JW
JW

y = l1 s1 + l2 s12 + l3 s123

LMl s l s
MM l c +0l c
=M
MM 0
MN 01
1 1

Wrist Point
x = l1 c1 + l2 c12

l2

End-Effector Point
x = l1 c1 + l2 c12 + l3 c123
Jacobian (W)
y = l1 s1 + l2 s12 + l3 s123

2 12

Je =

y = l1 s1 + l2 s12

l2 s12
l2 c12

Wrist Point
x = l1 c1 + l2 c12

2 12

Pne

ve

n0 R n Pne n0 RT n
J n
0
R
n

E
l3

P
0
0
P = ( n0 R. n P) n0 R. n P
P n0 R n P ;
0
P 0 = n0 R.( n P n )
0  0
P. = n0 R.( n P . n ) = n0 R.( n P . n0 R T . 0 )

FG IJ
H K

{e}

2 12

2 12

l2 s12
l2 c12
0
0
0
1

OP
PP
PP
0P
P
1Q
0
0
0
0

LM l s l s
MM l c + l c0
=M
0
MM
0
MN
1
1 1

1 1

JE

2 12

2 12

l3 s123

l 2 s12 l3 s123

l3 s123

+ l3 c123

l 2 c12 + l3 c123

l3 c123

OP
PP
PP
PP
Q

15

Wrist Point
x = l1 c1 + l2 c12

l3

End-Effector Point
x = l1 c1 + l2 c12 + l3 c123

l1
1 1

1 1

JW

l2 s12
l2 c12

2 12

2 12

0
0
0
1

Ll c
= Ml s
MM 0
N

3 123

PWE

x = J ( )

y = l1 s1 + l2 s12

l2

LMl s l s
MM l c +0l c
=M
MM 0
MN 10

Resolved Motion Rate Control (Whitney 72)

3 123

JE =

OP
PP P
Q
0

WE

q1

J -1

= J 1 ( )x

y = l1 s1 + l2 s12 + l3 s123

OP
P
0P
0 PP
0P
P
1Q
0
0

FI
GH 0

F 0
=G 0
GH l s

0 PWE
I

3 123

I
JK

Arm at Configuration
0

q2

Control

l3 s123

0
l3 c123

l3 c123
0

Control

Joint 1

I
JJ
K

Joint 2

= J 1x

+ = +

Jacobian
Differential Motion

q1

q2

Linear & Angular Motion

Velocity Propagation

qn

Control

Joint n

qn

Explicit Form
Static Forces

Forward
Kinematics

Angular/Linear Velocities/Forces

Angular/Linear Velocities/Forces

v = p
p

x = f ( )
x = x d x

JW

Resolved Motion Rate Control

xd
x

Outside singularities

v
F

= p F

v
y
x

p
z

v = p

v = p
vx p y 
=

v y px

v = J 

z
y
x

= p F
= p F

= ( p )T F
F
= ( p p )
F
x

=J F
T

16

x = J 

= JT F

2
1

Velocity/Force Duality

Propagation
Elimination of
Internal forces

3
2

Virtual Work
Static Equilibrium

2
1

-n1
-f1

-fi+1

-fi+1

-ni+1

Link i

Link i
Pi+1

ni

fi

ni

-n
link 3

-f
n3

Energy Analysis

f1

n2
-f2

f3

-f3

-n3
link 2
f2

-n2
n1

link 1

-ni+1

Static Equilibrium
Pi+1 forces = 0
moments / a point = 0
About origin {i}
fi
f i + ( f i +1 ) = 0
ni + ( ni +1 ) + Pi +1 ( f i +1 ) = 0
fi = fi +1

ni = ni +1 + Pi +1 fi +1

17

f Virtual Work Principal

fi

ni

zi

Prismatic Joint

F=

zi

1
= 2

3
2

Revolute Joint

i = fi T Z i

i = ni T Z i

Algorithm

fn = nf

nn = n + Pn +1 f

fi = R. fi +1

ni = i +1i R. i +1 ni +1 + i Pi +1 i fi

i
i +1

T q = F T x using x = J q

l1 = l2 = 1; 1 = 0; 2 = 60D

Example (Static Forces)

l 2 S12

l1C1 + l 2C12 0
l 1C1 + l 2C12
3 / 2
=
=

l
C
12
l 2C12 1
1/ 2
2

1000N

1000N

l2

l2 2

l 2 S12

(x,y)

(x,y)

l1S1 + l 2 S12

l 1S1 + l 2 S12

= JT F

= JT F

virtual
displacements

Example (Static Forces)

( l1S1 + l 2 S12 ) l 2 S12
(x,y)

J =
l1C1 + l 2C12
l 2C12

l2 2
( l1S1 + l 2 S12 ) l1C1 + l 2C12
{0}

JT =
l
y
1

1N
l 2 S12
l 2C12

1
{1}

=J F

i
applied
forces

T q + (F )T x = 0

T = FT J

l1

w = f i xi

Static Equilibrium:
If the virtual work done by applied
forces is zero in displacements
consistent with constraints

i +1

x = J 

y
{1}

Internal
forces are
workless

Velocity/Force Duality

{0}

FG f IJ
H nK

= JT F

y
{1}

l1C1 + l 2C12 0 l1C1 + l 2C12

0

(1K ) =
= l C12

1
K
0
l 2C12
2

l1 = l2 = 1; 1 = 90; 2 = 0D

18