See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/330993732

Design and Control of 6 DOF Robotic Manipulator

Thesis · May 2018

DOI: 10.13140/RG.2.2.28474.85440

CITATIONS READS

0 3,362

4 authors, including:

Muhammad Bilal Muhammad Osama Khan

University of Engineering and Technology, Lahore Dewan Cement Limited, Karachi, Pakistan
2 PUBLICATIONS   0 CITATIONS    1 PUBLICATION   0 CITATIONS   

SEE PROFILE SEE PROFILE

All content following this page was uploaded by Muhammad Bilal on 10 February 2019.

The user has requested enhancement of the downloaded file.

 Design and Control of 6 DOF Robotic
Manipulator

Authors
Muhammad Bilal 2014-MC-103
Muhammad Osama Khan 2014-MC-181
Awais Mughal 2013-MC-111
Noman Ali 2014-MC-147

Supervisor
Engr. Abdullah Tahir
Co-Advisor
Engr. Dilruba Siddiqi
DEPARTMENT OF MECHATRONICS & CONTROL ENGINEERING
FACULTY OF MECHAINCAL, MECHATRONICS &
MANUFACTURING ENGINEERING
UNIVERSITY OF ENGINEERING AND TECHNOLOGY LAHORE
FAISALABAD CAMPUS
May 2018
 Design and Control of 6 DOF Robotic Manipulator

Authors

Muhammad Bilal 2014-MC-103

Muhammad Osama khan 2014-MC-181
Awais Mughal 2013-MC-111
Noman Ali 2014-MC-147

A thesis submitted in partial fulfillment of the requirement for the degree of

B.Sc. Mechatronics & Control Engineering

Thesis Supervisor:
Engr. Abdullah Tahir

Lecturer (Industrial Tour Coordinator)

Mechatronics and Control Engineering Department

External Examiner’s Signature: _________________________________________

Thesis Supervisor’s Signature: __________________________________________

DEPARTMENT OF MECHATRONICS & CONTROL ENGINEERING

FACULTY OF MECHAINCAL, MECHATRONICS &
MANUFACTURING ENGINEERING
UNIVERSITY OF ENGINEERING AND TECHNOLOGY LAHORE
FAISALABAD CAMPUS

May 2018
 ABSTRACT

Multi-degree of freedom robots are playing very important role in different application
of automation. They are providing much more accuracy in carrying out a typical
procedure as compared to the manual work done by human. In recent years the design,
fabrication and development of robotic arms have been active research areas in robotics
all around the world. This project describe a mechanical system, design concept and
prototype implementation of a 6 DOF robotic arm, which should perform industrial task
such as pick and place of fragile objects operation. This robot arm being controlled by
micro-controller has base, shoulder, elbow, wrist rotation and a functional gripper.
Gripper has been built as end-effector and is capable of grasping diverse objects within
own workspace of the arm possible. PID controller is implemented on each motor. The
microcontroller implement forward kinematics and position control of DC motors. The
design aims to provide fine manipulation in performing pick and place task, while still
maintaining the simplicity of design.
Keywords: Manipulator, CCS, VS, Kinematics

I|P age
 UNDERTAKING

We certify that research work titled “Design and Control of 6 DOF Robotic
Manipulator” is our own work. The work has not been presented elsewhere for
assessment. Where material has been used from other sources it has been properly
acknowledged / referred.

Signature of Students

Muhammad Bilal 2014-MC-103

Signature: ____________________

Noman Ali 2014-MC-147

Signature: ____________________

Muhammad Osama khan 2014-MC-181

Signature: ____________________

Awais Mughal 2013-MC-111

Signature: ____________________

II | P a g e
 ACKNOWLEGEMENT

All praises and thanks are to Almighty Allah, the most beneficent and merciful. We
offer our humblest thanks and praises to our Holy Prophet Hazrat Muhammad
(S.A.W) whose moral and spiritual teaching is a forever source of guidance and
knowledge for humanity as a whole enlightened. We would like to express our
gratitude and appreciation to Supervisor, Engr. Abdullah Tahir and Co-Advisor
Engr. Dilruba Siddiqi whose help, stimulating suggestion and engagement, helped us
to coordinate our project. His illustration advice and keen interest was helpful for us in
the completion of this project. His influence and support were key elements of our
project.

III | P a g e
Table of Contents
ABSTRACT .................................................................................................................. I
UNDERTAKING ........................................................................................................ II
ACKNOWLEGEMENT........................................................................................... III
Chapter 1 Introduction ............................................................................................ 1
1.1 Introduction to Robotics ............................................................................................... 1

1.2 History of Robots ........................................................................................................... 1

1.3 Objectives ....................................................................................................................... 2

1.4 Application of Robotic Manipulators .......................................................................... 3

1.5 Components of Manipulator ........................................................................................ 3

1.6 Configuration of Robots................................................................................................ 4

1.7 Characteristics of Robot ............................................................................................... 5

Chapter 2 Mechanical Design ................................................................................. 6

2.1 SolidWorks Design ........................................................................................................ 6

2.1.1 Joints Angle ........................................................................................................... 11

2.2 Motion Transmission................................................................................................... 12

2.3 Material Selection ........................................................................................................ 13

Chapter 3 Kinematics and Dynamics of Manipulator ........................................ 14

3.1 Forward Kinematics.................................................................................................... 14

3.1.1 Workspace ............................................................................................................. 16

3.2 Inverse Kinematics ...................................................................................................... 16

3.3 Velocity Jacobain ......................................................................................................... 17

3.4 Singularities ................................................................................................................. 17

Chapter 4 Hardware and Software Modules....................................................... 19

4.1 Tiva C Launchpad Controller .................................................................................... 19

4.2 Actuators ...................................................................................................................... 20

4.3 Sensors .......................................................................................................................... 22

4.4 Motor Driver ................................................................................................................ 23

4.5 MATLAB ..................................................................................................................... 23

 4.6 Visual Studio ................................................................................................................ 24

4.7 Code Composer Studio................................................................................................ 25

4.8 Circuit Diagram ........................................................................................................... 26

4.1.8 Schematics Diagram ............................................................................................. 26

Chapter 5 Simulation and Hardware Implementation....................................... 28
5.1 DC Motor Transfer Function ..................................................................................... 28

5.1.1 PID Tuning using MATLAB ............................................................................... 30

5.2.1 PID Controller Implementation .......................................................................... 31
5.2 Simulation .................................................................................................................... 39

5.1.2 Forward Kinematics Simulation Results ............................................................ 39

5.3 Forward Kinematics Implementation ....................................................................... 42

Project Cost ................................................................................................................ 45

Conclusion .................................................................................................................. 46
Appendix ..................................................................................................................... 47
List of Figures

Figure 1: Free Body Diagram ........................................................................................ 6

