Kinematics: Velocity

Kinematics - the Jacobian

Doan The Thao
 Derivatives of Vector-Valued Functions
Definition of Vector-Valued Functions
• A vector-valued function of one variable in Cartesian
3D space has the form
r (t )  f (t )i  g (t ) j  h(t )k or r (t )   f (t ) g (t ) h(t ) 
T

where f(t), g(t), h(t) are called the component functions.

Limits and Continuity of Vector-Valued Functions
• Suppose that r (t )   f (t ) g (t ) h(t ) 
T

The limit of r(t) as t approaches a is given by

lim r (t )  lim f (t ) lim g (t ) lim h(t )   r( a)
t a  t a t a t a 
provided the limit of the component functions exist.
• The vector-valued function r(t) is continuous at t=a if
lim r (t )  r ( a)
t a
 Derivative of a Vector-Valued Function

• The derivative r′(t) of the vector-valued function r(t) is

defined by r (t  t )  r (t ) dr
r(t ) or r (t )  lim 
t 0 t dt
for any values of t for which the limit exists.
• The vector r′(t) is called the tangent vector to the
curve defined by r
• If r (t )   f (t ) g (t ) h(t ) 
T

where f, g and h are differentiable functions, then

f (t  t )  f (t ) g (t  t )  g (t ) h(t  t )  h(t ) 
T

r(t )   lim lim lim 
 t 0 t t 0 t t  0 t 
r(t )   f (t ) g (t ) h(t ) 
T

Thus, we can differentiate vector-valued functions by

differentiating their component functions.
 Some Basics on Frames and Derivatives of Vectors
• A reference frame is a perspective from which a
system is observed.
• A vector can change in both magnitude and direction
• dR/dt cannot be evaluated except
by specific association with a frame
of reference.
• If α is a scalar and A and B are vector
functions of t, then
David J. McGill _ Wilton W. King
d d dA d dA dB
Engineering mechanics an
introduction to dynamics ( A)  A  ; ( A  B)   ;
dt dt dt dt dt dt
d dA dB
( A  B)  B  A ;
dt dt dt
d dA dB
( A  B)  B  A ;
dt dt dt
 Physical Interpretation
• If r(t) represents the position of a particle, then the
derivative is the velocity of the particle:
dr
v (t )   r(t )
dt
• The speed of the particle is the magnitude of the
velocity vector:
v (t )  ( f (t )) 2  ( g (t )) 2  (h(t )) 2
• In a similar way, the derivative of the velocity is the
acceleration:

dv
a(t )   v(t )  r(t )
dt
ANGULAR VELOCITY: THE FIXED AXIS CASE
rOP [ s (t )];
drOP drOP ds drOP
  s (t )
dt ds dt ds
In the limit:
rOP  s; rOP  et

Velocity is always
Georgia Tech tangent to the path
drOQ drOP drPQ
rOQ  rOP  rPQ ;   ; vQ  v P  vQ / P
dt dt dt
rPQ  rPQ [cos( )i  sin( ) j ]
drPQ
 rPQ [ ( sin( )i  cos( ) j)]  rPQ [ k  (cos( )i  sin( ) j)];
dt
drPQ
 v Q / P  ω  rPQ
dt
 ANGULAR VELOCITY: THE FIXED AXIS CASE
• If k is a unit vector in the direction of the axis of
rotation, then the angular velocity is given by
0
dθ d  dk d 
θ  θ k   k; ω  k   k  k
dt dt dt dt
The linear velocity of any point on the body is given by the
equation: v  ωr
^

ω d XB ^ ^ ^
 ω  X B   k  X B   (k  X B )
dt

v
r
k
Georgia Tech
O
 SKEW SYMMETRIC MATRICES

Homogeneous Transformation
A
P  ABT B P  A PBORG  BA R B P
Compound Transformations
A
C T T T
A
B
B
C

 A
R B
CR
A
R B PCorg  A PBorg 
CT  
A B B

 0 1 
dR ( )
• How to compute ?
dt
A B
• How to compute d ( B R C R)
?
dt
 SKEW SYMMETRIC MATRICES

• Symmetric matrices A T
A
• Skew Symmetric matrices S  S; S  S  0
T T

• If a = [ax, ay, az]T is a 3-vector, we define the skew

symmetric matrix S(a) as

 0 az ay 
 
S(a)   az 0 ax 
 a y ax 0 


0 0 0   0 0 1 0 1 0 
S(i )  0 0 1 S( j)   0 0 0  S(k )  1 0 0 
0 1 0   1 0 0  0 0 0 
 Properties of Skew Symmetric Matrices

1. The operator S is linear, i.e.,

S( a   b)   S(a)   S(b)
for any vectors a and b belonging to R3 and scalars 
and β.

2. For any vectors a and p belonging to R3,

S(a)p  a  p
3. If R is a rotation matrix and a, b are vectors in R3 it can
also be shown by direct calculation that:
R (a  b)  Ra  Rb
 Properties of Skew Symmetric Matrices
Derivation of Equation: R (a  b)  Ra  Rb
(a  b)  (c  d)  [a  c][b  d]  [a  d][b  c]
a  [ax a y az ]T b  [bx by bz ]T c  [cx c y cz ]T d  [d x d y d z ]T
 0 az a y  bx   az by  a y bz   0 c z c y   d x   c z d y  c y d z 
       
a  b   az 0  ax  by    az bx  ax bz  ; c  d   cz 0 cx   d y    cz d x  cx d z  ;
 a y ax 0   bz   a y bx  ax by   c y cx 0   d z   c y d x  cx d y 
 
T
  az by  a y bz   c z d y  c y d z 
   
