Movie Segment

Locomotion Gates with Polypod,

Mark Yim, Stanford University,
ICRA 1994 video proceedings

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

Differential Motion {n}

{n}
Joint Coordinates
⎧ θ revolute
coordinate − i : ⎨ i
{0} ⎩ d i prismatic
Forward Kinematics
θ →x Joint coordinate-i: qi = ε iθ i + ε i d i
Instantaneous Kinematics ⎧ 0 revolute
with εi = ⎨
θ + δθ → x + δ x ⎩1 prismatic
Relationship: δθ ↔ δx and εi = 1 − εi
θ ↔ x
Linear Velocity
Angular Velocity Joint Coordinate Vector: q = (q1q2 ....qn )T

1
Jacobians: Direct Differentiation
F x I F f (q ) I Jacobian
G J G J
1 1

x = f (q); G x J = G f (q) J
δ x( m×1) = J ( m×n ) (q ) δ q( n×1)
2 2

GG # JJ GG # JJ
H x K H f (q)K m m
x( m×1) = J ( m×n ) (q ) q( n×1)
δx 1 =
∂f1
δq +"+
∂f
δq 1 LM ∂f 1
"
∂f1 OP
∂q ∂q
1 n
where
1 n
M ∂q
δx = M #
1 ∂q n
P
# P. δq ∂
# #
J ij (q) = fi (q)
∂f ∂f MM ∂f ∂f P
∂q j
∂q PQ
δx m = δq +"+ δq
m
" m

N ∂q
m m

∂q 1 ∂q
1
n
n
1 n

δ x( m×1) = J ( m×n ) (q ) δ q( n×1)

Example (x,y) x = l1 c1 + l2 c12

Stanford Scheinman Arm
l2
y = l1 s1 + l2 s12
l1
δx = −(l1 s1 + l2 s12 )δθ 1 − l2 s12 δθ 2
δy = (l1 c1 + l2 c12 )δθ 1 + l2 c12 δθ 2
LMδx OP = LM− y −l s OPFG δθ IJ
δX =
2 12 1

Nδy Q N x l c QH δθ K 2 12 2

δ x = J (θ )δθ
F ∂x ∂x I
J≡G
G ∂θ ∂θ JJ = ⎡− y −l2 s12 ⎤
GH ∂θ∂y ∂θ∂y JK ⎢⎣ x l2c12 ⎥⎦
1 2

x = J (θ )θ 
1 2

θ2 θ2
d2 d2
{0} z d3 {0} z d3
θ1 θ1
0 0
y0 y0
x6 x6
x0 y6 x0 y6
θ4 θ5 θ4 θ5
z6 z6

θ6 θ6

C 1 S 2 d 3 − S1 d 2
S1 S 2 d 3 + C1 d 2
i αi-1 ai-1 di θi C2 d3
⎛ xP ⎞
1 0 0 0 θ1 ⎜ ⎟
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 )
2 -90 0 d2 θ2 ⎜ r1 ⎟ − S 2 ( C 4 C 5 C 6 − S 4 S 6 ) − C 2 S5 C 6
3 90 0 d3 0 x = ⎜ r2 ⎟ = 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 )
4 0 0 0 θ4 ⎜ ⎟ 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 )
5 -90 0 0 θ5 ⎜ r3 ⎟ S2 (C 4 C 5 S6 + S4 C 6 ) + C 2 S5 S6
6 90 0 0 θ6 ⎜ ⎟ 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

2
 Stanford Scheinman Arm Orientation: Direction Cosines LMr (q)OP
1

Position LM
c1 s 2 d 3 − s1 d 2 OP xR = Mr ( q )P
MNr (q)PQ
2

MM
x p = s1 s2 d 3 + c1 d 2 PP x R = J X R ( q )q 3
N
c2 d 3 Q LM q OP
1
F ∂r ∂r1 I
0OM P GG ∂q JJ F q I
q
F xI LM− y c1 c2 d 3 c1 s2 0 0
M
2

q P F r I
1
"
∂q 6
= G y J = M x 0 PP M P GG q JJ
1
1

= G r J =G JJ
x p
3

GH zJK M 0 ∂r ∂r2
s1 c 2 d 3 s1 s2 0 0 1

0 PQ M P
q
N x R
GH r JK
2

GG ∂q "
2
− s2 d 3
MMq PP GG # JJ
4
c2 0 0 2
∂q 6
5

Nq Q 3 ( 9 x1)
1

GH ∂∂qr "
∂r3 JJ H q K
K
6 3 6 ( 6 x1)
x p(3×1) = J xp (3×6) (q) q(6x1) 1 ∂q 6 (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 )
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 ) Representations
− 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 •Cartesian

L x O
C 1 ( C 2 C 4 S 5 + S 2 C 5 ) − S1 S 4 S 5 •Spherical
S1 ( C 2 C 4 S 5 + S 2 C 5 ) + C 1 S 4 S 5
•Cylindrical

X=M P
− S2 C 4 S5 + C 2 C 5
P •….

F r I
F ∂r
GG ∂q 1

