EN4563: ROBOTICS

Mini Project
Kinematic Analysis of a Robot Arm

Team Member Index Number

G.B.U.Perera 190446F
S.P.Prathapasighe 190470X
D.R.R.T.Wijesuriya 190712T

Department of Electronics and Telecommunication Engineering

University of Moratuwa
Faculty of Engineering
 1. Introduction

In the project we have designed a 5 degree of freedom robot arm manipulator design with RRRRR
configuration.

2. Denavit-Hartenberg (DH) Table

Figure 1: Image of the Robot Figure 2: Frame Assignment Diagram

As the above diagrams we can determine the DH table as follows,

Link ai ⍺i di θi
1 0 0o d1 0
2 a2 -90o d2 θ2*
3 a3 0o 0 θ3*- 45o
4 0 -90o 0 θ4*- 45o
5 0 90o d5 θ5*
6 a6 0o 0 90o + θ6*

For our robot arm, link length and link offset values are given as follows,
𝑑! = 6.1𝑐𝑚 , 𝑑" = 7.001𝑐𝑚 , 𝑎" = 1.3𝑐𝑚 , 𝑎# = 12.021𝑐𝑚 , 𝑑$ = 12.171𝑐𝑚 , 𝑎% = 13𝑐𝑚

3. Forward Kinematics

3.1. Forward Kinematics Calculation

From the above determined DH Parameters, we calculated the transformation matrices of each joint
frames, and we calculated the transformation matrix of the end effector. It can be expressed as follows,
𝑟!! 𝑟"! 𝑟#! 𝑡!
𝑟 𝑟 𝑟 𝑡
𝐴 = 1 !" "" #" " 4
𝑟!# 𝑟"# 𝑟## 𝑡#
0 0 0 1
r11 = cos(0.0174θ6+1.571θ5)(cos(0.017θ3-0.785)cos(0.017θ4-0.785)cos(0.017θ2)-sin(0.017θ3-0.785)sin(0.017θ4-0.785)cos(0.017θ2))+
sin(0.017θ2)sin(0.017θ5))-sin(0.017θ6+1.57)(cos(0.017θ3-0.785)sin(0.017θ4-0.785)cos(0.017θ2)+cos(0.017θ4-
0.785)sin(0.017θ30.785)cos(0.0174θ2)

r21 = -cos(0.017θ6+1.570)(cos(0.017θ3-0.785)sin(0.017θ4-0.785)cos(0.017θ2)+cos(0.017θ4-0.785)sin(0.017θ3-0.785)cos(0.017θ2))-
sin(0.017θ6+1.571)(cos(0.017θ5)(cos(0.017θ3-0.785)cos(0.017θ4-0.785)cos(0.017θ2)-sin(0.017θ3-0.785)sin(0.017θ4-0.785)cos(0.017θ2))+
sin(0.017 θ2)sin(0.017 θ5))

r31 = sin(0.017θ5)(cos(0.017θ3-0.785)cos(0.017 θ4 - 0.785)cos(0.017 θ2) - sin(0.017 θ3 - 0.785)sin(0.017 θ4-0.785)cos(0.017x2))- cos(0.017

θ5)sin(0.017 θ2)
r12 = -sin(0.017θ6+1.571)(cos(0.017θ3-0.785)sin(0.017θ4-0.785)sin(0.017θ2) + cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785)sin(0.017 θ2)) -
cos(0.017 θ6 + 1.571)(cos(0.017 θ2)sin(0.017 θ5) - cos(0.017 θ5)(cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)sin(0.017 θ2) - sin(0.017 θ3 -
0.785)sin(0.017 θ4 - 0.785)sin(0.017 θ4)))