(a  b)  (c  d)   az bx  ax bz   z x x z 
c d  c d
  a y bx  ax by   c y d x  c x d y 
   
 [ az by  a y bz ][cz d y  c y d z ]  [az bx  axbz ][cz d x  cx d z ]  [a y bx  a xby ][c y d x  cx d y ]
 az by cz d y  a y bz c y d z  az bx cz d x  ax bz cx d z  a y bx c y d x  a xby cx d y 
 az by c y d z  a y bz cz d y  az bx cx d z  a xbz cz d x  a y bx cx d y  a xby c y d x
 [ax cx  a y c y  az cz ][bx d x  by d y  bz d z ]  a x cxbx d x  a y c y by d y  a z cz bz d z
[ax d x  a y d y  az d z ][bx cx  by c y  bz cz ]  a x d xbx cx  a y c y by d y  az cz bz d z
 [a  c][b  d]  [a  d][b  c]
 (a  b)  (c  d)  [a  c][b  d]  [a  d][b  c]
 Properties of Skew Symmetric Matrices
Derivation of Equation: R (a  b)  Ra  Rb
 r1   r1  a   r1  b 
R  r2  ;  Ra  r2  a  ; Rb  r2  b 
 r3   r3  a   r3  b 

 0 r3  a r2  a   r1  b  [r2  a][r3  b ]  [r3  a][r2  b ]

Ra  Rb   r3  a 0 r1  a  r2  b    [r3  a][r1  b ]  [r1  a][r3  b ] 
 r2  a r1  a 0  r3  b   [r1  a][r2  b]  [r2  a][r1  b ] 

(a  b)  (c  d)  [a  c][b  d]  [a  d][b  c]

[r2  a][r3  b]  [r2  b][r3  a] (r2  r3 )  (a  b) 

Ra  Rb   [r3  a][r1  b]  [r3  b][r1  a]    (r3  r1 )  (a  b) 
 [r1  a][r2  b]  [r1  b][r2  a]   (r1  r2 )  (a  b) 
 Properties of Skew Symmetric Matrices
Derivation of Equation: R (a  b)  Ra  Rb
 m1 
For 3x3 matrix M  m 2  , we know that:
 m 3 

det(M )  (m1  m 2 )  m 3  (m 2  m 3 )  m1  (m 3  m1 )  m 2
Additionally, since R is a rotation matrix, we know that:
det(R )  (r1  r2 )  r3  (r2  r3 )  r1  (r3  r1 )  r2  1
and r3  r3  r1  r1  r2  r2  1
Taking this into consideration
(r1  r2 )  r3  r3  r3 ; (r2  r3 )  r1  r1  r1 ; (r3  r1 )  r2  r2  r2
And thus
(r1  r2 )  r3 ; (r2  r3 )  r1 ; (r3  r1 )  r2
 Properties of Skew Symmetric Matrices
Derivation of Equation: R (a  b)  Ra  Rb
(r2  r3 )  (a  b) 
Ra  Rb   (r3  r1 )  (a  b) 
 (r1  r2 )  (a  b) 

(r1  r2 )  r3 ; (r2  r3 )  r1; (r3  r1 )  r2

Baring this in mind without loss of generality, this allows:

(r2  r3 )  (a  b)   r1  (a  b) 
Ra  Rb   (r3  r1 )  (a  b)   r2  (a  b)   R (a  b)
 (r1  r2 )  (a  b)   r3  (a  b) 
 Properties of Skew Symmetric Matrices

4. For R is a rotation matrix and a vector a belonging to R3

RS(a)R T  S(Ra) R (a  b)  Ra  Rb
Proof:

RS(a)R T b  R (a  R T b) 
RS(a)R T b  Ra  [R (R T b)]  R (a)  b  S(Ra)b
 RS(a)R T  S(Ra)
 The Derivative of a Rotation Matrix
dR ( ) T
Proof that R ( ) is a skew symmetric matrix
d
 Xˆ B  Xˆ A YˆB  Xˆ A Zˆ B  Xˆ A   Xˆ B  Xˆ A Xˆ B  YˆA Xˆ B  Zˆ A 
  A T  
ˆ ˆ YˆB  YˆA Zˆ B  YˆA  ; B R   YˆB  Xˆ A YˆB  YˆA YˆB  Zˆ A 
B R   X B  YA
A

 ˆ ˆ ˆ ˆ ˆ ˆ  ˆ ˆ ˆ ˆ ˆ ˆ 
 X B  Z A YB  Z A Z B  Z A   Z B  X A Z B  YA Z B  Z A 
 Xˆ B  Xˆ A YˆB  Xˆ A Zˆ B  Xˆ A   Xˆ B  Xˆ A Xˆ B  YˆA Xˆ B  Zˆ A 
dR ( ) T d  ˆ ˆ ˆ ˆ ˆ ˆ

ˆ ˆ ˆ ˆ ˆ ˆ

R ( )   X B  YA YB  YA Z B  YA   YB  X A YB  YA YB  Z A 
d d 
ˆ  Zˆ ˆ  Zˆ ˆ  Zˆ   Zˆ  Xˆ ˆ  Yˆ Zˆ  Zˆ 
 B A
X Y B A Z B A  B A Z B A B A

 Xˆ B  Xˆ A YˆB  Xˆ A Zˆ B  Xˆ A   Xˆ B  Xˆ A Xˆ B  YˆA Xˆ B  Zˆ A   s s12 s13 
d  ˆ ˆ    11
ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ 
 X B  YA YB  YA Z B  YA   YB  X A YB  YA YB  Z A    s21 s22 s23 
d   ˆ ˆ  s
ˆ  ˆ ˆ  ˆ ˆ  ˆ  ˆ  ˆ ˆ  ˆ  s32 s33 
 X B Z A Y B Z A Z B Z A   Z B X A Z B Y A Z B Z A  31
 The Derivative of a Rotation Matrix

