KON 318E

Homework II

Semanur GÜLMEZ 040220619

May 23, 2025

Istanbul Technical University

Faculty of Electrical and Electronics Engineering
Department of Control and Automation Engineering
KON 318E - Introduction to Robotics
 Q1
To make the singularity analysis, the complete Jacobian is required. Let us use the code provided
in the previous work, and use Matlab to determine its determinant.
1 % Calculate the determinant of the Jacobian and simplify it
2 det_J = det ( J ) ;
3 d e t _ J v _ s i m p l i f i e d = simplify ( det_J )

The result is printed as follows:

sin(θ5 )(a3 + a4 + a5 + d3 ) (1)

The singularity points are where the determinant equals to zero. Therefore, equating the Equation
(1) to zero, reveals the following singularity conditions:
• sin(θ5 ) = 0, π
• a3 + a4 + a5 + d3 = 0
Let us analyze each condition in detail. Figure 1 shows the z-axes that have been determined in the
previous work. The Jacobian is also provided, with its rank calculated as 6, making the Jacobian
full rank, therefore its Null Vector (N) does not exists.
1 J
2 rank ( J )
3 N = simplify ( null ( J ) ) % returns empty vector
 
0 α1 −s2 c2 c4 s5 (a6 + a7 ) (a6 + a7 )(s2 s5 + c2 c5 s4 ) 0
0 α2
 c2 c4 s2 s5 (a6 + a7 ) −(a6 + a7 )(c2 s5 − c5 s2 s4 ) 0 

1 0 0 −s4 s5 (a6 + a7 ) c4 c5 (a6 + a7 ) 0 
J = 
0 0
 0 −s2 c2 c4 c2 s4 s5 − c5 s2 

0 0 0 c2 c4 s2 c2 c5 + s2 s4 s5 
0 1 0 0 −s4 c4 s5
where
α1 = −(a6 + a7 )(c2 c5 + s2 s4 s5 ) − c2 (a3 + a4 + a5 + d3 )
α2 = −(a6 + a7 )(c5 s2 − c2 s4 s5 ) − s2 (a3 + a4 + a5 + d3 )

𝑎3 𝑎4 𝑎5 𝑎6 𝑎7

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

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

𝑑3
𝑎2 𝜃5
𝒛𝟏

𝜃2

𝒐𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎

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

1
 𝑎3 𝑎4 𝑎5 𝑎6 𝑎7

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

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

𝑑3
𝑎2
𝒛𝟏

𝜃2

𝒐𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎

Figure 2: Configuration with θ5 = 0

1. sin(θ5 ) = 0, π
Consider the case where sin(θ5 ) = 0, π, specifically when θ5 = 0. Figure 2 illustrates the corre-
sponding configuration, which closely resembles that of Figure 1, with the exception that θ5 is fixed
at a constant value of zero. In this configuration, the z3 and z5 axes are aligned parallel to each
other. Notably, both z3 and z5 correspond to the rotation axes of revolute joints, and under this
condition, they effectively coincide, enabling rotation about a common axis. This alignment allows
the definition of a combined angle θ4,6 = θ4 + θ6 , which can represent the cumulative rotation con-
tributed by joints 4 and 6. Consequently, a single angle suffices to describe the motion that would
otherwise require two independent joint angles. This redundancy renders one of the revolute joints
effectively unnecessary. Mathematically, this configuration leads to linear dependence between the
Jacobian columns corresponding to joints 4 and 6, causing the Jacobian matrix to lose rank.
The Jacobian with θ5 = 0 is calculated and its rank is found as 5, meaning it is no longer full
rank. Looking at the Jacobian, it is easy to see that the column 4 and 6 are the same, thus they
are linearly dependent. Therefore, Jacobian’s null vector exists and shows the dependency among
the columns of the Jacobian.
1 theta5 = 0
2 J_theta5 = simplify ( subs ( J ) )
3 rank ( J_theta5 )
4 N = simplify ( null ( J_theta5 ) )
 
