KON 318E

Homework I

Semanur GÜLMEZ 040220619

April 30, 2025

Istanbul Technical University

Faculty of Electrical and Electronics Engineering
Department of Control and Automation Engineering
KON 318E - Introduction to Robotics
Q1
Since the coordinate frame o1 x1 y1 z1 is obtained via three consecutive rotational transformations
R1 , R2 and R3 on o0 x0 y0 z0 , then defining oa xa ya za and ob xb yb zb as the intermediate frames, it
can be written such that

oa xa ya za = R1 · o0 x0 y0 z0 (1)
ob xb yb zb = R2 · oa xa ya za (2)
o1 x1 y1 z1 = R3 · ob xb yb zb (3)
The transformations are defined as follows
• R1 = rotation of π/6 about the x-axis
   
1 0 0 1 0 0
R1 = Rx,π/6 = 0 cos π6 − sin π6  = 0 0.87 −0.5
0 sin π6 cos π6 0 0.5 0.87

• R2 = rotation of π/4 about the fixed y-axis

cos π4 0 sin π4
   
0.71 0 0.71
R2 = Ry,π/4 =  0 1 0 = 0 1 0 
− sin π4 0 cos π4 −0.71 0 0.71

• R3 = rotation of π/3 about the current z-axis, since R3 is about the current axis, similarity
transformation is applied

cos π3 − sin π3
 
0
−1
R3 = (R2 · R1 )Rz,π/3 (R2 · R1 ) where Rz,π/3 =  sin π3 cos π3 0
0 0 1
 
0.69 −0.68 −0.25
R3 = 0.38 0.63 −0.68
0.62 0.38 0.69

The final rotation matrix is found by the composition of all the rotations. Using Equations (1),
(2) and (3), the relation between o0 x0 y0 z0 and o1 x1 y1 z1 is defined as:

o1 x1 y1 z1 = R3 R2 R1 · o0 x0 y0 z0 (4)

Where R = R3 R2 R1 is computed as:

 
0.66 −0.44 0.61
R =  0.75 0.43 −0.5
−0.05 0.79 0.61

To find o1 x1 y1 z1 define a coordinate frame on the origin of a three dimensional space defined
by the unit vectors x0 , y0 , z0 .
     
1 0 0
x0 = 0 , y0 = 1 , z0 = 0
0 0 1

Then o1 x1 y1 z1 can be calculated using Equation (4).

     
0.66 −0.44 0.61
x1 =  0.75  , y1 =  0.43  , z1 = −0.5 (5)
−0.05 0.79 0.61

1
 Visualizing the frames and transformations will verify the results. To do so, let us apply each
transformation to o0 x0 y0 z0 as given in Equation (1), (2) and (3), and plot after each transformation
to check if the geometric movement overlaps with the definition of the rotations.
Using Equation (1), calculate the new frame’s defining unit vectors as xa , ya , za and plot them
on the space.      
1 0 0
xa = 0 , ya = 0.87 , za = −0.5
0 0.5 0.87

(a) Perspective view of the frames (b) Orthographic view from x0

Figure 1: Frames o0 x0 y0 z0 and oa xa ya za shown in the same space

Figure 1b reveals that there is π/6 radians from y0 to ya and from z0 to za around x0 , which
means that R1 rotated the frame o0 x0 y0 z0 by π/6 around x0 (fixed frame). Therefore R1 is verified.
Using Equation (2), calculate the new frame’s defining unit vectors as xb , yb , zb and plot them
on the space.      
0.71 0.35 0.61
xb =  0  , yb = 0.87 , zb =  −0.5 
−0.71 0.35 −0.61

(a) Perspective view of the frames (b) Orthographic view from y0

Figure 2: Frames o0 x0 y0 z0 , oa xa ya za and ob xb yb zb shown in the same space

Figure 2b reveals that there’s π/2 radians from ya to yb and from za to zb around y0 , which
means that R2 rotated the frame oa xa ya za by π/2 around y0 (fixed frame). Therefore R2 is verified.
Using Equation (3), calculate the new frame’s defining unit vectors as x1 , y1 , z1 and plot them
on the space.      
0.66 −0.44 0.61
x1 =  0.75  , y1 =  0.43  , z1 = −0.5 (6)
−0.05 0.79 0.61

2
 (a) Perspective view of the frames (b) Orthographic view from zb

Figure 3: Frames ob xb yb zb and o1 x1 y1 z1 shown in the same space

Figure 3b reveals that there’s π/3 radians from yb to y1 and from zb to z1 around zb , which
means that R3 rotated the frame ob xb yb zb by π/3 around zb (current frame). Therefore R3 is
verified.
Since the resulting o1 x1 y1 z1 defined in both Equation (5) and (6) are equivalent, it is proven
that R is making the intended rotations as expected.