d ˆ ˆ d ˆ ˆ d ˆ ˆ
s11  [ ( X B  X A )][ X B  X A ]  [ (YB  X A )][YB  X A ]  [ ( Z B  X A )][ Zˆ B  Xˆ A ]
ˆ ˆ ˆ ˆ
d d d
d ˆ d d
s11  [( X B )  ( Xˆ A )][ Xˆ B  Xˆ A ]  [( YˆB )  ( Xˆ A )][YˆB  Xˆ A ]  [( Zˆ B )  ( Xˆ A )][ Zˆ B  Xˆ A ]
d d d
d ˆ d d
s11  ([( X B )  ( Xˆ A )][ Xˆ B ]  [( YˆB )  ( Xˆ A )][YˆB ]  [( Zˆ B )  ( Xˆ A )][ Zˆ B ])  ( Xˆ A )
d d d
s11  ([(k  Xˆ B )  ( Xˆ A )][ Xˆ B ]  [(k  YˆB )  ( Xˆ A )][YˆB ]  [(k  Zˆ B )  ( Xˆ A )][ Zˆ B ])  ( Xˆ A )
s  ([( Xˆ  k )  ( Xˆ )][ Xˆ ]  [( Xˆ  k )  (Yˆ )][Yˆ ]  [( Xˆ  k )  ( Zˆ )][ Zˆ ])  ( Xˆ )
11 A B B A B B A B B A

s11  ( Xˆ A  k )  ( Xˆ A )  0

Scalar triple product

a.(b  c)  b.(c  a )  c.(a  b)
 The Derivative of a Rotation Matrix

 Xˆ B  Xˆ A YˆB  Xˆ A Zˆ B  Xˆ A   Xˆ B  Xˆ A Xˆ B  YˆA Xˆ B  Zˆ A 
dR ( ) T d  ˆ ˆ  
R ( )   X B  YA YB  YA Z B  YA   YˆB  Xˆ A
ˆ ˆ ˆ ˆ Yˆ  Yˆ ˆ ˆ
YB  Z A 
d d 
B A

ˆ ˆ ˆ ˆ ˆ ˆ  ˆ ˆ 
 X B  Z A YB  Z A Z B  Z A   Z B  X A Zˆ B  YˆA ˆ ˆ
Z B  Z A 
d d d
s12  [ ( Xˆ B  Xˆ A )][ Xˆ B  YˆA ]  [ (YˆB  Xˆ A )][YˆB  YˆA ]  [ ( Zˆ B  Xˆ A )][ Zˆ B  YˆA ]
d d d
d d d
s21  [ ( Xˆ B  YˆA )][ Xˆ B  Xˆ A ]  [ (YˆB  YˆA )][YˆB  Xˆ A ]  [ ( Zˆ B  YˆA )][ Zˆ B  Xˆ A ]
d d d
 The Derivative of a Rotation Matrix

d ˆ ˆ d d
s12  [ ( X B  X A )][ Xˆ B  YˆA ]  [ (YˆB  Xˆ A )][YˆB  YˆA ]  [ ( Zˆ B  Xˆ A )][ Zˆ B  YˆA ]
d d d
d d d
s12  ([ ( Xˆ B  Xˆ A )][ Xˆ B ]  [ (YˆB  Xˆ A )][YˆB ]  [ ( Zˆ B  Xˆ A )][ Zˆ B ])  (YˆA )
d d d
s12  ([(k  Xˆ B )  ( Xˆ A )][ Xˆ B ]  [(k  YˆB )  ( Xˆ A )][YˆB ]  [(k  Zˆ B )  ( Xˆ A )][ Zˆ B ])  (YˆA )
s12  ( Xˆ A  k )  (YˆA )  (k  YˆA )  ( Xˆ A )
d ˆ ˆ d d
s21  [ ( X B  YA )][ Xˆ B  Xˆ A ]  [ (YˆB  YˆA )][YˆB  Xˆ A ]  [ ( Zˆ B  YˆA )][ Zˆ B  Xˆ A ]
d d d
s21  ([(k  Xˆ B )  (YˆA )][ Xˆ B ]  [(k  YˆB )  (YˆA )][YˆB ]  [(k  Zˆ B )  (YˆA )][ Zˆ B ])  ( Xˆ A )
s  (Yˆ  k )  ( Xˆ )   s
21 A A 12
 The Derivative of a Rotation Matrix
Suppose now that a rotation matrix R is a function of
the single variable .
Since R is orthogonal for all  it follows that:
R ( )R ( )  I
T

Differentiating both sides of above equation with respect to

 gives: S ST
d dR ( ) T d
[R ( )R ( )] 
T
R ( )  R ( ) [ R T ( )]  0
d d d
dR ( ) T
S
Let us define the matrix S as R ( ) (*)
d
d R ( ) d R ( ) d R T
( )
S [
T
R ( )]  R ( )[
T T
]  R ( )
T

d d d
S defined by Equation (*) is skew symmetric.
 The Derivative of a Rotation Matrix
Example
If R()=Rz(), that is the basic rotation around the axis z, then:
  s c 0   c s 0
dR z ( ) T
S R z ( )   c  s 0    s c 0 
d
 0 0 0   0 0 1 
  s c  c s  s s  c c 0  0 1 0 
S   c c  s s c s  c s 0  ; S  1 0 0   S(k )
 0 0 0  0 0 0 
dR z ( ) T dR z ( )
R z ( )  S(k )   S(k )R z ( )
d d
dR z ( ) dR x ( )
 S(k )R z ( );  S(i )R x ( );
d d
dR y ( ) dR k ( )
 S( j)R y ( );  S(k )R k ( );
d d
 The Derivative of a Rotation Matrix

dR ( ) T dR ( )
SR ( )  [ R ( )]R ( ) 
d d

