Download Jacobian - Prof. Tarek Sobh

Introduction to ROBOTICS
Velocity Analysis
Jacobian
University of Bridgeport
1
Kinematic relations
1 
 
 2
 3 
  
 4 
 5 
 
 6 
X=FK(θ)
Joint Space
θ =IK(X)
x 
y
 
z 
X  
 
 
 
 
Task Space
Location of the tool can be specified using a joint space or a cartesian space
description
Velocity relations
• Relation between joint velocity and cartesian
velocity.
• JACOBIAN matrix J(θ)
1  X  J ( )  x 
 y 
 
 
 2 
 z 
3 
 
 
 x 
 4 
 y 
 
 5    J 1 ( ) X  
 z 
6 
Joint Space
Task Space
Jacobian
• Suppose a position and orientation vector of a
manipulator is a function of 6 joint variables: (from
forward kinematics)
X = h(q)
 h1 (q1 , q2 ,  , q6 ) 
 q1 
x 
h (q , q , , q )
q 
y
6 
2
 2 1 2
 
 
 h3 (q1 , q2 ,  , q6 ) 
 q3 
z 
X     h(   ) 61 h (q , q ,  , q )
6 
 4 1 2
q4 
 
 h5 (q1 , q2 ,  , q6 ) 
 q5 
 


 
 
 h6 (q1 , q2 ,  , q6 )  61
 q6 
 
Jacobian Matrix
Forward kinematics
X 61  h(qn1 )
d
dh(q) dq dh(q)

X 61  h(qn1 ) 

q
dt
dq dt
dq
 x 
 y 
 
 z 
 
 x 
 y 
 
 z 
 q1 
 q 
 dh(q )   2 
 dq    

 6n
 
q n  n1
X 61  J 6n q n1
dh(q)
J
dq
Jacobian Matrix
 x 
 y 
 q1 
 
 z   dh(q )   q 2 
 
  

 x   dq  6n   
 
 y 
q n  n1
 
 z 
 dh(q ) 

J  
 dq  6n
Jacobian is a function of
q, it is not a constant!
 h1
 q
 1
 h2
  q1
 
 h
 6
 q1
h1
q2
h2
q2

h6
q2
h1 

qn 

h2 

qn 

 
h6 


qn  6n
Jacobian Matrix
 x 
 y 
 
 z  V 

X   
 x   
 y 
 
 z 
Linear velocity
 x 
V   y 
 z 
The Jacbian Equation
X  J 6n q n1
Angular velocity
  x   



   y   
 z   


 q1 
 q 
q n1   2 

 
 q n  n1
Example
• 2-DOF planar robot arm
– Given l1, l2 , Find: Jacobian
 x  l1 cos1  l2 cos(1   2 )  h1 (1 , 2 ) 
 y    l sin   l sin(    )   h ( , )
  1
1
2
1
2 
 2 1 2 
1 
 x 

Y     J 

 y 
 2 
 h1
 
J  1
 h2
 1
(x , y)
2
l2
1 l1
h1 
 2   l1 sin 1  l2 sin( 1   2 )  l2 sin( 1   2 )

h2   l1 cos 1  l2 cos(1   2 ) l2 cos(1   2 ) 
 2 
Singularities
• The inverse of the jacobian matrix cannot be
calculated when
det [J(θ)] = 0
• Singular points are such values of θ that
cause the determinant of the Jacobian to
be zero
• Find the singularity configuration of the 2-DOF
planar robot arm
determinant(J)=0
Not full rank



x


X     J  1 


 y 
 2
V
 l1 sin 1  l2 sin( 1   2 )  l2 sin( 1   2 )
J 

l
cos


l
cos(



)
l
cos(



)
1
2
1
2
2
1
2 
1
l2
Y
2 =0
Det(J)=0
2  0
1 l1
x
(x , y)
Jacobian Matrix
• Pseudoinverse
– Let A be an mxn matrix, and let A  be the pseudoinverse
of A. If A is of full rank, then A  can be computed as:
 AT [ AAT ]1
 1

A  A
[ AT A]1 AT

mn
mn
mn
Jacobian Matrix
– Example: Find X s.t.
1 0 2
3
1  1 0 x   2


 
1 1 
1 4 
1
5 1
1




T
T 1
A  A [ AA ]  0  1 

1

5


1
2
9


2 0 
4  2
Matlab Command: pinv(A) to calculate A+
  5
1 

x  A b   13 
9
 16 
