1

Movie Segment
Skillful manipulation based on
high-speed sensory-motor
fusion, T. Senoo et al.,
University of Tokyo, ICRA 2009
video proceedings
Instantaneous
Kinematics
Differential Motion
{0}
{n}
{n}
Forward Kinematics
Instantaneous Kinematics
x x ou o u + +
x u
Relationship: ou o x

u x
Linear Velocity
Angular Velocity
Linear & Angular Motion
Velocity Propagation
J a c o b i a n
Differential Motion
Explicit Form
Static Forces
i i i i i
q d c u c = +
J oint Coordinates
i
i
revolute
:
d prismatic
coordinate i
u

J oint coordinate-i:
0 revolute
1 prismatic
i
c

=

with
1
i i
c c = and
J oint Coordinate Vector:
1 2
( .... )
T
n
q q q q =
2
J acobians: Direct Differentiation
( ); x f q =
x
x
x
f q
f q
f q
m m
1
2
1
2

F
H
G
G
G
G
I
K
J
J
J
J
=
F
H
G
G
G
G
I
K
J
J
J
J
( )
( )
( )
o
c
c
o
c
c
o
o
c
c
o
c
c
o
x
f
q
q
f
q
q
x
f
q
q
f
q
q
n
n
m
m m
n
n
1
1
1
1
1
1
1
= + +
= + +

o
c
c
c
c
c
c
c
c
o x
f
q
f
q
f
q
f
q
q
n
m m
n
=
L
N
M
M
M
M
M
O
Q
P
P
P
P
P
1
1
1
1

.
( 1) ( ) ( 1)
( )
m m n n
x J q q o o

=
Jacobian
( 1) ( ) ( 1)
( )
m m n n
x J q q

=
where
( ) ( )
ij i
j
J q f q
q
c
=
c
( 1) ( ) ( 1)
( )
m m n n
x J q q o o

=
( ) x J o u ou =
Example
l
1
l
2
(x,y) x l c l c
y l s l s
= +
= +
1 1 2 12
1 1 2 12
o ou ou
o ou ou
x l s l s l s
y l c l c l c
= +
= + +
( )
( )
1 1 2 12 1 2 12 2
1 1 2 12 1 2 12 2
o
o
o
ou
ou
X
x
y
y l s
x l c
=
L
N
M
O
Q
P
=

L
N
M
O
Q
P
F
H
G
I
K
J
2 12
2 12
1
2
J
x x
y y

F
H
G
G
G
I
K
J
J
J
c
cu
c
cu
c
cu
c
cu
1 2
1 2
( ) x J u u =