dR ( )
  SR ( )
d

This equation is very important. The most commonly

encountered situation is the case where R is a basic
rotation matrix or a product of basic rotation matrices.
 The Derivative of a Rotation Matrix

d d d
dR ( ) R z ( )  R z ( )
  SR ( ) dt d dt
d d d .
R z ( )  R z ( ) 
dt d
d .
R z ( )  S(k ) R z ( ) 
dt

d
R ( )  S(ω) R ( )
dt
 ADDITION OF ANGULAR VELOCITIES
We now derive the expressions for the composition of
angular velocities of two moving frames o1x1y1z1 and
o2x2y2z2 relative to the fixed frame o0x0y0z0.
0
2 R (t )  01 R (t ) 21 R (t )
Taking derivatives of both sides of above equation with
respect to time yields

d 0 d 0 d 1
2 R (t )  [ 1 R (t )] 2 R (t )  1 R (t ) [ 2 R (t )]
1 0

dt dt dt
S(ω 00,2 ) 20 R (t )  S(ω 0,10
) 01 R (t ) 21 R (t )  01 R (t )S(ω1,2
1
) 21 R (t )
 ADDITION OF ANGULAR VELOCITIES

S(ω 00,2 ) 20 R (t )  S(ω 0,1

0
) 01 R (t ) 21 R (t )  01 R (t )S(ω1,2
1
) 21 R (t )
S(ω 00,2 ) 20 R (t )  S(ω 0,1
0
) 20 R (t )  01 R (t )S(ω1,2
1
) 01 R T (t ) 01 R (t ) 21 R (t )
S(ω 00,2 ) 20 R (t )  S(ω 0,1
0
) 20 R (t )  S[ 01 R (t )ω1,2
1
] 20 R (t )
 S(ω 00,2 )  S(ω 0,1
0
)  S[ 01 R (t )ω1,2
1
]

Since S(a) + S(b) = S(a + b), we see that

ω 0
0,2  ω  R (t )ω
0
0,1
0
1
1
1,2
 ADDITION OF ANGULAR VELOCITIES

In particular, suppose that we are given

0
n R  01 R 21 R... n n1 R
d 0
n R  S (ω 0, n n R;
0
) 0

dt
in which
n 1
ω 0
0, n  ω  Rω  Rω
0
0,1
0
1
1
1,2
0
2
2
2,3  ...  0
n 1 Rω n 1, n

ω 0
0, n  ω ω ω0
0,1
0
1,2
0
2,3  ...  ω 0
n 1, n
 LINEAR VELOCITY OF A POINT ATTACHED TO A
MOVING FRAME

Suppose that the motion of the frame onxnynzn relative to

o0x0y0z0 is more general. Suppose that the homogeneous
transformation relating the two frames is time-dependent,
so that
d 0 d d
 0
R O 0
 0
P  n0 R n P  O 0n ; P  [ n0 R n P ]  O 0n
nT  
n n

0 dt dt dt
 0 1  d d 0 n d n 0d 0
0
P  [ n R] P  n R 0
P  On
dt dt dt dt
v P  ω 00,n  0 P  v On d 0 0 0 n d 0
P  S(ω 0,n ) n R P  O n
dt dt
If we know the angular velocity ω
0
n of the end-effector frame
and the linear velocity v On of the the end-effector frame origin
Then the velocity of any point in the end-effector frame can
be computed.
 DERIVATION OF THE JACOBIAN

• Consider an n-link manipulator with the vector of joint

q(t )   q1 (t ) q2 (t ) qn (t ) 
variables T

• What do the functions n and v On

ω 0
depend on?
We seek expressions of the form

 v 0n   Jv 
ξ   0   Jq    q
ω 0,n  J 
where Jv and J are 3×n matrices.
• The vector  is sometimes called a body velocity
• The matrix J is called the Manipulator Jacobian or
Jacobian for short. Note that J is a 6×n matrix where n is
the number of links.
 Angular Velocity Jacobian
• If the ith joint is revolute: the ith joint variable qi equals i and
the axis of rotation is zi−1;
Let ωii 11,i represent the angular velocity of link i with respect to
frame oi−1xi−1yi−1zi−1. Then we have i 1 i 1
ωi 1,i  qi z i 1
• If the ith joint is prismatic: the motion of frame i relative to
frame i-1 is a translation and ωii 11,i  0
• By using already derived formula:
ω00,n  ω0,1
0
 ω1,2
0
 ω02,3  ...  ω0n1,n , we get

ω 00,n  1q1z 00   2 q2 01 Rz11  3 q3 20 Rz 22  ...   n qn n 01 Rz nn 11

ω 00,n  1z 00 q1   2 z10 q2  3 z 02 q3  ...   n z 0n 1qn
1 if joint i is revolute
where i  
0 if joint i is prismatic
 Angular Velocity Jacobian

• The complete Jacobian:

 v 0n   J v 
 0    q
ω 0,n   J  

• Jacobian for angular velocities:

J   [ 1z 00  2 z10 3 z 02  n z 0n 1 ]  [J 
1
J 2 J 3 J n ]
J i  i z i01 (i  1..n)
 Linear velocity Jacobian
0
• The linear velocity of the end effector is just O n
• By the chain rule for differentiation

O 0
O 0
O 0n
O 0n  n q1  n q2   qn
q1 q2 qn
we find Jacobian for linear velocities

 O 0n O 0n O 0n 
Jv      J v1 J v2 J vn 
 q1 q2 qn 
O 0n
J vi 
qi
 Linear velocity Jacobian
Case 1: prismatic joints
0
We can write the T as the product of three transformations
n
as follows
0 i 1 i
 0
i 1 R O 0
i 1
  i 1
R O i 1
  i