Q2
Provided that H21 and H32 , it is required to find H31 . This can be achieved via the property of
cascaded homogeneous transformation matrices given as:

Hac = Hbc · Hab

Therefore
H32 = H12 · H31 (7)
The inverse property of homogeneous transformation matrices given as:

Hab = (Hba )−1

Therefore H12 can be calculated as:

 −1  
0 0 1 1 0 0 1 0
0 −1 0 0 0 −1 0 0
H12 = (H21 )−1 =  = 
1 0 0 0 1 0 0 −1
0 0 0 1 0 0 0 1

Using Equation (7)

1 0 0√ 0
     
0 0 1 0 0 0.87 0.5 0
1 3
2 2 1
0 −1 0 0  0
  − 1 0 −0.5 0.87 −1
H3 = H1 · H3 =  · √2 2 = 
1 0 0 −1 0 3 1
0 1 0 0 −1
2 2
0 0 0 1 0 0 0 1 0 0 0 1

Q3
Frame Assignment and DH-Table
For the given 6-DOF Robotic Arm in Figure 4, forward kinematic model can be derived using
DH-Table. To do so, it is required to assign frames according to the DH rules.

3
 𝑎3 𝑎4 𝑎5 𝑎6 𝑎7

𝜃4 𝜃6

𝑑3
𝑎2 𝜃5

𝜃2

𝑎1

𝑑1

Figure 4: 6-DOF Robotic Arm Model

The first step is to assign zi axes on the direction of movement of the jointi+1 . Figure 5 shows
the assigned axes.

𝑎3 𝑎4 𝑎5 𝑎6 𝑎7

𝒛𝟓

𝒛𝟐 𝒛𝟑 𝒛𝟔
𝒛𝟒
𝜃4 𝜃6

𝑑3
𝑎2 𝜃5
𝒛𝟏

𝜃2

𝒛𝟎
𝑎1

𝑑1

Figure 5: 6-DOF Robotic Arm Model with zi axes shown

The second step is to assign oi base frames. Figure 6 shows the assigned frames. The assign-
ments are done as in the following order.
o2 is assigned to the intersection of z1 and z2 .
o4 is assigned to the intersection of z3 and z4 .
o5 is assigned to the intersection of z4 and z5 .
o0 , o1 , o3 and o6 is assigned such that to minimize visual clutter and also benefit from the
known quantities of ai .

4
 𝑎3 𝑎4 𝑎5 𝑎6 𝑎7

𝒐𝟐 𝒐𝟑 𝒐𝟒 𝒐𝟓 𝒛𝟓 𝒐𝟔

𝒛𝟑 𝒛𝟔
𝒛𝟐
𝒛𝟒
𝜃4 𝜃6

𝑑3
𝑎2 𝜃5
𝒛𝟏

𝜃2

𝒐𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎

Figure 6: 6-DOF Robotic Arm Model with zi axes and oi frames shown

The third step is to assign xi axes. Figure 7 shows the assigned axes. The assignments are
done as in the following order.
x2 is assigned as the normal of intersection of the z1 and z2 . The direction is chosen as out of
the page to avoid visual cluttering.
x4 is assigned as the normal of intersection of the z3 and z4 . The direction is chosen as upwards
to avoid visual cluttering.
x5 is assigned as the normal of intersection of the z4 and z5 . The direction is chosen as upwards
to avoid visual cluttering.
x0 , x1 , x3 and x6 is assigned such that to minimize the required rotations between them.
Particularly, x0 , x1 , and x3 is chosen in the same direction as x2 , and x6 is chosen in the same
direction as x5 .

𝑎3 𝑎4 𝑎5 𝑎6 𝑎7
𝒙𝟒 𝒙𝟓 𝒙𝟔

𝒐𝟐 𝒐𝟑 𝒐𝟒 𝒐𝟓 𝒛𝟓 𝒐𝟔

𝒛𝟑 𝒛𝟔
𝒙𝟐 𝒛𝟐 𝒙𝟑
𝒛𝟒
𝜃4 𝜃6

𝑑3
𝑎2 𝜃5
𝒛𝟏

𝜃2

𝒐𝟏

𝒙𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎

𝒙𝟎

Figure 7: 6-DOF Robotic Arm Model with zi , xi axes and oi frames shown

The last step is to assign yi frames according to the right-hand-rule. Figure 8 shows the assigned
axes.

5
 𝑎3 𝑎4 𝑎5 𝑎6 𝑎7
𝒙𝟒 𝒙𝟓 𝒙𝟔

