Robotics Systems

Assignment 2
Group AG19

England Kwok, 702274 Josh Findlay, 641277 Robert Smith, 636889

1 Introduction
In this report we will use the kinematics equations obtained in Assignment 1 to derive the Jacobian
matrix for our robot, which is used to transform joint velocities into the end-effectors translational
and angular velocities.

1
2 Jacobian

Figure 1: A diagram of the robot configuration with the associated joints and frames.
 T  T
Let Q = q1 q2 q3 q4 q5 define the rotation of the joint about z 1 z 2 z 3 z 4 z 5
respectively. Let α1 , α2 , and α3 define the offset for the home position angles of z 2 , z 3 and z 4
respectively. From these definitions, the absolute rotation about each frame is:
 T  T
θ1 θ2 θ3 θ4 θ5 = q1 q2 + α1 q3 − α2 q4 + α3 q5

Frame W (wrist frame) is defined such that it is attached to the end-effector frame but the origin
lies on the intersection of ẑ 4 and ẑ 5 . This is because this point is where the spherical joint which
describes the “wrist” of the robot acts yet is aligned with the rotation of the end-effector. We will
calculate the Jacobian of the end-effector by first calculating the Jacobian of frame W and then
adding the relative velocities between frame W and frame E. For the translational velocities, we
only consider up to ẑ 4 as any further joint frames will not contribute to the translational velocity
of the wrist frame. For the angular velocity, we consider all joints as the wrist frame is attached
to the end-effector frame. Due to our robot only consisting of revolute joints, the translational
velocities is the sum of all the cross products between the absolute angular velocities of the joint
frame and the translational vector of the joint frame to the wrist frame. The absolute angular
velocity of the wrist is the summation of all the absolute angular velocities of each joint frame.
4
X
0
v iw = 0
v i where 0 v i = 0 ω i × 0 piw
i=1
XE
0 0
ωw = ωi
i=1
0 0
Given: ω i = ẑ i q̇i

2
The Jacobian is the matrix which maps the joint velocities to the task space velocities:
   
q˙1 q˙1
0  q˙2   0  q˙2 
vw J
= 0 wv 
     
0 = 0J w  q˙3  q˙3 

ωw 
q˙4 
 J wω q˙4 
q˙5 q˙5
Where: 0 J wv = 0 ẑ 1 × 0 p1w 0 ẑ 2 × 0 p2w 0 ẑ 3 × 0 p3w 03×2
 
0
 
J wω = 0 ẑ 1 0 ẑ 2 0 ẑ 3 0 ẑ 4 0 ẑ 5
Using the transformation matrices obtained in Assignment 1 (Appendix), the rotation matrices,
i−1 i 0 0
i R and the vectors, r iw can be identified. We can obtain ẑ i and r iw by applying the rotation
matrices to i ẑ i and i r iw .
0
 
J wω = 0 ẑ 1 0 ẑ 2 0 ẑ 3 0 ẑ 4 0 ẑ 5
         
0 0 sin (q1 ) sin (q1 ) sin (q1 )
∴ i ẑ i = 0 =⇒ 0 ẑ 1 = 0 0 ẑ 2 = − cos (q1 ) 0 ẑ 3 = − cos (q1 ) 0 ẑ 4 = − cos (q1 )
1 1 0 0 0
 
sin (α1 − α2 + α3 + q2 + q3 + q4 ) cos (q1 )
0
ẑ 5 = sin (α1 − α2 + α3 + q2 + q3 + q4 ) sin (q1 ) 

− cos (α1 − α2 + α3 + q2 + q3 + q4 )
0
 
J wv = 0 J 1wv 0 J 2wv 0 J 3wv 03×2
 
l2 cos (a1 + q2 ) + l3 cos (a1 − a2 + q2 + q3 )
∴ 1 p1w =  0 
l2 sin (a1 + q2 ) + l3 sin (a1 − a2 + q2 + q3 )
 
cos (q1 ) (l2 cos (a1 + q2 ) + l3 cos (a1 − a2 + q2 + q3 ))
=⇒ 0 p1w =  sin (q1 ) (l2 cos (a1 + q2 ) + l3 cos (a1 − a2 + q2 + q3 )) 
l2 sin (a1 + q2 ) + l3 sin (a1 − a2 + q2 + q3 )
 
− sin (q1 ) (l2 cos (α1 + q2 ) + l3 cos (α1 − α2 + q2 + q3 ))
=⇒ 0 J 1wv =  cos (q1 ) (l2 cos (α1 + q2 ) + l3 cos (α1 − α2 + q2 + q3 )) 
0
 
l2 + l3 cos (a2 − q3 )
2
∴ p2w =  −l3 sin (a2 − q3 ) 
0
 
cos (q1 ) (l2 cos (a1 + q2 ) + l3 cos (a1 − a2 + q2 + q3 ))
=⇒ 0 p2w =  sin (q1 ) (l2 cos (a1 + q2 ) + l3 cos (a1 − a2 + q2 + q3 )) 
l2 sin (a1 + q2 ) + l3 sin (a1 − a2 + q2 + q3 )
 