Figure 2: Complete CAD Model.................................................................................... 7
Figure 3: Base CAD Model ........................................................................................... 8
Figure 4: Bottom Drawing ............................................................................................. 8
Figure 5: Base Side Part Drawing .................................................................................. 8
Figure 6: Top Plate of base ............................................................................................ 8
Figure 7: Shoulder Motor Pulley mechanism ................................................................ 9
Figure 8: Shoulder Side Part Drawing ........................................................................... 9
Figure 9: Elbow CAD Model ......................................................................................... 9
Figure 10: Elbow Part Drawing ................................................................................... 10
Figure 11: Spherical Wrist CAD Model ...................................................................... 10
Figure 12: Wrist Part Drawing ..................................................................................... 10
Figure 13: End-Effector ............................................................................................... 11
Figure 14: Actual Picture of Manipulator .................................................................... 11
Figure 15: G2T and G3T Belt-Pulley mounted on Motor ........................................... 12
Figure 16: Free Body Diagram .................................................................................... 15
Figure 17: Workspace Front View (Units inch) .......................................................... 16
Figure 18: Workspace X-Y view (Unit inch) .............................................................. 16
Figure 19: Singular Configuration ............................................................................... 18
Figure 20: Tiva C Launchpad ...................................................................................... 19
Figure 21: Free Body diagram ..................................................................................... 20
Figure 22: Quadrature Encoder .................................................................................... 22
Figure 23: Limit Switch ............................................................................................... 23
Figure 24: Force Sensor ............................................................................................... 23
Figure 25: L298 Motor Driver ..................................................................................... 23
Figure 26: GUI ............................................................................................................. 24
Figure 27: Forward Kinematics Window .................................................................... 24
Figure 28: Inverse Kinematics Window ...................................................................... 25
Figure 29: CCS Environment....................................................................................... 25
Figure 30: Circuit Diagram .......................................................................................... 26
Figure 31: Schematic Diagram .................................................................................... 26
Figure 32: Controller Connections............................................................................... 27
Figure 33: General Block Diagram .............................................................................. 28
Figure 34: System Identification Toolbox ................................................................... 29
Figure 35: Step response .............................................................................................. 29
Figure 36: PID Tuner ................................................................................................... 30
Figure 37: Bode Plot .................................................................................................... 31
Figure 38: Step Response............................................................................................. 31
Figure 39: Root Locus of Motor .................................................................................. 31
Figure 40: Step Response of Base Joint Motor............................................................ 33
Figure 41: Bode Plot of Base Joint Motor ................................................................... 33
Figure 42: Root Locus of Base Joint Motor................................................................. 33
Figure 43: Bode Plot of Shoulder Joint Motor ............................................................ 34
Figure 44: Step Response of Shoulder Joint Motor ..................................................... 34
Figure 45: Root Locus of Shoulder Joint Motor .......................................................... 35
Figure 46: Step Response of Elbow Joint Motor ......................................................... 36
Figure 47: Bode Plot of Elbow Joint Motor ................................................................ 36
Figure 48: Root Locus of Elbow Joint Motor .............................................................. 36
Figure 49: Step Response Wrist 2nd Joint Motor ........................................................ 37
Figure 50: Bode Plot Wrist 2nd Joint Motor ............................................................... 37
Figure 51: Root Locus Wrist 2nd Joint Motor ............................................................. 38
Figure 52: Step Response Wrist 3rd Joint Motor......................................................... 39
Figure 53: Bode Plot Wrist 3rd Joint Motor ................................................................ 39
Figure 54: Root Locus of Wrist 3rd Joint Motor ......................................................... 39
Figure 55: Home Position ............................................................................................ 40
Figure 56: Base Joint Rotation ..................................................................................... 40
Figure 57: Shoulder Joint Rotation .............................................................................. 41
Figure 58: Elbow Joint Rotation .................................................................................. 41
Figure 59: Home Position ............................................................................................ 42
Figure 60: Shoulder Joint Movement .......................................................................... 42
Figure 61: Elbow Joint Movement .............................................................................. 43
Figure 62: wrist 1st joint Movement ............................................................................ 43
Figure 63: Wrist 2nd Joint Movement ......................................................................... 44
Figure 64: End-effector Movement ............................................................................. 44
CHAPTER 1

INTRODUCTION

1.1 Introduction to Robotics

Robotics is an interdisciplinary branch of engineering that comprises the formation,
design, building, and action of robots. This field overlays with mechanical-,
mechatronics-, electrical engineering, computer science, electronics, AI, Nano-
technology and bio-engineering. In addition, Robotics deal with the computer systems
for their control, sensory response, and information processing.
“A Robot is a reprogrammable multifunctional manipulator designed to move
materials, parts, tools, or specialized devices through variable programmed motions
for the performance of a variety of tasks” [Robot Institute of America, RIA]
OR
“A goal oriented machine that can sense, plan and act” [Peter Corke]
These different fields knowledge are utilize to build machines that can work like
human. Robots operate in different environment and can be used for various purpose
where human cannot perform any task.
Robots can be of any kind/shape but some are built to correspond humans in look. Such
robots plan to replicate walking, lifting, speech, cognition, and primarily something an
individual's will do. Several of today's robots unit stimulated naturally, contributive to
the field of bio-inspired artificial intelligence.
Well it's a system that contains feedback sensors, control systems, manipulators, power
source and software all operating collectively to perform a task. Designing, building,
programming and testing a robots could be a combination mechanical-, mechatronics-,
electrical engineering, computer science, electronics, AI, Nano-technology and bio-
engineering. In some cases biology, medicine, chemistry may additionally be
concerned.

1.2 History of Robots

Robot word derived from “Robota” a Czech word meaning ‘work’. Sir Isaac Asimov
is frequently specified the first person to usage the term robotics in his story in 1940s.
In story, He recommended 03 laws to monitor the performance of robots. Sir Isaac

1|P age
Asimov proposed three philosophical laws for robotics which continued to the present
given as
 Robot First Law
A Robot must not harm a human being or, through inaction, allow one to come
to harm.
 Robot Second Law
A Robot must always obey human beings unless it is in conflict with higher
order law.
 Robot Third Law
A Robot must protect itself from harm unless that is in conflict with a higher
order law.
 Zeroth Law
A Robot must not injure humanity or, through action, allow humanity to come
to harm.
Following are the list of Robots
 In 1954 First programmable robot is presented by George Devol
 In 1971 Stanford University presented Stanford Arm
 In 1974 Cincinnati Milacron presents the T3 robot with computer control
 In 1978 Unimation presents the PUMA robot
 In 1979 Japan presents SCARA Robot
 In 1996 Honda expose its Humanoid robot

1.3 Objectives
 To design and control 6 DOF Robotic manipulator (Articulated + spherical
wrist) which grasp fragile object to move it to desired location as well as
regulating internal forces of object. Using force sensor (tactile sensor which is
based on resistive principle) allows actuator to maintain constant gripping force
on object to avoid damaging of object.
 To apply PID controller for position control of actuators which allows
manipulator to precisely place objects at desired location.
 To develop graphical user interface (GUI) working as HMI taking commands
from user and transferring to manipulator.

2|P age
1.4 Application of Robotic Manipulators
Among many other applications, some applications are:
 They can be used for Military purpose.
 They can perform group task.
 They are widely used in industry even number of robots as compared to labor
increasing day by day. World top industries are fully automated like IBM etc.
 They are used in hospital to monitor different action. Robots can easily perform
stitching and various surgical operations.
 They can b serve as a cooker and play role of a waiter in restaurant.
 They can play sports with human.
 They can be used for agricultural purpose.
 Nano-robots are perform operation at Nano level like in medical to remove
blood blockage in vessel
 Swarm robotics

1.5 Components of Manipulator

There are following main components of manipulator
 End Effector
End-effector is a robot device or tool that connect with the robot arm. End-
effector is part of manipulator which interact with the environment. It may be
of different types
 Most common type of end effector is gripper.
 Force/Torque Sensors end effector
 Any type of material Abstraction Tools.
 Welding Torches.
 Actuators
Any device which perform any action called actuators. In robotics, actuators
deals motor which cause movement of the manipulator. Different types of
actuators is used for said purpose, for example most commonly actuators used
are DC, Stepper and DC Servo motors.
 Sensors
Any device which convert electrical signal into physical signal that can be
measurable called sensor. Depend on requirement multiple sensor are available

3|P age
 to use. Feedback sensor are used for locating manipulator current position.
Sensors attached with end effector are used for interaction with environment.
 Controller
Controller are the main unit of any robot or controlled machine. Multiple
controller are can be used for said purpose PIC micro-controller, Raspberry Pi,
Tiva C Launchpad, Arduino etc.
 Software
Software are also the main tool of manipulator. Multiple Software are available
to controlled manipulator. For example RACS - Robot Arm Control Software,
ArmCtrl, Arm Link etc.

1.6 Configuration of Robots

There are five basic configuration of robots which are:
 Cartesian Configuration (PPP)
A Cartesian robot, also known as a Cartesian coordinate robot, is a common
type of industrial robot. It has three "arms" that each function along linear axes
of control. Each of these axes is at a right angle to the other two.
 Cylindrical Configuration (RPP)