𝒐𝟐 𝒚𝟒 𝒐𝟒 𝒐𝟓 𝒛𝟓 𝒐𝟔
𝒐𝟑

𝒛𝟑 𝒛𝟔
𝒙𝟐 𝒛𝟐 𝒙𝟑
𝒛𝟒
𝜃4 𝒚𝟓 𝜃6 𝒚𝟔
𝒚𝟐 𝒚𝟑
𝑑3
𝑎2 𝜃5
𝒛𝟏

𝜃2

𝒐𝟏 𝒚𝟏

𝒙𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎 𝒚𝟎

𝒙𝟎

Figure 8: 6-DOF Robotic Arm Model with zi , xi , yi axes and oi frames shown

Assignment of the frames is completed. DH-Table can be filled looking at the frames xi and zi
only. Since there are 6 joints, the table will have 6 rows. Each column of the table is a parameter
defined as follows:
ai : distance from intersection of zi−1 and xi to oi along xi
αi : angle from zi−1 to zi along xi
di : distance from oi−1 to intersection of zi−1 and xi along zi−1
θ: angle from xi−1 to xi around zi−1
Filling the table is done row by row. ith row represents the transmission from jointi−1 to jointi .

Let us fill the first row of the table.

θ: Since x0 and x1 direct the same position, there’s no rotation around z0 . Therefore 0.
d: The distance along z0 is a1 . Since the joint itself is prismatic, a variable distance of d1 is
also added. Therefore a1 + d1 .
α: Since z0 and z1 direct the same position, there’s no rotation around x1 . Therefore 0.
a: There’s no movement along x1 , therefore 0.

Joint i ai−1 αi−1 di θi

1 0 0 a1 + d1 0

The second row is filled as follows:

θ: Since x1 and x2 direct the same position, there’s no rotation around z1 . Since the joint itself
is revolutionary, a variable angle of θ2 is added. Therefore θ2 .
d: The distance along z1 is a2 . Therefore a2 .
α: The rotation from z1 to z2 around x2 is −90◦ . Therefore −90◦ .
a: There’s no movement along x2 , therefore 0.

Joint i ai−1 αi−1 di θi

2 0 −90◦ a2 θ2

The third row is filled as follows:

θ: Since x2 and x3 direct the same position, there’s no rotation around z2 . Therefore 0.
d: The distance along z2 is a3 . Since the joint itself is prismatic, a variable distance of d3 is
also added. Therefore a3 + d3 .
α: Since z2 and z3 direct the same position, there’s no rotation around x3 . Therefore 0.
a: There’s no movement along x3 , therefore 0.

6
 Joint i ai−1 αi−1 di θi
3 0 0 a3 + d3 0

The fourth row is filled as follows:

θ: The angle from x3 to x4 around z3 is −90◦ . Since the joint itself is revolutionary, a variable
angle of θ4 is added. Therefore θ4 − 90◦ .
d: The distance along z3 is a4 + a5 . Therefore a4 + a5 .
α: The rotation from z3 to z4 around x4 is −90◦ . Therefore −90◦ .
a: There’s no movement along x4 , therefore 0.

Joint i ai−1 αi−1 di θi

4 0 −90◦ a4 + a5 θ4 − 90◦

The fifth row is filled as follows:

θ: Since x4 and x5 direct the same position, there’s no rotation around z4 . Since the joint itself
is revolutionary, a variable angle of θ5 is added. Therefore θ5 .
d: There’s no movement along z4 , therefore 0.
α: The rotation from z4 to z5 around x5 is 90◦ . Therefore 90◦ .
a: There’s no movement along x5 , therefore 0.

Joint i ai−1 αi−1 di θi

5 0 90◦ 0 θ5

The sizth row is filled as follows:

θ: Since x5 and x6 direct the same position, there’s no rotation around z5 . Since the joint itself
is revolutionary, a variable angle of θ6 is added. Therefore θ6 .
d: The distance along z5 is a6 + a7 .
α: Since z5 and z6 direct the same position, there’s no rotation around x6 . Therefore 0.
a: There’s no movement along x6 , therefore 0.

Joint i ai−1 αi−1 di θi

6 0 0 a6 + a7 θ6

The complete DH-Table is shown in Table 1.

Joint i ai−1 αi−1 di θi

1 0 0 a1 + d1 0
2 0 −90◦ a2 θ2
3 0 0 a3 + d3 0
4 0 −90◦ a4 + a5 θ4 − 90◦
5 0 90◦ 0 θ5
6 0 0 a6 + a7 θ6

Table 1: DH-Table of 6-DOF Robot Arm

Forward Kinematics Using Matlab