0 −c2 (a3 + a4 + a5 + a6 + a7 + d3 ) −s2 0 (a6 + a7 )c2 s4 0
0
 −s2 (a3 + a4 + a5 + a6 + a7 + d3 ) c2 0 (a6 + a7 )s2 s4 0 
1 0 0 0 c4 (a6 + a7 ) 0 
Jθ5 =0 = 
0
 0 0 −s2 c2 c4 −s2 

0 0 0 c2 c4 s2 c2 
0 1 0 0 −s4 0
 
0
0
 
0
Nθ5 =0 = 
−1

 
0
1
As for θ5 = π, the same results are valid with the exception that the rotation axes are in
opposite direction causing θ4,6 = θ4 − θ6 . This change does not effect the linear dependency, thus

2
 valid.

2. a3 + a4 + a5 + d3 = 0
The corresponding configuration is shown in Figure 3. The loss of rank is rather harder to notice.
Therefore only the Jacobian with d3 = −(a3 + a4 + a5 ), its rank which is 5, and its Null Vector is
calculated. The loss of rank proves the existence of the Null Vector, which shows the dependency
among the columns of the Jacobian.
1 d3 = -( a3 + a4 + a5 )
2 J_d3 = simplify ( subs ( J ) )
3 rank ( J_d3 )
4 N = simplify ( null ( J_d3 ) )

Jd3 =
 
0 −(a6 + a7 )(c2 c5 + s2 s4 s5 ) −s2 c2 c4 s5 (a6 + a7 ) (a6 + a7 )(s2 s5 + c2 c5 s4 ) 0
0
 −(a6 + a7 )(c5 s2 − c2 s4 s5 ) c2 c4 s2 s5 (a6 + a7 ) −(a6 + a7 )(c2 s5 − c5 s2 s4 ) 0 

1
 0 0 −s4 s5 (a6 + a7 ) c4 c5 (a6 + a7 ) 0 

0
 0 0 −s2 c2 c4 c2 s4 s5 − c5 s2 

0 0 0 c2 c4 s2 c2 c5 + s2 s4 s5 
0 1 0 0 −s4 c4 s5
 
0
 −s5 /c4 
 
 0 
Nd3 =−(a3 +a4 +a5 ) = 
 
 −c 5


−s4 s5 /c4 
1

𝜃5 𝑎6 𝑎7

𝒐𝟐,𝟑,𝟒,𝟓 𝒐𝟔

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

𝑎2
𝒛𝟏

𝜃2

𝒐𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎

Figure 3: Configuration with a3 + a4 + a5 + d3 = 0

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

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

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

𝜃2

𝒐𝟏 𝒚𝟏

𝒙𝟏

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎 𝒚𝟎

𝒙𝟎

Figure 4: Frames of the model

Q2
The inverse kinematics problem can be divided into two parts: from o0 to o3 , and from o3 to o6 .
The relation between these three frames are as follows.
 
1 0 0 0
0 1 0 0
o00 = 
0 0 1 0


0 0 0 1

o06 = Td 06 · o00 (2)

o03 = o06 · T36 (3)
Where Td 06
is the transformation from to o00 o06 .
Figure 5 shows the proper configuration for inverse kinematics calculation for the first part.
Since we have calculated the position and orientation of o03 , let (x3 , y3 , z3 ) be the position vector
of o03 . The height of the o3 is determined by a1 + a2 + d1 , which is p
equal to z3 . Similarly, the length
of the extending arm is determined by a3 + d3 which is equal to x23 + y32 .
θ2 ’s direction is in the z2 direction starting from y2 . In the figure, its direction is shown. The
angle of the right triangle with perpendicular sides y3 and x3 . The angle of the triangle can be
calculated with atan2 function. The resulting angle plus −θ2 adds up to π/2.

d1 = z3 − a1 − a2 (4)

θ2 = atan2(y3 , x3 ) − π/2 (5)