Cylindrical robot consist of one revolute and two prismatic joint. Mainly used
for cylindrical operation.
 Spherical Configuration (RRP)
Spherical arm presented by Stanford University and consist of two revolute and
one prismatic joint. All three joints are mutually perpendicular to each other.
 Articulated/Anthropomorphic Configuration (RRR)
The articulated or jointed arm robot (or sometime called Anthropomorphic
arms) thoroughly look like human arm. It consist of three revolute joint.
 SCARA Configuration (RRP)
SCARA (stands for selective complaint articulated robotic assembly) mainly
consist of two revolute and one prismatic joint which are parallel to each other.

4|P age
1.7 Characteristics of Robot
Following table characteristics of robot
Workspace Workspace define total volume covered by manipulator
Payload How much weight manipulator can pick is represent by
payload.
Controller Resolution The smallest increment that controller can sense

How many times manipulator achieve same position is

Precision
represented by precision.
Accuracy deal how much manipulator close to exact
Accuracy
position
Repeatability deal how much manipulator reach to same
Repeatability
position after repeating process.

5|P age
CHAPTER 2

MECHANICAL DESIGN

Mechanical Design can be introduce as the procedure through which resources or

energy is transformed into suitable mechanical forms, or the mechanism which provide
useful actions as per the requirements of users/human beings. Mechanical design
provide establishment of the completely new machine or it can lead to up-grade or
enhancement of the current machines.
It’s desired to design 6 DOF robotic manipulator which carry fragile object from one
place to desired place. Articulated + spherical manipulator is selected for said purpose
because it’s mostly used in industry. Using CAD software’s for analyze forces/torque
that how much torque/forces is required at each joint to move manipulator and to find
stresses that how much each element can bear without deflection or damaging.

2.1 SolidWorks Design

Robotic Manipulators mainly consist of an assembly of joints and links. Joints can be
defined as the connection between two links while links can be defined as the rigid
segments that construct the mechanism. End-effector is a device which interact with the
environment. Free body diagram as shown in figure 1.

Z-axis

Z-axis
Z-axis

Z-axis

Z-axis
Z-axis

X-axis
Figure 1: Free Body Diagram

6|P age
 Multiple CAD software are available for designing, solid works is preferred for
designing manipulator. Solid works allow a numerous features to design and calculate
important parameters.
 Six DOF Manipulator CAD Model

𝜃4,5,6

𝜃3

End-
Effector
Elbow

𝜃2

Spherical
Wrist
Shoulder

𝜃1

Base

Figure 2: Complete CAD Model

 Base CAD Model

Figure 1 represent base model of our project. Belt-pulley is used for rotation
transmission. Because to increase motor torque as well as direct coupling of motor is
not favorable.

7|P age
 Figure 3: Base CAD Model

Figure 4: Bottom Drawing

Figure 5: Base Side Part Drawing

Figure 6: Top Plate of base

8|P age
  Shoulder CAD Model

Shaft
Solid
Shaft Collar

Spacer

DC Gear Pulley
Motor

Figure 7: Shoulder Motor Pulley mechanism

Figure 8: Shoulder Side Part Drawing

 Elbow CAD Model

Spacer G2T 25mm

G2T 12mm
DC Gear
Motor

Shaft

Figure 9: Elbow CAD Model

9|P age
 Figure 10: Elbow Part Drawing

 Spherical Wrist CAD Model

Wrist
Joint 5

Wrist
Joint 4
Wrist
Joint 6

Figure 11: Spherical Wrist CAD Model

Figure 12: Wrist Part Drawing

10 | P a g e
  End-effector CAD Model

Gripper

Figure 13: End-Effector

 Actual Picture of Robotic Manipulator

Figure 14: Actual Picture of Manipulator

2.1.1 Joints Angle

The angle of joint 𝑖 (𝑖 = 1, 2..., 6), 𝜃𝑖 , is measured about the z axis associated with the
given joint using the right-hand rule. Note that the direction of rotation for each joint is
indicated on the robot’s body. The mechanical limits of the robot’s joints are as follows:
−𝟒𝟓 < 𝜽𝟏 < +𝟒𝟓 𝟎 < 𝜽𝟐 < +𝟖𝟎
𝟎 < 𝜽𝟑 < +𝟔𝟎 −90 < 𝜃4 < +90
−𝟗𝟎 < 𝜽𝟓 < +𝟗𝟎 −90 < 𝜃6 < +90

11 | P a g e
2.2 Motion Transmission
Timing belt-pulley is used for motion transmission. Also for increasing motor torque
it’s favorable. Gt2 (2mm Pitch) belt is used for actuators mounted on elbow, base and
wrist because load effect is low as compared to shoulder. Gt3 belt (3mm Pitch) is used
for shoulder transmission due to load capacity on shoulder motor
is greater than remaining actuators.
G3T: Ratio is 1: 3.57
G2T: Ratio is 1:2.08

25mm
G2T

12mm
G2T

43mm 12mm
G3T G3T

12mm 25mm
G2T G2T

Figure 15: G2T and G3T Belt-Pulley mounted on Motor

12 | P a g e
2.3 Material Selection
In the process of designing any equipment or any physical object, material selection is
an important consideration. During product designing, main goal of selection is on the
basis of minimal cost while achieving desired strength goal. Various materials are
available but aluminum is preferable for manipulator because it have following useful
specification
 Density
As compared to steel or copper, aluminum have a density around 1/3.
Aluminum have high strength to weight ratio make it more suitable for
structural material.
 Strength
Pure aluminum doesn’t have sufficient strength but combination of elements
with aluminum makes it a high strength alloy. As compared to steel, at low
temperature steel become brittle while aluminum is well suited for such
environment.
 Corrosion Resistance
Aluminum show inert behavior when exposed to air due to layer of oxide.
 Thermal Conductivity
As compared to steel, thermal conductivity of aluminum is about three times
greater than of steel.

13 | P a g e
CHAPTER 3

KINEMATICS AND DYNAMICS OF

MANIPULATOR

Kinematics is the branch of science that examines the movement of manipulator links
without regard to the forces that cause it. In that case the motion is determined with
trajectory, i.e. position, velocity, acceleration, jerk and additional higher derivative
terms.
Dynamics deals the relation between the applied forces/torques and the resulting motion
of an industrial manipulator.

3.1 Forward Kinematics

Forward kinematics deal with the problem of finding end-effector pose (position +
orientation) with given joints variables. There are two method used for solving forward
kinematics.
 Homogenous transformation
 Denavit–Hartenberg Representation
Most commonly method used for solving forward kinematics is DH transformation
which based on four parameter
α alpha is a twist angle between current z-axis and previous z-axis
Amount of rotation around the current z-axis needed to align the previous
θ
x-axis with the current x-axis.
d distance along z-axis
a link length which measure between current x-axis and previous x-axis

Using convention, each homogeneous transformation Ai is represented as a product of

four basic transformations matrices
Ai = Rot z, θi * Trans z,di * Trans x,ai * Rotx,αi

14 | P a g e
 𝑐𝜃𝑖 −𝑠𝜃𝑖 0 0 1 0 0 0 1 0 0 𝑎𝑖 1 0 0 0
𝑠𝜃𝑖 𝑐𝜃𝑖 0 0 0 1 0 0 0 1 0 0 0 𝑐𝛼𝑖 −𝑠𝛼𝑖 0
Ai=[ ][ ][ ][ ]
0 0 1 0 0 0 1 𝑑𝑖 0 0 1 0 0 𝑠𝛼𝑖 𝑐𝛼𝑖 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1

𝑐𝜃𝑖 −𝑠𝜃𝑖𝑐𝛼𝑖 𝑠𝛼𝑖𝑠𝜃 𝑎𝑖𝑐𝜃𝑖

𝑠𝜃𝑖 𝑐𝜃𝑖𝑐𝛼𝑖 −𝑐𝜃𝑖𝑠𝛼𝑖 𝑎𝑖𝑠𝜃𝑖
Ai=[ ] general form of DH matrices
0 𝑠𝛼𝑖 𝑐𝛼𝑖 𝑑𝑖
0 0 0 1

