Академический Документы
Профессиональный Документы
Культура Документы
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
q = (q1q2 ....qn )T
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
"
J ij (q) =
fi (q)
q j
Jacobian
x( m1) = J ( mn ) (q ) q( n1)
x( m1) = J ( mn ) (q ) q( n1)
Example
l1
x = l1 c1 + l2 c12
(x,y)
l2
y = l1 s1 + l2 s12
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
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
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
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
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
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)
= 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
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
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
{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}
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
ax
bx
a = ay , b = by
az
bz
: a skew-symmetric matrix
matrices
vectors
a a
LM
MM
N
c = ab
{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
Vj
Vj
Prismatic
Revolute
Vj
i Pin
Linear Vel:
Angular Vel:
none
Prismatic Joint
Vi = Z i qi
i Pin
Vj
Effector
Prismatic
Revolute
Vj
i Pin
Linear Vel:
Angular Vel:
none
Vi = Zi qi
i =1
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
= i i
i = Zi qi
i =1
(
i =1
Z i ) q i
The Jacobian
q1
q
v = [1 Z1 + 1 (Z1 P1n ) 2 Z2 + 2 (Z2 P2n ) "] 2
#
qn
v = J v q
= [1 Z1 2 Z 2 " n Z n ]
= J q
Pi n
Effector
i = Z i qi
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
Vj
Pi n
J
J = v
Jw
v = J v q
= J q
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
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
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
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
12
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
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}
(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
1
1
1
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
3 123
2 12
3 123
l2c12 + l3c123
l3c123
0
1
0
1
OP
PP
PP
PP
Q
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
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
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
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
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
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
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
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
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}
(1K ) =
= l C12
1
K
0
l 2C12
2
l1 = l2 = 1; 1 = 90; 2 = 0D
18