p
d3 = (x3 )2 + (y3 )2 − a3 (6)

4
 𝑧3

𝑎2 𝒐𝟑

𝜃2

𝑎1 𝒛𝟎

𝑑1

𝒐𝟎 𝒚𝟎 𝑦3
-𝜃2
𝒙𝟎

𝑥32 + 𝑦32
𝑎𝑡𝑎𝑛2 𝑦, 𝑥

𝑥3

Figure 5: Inverse Kinematics from o0 to o3

Assuming o6 is located at an arbitrary location (x6 , y6 , z6 ) and is represented in o3 as x36 , y63 , z63 ,
the relation between these two frames are as follows:
o36 = T03 o06 (7)
Figure 6 shows the proper configuration for inverse kinematics calculation for the second part.
Looking at the Figure 6, the angle θ4 is the angle of a right triangle with the perpendicular size
length as x36 and y63q
. The hypotenuse of this triangle is the projection of the expanding arm on to
2 2
the x3 y3 , which is x36 + y63 .
The length of the expandingq arm is a6 + a7 , using Pythagorean theorem, the projection of the
2 2
expanding arm on to z3 axis is (a6 + a7 )2 − x36 − y63 .
q
2 2
Therefore θ5 is the angle of a right triangle with the perpendicular size length as (a6 + a7 )2 − x36 − y63
q
2 2
and x36 + y63 . But since the direction of θ5 is opposite to the initial definition of it, here, −θ5
is used. q
2 2
The height of the end effector (z63 ) is then given by a4 + a5 + (a6 + a7 )2 − x36 − y63 .

θ4 = atan2(y63 , x36 ) (8)

q q
2 2 2 2
θ5 = atan2( (a6 + a7 )2 − x36 − y63 , x36 + y63 ) − π/2 (9)
In this model θ6 only effects the orientation of the end effector. Therefore it can be calculated
from rotation matrices. Since R60 is known, and both R65 and R50 can be computed from DH-Table,
an equation in terms of other joint variables for θ5 can be derived.
R50 R65 = R60 ⇐⇒ R65 = R05 R60
 
c6 −s6 0
R65 = s6 c6 0
0 0 1
 
c4 c5 s2 s5 + c2 c5 s4 c5 s2 s4 − c2 s5
R05 R60 =  −s4 c2 c4 c4 s2 
c4 s5 c2 s4 s5 − c5 s2 c2 c5 + s2 s4 s5
Then equating the first columns’ first two elements reveals an equation for θ6 .
c6 = c4 c5

5
 𝑧63

2 2
𝑎𝑡𝑎𝑛2 𝑦, 𝑥 (𝑎6 + 𝑎7 )2 −(𝑥63 + 𝑦63 )

𝑎5
2 2
𝑥63 + 𝑦63

𝑎4 𝑎4 + 𝑎5

𝒛𝟑
𝒚𝟑
𝑦63

𝒐𝟑 𝒙𝟑 𝜃4

𝑥63

Figure 6: Inverse Kinematics from o3 to o6