Figure 16: Free Body Diagram

 D-H Table
Table 3-1: DH Parameter Table

Link Alpha (α) Link Joint offset Theta(θ) Offset

Number length(a) (d)
1 pi/2 0 10 * 0
2 0 6 0 * Pi/1.09
3 -pi/2 0 0 * -7pi/6
4 pi/2 0 6 * 0
5 -pi/2 0 0 * -pi/1.6
6 0 0 6 * 0

Orientation Matrix  r11 r12 r13 r14 

r Translation vector
r22 r23 r24 
T   21
 r31 r32 r33 r34 
 
r41 r42 r43 r44 

Transformation matrix are represented in appendix A

15 | P a g e
3.1.1 Workspace

Figure 17: Workspace Front View (Units inch)

Figure 18: Workspace X-Y view (Unit inch)

3.2 Inverse Kinematics

Inverse kinematics deals the problem of finding joints variable given end-effector pose.
For six DOF we decouple manipulator into two parts
 Inverse Position
 Inverse Orientation
Inverse position kinematics deals with the finding 1st three joints while rest of three
(spherical wrist joint) deal by inverse orientation. Inverse position kinematics and
orientation are mathematically solved in Appendix D

16 | P a g e
3.3 Velocity Jacobain
Jacobain matrix help in find out relationship between linear and angular velocity of end
effector to joints velocity. From forward kinematics equations, partial derivative of x
and y with respect to q1 and q2 yield matrix called Jacobain matrix. The jacobain is a
matrix that is a function of joint position that linearly relate joint velocity to tool point
velocity. Jacobian matrix is represented by notation J
𝑞1
𝑣 𝑞2
[ ] = 𝐽(𝑞1 , 𝑞2 , … … . , 𝑞𝑛 ) [ ⋮ ]
𝜔
𝑞𝑛
𝑣 = 𝑙𝑖𝑛𝑒𝑎𝑟 𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦
𝜔 = 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦
Prismatic joint contribute only linear velocity to end-effector while revolute joint
contribute both linear as well as angular velocity to end-effector. Velocity jacobian
calculation are represent in Appendix F

3.4 Singularities
Singular configurations of a manipulator can be determined by using jacobain matrix.
A configuration in which manipulator lose one or more degree of freedom is said to be
singular configuration. There are many solutions to avoid singularity, for example to
design manipulator in such a way that end-effector cannot pass through these points
which leads to singular configuration. It can also be compensated by defining specific
variable through programming.
Singularity of 6 DOF can by find out by decoupling it into two portion, first three joints
and spherical wrist joints.

J J12   J11 0
J  J A J w    11 
 J 21 J 22   J 21 J 22 

17 | P a g e
In case of spherical wrist, due to revolute joint, linear velocity Jv (J11) term is zero. So
arm singularity can be calculated using relation
det(𝐽11 ) = 0
While for wrist singularity use relation
det(𝐽22 ) = 0
For derivation, see Appendix G
Singular configuration is avoided by defining specific
angle. For first three joints, we define zero angle and
as same defined for spherical wrist.

Figure 19: Singular Configuration

18 | P a g e
CHAPTER 4

HARDWARE AND SOFTWARE MODULES

This chapter deal with the discussion of important hardware and software modules used
in designing and controlling of our manipulator. Main hardware components used in
project are
 Controller
 Actuators
 Sensors
It's a software program that permits hardware to interconnect and control with the
computer software. Software used in this project are
 Code Composer Studio
 MATLAB
 Visual Studio

4.1 Tiva C Launchpad Controller

Multiple controllers are available in market for said purpose. Tiva-C tm4c123gxl
Launch Pads is preferable which are inexpensive self-contained, single-board
microcontrollers, featuring an ARM Cortex-M4F 32-bit CPU operating at 80 to 120
MHz, manufactured by Texas Instruments. Stated micro-controller contain two encoder
modules and 16 PWM pins. There are many editor available like Kiel, eclipse, code
composer studio and many more. We choose code composer studio for programming.

Figure 20: Tiva C Launchpad

19 | P a g e
4.2 Actuators
The projected manipulator uses electromechanical actuators which are suitable for
manipulator such as surface finishing, force control applications and application where
accuracy is necessary. Six permanent magnet dc motor 12V with incremental encoder
is used in project. Three actuators are used for wrist while rest of three for base, shoulder
and elbow. Following are the specification of mounted DC gear motor.

Figure 21: Free Body diagram

Table 4-1: Motor Specifications

Motors Gear Ratio Stall Torque Stall Current RPM Encoder

(Kg.cm) (A) Resolution
(CPR)
Base 43.8:1 12 7 251 32
Shoulder 270:1 80 6 40 32
Elbow 168:1 58 5 80 32
Wrist 1 131:1 18 5 80 32
Wrist 2 100:1 16 5 100 32
Wrist 3 43.8:1 12 7 251 32

Now calculating a factor of each motor to track exact rotation/angle at each end points.
Factor is necessary because of using different ratio pulleys for transmission.
 Base Motor
Motor Gear ratio = 43.8:1
Pulley ratio = 2.08:1
To find out gear ratio between encoder and shaft, means one revolution of shaft equal
to how many revolution of encoder. We perform calculation as
43.8 × 2.08 = 91.104

20 | P a g e
Now we multiply 272.48 with encoder resolution factor which is 32 to calculate total
bits
91.104 × 32 = 2915.328 𝑏𝑖𝑡𝑠
360 𝑑𝑒𝑔𝑟𝑒𝑒 = 2915.328 𝑏𝑖𝑡𝑠
360
1 𝑏𝑖𝑡 = 2915.328 = 𝟎. 𝟏𝟐𝟑𝟒

This factor will multiply with current encoder position to get exact position/angle of
shaft.

 Shoulder Motor
Motor Gear ratio = 270:1
Pulley ratio = 3.57:1
𝑒𝑛𝑐𝑜𝑑𝑒𝑟 𝑎𝑛𝑑 𝑠ℎ𝑎𝑓𝑡 𝑔𝑒𝑎𝑟 𝑟𝑎𝑡𝑖𝑜 = 270 × 3.57 = 963.9
963.9 × 32 = 30844.8 𝑏𝑖𝑡𝑠
360 𝑑𝑒𝑔𝑟𝑒𝑒 = 30844.8 𝑏𝑖𝑡𝑠
360
1 𝑏𝑖𝑡 = 30844.8 = 𝟎. 𝟎𝟏𝟏𝟔𝟕

This factor will multiply with current encoder position to get exact position/angle of
shaft.
 Elbow Motor
Motor Gear ratio = 168:1
Pulley ratio = 2.08:1
𝑒𝑛𝑐𝑜𝑑𝑒𝑟 𝑎𝑛𝑑 𝑠ℎ𝑎𝑓𝑡 𝑔𝑒𝑎𝑟 𝑟𝑎𝑡𝑖𝑜 = 168 × 2.08 = 349.8
349.8 × 32 = 11182.8 𝑏𝑖𝑡𝑠
360 𝑑𝑒𝑔𝑟𝑒𝑒 = 11182.8 𝑏𝑖𝑡𝑠
360
1 𝑏𝑖𝑡 = 11182.8 = 𝟎. 𝟎𝟑𝟐𝟒

This factor will multiply with current encoder position to get exact position/angle of
shaft.
 First Wrist Motor
Motor Gear ratio = 131:1
131 × 32 = 4192 𝑏𝑖𝑡𝑠
360 𝑑𝑒𝑔𝑟𝑒𝑒 = 4192 𝑏𝑖𝑡𝑠
360
1 𝑏𝑖𝑡 = 4192 = 𝟎. 𝟎𝟖𝟓𝟖𝟕

21 | P a g e
This factor will multiply with current encoder position to get exact position/angle of
shaft.
 Second Wrist Motor