1
"
∂r1
∂q 6
I
JJ F q I
GG q JJ
1 Nx Q R •Euler Angles
•Direction Cosines

= G r J =G JJ
∂r ∂r2
1
•Euler Parameters
x R
GH r JK
2

GG ∂q "
2
2
∂q 6
JJ GG # JJ
H q K
1
3 ( 9 x1)
GH ∂∂qr 3

1
"
∂r3
∂q 6 K 6 ( 6 x1)

(9 x6)

Jacobian for X Basic Jacobian

x P = J X P ( q )q FG IJ FG
x P
=
J XP (q)
q
IJ linear velocity
x R = J X R ( q )q H K H
x R J XR (q) K {0} v

ω angular velocity

Cartesian & Direction Cosines

FG v IJ = J 0 (q) ( 6 xn ) q ( nx1)
X (12 x1) = J X ( q ) (12 x 6 ) q ( 6 x1) Hω K (6 x1)

x P = E P ( x P )v
The Jacobian is dependent on the representation
x R = E R ( x R )ω

3
Examples Jacobian for X
⎡x ⎤
Fα I F − sα . cβ cα . cβ I Given a representation x = ⎢ P⎥
G sβ sβ
1
J ⎣ xR ⎦
= G β J ; E ( x ) = G cα sα 0J
*
xR
GH γ JK GG sα
R R

−
cα
0J
J x = J x ( q ) q
H sβ sβ K
F xI F 1 0 0I J x (q) = E ( x) J 0 (q)
= y ; E ( x ) = G 0 1 0J
G J
* xP
GH z JK GH 0 0 1JK
P P
⎛v⎞
Basic Jacobian ⎜ ⎟ = J 0 (q ) q
⎝ w⎠

Jacobian and Basic Jacobian ⎛J ⎞ ⎛ EP 0 ⎞ ⎛ Jv ⎞

FG v IJ = J (q). q J = ⎜ XP ⎟=⎜ ⎟⎜ ⎟
Hω K 0
⎝ J XR ⎠ ⎝ 0 ER ⎠ ⎝ J w ⎠
FG v IJ = FG J IJ . q
Hω K H J K
v

ω
J (q) = E( X ) J 0 (q)
RS v = J . q
v

Tω = J . q
ω ⎛v⎞
= J 0 (q ) q
x P = E P . v ⇒ x P = ( E P . J v )q ⎜ ⎟
⎝ w⎠
x R = E R . ω ⇒ x R = ( E R . J ω )q
With Cartesian Coordinates
RS J XP = EP . Jv ⎛I 0 ⎞
EP = I 3 ; J XP = J v ; and E = ⎜ ⎟
TJ XR = E R . Jω
⎝ 0 ER ⎠

Position Representations Spherical Coordinates ( ρ ,θ , φ )

Cartesian Coordinates ( x, y , z ) Using
EP ( X ) = I 3 ( x y z )T = ( ρ cos θ sin φ ρ sin θ sin φ ρ cosθ )T
Cylindrical Coordinates ( ρ ,θ , z ) ⎛ ⎞
⎜ cos θ sin φ sin θ sin φ cos φ ⎟
Using ( x y z )T = ( ρ cos θ ρ sin θ z )T ⎜ ⎟
EP ( X ) = ⎜ − sin θ cos θ 0 ⎟
⎛ cosθ sin θ 0⎞ ⎜ ( ρ sin φ ) ( ρ sin φ ) ⎟
⎜ ⎟ ⎜ ⎟
cosθ cos φ sin θ cos φ − sin φ ⎟
EP ( X ) = ⎜ − sin θ cos θ 0⎟ ⎜ ρ ρ ρ⎠
⎜ ρ ρ ⎟ ⎝
⎜ 1 ⎟⎠
⎝ 0 0

4
 Jacobian for X
Euler Angles ⎡x ⎤
⎛ sα .c β cα .c β ⎞
Given a representation x = ⎢ P⎥
⎛α ⎞ ⎜ − sβ sβ
1⎟ ⎣ xR ⎦
⎜ ⎟ ⎜ ⎟
x R = ⎜ β ⎟ ; E R ( x R ) = ⎜ cα sα 0⎟ x = J x ( q ) q
⎜γ ⎟ ⎜ ⎟
⎝ ⎠ ⎜ sα −
cα
0 ⎟⎟
⎜ sβ sβ J x (q) = E ( x) J 0 (q)
⎝ ⎠
Singularity of the representation ⎛v⎞
for β = kπ Basic Jacobian ⎜ ⎟ = J 0 (q ) q
⎝ w⎠

Jacobian Linear & Angular Velocities

linear velocity
{0} v
linear velocity
ω angular velocity {0} v

FG v IJ = J (q) ( 6 xn ) q ( nx1)
ω angular velocity

Hω K ( 6 x1)

Linear Velocity Pure Translation

{B}
vP/ A B vP/ A {A} vP/ A
{A}
P
AvP/ A {B} v A/ B

{C}
C vP/ A

5
Pure Translation vP/ B Rotational Motion
{A} vP/ A

{B} v A/ B Axis of rotation

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 P