s6 = −s4
θ6 = atan2(s6 , c6 ) = atan2(−s4 , c4 c5 ) (10)
Equations (4, 5, 6, 8, 9) and (10) can be numerically solved to find the joint variables for a given
Td . All the process so far is given below.
1 % symbolic variables used in DH - Table
2 clear ; clc ;
3 syms a1 a2 a3 a4 a5 a6 a7
4 syms d1 d3
5 syms theta2 theta4 theta5 theta6
6
7 % Define DH - Table in a matrix form
8 dh_table = [0 , 0 , a1 + d1 , 0;
9 0 , - pi /2 , a2 , theta2 ;
10 0, 0, a3 + d3 , 0;
11 0 , - pi /2 , a4 + a5 , theta4 - pi /2;
12 0 , pi /2 , 0, theta5 ;
13 0, 0, a6 + a7 , theta6 ]
14
15 a1 =1; a2 =1; a3 =1; a4 =0; a5 =0; a6 =0; a7 =0; % Example joint lenghts
16 dh_table = subs ( dh_table ) % Insert joint lenghts to DH - Table .
17
18 o00 = [1 0 0 0; % Fixed frame
19 0 1 0 0;
20 0 0 1 0;
21 0 0 0 1]
22 Td = [0 , 1 , 0 , 0; % Example Td ( initial configuration of the robot )
23 0, 0 , 1 , 1;
24 1, 0 , 0 , 2;
25 0, 0 , 0 , 1]
26
27 o06 = Td * o00 % 6 th frame , Equation (2)
28 x6 = o06 (1 , 4) % 6 th frame locations
29 y6 = o06 (2 , 4)
30 z6 = o06 (3 , 4)
31
32 T36 = simplify ( f k _ f r o m _ d h _ t a b l e ( dh_table (4:6 ,:) ) )
33 T63 = inv ( T36 )
34 o03 = o06 * T63 % 3 th frame , Equation (3)
35 x3 = o03 (1 , 4)
36 y3 = o03 (2 , 4)
37 z3 = o03 (3 , 4)

6
38
39 eq_d1 = d1 == simplify ( z3 - a1 - a2 ) % Equation (4)
40 eq_theta2 = theta2 == simplify ( atan2 ( y3 , x3 ) ) - pi /2 % Equation (5)
41 eq_d3 = d3 == simplify ( sqrt ( x3 ^2 + y3 ^2) - a3 ) % Equation (6)
42
43 T03 = simplify ( f k _ f r o m _ d h _ t a b l e ( dh_table (1:3 ,:) ) )
44 T30 = inv ( T03 )
45 o36 = T30 * o06 % 6 th frame relative to 3 rd frame , Equation (7)
46 x36 = o36 (1 , 4)
47 y36 = o36 (2 , 4)
48 z36 = o36 (3 , 4)
49
50 eq_theta4 = theta4 == atan2 ( y36 , x36 ) % Equation (8)
51 eq_theta5 = theta5 == atan2 ((( a6 + a7 ) ^2 - x36 ^2 - y36 ^2) ^0.5 , ( x36 ^2 + y36 ^2) ^0.5) -
pi /2 % Equation (9)
52
53 T05 = simplify ( f k _ f r o m _ d h _ t a b l e ( dh_table (1:5 ,:) ) )
54 R05 = T05 (1:3 , 1:3)
55 R06 = Td (1:3 , 1:3)
56 R56 = simplify ( R05 . ’ * R06 )
57 eq_theta6 = theta6 == atan2 ( R56 (2 ,1) , R56 (1 ,1) ) % Eqaution (10)

Q3
With the nonlinear property of the inverse kinematics, it is hard to find a solution without proper
initial guesses. In fact, it was not possible to find a proper script that can found the solution at
any reachable location. The following script is used to solve the system of equations.
1 assumeAlso ([ x3 , y3 , z3 , d1 , d3 , theta2 , theta4 , theta5 , theta6 ] , ’ real ’) ;
2 assumeAlso ( x3 ^2 + y3 ^2 >= a3 ^2) ;
3
4 initialGuess = [0 0 0 0 0 0]
5 eqs = [ eq_d1 , eq_theta2 , eq_d3 , eq_theta4 , eq_theta5 , eq_theta6 ]
6 sol = vpasolve ( eqs , [ d1 , theta2 , d3 , theta4 , theta5 , theta6 ] , initialGuess )
7 d1 = sol . d1
8 theta2 = sol . theta2
9 d3 = sol . d3
10 theta4 = sol . theta4
11 theta5 = sol . theta5
12 theta6 = sol . theta6
13 dh_table_val = subs ( dh_table )
14 T06 = f k _ f r o m _ d h _ t a b l e ( dh_table_val )