Each row of the DH-Table defines the transformation matrix from one joint to the next. This
transformation matrix is defined as
Ai = T ranszi−1 ,di Rotzi−1 ,θi T ransxi ,ai , Rotxi ,αi
    
1 0 0 0 cθi −sθi 0 0 1 0 0 ai 1 0 0 0
0 1 0 0 sθi c θi 0 0  0 1 0 0  0 cαi
  −sαi 0
Ai = 
0
  
0 1 di   0 0 1 0 0 0 1 0  0 sαi cαi 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1

7
  
cθ i −sθi cαi sθi sαi ai cθi
sθi cθi cαi −cθi sαi ai sθi 
Ai = 
0
 (8)
sαi cαi di 
0 0 0 1
If T is defined to be the transformation between any two joints, then knowing that A is the
transition between any two consecutive joints, it can be written that

Ai = Tii−1

And also
Tba = Aa+1 Aa+2 . . . Ab−1 Ab
Then the forward kinematic model of a 6-DOF system is

T60 = A1 A2 A3 A4 A5 A6 (9)

Since such matrix multiplications with lots of symbolic variables is hard to compute, Matlab is
used. Starting with generating Ai matrices, Equation (8) is implemented. The function ai matrix
takes a row from DH-Table and returns the corresponding Ai .
1 % Takes four parameters from a row of the DH - Table , returns A Matrix of the row .
2 function A = ai_matrix (a , alpha , d , theta )
3 % Abbreviations
4 ca = cos ( alpha ) ; sa = sin ( alpha ) ;
5 ct = cos ( theta ) ; st = sin ( theta ) ;
6
7 % Equation (8)
8 A = [ ct , - st * ca , st * sa , a * ct ;
9 st , ct * ca , - ct * sa , a * st ;
10 0, sa , ca , d;
11 0, 0, 0, 1];
12
13 % Simplify if possible
14 A = simplify ( A ) ;
15 end

Using ai matrix recursively, a function that computes forward kinematics from the whole DH-
Table can be implemented. Below defined f k f rom dh table takes a DH-Table, splits it into rows
and feeds the rows to ai matrix to generate Ai matrices, which then will be multiplied consecutively
according to the Equation (9).
1 % Takes DH - Table in the form of a matrix as a parameter , returns forward kinematic
model of the table .
2 function T = f k _ f r o m _ d h _ t a b l e ( dh_table )
3 T = sym ( eye (4) ) ; % Identity starter
4 for i = 1: size ( dh_table , 1) % For each row
5 % Extract parameters from the row
6 a = dh_table (i , 1) ;
7 alpha = dh_table (i , 2) ;
8 d = dh_table (i , 3) ;
9 theta = dh_table (i , 4) ;
10
11 % Calculate A matrix of the row
12 T_i = ai_matrix (a , alpha , d , theta ) ;
13
14 % Multiply the A matrix with the cumulative multi plicatio n of A ’ s .
15 % Equation (9)
16 T = T * T_i ;
17 end
18
19 % Simplify if possible
20 T = simplify ( T ) ;
21 end

To generate the forward kinematics, DH-Table is required to be defined. Symbolic variables

are used and dh table is defined in the form of a matrix.

8
 1 % symbolic variables used in DH - Table
2 syms a1 a2 a3 a4 a5 a6 a7
3 syms d1 d2 d3 d4 d5 d6
4 syms theta1 theta2 theta3 theta4 theta5 theta6
5
6 % Define DH - Table in a matrix form
7 dh_table = [0 , 0 , a1 + d1 , 0;
8 0 , - pi /2 , a2 , theta2 ;
9 0, 0, a3 + d3 , 0;
10 0 , - pi /2 , a4 + a5 , theta4 - pi /2;
11 0 , pi /2 , 0, theta5 ;
12 0, 0, a6 + a7 , theta6 ];

Ai matrices can be found by only giving the one row of the table to the function f k f rom dh table.
1 A1 = f k _ f r o m _ d h_ t a b l e ( dh_table (1 ,:) )
2 A2 = f k _ f r o m _ d h_ t a b l e ( dh_table (2 ,:) )
3 A3 = f k _ f r o m _ d h_ t a b l e ( dh_table (3 ,:) )
4 A4 = f k _ f r o m _ d h_ t a b l e ( dh_table (4 ,:) )
5 A5 = f k _ f r o m _ d h_ t a b l e ( dh_table (5 ,:) )
6 A6 = f k _ f r o m _ d h_ t a b l e ( dh_table (6 ,:) )

The output is given in the following code snippet.