R O i

n T  i 1T iT nT  
i i n n
  
0

 0 1   0 1   0 1 
 0
iR
0
i 1 ROii 1  Oi01   ni R Oin   n0 R 0
ROin  i 01 ROii 1  Oi01 
nT   
i
 
0

 0 1   0 1   0 1 

 0
R O 0
  0
nR
0
ROin  i 01 ROii 1  Oi01 
nT   
n n i

0

 0 1   0 1 

which gives O 0n  0i ROin  i 01 ROii 1  Oi01

 Linear velocity Jacobian 0
i 1
Case 1: prismatic joints O  RO  RO  O
0
n
0
i
i
n
0
i 1 i
0
i 1

If only joint i is allowed to move,

then both of Oin and Oi0 1 are
constant.
Furthermore, if joint i is prismatic,
0
R
then the rotation matrix i is also
constant

Finally, recall that, by the DH

convention,
i 1
  ai ci ai si di 
T
O i

d 0 d 0 i 1
Thus, differentiation of O n  ( i 1 ROi )
gives dt dt
 Linear velocity Jacobian
Case 1: prismatic joints

d 0   0 i 1  i 1
On  (O n )di 
0
( i 1 ROi )di  i 1 R[ Oi ]di
0

dt di di di

 ai ci  0
  
O n  i 1 R (  ai si  )di  i 01 R 0  di  i 01 Rz i 1di  z i01di
d 0 0

dt di
 di  1 


J vi  (O n )  z i 1
0 0

di
Linear velocity Jacobian
Case 2: revolute joints
If joint i is revolute, then we
have qi = i.
Oii 1
O0n  0i ROin  i 01 ROii 1  Oi01
0
O i 1
n O i since R is not constant with
i
n
respect to i, we obtain:

d 0   0 i
On  (O 0n )i  ( i RO n  i 01 ROii 1  Oi01 )i
dt i i
0
d 0  0 i  0 i 1 
O n  [ ( i RO n )  ( i 1 ROi )  (Oi01 )]i
dt i i i
Linear velocity Jacobian
Case 2: revolute joints d O0n  [  ( 0i ROin )   ( i 01 ROii 1 )]i
dt i i
i 1
d 0  0i R i O
On  [ O n  i 01 R i ]i
dt i i
 0i R i
O n  S(z i01 ) 0i ROin  z i01  0i ROin
i

i 1
 ai ci   ai si 
O    0    0 RS(k )Oi 1
0
i 1 R i
 0
i 1 R  a s
i i  i 1 R  a c
i i  i 1
i i
i

 di   0 
i 1
O i 1 i 1
0
i 1 R i
 0
i 1 RS (k ) 0 T 0
i 1 R i 1 RO  S ( 0
i 1 Rk ) 0
i 1 RO
i
i i

i 1
O i 1
0
i 1 R i
 z 0
 0
i 1 i 1 RO
i
i
 Linear velocity Jacobian
Case 2: revolute joints

i 1
d 0  R i 0 O 0
On  [ O n  i 1 R
i
]i i

Oii 1 dt i i

 [z  RO  z  RO ]i
0
i 1
0
i
i
n
0
i 1
0
i 1
i 1
i
Oin1 Oin  z i01  ( 0i ROin  i 01 ROii 1 )
 z i01  (Oi0,n  Oi01,i )
 z O
0
i 1
0
i 1, n

J vi  (O 0n )  z i01  (O 0n  Oi01 ) d 0 0
i O n  z i 1  (O n  Oi 1 )
0 0

dt
 Combining the linear and
angular velocity Jacobians
• The Jacobian is given by

 v   Jv 
0
 J v1 J v2 J vn 
   q   q
n

ω   J  
0
0, n  J 1 J 2 J n 

z  (O  O ) for revolute joint i

0
i 1
0 0
i 1
J vi   0
n

 z for prismatic joint i

i 1

z 0
for revolute joint i
J i   i 1

 0 for prismatic joint i

 Computation of the Jacobian
0 0 0
• We need to compute z ,O
i 1 i 1 and O n

• The coordinates for zi-1 with respect to the base

frame are given by the first three elements in the third
0
column of i1T
0
O
• While i1 is given by the first three elements of the
0
fourth column of i1T

• O 0n is given by the first three elements of the fourth

0
column of nT

• Conclusion: The Jacobian of any

manipulators can be easily computed once the
forward kinematics are worked out.
 Jacobian of 2-link Revolute-Revolute
(RR) Manipulator

Y2 c1  s1 0 L1c1 
s c1 0 L1s1 
0
T  1
1
0 0 1 0 
Y1  
Y0  0 0 0 1 
Joint 2
c2  s2 0 L1c2 
s c2 0 L1s2 
Joint 1 1
T   2
X0
2
0 0 1 0 
Base  
Link 0  0 0 0 1 

researchgate.net c12  s12 0 L1c1  L2 c12 

Link ai i di i * s c12 0 L1s1  L2 s12 
0
T   12
1 L1 0 0 1 2
0 0 1 0 
2 L2 0 0 2  
0 0 0 1 
 Jacobian of 2-link Revolute-Revolute
(RR) Manipulator 0 0 0
z i 1  (O n  Oi 1 )
J vi  
 J v1 J vn 
0
J v2  z i 1

J  z i01
 J 1 J 2 J n  J i  
 0

 J v1 J v2   z 00  (O 02  O 00 ) z10  (O 02  O10 ) 
J  0 0 
 J 1 J 2   z0 z1 
 z 00  O 02 z10  (O 02  O10 ) 
J 0 0 
 z 0 z1 
 Jacobian of 2-link Revolute-Revolute
(RR) Manipulator
0  L1c1  L2 c12  0
z 00  0  ; O 02   L1s1  L2 s12  ; z10   0  ;
 z 00  O 02 z10  (O 02  O10 ) 