− cos (q1 ) (l2 sin (α1 + q2 ) + l3 sin (α1 − α2 + q2 + q3 ))
=⇒ 0 J 2wv =  − sin (q1 ) (l2 sin (α1 + q2 ) + l3 sin (α1 − α2 + q2 + q3 )) 
l2 cos (α1 + q2 ) + l3 cos (α1 − α2 + q2 + q3 )
     
l3 l3 −l3 cos (q1 ) sin (α1 − α2 + q2 + q3 )
∴ 3 p3w =  0  =⇒ 0 p3w =  0  =⇒ 0 J 3wv =  −l3 sin (α1 − α2 + q2 + q3 ) sin (q1 ) 
0 0 l3 cos (α1 − α2 + q2 + q3 )

3
Now to calculate the absolute velocities of the end-effector frame, we can add the relative trans-
lational velocity between wrist frame and frame E with the absolute velocities obtained for the
wrist frame. The rotational velocities are already calculated as the rotational velocities of the wrist
frame and end-effector are identical as the wrist frame is attached to the end-effector.
   
 0   0  0  r x 0 r z −ry
vE v ω w × 0 pwE
0 = 0 w + . Let 0 pwE = ry  and skew( 0 pwE ) = −rz 0 rx 
ωE ωw 03×6
rz ry −rx 0
0 
 v
such that 0 ω w × 0 pwE = 03×3 skew( 0 pwE ) 0 w

ωw
 
q˙1
0  0           q˙2 
vE vw 03×3 skew( 0 pwE ) 0
vw I 3×3 skew( 0 pwE ) 0 J wv  
∴ 0 = 0 + 0 = 0
 q˙3 
ωE ωw 03×3 03×3 ωw 03×3 I 3×3 J wω  

q˙4 
q˙5
     
0 I 3×3 skew( 0 pwE ) 0 J wv 0
J 1Ev 0 J 2Ev 0 J 3Ev 0 J 4Ev 0 J 5Ev
=⇒ J E = 0 = 0 0 0 0 0
03×3 I 3×3 J wω ẑ 1 ẑ 2 ẑ 3 ẑ 4 ẑ 5
0
0 0 0 0 0

ẑ 1 ẑ 2 ẑ 3 ẑ 4 ẑ 5 is defined above through J wω
   
0 sin (α1 − α2 + α3 + q2 + q3 + q4 ) cos (q1 ) (l4 + l5 )
∴4 pwE = −l4 − l5  =⇒ 0 pwE =  sin (α1 − α2 + α3 + q2 + q3 + q4 ) sin (q1 ) (l4 + l5 ) 
0 − cos (α1 − α2 + α3 + q2 + q3 + q4 ) (l4 + l5 )
 
− sin (q1 ) (l2 cos (α1 + q2 ) + l3 cos (α1 − α2 + q2 + q3 )) − . . .

 . . . sin (α1 − α2 + α3 + q2 + q3 + q4 ) sin (q1 ) (l4 + l5 ) 

0
J 1Ev =  cos (q1 ) (l2 cos (α1 + q2 ) + l3 cos (α1 − α2 + q2 + q3 )) + . . . 


 . . . sin (α1 − α2 + α3 + q2 + q3 + q4 ) cos (q1 ) (l4 + l5 ) 
0
 
cos (α1 − α2 + α3 + q2 + q3 + q4 ) cos (q1 ) (l4 + l5 ) − . . .

 . . . cos (q1 ) (l2 sin (α1 + q2 ) + l3 sin (α1 − α2 + q2 + q3 )) 

0
 cos (α 1 − α2 + α 3 + q 2 + q 3 + q 4 ) sin (q 1 ) (l4 + l 5 ) − . . . 
J 2Ev =  

 . . . sin (q 1 ) (l2 sin (α 1 + q 2 ) + l3 sin (α 1 − α 2 + q 2 + q 3 )) 

 l2 cos (α1 + q2 ) + l4 sin (α1 − α2 + α3 + q2 + q3 + q4 ) + . . . 
. . . l5 sin (α1 − α2 + α3 + q2 + q3 + q4 ) + l3 cos (α1 − α2 + q2 + q3 )
 
cos (α1 − α2 + α3 + q2 + q3 + q4 ) cos (q1 ) (l4 + l5 ) − l3 cos (q1 ) sin (α1 − α2 + q2 + q3 )
0
 cos (α1 − α2 + α3 + q2 + q3 + q4 ) sin (q1 ) (l4 + l5 ) − l3 sin (α1 − α2 + q2 + q3 ) sin (q1 ) 
J 3Ev =   l4 sin (α1 − α2 + α3 + q2 + q3 + q4 ) + l5 sin (α1 − α2 + α3 + q2 + q3 + q4 ) + . . . 