Motor Gear ratio = 100:1
Pulley ratio = 2.08:1
𝑒𝑛𝑐𝑜𝑑𝑒𝑟 𝑎𝑛𝑑 𝑠ℎ𝑎𝑓𝑡 𝑔𝑒𝑎𝑟 𝑟𝑎𝑡𝑖𝑜 = 100 × 2.08 = 208
208 × 32 = 6656 𝑏𝑖𝑡𝑠
360 𝑑𝑒𝑔𝑟𝑒𝑒 = 6656 𝑏𝑖𝑡𝑠
360
1 𝑏𝑖𝑡 = 6656 = 𝟎. 𝟎𝟓𝟒𝟎

This factor will multiply with current encoder position to get exact position/angle of
shaft.
 Third Wrist Motor
Motor Gear ratio = 43.8:1
43.8 × 32 = 1401.6 𝑏𝑖𝑡𝑠
360 𝑑𝑒𝑔𝑟𝑒𝑒 = 1401.6 𝑏𝑖𝑡𝑠
360
1 𝑏𝑖𝑡 = 1401.6 = 𝟎. 𝟐𝟓𝟔𝟖

This factor will multiply with current encoder position to get exact position/angle of
shaft

4.3 Sensors
DC gear motor is used with built in incremental encoder which generate pulse for each
incremental step of motor rotation. Two channel or quadrature type encoder is used
which detect direction of rotation as well. These encoder serve as a feedback for
position information. Resolution of each encoder is 32 CPR.

Figure 22: Quadrature Encoder

22 | P a g e
Limit switches is used for protecting mechanical damaging, stop manipulator at home
position as power ON and protect the manipulator to move outside from its workspace.
Force sensor is used for detecting applied gripping force to pick fragile object and move
to desired location.

Figure 24: Force Sensor Figure 23: Limit Switch

4.4 Motor Driver

Motor driver is used for controlling motor direction as well as speed. L298 driver is
widely used for controlling DC motor. See appendix for datasheet of L298 driver.
 Operating supply voltage up to 40 V
 Dc current up to 4 A
 Over temperature protection
 High noise immunity

Figure 25: L298 Motor Driver

4.5 MATLAB
MATLAB has a various advantages in field of engineering. It provide multiple toolbox
to simulate or implementation various problems. For stated purpose we used following
toolbox
 PID Tuner
 System identification Toolbox
 SISO Toolbox

23 | P a g e
PID tuner is used for calculating PID parameters values, system identification toolbox
is used for finding unknown transfer function of DC gear motor and SISO toolbox is
also used for plot bode, step response of our system.

4.6 Visual Studio

Basic purpose of using visual studio IDE is for window application and it is best suitable
for C++ coding environment. Here VS is used for graphical user interface (GUI) to
communicate data with controller through serial communication. Following figure
represent GUI

Serial COM
Rate

Figure 26: GUI

Figure 27: Forward Kinematics Window

24 | P a g e
 Figure 28: Inverse Kinematics Window

4.7 Code Composer Studio

Code composer studio is best IDE for Texas instruments. Like visual studio it provide
multiple features which makes programs easily understandable. Most useful feature of
CCS is to provide Debug mode thorough which we can easily observe variables
variation. PID parameter can be easily tune. Following graph shown main environment
of CCS

Figure 29: CCS Environment

25 | P a g e
4.8 Circuit Diagram

Figure 30: Circuit Diagram

4.1.8 Schematics Diagram

Figure 31: Schematic Diagram

26 | P a g e
Connection Between Micro-controllers for communication are given as

Figure 32: Controller Connections

27 | P a g e
CHAPTER 5

SIMULATION AND HARDWARE IMPLEMENTATION

This chapter deal with the interfacing hardware modules explained in chapter 5 with
software modules explained in chapter 6. PID controller is used for position control of
dc motor. DC motor transfer function is identified using MATLAB system
identification toolbox and then find out PID parameters using PID Tuner in MATLAB.
General Block Diagram

Figure 33: General Block Diagram

Controller maybe of any type for example PID, FOPID, Lead/lag compensator, digital
controller. But PID controller is preferable in project because its simplicity, simple
algorithm.
Here plant show DC motor which is represented by transfer function. DC motor transfer
function can be find out using different technique. Next section explain method that
how it can find unknown motor transfer function.

5.1 DC Motor Transfer Function

Simple experiment is performed by applying various input voltage and check output
rpm using tachometer (A device used for measuring rpm). By finding input/output
relation provide 100 sample data.

System identification toolbox require two type of data, estimation data and validation
data. Prior data used for finding transfer function while validation data used for
verification. MATLAB Code are shown in Appendix B

28 | P a g e
 System Identification of Motor Using MATLAB Toolbox

Validation
Data Transfer
Function
Model

Estimation
Data
Validation
Data

Figure 34: System Identification Toolbox

First Wrist Motor Transfer Function

By right click on transfer function model as shown in figure 29, we get TF as

144s  72.35
Transfer _ Function 
s  16.04s  8.052
2

Step Response

Step response of transfer function is shown in figure 30

Figure 35: Step response

29 | P a g e
 5.1.1 PID Tuning using MATLAB
By implementing PID Controller, following results obtain as shown in figure 22.
Step involved in implementing PID are as follow
 Import plant transfer function
 Select PID type
 Adjust response time
 Adjust transient response
Controller
Type

Input PID
Plant Parameter

Figure 36: PID Tuner

LHS plot show step response of controlled system while RHS show bode plot of
controlled system. Comparing step response of a system using PID and without using
PID. The difference can be observed by comparing figure 30 and 31.

8.409  104 s 2 + 2.599  106 s + 1.282  106

( Plant * PID _ controller) _ transfer _ Function = 4
s + 3496 s 3 + 5.583  104 s 2 + 2.802  104 s

8.409  106 s 2 + 2.599  106 s + 1.282  106

Colsedloop_ transfer _ function 
s 4 + 3496 s3 + 1.399  105 s 2 + 2.627  106 s + 1.282  106

30 | P a g e
Closed loop TF Step Response and Bode Plot

Figure 38: Step Response Figure 37: Bode Plot

Figure 39: Root Locus of Motor

5.2.1 PID Controller Implementation

PID controller is used to implement position control of DC motor. The main algorithm
of PID are represented in appendix C. Following table show PID parameters values of
six motors.

31 | P a g e
Table 5-1: PID Parameters Values

Motor Kp Kd Ki
Base Motor 100 400 0.00001
Shoulder Motor 120 550 0.00006
Elbow Motor 78 289 0.0009
Wrist 1 Motor 144 362 0.008
Wrist 2 Motor 206 199 0.0007
Wrist 3 Motor 165 183 0.0054

As graph show step response and bode plot for wrist 1st motor, similarly now plot step
response, bode and root locus of each motor as shown below. MATLAB code are
shown in Appendix D
 Base Motor
100s  45.65
Transfer _ Function 
12s  22.25s  65.091
2

From Table knowing kp, ki, kd values. Step response, bode plot and root locus are
shown below

32 | P a g e
Figure 41: Bode Plot of Base Joint Motor Figure 40: Step Response of Base Joint Motor

Figure 42: Root Locus of Base Joint Motor

33 | P a g e
  Shoulder Motor
75s  58.65
Transfer _ Function 
s  14.29s  78.091
2

From Table knowing kp, ki, kd values. Step response, bode plot and root locus are
shown below

Figure 44: Step Response of Shoulder Joint Motor Figure 43: Bode Plot of Shoulder Joint Motor

34 | P a g e
 Figure 45: Root Locus of Shoulder Joint Motor

 Elbow Motor
13s  12.65
Transfer _ Function 
10.25s 2  12.25s  8.091

From Table knowing kp, ki, kd values. Step response, bode plot and root locus are
shown below

35 | P a g e
Figure 47: Bode Plot of Elbow Joint Motor Figure 46: Step Response of Elbow Joint Motor

Figure 48: Root Locus of Elbow Joint Motor

36 | P a g e
  Wrist 2nd Motor