Rotational Motion Rotational Motion

Ω Angular Velocity Ω Angular Velocity

vP = ?
vP vP
vR P vR P

fixed point

6
 Rotational Motion Rotational Motion
Ω Angular Velocity Ω Angular Velocity vP is proportional to:
• ||Ω||
• ||Psinφ||
Psinφ and
vP vP
P P • vP _| Ω
vR vR • vP _| P
P φ P

fixed point fixed point

vP = Ω × P

Cross Product Operator Cross Product Operator

P
vP = Ω × P ⇒ vP = Ω
⎡ax ⎤ ⎡bx ⎤
c = a × b ⇒ c = ab
ˆ  : a skew-symmetric matrix
a = ⎢ay ⎥ , b = ⎢⎢by ⎥⎥
⎢ ⎥ Ω× ⇒ Ω
vectors matrices
⎢⎣az ⎥⎦ ⎢⎣bz ⎥⎦ Ωx LM Px OP LM OP
a × ⇒ aˆ MM
Ω = Ω y ; P = Py PP MM PP
: a skew-symmetric matrix Ωz N Pz Q N Q
⎡ 0 −az ay ⎤ ⎡bx ⎤ LM 0 −Ω z Ωy OP LP O
⎢ ⎥
0 −ax ⎥ ⎢⎢by ⎥⎥ PP MM PP
x

c = ab
ˆ = ⎢ az c = ab
ˆ P=
vP = Ω MM Ω 0 − Ω x . Py P
vP = Ω
Q MN P PQ
z

⎢−ay ax
⎣ 0 ⎥⎦ ⎢⎣bz ⎥⎦ N−Ω y Ωx 0 z

Simultaneous linear and angular motion

Ω vP/ B
{B} P
v B/ A
{A}

v P / A = v B / A + v P / B + Ω × PB Movie Segment
A
v P / A = A v B / A + BA R. B v P / B + A Ω B × BA R. B PB Beach Volleyball, Toshiba,
ICRA 1999 video proceedings

7
 Spatial Mechanisms
{2}
{1}
{n}
{0} v
ω
Propagation of velocities

x v : linear velocity
ω : angular velocity

x = J (θ ).θ

Velocity propagation Velocity propagation

{i+1} zi+1
{i} zi Joint 1
ωi Ωι+1 ωi+1
vi+1 v1 and ω1 in frame {1}
vi
Pi+1 Joint i+1
i +1
ω i +1 = i +i1R. i ω i + θ i +1 . i +1 Z i +1

Linear v i +1 = v i + ω i × Pi +1 + d i +1 . Z i +1
i +1
v i +1 = i +i1R.( i v i + i ω i × i Pi +1 ) + di +1 . i +1 Z i +1
⇒ nω n n
and v n {n-1}
ω i +1 = ω i + Ω i +1 {n} v
Angular
Ω i +1 = θ . Z i +1 FG v IJ = FG R 0 IJ.FG v IJ
0
n
0
n
n
n ω

H ω K H 0 RK H ω K
i +1
0 0 n
n n n

Example
l3 v i +1 = v i + ω i × Pi +1
0
v P3 = 0 v P2 + 0 ω 2 × 0 P3

{0}
P3 θ 3
v P1 = 0
0
ω 1 = θ 1 . 0 Z1
LM−l . s OP 1 1 LM0 −1 0OP
= l . c .θ + 1 0 0 .(θ + θ
MM 0 PP MM0 0 0PP
l2 • 0
v P3 1 1 1 1 2 ). 0 P3
P2
θ 1
l1
θ1
θ 2 • v P2 = v P1 + ω 1 × P2
N Q N Q LMl . c OP
v P3 = v P2 + ω 2 × P3 LM−l . s OP L O
− l . s
2

MMl .0s
12

PP
+ M l . c P.(θ + θ )
P1 • 1 1 2 12 2 12

= l . c .θ
MM 0 PP MM 0 PP N Q
LM 0 −θ 1 OP L
0 l1 . c1 OP
−l1 . s1 LM OP N Q
1 1 1 2

N Q
12 1 2

0
v P2 = 0 + Mθ
MN 0
0 PP MM
0 . l1 . s1 = l1 . c1 .θ 1
PP MM PP ω 3 = (θ 1 + θ 2 + θ 3 ). 0 Z 0
Q MN
1

Q N Q
0
0 0 0 0

8
 The Jacobian (EXPLICIT FORM)
L − (l s + l s ) − l2 s12 0 OP LMθ OP
= M l c +l c
1 1 2 12 1
Ωi
0
v P3
MM 0 1 1 2 12 l2 c12
P0P MMθθ PP
0 . 2

N 0 QN Q 3
Vj

LM0 0 0 O Lθ O
Jv
M P
0 0 P. Mθ P
1
0
ω3 MM1
= 0
P
1 1 PQ MNθ PQ
2

N Revolute Joint Ω i = Z i qi

FG v IJ = J. FGθθ IJ
3

Jω 1
Prismatic Joint Vi = Z i qi
Hω K GGHθ JJK 2

The Jacobian (EXPLICIT FORM) The Jacobian (EXPLICIT FORM)