1 A1 =
2
3 [1 , 0, 0, 0]
4 [0 , 1, 0, 0]
5 [0 , 0, 1 , a1 + d1 ]
6 [0 , 0, 0, 1]
7
8
9 A2 =
10
11 [ cos ( theta2 ) , 0 , - sin ( theta2 ) , 0]
12 [ sin ( theta2 ) , 0 , cos ( theta2 ) , 0]
13 [ 0 , -1 , 0 , a2 ]
14 [ 0, 0, 0 , 1]
15
16
17 A3 =
18
19 [1 , 0, 0, 0]
20 [0 , 1, 0, 0]
21 [0 , 0, 1 , a3 + d3 ]
22 [0 , 0, 0, 1]
23
24
25 A4 =
26
27 [ sin ( theta4 ) , 0 , cos ( theta4 ) , 0]
28 [ - cos ( theta4 ) , 0 , sin ( theta4 ) , 0]
29 [ 0 , -1 , 0 , a4 + a5 ]
30 [ 0, 0, 0, 1]
31
32
33 A5 =
34
35 [ cos ( theta5 ) , 0 , sin ( theta5 ) , 0]
36 [ sin ( theta5 ) , 0 , - cos ( theta5 ) , 0]
37 [ 0, 1, 0, 0]
38 [ 0, 0, 0, 1]
39
40
41 A6 =
42
43 [ cos ( theta6 ) , - sin ( theta6 ) , 0, 0]
44 [ sin ( theta6 ) , cos ( theta6 ) , 0, 0]
45 [ 0, 0, 1 , a6 + a7 ]
46 [ 0, 0, 0, 1]

9
 Then the forward kinematics to each joint can be found by giving the first ith row of the
DH-Table to the f k f rom dh table.
1 T01 = f k _ f r o m _ d h _ t a b l e ( dh_table (1:1 ,:) )
2 T02 = f k _ f r o m _ d h _ t a b l e ( dh_table (1:2 ,:) )
3 T03 = f k _ f r o m _ d h _ t a b l e ( dh_table (1:3 ,:) )
4 T04 = f k _ f r o m _ d h _ t a b l e ( dh_table (1:4 ,:) )
5 T05 = f k _ f r o m _ d h _ t a b l e ( dh_table (1:5 ,:) )
6 T06 = f k _ f r o m _ d h _ t a b l e ( dh_table (1:6 ,:) )

The outputs are given in the following code snippets. To avoid visual clutter, some abbreviation
are made on sin to s, cos to c and theta to t.

10
 1 T01 =
2 [1 , 0, 0, 0]
3 [0 , 1, 0, 0]
4 [0 , 0, 1 , a1 + d1 ]
5 [0 , 0, 0, 1]
6
7 T02 =
8 [ c ( t2 ) , 0, -s ( t2 ) , 0]
9 [ s ( t2 ) , 0, c ( t2 ) , 0]
10 [ 0, -1 , 0, a1 + a2 + d1 ]
11 [ 0, 0, 0, 1]
12
13 T03 =
14 [ c ( t2 ) , 0, -s ( t2 ) , -s ( t2 ) *( a3 + d3 ) ]
15 [ s ( t2 ) , 0, c ( t2 ) , c ( t2 ) *( a3 + d3 ) ]
16 [ 0, -1 , 0, a1 + a2 + d1 ]
17 [ 0, 0, 0, 1]
18
19 T04 =
20 [ c ( t2 ) * s ( t4 ) , s ( t2 ) , c ( t2 ) * c ( t4 ) , -s ( t2 ) *( a3 + a4 + a5 + d3 ) ]
21 [ s ( t2 ) * s ( t4 ) , -c ( t2 ) , c ( t4 ) * s ( t2 ) , c ( t2 ) *( a3 + a4 + a5 + d3 ) ]
22 [ c ( t4 ) , 0, -s ( t4 ) , a1 + a2 + d1 ]