r22 = sin(0.017 θ6 + 1.571)(1.0cos(0.017 θ2)sin(0.017 θ5) - cos(0.017 θ5)(cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)sin(0.017 θ2) - sin(0.017 θ3
- 0.785)sin(0.017 θ4 - 0.785)sin(0.017 θ2))) - cos(0.017 θ6 + 1.571)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785)sin(0.017 θ2) + cos(0.017 θ4 -
0.785)sin(0.017 θ3 - 0.785)sin(0.017 θ2))

r32 = cos(0.017 θ2)cos(0.017 θ5) + sin(0.017 θ5)(cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)sin(0.017 θ2) - sin(0.017 θ3 - 0.785)sin(0.017 θ4 -
0.785)sin(0.017 θ2))

r13 = sin(0.01 θ6 + 1.571)(sin(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) - cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)) - cos(0.017 θ6 +

1.571)cos(0.017 θ5)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) + cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785))

r23 = cos(0.017 θ6 + 1.571)(sin(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) - cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)) + sin(0.017 θ6 +

1.571)cos(0.017 θ5)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) + cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785))

r33 = sin(0.017 θ5)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) + cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785))

t1 = 1.3cos(0.017 θ2) + 13.0cos(0.017 θ6 + 1.571)(cos(0.017 θ5)(cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)cos(0.017 θ2) - 1.0sin(0.017 θ3 -

0.785)sin(0.017 θ4 - 0.785)cos(0.017 θ2)) + sin(0.017 θ2)sin(0.017 θ5)) - 13.0sin(0.017 θ6 + 1.571)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 -
0.785)cos(0.017 θ2) + cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785)cos(0.017 θ2)) + 12.021cos(0.017 θ3 - 0.785)cos(0.017 θ2) - 12.171cos(0.017
θ3 - 0.785)sin(0.017 θ4 - 0.785)cos(0.017 θ2) - 12.171cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785)cos(0.017 θ2)

t2 = 1.3sin(0.017 θ2) - 13.0sin(0.017 θ6 + 1.571)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785)sin(0.017 θ2) + cos(0.017 θ4 - 0.785)sin(0.017 θ3

- 0.785)sin(0.017 θ2)) + 12.021cos(0.017 θ3 - 0.785)sin(0.017 θ2) - 13.0cos(0.017 θ6 + 1.571)(cos(0.017 θ2)sin(0.017 θ5) - cos(0.017
θ5)(cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)sin(0.017 θ2) - sin(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785)sin(0.017 θ2))) - 12.171cos(0.017 θ3
- 0.785)sin(0.017 θ4 - 0.785)sin(0.017 θ2) - 12.171cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785)sin(0.017 θ2)

t3 = 12.171sin(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) - 12.021sin(0.017 θ3 - 0.785) + 13.0sin(0.017 θ6 + 1.571)(sin(0.017 θ3 - 0.785)sin(0.017

θ4 - 0.785) - cos(0.017 θ3 - 0.785)cos(0.017 θ4 - 0.785)) - 12.171cos(0.017 θ4 - 0.785)cos(0.017 θ4 - 0.785) - 13.0cos(0.0176 θ6 +
1.571)cos(0.017 θ5)(cos(0.017 θ3 - 0.785)sin(0.017 θ4 - 0.785) + cos(0.017 θ4 - 0.785)sin(0.017 θ3 - 0.785)) + 13.101

3.2. Forward Kinematics Verification

For the given joint parameters as,
𝜃" = 0, 𝜃# = −5, 𝜃& = 50, 𝜃$ = 0, 𝜃% = −55

We get the homogenous matrix as,

The MATLAB plot of the manipulator is given below,

Figure 3: MATLAB plot for forward kinematics

The measured location from the manipulator is nearly equal with the above results. (x = 29cm, y = 0cm,
z =13.5cm)

4. Inverse Kinematics

4.1. θ2 Calculation

In the initial calculations, we are calculating the θ2 value using aligning the robot’s plane and robot’s
origin to (x,y) plane as the figure 4.
 From the figure 4, we can calculate θ2 as below.
𝑦
𝜃" = atan ; > ; 0° < 𝜃" < 165°
𝑥