53s  16.65
Transfer _ Function 
4.64s  25.256s  351
2

From Table knowing kp, ki, kd values. Step response, bode plot and root locus are
shown below

Figure 50: Bode Plot Wrist 2nd Joint Motor Figure 49: Step Response Wrist 2nd Joint
Motor

37 | P a g e
 Figure 51: Root Locus Wrist 2nd Joint Motor

 Wrist 3rd Motor

100s  45.65
Transfer _ Function 
12s  22.25s  65.091
2

From Table knowing kp, ki, kd values. Step response, bode plot and root locus are
shown below

38 | P a g e
 Figure 53: Bode Plot Wrist 3rd Joint Motor Figure 52: Step Response Wrist 3rd Joint
Motor

Figure 54: Root Locus of Wrist 3rd Joint Motor

5.2 Simulation
MATLAB software is used for simulation purpose, MATLAB code are shown in
Appendix E

5.1.2 Forward Kinematics Simulation Results

Home Position
Final DH matrix of manipulator can be calculated using RVC MATLAB toolbox as

39 | P a g e
 −.6410 0 0.7675 3.038
0 1 0 0
𝑇=[ ]
−0.7675 0 −.6410 11.95
0 0 0 1

Figure 55: Home Position

Now simulating single joint step by step and find out final DH matrix in each case.

 Base Joint Simulation

Final DH matrix for base joint rotation from 0 to 90 or pi/2 degree.
0 −1 0 0
−0.6410 0 0.7675 3.038
𝑇=[ ]
−0.7675 0 −.6410 11.95
0 0 0 1

Figure 56: Base Joint Rotation

40 | P a g e
  Shoulder Joint Simulation
Final DH matrix for Shoulder joint rotation from 0 to –(60 or pi/3) degree.
−0.9852 0 −0.1714 3.204
0 1 0 0
𝑇=[ ]
0.1714 0 −0.9852 8.342
0 0 0 1

Figure 57: Shoulder Joint Rotation

 Elbow Joint Simulation

Final DH matrix for elbow joint rotation from 0 to –(60 or pi/3) degree.
0.3442 0 0.9389 −1.733
0 1 0 0
𝑇=[ ]
−0.9389 0 0.3442 19.4
0 0 0 1

Figure 58: Elbow Joint Rotation

41 | P a g e
5.3 Forward Kinematics Implementation
Home position of manipulator can be shown in following picture

Figure 59: Home Position

 Shoulder Joint 60 Degree Rotation

Figure 60: Shoulder Joint Movement

42 | P a g e
 Elbow Joint 45 Degree Rotation

Figure 61: Elbow Joint Movement

 First Wrist Joint 45 Degree Rotation

Figure 62: wrist 1st joint Movement

43 | P a g e
 Second Wrist Joint 45 Degree Rotation:

Figure 63: Wrist 2nd Joint Movement

 Third Wrist Joint 45 Degree Rotation

Figure 64: End-effector Movement

44 | P a g e
 PROJECT COST

S. No Item Name No. of Units Per Unit Cost Total

1 Actuators (DC 6 4500 27000
encoded Motor)
2 Micro-controller 3 2400 7200
3 Motor driver 4 300 1200
4 Gripper 1 2500 2500
5 Mechanical Structure - 40000 40000
6 Miscellaneous - 10000 10000
7 Total 87900 PKR

45 | P a g e
 CONCLUSION

Achievements
The goal of our project is to improve end-effector control for an industrial robotic
manipulator operating with flexible tools. In this project we wanted to achieve
maximum goals but due to some limitations, we are restricted. So we have achieved
following task:
 Six DOF Robotic Manipulator as proposed in synopsis
 Forward and inverse kinematics included in the robotic manipulator
 Position control of encoded DC gear motor using PID controller
 Force sensor is used for pick and place of fragile objects
 Graphical user interface (GUI)
Future Extensions
 Velocity Control of actuators
 Trajectory and path planning
 Impedance Control
 Parallel working of actuators

46 | P a g e
 APPENDIX

Appendix A
Forward Kinematics
𝒄𝒐𝒔(𝒕𝟏) 𝟎 𝒔𝒊𝒏(𝒕𝟏) 𝟎
𝒔𝒊𝒏(𝒕𝟏) 𝟎 −𝒄𝒐𝒔(𝒕𝟏) 𝟎
𝑻𝟏𝟎 = [ ]
𝟎 𝟏 𝟎 𝟏𝟎
𝟎 𝟎 𝟎 𝟏
𝑻𝟐𝟎
𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) − 𝐬𝐢𝐧(𝐭𝟐 + 𝐰) ∗ 𝐜𝐨𝐬(𝐭𝟏) 𝒔𝒊𝒏(𝒕𝟏) 𝐛 ∗ 𝐜𝐨𝐬(𝐭𝟐 + 𝐰) ∗ 𝐜𝐨𝐬(𝐭𝟏)
𝐜𝐨𝐬(𝐭𝟐 + 𝐰) ∗ 𝐬𝐢𝐧(𝐭𝟏) − 𝐬𝐢𝐧(𝐭𝟐 + 𝐰) ∗ 𝐬𝐢𝐧(𝐭𝟏) −𝒄𝒐𝒔(𝒕𝟏) 𝐛 ∗ 𝐜𝐨𝐬(𝐭𝟐 + 𝐰) ∗ 𝐬𝐢𝐧(𝐭𝟏)
=[ ]
𝐬𝐢𝐧(𝐭𝟐 + 𝐰) 𝐜𝐨𝐬(𝐭𝟐 + 𝐰) 𝟎 𝐚 + 𝐛 ∗ 𝐬𝐢𝐧(𝐭𝟐 + 𝐰)
𝟎 𝟎 𝟎 𝟏

𝒓𝟏𝟏 𝒓𝟏𝟐 𝒓𝟏𝟑 𝒓𝟏𝟒

𝒓 𝒓𝟐𝟐 𝒓𝟐𝟑 𝒓𝟐𝟒
𝑻𝟑𝟎 = [ 𝟐𝟏 ]
𝒓𝟑𝟏 𝒓𝟑𝟐 𝒓𝟑𝟑 𝒓𝟑𝟒
𝟎 𝟎 𝟎 𝟏
𝑤ℎ𝑒𝑟𝑒 𝑢 = 𝑝𝑖/2; 𝑤 = 𝑝𝑖/1.09; 𝑒 = 𝑝𝑖/6 + 𝑝𝑖; 𝑟 = 𝑝𝑖/1.6363;
𝒓𝟏𝟏 = 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏)
𝒓𝟏𝟐 = −𝒔𝒊𝒏(𝒕𝟏)
𝒓𝟏𝟑 = 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)
𝒓𝟏𝟒 = 𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏)
𝒓𝟐𝟏 = 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)
𝒓𝟐𝟐 = 𝒄𝒐𝒔(𝒕𝟏)
𝒓𝟐𝟑 = 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)
𝒓𝟐𝟒 = 𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏)
𝒓𝟑𝟏 = −𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘)
𝒔𝒊𝒏(𝟐 ∗ 𝒖) ∗ (𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) − 𝟏)
𝒓𝟑𝟐 =
𝟐
𝒓𝟑𝟑 = 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)
𝒓𝟑𝟒 = 𝒂 + 𝒃 ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)

𝒓𝟏𝟏 𝒓𝟏𝟐 𝒓𝟏𝟑 𝒓𝟏𝟒

𝒓 𝒓𝟐𝟐 𝒓𝟐𝟑 𝒓𝟐𝟒
𝑻𝟒𝟎 = [ 𝟐𝟏 ]
𝒓𝟑𝟏 𝒓𝟑𝟐 𝒓𝟑𝟑 𝒓𝟑𝟒
𝟎 𝟎 𝟎 𝟏

𝒓𝟏𝟏 = 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + ∗

𝒄𝒐𝒔(𝒕𝟏)) − 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒)
𝒓𝟏𝟐 = 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)