11
23 [ 0, 0, 0, 1]
24
25 T05 =
26 [ s ( t2 ) * s ( t5 ) + c ( t2 ) * c ( t5 ) * s ( t4 ) , c ( t2 ) * c ( t4 ) , c ( t2 ) * s ( t4 ) * s ( t5 ) - c ( t5 ) * s ( t2 ) , -s ( t2 ) *( a3 + a4 + a5 + d3 ) ]
27 [ c ( t5 ) * s ( t2 ) * s ( t4 ) - c ( t2 ) * s ( t5 ) , c ( t4 ) * s ( t2 ) , c ( t2 ) * c ( t5 ) + s ( t2 ) * s ( t4 ) * s ( t5 ) , c ( t2 ) *( a3 + a4 + a5 + d3 ) ]
28 [ c ( t4 ) * c ( t5 ) , -s ( t4 ) , c ( t4 ) * s ( t5 ) , a1 + a2 + d1 ]
29 [ 0, 0, 0, 1]
30
31
32 T06 =
33 [ c ( t6 ) *( s ( t2 ) * s ( t5 ) + c ( t2 ) * c ( t5 ) * s ( t4 ) ) + c ( t2 ) * c ( t4 ) * s ( t6 ) , c ( t2 ) * c ( t4 ) * c ( t6 ) - s ( t6 ) *( s ( t2 ) * s ( t5 ) + c ( t2 ) * c ( t5 ) * s ( t4 ) ) , c ( t2 ) * s (
t4 ) * s ( t5 ) - c ( t5 ) * s ( t2 ) , - ( a6 + a7 ) *( c ( t5 ) * s ( t2 ) - c ( t2 ) * s ( t4 ) * s ( t5 ) ) - s ( t2 ) *( a4 + a5 ) - s ( t2 ) *( a3 + d3 ) ]
34 [ c ( t4 ) * s ( t2 ) * s ( t6 ) - c ( t6 ) *( c ( t2 ) * s ( t5 ) - c ( t5 ) * s ( t2 ) * s ( t4 ) ) , s ( t6 ) *( c ( t2 ) * s ( t5 ) - c ( t5 ) * s ( t2 ) * s ( t4 ) ) + c ( t4 ) * c ( t6 ) * s ( t2 ) , c ( t2 ) * c (
t5 ) + s ( t2 ) * s ( t4 ) * s ( t5 ) , ( a6 + a7 ) *( c ( t2 ) * c ( t5 ) + s ( t2 ) * s ( t4 ) * s ( t5 ) ) + c ( t2 ) *( a4 + a5 ) + c ( t2 ) *( a3 + d3 ) ]
35 [ c ( t4 ) * c ( t5 ) * c ( t6 ) - s ( t4 ) * s ( t6 ) , - c ( t6 ) * s ( t4 ) - c ( t4 ) * c ( t5 ) * s ( t6 ) ,
c ( t4 ) * s ( t5 ) , a1 + a2 + d1 + c ( t4 ) * s ( t5 ) *( a6 + a7 ) ]
36 [ 0, 0,
0, 1]
 Jacobian
The Jacobian Matrix is defined with frame origins and z axes. oi and zi can be found inside of Ti0 .
oi is the first three rows of the last column of Ti0 and zi is the first three row of the third column
of Ti0 . Implementing this property in the Matlab as a filter over Ti results in zi and oi vectors.
Each z and o is stored as vectors in Z and O matrices.
1 T = { T01 T02 T03 T04 T05 T06 }; % All the T matrices found before
2 Z = sym ( zeros (3 ,6) ) ; % Stores z vectors
3 O = sym ( zeros (3 ,6) ) ; % Stores o vectors
4 for i = 1:6 % From each T
5 Ti = T { i };
6 Z (: , i ) = Ti (1:3 ,3) ; % Extract z , store in Z
7 O (: , i ) = Ti (1:3 ,4) ; % Extract o , store in O
8 end

The resulting zi and oi vectors can be extracted from Z and O.

1 % Print all z inside of Z
2 for i = 1:6
3 fprintf ( ’z % d =\ n ’ , i ) ;
4 disp ( Z (: , i ) ) ;
5 end
6 % Print all o inside of O
7 for i = 1:6
8 fprintf ( ’o % d =\ n ’ , i ) ;
9 disp ( O (: , i ) ) ;
10 end

Printing zi and oi gives the following output.

1 z1 =
2 0
3 0
4 1
5
6 z2 =
7 - sin ( theta2 )
8 cos ( theta2 )
9 0
10
11 z3 =
12 - sin ( theta2 )
13 cos ( theta2 )
14 0
15
16 z4 =
17 cos ( theta2 ) * cos ( theta4 )
18 cos ( theta4 ) * sin ( theta2 )
19 - sin ( theta4 )
20
21 z5 =
22 cos ( theta2 ) * sin ( theta4 ) * sin ( theta5 ) - cos ( theta5 ) * sin ( theta2 )
23 cos ( theta2 ) * cos ( theta5 ) + sin ( theta2 ) * sin ( theta4 ) * sin ( theta5 )
24 cos ( theta4 ) * sin ( theta5 )
25
26 z6 =
27 cos ( theta2 ) * sin ( theta4 ) * sin ( theta5 ) - cos ( theta5 ) * sin ( theta2 )
28 cos ( theta2 ) * cos ( theta5 ) + sin ( theta2 ) * sin ( theta4 ) * sin ( theta5 )
29 cos ( theta4 ) * sin ( theta5 )
30
31 o1 =
32 0
33 0
34 a1 + d1
35
36 o2 =
37 0
38 0
39 a1 + a2 + d1

