| Date post: | 07-Mar-2016 |
| Category: |
Documents |
| Upload: | amr-amin-elsharkawy |
| View: | 213 times |
| Download: | 0 times |
of 18
1Movie SegmentLocomotion Gates with Polypod, Mark Yim, Stanford University, ICRA 1994 video proceedings
InstantaneousKinematics
Linear & Angular Motion Velocity Propagation
J a c o b i a n Differential Motion
Explicit Form Static Forces
Differential Motion
{0}
{n}{n}
Forward Kinematics
Instantaneous Kinematicsx x + +x
Relationship: x x Linear VelocityAngular Velocity
i i i i iq d = +
Joint Coordinates
i
i
revolute:
d prismaticcoordinate i
Joint coordinate-i:
0 revolute1 prismatici
= with
1i i = andJoint Coordinate Vector: 1 2( .... )
Tnq q q q=
2Jacobians: Direct Differentiation
( );x f q=xx
x
f qf q
f qm m
1
2
1
2
# #
F
H
GGGG
I
K
JJJJ=F
H
GGGG
I
K
JJJJ
( )( )
( )
x fq
q fq
q
x fq
q fq
q
nn
mm m
nn
11
11
1
11
= + +
= + +
"
#"
xfq
fq
fq
fq
qn
m m
n
=
L
N
MMMMM
O
Q
PPPPP
1
1
1
1
"# # #"
.
( 1) ( ) ( 1)( )m m n nx J q q =
Jacobian
( 1) ( ) ( 1)( )m m n nx J q q = where
( ) ( )ij ij
J q f qq=
( 1) ( ) ( 1)( )m m n nx J q q =
( )x J =
Example
l1
l2 (x,y) x l c l cy l s l s
= += +
1 1 2 12
1 1 2 12
x l s l s l sy l c l c l c
= + = + +
( )( )
1 1 2 12 1 2 12 2
1 1 2 12 1 2 12 2
X
xy
y l sx l c
= LNMOQP =
LNM
OQPFHGIKJ
2 12
2 12
1
2
J
x x
y yF
H
GGG
I
K
JJJ
1 2
1 2
( )x J = 2 12
2 12
y l sx l c
=
Stanford Scheinman Arm
d2d3
x0
z0y0
1{0}
2
x6 y6z6
4 5
6
i i-1 ai-1 di i1 0 0 0 12 -90 0 d2 23 90 0 d3 04 0 0 0 45 -90 0 0 56 90 0 0 6
d2d3
x0
z0y0
1{0}
2
x6 y6z6
4 5
6
C C C C S S C S S S S S C S C CS C C C S S C S S S C
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
1 2 4 5 6 4 6 2 5 6 1
[ ( ) ] ( )[ ( ) ]
+ + + + + + ( )
( ) +
+ +S C S C C
S C C S S C C S S4 5 6 4 6
2 4 5 6 4 6 2 5 6
C C C C C S S S S C S S C C C SS C C C C S S S S C C S C C C S
S C C C S S C S C
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
2 4 5 6 4 6 2 5 6
[ ( ) ] ( )[ ( ) ] ( )
( )
+ + +
( )( )
+ + +
+
C C C S S C S S SS C C S S C C S S
S C S C C
1 2 4 5 2 5 1 4 5
1 2 4 5 2 5 1 4 5
2 4 5 2 5
+
C S d S dS S d C d
C d
1 2 3 1 2
1 2 3 1 2
2 3
1
2
3
Pxr
x rr
= =
3Stanford Scheinman Arm
xc s d s ds s d c d
c dp =
+
L
NMMM
O
QPPP
1 2 3 1 2
1 2 3 1 2
2 3
qqqqqq
O
QPPP
L
N
MMMMMMM
O
Q
PPPPPPP
1
2
3
4
5
6
(3 1) (3 6) (6 1)( )pp x xx J q q =
xyz
F
HGGI
KJJ =L
NMMM
y c c d c sx s c d s s
s d c
L
N
1 2 3 1 2
1 2 3 1 2
2 3 2
0 0 00 0 0
0 0 0 0
Linear Velocity VLinear Velocity V
x p =
Position xr qr qr q
R =L
NMMM
O
QPPP
1
2
3
( )( )( )
(( )
(
xrrr
rq
rq
rq
rq
rq
rq
q
R
x
x
x
=F
HGGI
KJJ =
F
H
GGGGGG
I
K
JJJJJJ
F
H
GGGG
I
K
JJJJ
1
2
3 9 1)
1
1
1
6
2
1
2
6
3
1
3
6 9 6
1
2
6 6 1)
"
"
"#
( ) x J q qR XR=Orientation: Direction Cosines
C C C C S S C S S S S S C S C CS C C C S S C S S S C
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
1 2 4 5 6 4 6 2 5 6 1
[ ( ) ] ( )[ ( ) ]
+ + + + + + ( )
( ) +
+ +S C S C C
S C C S S C C S S4 5 6 4 6
2 4 5 6 4 6 2 5 6
C C C C C S S S S C S S C C C SS C C C C S S S S C C S C C C S
S C C C S S C S C
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
2 4 5 6 4 6 2 5 6
[ ( ) ] ( )[ ( ) ] ( )
( )
+ + +
( )( )
+ + +
+
C C C S S C S S SS C C S S C C S S
S C S C C
1 2 4 5 2 5 1 4 5
1 2 4 5 2 5 1 4 5
2 4 5 2 5
Rx =
(( )
(
xrrr
rq
rq
rq
rq
rq
rq
q
R
x
x
x
=F
HGGI
KJJ =
F
H
GGGGGG
I
K
JJJJJJ
F
H
GGGG
I
K
JJJJ
1
2
3 9 1)
1
1
1
6
2
1
2
6
3
1
3
6 9 6
1
2
6 6 1)
"
"
"#
Representations
XxxP
R= LNMOQP
CartesianSphericalCylindrical.
Euler AnglesDirection CosinesEuler Parameters
Jacobian for X ( ) ( ) x J q q
x J q qP X
R X
P
R
==
( )( )x
xJ qJ q q
P
R
X
X
P
R
FHGIKJ =FHGIKJ
( ) (12 (12 ) (X J q qx X x x1) 6 6 1)=The Jacobian is dependent on the representation
Cartesian & Direction Cosines
Basic Jacobian
x E x vx E xP P P
R R R ==
( ) ( )
F IxH K (6 1)
vJ q qxn nxG J = ( ) (( ) 0 6 1)
{0}angular velocity
linear velocity
v
4Examples
x E x
s cs
c cs
c sss
cs
xxyz
E x
R R R
P P P
=F
HGGI
KJJ =
F
H
GGGG
I
K
JJJJ
=F
HGGI
KJJ =
F
HGG
I
KJJ
; ( )
. .
; ( )
1
00
1 0 00 1 00 0 1
*
*
Jacobian for X
( )xx J q q=
0( ) ( ) ( )xJ q E x J q=
P
R
xx
x = Given a representation
0 ( )v
J q qw
= Basic Jacobian
Jacobian and Basic Jacobianv
J q q
v JJ
q
v J qJ q
v
v
FHGIKJ =FHGIKJ =FHGIKJ
==RST
0 ( ).
.
. .
. ( . ) . ( . ) x E v x E J qx E x E J qP P P P v
R R R R
= == = J E JJ E J
X P v
X R
P
R
==
RST..
00P vXP
RXR w
E JJJEJ J
= =
0( ) ( ) ( )J q E X J q=
0 ( )v
J q qw
=
With Cartesian Coordinates
3 ; ;P XP vE I J J= = and 00 RI
EE
=
Position Representations
3( )PE X I=Cartesian Coordinates ( , , )x y z
cos sin 0
( ) sin cos 0
0 0 1
PE X
=
Cylindrical Coordinates ( , , )z Using ( ) ( cos sin )T Tx y z z =
( ) ( )cos sin sin sin cos
( ) sin cos 0sin sincos cos sin cos sin
PE X
=
Spherical Coordinates ( , , )
( ) ( cos sin sin sin cos )T Tx y z =Using
5Euler Angles
Singularity of the representationfor k =
. . 1
; ( ) 0
0
R R R
s c c cs s
x E x c ss cs s
= =
Jacobian for X
( )xx J q q=
0( ) ( ) ( )xJ q E x J q=
P
R
xx
x = Given a representation
0 ( )v
J q qw
= Basic Jacobian
Jacobian
vJ q q
xxn nx
FHGIKJ =( ( ) (( )
6 1)
6 1)
{0}angular velocity
linear velocity
v
Linear & Angular Velocities
{0}angular velocity
linear velocity
v
Linear VelocityvP A/
P{A}
{B}
{C}
vP A/A
vP A/C
vP A/BPure Translation
{B}
vP A/{A}
vA B/
6vP B/
{B}
vP A/{A}
vA B/
v v vP B A B P A/ / /= +
Pure Translation Rotational Motion
rigid bodyfixed points on the rigid bodyAxis of rotation
Angular VelocityRotational Motion
Angular Velocity
vR
Rotational Motion
P
Angular Velocity
PvP
vR
?Pv =
Rotational Motion
Angular Velocity
PvP
fixed point
vR
Rotational Motion
7 Angular Velocity
PvP
P
fixed point
vR
Rotational Motion
Angular Velocity
PsinP
vP
Pfixed point
vR
vP is proportional to: |||| ||Psin||
and vP _| vP _| P
v PP =
Rotational Motion
c a b= Cross Product Operator
0 0
0
z y x
z x y
y x z
a a bc ab a a b
a a b
= = c ab=
: a skew-symmetric matrixa a vectors matrices
,x x
y y
z z
a ba a b b
a b
= =
c ab = v P v PP P= = Cross Product Operator
=L
NMMM
O
QPPP
x
y
z
;PPPP
x
y
z
=L
NMMM
O
QPPP
v PPPP
P
z y
z x
y x
x
y
z
= =
L
NMMM
O
QPPP
L
NMMM
O
QPPP
.
00
0v PP =
: a skew-symmetric matrix
Simultaneous linear and angular motion
{A}
{B}vB A/
vP B/P
AP A
AB A B
A BP B
AB B
A BBv v R v R P/ / /. .= + +
v v v PP A B A P B B/ / /= + + Movie SegmentBeach Volleyball, Toshiba, ICRA 1999 video proceedings
8Spatial Mechanisms{2}
{0}
{1}{n}
v
( ). x J= x v : linear velocity : angular velocity
Propagation of velocities
{i} ivi
vi+1i+1+1
{i+1}zi
Pi+1
zi+1
i i i
i i iZ+ +
+ + +
= +=
1 1
1 1 1
.
v v P d Zi i i i i i+ + + += + +1 1 1 1 .
Velocity propagation
Linear
Angular
ii i
i ii
ii
ii i
iiv R v P d Z
++
++ +
++= + +1 1 1 1 1 1 1.( ) .
Velocity propagationJoint 1
v1 and 1 in frame {1}Joint i+1
ii i
i ii i
iiR Z
++
++
++= +1 1 1 1 1 1 . .
nn n nv and
0
0
0
0
00
v RR
vnn
n
n
nn
nn
FHGIKJ =FHG
IKJFHGIKJ.
{n-1} {n} v
Example
1
{0}
1P1
P2
P3
l1
l2
l3
2
3v v Pi i i i+ += + 1 1
v
v v P
v v P
P
P P
P P
1
2 1
3 2
0
1 2
2 3
== + = +
01 1
01 = . Z
01
1
1 1
1 1
1 1
1 1 120
0 00 0
0 0 0 0 0v
l cl s
l sl cP = +
L
NMMM
O
QPPP
L
NMMM
O
QPPP
=L
NMMM
O
QPPP
.
.
..
. .
0 0 02
033 2
v v PP P= + 0
1 1
1 1 1 1 20
33
0
0 1 01 0 00 0 0
vl sl c PP =
L
NMMM
O
QPPP
+L
NMMM
O
QPPP
+.
. . .( ).
=L
NMMM
O
QPPP
+L
NMMM
O
QPPP
+l sl c
l sl c
1 1
1 1 1
2 12
2 12 1 2
0 0
.. .
.. .( )
03 1 2 3
00 = + +( ). Z
l cl s2 12
2 12
0
.
.L
NMMM
O
QPPP
901 1 2 12 2 12
1 1 2 12 2 12
1
2
3
3
00
0 0 0v
l s l s l sl c l c l cP =
+ +
L
NMMM
O
QPPP
L
NMMM
O
QPPP
( ).
03
1
2
3
0 0 00 0 01 1 1
=L
NMMM
O
QPPP
L
NMMM
O
QPPP
.
vJ
FHGIKJ =F
HGGG
I
KJJJ
.
1
2
3
vJ
J Revolute Joint i i iZ q =
i i iV Z q= Prismatic Joint
jVi
The Jacobian (EXPLICIT FORM)
The Jacobian (EXPLICIT FORM)
Revolute
ijV
Linear Vel: jVAngular Vel:
jV
i inP inP
i inP i
i
Prismatic
none
Effector Linear Velocity
Effector Angular Velocity1[ ( )]
n
i i i i ini
v V P=
= + =
= i ii
n
1
Effector
i i iV Z q=
i i iZ q =
v
The Jacobian (EXPLICIT FORM)
Revolute
ijV
Linear Vel: jVAngular Vel:
jV
i inP inP
i inP i
i
Prismatic
none
Effector Linear Velocity
Effector Angular Velocity1[ ( )]
n
i i i i in ii
v Z Z P q=
= +
1( )
n
i i ii
Z q=
=
Effector
i i iV Z q=
i i iZ q =
v
1 1 1 1 1 1
1 1 1 1 ( 1) 1
[ ( )][ ( )]
n
n n n n n n n n n n
v Z Z P qZ Z P q Z q
= + ++ + +
"
[ ]1
21 1 1 1 1 2 2 2 2 2( ) ( )n n
n
v Z Z P Z Z P
q
= + +
" #
vv J q= 1 1 1 2 2 2 n n nZ q Z q Z q = + + + "
[ ]1
21 1 2 2 n n
n
Z Z Z
q
=
" #J q =
The Jacobian
vxyz
xxq
qxq
qxq
qPP P P
nn=
F
HGGI
KJJ = = + + +
. . .
1 1 2 2 "
1 2
P P Pv
n
x x xJq q q
= "
v
w
JJJ
= Matrix (direct differentiation)vJ
10
Jacobian in a FrameVector Representation
Jxq
xq
xq
Z Z Z
P P P
n
n n
= FHGG
IKJJ
1 2
1 1 2 2
"". . .
In {0}
0
0
1
0
2
0
10
1 20
20
Jxq
xq
xq
Z Z Z
P P P
n
n n
=
F
HGG
I
KJJ
"
". . .
J in Frame {0}
01
0
2
0 0
1 10
2 20 0
J qx
qx
qx
R Z R Z R Z
P Pn
P
n n
=
F
HGG
I
KJJ
( ) ( ) ( )
.( . ) .( . ) .( . )
""
0iZ = 0 ;ii iR Z
001
iiZ Z
= =
Stanford Scheinman Arm
J =F
HGG
I
KJJ1 13
Z P 2 23Z P 3Z2Z1Z 4Z 5Z 6Z
0 0 0
0
Z6
Z0 X6X0
d3d2
Y6Y0
i i-1 ai-1 di i1 0 0 0 12 -90 0 d2 23 90 0 d3 04 0 0 0 45 -90 0 0 56 90 0 0 6
Z6
Z2
Z3 Z4
Z5Z1Z0
X2
X3 X4
X5X6
X0X1
d3d2 d2 d3
x0
z0y0
1{0}
2
x6 y6z6
4 5
6
i i-1 ai-1 di i1 0 0 0 12 -90 0 d2 23 90 0 d3 04 0 0 0 45 -90 0 0 56 90 0 0 6
T =i - 11
ci -si 0 ai-1si ci-1 ci ci-1 -si-1 -s i-1 disi si-1 ci si-1 ci-1 ci-1 di
0 0 0 1
T = T T ... T0N0
1
1
2
N-1
NForward Kinematics:
11
10
1 1
1 1
0 00 0
0 0 1 00 0 0 1
T
c ss c=
L
N
MMMM
O
Q
PPPP
21
2 2
2
2 2
0 00 0 1
0 00 0 0 1
T
c sd
s c=
L
N
MMMM
O
Q
PPPP
32 3
1 0 0 00 0 10 1 0 00 0 0 1
Td=
L
N
MMMM
O
Q
PPPP
Stanford Scheinman Arm
43
4 4
4 4
0 00 0
0 0 1 00 0 0 1
T
c ss c=
L
N
MMMM
O
Q
PPPP
54
5 5
5 5
0 00 0 1 0
0 00 0 0 1
T
c s
s c=
L
N
MMMM
O
Q
PPPP
65
6 6
6 6
0 00 0 1 0
0 00 0 0 1
T
c s
s c=
L
N
MMMM
O
Q
PPPP
20
1 2 1 2 1 1 2
1 2 1 2 1 1 2
2 2 0 00 0 0 1
T
c c c s s s ds c s s c c ds c
=
L
N
MMMM
O
Q
PPPP
30
1 2 1 1 2 1 3 2 1 2
1 2 1 1 2 1 3 2 1 2
2 2 3 200 0 0 1
T
c c s c s c d s s ds c c s s s d s c ds c d c
=
+
L
N
MMMM
O
Q
PPPP
10
1 1
1 1
0 00 0
0 0 1 00 0 0 1
T
c ss c=
L
N
MMMM
O
Q
PPPP
1 2 4 1 4 1 2 4 1 4 1 2 1 3 2 1 2
1 2 4 1 4 1 2 4 1 4 1 2 1 3 2 1 204
2 4 2 4 2 3 2
0 0 0 1
c c c s s c c s s c c s c d s s ds c c c s s c s c c s s s d s c d
Ts c s s c d c
+ + + =
50
1 2 4 1 4 1 3 2 1 2
1 2 4 1 4 1 3 2 1 2
2 4 3 2
0 0 0 1
T
X X c c s s c c d s s dX X s c s c c s d s c dX X s s d c
= + +
L
N
MMMM
O
Q
PPPP
60
1 2 4 5 1 4 5 1 2 5 1 3 2 1 2
1 2 4 5 1 4 5 1 2 5 1 3 2 1 2
2 4 5 5 2 3 2
0 0 0 1
T
X X c c c s s s s c s s c d s s dX X s c c s c s s s s c s d s c dX X s c s c c d c
= + + + +
+
L
N
MMMM
O
Q
PPPP
C C C C S S C S S S S S C S C CS C C C S S C S S S C
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
1 2 4 5 6 4 6 2 5 6 1
[ ( ) ] ( )[ ( ) ]
+ + + + + + ( )
( ) +
+ +S C S C C
S C C S S C C S S4 5 6 4 6
2 4 5 6 4 6 2 5 6
C C C C C S S S S C S S C C C SS C C C C S S S S C C S C C C S
S C C C S S C S C
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6
2 4 5 6 4 6 2 5 6
[ ( ) ] ( )[ ( ) ] ( )
( )
+ + +
( )( )
+ + +
+
C C C S S C S S SS C C S S C C S S
S C S C C
1 2 4 5 2 5 1 4 5
1 2 4 5 2 5 1 4 5
2 4 5 2 5
+
C S d S dS S d C d
C d
1 2 3 1 2
1 2 3 1 2
2 3
1
2
3
Pxr
x rr
= =
60
1 2 4 5 1 4 5 1 2 5 1 3 2 1 2
1 2 4 5 1 4 5 1 2 5 1 3 2 1 2
2 4 5 5 2 3 2
0 0 0 1
T
X X ccc s ss s cs s cds sdX X sccs cs s ss c sds cdX X scs cc dc
= + + + +
+
L
N
MMMM
O
Q
PPPPZ6
Z0 X6X0
d3d2Y6
Y00 0 0
01 2 3
0 0 0 0 01 2 4 5 6
0 0 0
0
P P Px x xJ q q q
Z Z Z Z Z
= c d s s d c c d c ss d c s d s c d s s
s d cs c s c c s s c c c c s s s s c s cc s s s c s c c s c c s c s s s s c
c s s s c s c c
+
+
+ + + +
L
N
MMMMMMM
O
Q
P1 2 1 2 3 1 2 3 1 21 2 1 2 3 1 2 3 1 2
2 3 2
1 1 2 1 2 4 1 4 1 2 4 5 1 4 5 1 2 5
1 1 2 1 2 4 1 4 1 2 4 5 1 4 5 1 2 5
2 2 4 2 4 5 5 2
0 0 00 0 0
0 0 0 00 00 01 0 0
PPPPPP
Stanford Scheinman Arm Jacobian
12
Kinematic Singularity
( )1 2= " nJ J J J
The Effector Locality loses the abilityto move in a direction or to rotate abouta direction - singular direction
( )det 0=J( ) ( )det det=i jJ J
det[ ] det[ ]B AJ J
Kinematic Singularity
00
BB AA
BA
RJ J
R =
( ) ( )det det=i jJ J
Singular Configurations
det[ ( )] 0J q = Singular Configurations
1 2det[ ( )] ( ) ( )... ( ) 0sJ q S q S q S q= =1
2
( ) 0( ) 0
( ) 0s
S qS q
S q
==
=#
( )1 2 21 2 2
1 12 12
1 12 12
l S l S l SJ
l C l C l C
+ = + ( ) 1 2det 2=J l l S
Example (Kinematic Singularities)
1 21 12= +x l C l C1 21 12= +y l S l S
Singularity at 2 =q k
l1
l2
1y
{0}
{1}
2
(x,y)
At Singularity
2 21
1 2 2
2 21 12 21 1
l S l SC SJ
l l C l CS C = +
1 1 00J R J=
1
1 2 2
0 0J
l l l = +
l1
l2
1y
{0}
{1}
2
(x,y)
Example (Kinematic Singularities)
1
11 2 1 2 2
0 x
y l l l
== + +( )
L
NMMM
(x,y)
1 2 11(1)
1 2
1 2 2 1
1 1
1
l lJ
l ll l l
+
small 2
1 = q J X
Small Displacements ,q X = x
Xy
1q1q
{1} 2q 2q
13
(1) (1)1
1 2 1
1 = +x yql l( )1 2 (1) (1)
21 2 2 1
1+ = +l l x yql l l
Small Displacements ,q X = x
Xy
1q1q
{1} 2q 2q
(1)
1
yl
1q
2(1)
(1)
xy
(1)
1
yl
2q
21 2 (1)
2 (1)
( )+ l l xl y
1q1q
{1} 2q 2q
( )1 2 21 2 2
1 12 12
1 12 12
l S l S l SJ
l C l C l C
+ = + ( ) 1 2det 2=J l l S
Kinematic Singularities (reduced matrix)
Singularity at 2 =q k
l1
l2
1y
{0}
{1}
2
(x,y) ( )1 2 21 2 2
1 12 12
1 12 12
0 00 01 1
l S l S l S
l C l C l CJ
+ + =
l1
l2
l3
E
0
1 1 2 12 3 123 2 12 3 123 3 123
1 1 2 12 3 123 2 12 3 123 3 123
0 0 00 0 00 0 01 1 1
J
l s l s l s l s l s l sl c l c l c l c l c l c
E =
+ + +
L
N
MMMMMMM
O
Q
PPPPPPP
1 1 2 12 3 123 2 12 3 123 3 1230
1 1 2 12 3 123 2 12 3 123 3 123
1 1 1E
l s l s l s l s l s l sJ l c l c l c l c l c l c
= + + +
0
1 1 2 12 3 123 2 12 3 123 3 123
1 1 2 12 3 123 2 12 3 123 3 123
0 0 00 0 00 0 01 1 1
J
l s l s l s l s l s l sl c l c l c l c l c l c
E =
+ + +
L
N
MMMMMMM
O
Q
PPPPPPP Movie SegmentAutomatic Parallel Parking, INRIA, ICRA 1999 video proceedings
14
Stanford Scheinman Arm
d2d3
x0
z0y0
1{0}
2
x6 y6z6
4 5
6
i i-1 ai-1 di i1 0 0 0 12 -90 0 d2 23 90 0 d3 04 0 0 0 45 -90 0 0 56 90 0 0 6
T =i - 11
ci -si 0 ai-1si ci-1 ci ci-1 -si-1 -s i-1 disi si-1 ci si-1 ci-1 ci-1 di
0 0 0 1
T = T T ... T0N0
1
1
2
N-1
NForward Kinematics:
0 0 0
01 2 3
0 0 0 0 01 2 4 5 6
0 0 0
0
P P Px x xJ q q q
Z Z Z Z Z
= c d s s d c c d c ss d c s d s c d s s
s d cs c s c c s s c c c c s s s s c s cc s s s c s c c s c c s c s s s s c
c s s s c s c c
+
+
+ + + +
L
N
MMMMMMM
O
Q
P1 2 1 2 3 1 2 3 1 21 2 1 2 3 1 2 3 1 2
2 3 2
1 1 2 1 2 4 1 4 1 2 4 5 1 4 5 1 2 5
1 1 2 1 2 4 1 4 1 2 4 5 1 4 5 1 2 5
2 2 4 2 4 5 5 2
0 0 00 0 0
0 0 0 00 00 01 0 0
PPPPPP
Stanford Scheinman Arm Jacobian
5 k =
1 2 1 2 3 1 2 3 1 2
1 2 1 2 3 1 2 3 1 2
2 3 2
1 1 2 1 2 4 1 4 1 2
1 1 2 1 2 4 1 4 1 2
2 2 4 2
0 0 00 0 0
0 0 0 00 00 01 0 0
cd s s d cc d c ss d c s d sc d s s
s d cJ
s c s cc s s c c sc s s sc s cc s s
c s s c
+ = +
Stanford Scheinman Arm Jacobian Jacobian at the End-Effector
Pne{n}
{e}nvn
eve
v v Pe n n ne= + v v Pe n ne n
e n
= =
RST
15
v vee
n
n FHGIKJ =FHGIKJFHGIKJ
Pne
J Je n= FHGIKJ
Pne
v v Pe n ne ne n
= =
RST
J q J qe n = FHGIKJ
Pne
00
0
0J I P
IJe ne n= FHGIKJ
Cross Product Operator (in diff. frames)
0 0 0P R P R Pnn
nn= ( . ) .
0 0 0P R Pnn n = .( )
0 0 0 0 0 0 . .( . ) .( . . )P R P R P Rn n n n n n T = =0 0 0 n T
n nP R P R=
0 0 ;nnP R P
Pne{n}
{e}nvn
eve
nP
0 0 00
0
0
n Tnn n ne n
e nn
R R P RJ JR
=
i ji
ji
jJR
RJ=FHGIKJ
00 l1
l2
l3 W
E Wrist Pointx l c l cy l s l s
= += +
1 1 2 12
1 1 2 12
End-Effector Pointx l c l c l cy l s l s l s
= + += + +
1 1 2 12 3 123
1 1 2 12 3 123
l1
l2
l3 W
E Wrist Pointx l c l cy l s l s
= += +
1 1 2 12
1 1 2 12
End-Effector Pointx l c l c l cy l s l s l s
= + += + +
1 1 2 12 3 123
1 1 2 12 3 123Jacobian (W)
J
l s l s l sl c l c l c
W =
+
L
N
MMMMMMM
O
Q
PPPPPPP
1 1 2 12 2 12
1 1 2 12 2 12
00
0 0 00 0 00 0 01 1 1
00
0
0J I P
IJE WE W= FHGIKJ
;
l1
l2
l3 W
E Wrist Pointx l c l cy l s l s
= += +
1 1 2 12
1 1 2 12
End-Effector Pointx l c l c l cy l s l s l s
= + += + +
1 1 2 12 3 123
1 1 2 12 3 123
J
l s l s l sl c l c l c
W =
+
L
N
MMMMMMM
O
Q
PPPPPPP
1 1 2 12 2 12
1 1 2 12 2 12
00
0 0 00 0 00 0 01 1 1
0
1 1 2 12 3 123 2 12 3 123 3 123
1 1 2 12 3 123 2 12 3 123 3 123
0 0 00 0 00 0 01 1 1
J
l s l s l s l s l s l sl c l c l c l c l c l c
E =
+ + +
L
N
MMMMMMM
O
Q
PPPPPPP
16
l1
l2
l3 W
E Wrist Pointx l c l cy l s l s
= += +
1 1 2 12
1 1 2 12
End-Effector Pointx l c l c l cy l s l s l s
= + += + +
1 1 2 12 3 123
1 1 2 12 3 123
J
l s l s l sl c l c l c
W =
+
L
N
MMMMMMM
O
Q
PPPPPPP
1 1 2 12 2 12
1 1 2 12 2 12
00
0 0 00 0 00 0 01 1 1
00
0
0J I P
IJE WE W= FHGIKJ
03 123
3 1230
3 123
3 123
3 123 3 1230
0 00 0
0P
l cl s P
l sl c
l s l cWE WE=L
NMMM
O
QPPP =
F
HGG
I
KJJ
Resolved Motion Rate Control (Whitney 72)
x J= ( )Outside singularities
= J x1( )Arm at Configuration
x f= ( )x x xd= = J x1
+ = +
Resolved Motion Rate Control
xJ -1
q q
Control
Control
Control
Joint n
Joint 2
Joint 1q1
qn
q2 q2
qn
q1
ForwardKinematics
xdx
Linear & Angular Motion Velocity Propagation
J a c o b i a n Differential Motion
Explicit Form Static Forces
v p=
F
Angular/Linear Velocities/Forces
v
p F = p
v
p
Fp
v p= v p=
p F = p F =
Angular/Linear Velocities/Forces
xy
z
xy
z
( ) xy xy
Fp p
F =
x y
y x
v pv p
=
v J = T FJ =
( )Tp F =
17
Jx = Velocity/Force Duality
TFJ =
n f
1
2
3
PropagationElimination of Internal forces
Energy AnalysisVirtual Work
Static Equilibrium1
n f
2
3
n f
1
2
3
n f
-f -nlink 3n3 f3
-n1
-f1
-f2
-n2n1
f1
link 1
-n3
f2
n2 link 2-f3
Link i
Pi+1
ni fi
-fi+1 -ni+1
forces = 0 moments / a point = 0
About origin {i}f fn n P fi i
i i i i
+ =+ + =
+
+ + +
( )( ) ( )
1
1 1 1
00
f fn n P fi i
i i i i
== +
+
+ + +
1
1 1 1
Static EquilibriumLink i
Pi+1
ni fi
-fi+1 -ni+1
18
i zi
fi
i zi
ni
Prismatic Joint Revolute Joint i i T if Z= i i T in Z=Algorithm n n
n
nn
n nn
n
ii i
i ii
ii i
i ii
ii
ii
f f
n n P f
f R f
n R n P f
== +
== +
+
++
+
++
+ +
1
11
1
11
1 1
.
.
Virtual Work Principaln f
1
2
3
Ffn
= FHGIKJ
T Tq F x = x J q =T TF J =
Static Equilibrium:If the virtual work done by applied forces is zero in displacements consistent with constraints
Internalforces areworkless
i ii
w f x = appliedforces
virtualdisplacements
( ) 0TF x =using
TJ F =
1
2
3
= T q +
Jx = Velocity/Force Duality
TFJ =
( )1 2 21 2 2
1 12 12
1 12 12
l S l S l SJ
l C l C l C
+ = +
Example (Static Forces)
l1
l2
1y
{0}
{1}
2
(x,y)
( )1 2 1 22 2
1 12 1 12
12 12T l S l S l C l CJ
l S l C
+ + = TJ F =
( ) 1 21 2 1 222 2
1 121 12 1 12 01 1212 12
l C l Cl S l S l C l Cl Cl S l C
+ + + = = =
1N
1 2 1 21; 0; 60l l = = = = D3/ 21/ 2
Example (Static Forces)
l1
l2
1y
{0}
{1}
2
(x,y)
TJ F =( ) 1 21 2 1 2
22 2
1 121 12 1 12 0( 1 )
1 1212 12
l C l Cl S l S l C l CK
K l Cl S l C + + + = = =
1000N
1 2 1 21; 90; 0l l = = = = D
00
l2
y{1}
(x,y)
1000N