Unfortunately, the provided script is not able to determine a solution, but can only verify
some of the solutions, due to the complexity of the problem, as well as multiple solutions existing
at the same time.
Therefore, an assumption is made: The last three joints’ frames are at the same position,
making them a spherical wrist, and joint lengths zero. With this assumption, the problem can
be divided into two parts: Determining the position of the end effector, and determining the
orientation of the end effector.
The position problem is the same as before, Equation (4), (5) and (6) can be used to calculate
the first three joint variables.
The orientation problem is similar to how θ6 was calculated. After the position part is solved,
using the values of the first three joints’ variables, a numerical T30 can be written. It is also known
that the given Td = T60 is numerical. Therefore,

T63 = T30 T60

can be written and is numeric. The same matrix, T63 can be derived from the DH-Table as a
function of the last three joint variables, θ4 , θ5 and θ6 . Equating both matrices term-by-term will
give the result for the joint variables.
After the assumption is done, the code snippet can be updated as:

7
 1 clear ; clc ;
2 syms d1 d3 theta2 theta4 theta5 theta6 real
3
4 % Parameters and DH - Table
5 a1 = 1; a2 = 1; a3 = 1; a4 = 0; a5 = 0; a6 = 0; a7 = 0;
6 dh_table = [0 , 0 , a1 + d1 , 0;
7 0 , - pi /2 , a2 , theta2 ;
8 0, 0, a3 + d3 , 0;
9 0 , - pi /2 , a4 + a5 , theta4 - pi /2;
10 0 , pi /2 , 0, theta5 ;
11 0, 0, a6 + a7 , theta6 ];
12
13 % Matrix defining desired position and orientation
14 Td = [0 , 1 , 0 , 0;
15 0, 0 , 1 , 1;
16 1, 0 , 0 , 2;
17 0, 0 , 0 , 1];
18
19 % Positional Inverse Kinematics
20 pd = Td (1:3 ,4) ;
21 eq_d1 = d1 == pd (3) - a1 - a2 ; % Equation (4)
22 eq_theta2 = theta2 == atan2 ( pd (2) , pd (1) ) - pi /2; % Equation (5)
23 eq_d3 = d3 == sqrt ( pd (1) ^2 + pd (2) ^2) - a3 ; % Equation (6)
24 eqs_pos = [ eq_d1 , eq_theta2 , eq_d3 ];
25 sol_pos = vpasolve ( eqs_pos , [ d1 , theta2 , d3 ] , [1 , pi /2 , 1]) ;
26
27 % Solution for position
28 d1_val = double ( sol_pos . d1 ) ;
29 theta2_val = double ( sol_pos . theta2 ) ;
30 d3_val = double ( sol_pos . d3 ) ;
31
32 % Orientation Problem
33 dh_partial = subs ( dh_table (1:3 ,:) , [ d1 , theta2 , d3 ] , [ d1_val , theta2_val , d3_val ]) ;
34 T03 = double ( f k _ fr o m _ d h _ t a b l e ( dh_partial ) ) ;
35 T36 = inv ( T03 ) * Td ; % Numerical T36 with known joint variables , and Td
36 T36_sym = simplify ( f k _ f r o m _d h _ t a b l e ( dh_table (4:6 , :) ) ) ; % Symbolical T36 from the
table , unknown variables
37 orientaion = T36 == T36_sym ;
38 sol = solve ( orientaion , [ theta4 , theta5 , theta6 ]) ;
39
40 % Solution for Orientation
41 theta4_val = sol . theta4 (1) ; % In case solver finds multiple solutions , take the
first one
42 theta5_val = sol . theta5 (1) ;
43 theta6_val = sol . theta6 (1) ;
44
45 disp ( ’ Number of solutions : ’)
46 disp ( length ( sol . theta4 ) ) ;
47
48 % Substitude the solution to the table
49 dh_all = subs ( dh_table , ...
50 [ d1 , theta2 , d3 , theta4 , theta5 , theta6 ] , ...
51 [ d1_val , theta2_val , d3_val , theta4_val , theta5_val , theta6_val ]) ;
52
53 fprintf ( ’ Solution is :\ n ’) ;
54 fprintf ( ’ d1 = %.4 f \ n ’ , d1_val ) ;
55 fprintf ( ’ theta2 = %.4 f \ n ’ , theta2_val ) ;
56 fprintf ( ’ d3 = %.4 f \ n ’ , d3_val ) ;
57 fprintf ( ’ theta4 = %.4 f \ n ’ , theta4_val ) ;
58 fprintf ( ’ theta5 = %.4 f \ n ’ , theta5_val ) ;
59 fprintf ( ’ theta6 = %.4 f \ n ’ , theta6_val ) ;
60
61 T06 = double ( f k _ fr o m _ d h _ t a b l e ( dh_all ) ) ;
62 disp ( ’ Real T06 : ’)
63 disp ( T06 )
64 disp ( ’ Expected Td : ’)
65 disp ( Td )