12
40
41 o3 =
42 - sin ( theta2 ) *( a3 + d3 )
43 cos ( theta2 ) *( a3 + d3 )
44 a1 + a2 + d1
45
46 o4 =
47 - sin ( theta2 ) *( a3 + a4 + a5 + d3 )
48 cos ( theta2 ) *( a3 + a4 + a5 + d3 )
49 a1 + a2 + d1
50
51 o5 =
52 - sin ( theta2 ) *( a3 + a4 + a5 + d3 )
53 cos ( theta2 ) *( a3 + a4 + a5 + d3 )
54 a1 + a2 + d1
55
56 o6 =
57 - ( a6 + a7 ) *( c ( t5 ) * s ( t2 ) - c ( t2 ) * s ( t4 ) * s ( t5 ) ) - s ( t2 ) *( a4 + a5 ) - s ( t2 ) *( a3 + d3 )
58 ( a6 + a7 ) *( c ( t2 ) * c ( t5 ) + s ( t2 ) * s ( t4 ) * s ( t5 ) ) + c ( t2 ) *( a4 + a5 ) + c ( t2 ) *( a3 + d3 )
59 a1 + a2 + d1 + c ( t4 ) * s ( t5 ) *( a6 + a7 )

To construct the Jacobian, the type of the joint is important. 6-DOF Robotic Arm has a total
of 6 joints, with 2 prismatic and 4 revolutionary. For a revolute joint, the Jacobian is defined as:
   
J z × (on − oi−1 )
Ji,revolute = vi = i−1
Jwi zi−1
For a prismatic joint, the Jacobian is defined as:
   
J vi zi−1
Ji,prismatic = =
Jwi 0
The first and the third joint is prismatic, and the others are revolute. Then the total Jacobian
can be found as:
 
J = J1,prismatic J2,revolute J3,prismatic J4,revolute J5,revolute J6,revolute (10)

 
z z1 × (o6 − o1 ) z2 z3 × (o6 − o3 ) z4 × (o6 − o4 ) z5 × (o6 − o5 )
J= 0
0 z1 0 z3 z4 z5
Where z0 is the unit z vector of initial frame, assumed to be [0 0 1]T , and 0 is [0 0 0]T . The
calculations are done in Matlab. Since the resulting Jacobian is too long to fit to the paper, they
are not shared.
1 Z0 = [0;0;1];
2 zv = [0;0;0];
3
4 % Each Ji column :
5 J1 = [ Z0 ;
6 zv ];
7
8 J2 = [ cross ( Z (: ,1) , O (: ,6) - O (: ,1) ) ;
9 Z (: ,1) ];
10
11 J3 = [ Z (: ,2) ;
12 zv ];
13
14 J4 = [ cross ( Z (: ,3) , O (: ,6) - O (: ,3) ) ;
15 Z (: ,3) ];
16
17 J5 = [ cross ( Z (: ,4) , O (: ,6) - O (: ,4) ) ;
18 Z (: ,4) ];
19
20 J6 = [ cross ( Z (: ,5) , O (: ,6) - O (: ,5) ) ;
21 Z (: ,5) ]; % Equation (10)
22
23 J = [ J1 J2 J3 J4 J5 J6 ];

13
 Demonstration of Velocity Kinematics
The velocity kinematics is defined by the first three rows of the Jacobian. To demonstrate the
end-effector velocity in the base frame, a configuration must be specified. The configuration is
determined by the joint parameters d1 , θ2 , d3 , θ4 , θ5 , and θ6 . The choice of parameters defines
the current position (configuration) of the model and affects the Jacobian. For simplicity, let us
choose all parameters as 0, so that the model is as shown in Figure 4.
To start with a simple end-effector result, let us set the revolute joints’ angular velocities to
zero (ω = 0) and assign the prismatic joints velocities of 3 and 4. If the first prismatic joint is
moving upward at 3 units per second, and the second prismatic joint is moving right at 4 units per
second, we expect the end-effector to move accordingly in the base frame. Since ”up” corresponds
to z0 and ”right” corresponds to y0 , it is expected that the end-effector will move 3 units per
second along z0 and 4 units per second along y0 .

Ẋ = J · q̇ (11)

Inserting these values into the Jacobian and applying the mathematical definition of the Jaco-
bian given in Equation (11), the end-effector velocity in the base frame can be calculated:
1 q_num = [0 0 0 0 0 0]; % configuration of the model
2 q_dot = [3; 0; 4; 0; 0; 0]; % speed of each joint
3 J_num = subs (J , [ d1 theta2 d3 theta4 theta5 theta6 ] , q_num ) ; % current Jacobian
4 x_dot = J_num * q_dot ; % end - effector speed , Equation (11)