2 12
2 12
y l s
x l c
(
=
(

Stanford Scheinman Arm
d
2
d
3
x
0
z
0
y
0
u
1
{0}
u
2
x
6
y
6
z
6
u
4
u
5
u
6
i o
i-1
a
i-1
d
i
u
i
1 0 0 0 u
1
2 -90 0 d
2
u
2
3 90 0 d
3
0
4 0 0 0 u
4
5 -90 0 0 u
5
6 90 0 0 u
6
d
2
d
3
x
0
z
0
y
0
u
1
{0}
u
2
x
6
y
6
z
6
u
4
u
5
u
6
C C C C S S C S S S S S C S C C
S 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 S
4 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 S
S 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 S
S 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 d
S S d C d
C d
1 2 3 1 2
1 2 3 1 2
2 3
1
2
3
P
x
r
x r
r
| |
|
|
|
= =
|
|
|
\ .
3
Stanford ScheinmanArm
x
c s d s d
s s d c d
c d
p
=

+
L
N
M
M
M
O
Q
P
P
P
1 2 3 1 2
1 2 3 1 2
2 3

q
q
q
q
q
q
O
Q
P
P
P
L
N
M
M
M
M
M
M
M
O
Q
P
P
P
P
P
P
P
1
2
3
4
5
6
(31) (3 6) (6 1)
( )
p
p x x
x J q q

=

x
y
z
F
H
G
G
I
K
J
J
=
L
N
M
M
M
y c c d c s
x 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 0
0 0 0
0 0 0 0
Linear Velocity V Linear Velocity V
x
p
=
Position
x
r q
r q
r q
R
=
L
N
M
M
M
O
Q
P
P
P
1
2
3
( )
( )
( )

(
( )
(
x
r
r
r
r
q
r
q
r
q
r
q
r
q
r
q
q
q
q
R
x
x
x
=
F
H
G
G
I
K
J
J
=
F
H
G
G
G
G
G
G
I
K
J
J
J
J
J
J
F
H
G
G
G
G
I
K
J
J
J
J
1
2
3
9 1)
1
1
1
6
2
1
2
6
3
1
3
6
9 6
1
2
6
6 1)
c
c
c
c
c
c
c
c
c
c
c
c

( ) x J q q
R X
R
=
Orientation: Direction Cosines
C C C C S S C S S S S S C S C C
S 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 S
4 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 S
S 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 S
S 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
R
x =

(
( )
(
x
r
r
r
r
q
r
q
r
q
r
q
r
q
r
q
q
q
q
R
x
x
x
=
F
H
G
G
I
K
J
J
=
F
H
G
G
G
G
G
G
I
K
J
J
J
J
J
J
F
H
G
G
G
G
I
K
J
J
J
J
1
2
3
9 1)
1
1
1
6
2
1
2
6
3
1
3
6
9 6
1
2
6
6 1)
c
c
c
c
c
c
c
c
c
c
c
c

Max rank:
Representations
P
R
x
x
x
(
=
(

Cartesian
Spherical
Cylindrical
.
Euler Angles
Direction Cosines
Euler Parameters
.
J acobian for a representation X
( )
( )
P
R
P x
R x
x J q q
x J q q
=
=


( )
( )
P
R
x
P
R x
J q
x
q
x J q
| |
| |
=
|
|
|
\ .
\ .

(12 1) (12 6) (6 1)
( )
x x x x
x J q q =
The J acobian is dependent on the representation
Cartesian & Direction Cosines
Basic Jacobian
x E x v
x E x
P P P
R R R
e
=
=
( )

( )
F I
x
H K
(6 1)
v
J q q
xn nx
e
G J
=
( ) (
( )
0 6 1)
{0}
angular velocity
linear velocity
e
v
4
Examples
1 0 0
0 1 0
0 0 1
P
x
x y v
z
| | | |
| |
= =
| |
| |
\ . \ .

*
1 0 0
( ) 0 1 0
0 0 1
P P
E x
| |
|
=
|
|
\ .
Examples
. .
1
0
0
R
s c c c
s s
x c s
s c
s s
o | o |
o | |
| o o e
o o
| |
| |

|
| |
|
|
= = |
|
|
|
\ .
|

|
\ .

*
. .
1
( ) 0
0
R R
s c c c
s s
E x c s
s c
s s
o | o |
| |
o o
o o
| |
| |

|
|
= |
|
|

|
\ .
J acobian for X
( )
x
x J q q =
0
( ) ( ) ( )
x
J q E x J q =
P
R
x
x
x
(
=
(

Given a representation
0
( )
v
J q q
e
| |
=
|
\ .

Basic J acobian
Jacobian and Basic Jacobian
v
v J q
J q
e
e
=

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

0
0
P
R
X P
v
x
R X
J E
J
J
E J J
e
| |
| | | |
= = |
| |
|
\ . \ .
\ .
0
( ) ( ) ( )
x
J q E x J q =
0
( )
v
J q q
e
| |
=
|
\ .

With Cartesian Coordinates
3
; ;
P
P x v
E I J J = = and
0
0
R
I
E
E
| |
=
|
\ .
Position Representations
x
y
z
Cartesian: (x, y, z)
Cylindrical: (, u, z)
Spherical: (r, u, |)
r

u
|
5
Position Representations
3
( )
P
E X I =
Cartesian Coordinates ( , , ) x y z
cos sin 0
( ) sin cos 0
0 0 1
P
E X
u u
u u

| |
|
| =
|
|
\ .
Cylindrical Coordinates ( , , ) z u
Using ( ) ( cos sin )
T T
x y z z u u =
( ) ( )
cos sin sin sin cos
( ) sin cos 0
sin sin
cos cos sin cos sin
P
E X
u | u | |
u u
| |
u | u | |

| |
|
|
|
=
|
|
|
|
\ .
Spherical Coordinates ( , , ) u |
( ) ( cos sin sin sin cos )
T T
x y z u | u | u =
Using
Euler Angles
Singularity of the representation
for k | t =
. .
1
; ( ) 0
0
R R R
s c c c
s s
x E x c s
s c
s s
o | o |
o | |
| o o
o o
| |
| |

|
| |
|
|
= = |
|
|
|
\ .
|

|
\ .
J acobian for X
( )
x
x J q q =
0
( ) ( ) ( )
x
J q E x J q =
P
R
x
x
x
(
=
(

Given a representation
0
( )
v
J q q
e
| |
=
|
\ .

Basic J acobian
Jacobian
v
J
q q
x
xn nx
e
F
H
G
I
K
J
=
(
( ) (
( )
6 1)
6 1)
{0}
angular velocity
linear velocity
e
v
Linear & Angular Velocities
{0}
angular velocity
linear velocity
e
v
6
Linear Velocity
v
P A /
P
{A}
{B}
{C}
v
P A /
A
v
P A /
C
v
P A /
B
Pure Translation
{B}
v
P A /
{A}
v
A B /
v
P B /
{B}
v
P A /
{A}
v
A B /
v v v
P B A B P A / / /
= +
Pure Translation
Rotational Motion
rigid body
fixed points on the rigid body
Axis of rotation
O Angular Velocity
Rotational Motion
O Angular Velocity
v
R
Rotational Motion
P
7
O Angular Velocity
P
v
P
v
R
?
P
v =
Rotational Motion
O Angular Velocity
P
v
P
fixed point
v
R
Rotational Motion
O Angular Velocity
P
v
P
P
fixed point
v
R
Rotational Motion
O Angular Velocity
Psin|
P
v
P
P
|
fixed point
v
R
v
P
is proportional to:
||O||
||Psin|||
and
v
P
_| O
v
P
_| P
v P
P
= O
Rotational Motion
c a b =
Cross Product Operator
0
0
0
z y x
z x y
y x z
a a b
c ab a a b
a a b
( (
(
(
= =
(
(
(
(


c ab =
: a skew-symmetric matrix a a
vectors
matrices
,
x x
y y
z z
a b
a a b b
a b
( (
( (
= =
( (
( (

c ab =
v P v P
P P
= = O O

Cross Product Operator

O
O
O
O
=
L
N
M
M
M
O
Q
P
P
P
x
y
z
;P
P
P
P
x
y
z
=
L
N
M
M
M
O
Q
P
P
P
v P
P
P
P
P
z y
z x
y x
x
y
z
= =

L
N
M
M
M
O
Q
P
P
P
L
N
M
M
M
O
Q
P
P
P

. O
O O
O O
O O
0
0
0
v P
P
=

O
: a skew-symmetric matrix O O

8
Simultaneous linear and angular motion
{A}
{B}
v
B A /
O v
P B /
P
A
P A
A
B A B
A B
P B
A
B B
A B
B
v v R v R P
/ / /
. . = + + O
v v v P
P A B A P B B / / /
= + + O
Spatial Mechanisms
{2}
{0}
{1}
{n}
e
v
( ).

x J = u u
x
v : linear velocity
e : angular velocity
Propagation of velocities
{i}
e
i
v
i
v
i+1
e
i+1
O
i+1
{i+1}
z
i
P
i+1
z
i+1
e e
u
i i i
i i i
Z
+ +
+ + +
= +
=
1 1
1 1 1
O
O

.
v v P d Z
i i i i i i + + + +
= + +
1 1 1 1
e

.
Velocity propagation
Linear
Angular
i
i i
i i
i
i
i
i
i i
i
i
v R v P d Z
+
+
+
+ +
+
+
= + +
1
1
1
1 1
1
1
.( )

. e
Velocity propagation
J oint 1
v
1
and e
1
in frame {1}
J oint i+1
i
i i
i i
i i
i
i
R Z
+
+
+
+
+
+
= +
1
1
1
1
1
1
e e u .

.
n
n
e
n
n
v and
0
0
0
0
0
0
v R
R
v
n
n
n
n
n
n
n
n
e e
F
H
G
I
K
J
=
F
H
G
I
K
J
F
H
G
I
K
J
.
{n-1}
{n}
v
e
Example

u
1
{0}
u
1
P
1
P
2
P
3
l
1
l
2
l
3

u
2

u
3
v v P
i i i i + +
= +
1 1
e

v
v v P
v v P
P
P P
P P
1
2 1
3 2
0
1 2
2 3
=
= +
= +
e
e
0
1 1
0
1
e u =

. Z
0
1
1
1 1
1 1
1 1
1 1 1
2
0
0 0
0 0
0 0 0 0 0
v
l c
l s
l s
l c
P
= +
L
N
M
M
M
O
Q
P
P
P
L
N
M
M
M
O
Q
P
P
P
=

L
N
M
M
M
O
Q
P
P
P

.
.
.
.
. .

u
u u
0 0 0
2
0
3
3 2
v v P
P P
= + e
0
1 1
1 1 1 1 2
0
3
3
0
0 1 0
1 0 0
0 0 0
v
l s
l c P
P
=

L
N
M
M
M
O
Q
P
P
P
+

L
N
M
M
M
O
Q
P
P
P
+
.
. .

.(

). u u u
=

L
N
M
M
M
O
Q
P
P
P
+

L
N
M
M
M
O
Q
P
P
P
+
l s
l c
l s
l c
1 1
1 1 1
2 12
2 12 1 2
0 0
.
. .

.
. .(

) u u u
0
3 1 2 3
0
0
e u u u = + + (

). Z
l c
l s
2 12
2 12
0
.
.
L
N
M
M
M
O
Q
P
P
P
9
0
1 1 2 12 2 12
1 1 2 12 2 12
1
2
3
3
0
0
0 0 0
v
l s l s l s
l c l c l c
P
=
+
+
L
N
M
M
M
O
Q
P
P
P
L
N
M
M
M
O
Q
P
P
P
( )
.

u
u
u
0
3
1
2
3
0 0 0
0 0 0
1 1 1
e
u
u
u
=
L
N
M
M
M
O
Q
P
P
P
L
N
M
M
M
O
Q
P
P
P
.

v
J
e
u
u
u
F
H
G
I
K
J
=
F
H
G
G
G
I
K
J
J
J
.

1
2
3
v
J
J
e