Jacobian Matrix
• Inverse Jacobian
 J11
J
X  Jq   21
 

 J 61
q  J X
1
J12  J16   q 

q 

J 22  J 26  q 
 


  q 
 q 
J 62  J 66  q 
1
q5
2
3
4
5
6
q1
• Singularity
– rank(J)<n : Jacobian Matrix is less than full rank
– Jacobian is non-invertable
– Boundary Singularities: occur when the tool tip is on the surface
of the work envelop.
– Interior Singularities: occur inside the work envelope when two
or more of the axes of the robot form a straight line, i.e., collinear
Singularity
• At Singularities:
– the manipulator end effector cant move in
certain directions.
– Bounded End-Effector velocities may
correspond to unbounded joint velocities.
– Bounded joint torques may correspond to
unbounded End-Effector forces and torques.
Jacobian Matrix
• If
 ax 
 bx 
A   a y  , B  by 
 az 
 bz 
• Then the cross product
i
j
A  B  ax
ay
bx
by
 a y bz  az by 


az   (axbz  az bx ) 
bz  axby  a y bx 
k
Remember DH parmeter
ci
 s
Ai   i
 0

 0
-c isi
s isi
ci c i
-s i ci
s i
c i
0
0
a i ci 
a isi 
di 

1 
• The transformation matrix T
T0i  A1 A2 ..... Ai
Jacobian Matrix
J  J1
J 2 .... J n 
Jacobian Matrix
2-DOF planar robot arm
Given l1, l2 , Find: Jacobian
• Here, n=2,
(x , y)
2
l2
1 l1
ci
 s
Ai   i
 0

 0
-c isi
s isi
ci c i
-s i ci
s i
c i
0
0
a i ci 
a isi 
di 

1 
Where (θ1 + θ2 ) denoted by θ12 and
0 
Z 0  Z1   0 
1 
0
 a1 cos 1 
 a1 cos 1  a2 cos(1   2 ) 
O0  0 , O1   a1 sin 1  , O2   a1 sin 1  a2 sin(1  2 ) 
0
 0 


0
19
Jacobian Matrix
2-DOF planar robot arm
Given l1, l2 , Find: Jacobian
• Here, n=2
 z0  (o2  o0 ) 
 z1  (o2  o1 ) 
J1  
, J2  


z
z
0

1



(x , y)
2
l2
1 l1
Jacobian Matrix
 z0  (o2  o0 ) 
J1  

z
0


0   a1 cos 1  a2 cos(1   2 ) 
Z 0  (o2  o0 )  0    a1 sin 1  a2 sin(1   2 ) 
1  

0
i
j
k

 
0
0
1 
 a1 cos 1  a2 cos(1   2 ) a1 sin 1  a2 sin(1   2 ) 0 
 a1 sin 1  a2 sin(1   2 ) 
  a1 cos 1  a2 cos(1   2 ) 


0
Jacobian Matrix
 z1  (o2  o1 ) 
J2  

z

1

0   a2 cos(1   2 ) 
Z1  (o2  o1 )  0    a2 sin(1   2 ) 
1  

0
i
j
k

 
0
0
1 
 a2 cos(1   2 ) a2 sin(1   2 ) 0 
 a2 sin(1   2 ) 
  a2 cos(1   2 ) 


0
Jacobian Matrix
 a1 sin 1  a2 sin(1   2 ) 
 a cos   a cos(   ) 
1
2
1
2 
 1


0
J1  

0




0


1


  a2 sin(1   2 ) 
 a cos(   ) 
1
2 
 2


0
J2  

0




0


1


The required Jacobian matrix J
J   J1
J2 
Stanford Manipulator
The DH parameters are:
ci
 s
Ai   i
 0

 0
-c isi
s isi
ci c i
-s i ci
s i
c i
0
0
a i ci 
a isi 
di 

1 
Stanford Manipulator
T01  A1
c1c2
s c
T02  A1 A2   1 2
  s2

 0
c1c2
s c
T03  A1 A2 A3   1 2
  s2

 0
 s1 c1s2
c1
s1s2
0
c2
0
0
 s1 c1s2
c1
s1s2
0
c2
0
0
d 2 s1 
d 2c1 
0 

1 
0 
c1s2 
  s1 
c1s2 
z0  0  z   c  z   s s  z3   s1s2 
1
 
 1  2  1 2
1 
 c2 
 0 
 c2 
 d 3c1s2  d 2 s1 
 d 2 s1 