Ωi ω Ωi ω
Ωi Ωi
Vj Ωi × Pin Vj Ωi × Pin
Pi n v Pi n v
Prismatic Revolute
Vj Prismatic Revolute
Vj
Effector Effector

Linear Vel: Vj Ωi × Pin Linear Vel: Vj Ωi × Pin

Angular Vel: none Ωi Angular Vel: none Ωi
Effector Linear Velocity Effector Linear Velocity
n n
v = ∑ [∈i Vi + ∈i (Ω i × Pin )] Vi = Zi qi v = ∑ [∈i Z i + ∈i ( Z i × Pin )] qi Vi = Zi qi
i =1 i =1
Effector Angular Velocity Effector Angular Velocity
n n
ω = ∑ ∈i Ω i Ωi = Zi qi ω = ∑ (∈
i =1
i Z i ) q i Ωi = Zi qi
i =1

v = [∈1 Z1 + ∈1 ( Z1 × P1n )]q1 + " The Jacobian ⎛J ⎞

+ [∈n−1 Z n−1 + ∈n−1 (Z n−1 × P( n−1) n )]qn−1 + ∈n Z n qn
J =⎜ v ⎟
⎡ q1 ⎤ ⎝ Jw ⎠
⎢q ⎥
v = [∈1 Z1 + ∈1 (Z1 × P1n ) ∈2 Z2 + ∈2 (Z2 × P2n ) "] ⎢ 2 ⎥ Matrix J v (direct differentiation)
⎢#⎥
v = J v q
⎢ ⎥ F xI
v = G y J = x
⎢⎣qn ⎥⎦ ∂x P ∂x ∂x
= . q1 + P . q 2 +"+ P . q n
GH zJK P
∂q1 ∂q 2 ∂q n
ω =∈1 Z1q1 +∈2 Z2q2 +"+∈n Znqn ⎡ q1 ⎤
⎢ q ⎥
ω = [∈1 Z1 ∈2 Z 2 " ∈n Z n ] ⎢ 2⎥ ⎛ ∂ x ∂ xP ∂ xP ⎞
⎢#⎥ Jv = ⎜ P " ⎟
⎝ ∂ q1 ∂ q2 ∂ qn ⎠
ω = Jω q ⎢ ⎥
⎣⎢ q n ⎦⎥

9
Jacobian in a Frame J in Frame {0}
Vector Representation
⎡0⎤
F ∂x ∂x P ∂x PI 0
Zi = R Zi ; Zi = Z = ⎢⎢0⎥⎥
0 i i

GG ∂q JJ
i
P
"
J= ∂q 2 ∂q n ⎢⎣1 ⎥⎦
H ∈. Z 1
∈2 . Z 2 " ∈ .Z K
F∂ I
1 1 n n

∂ 0 ∂ 0
In {0}
F∂ x ∂ 0 xP I
∂ 0 xP
0
J = ∂q1 GG(0 xP )
∂q 2
( xP ) "
∂q n
( xP )
JJ
H ∈ .( R. Z )K
0

J = G ∂q JJ ∈1 .( 10 R. Z ) ∈2 .( 20 R. Z ) "
P
" 0
∂q 2 ∂q n
GH ∈ . Z
0 n n

∈. Z K
1

1
0
1 ∈2 . 0 Z 2 " n
0
n

Stanford Scheinman Arm

d2 d3
Z0 Y0
X6 Y6
X0
Z6

F I
GG
J = Z1 ×P13 Z2 ×P23 Z3 0 0 0 JJ
H Z1 Z2 0 Z4 Z5 Z6 K

Z2 θ2
d2 d3 d2
{0} z d3 i αi-1 ai-1 di θi
θ1 0
Z0 Z1 X
y0
x0
x6 y6
1 0 0 0 θ1
θ4 θ5
X2
X5 6 Z5 z6 2 -90 0 d2 θ2
X1 X 3 90 0 d3 0
0 Z3 Z4 θ6 4 0 0 0 θ4
Z6 X3 X 5 -90 0 0 θ5
4 6 90 0 0 θ6

i αi-1 ai-1 di θi cθi -sθi 0 ai-1

1 0 0 0 θ1 i-1 sθi cαi-1 cθi cαi-1 -sαi-1 -sα i-1 di
2 -90 0 d2 θ2 1 T= sθi sαi-1 cθi sαi-1 cαi-1 cαi-1 di
3 90 0 d3 0
4 0 0 0 θ4 0 0 0 1
5 -90 0 0 θ5 0 0 1 N-1
6 90 0 0 θ6
Forward Kinematics: T = T T ... T
N 1 2 N

10
 Stanford Scheinman Arm LMc 4 −s4 0 0 OP
LMc 1 − s1 0 0 OP T=M
s 4 c4 0 0
P
MM 0 1 0P
3

T=M PP 4
s c1 0 0 0
0 1 PQ
1

MM 0
0
1
0 1 0
PQ N0 0
N0 0 0 1 LM c − s5 0 0 OP
LM O
5
− s2
T=M P
c2 0 0
d P
0 0 1 0
MM−s 0P
4

T=M P
0 0 1 5
− c5 0
1 PQ
2

MM−s 0P
1
5

N0
2
c2 0
1 PQ
2 0 0
N0 0 0
LMc − s6 0 0 OP
LM1 0 0 0O 6

−d P T=M P
0 0 −1 0
T=M P
0 −1
MMs 0P
5
0 3

MM0 0 P
2 6
c6 0
1 PQ
3 6
1 0
N0 0 0 1 PQ N0 0 0

LMc − s1 0 0 O ⎡c1c2c4 − s1s4 −c1c2 s4 − s1c4 c1s2 c1d3s2 − s1d2 ⎤

0P
1
⎢s c c + c s −s c s + c c s s s d s + c d ⎥
T=M P
s 1 c1 0
4T =
0 ⎢12 4 14 1 2 4 1 4 1 2 1 3 2 1 2⎥

MM 0 0P
0
1
0 1 ⎢ −s2c4 s2 s4 c2 d3c2 ⎥
1 PQ
⎢ ⎥
N0 0 0 ⎣ 0 0 0 1 ⎦
LMc c 1 2 − c1 s2 − s1 − s1 d 2 OP LM X X − c1 c2 s 4 − s1 c 4 c1 d 3 s 2 − s1 d 2 OP
T=M PP T=M PP
sc − s1 s 2 c1 c1 d 2 X X − s1 c2 s 4 + c1 c 4 s1 d 3 s 2 + c1 d 2
MM X
1 2 0

MM −s
0
5
2
−c2 X s2 s 4 d3 c2
N0
2

0
0
0
0
1 PQ N0 0 0 1 PQ
LMc c 1 2 − s1 c1 s 2 c1 d 3 s 2 − s1 d 2 OP LM X X c1c 2 c 4 s 5 − s1 s 4 s 5 + c1 s 2 s 5 c1 d 3 s 2 − s1 d 2 OP
T=M PP T=M PP
sc 1 2 c1 s1 s2 s1 d 3 s2 + c1 d 2 X X s1c 2 c 4 s 5 + c1 s 4 s 5 + s1 s 2 c 5 s1 d 3 s 2 + c1 d 2
MM −s MM X X
0 0
3 6
− s 2 c 4 s5 + c5 c2 d3 c2
2 0 c2 d3 c2
PQ N0 0 PQ
N0 0 0 1 0 1

Stanford Scheinman Arm Jacobian

LMX X cc c s −ss s +cs s O
1 3 2 −sd
cds
sds +cd P
1 2 4 5 14 5 12 5 1 2 d2 d3
T =M
X X sc c s +cs s +ss c
1 2 4 5 14 5 12 5 1 32
P 1 2 Z0 Y0 ⎛ ∂ 0 xP ∂ 0 xP ∂ 0 xP ⎞
MMX X −s c s +cc dc P
0
X6 Y6
⎜ 0 0 0 ⎟
6

1 PQ J = ⎜ ∂ q1 ∂ q2 ∂ q3
2 4 5 5 2 3 2

N0 0 0
X0
Z6
0
⎟
⎜ 0Z 0 0 0 0
Z6 ⎟⎠
⎝ 1 Z2 0 Z4 Z5
C 1 S 2 d 3 − S1 d 2
S1 S 2 d 3 + C 1 d 2
C2 d3
LM−c d
1 2 − s1 s2 d 3 c1 c 2 d 3 c1 s2 0 0 OP 0
⎛ xP ⎞ 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 ) MM−s d
1 2 + c1 s 2 d 3 s1 c 2 d 3 s1 s 2 0 0
PP 0
⎜ ⎟ 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 d 3
⎜ r1 ⎟ − S2 (C 4 C 5 C 6 − S4 S6 ) − C 2 S5 C 6 MM 0
− s1
c2 0 0
− c1 c 2 s 4 − s1 c 4
0
c c c s −s s s +c s c P
P
x = ⎜ r2 ⎟ = 0 0 c1 s2
MM s c c s +c s s +s s c P
1 2 4 5 1 4 5 1 2 5
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 s 4 + c1 c 4
⎜ r3 ⎟
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
N
0
1
c1
0
0
0
s1 s 2
c2 s2 s4
1 2 4 5

−s c s + c c
PQ
1 4 5 1 2 5

⎜ ⎟ C1 ( C 2 C 4 S 5 + S 2 C 5 ) − S1 S 4 S 5
2 4 5 5 2

⎝ ⎠ 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
Kinematic Singularity Kinematic Singularity
The Effector Locality loses the ability
to move in a direction or to rotate about
⎛ ABR 0 ⎞A
B
J =⎜ B ⎟
J
a direction - singular direction
⎝0 A R⎠

J = ( J1 J 2 " J n )
det[ B J ] ≡ det[ A J ]
det ( J ) = 0
( )
det i J = det ( J)
j

det J = det( )
i
( J)j

Singular Configurations Example (Kinematic Singularities)

det[ J ( q )] = 0 (x,y)
x = l1C1 + l 2C12
⇒ Singular Configurations
{0}
l2 θ2
y = l 1S1 + l 2 S12
l1
det[ J (q)] = S1 (q)S2 (q)...Ss (q) = 0 y
{1} θ1

J =⎜
(
⎛ − l1S1 + l 2 S12 ) −l 2 S12 ⎞
⎟
S1 (q) = 0
⎜ l1C1 + l 2C12 l 2C12 ⎟⎠
S2 (q) = 0 ⎝
# det ( J ) = l1l 2 S 2
S s (q) = 0 Singularity at q2 = kπ

Example (Kinematic Singularities) Small Displacements Δq, ΔX

⎛ Δx ⎞
ΔX = ⎜ ⎟
(x,y) 1
J =10 R 0 J ⎝ Δy ⎠
(x,y) q2
l2 θ2 {1} Δq2
⎛ C1 − S1⎞ ⎛ −l 2 S 2 −l 2 S 2 ⎞
{0}
l1
1
J =⎜ ⎟⎜ ⎟ Δ q1 q1 Δq = J −1ΔX
⎝ S1 C1 ⎠ ⎝ l 1 + l 2C 2 l 2C 2 ⎠
y
{1} θ1

⎛ 0 0⎞ ⎛ 1 1 ⎞
At Singularity J =⎜ small θ 2 ⎜ lθ l1 ⎟
1
⎟
⎝ l1 + l 2 l2 ⎠ −1
≅⎜
1 2 ⎟
LM δx = 0
1 J (1)
⎜ l1 + l 2 1⎟
MM δy = (l ⎜− − ⎟
1
+ l2 )δθ 1 + l2 δθ 2 ⎜ l1l 2θ 2 l1 ⎟⎠
N 1 ⎝

12
 Δq1
Small Displacements Δq, ΔX
⎛ Δx ⎞
ΔX = ⎜ ⎟
⎝ Δy ⎠ Δy(1)
q2 l1
{1} Δq2 Δq2
• θ2
Δ q1 Δ x(1)
q1 −
Δy(1)
(l1 + l 2 ) Δ x(1)
−
Δ x(1) 1 Δy(1) l2 Δy(1)
Δ q1 = ⋅ + • θ2
l1 θ 2 l1 q2

( l1 + l 2 ) Δ x(1) ⋅ 1 + Δy(1)
{1} Δ q2 Δy(1)
−
Δ q1
Δ q2 = q1 l1
l1l 2 θ2 l1

E
Kinematic Singularities (reduced matrix)
l3
(x,y)
⎜
(
⎛ − l1S1 + l 2 S12 ) −l 2 S12 ⎞
⎟ l2
l2 θ2 ⎜ l1C1 + l 2C12 l 2C12 ⎟
J =⎜ ⎟ l1
⎜ 0 0 ⎟
y
{0}
l1
θ1
⎜
⎜⎜
0 0 ⎟
1 ⎟⎠
⎟ LM−l s − l s − l s
1 1 2 12 3 123 −l2 s12 − l3 s123 − l3 s123 OP
MM l c + l c0 + l c PP
{1} ⎝ 1
1 1 2 12 3 123 l2 c12 + l3 c123 l3 c123

( )
0 0
⎛ − l1S1 + l 2 S12 −l 2 S12 ⎞ 0
=M PP
det ( J ) = l1l 2 S 2
JE
J =⎜
⎜ l1C1 + l 2C12
⎝ l 2C12 ⎟⎠
⎟
MM 0 0 0
PP
Singularity at q2 = kπ
MN 0
1
0
1
0
1 Q

LM−l s − l s − l s
1 1 2 12 3 123 −l2 s12 − l3 s123 − l3 s123 OP
MM l c + l c0 + l c
1 1 2 12 3 123 l2 c12 + l3 c123
0
l3 c123
0
PP
0
JE =M PP
MM 0 0 0
PP
MN Movie Segment
0 0 0
1 1 1 Q
⎛ −l1s1 − l2 s12 − l3s123 −l2 s12 − l3s123 −l3s123 ⎞
⎜ ⎟ Automatic Parallel Parking, INRIA,
0
J E = ⎜ l1c1 + l2c12 + l3c123 l2c12 + l3c123 l3c123 ⎟
⎜ 1 ⎟⎠ ICRA 1999 video proceedings
⎝ 1 1

13
 Stanford Scheinman Arm

Stanford Scheinman Arm Jacobian

⎛ ∂ 0 xP ∂ 0 xP ∂ 0 xP ⎞
θ2
d2
{0} z d3 i αi-1 ai-1 di θi
θ1 0
θ1 ⎜ 0 0 0 ⎟
J = ⎜ ∂ q1 ∂ q2 ∂ q3
y0
x6 1 0 0 0 0
⎟
x0 y6
θ4 θ5
z6 2 -90 0 d2 θ2
3 90 0 d3 0 ⎜ 0Z 0 0 0 0
Z6 ⎟⎠
θ6 4 0 0 0 θ4 ⎝ 1 Z2 0 Z4 Z5
5 -90 0 0 θ5
6 90 0 0 θ6
LM−c d
1 2 − s1 s2 d 3 c1 c 2 d 3 c1 s2 0 0 0 OP
cθi -sθi 0 ai-1
MM−s d
1 2 + c1 s 2 d 3 s1 c 2 d 3
− s2 d 3
s1 s 2 0 0 0
P
PP
i-1 sθi cαi-1 cθi cαi-1 -sαi-1 -sα i-1 di MM 0
0 − s1
c2
0
0
c1 s2
0
− c1 c 2 s 4 − s1 c 4
0
c c c s −s s s +c s c P
T= MM s c c s +c s s +s s c P
1 2 4 5 1 4 5 1 2 5

− s1 c 2 s 4 + c1 c 4
sθi sαi-1 cθi sαi-1 cαi-1 cαi-1 di 0 c1 0 s1 s 2
PQ
1
1 2 4 5 1 4 5 1 2 5

0 0 0 1 N 1 0 0 c2 s2 s4 −s c s + c c
2 4 5 5 2

0 0 1 N-1
Forward Kinematics: T = T T ... T
N 1 2 N

Stanford Scheinman Arm Jacobian Jacobian at the End-Effector

ωe
{e}
θ 5 = kπ ve
ωn
Pne
{n} vn
⎡−c1d2 − s1s2d3 c1c2d3 c1s2 0 0 0⎤
⎢−s d + c s d s c d s s 0
⎢ 1 2 12 3 12 3 12 0 0 ⎥⎥ v e = v n + ω n × Pne

RSv = v − P
⎢ 0 −s2d3 c2 0 0 0⎥
J =⎢
⎢ 0 −s1 0 c s
1 2 −c c s
1 2 4 − s c
1 4 c
⎥
1s2 ⎥ e n ne ×ωn
⎢
⎢
⎣⎢
0
1
c1
0
0 s1s2 −s1c2s4 + c1c4 s1s2 ⎥
0 c2 s2s4
⎥
c2 ⎦⎥ Tω = ω e n

14
 RS v = v − P × ω
e n ne n
Cross Product Operator (in diff. frames)

Tω = ω {e}
ωe
FI − 0 Pne I
GH 0 JK
e n ve
ωn 0
Je = 0

FG v IJ = FG Ι − P IJ FG v IJ {n} vn
Pne
I
Jn

Hω K HΟ Ι K Hω K
e ne n
Pˆ n

e n
P ≠ n0 R n Pˆ ;
0 ˆ 0
P = ( n0 R. n P)≠ n0 R. n P
J e q = Ο Ι ne J n q
FG Ι IJ − P 0
P × 0 ω = n0 R.( n P × n ω )
H K 0  0
P. ω = n0 R.( n P . n ω ) = n0 R.( n P . n0 R T . 0 ω )

F I
= GΟ J J
Ι − Pne 0
Pˆ = n0 R n Pˆ 0
RT
Je
H K
n
Ι n

E Wrist Point

FR
J =G
i
0 j I l3
x = l1 c1 + l2 c12

JK
j
W y = l1 s1 + l2 s12
i

H0 j
i
R
J
l1
l2 End-Effector Point
x = l1 c1 + l2 c12 + l3 c123
y = l1 s1 + l2 s12 + l3 s123

⎛ 0R − n0 R n Pˆne n0 RT ⎞ n
0
J e = ⎜⎜ n 0 ⎟⎟ J n
⎝ 0 n R ⎠

E Wrist Point E Wrist Point

x = l1 c1 + l2 c12 x = l1 c1 + l2 c12
l3 l3
W y = l1 s1 + l2 s12 W y = l1 s1 + l2 s12
l2 End-Effector Point l2 End-Effector Point
x = l1 c1 + l2 c12 + l3 c123 x = l1 c1 + l2 c12 + l3 c123
l1 l1
Jacobian (W) y = l1 s1 + l2 s12 + l3 s123 y = l1 s1 + l2 s12 + l3 s123
LM−l s − l s
1 1 2 12 −l2 s12 0 OP
MM l c +0l c l2 c12 0
P LM−l s − l s − l2 s12 0OP LM− l s − l s − l3 s123 − l 2 s12 − l3 s123 − l3 s123 OP
FI I
1 1 2 12

0P
1 1 2 12 1 1 2 12

− 0 PWE MM l c +0l c PP MM l c + l c0 + l3 c123 l 2 c12 + l3 c123

PP
=M =G JK
0 PP
0 0 0 1 1 2 12 l2 c12 0 1 1 2 12 l3 c123
JW
MM 0 0
; JE
H0 I
JW
=M
0 0
PP 0
=M
0 0
PP
0P
JW JE
MM 0 0 0 MM 0 0 0
MN 10 0
1 1Q
P MN 01 0 0P
P MN 0 0 0 PP
1 1Q 1 1 1 Q

15
 E Wrist Point
x = l1 c1 + l2 c12
Resolved Motion Rate Control (Whitney 72)
l3
W y = l1 s1 + l2 s12 δx = J (θ )δθ
l2 End-Effector Point
Outside singularities
x = l1 c1 + l2 c12 + l3 c123
l1
y = l1 s1 + l2 s12 + l3 s123 δθ = J −1 (θ )δx
LM−l s − l s
1 1 2 12 −l2 s12 0 OP
MM l c +0l c l2 c12 0
P Arm at Configuration θ
FI I
1 1 2 12

0P − 0 PWE
=M
0 PP GH 0 JK
0
JE = x = f (θ )
0 0
JW JW
MM 0 0 I
0P δx = x d − x
MN 10 0
1 1Q
P
Ll c OP F 0 0 l3 s123 I δθ = J −1δx
= Ml s =G 0 JJ
3 123
0
PWE
MM 0
3 123 PP⇒ P 0
WE
GH −l s 0 − l3 c123
θ + = θ + δθ
N Q 3 123 l3 c123 0 K

Resolved Motion Rate Control

Jacobian
δq1 q1

• Differential Motion
Control Joint 1

xd δx δq
δq2
Control Joint 2
q2
q • Linear & Angular Motion
J -1
x • Velocity Propagation
• Explicit Form
δqn qn
Control Joint n

Forward
Kinematics • Static Forces

Angular/Linear – Velocities/Forces Angular/Linear – Velocities/Forces

ω v τ F

p
ω
p τ = p× F
v =ω× p z
v =ω× p
z

τ τ = p̂ F
y y
x x
v = − pˆ ω τ = (− pˆ )T F
τ = p× F ⎛ vx ⎞ ⎛ − p y ⎞  ⎛F ⎞
p τ = (− p p )⎜ ⎟
v
x
⎜ ⎟=⎜ ⎟θ y
F x
⎝ v y ⎠ ⎝ px ⎠ ⎝ y ⎠

F v = J θ τ =J F T

16
 n f
Velocity/Force Duality

x = J θ τ3

τ = JT F τ2

τ1

f f n f
n n

Propagation -n
-f link 3
Elimination of n3
τ3 Internal forces τ3 f3
-f3 -n3
n2
τ2 Energy Analysis τ2 -f2 link 2
Virtual Work f1 f2
τ1 Static Equilibrium τ1 -n1 -n2
-f1 n1 link 1

-fi+1 -fi+1
-ni+1 -ni+1

Link i Link i Static Equilibrium

Pi+1 Pi+1 Σ forces = 0
Σ moments / a point = 0
About origin {i}
ni fi ni 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
 n f Virtual Work Principal
fi ni
F=
FG f IJ Internal δ w = ∑ f i δ xi
τi τi H nK forces are
workless
i
applied
forces
virtual
zi zi displacements
Static Equilibrium:
Prismatic Joint Revolute Joint ⎛ τ1 ⎞
τ3 If the virtual work done by applied
τ i = fi T Z i τ i = ni T Z i τ = ⎜⎜τ 2 ⎟⎟ forces is zero in displacements
⎜τ ⎟ consistent with constraints
⎝ 3⎠
Algorithm
n
fn = nf τ2 τ T δ q + (−F )T δ x = 0
n
nn = n + Pn +1 × f
n n n

τ1 τ T δ q = F T δ x using δ x = J δ q
i +1
i
fi = R. fi +1
i +1
i

i
ni = i +1i R. i +1 ni +1 + i Pi +1 × i fi
τ T = FT J τ = JT F

Velocity/Force Duality Example (Static Forces)

⎛ − ( l1S1 + l 2 S12 ) −l 2 S12 ⎞
(x,y)
J =⎜ ⎟
⎜ l1C1 + l 2C12 l 2C12 ⎟⎠
⎝

x = J θ
l2 θ2
⎛ − ( l1S1 + l 2 S12 ) l1C1 + l 2C12 ⎞
{0} JT = ⎜ ⎟
y l 1 1N ⎜ −l 2 S12 l 2C12 ⎟⎠
{1} θ1 ⎝
τ = JT F
τ =J F T
l1 = l2 = 1; θ1 = 0; θ 2 = 60D

τ =⎜
(
⎛ − l 1S1 + l 2 S12 ) l1C1 + l 2C12 ⎞ ⎡ 0 ⎤ ⎡l 1C1 + l 2C12⎤
⎟⎢ ⎥ = −⎢
⎡3 / 2 ⎤
⎥ = −⎢ ⎥
⎜ −l 2 S12 l 2C12 ⎟⎠ ⎣ −1⎦ ⎣⎢ 2
l C12 ⎦⎥ ⎣1/ 2 ⎦
⎝

Example (Static Forces) (x,y)

(x,y)
l2
l2 θ2
1000N
{0}
l1
y 1000N
{1} θ1 τ = JT F y
{1}

τ =⎜
(
⎛ − l1S1 + l 2 S12 ) l1C1 + l 2C12 ⎞ ⎡0 ⎤ ⎡l1C1 + l 2C12⎤
⎟⎢
⎡0⎤
⎜ ⎟ − ⎥ = ⎢l C12 ⎥ (−1K ) =⎢ ⎥
⎝ −l 2 S12 l 2C12 ⎠ ⎣ 1K ⎦ ⎣⎢ 2 ⎦⎥ ⎣0⎦

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

18