47 | P a g e
𝒓𝟏𝟑 = 𝒄𝒐𝒔(𝒕𝟒) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗
𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))
𝒓𝟏𝟒 = 𝒄 ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) +
𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏)
𝒓𝟐𝟏 = 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) + 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 +
𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))
𝒓𝟐𝟐 = 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)
𝒓𝟐𝟑 = 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 −
𝒕𝟑)) − 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒕𝟒)
𝒓𝟐𝟒 = 𝒄 ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) +
𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏)
𝒓𝟑𝟏 = (𝒔𝒊𝒏(𝟐 ∗ 𝒖) ∗ 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) − 𝟏))/𝟐 − 𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗
𝒄𝒐𝒔(𝒕𝟒)
𝒓𝟑𝟐 = 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)
𝒓𝟑𝟑 = −𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟒)
𝒓𝟑𝟒 = 𝒂 + 𝒃 ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) + 𝒄 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒄 ∗ 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 +
𝒘)
𝒓𝟏𝟏 𝒓𝟏𝟐 𝒓𝟏𝟑 𝒓𝟏𝟒
𝒓 𝒓𝟐𝟐 𝒓𝟐𝟑 𝒓𝟐𝟒
𝑻𝟓𝟎 = [ 𝟐𝟏 ]
𝒓𝟑𝟏 𝒓𝟑𝟐 𝒓𝟑𝟑 𝒓𝟑𝟒
𝟎 𝟎 𝟎 𝟏

𝒓𝟏𝟏 = − 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) − 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 −

𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))) − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 +
𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))
𝒓𝟏𝟐 = − 𝒄𝒐𝒔(𝒕𝟒) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 −
𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))
𝒓𝟏𝟑 = 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗

𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) − 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗

𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏)))

𝒓𝟏𝟒 = 𝒄 ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) +

𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏)
𝒓𝟐𝟏 = [ 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) + 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗
𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗
𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)),
𝒓𝟐𝟐 = 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒕𝟒) − 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗
𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)),

48 | P a g e
𝒓𝟐𝟑 = 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) + 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) +

𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) + 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗

𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)),

𝒓𝟐𝟒 = 𝒄 ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) +
𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏)]
𝒓𝟑𝟏 = − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)) −
𝒔𝒊𝒏(𝟐∗𝒖)∗𝒔𝒊𝒏(𝒕𝟒)∗(𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘)− 𝟏)
𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) − ),
𝟐

𝒓𝟑𝟐 = 𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟒),

𝒓𝟑𝟑 = 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)) −
𝒔𝒊𝒏(𝟐∗𝒖)∗𝒔𝒊𝒏(𝒕𝟒)∗(𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘)− 𝟏)
𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) − ),
𝟐

𝒓𝟑𝟒 = 𝒂 + 𝒃 ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) + 𝒄 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒄 ∗ 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 +

𝒘)

𝒓𝟏𝟏 𝒓𝟏𝟐 𝒓𝟏𝟑 𝒓𝟏𝟒

𝒓 𝒓𝟐𝟐 𝒓𝟐𝟑 𝒓𝟐𝟒
𝑻𝟔𝟎 = [ 𝟐𝟏 ]
𝒓𝟑𝟏 𝒓𝟑𝟐 𝒓𝟑𝟑 𝒓𝟑𝟒
𝟎 𝟎 𝟎 𝟏

𝒓𝟏𝟏 = − 𝒔𝒊𝒏(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒕𝟒) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) +

𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))) − 𝒄𝒐𝒔(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) −
𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗
𝒄𝒐𝒔(𝒕𝟏))) + 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗
𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))),

𝒓𝟏𝟐 = 𝒔𝒊𝒏(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) − 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗

𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))) + 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗

𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) − 𝒄𝒐𝒔(𝒕𝟔) ∗

(𝒄𝒐𝒔(𝒕𝟒) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗

𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))),

𝒓𝟏𝟑 = 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗

𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) − 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗

𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))),

𝒓𝟏𝟒 = 𝒄 ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) −

𝒅 ∗ (𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) − 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) +
𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏))) − 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗
𝒄𝒐𝒔(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) + 𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟏)]

49 | P a g e
𝒓𝟐𝟏 = [ 𝒔𝒊𝒏(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒕𝟒) − 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) +
𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) + 𝒄𝒐𝒔(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) +
𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 −
𝒕𝟑))) − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗
𝒄𝒐𝒔(𝒆 − 𝒕𝟑))),

𝒓𝟐𝟐 = 𝒄𝒐𝒔(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒕𝟒) − 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) +

𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) − 𝒔𝒊𝒏(𝒕𝟔) ∗ (𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) +

𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) + 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗

𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) − 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗

𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))),

𝒓𝟐𝟑 = 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) + 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) +

𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) + 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗

𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)),

𝒓𝟐𝟒 = 𝒄 ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)) +
𝒅 ∗ (𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟏) ∗ 𝒔𝒊𝒏(𝒕𝟒) + 𝒄𝒐𝒔(𝒕𝟒) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) +
𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) + 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗
𝒔𝒊𝒏(𝒕𝟏) − 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑))) + 𝒃 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟏)]

𝒓𝟑𝟏 = [ 𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒔𝒊𝒏(𝒕𝟒) ∗ 𝒔𝒊𝒏(𝒕𝟔) − 𝒄𝒐𝒔(𝒕𝟔) ∗ (𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟐 +

𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)) + 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 −
𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) − (𝒔𝒊𝒏(𝟐 ∗ 𝒖) ∗ 𝒔𝒊𝒏(𝒕𝟒) ∗ (𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) − 𝟏))/𝟐)),

𝒓𝟑𝟐 = 𝒔𝒊𝒏(𝒕𝟔) ∗ (𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗

𝒔𝒊𝒏(𝒕𝟐 + 𝒘)) + 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) −

𝒔𝒊𝒏(𝟐∗𝒖)∗𝒔𝒊𝒏(𝒕𝟒)∗(𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘)− 𝟏)
)) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟔) ∗ 𝒔𝒊𝒏(𝒕𝟒),
𝟐

𝒓𝟑𝟑 = 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ (𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘)) −
𝒔𝒊𝒏(𝟐∗𝒖)∗𝒔𝒊𝒏(𝒕𝟒)∗(𝒄𝒐𝒔(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘)− 𝟏)
𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ (𝒔𝒊𝒏(𝒆 − 𝒕𝟐 − 𝒕𝟑 − 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) − ),
𝟐

𝒓𝟑𝟒 = 𝒂 + 𝒃 ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) + 𝒄 ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒄 ∗ 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗

𝒔𝒊𝒏(𝒕𝟐 + 𝒘) + 𝒅 ∗ 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑) + 𝒅 ∗ 𝒄𝒐𝒔(𝒓 − 𝒕𝟓) ∗ 𝒔𝒊𝒏(𝒆 −
𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) − 𝒅 ∗ 𝒔𝒊𝒏(𝒆 − 𝒕𝟑) ∗ 𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ 𝒄𝒐𝒔(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) + 𝒅 ∗
𝒔𝒊𝒏(𝒓 − 𝒕𝟓) ∗ 𝒔𝒊𝒏(𝒕𝟐 + 𝒘) ∗ 𝒄𝒐𝒔(𝒕𝟒) ∗ 𝒄𝒐𝒔(𝒆 − 𝒕𝟑)]

50 | P a g e
𝑥 = 𝑟14 = 6 ∗ 𝑠𝑖𝑛(𝑡5 − 110) ∗ (𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) − 𝑐𝑜𝑠(𝑡4) ∗ (𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡3 −
210) ∗ 𝑐𝑜𝑠(𝑡1) − 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) ∗ 𝑐𝑜𝑠(𝑡1))) − 6 ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) − 6 ∗
𝑐𝑜𝑠(𝑡5 − 110) ∗ (𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) ∗ 𝑐𝑜𝑠(𝑡1) + 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡2 +
165) ∗ 𝑐𝑜𝑠(𝑡1)) + 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡1) + 6 ∗ 𝑐𝑜𝑠(𝑡4) ∗ (𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡3 −
210) ∗ 𝑐𝑜𝑠(𝑡1) − 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) ∗ 𝑐𝑜𝑠(𝑡1)) − 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 −
210) ∗ 𝑐𝑜𝑠(𝑡1) − 6 ∗ 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡1)