The output from the above code is:  

0
4
 
3
Ẋ =  
0
 
0
0
The first three elements of Ẋ correspond to the linear velocity ⃗v of the end effector expressed
in the base frame, while the last three elements correspond to the angular velocity ω ⃗ of the end
effector, also expressed in the base frame.
More specifically:
• The first entry 0 indicates that there is no velocity along the x0 axis.
• The second entry 4 represents a velocity of 4 units per second along the y0 axis.
• The third entry 3 represents a velocity of 3 units per second along the z0 axis.

• The fourth, fifth, and sixth entries are zero, meaning there is no rotational motion of the end
effector around any axis.
In conclusion, the computed end-effector velocity matches the expected behavior based on the
joint movements: the first prismatic joint causes motion along z0 and the second prismatic joint
causes motion along y0 . This demonstrates that the Jacobian matrix correctly captures the velocity
kinematics of the robot for the given configuration and joint velocities.

14
 𝑎𝑎7

𝜃𝜃6

𝑎𝑎6

𝑎𝑎3 𝑎𝑎4 𝑎𝑎5

𝜃𝜃4
𝑑𝑑3
𝑎𝑎2
𝜃𝜃5

𝜃𝜃2

𝑎𝑎1 𝒛𝒛𝟎𝟎

𝑑𝑑1
𝒐𝒐𝟎𝟎 𝒚𝒚𝟎𝟎

𝒙𝒙𝟎𝟎

Figure 9: Configuration with θ5 = 90◦

Figure 9 shows a new configuration with θ5 = 90◦ . Using this configuration, let us define the
angular speed of joint 2 to be 2 radians per second, and keep the prismatic joint velocities the
same as before.
Since the direction of rotation of joint 2 (z⃗1 ) coincides with the z0 axis and the magnitude of
the rotation is 2, the rotation around z⃗0 is also 2.

ω⃗0 = z⃗1 = 2z⃗0

The radius from the z0 axis to the end effector is a3 + a4 + a5 in the direction of y0 , therefore

⃗r = (a3 + a4 + a5 )y⃗0

Then, since linear velocity is the cross product of angular velocity and radius:

⃗ × ⃗r = 2z⃗0 × (a3 + a4 + a5 )y⃗0 = −2(a3 + a4 + a5 )x⃗0

⃗v = ω

This result represents the linear speed in the −x⃗0 direction, caused by the angular velocity
from joint 2, with a magnitude of 2(a3 + a4 + a5 ).
From the previous example, it was found that due to joints 1 and 3, there also exists linear
speed along y⃗0 and z⃗0 . Combining all contributions, it is expected that the end effector has a linear
velocity of
⃗v = −2(a3 + a4 + a5 )x⃗0 + 4y⃗0 + 3z⃗0
and an angular velocity of
ω
⃗ = 2z⃗0 .
Computing the result in MATLAB:
1 q_num = [0 0 0 0 pi /2 0]; % configuration of the model
2 q_dot = [3; 2; 4; 0; 0; 0]; % speed of each joint
3 J_num = subs (J , [ d1 theta2 d3 theta4 theta5 theta6 ] , q_num ) ; % current Jacobian
4 x_dot = J_num * q_dot % end - effector speed

The result is:  

−2a3 − 2a4 − 2a5

 4 

 3 
Ẋ = 
 .
 0 

 0 
2

15
 The first three elements of Ẋ correspond to the linear velocity ⃗v of the end effector expressed
in the base frame, while the last three elements correspond to the angular velocity ω ⃗ of the end
effector, also expressed in the base frame.
More specifically:
• The first entry −2(a3 + a4 + a5 ) represents the velocity along the −x0 axis. This matches
our geometric expectation that the rotation of joint 2 creates a movement toward −x0 .
• The second entry 4 indicates a velocity of 4 units per second along the y0 axis, resulting from
the prismatic joint 3 motion.
• The third entry 3 represents a velocity of 3 units per second along the z0 axis, resulting from
the prismatic joint 1 motion.
• The fourth and fifth entries, corresponding to angular velocities around the x0 and y0 axes,
are zero. This means the end effector is not rotating around these axes.
• The sixth entry 2 represents an angular velocity of 2 radians per second about the z0 axis,
which is exactly the contribution from the rotation of joint 2.
In conclusion, the output Ẋ provides a complete description of the instantaneous motion of the
end effector in the base frame, both in terms of its translational velocity and its rotational velocity.
The numerical result obtained using the Jacobian matches the physical intuition and the manual
geometric analysis performed earlier. This demonstrates the accuracy and validity of the velocity
kinematics via the Jacobian matrix.

16