0 
 d c  O  d s s  d c 


O

d3c1s2  d 2 s1  O0  O1  0 2  2 1  3  3 1 2
2 1
 


d 3c2
 0 
 0 
d3 s1s2  d 2c1 

d3c2

1

Stanford Manipulator
T04  A1 A2 A3 A4
T05  A1 A2 A3 A4 A5
T06  A1 A2 A3 A4 A5 A6
T4 =
[ c1c2c4-s1s4,
-c1s2, -c1c2s4-s1*c4, c1s2d3-sin1d2]
[ s1c2c4+c1s4, -s1s2, -s1c2s4+c1c4, s1s2d3+c1*d2]
[-s2c4, -c2, s2s4, c2*d3]
[ 0, 0, 0, 1]
 c1c2 s4  s1c4 
z4    s1c2 s4  c1c4 


s2 s4
 d3c1s2  d 2 s1 
O4   d3 s1s2  d 2 c1 


d3c2
Stanford Manipulator
T5 =
[ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5,
c1s2d3-s1d2]
[ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5,
s1s2d3+c1d2]
[ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3]
[ 0, 0, 0, 1]
c1c2c4 s5  s1s4 s5  c1s2c5 
z5   s1c2c4 s5  c1s4 s5  s1s2c5 


 s2c4 s5  c2c5
c1c2c4 s5  s1s4 s5  c1s2c5 
z5   s1c2c4 s5  c1s4 s5  s1s2c5 


 s2c4 s5  c2c5
Stanford Manipulator
T5 =
[ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5,
c1s2d3-s1d2]
[ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5,
s1s2d3+c1d2]
[ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3]
[ 0, 0, 0, 1]
c1c2c4 s5  s1s4 s5  c1s2c5 
z5   s1c2c4 s5  c1s4 s5  s1s2c5 


 s2c4 s5  c2c5
 d 3c1s2  d 2 s1 
O5   d 3 s1s2  d 2 c1 


d 3c2
Stanford Manipulator
T6 = [ c6c5c1c2c4-c6c5s1s4-c6c1s2s5+s6c1c2s4+s6s1c4, c5c1c2c4+s6c5s1s4+s6c1s2s5+c6c1c2s4+c6s1c4, s5c1c2c4-s5s1s4+c1s2c5,
d6s5c1c2c4-d6s5s1s4+d6c1s2c5+c1s2d3-s1d2]
[ c6c5s1c2c4+c6c5c1s4-c6s1s2s5+s6s1c2s4-s6c1c4, -s6c5s1c2c4s6c5c1s4+s6s1s2s5+c6s1c2s4-c6c1c4, s5s1c2c4+s5c1s4+s1s2c5,
d6s5s1c2c4+d6s5c1s4+d6s1s2c5+s1s2d3+c1d2]
[ -c6s2c4c5-c6c2s5-s2s4s6, s6s2c4c5+s6c2s5-s2s4c6, -s2c4s5+c2c5, d6s2c4s5+d6c2c5+c2d3]
[ 0, 0, 0, 1]
d6s5c1c2c4  d6s5s1s4  d6c1s2c5  c1s2d3  s1d2
O6  d6s5s1c2c4  d6s5c1s4  d6s1s2c5  s1s2d3  c1d2


d6s2c4s5  d6c2c5  c2d3
Stanford Manipulator
 z0  (o6  o0 ) 
 z1  (o6  o1 ) 
J1  
, J2  


z
z
0

1



 z2 
J3   
0
Joints 1,2 are revolute
Joint 3 is prismatic
 z3  (o6  o3 ) 
 z5  (o6  o5 ) 
 z4  (o6  o4 ) 
J4  
, J5  
, J6  



z
z
z
3
5

4





The required Jacobian matrix J
J   J1
J 2 J3 J 4 J5 J 6 
Inverse Velocity
– The relation between the joint and end-effector velocities:
X  J (q )q
where j (m×n). If J is a square matrix (m=n), the joint
velocities:
q  J 1 (q) X
– If m<n, let pseudoinverse J+ where J   J T [ JJ T ]1
q  J  (q ) X
Acceleration
– The relation between the joint and end-effector velocities:
X  J (q )q
– Differentiating this equation yields an expression for the
acceleration:
d
X  J (q )q  [ J (q )]q
dt
– Given X of the end-effector acceleration, the joint
acceleration q
d
J (q)q  X  [ J (q )]q
dt
d
1
q  J (q)[ X  J (q )]q]
dt