𝑦 = 𝑟24 = 6 ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 6 ∗ 𝑐𝑜𝑠(𝑡4) ∗ (𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡1) −

𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡1)) − 6 ∗ 𝑠𝑖𝑛(𝑡5 − 110) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) +
𝑐𝑜𝑠(𝑡4) ∗ (𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡1) − 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) ∗
𝑠𝑖𝑛(𝑡1))) + 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡1) − 6 ∗ 𝑐𝑜𝑠(𝑡5 − 110) ∗ (𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 −
210) ∗ 𝑠𝑖𝑛(𝑡1) + 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡1)) − 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 −
210) ∗ 𝑠𝑖𝑛(𝑡1) − 6 ∗ 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡1)

𝑧 = 𝑟34 = 6 ∗ 𝑠𝑖𝑛(𝑡2 + 165) + 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡3 − 210) − 6 ∗ 𝑠𝑖𝑛(𝑡2 + 165) ∗

𝑠𝑖𝑛(𝑡3 − 210) + 6 ∗ 𝑐𝑜𝑠(𝑡5 − 110) ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡3 − 210) − 6 ∗ 𝑐𝑜𝑠(𝑡5 − 110) ∗
𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) + 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡3 − 210) ∗ 𝑐𝑜𝑠(𝑡4) + 6 ∗
𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡4) − 6 ∗ 𝑐𝑜𝑠(𝑡2 + 165) ∗ 𝑠𝑖𝑛(𝑡5 − 110) ∗ 𝑠𝑖𝑛(𝑡3 −
210) ∗ 𝑐𝑜𝑠(𝑡4) − 6 ∗ 𝑐𝑜𝑠(𝑡3 − 210) ∗ 𝑠𝑖𝑛(𝑡5 − 110) ∗ 𝑠𝑖𝑛(𝑡2 + 165) ∗ 𝑐𝑜𝑠(𝑡4) + 10

Inverse Position Kinematics

𝑦
𝜃1 = 𝑡𝑎𝑛−1 ( )
𝑥
𝑧 − 𝑙1
𝐾 = 𝑡𝑎𝑛−1 ( )
√𝑥 2 + 𝑦 2

−1
𝑙22 − 𝑙32 + (𝑥 2 + 𝑦 2 + 𝑧 2 + 𝑙12 − (2 × 𝑧 × 𝑙1 ))
𝑃 = 𝑐𝑜𝑠 ( )
2 × 𝑙2 × √𝑥 2 + 𝑦 2 + 𝑧 2 + 𝑙12 − (2 × 𝑧 × 𝑙1 )
𝜃2 = 𝐾 − 𝑃
𝑧 − 𝑙1 − 𝑙2 × sin(𝜃2 )
𝜃3 = 𝑡𝑎𝑛−1 ( ) − 𝜃2
√𝑥 2 + 𝑦 2 − cos(𝜃2 )
Where
x, y and z represent coordinates. l1, l2 and l3 represent link length of base, shoulder
and elbow respectively.

51 | P a g e
 𝒓𝟏𝟏 𝒓𝟏𝟐 𝒓𝟏𝟑 𝒓𝟏𝟒
𝒓 𝒓𝟐𝟐 𝒓𝟐𝟑 𝒓𝟐𝟒
𝑻𝟔𝟑 = [ 𝟐𝟏 ]
𝒓𝟑𝟏 𝒓𝟑𝟐 𝒓𝟑𝟑 𝒓𝟑𝟒
𝟎 𝟎 𝟎 𝟏

2 2
𝜃5 = 𝑎𝑡𝑎𝑛2(√𝑟31 + 𝑟32 , 𝑟33 )

𝑖𝑓 𝜃5 = 0
{
𝜃4 = 0
𝜃6 = 𝑎𝑡𝑎𝑛2(−𝑟12 , 𝑟11 )
}
𝑖𝑓 𝜃5 = 180
{
𝜃4 = 0
𝜃6 = 𝑎𝑡𝑎𝑛2(𝑟12 , −𝑟11 )
}
𝑂𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒
𝑟23 𝑟13
𝜃4 = 𝑎𝑡𝑎𝑛2 ( , )
sin(𝜃5 ) sin(𝜃5 )
𝑟32 −𝑟31
𝜃6 = 𝑎𝑡𝑎𝑛2 ( , )
sin(𝜃5 ) sin(𝜃5 )

Appendix B
Clc
Clear all
Close all
vol=[1:1:100]'; % Applied Volatge
rpm=[3.555 12.54 21.525 ...... 100 ]
ts=0.1; % Sampling Time
z = iddata(rpm,vol,ts) % Create identification variable
ze=z(1:50); % first half is use for estimation
zv=z(51:100);% Second half is use for validation
ident % command for Toolbox

52 | P a g e
Appendix C
Current error = desired value - actual value;
Integral error = (accumulator error) * time period;
Derivative error = (Current error – previous error) * 1/time period;
PID = kp * Current error + ki * Integral error + kd * Derivative error ;
Previous error = Current error;
Accumulator error += Current error;
Where
Kp = proportional term, Ki = Integral term, Kd = derivative term
Appendix D

%%%%% Base Motor %%%%%

num=[100 45.65];
den=[12 22 65];
sys=tf(num,den);
rlocus(sys)
%%%%% Shoulder Motor %%%%%
num=[75 58.025];
den=[1 14.29 78.065];
sys2=tf(num,den);
rlocus(sys2)
%%%%% Elbow Motor %%%%%
num=[13 12];
den=[10 12 8];
sys3=tf(num,den);
rlocus(sys3)
%%%%%% Wrist 2nd Motor %%%%%
num=[53 16.52];
den=[4.64 25.256 35];
sys4=tf(num,den);
rlocus(sys4)
%%%%%% Wrist 3rd Motor %%%%%
num=[100 45.65];
den=[12 22 65];

53 | P a g e
sys5=tf(num,den);
rlocus(sys5)

Appendix E
Clc
Clear all
Close all
u=pi/2; w=pi/1.09; e=pi/6+pi; r=-pi/1.6363+pi/7.2;
a=10; b=6; c=6; d=6;
%syms a b c d u w e r t1 t2 t3 t4 t5 t6
L(1)=Link([0 a 0 u 0]);
L(2) = Link('revolute', 'd', 0, 'a', b, 'alpha', 0,'offset',w);
L(3) = Link('revolute', 'd', 0, 'a', 0, 'alpha', -u,'offset',-
e);
L(4) = Link('revolute', 'd', c, 'a', 0, 'alpha', u,'offset',0);
L(5) = Link('revolute', 'd', 0, 'a', 0, 'alpha', -u,'offset',r);
L(6)=Link([0 d 0 0 0]);
Rob=SerialLink(L,'name','Articulated');
T=Rob.fkine([0 0 0 0 0 0])
Rob.plot([0 0 0 0 0 0])

Appendix F
Velocity jacobain of 1st three joints can be represented as
0 s1 s1 
J w  0  c1  c1
1 0 0 

J 
J  v
J w 
Spherical wrist velocity jacobain can be represented as
Due to origin lie at same point O4=O5=O6
0 0 0 
J v  0 0 0
0 0 0

c1 s 23   c 4 s1  s 4 (c1 s 2 s3  c1c2 c3 )


z 3   s1 s 23 , z 4   s 4 ( s1 s 2 s3  c 2 c3 c1 )  c1c 4 , z 5  Third _ column _ of _ T50
  
 c23   s 23 s 4 

54 | P a g e
 Appendix G
By calculating determinant of J11, we get
a 2 a3 s3 (a 2 c 2  a3 c 23 )  0
a 2 a3 s3  0
a 2 a3  0
s3  0
 3  sin 1 (0)
 3  0, 
Similarly solving for wrist singularity, we get
 5  0, 

55 | P a g e

View publication stats