Figure 4: Calculating θ2

4.2. β1 and β2 Calculation

After calculating θ2 we should calculate β1 and β2 in the figure 6. To calculate this, we already have the
homogenous matrix of end effector respect to the origin of the robot. It can be expressed as the figure
5.

Figure 5: Homogenous Matrix of end effector

respect to origin frame

Figure 6: Calculating β1 and β2

From the figure 6, we can derive the following equations,

𝑧 − 13.101 = 12.021sin𝛽! − 12.171𝑠𝑖𝑛𝛽"

H𝑥 " + 𝑦 " − 14.3 = 12.021𝑐𝑜𝑠𝛽! + 12.171𝑐𝑜𝑠𝛽"

β1 and β2 are under the below constraints.

0° < 𝛽! < 120° 𝑎𝑛𝑑 − 80° < 𝛽" < 85°

These equations are solved using python SciPy library when operating the robot.
After calculating β values we can calculate remaining joint parameters as follows,
𝛽! = 45° − 𝜃# 𝑎𝑛𝑑 𝛽" = 𝜃# + 𝜃$

4.3.Inverse Kinematics Verification

For the given coordinates (x = 25cm, y = 25cm, z = 7cm), we found the joint parameters and they are
given to the MATLAB forward kinematics calculation and got the following homogenous matrix.
The calculated translation is nearly equal to the input coordinate values.

Figure 8: MATLAB plot for Inverse Kinematics Figure 7: Robot manpulator pose for
give coordinates

5. Manipulator Jacobian

Jacobian for the robot arm can be expressed as follows,

𝐽!! 𝐽"! 𝐽#! 𝐽&! 𝐽$! 𝐽%!

⎡𝐽 𝐽"" 𝐽#" 𝐽&" 𝐽$" 𝐽%" ⎤
⎢ !" ⎥
𝐽 𝐽"# 𝐽## 𝐽&# 𝐽$# 𝐽%# ⎥
𝐽 = ⎢ !#
⎢𝐽!& 𝐽"& 𝐽#& 𝐽&& 𝐽$& 𝐽%& ⎥
⎢𝐽!$ 𝐽"$ 𝐽#$ 𝐽&$ 𝐽$$ 𝐽%$ ⎥
⎣𝐽!% 𝐽"% 𝐽#% 𝐽&% 𝐽$% 𝐽%% ⎦

In the following equations, symbols are as follows,

𝑑! = 6.1𝑐𝑚 , 𝑑" = 7.001𝑐𝑚 , 𝑎" = 1.3𝑐𝑚 , 𝑎# = 12.021𝑐𝑚 , 𝑑$ = 12.171𝑐𝑚 , 𝑎% = 13𝑐𝑚 ,

° ° °
ψ# = 45 , ψ& = −45 , ψ% = 90

J11 = d5(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)) -

a2sin((πθ2)/180) - a3sin((πθ2)/180)cos((π(θ3 - ψ3))/180) + a6cos((π(θ6 + ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4
+ ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 +
ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)

J21 = d5(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)) -

a2sin((πθ2)/180) - a3sin((πθ2)/180)cos((π(θ3 - ψ3))/180) + a6cos((π(θ6 + ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4
+ ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 +
ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180))

J31 = -cos((πθ2)/180)(d5(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a3sin((π(θ3 - ψ3))/180)

+ a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180)))

J41 = -cos((πθ2)/180)(d5(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6sin((π(θ6 +

ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180)))

J51 = cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180))(d5(cos((π(θ4 +

ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 -
ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 + ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)
+ sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180))) - (cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 -
ψ3))/180))(d5(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)) +
a6cos((π(θ6 + ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4
+ ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) +
sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180))

J61 = a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +

ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 -
ψ3))/180)))(cos((πθ2)/180)cos((πθ5)/180) + sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) - sin((πθ5)/180)(a6cos((π(θ6 + ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) -
cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) +
a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 -
ψ3))/180)))(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180)