J 0 0 
1   0  1 

 z0 z1  ( L1c1  L2 c12 )  L1c1   L2 c12 

(O 02  O10 )   ( L1s1  L2 s12 )  L1s1    L2 s12 
  L1s1  L2 s12  L2 s12   0   0 
 Lc L c L2 c12 
 11 2 12

 0 0  0 1 0   L1c1  L2 c12    L1s1  L2 s12 


J 0 0   L1s1  L2 s12    L1c1  L2 c12 
 z 0  O 2  1
0 0

 0 0  0 0 0   0   0 

 0 0  0 1 0   L2 c12    L2 s12 

  z 1  (O 2  O1 )   1
0 0 0
0 0   L2 s12    L2 c12 
 1 1  0 0 0   0   0 
 Jacobian of 2-link Revolute-Revolute
(RR) Manipulator    
  ( L1c1  L2 c12 )1  ( L c  L c )
2 12 2 

1 1

 x   L1c1  L2c12  x  

1 2

 y  L s  L s   y    ( L s  L s )   ( L s  L s ) 
   1 1 2 12     1 1 1 2 12 1  2 1 1 2 12 2 
X  z   0  X  z   
 θ       ;        0 
   x  0   θ   x   0

 y   0   y   
       0 
 z   1   2   z   
   
(1   2 )1  (1   2 ) 2
 1  2 
( L1s1  L2 s12 )1  L2 s12 2    L1s1  L2 s12  L2 s12 
   
( L c  L c ) 
 1 1 2 12 1 2 12 2   L c  L1c1  L2 c12 L2 c12 
X  0   0 0  1 
       Jθ
θ  0   0 0   2 
 0   0 0 
   
 1   2   1 1 
 Jacobian of
SCARA Manipulator
z0
z1
Link ai i di i

1 a1 0 0 1* z2
a1
2 a2  0 2* a2
z3
3 0 0 d 3* 0 d4
4 0 0 d4 4* z4

c1  s1 0 a1c1  c2 s2 0 a2c2  1 0 0 0

s 0 a1s1  1  s c  0 1 0 0 
0
T   1 c1 ; 2T  
2 2 0 a 2 s 2 ; T  
2
;
1
0 0 1 0  0 0 1 0  3 0 0 1 d3 
     
 0 0 0 1   0 0 0 1  0 0 0 1 
c4  s4 0 0 c12 c4  s12 s4 s12 c4  c12 s4 0 a1c1  a2 c12 
s c4 0 0  0  s c  c s c c  s s 0 a1s1  a2 s12 
 
4T  ; T
3 4 12 4 12 4 12 4 12 4

0 0 1 d4  4  0 0 1 d3  d 4 
   
 0 0 0 1   0 0 0 1 
 Jacobian of
SCARA Manipulator
z i01  (O0n  Oi01 )
J vi  
 J v1 J vn 
0
J v2  z i 1

J  z i01
 J1 J2 Jn  Ji  
0

 J v1 J v2 J v3 J v4 
J 
 J 1 J 2 J 3 J 4 
 z 00  (O 04  O 00 ) z10  (O 04  O10 ) z 02 z 30  (O 04  O 30 ) 
J 0 0 0 
 z 0 z 1 0 z3 
 z 00  O 04 z10  (O 04  O10 ) z 02 z 30  (O 04  O 30 ) 
J 0 
 z 0 z10 0 z3 0

 Jacobian of
SCARA Manipulator
 z 00  O 04 z10  (O 04  O10 ) z 02 z 30  (O 04  O30 ) 
J 0 
 z 0 z10 0 z3 0

c1  s1 0 a1c1  c2 s2 0 a2 c2  c12 s12 0 a2c12  a1c1 
s c1 0 a1s1   s2 c2 0 a2 s2   s12 c12 0 a2 s12  a1s1 
0
T  1 
2
0 0 1 0 0 0 1 0   0 0 1 0 
    
 0 0 0 1   0 0 0 1   0 0 0 1 
c12 s12 0 a2 c12  a1c1  1 0 0 0   c12 s12 0 a2 c12  a1c1 
s c12 0 a2 s12  a1s1   0 1 0 0   s12 c12 0 a2 s12  a1s1 

3T  2T 3T  
0 0 2 12

0 0 1 0  0 0 1 d3   0 0 1 d3 
    
 0 0 0 1   0 0 0 1   0 0 0 1 
 Jacobian of SCARA Manipulator

0  0 0 0

z 00  0  ; z10  0  ; z 02   0  ; z 30   0 
1  1   1  1
 a1c1  a2c12   a1c1  a2c12   a1c1   a2c12 
0   0 0     
O 4   a1s1  a2 s12  ; O 4  O1   a1s1  a2 s12    a1s1    a2 s12  
 d3  d 4   d3  d 4   0   d3  d 4 
 a1c1  a2c12   a1c1  a2c12   0 
O 04  O30   a1s1  a2 s12    a1s1  a2 s12    0 
 d3  d 4   d3   d 4 
Jacobian of SCARA Manipulator

0 1 0   a1c1  a2c12   a1s1  a2 s12 

z 00  O 04  1 0 0   a1s1  a2 s12    a1c1  a2c12 
0 0 0   d3  d 4   0 
0 1 0   a2 c12   a2 s12 
z10  (O 04  O10 )  1 0 0   a2 s12    a2 c12 
0 0 0   d3  d 4   0 
 0 1 0  0  0
z 30  (O 04  O30 )   1 0 0   0   0 
 0 0 0   d 4  0 
 Jacobian of SCARA Manipulator

 z 00  O 04 z10  (O 04  O10 ) z 02 z 30  (O 04  O30 ) 