. . . l3 cos (α1 − α2 + q2 + q3 )
   
cos (α1 − α2 + α3 + q2 + q3 + q4 ) cos (q1 ) (l4 + l5 ) 0
0 0
J 4Ev =  cos (α1 − α2 + α3 + q2 + q3 + q4 ) sin (q1 ) (l4 + l5 )  J 5Ev = 0
sin (α1 − α2 + α3 + q2 + q3 + q4 ) (l4 + l5 ) 0

4
3 Verification
The end-effector velocities (linear v E and rotational ω E ) are given in task space by
 
q˙1
  q˙2 
vE  
= J E Q̇ where q˙3 
Q̇ =  
ωE q˙4 
q˙5

Note that J E is a function of the joint space angles:

J E = f (q1 , q2 , q3 , q4 , q5 )

Therefore to find v E and ω E we need to know both the joint velocities q̇i and positions qi . Whilst
this alone is sufficient to find v E and ω E for verification, it is made much clearer if the robot is
driven by constant joint velocities for a short period of time. That is, if we find some path of the
end-effector position pE (t) and confirm that v E (t) = p˙E (t) and appears as the tangent to the path
pE (t).
This is simulated within Matlab by calculating the joint angles after each time step (∆t) as
follows:
qi [k + 1] = qi [k] + q˙i ∆t where k is the nth time step
Since the joint velocities q˙i will be held constant, there will be no accumulated error from ap-
proximating an integral in this manner. Additionally, joint limits will be respected which requires
setting a joints velocity to 0 if it becomes limited.

Parameter Value
l1 0.15m
qi lower upper
l2 0.4m
q1 −90◦ 90◦
l3 0.25m
q2 −10◦ 30◦
l4 0.05m
q3 −70◦ 40◦
l5 0.05m
q4 −90◦ 50◦
α1 30◦
q5 −90◦ 90◦
α2 60◦
α3 30◦
Table 1: Parameters and joint limits

5
3.1 Test Configurations
Three test configurations are displayed here for verification. They all start from the home position,
with ∆t = 0.01s and have the joint velocities defined by Q̇1 , Q̇2 and Q̇3 respectively. Figures 2, 3
and 4 show the simulated results over one second with several key poses displayed. The red vectors
indicate the linear velocity of the end-effector at that point.
       
q˙1 1 0 1
q˙2  0 2  1.5 
       
Q̇1 = q˙3  = 0 rad/s
    Q̇2 =  0  rad/s
  Q̇3 =  0.5  rad/s

q˙4  0 −2 −1.5
q˙5 0 0 0
Test 1 (fig.2) shows a simple rotation around the base at constant speed, reflected in the unchanging
lengths of the velocity vectors displayed. Test 2 (fig.3) shows the second joint reaching it’s limit by
the 3rd pose displayed. Correspondingly, q̇2 is set to 0 so that the Jacobian uses the correct joint
velocities reflecting the updated path, and the end-effector velocity is slower. Test 3 (fig.4) shows
4 joints actuating. The velocity vectors always remain tangential to the path of the end-effector,
so the Jacobian is correct in the cases tested.

Appendix
We can extract the rotation matrices and the vector from frame a to frame b from our transfor-
mation matrices obtained in Assignment 1.
1 1

1 4 R p 14
4T =
0 1
 
l2 cos (α1 + q2 ) + l3 cos (α1 − α2 + q2 + q3 )
1
p14 =  0 
l2 sin (α1 + q2 ) + l3 sin (α1 − α2 + q2 + q3 )
 
cos (α1 − α2 + α3 + q2 + q3 + q4 ) − sin (α1 − α2 + α3 + q2 + q3 + q4 ) 0
1
4R =
 0 0 −1
sin (α1 − α2 + α3 + q2 + q3 + q4 ) cos (α1 − α2 + α3 + q2 + q3 + q4 ) 0
 
2  cos (α 3 − α 2 + q 3 + q 4 ) − sin (α 3 − α 2 + q 3 + q 4 ) 0 l2 + l3 cos (α 2 − q 3 )
2  sin (α3 − α2 + q3 + q4 ) cos (α3 − α2 + q3 + q4 ) 0 −l3 sin (α2 − q3 ) 
2 4R p24
4T = = 
0 1  0 0 1 0 
0 0 0 1
 
3  cos (α3 + q4 ) − sin (α3 + q4 ) 0 l3
3
4R p34 sin (α3 + q4 ) cos (α3 + q4 ) 0 0 
3

4T = = 
0 1  0 0 1 0
0 0 0 1
 
4  cos (q 5 ) − sin (q 5 ) 0 0
4
4 ER p4E  0 0 −1 −l4 − l5 
E T = =  
0 1  sin (q5 ) cos (q5 ) 0 0 
0 0 0 1

6
Figure 2: Test Configuration 1

Figure 3: Test Configuration 2

7
Figure 4: Test Configuration 3