8
 Example 1
The initial configuration (Figure 4) of the robot will be found, with the joint lengths as 1 (Spherical
joint lengths are assumed to be 0).
 
0 1 0 0
0 0 1 1
Td = 1 0 0 2


0 0 0 1

Since this is the initial configuration, all joint variables are expected to be zero. Running the script
with Td reveals that:
1 Number of solutions :
2 1
3
4 Solution is :
5 d1 = 0.0000
6 theta2 = 0.0000
7 d3 = 0.0000
8 theta4 = 0.0000
9 theta5 = 0.0000
10 theta6 = 0.0000
11 Real T06 :
12 0 1 0 0
13 0 0 1 1
14 1 0 0 2
15 0 0 0 1
16
17 Expected Td :
18 0 1 0 0
19 0 0 1 1
20 1 0 0 2
21 0 0 0 1

The solution is found as expected.

Example 2
 
1 0 0 −2
0 1 0 0
Td = 
0

0 1 3
0 0 0 1

1 Number of solutions :
2 1
3
4 Solution is :
5 d1 = 1.0000
6 theta2 = 1.5708
7 d3 = 1.0000
8 theta4 = 0.0000
9 theta5 = 1.5708
10 theta6 = 0.0000
11 Real T06 :
12 1 0 0 -2
13 0 1 0 0
14 0 0 1 3
15 0 0 0 1
16
17 Expected Td :
18 1 0 0 -2
19 0 1 0 0
20 0 0 1 3
21 0 0 0 1

The solution is found as expected.

9
 Example 3
 
0 1 0 0
0 0 1 1
Td = 
1

0 0 2
0 0 0 1
1 Number of solutions :
2 4
3
4 Solution is :
5 d1 = 0.0000
6 theta2 = 0.0000
7 d3 = 0.0000
8 theta4 = 0.0000
9 theta5 = 0.0000
10 theta6 = 0.0000
11 Real T06 :
12 0 1 0 0
13 0 0 1 1
14 1 0 0 2
15 0 0 0 1
16
17 Expected Td :
18 0 1 0 0
19 0 0 1 1
20 1 0 0 2
21 0 0 0 1

4 solutions were found, one of them is checked and is as expected.

Example 4
 
0 0 1 7
−1 0 0 3
Td =  
0 −1 0 5
0 0 0 1
Solver found a solution for the position, but could not found a solution for the orientation. Change
the Td slightly.  
0 0 1 7
−1 0 0 0
Td = 
 0 −1 0 5


0 0 0 1
1 Number of solutions :
2 2
3
4 Solution is :
5 d1 = 3.0000
6 theta2 = -1.5708
7 d3 = 6.0000
8 theta4 = 0.0000
9 theta5 = 0.0000
10 theta6 = 1.5708
11 Real T06 :
12 0 0 1 7
13 -1 0 0 0
14 0 -1 0 5
15 0 0 0 1
16
17 Expected Td :
18 0 0 1 7
19 -1 0 0 0
20 0 -1 0 5
21 0 0 0 1

2 solutions found, one of them is checked and is as expected.

10