J 0 0 0 
 z0 z1 0 z3 
  a1s1  a2 s12 a2 s12 0 0 
 ac a c a c 0 0 
 1 1 2 12 2 12 
 0 0 1 0 
J 
 0 0 0 0
 0 0 0 0
 
 1 1 0 1
 MORE ON REPRESENTATION OF ORIENTATION
Euler angles
Z-Y-Z  r11 r12 r13 
Inverse Problem Given:
A
B R  r
 21 r22 r23  Find: ( ,  ,  )
 r31 r32 r31 

 c c c  s s c c s  s c c s  
A
R ( ,  ,  )   s c c  c s  s c s  c c s s  (*)
B Z 'Y ' Z ' 
  s  c s  s c 

 s   r 2  r 2
 31 32
   atan2 ( r 2
31  r 2
32 , r33 )
 c  r33

If s   0
  atan2(r23 / s  , r13 / s  )
  atan2(r32 / s  , r31 / s  )
 MORE ON REPRESENTATION OF ORIENTATION
Euler angles
Z-Y-Z
Inverse Problem

If s   0;   0 or   180o Singularity of the

representation
  0 ; s   0; c  1
o

 c c  s s c s  s c 0
A
R ( , 0 o
,  )   s c  c s  s s  c c 0 
B Z 'Y ' Z ' 
 0 0 1 
c(   )  s (   ) 0 
A
R ( , 0 o
,  )   s(   ) c(   ) 0 
B Z 'Y ' Z '  
 0 0 1 
One possible convention is to choose  0
  atan2(r12 , r11 );   0o
 MORE ON REPRESENTATION OF ORIENTATION
Euler angles
Z-Y-Z
Inverse Problem

If s   0;   0 or   180o Singularity of the

representation
  180 ; s   0; c  1
o

 c c  s s c s  s c 0
A
R ( ,180 0
,  )    s c  c s s s  c c 0 
B Z 'Y ' Z ' 
 0 0 1
 c(   )  s (   ) 0 
A
R ( ,180 0
,  )    s(   ) c(   ) 0 
B Z 'Y ' Z '  
 0 0 1
One possible convention is to choose  0
  atan2(r12 , r11 );   180o
 MORE ON REPRESENTATION OF ORIENTATION
Z-Y-ZEuler angles
Inverse Problem
s   0;   0 or   180o Singularity of the
representation

At a singularity, the mobility of

a manipulator is reduced. Usually,
arbitrary motion of the manipulator in
a Cartesian direction is lost. This is
referred to as “Losing a DOF”
 Singularities

• If the determinant of a square Jacobian is

zero, the manipulator cannot be controlled.
• It is useful to observe the determinant of
the Jacobian as the robot moves to avoid
singularities.
• Avoid configuration where the determinant
approaches zero.
• By analyzing the Jacobian matrix of a
manipulator we can find the singular
positions of the robot.
 Finding Singularites of the 2-Link Manipulator

X  Jv 
ξ     Jq    q; X  J vq
θ J 
  L1s1  L2 s12  L2 s12 
Jv   
 L1c1  L2 c12 L2 c12 
If we invert the Jacobian, we get: J v1X  q
The inverse is undefined whenever det (Jv)=0. (It is a
singular matrix.) So, by solving det (Jv)=0, we can
find singularities in the robot workspace.
 Finding Singularites of the 2-Link Manipulator

X  Jv 
ξ     Jq    q; X  J vq
θ J 
  L1s1  L2 s12  L2 s12 
Jv   
 L1c1  L2 c12 L2 c12 
If we invert the Jacobian, we get: J v1X  q
The inverse is undefined whenever det (Jv)=0. (It is a
singular matrix.) So, by solving det (Jv)=0, we can
find singularities in the robot workspace.
 Finding Singularites of the 2-Link Manipulator
  L1s1  L2 s12  L2 s12 
Jv   
 11L c  L c
2 12 L 2 12 
c
det(J v )  ( L1s1  L2 s12 )( L2 c12 )  ( L2 s12 )( L1c1  L2c12 )
det(J v )   L1s1 L2 c12  L2 s12 L2 c12  L2 s12 L1c1  L2 s12 L2c12
det(J v )  L2 s12 L1c1  L1s1 L2 c12  L1 L2 sin(1   2  1 )
det(J v )  L1 L2 sin( 2 )
Setting this equal to zero, we find singular positions:
det(J v )  L1 L2 sin( 2 )  0

1. if L1=0, cannot move arm radially; also 1 indeterminate

2. if L2=0, cannot move arm radially; also 2 indeterminate
3. if S2=0, arm is at full extension (2=0), or looped back
onto link 1 (2=180)- again, we cannot move radially in
Cartesian space. Loss of Cartesian motion.
Finding Singularites of the 2-Link Manipulator
We want to move the endpoint of the robot at a
constant speed along a path starting at point “A” on
the x-axis, (+2.0, 0), go around the origin through point
“B” (+ɛ, 0). Consider L1= L2=1. Work out joints
velocities along this path.
qdots(i,:)=Jv_inv*[-1;0];
 Finding Singularites of the 2-Link Manipulator

Note that the joint velocities are extremely large

near the initial and the final points.
 Wrist Singularities

Mechanical limits on the wrist design to restrict its

motion in such a way that z3 and z5 are
prevented from lining up.
Static Force/Torque Analysis of
Manipulators.
This statics
analysis can be
done iteratively
according to:
 The static torque balance
equation must be computed
around a point.
(ignoring gravitational effects)
The point chosen is the origin
of frame i.

i
ni  i ni 1  ( i Pi 1  i fi 1 )  0

This leads to a
coupled pair of
iterative equations
relating forces and
moments applied to
link {i + 1} to forces
and moments applied
to link {i}.
 Iterative algorithms for static contact forces.