J12 = a2cos((πθ2)/180) - d5(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 -

ψ3))/180)) + a3cos((πθ2)/180)cos((π(θ3 - ψ3))/180) + a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3
- ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) - a6sin((π(θ6 + ψ6))/180)(cos((π(θ4
+ ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180)

J22 = a2cos((πθ2)/180) - d5(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 -

ψ3))/180)) + a3cos((πθ2)/180)cos((π(θ3 - ψ3))/180) + a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3
- ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) - a6sin((π(θ6 + ψ6))/180)(cos((π(θ4
+ ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))

J32 = -sin((πθ2)/180)(d5(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a3sin((π(θ3 - ψ3))/180) +

a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180)))

J42 = -sin((πθ2)/180)(d5(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6sin((π(θ6 +

ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180)))

J52 = cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180))(d5(cos((π(θ4 +

ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180)) - a6cos((π(θ6 +
ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 -
ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4
+ ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))) - (cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 +
ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))(d5(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) +
a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180))

J62 = a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +

ψ6))/180)cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 -
ψ3))/180)))(cos((πθ5)/180)sin((πθ2)/180) - sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180))) - sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3
- ψ3))/180))(a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) - a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))

J13 = 0
J23 = 0

J33 = cos((πθ2)/180)(d5(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 -

ψ3))/180)) - a3cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3
- ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4
+ ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))) + sin((πθ2)/180)(d5(cos((π(θ4
+ ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)) - a3sin((πθ2)/180)cos((π(θ3 -
ψ3))/180) + a6cos((π(θ6 + ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)
- sin((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 -
ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)))

J43 = cos((πθ2)/180)(d5(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 -

ψ3))/180)) - a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))) + sin((πθ2)/180)(d5(cos((π(θ4 +
ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) +
sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)))

J53 = cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180))(d5(cos((π(θ4 +

ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)) + a6cos((π(θ6 +
ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) +
sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180))) - (cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 +
ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180))(d5(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 +
ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180)) - a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 -
ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180)))

J63 = a6cos((π(θ6 + ψ6))/180)(cos((πθ2)/180)sin((πθ5)/180) - cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) -

sin((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))) + a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 -
ψ3))/180) + sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)))(cos((πθ5)/180)sin((πθ2)/180) - sin((πθ5)/180)(cos((π(θ4 +
ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180))) - (cos((πθ2)/180)cos((πθ5)/180) +
sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 -
ψ3))/180)))(a6cos((π(θ6 + ψ6))/180)(cos((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) + sin((πθ2)/180)sin((πθ5)/180)) - a6sin((π(θ6 + ψ6))/180)(cos((π(θ4 +
ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180)))

J14 = 0
J24 = 0
J34 = -sin((πθ2)/180
J44 = -sin((πθ2)/180
J54 = - cos((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180
J64 = sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)cos((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)cos((πθ2)/180)sin((π(θ3 - ψ3))/180)) -
cos((πθ5)/180)sin((πθ2)/180)
J15 = 0
J25 = 0
J35 = cos((πθ2)/180)
J45 = cos((πθ2)/180)
J55 = - cos((π(θ4 + ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180) - sin((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180)
J65 = cos((πθ2)/180)cos((πθ5)/180) + sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((πθ2)/180)cos((π(θ3 - ψ3))/180) - sin((π(θ4 +
ψ4))/180)sin((πθ2)/180)sin((π(θ3 - ψ3))/180))
J16 = 1
J26 = 1
J36 = 0
J46 = 0
J56 = sin((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) - cos((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180)
J66 = -sin((πθ5)/180)(cos((π(θ4 + ψ4))/180)sin((π(θ3 - ψ3))/180) + sin((π(θ4 + ψ4))/180)cos((π(θ3 - ψ3))/180))

6. Pick and place

For the pick and place task, our robot arm is going from (2, 32, 14) to (27,10,6) position.