Define the generalized force vector as F = (F, N), where F is the linear force and
N is the rotational torque applied by the origin contact point PT0 of the tool frame.

The generalized force applied at each active joint variable due to a force
F = (F, N) applied at link i can be calculated iteratively:

1. Set f n  F and n nn  N . Thus, the initial reference frame is taken as the

n

end effector frame (or tool frame). This force F = (F, N) must be specified in
frame {n}.
2. Decrement i by one. (The index i takes the values i = n-1,n-2,…,1, 0).
Compute the link i force in frame {i} according to the iterative equations,

i
fi  i fi 1  i
i 1 R i 1 fi 1
i 1
i
ni  ni 1  ( Pi 1  fi ) 
i i i i
i 1 R n i 1  ( Pi 1  fi )
i i
 Iterative algorithms for static contact forces.

3. If the joint is revolute, set

 i  ni z i
i T i

Else the joint is prismatic. Set

 i  fi z i
i T i

4. If i > 0 return to step 2). Otherwise end.

  fx   f x  0 0 0   fx   0 
 f ;  f   0 0  
 L2   f y    0 
^
2
2 f   y
2
2 n  L2 X 
2  y 
 0   0   0 L2 0   0   L2 f y 
c2  s2 0   f x   c2 f x  s2 f y 
1
f  s c2 0   f y    s2 f x  c2 f y 
1  2
 0 0 1   0   0 
c2  s2 0  0   0  0 0 0   c2 f x  s2 f y 
s    
0   0   L1 X1 11 f   0   0 0
  L1   s2 f x  c2 f y 
^
1
1 n   2 c2
 0 0 1   L2 f y   L2 f y  0 L1
  0   0 
 0   0   0 
     
1
1 n   0 
  0 
  0 
 L2 f y   L1 ( s2 f x  c2 f y )   L1 ( s2 f x  c2 f y )  L2 f y 
     
 1  L1s2 f x  ( L1c2  L2 ) f y ;  2  L2 f y (*)
 1   L1s2 ( L1c2  L2 )   f x  i
fi  i fi 1  i
R i 1 fi 1
τ    f  i 1

 2   0 L2  y i
ni  i ni 1  ( i Pi 1  i fi )  i
i 1 R i 1 n i 1  ( i Pi 1  i fi )
 Static Force/Torque
Relationships
Let F = (Fx; Fy; Fz; nx; ny; nz) represent the vector of
forces and moments at the end effector.
Let δX and δq represent infinitesimal displacements in the
task space and joint space, respectively. These
displacements are called virtual displacements

These virtual displacements are related through the

manipulator Jacobian J(q) according to
 X  J (q) q
The virtual work δw of the system is

 w  F  X  τ q
T T
 Static Force/Torque
Relationships
 X  J (q) q
 w  FT  X  τT  q  FT J (q) q  τ T  q  [FT J (q)  τ T ] q
 w is equal to zero if the manipulator is in equilibrium
FT J (q)  τT  0
τ  J (q)T F

τ  J (q) F T
where
• F is a 6 x 1
Cartesian force-
moment vector
acting at the end-
effector

•  is a 6 x 1 vector
of torques at the
joints,

• X is a 6 x 1
infinitesimal
Cartesian
displacement of the
end-effector

• and  is a
6 x 1 vector of
infinitesimal joint
displacements.
 Two-link planar manipulator
  L1s1  L2 s12  L2 s12 
τ J 0 T 0
F
 Lc L c L2 c12 
 11 2 12

 0 0   L s  L s L1c1  L2 c12 0 0 0 1
0
J ;
0
J T   1 1 2 12
 0 0    L2 s12 L2 c12 0 0 0 1
 0 0 
 
 1 1 
c12  s12 0   f x   c12 f x  s12 f y 
0
F  02 R 22 F   s12 c12 0   f y    s12 f x  c12 f y 
 0 0 1   0   0 
 c12 f x  s12 f y 
s f  c f 
 12 x 12 y 
  L1s1  L2 s12 L1c1  L2 c12 0 0 0 1  0 
τ J 0 T 0
F   
  L2 s12 L2 c12 0 0 0 1  0 
 0 
 
 0 
  c12 f x  s12 f y 
s f  c f 
 12 x 12 y 
  L1s1  L2 s12 L1c1  L2 c12 0 0 0 1  0 
τ J F
0 T 0
 
  L s
2 12 L c
2 12 0 0 0 1 0 
 0 
 
 0 
 1  ( L1s1  L2 s12 )(c12 f x  s12 f y )  ( L1c1  L2c12 )( s12 f x  c12 f y )
 1   L1s1c12 f x  L2 s12 c12 f x  L1s1s12 f y  L2 s12 s12 f y
 L1c1s12 f x  L2 c12 s12 f x  L1c1c12 f y  L2 c12 c12 f y

 1  L1 f x ( s12 c1  s1c12 )  L1 f y (c12c1  s12 s1 )  L2 f y

 1  L1 f x s2  L1 f y c2  L2 f y
 1  L1s2 f x  ( L1c2  L2 ) f y ;  2  L2 f y (*)
  c12 f x  s12 f y 
s f  c f 
 12 x 12 y 
  L1s1  L2 s12 L1c1  L2 c12 0 0 0 1  0 
τ J F
0 T 0
 
  L s
2 12 L c
2 12 0 0 0 1 0 
 0 
 
 0 
 2   L2 s12 (c12 f x  s12 f y )  L2c12 ( s12 f x  c12 f y )
 2   L2 s12 c12 f x  L2 c12 s12 f x  L2 s12 s12 f y  L2c12c12 f y
 2  L2 f y

 1  L1s2 f x  ( L1c2  L2 ) f y ;  2  L2 f y (*)