Report submitted in the fulfilment of the requirements of

SUMMER INTERNSHIP REPORT

On

PID CONTROL OF 2 DOF ROBOTIC MANIPULATOR

BY

UJJAWAL TRIPATHI

VIT, VELLORE

Submitted to

DEPARTMENT OF
INSTRUMENTATION AND CONTROL ENGINEERING

NATIONAL INSTITUTE OF TECHNOLOGY

TIRUCHIRAPPALLI – 620015

1
 Report submitted in the fulfilment of the requirements of

SUMMER INTERNSHIP REPORT

On

PID CONTROL OF 2 DOF ROBOTIC MANIPULATOR

BY

UJJAWAL TRIPATHI

VIT, VELLORE

Submitted to

DEPARTMENT OF
INSTRUMENTATION AND CONTROL ENGINEERING

NATIONAL INSTITUTE OF TECHNOLOGY

TIRUCHIRAPPALLI – 620015

2
 BONAFIDE CERTIFICATE

This is to certify that the project titled PID CONTROL OF 2-DOF ROBOTIC
MANIPULATOR is a bonafide record of the work done by

UJJAWAL TRIPATHI

in partial fulfillment of the requirements for the award of the certificate of Summer Internship in
INSTRUMENTATION & CONTROL ENGINEERING of the NATIONAL INSTITUTE OF
TECHNOLOGY, TIRUCHIRAPPALLI, during the 4 weeks.

Dr. RAHUL KUMAR SHARMA Dr. K. DHANALAKSHMI

Project Guide Head of the Department

3
 ABSTRACT

The manufacturing sector heavily relies on robotic manipulators. They are utilized for a variety
of things, such as reproducibility, accuracy, and speed. Robotic manipulators are being used more
and more integrating themselves into our daily lives. In reality, a robotic manipulator has been
used in the creation of practically every object we use. One of the most seen challenges in
robotics is to maneuver a robotic arm quickly and precisely. We can begin to understand the
complex movement of human arms after defining the movement of the two-link manipulator.
The two-link system's total potential and kinetic energies are defined and used to form the
Lagrangian. The Euler-Lagrangian equation was used to define the torques applied to each link.
A proportional-integral-derivative (PID) controller is implemented to simulate the behavior of
the system when torques are applied. The PID controllers are the most commonly used
controllers, due to their easy implementation and versatility. PID controllers remain the standard
for industry and research given their flexibility and ease of use in both simulation and hardware
systems. The equation of motion for a two-link robot is a nonlinear differential equation. After
deriving the equation of motion, the PID Control law is designed for the system for trajectory
tracking problem. The simulation results are obtained for 2-degrees-of-freedom (DOF) Robotic
Manipulator.

4
 ACKNOWLEDGEMENTS

I would like to express my sincere gratitude to our project guide Dr. Rahul Kumar Sharma,
Department of Instrumentation and Control Engineering, National Institute of Technology.
Tiruchirappalli for providing me with an opportunity to work with him. While providing constant
support throughout the project, he greatly helped me understand the concepts involved and
pushed us to think more critically and without his guidance, I would not have been able to
successfully complete the project. His valuable feedback helped me a lot in avoiding potential
pitfalls and dealing with possible errors. His patience and genial attitude will always be a source
of inspiration to me.

I would also like to thank the faculty and staff members of the Department of Instrumentation
and Control Engineering and our individual parents and friends for their constant support and
help.

Ujjawal Tripathi (20BEI0012)

5
 LIST OF FIGURES

Figure No. Title Page no.

1.1 Robotic manipulator 9

1.2 Two degree of freedom manipulator 15

1.3 Kinematic Control Model 20

1.4 Kinematic Control in Joint Space 22

1.5 Output (with error) 23

2.1 Output (with proportional Controller) 23

2.2,2.3 Output (with PI Controller) 24

2.4 Dynamic Control Model 25

2.5 Dynamic Control in Task Space 29

2.6 Final Block Diagram 29

3.1 Output (Cascade Control) 34

3.3 Output (Without Gravity Compensation) 35

4.1,4.2,4.3 Output (Computed Torque Control) 36,37

4.4,4.5,4.6 Output (PID Control) 38

5.1,5.2 Inverse Dynamics in Task Space 39

5.3,5.4 Motion Control (PD Controller) 41

5.5,5.6 Motion Control (With Gravity Compensation) 42

6.1,6.2 Motion Control (PID Controller) 43

6
TABLES OF CONTENT: -

Certificate……………………………………………………………………………………….…..[3]

Abstract…………………………………………………………………………………………......[4]
Acknowledgment………………………………………………………………………………..[5]
List of Figure……………………………………………………………………………………...[6]

Chapter 1

1. Introduction…………………………….………………………………………………………….[9]

Chapter 2

Kinematics of 2-DOF Robotic Manipulator...…………………………………….[13]

2.1 Forward Kinematics

2.2 Inverse Kinematics

Chapter 3

Dynamics of 2-DOF Robotic Manipulator………………………………………………[15]

3.1 Lagrangian-Euler Equation

3.2 State-Space Model

Chapter 4

Trajectory Tracking Problem…………………………………………………….………………[20]

4.1 Kinematic Control…………………………………………………………………………..[21]

4.2 P Controller…………………………………………………………………………………… [22]
4.3 PI Controller…………………………………………………………………………………. [23]
4.4 Cascade Controller…………………………………………………………………..……..[32]
4.5 Computed Torque Control……………………………………………………………....[37]
4.6 PID Control………………..………………………………………………………….…[39]

Chapter 5

Inverse Dynamics in task space ………………………………………………….…………..[40]

Chapter 6

Motion Control.………………………………………………………….…………………………….[41]

6.1 PD Controller.…………………………………………………………………………………[42]

7
 6.2 PD Control with gravity compensation…………………………………………...[43]
6.3 PID Controller. .……………………………………………………………………………..[44]

Chapter 7

Conclusion……………………………………………………..………………………………………… [67]

Chapter 8

Future Scope………………………………………………………………………………….…….…..[69]
Reference……………………………………………………………………………………….…..[70]

8
 CHAPTER-1

Introduction

Controlling robot manipulators is essential for attaining precise and accurate movements in the
field of robotics. Robot manipulators are multi-jointed mechanical devices used to carry out a
variety of activities, including complex movements, assembly work, and pick-and-place
operations. Advanced control methods, such as the PID (Proportional-Integral-Derivative)
controller, are utilized to regulate the movements of these manipulators.

In recent years, the sector has seen a minor increase in robot arm manipulators. When calculating
the inertia tensor of a moving rigid body, the system becomes complicated due to non-linearities
between the robot's joints and the issue of modeling dynamics and motion control. Robot arm
control is used to manage a computer-based manipulator's dynamic reaction in accordance with
predetermined system performance and objectives. Electric, hydraulic, or pneumatic actuators
power the majority of robot manipulators, applying torques (or forces, in the case of linear
actuators) to the robot's joints.

A 2-degrees-of-freedom (DOF) robot manipulator's model is a condensed depiction of the robot's

kinematic and dynamic behavior. The robot may achieve desired positions and orientations in its
workspace by manipulating its two joints, which can rotate about their axes. In order to reduce
the error between the desired and actual positions, the PID controller, a feedback control
mechanism, is frequently employed to manage the movement of the robot.

Fig1.1(Robotic manipulator)

Traditional techniques of robot control heavily rely on precise mathematical modelling, analysis,
and synthesis. A relatively active area of study is dynamic modelling of manipulators, which can
be used to examine system responses and properties, such as determining the stability of the

9
system. In closed-loop systems, the manipulators really employ proportional-derivative
controllers (PD) or proportional-integral-derivative controllers (PID) to achieve the necessary
configurations.

Proportional, integral, and derivative control actions make up the foundation of how the PID
controller functions. By scaling the difference between the planned and actual positions by a gain
factor, the proportional control action generates a control signal that is directly proportionate to
the error. The integral control action generates a control signal that corrects for long-term
deviations by accumulating the prior error values across time and accounting for steady-state
errors. The derivative control action takes into account the error's rate of change, enabling the
controller to react swiftly to unexpected changes and reduce oscillations.

The three gains of the proportional-integral-derivative (PID) control have simple structures and
distinct physical implications. The majority of industrial processes accept the control
performances. The majority of industrial robot manipulators use PID algorithms that are
individually controlled at each joint .

For robots whose gearbox system contains reduction devices like gears or bands, tuning PID
controllers is actually simpler. By efficiently increasing the torque or force produced by the
actuators through the application of these reductions, links with a great deal of mass can be
driven by these actuators.

The 2-DOF model's PID controller allows the robot manipulator to accomplish accurate and
reliable positioning and tracking. Rapid error response is made possible by the proportional term,
steady-state errors are eliminated by the integral term, and system stability is improved by the
derivative term. Effective trajectory tracking, disturbance rejection, and resilience in the face of
outside influences on the robot's mobility are all made possible by the combination of these three
control actions.

In order to maximize the performance of the system, a PID controller is implemented in the 2-
DOF model by adjusting the controller gains. To achieve the intended response, the gain values
are often adjusted iteratively while taking into account variables like the operating environment,
desired precision, and system dynamics. Advanced PID controller variants like cascade control
and adaptive control can also improve control performance in challenging situations.

A robotic arm with two joints or axes of rotation, allowing it to move in two separate directions,
is referred to as a 2-degree-of-freedom robotic manipulator. To regulate and control the
movements of robotic manipulators, industry frequently uses PID (Proportional-Integral-
Derivative) control. The following are some of the main industrial uses for a 2-degree-of-
freedom robotic manipulator with PID control.

10
Pick-and-area Operations: Using a robotic manipulator to pick objects out of one area and
place them in another is a popular application for such devices. PID control enables the robot to
accurately regulate the end-effector's position and orientation, enabling precise and dependable
object manipulation. Robots are utilised for a variety of jobs in the welding and cutting
industries, including the automotive and manufacturing sectors. PID control helps the robot to
retain stable and accurate management of the welding or cutting process while following a
desired trajectory, producing high-quality welds or cuts.

Inspection and quality control duties can be carried out using robotic manipulators that are
fitted with sensors and cameras. PID control enables the robot to move the sensors or cameras
along a predetermined path, ensuring accurate and reliable product or component examination.

Material Handling: Robotic manipulators are frequently employed in warehouses and logistics
facilities to perform material handling operations like palletizing and depalletizing.

Machine Tending: Workpieces from machines like CNC (Computer Numerical Control)
machines or injection moulding machines can be loaded and unloaded using robots with two
degrees of freedom. PID control enables the robot to manipulate the workpieces precisely and
effectively, cutting down on cycle times and boosting output.

PID control offers the necessary feedback loop in each of these uses, allowing the robotic
manipulator's control signals to be continuously adjusted to account for mistakes and
disturbances. In industrial operations employing 2-degree-of-freedom robotic manipulators, it
offers fine-grained control over position, velocity, and orientation, resulting in higher
performance, increased production, and improved product quality.

11
 CHAPTER-2

Kinematics of 2-DOF Robotic Manipulator

Robot Kinematics

The study of the position, speed, and acceleration of robot manipulators is referred to as
kinematics, which is a crucial component of robot control. Understanding the forward and
inverse kinematics is essential to figuring out the joint angles needed to accomplish the desired
end-effector position and orientation for a 2-degree-of-freedom (DOF) robotic manipulator.

Forward Kinematics:

The forward kinematics of a robot manipulator involves calculating the position and orientation
of the end effector based on the joint angles. In the case of a 2-DOF manipulator, the position of
the end effector can be determined by applying basic trigonometry and geometry. Let's denote
the joint angles as θ₁ and θ₂, the lengths of the links as L₁ and L₂, and the position of the end
effector as (X, Y).

Using the following equations, the forward kinematics can be derived:

X = L₁ * cos(θ₁) + L₂ * cos (θ₁ + θ₂) (1)

Y = L₁ * sin(θ₁) + L₂ * sin (θ₁ + θ₂) (2)

Inverse Kinematics:

The inverse kinematics of a robot manipulator involves calculating the joint angles required to
achieve a desired end-effector position and orientation. In the case of a 2-DOF manipulator,
solving the inverse kinematics problem requires determining the values of θ₁ and θ₂ given the
desired (X, Y) coordinates.

To solve the inverse kinematics problem, the previously mentioned forward kinematics equations
can be rearranged as follows:

θ₁ = tan−1(Y, X) - tan−1 (L₂ * sin(θ₂), L₁ + L₂ * cos(θ₂)) (3)

θ₂ = cos −1((X - L₁ * cos(θ₁))/L₂) - θ₁ (4)

12
Robot Dynamics

Dynamics is the study of systems that undergo changes of state as time evolves. In mechanical
systems such as robots, the change of states involves motion. In other words, dynamics is the
science of motion. It describes why and how a motion occurs when forces and moments are
applied on massive bodies.

The way in which the motion of the robotic system arises from torques/forces applied by the
actuators, or from external forces/moments applied to the system.

𝜏 = 𝑓𝑢𝑛(𝑞, 𝑞̇ , 𝑞̈ )

𝜏 = 𝑀(𝑞)𝑞̈ + 𝑛(𝑞, 𝑞̇ )

Forward Dynamics

For a given, input vector, tau and the known states q, q, find the resultant motion of the
manipulator, in other words, find 𝑞̈

𝑞̈ = 𝑀−1 (𝑞)(𝜏 − 𝑛(𝑞, 𝑞̇ )) (5)

Inverse Dynamics:

For a given, trajectory vectors 𝑞, 𝑞̇ ,𝑞̈ find the required input vector tau. It is the problem of
controlling the manipulator.

Formulation method- Lagrange- Euler method (energy-based approach).

13
 CHAPTER-3

Dynamics of 2-DOF Robotic Manipulator

Lagrange- Euler method:

Since, we are considering the system as a rigid body system, the Lagrangian can be defined as
the difference between the kinetic energy and the potential energy (it is a scalar quantity). The
system consists of two masses connected by weightless bars. The bars have lengths L1 and L2.
The masses are denoted by M1 and M2, respectively. Let θ1 and θ2 denote the angles in which the
first bar rotates about the origin and the second bar rotates about the endpoint of the first bar,
respectively. This system has two degrees of freedom θ1 and θ2.

The first step in deriving the equations of motion using the Lagrangian approach is to find the
kinetic energy KE and the potential energy PE of the system.

L = K.E - P.E

ⅆ 𝜕𝐿 𝜕𝐿
𝜏= ( ) − 𝜕𝑞
ⅆ𝑡 𝜕𝑞̇

1 𝑖
𝐾. 𝐸. = (∑ 𝑚𝑖 𝑂𝑣𝑇 𝑜𝑣𝐶𝑖 + ∑ 𝜔𝑇 𝐼𝑐𝑖 𝑖𝑖𝜔
2 ℂ𝑖 𝑖

𝑃. 𝐸 = ∑(𝑚𝑖 𝑜𝑔𝑇 𝑜𝑃 𝑐𝑖 )

14
 Fig1.2(Two degree of freedom manipulator)

1 1
𝐾. 𝐸 = 𝑚1 (𝑥̇ 1 2 + 𝑦̇ 1 2 ) + 𝑚2 (𝑥̇ 2 2 + 𝑦̇ 2 2 )
2 2

𝑥̇ 1 = 𝐿1 𝑆1 𝜃̇1

𝑦̇ 1 = 𝐿1 𝑐1 𝜃̇1

𝑥̇ 2 = −𝐿1 𝑆1 𝜃̇1 − 𝐿2 𝑆12 (𝜃1̇ + 𝜃̇2 )

𝑦̇2 = 𝐿1 𝑐1 𝜃̇1 − 𝐿2 𝑆12 (𝜃1̇ + 𝜃2̇ )

𝑃. 𝐸 = 𝑚1 𝑔𝑦1 + 𝑚2 𝑔𝑦2

𝑃. 𝐸 = 𝑔𝑚1 𝐿1 𝑆1 + 𝑔𝑚2 𝐿1 𝑆1 + 𝑔𝑚2 𝐿2 𝑆12

15
Lagranrian equation

1 2 1
𝐿 = 2 (𝑚1 𝐿21 + 𝑚2 𝐿21 + 𝑚2 𝐿22 + 2𝑚2 𝐿1 𝐿2 𝐶2 )𝜃̇1 + 𝑚2 𝐿22 𝜃̇22 + (𝑚2 𝐿22 +
2
𝑚2 𝐿1 𝐿2 𝐶2 )𝜃̇1 𝜃̇2 − 𝑔(𝑚1 𝐿1 + 𝑚2 𝐿1 )𝑠1 − 𝑔𝑚2 𝐿2 𝑆12 (6)

Joint 1 torque, 𝜏1 and 𝜏2

ⅆ 𝜕𝐿 𝜕𝐿
𝜏1 = ( )−
ⅆ𝑡 𝜕𝜃̇1 𝜕𝜃1

ⅆ 𝜕𝐿 𝜕𝐿
𝜏2 = ( )−
ⅆ𝑡 𝜕𝜃̇2 𝜕𝜃2

𝜏1
= (𝑚1 𝐿21 + 𝑚2 𝐿21 + 2𝑚2 𝐿1 𝐿2 𝐶2 )𝜃̈1 + (𝑚2 𝐿22 + 𝑚2 𝐿1 𝐿2 𝐶2 )𝜃̈2 − 2𝑚2 𝐿1 𝐿2 𝑆2 𝜃̇1 𝜃̇2
− 𝑚2 𝐿1 𝐿2 𝑆2 𝜃̇22 + 𝑔((𝑚1 𝐿1 + 𝑚2 𝐿1 )𝐶1
+ 𝑚2 𝐿2 𝐶12 (7)

𝜏2 = (𝑚2 𝐿22 +𝑚2 𝐿1 𝐿2 𝐶2 ) 𝜃̈1 + (𝑚2 𝐿22 ) 𝜃̈2 + 𝑚2 𝐿1 𝐿2 𝑆2 𝜃̇12 +

𝑔(𝑚2 𝐿2 )𝐶12 (8)

In State Space form

𝝉 = 𝑴(𝒒)𝒒̈ + 𝑩(𝒒)[𝒒̇ 𝒒̇ ] + 𝑪(𝒒)[𝒒̇ 𝟐 ] + 𝒈(𝒒) (9)

M(q) = Inertia matrix

B(q) = Coriolis effect matrix

C(q) = Centripetal effects matrix

g(q) = gravitational effects

16
 [(𝑚1 𝐿21 + 𝑚2 𝐿21 + 2𝑚2 𝐿1 𝐿2 𝐶2 ) (𝑚2 𝐿22 + 𝑚2 𝐿1 𝐿2 𝐶2 )
M(q) =
(𝑚2 𝐿22 + 𝑚2 𝐿1 𝐿2 𝐶2 ) 𝑚2 𝐿22 ]

𝑩(𝒒)[𝒒̇ 𝒒̇ ] = [2𝑚2 𝐿1 𝐿2 𝑆2

0 ]

𝑪(𝒒)[𝒒̇ 𝟐 ] = [0 − 𝑚2 𝐿1 𝐿2 𝑆2

𝑚2 𝐿1 𝐿2 𝑆2 0]

g(𝒒) = [ 𝑔(𝑚1 𝐿1 + 𝑚2 𝐿1 )𝐶1 + 𝑚2 𝐿2 𝐶12

𝑔(𝑚2 𝐿2 𝐶12 )]

Inclusion of non-rigid effects

𝜇̇ = J(q)𝑞̇

𝜇̈ = 𝐽(𝑞)𝑞̈ + 𝐽(̇ 𝑞)𝑞̇

𝜏 = 𝐽𝑇 (𝑞)𝐹 (10)

𝑞̈ = 𝐽−1 (q)[𝜇̈ -𝐽(̇ 𝑞)𝑞̇ ]

M(𝑞)(𝐽−1 (𝑞)[𝜇̈ − 𝐽(̇ 𝑞)𝑞̇ ]) + 𝑛(𝑞, 𝑞̇ ) = 𝐽𝑇 (𝑞)𝐹

𝐽−𝑇 (M(𝑞)(𝐽−1 (𝑞)[𝜇̈ − 𝐽(̇ 𝑞)𝑞̇ ]) + 𝐽−𝑇 (𝑛(𝑞, 𝑞̇ )) = 𝐹

𝑀𝜇 𝜇̈ + 𝑛𝜇 = 𝐹 (11)

Types of Robot manipulator control

1. Motion-based (model-free) control methods

(i) Pure feedback or sensing-based (reactive scheme) easy to use, but sluggish and
produce poor tracking performance

2. Model-based control methods

17
 (i) Comparatively efficient and simple and accurate models should be available.

The implicit assumptions in most existing control schemes are:

The model is accurately known and/or the constant physical parameters are accurately known.

Two general techniques deal with the absence of the above considerations:

1. Robust control aims at controlling, with a small error, a class of robot manipulators
(model is not accurately known) with the same controller.

2. Adaptive control deals with uncertainty in the system’s parameters (unknown constant
parameters) and requires precise knowledge of the system's structure.

18
 CHAPTER-4

Trajectory Tracking Problem

KINEMATIC CONTROL

Inverse Differential Kinematics (Open-loop/Feed-forward Control)

Objective: To follow the given position trajectory (assuming that the initial values of the actual
and desired states are equal) and a non-zero vector of desired velocities exist.

𝑞̇ ⅆ (𝑡) ≠ 0, 𝑞ⅆ (𝑡 = 0) = 𝑞(𝑡 = 0)

Control scheme:

𝑞̇ = 𝑞̇ ⅆ

Robot Kinematic (Motion) Control in Joint Space

➢ Desired:

Desired joint positions, 𝑞ⅆ (t)

Desired joint velocities, 𝑞̇ (𝑡),

for set-point control: 𝑞ⅆ̇ (t)= 0

➢ Available:

■ Actual joint positions, 𝑞 (t)

Jacobian matrix, 𝐽(𝑞), 𝐽−1 (𝑞)

To find:

Control inputs, 𝑞̇ (𝑡)

➢ Objective:

Asymptotically (exponentially) stable, t→ ∞, q→𝑞ⅆ (or)

■in other words, t→ ∞, 𝑞̃→ 0⇒ 𝑞̃(𝑡) = 𝑒 −𝜆𝑡 , 𝜆>0

where 𝑞̃ = 𝑞ⅆ − 𝑞

19
 𝑞̃(t)=𝑒 −𝜆𝑡 , 𝜆> 0

Differentiating the above relation w.r.t. time, it gives

𝑞̃̇ = 𝜆𝑒 −𝜆𝑡

𝑞̃̇ + 𝜆𝑞̃ = 0

𝑞̇ ⅆ − 𝑞̇ + 𝜆𝑞̃ = 0

Here

𝑞̇ = kinematic control scheme

𝑞̇ ⅆ = feedforward term

𝑞̃ = feedback term

Fig 1.3(Kinematic Control Model)

➢ Robot Kinematic (Motion) Control in Joint Space

Desired:

Desired joint positions,𝑞ⅆ (t)

20
 Available:

Actual joint positions, 𝑞(t)

To find:

Control inputs,

𝑞̇ (t)

➢ Proportional control:

𝑞̇ = 𝑘p [𝑞ⅆ − 𝑞 ], 𝑘p > 0 (12)

This scheme may end-up with some steady state errors.

➢ Proportional and Integral control:

𝑞̇ = 𝑘𝑝 [𝑞ⅆ − 𝑞] + 𝑘i ∫ [𝑞ⅆ − 𝑞]ⅆ𝑡, 𝑘𝑝 > 0, 𝑘 > 0 (13)

Robot Kinematic (Motion) Control in Task Space

■Desired:

Desired end-effector positions, 𝜇ⅆ (t)

Desired end-effector velocities, 𝜇̇ ⅆ (t), for set-point control: 𝜇̇ ⅆ (t) = 0

Available:

Actual end-effector positions, 𝜇(t), and actual joint-positions, q (t)

Jacobian matrix, J (q), 𝐽−1(q)

■To find:

■Control inputs, 𝑞̇ (t)

■ Objective: Asymptotically (exponentially) stable, t→∞, 𝜇→𝜇ⅆ (or) in other words, t→ ∞, 𝜇̃→
0→ 𝜇̃(t) = 𝑒 −𝛤𝑡 , г> 0

where 𝜇̃ = 𝜇ⅆ − 𝜇

𝜇̇ = 𝐽(𝑞)𝑞̇

𝜇̃̇ + 𝛤𝜇̃ = 0

21
Where 𝛤 > 0

𝜇̇ ⅆ − 𝜇 + 𝛤𝜇̃ = 0

𝜇̇ ⅆ − 𝐽(𝑞)𝑞̇ + 𝛤𝜇̃ = 0

𝑞̇ = 𝐽(𝑞)−1(𝜇̇ ⅆ + 𝛤𝜇̃) (14)

FINAL BLOCK DIAGRAM

Fig 1.4(Kinematic Control in Joint Space)

22
OUTPUT (with error)

Fig 1.5(Output with error)

➢ Since there is some error between the desired value and obtained value here now, we will
be using the proportional controller(lambda)

OUTPUT(With Proportional controller)

Fig 2.1(Output with proportional Controller)

23
OBSERVATION

➢ The current error, which is between the desired and actual joint angles, is taken into
account by the P controller when adjusting the control input (such as a torque or velocity
instruction).

➢ But the problem here will arise is steady state error which can be eliminated by using
Integral controller with proper parameters.

OUTPUT( PI controller)

Fig 2.2(Output with PI Controller) Fig 2.3(Output with PI Controller)

24
Block diagram for Dynamic control

Fig 2.4(Dynamic Control Model)

➢ Here when we are giving tau as input, we will get 𝑞 and 𝑞̇ as output
➢ Hence 𝜏 would be a function of 𝑞 and 𝑞ⅆ

• Robot Dynamic (Motion) Control in Joint space

Robot dynamic control in joint space is the process of directly controlling joint velocities or
torque inputs to move a robot along a desired joint trajectory. This kind of control considers the
robot's dynamics, such as its mass, inertia, and joint friction

■ Desired:

Desired joint positions, 𝑞ⅆ (t)

Desired joint velocities, 𝑞̇ ⅆ (t), for set-point control: 𝑞̇ ⅆ (t) = 0

Desired joint accelerations, 𝑞̈ ⅆ (𝑡), for set-point control: , 𝑞̈ ⅆ (𝑡) = 0

■ Available:

25
Actual joint positions, 𝑞 (t), Actual joint velocities, 𝑞̇ (t)

Inertia matrix, M (𝑞) and n (𝑞, 𝑞̇ )

■To find:

■Control inputs, 𝜏, sometimes 𝜏 = 𝐽𝑇 (q) F

■ Objective:

■ Asymptotically (exponentially) stable, t→ ∞,𝑞̃→ 0 and 𝑞̃̇ → 0

☐ where 𝑞̃̇ = 𝑞̇ ⅆ - 𝑞̇

𝑞̃̈ + 𝛬1 𝑞̃̇ + 𝛬2 𝑞̃ = 0

Where 𝛬1 𝑎𝑛ⅆ 𝛬2 > 0

𝑞̈ ⅆ − 𝑞̈ + 𝛬1 𝑞̃̇ + 𝛬2 𝑞̃ = 0

𝑞̈ = 𝑞̈ ⅆ +𝛬1 𝑞̃̇ + 𝛬2 𝑞̃

M(𝑞)𝑞̈ + n(q,𝑞̇ ) = 𝜏

𝑞̈ = 𝑀(𝑞)−1 (𝜏 − 𝑛(𝑞, 𝑞̇ )) ; 𝜏 = M(𝑞)[𝑞̈ ⅆ +𝛬1 𝑞̃̇ + 𝛬2 𝑞̃]+ 𝑛(𝑞, 𝑞̇ )

𝜏 = M(𝑞)[𝑞̈ ⅆ +𝛬1 𝑞̃̇ + 𝛬2 𝑞̃]+ 𝑛(𝑞, 𝑞̇ ) (15)

1. Here 𝑞̈ ⅆ = feedforward term.

2. 𝑛 (𝑞, 𝑞̇ ) = feedback linearization term.
3. Rest other term terms are simple PD control.

Robot Dynamic (Motion) Control in Task space

Robot dynamic control in task space is the direct command of Cartesian or task-related variables,
such as end-effector location, orientation, or a combination of both, to control a robot's mobility.
Regardless of the robot's joint design, this sort of control enables more flexible and intuitive
control of the robot's movements.

➢ Desired:

Desired end-effector positions, 𝜇ⅆ (t) Desired end-effector velocities, 𝜇̇ ⅆ (t)

26
 ➢ Desired end-effector accelerations, 𝜇ⅆ̈ (t)
➢ Available:

Actual end-effector positions, 𝜇(t),

Actual end-effector velocities,𝜇̇ (t)

Jacobian matrix, 𝐽 (𝑞), 𝐽−1 (𝑞), Inertia matrix, 𝑀𝜇 and 𝑛𝜇 (μ, 𝜇̇ ) -1

➢ To find:

Control inputs, 𝐹

➢ Objective:
➢ Asymptotically (exponentially) stable, t→ ∞,𝜇̃→ 0 and 𝜇̃̇ → 0
➢ where 𝜇̃ = 𝜇ⅆ -𝜇 and 𝜇̃̇ = 𝜇̇ ⅆ − 𝜇̇

𝜇̃̈ + 𝛤1 𝜇̃̇ + 𝛤2 𝜇̃ = 0

Where 𝛤1 , 𝛤2 > 0

𝜇ⅆ̈ − 𝜇̈ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃ = 0

𝜇̈ = 𝜇ⅆ̈ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃

𝑀𝜇 𝜇̈ + 𝑛𝜇 (𝜇, 𝜇̇ ) = 𝐹

𝜇̈ = 𝑀𝜇−1 (𝐹 − 𝑛𝜇 (𝜇, 𝜇̇ ))

𝑀𝜇−1 (𝐹 − 𝑛𝜇 (𝜇, 𝜇̇ )) = 𝜇ⅆ̈ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃

𝐹 = 𝑀𝜇 [𝜇ⅆ̈ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃] +𝑛𝜇 (𝜇, 𝜇̇ ) (16)

𝑀𝜇−1 (𝐹 − 𝑛𝜇 (𝜇, 𝜇̇ )) = 𝜇ⅆ̈ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃

𝐹 = 𝑀𝜇 [𝜇ⅆ̈ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃] +𝑛𝜇 (𝜇, 𝜇̇ )

𝑀𝜇 = 𝐽−𝑇 (𝑞)M(𝑞)𝐽−1 (𝑞)

𝑛𝜇 (𝜇, 𝜇̇ ) = 𝐽−𝑇 (𝑞)n(𝑞,𝑞̇ ) − 𝑀𝜇 𝐽(̇ 𝑞)𝑞̇

𝜏 = 𝐽𝑇 (𝑞)𝐹

27
τ = M(q) 𝐽−1 (𝑞)[ 𝜇ⅆ̈ − 𝐽(̇ 𝑞)𝑞̇ +𝛤1 𝜇̃̇ + 𝛤2 𝜇̃] + n(𝑞,𝑞̇ ) (17)

Fig 2.5(Dynamic Control in Task Space)

Final block diagram

Fig 2.6(Final Block Diagram)

28
Since robotics system is 2nd order system, we will have Robotic System

𝑥̇ 1 = 𝑓1 (𝑥1 ) + 𝑔1 (𝑥1 )𝑥2

𝑥̇ 2 = 𝑓2 (𝑥1 , 𝑥2 ) + 𝑔2 (𝑥1 , 𝑥2 )

Based on kinematic and dynamic relationships

𝜇̇ = 𝐽(𝑞)𝑞̇

𝜂̈ = 𝑀−1 (𝑞)[𝜏 − 𝑛(𝑞, 𝑞̇ )]

𝑥̇ 1 = 𝑓1 (𝑥1 ) + 𝑔1 (𝑥1 )𝑥2

𝑥̇ 2 = 𝑓2 (𝑥1 , 𝑥2 ) + 𝑔2 (𝑥1 , 𝑥2 )𝑢

Assume that x1 = 𝜇 and x2 = 𝑞̇

𝑥1 = 𝜇 , 𝑥2 = 𝑞̇

𝑓1 (𝑥1 ) = 0, 𝑔1 (𝑥1 ) = 𝐽(𝑞)

𝑓2 (𝑥1 , 𝑥2 ) = 𝑀−1 (𝑞)[−𝑛(𝑞, 𝑞̇ )] ,

𝑔2 (𝑥1 , 𝑥2 ) = 𝑀−1 (𝑞)

Robot Motion Control

➢ Desired:

Desired positions, 𝜇ⅆ (t)

Desired velocities, 𝜇̇ ⅆ (t)

Desired accelerations, 𝜇ⅆ̈ (t)

➢ Available:

Actual positions, 𝜇(t) and 𝑞 (t)

Actual velocities, 𝜇̇ (t) and 𝑞̇ (t)

Robot kinematics, μ = fun (𝑞), 𝑞 = 𝑓𝑢𝑛−1 (µ),

Jacobian matrix, 𝐽(𝑞), 𝐽−1 (𝑞)

29
Inertia matrix, M (𝑞), and n (𝑞, 𝑞̇ )

➢ To find:

■Control inputs, 𝜏

Cascade design

If the two sub-systems are independent, we can choose the following manner as the sub-system
error dynamics:

𝑒1̇ + 𝑘1 𝑒1 = 0

𝑒2̇ + 𝑘2 𝑒2 = 0

where,

𝑒1 = 𝜇ⅆ − 𝜇 , 𝑒2 = 𝑞ⅆ̇ − 𝑞̇

Considering only robot kinematics and the error of this sub-system can be given as:

𝑒1 = 𝑥1ⅆ − 𝑥1

𝑒̇1 = 𝑥̇ 1ⅆ − 𝑥̇ 1

where

𝑥̇ 1 = 𝐽(𝑞)𝑞̇

= 𝑔1 (𝑥1 )𝑥2

Considering x2 as an input to this sub-system and make the e1 tends to zero when the time t,
tends to infinity.

𝑥2 = 𝑔1−1 (𝑥1 )[𝑘1 (𝑥1ⅆ -𝑥1 ) + 𝑥̇ 1ⅆ ] − 𝑓1 (𝑥1 )

= 𝑔1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ] − 𝑓1 (𝑥1 )

But 𝑥2 is a state vector of the second sub-system and it is one of the system feedback and not a
control input, therefore, we consider 𝑥2ⅆ as a virtual control input vector for the first sub-system
and desired state variable to the second sub-system. Therefore, the error of the second sub-
system can be given as:

30
 𝑒2 = 𝑥2ⅆ − 𝑥2

𝑒̇2 = 𝑥̇ 2ⅆ − 𝑥̇ 2

Considering x2d as an input to the first sub-system and make the e1 tends to zero when the time
t, tends to infinity.

𝑥2ⅆ = 𝑔1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ] − 𝑓1 (𝑥1 )

We know that, the second sub-system is as:

𝑥̇ 2 = 𝑓2 (𝑥1 , 𝑥2 ) + 𝑔2 (𝑥1 , 𝑥2 )𝑢

By choosing a proper 𝑢, the error 𝑒; tends to zero when 𝑡 tends to infinity

𝑢 = 𝑔2−1 (𝑥1 , 𝑥2)[𝑘2 𝑒2 + 𝑥̇ 2ⅆ ] − 𝑓2 (𝑥1 , 𝑥2)

𝑥̇ 2ⅆ = 𝑔1−1 (𝑥1 )[𝑘1 𝑒̇1 + 𝑥̈ 1ⅆ ] − 𝑓1̇ (𝑥1 )+ 𝑔̇1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ]

However, in order to prove the closed-loop stability of the system,

the Lyapunov's direct method can be applied here.

Let us choose a Lyapunov's candidate function as follows:

1
𝑉(𝑒1 , 𝑒2 ) = 2[𝑒1𝑇 𝑒1 + 𝑒2𝑇 𝑒2 ] (18)

The time derivative of the above function along its state trajectories is given as:
1
𝑉̇ (𝑒1 , 𝑒2 ) = 2[𝑒1𝑇 𝑒̇1 + 𝑒2𝑇 𝑒̇2 ] (19)

31
But as per the Lyapunov's method; closed-loop system is

asymptotically stable only if V (𝑒1 , 𝑒2 ) > 0

𝑒̇1 = 𝑥̇ 1ⅆ − 𝑥̇ 1

= 𝑥̇ 1ⅆ −(𝑓1 (𝑥1 ) + 𝑔1 (𝑥1 )𝑥2 )

𝑥2 = 𝑥2ⅆ − 𝑒2

= 𝑔1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ] − 𝑓1 (𝑥1 ) - 𝑒2

𝑒̇1 = 𝑥̇ 1ⅆ −(𝑓1 (𝑥1 ) + 𝑔1 (𝑥1 )[ 𝑔1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ] − 𝑓1 (𝑥1 ) - 𝑒2 ]

= -𝑘1 𝑒1 + 𝑔1 (𝑥1 )𝑒2 (20)

𝑒̇2 = 𝑥̇ 2ⅆ − 𝑥̇ 2

= 𝑔1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̈ 1ⅆ ] − 𝑓1̇ (𝑥1 ) + 𝑔̇1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ] – [𝑓2 (𝑥1 , 𝑥2 )

+𝑔2 (𝑥1 , 𝑥2 )𝑢] (21)

𝑢 = 𝑔2−1 (𝑥1 , 𝑥2 )[𝑘2 𝑒2 + 𝑥̇ 2ⅆ ] − 𝑓2 (𝑥1 , 𝑥2 ) + 𝑔̇1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ]

+ 𝑔2−1 (𝑥1 , 𝑥2 )𝑔1𝑇 (𝑥1 ) 𝑒1 (22)

𝑒2̇ = −𝑘2 𝑒2 − 𝑔1𝑇 (𝑥1 ) 𝑒1

1
𝑉(𝑒1 , 𝑒2 ) = 2[𝑒1𝑇 𝑒1 + 𝑒2𝑇 𝑒2 ] ≥ 0

𝑉̇ (𝑒1 , 𝑒2 ) = [𝑒1𝑇 𝑒̇1 + 𝑒2𝑇 𝑒̇2 ]

= - [𝑒1𝑇 𝑘1 𝑒1 + 𝑒2𝑇 𝑘2 𝑒2]≤ 0

𝑢 = 𝑔2−1 (𝑥1 , 𝑥2 )[𝑔1𝑇 (𝑥1 )𝑒1 + 𝑘2 𝑒2 +𝑥̇ 2ⅆ ] − 𝑓2 (𝑥1 , 𝑥2 ) + 𝑔̇1−1 (𝑥1 )[𝑘1 𝑒1 + 𝑥̇ 1ⅆ ]

(23)

Now we will convert 𝝁 into 𝝉 because initially, we assumed 𝝁 is equal to 𝝉

32
G2-1(x1, x2) term is actually M, and gT1(x1)e1 term is J.

OUTPUT

Fig3.1(Output Cascade Control) Fig3.2(Output Cascade Control)

Here we have not considered the gravity term because if we consider gravity term then result will
be different.

g = 9.81; % gravity

Fig3.3(Output Without Gravity Compensation) Fig3.4

➢ Clearly this is not the expected output for 𝜃1 and 𝜃2

33
LIMITATION:

1. The open loop control shows trajectory uncertainty it does not follows the required path
as expected, it can be seen in the result that there is big deviation from the desired value
and obtained value.
2. For this we will be using the feedback control so to minimize the deviation or error.

COMPUTED TORQUE CONTROL

Inverse dynamics and feedback control are combined in this model-based control strategy to
provide control torques or forces that enable precise trajectory tracking and disturbance rejection.

𝜏 = 𝑀(𝑞)[𝑞̈ + 𝐾𝑝 (𝑞ⅆ − 𝑞) + 𝐾ⅆ (𝑞̇ ⅆ − 𝑞̇ )] + 𝑉(𝑞, 𝑞̇ ) + 𝑔(𝑞) (24)

1. We have assumed that the gravity term is included we have taken the two control gains
and initial joint velocity is not same as desired.

RESULT:

Fig 4.1(Output Computed Torque Control) Fig 4.2(Output Computed Torque Control)

34
 Fig 4.3(Output Computed Torque Control)

OBSERVATION:

1. Here we can see only problem is when we starts to include the frictional effect it will not
be able to compensate.

2. Computed torque control has other drawbacks as well, such as vulnerability to model
parameter mistakes and the presumption of an entirely reliable model. Control
performance may be impacted in real-world situations by changes to model parameters or
unmodeled dynamics.

PID CONTROL

𝜏 = 𝐾𝑝 (𝑞ⅆ − 𝑞) + 𝐾𝑖 ∫(𝑞ⅆ − 𝑞) + 𝐾ⅆ ( 𝑞ⅆ̇ − 𝑞̇ ) + 𝑔(𝑞) (25)

RESULT:

35
 Fig 4.4(Output PID Control) Fig 4.5(Output PID Control)

Fig4.6(Output PID Control)

OBSERVATION:

1. Here we can see the error approaches almost zero for both the joint positions and also we
have compensated the gravity term from it using the gravity function

36
 CHAPTER-5

INVERSE DYNAMICS IN TASK SPACE

The ability to precisely regulate the motion of the end effector is made possible by
inverse dynamics in task space, which offers a method of task-oriented behavior control
for the robotic system. It enables the system to carry out challenging tasks including force
control, impedance control, trajectory tracking, or position control.

F = Mμ 𝜇̈ ⅆ + 𝑛𝜇 (𝜇ⅆ, 𝜇̇ ⅆ ) (26)

RESULT:

Fig 5.1(Inverse Dynamics in Task Space) Fig 5.2(Inverse Dynamics in Task Space)

37
 CHAPTER-6

Motion Control

For Motion control (PD controller)

A common feedback control technique for achieving desired motion or trajectory tracking in
robotics and control systems is motion control utilizing a PD (Proportional-Derivative)
controller. The error between the desired motion and the actual motion of the system is used by
the PD controller, a form of linear controller, to generate control signals.

Proportional term: The PD controller's proportional term produces a control signal that is
proportional to the error. It makes the problem worse and offers a quick fix to make it less
noticeable. The control signal is calculated as the proportional gain (Kp) multiplied by the
current error.

Control signal (Proportional term): Kp * error

Derivative term: The PD controller's derivative term produces a control signal based on how
quickly the error is changing. It aids in predicting the error's future behaviour and helps dampen
the reaction to lessen oscillations. The derivative gain (Kd) and the rate of error change are
multiplied to provide the control signal.

The derivative term for the control signal is Kd * (d/dt error).

̇
𝜏 = 𝐽(𝑞)𝑇 [𝑀𝜇 [𝜇̈ ⅆ + 𝛤1 𝜇̃̇ + 𝛤2 𝜇̃] + 𝑛𝜇 (𝜇, 𝜇)] (27)

38
RESULT

Fig 5.3(Motion Control (PD Controller) Fig 5.4(Motion Control (PD Controller)

Observation:

1. It is starting from some initial position and taking the whole path.

2. Error is significantly reduced, but the problem is other effects are not compensated. For
example, we include the gravity term effects will be seen. So we can go with PD control
with gravity compensation.

39
PD control with gravity compensation

τ = J(𝑞)𝑇 [𝑘𝑝 (𝑞ⅆ − 𝑞) + 𝑘ⅆ (𝑞̇ ⅆ − 𝑞̇ )] + 𝑔(𝑞) (28)

RESULT:

Fig 5.5(Motion Control (With Gravity Compensation) Fig 5.6

Observation:

1. We can see the gravity term is compensated so in industry we can use PD control
manipulator for slow speed.

2. But the problem is some kind of hitch up and down are still coming that is the constraint
of jacobian that we can go for PID control

40
PID Control

τ = J(𝑞)𝑇 [𝑘𝑝 (𝑞ⅆ − 𝑞) + ∫ (𝑞ⅆ − 𝑞)ⅆ𝑡 + 𝑘ⅆ (𝑞̇ ⅆ − 𝑞̇ )] + 𝑔(𝑞) (29)

RESULT:

Fig 6.1(Motion Control PID Controller)

Fig 6.2(Motion Control PID Controller)

41
OBSERVATION:

1. We can see the output is more or less same as PD controller but due to integral action end
up with winding up.
2. PID control is working fine without gravity compensation but result is still not accurate
and steady state error is there.

42
 Appendix

1. MATLAB code for finding 𝜏1 , 𝜏2

%%Code for Finding VC01 and VC02

% Fig.

clear all; clc

syms t real
% variables
syms th1(t) th2(t) a1 a2 g real
PC01 = [a1*cos(th1);
a1*sin(th1);
0];
PC02 = [a2*cos(th1+ th2) + a1*cos(th1);
a2*sin(th1+ th2) + a1*sin(th1);
0];

VC01 = diff(PC01,t)
VC02 = diff (PC02,t)

%%Code for finding dhoL_dho_th1dot, dhoL_dho_th2dot

clear all; clc;

syms th1dot th2dot m1 m2 th1 th2 a1 a2 g real

PC01 = [a1*cos(th1);
a1*sin(th1);
0];
PC02 = [a2*cos(th1+ th2) + a1*cos(th1);
a2*sin(th1+ th2) + a1*sin(th1);
0];

VC01 = [-a1*sin(th1)*th1dot;
a1*cos(th1*th1dot)
0];
VC02 = [- a2*sin(th1 + th2)*(th1dot + th2dot) - a1*sin(th1)*(th1dot)
a2*cos(th1 + th2)*(th1dot + th2dot) + a1*cos(th1)*(th1dot)
0];

KE = 1/2 * (m1*VC01'*VC01+m2*VC02'*VC02);
PE = m1*g*PC01(2) + m2*g*PC02(2);

43
L = KE - PE;
dhoL_dho_th1 = diff (L, th1)
dhoL_dho_th2 = diff (L, th2)

dhoL_dho_th1dot = diff (L, th1dot)

dhoL_dho_th2dot = diff (L, th2dot)

tau1 = d_dhoL_dho_th1dot_dt – dhol_dho_th1

tau2 = d_dhoL_dho_th2dot_dt – dhol_dho_th2

%%RESULT

Tau1 = a1^2*m1*th1ddot + a1^2*m2*th1ddot + a2^2*m2*th1ddot

a2^2*m2*th2ddot + a2*g*m1*cos (th1 + th2) +

2*a1*g*m1*cos(th1) - a1*a2*m2*th2do^2*sin(th2) +
2*a1*a2*m2*th1ddot*cos (th2) + a1*a2*m2*th2ddot*cos(th2) -
2*a1*a2*m2*th1dot*th2dot*sin(th2)

Tau2 = a2* (a1*m2*sin (th2) *th1do^2 + a2*m2*th1ddot + a2*m2*th2ddot

+ g*m1*cos (th1 + th2) + a1*m2*th1ddot*cos (th2))

MATLAB CODE (Kinematic control)

• With error

44
(i) % Start
(ii) clear all; close all; clc;
(iii)
(iv) tf = 8; % trajectory duration
(v) dt = 0.1; % step size
(vi)
(vii) t = 0:dt:tf; % time span
(viii) global L1 L2
(ix) L1 =1; L2 = 1;
(x)
(xi) %% Joint positions using inverse kinematics
(xii) x0 = 1; y0 = 1;
(xiii)
(xiv) xf = -1; yf = 1.5;
(xv)
(xvi) xdot0 = 0; ydot0 = 0;
(xvii) xdotf = 0; ydotf = 0;
(xviii)
(xix) [th10,th20] = IK2R(x0,y0);
(xx) [th1f,th2f] = IK2R(xf,yf);
(xxi)
(xxii) J0 = Jaco2R(th10,th20);
(xxiii) Jf = Jaco2R(th1f,th2f);
(xxiv)
(xxv) qdot0 = inv(J0)*[xdot0;ydot0];
(xxvi) qdotf = inv(Jf)*[xdotf;ydotf];
(xxvii) th1dot0 = qdot0(1); th1dotf = qdotf(1);
(xxviii) th2dot0 = qdot0(2); th2dotf = qdotf(2);
(xxix)
(xxx) tc1 = Cubic_TR(th10, th1dot0, th1f, th1dotf,tf);
(xxxi) tc2 = Cubic_TR(th20,th2dot0, th2f, th2dotf, tf);
(xxxii) %% Initial conditions
(xxxiii)
(xxxiv) q(:,1) = [th10-0.1;th20-0.1];
(xxxv)
(xxxvi) %% Numerical Integration starts here
(xxxvii) t = 0:dt:tf;
(xxxviii) for i=1:length(t)
(xxxix) %% Desired Joint-space positions
(xl) th1_desired = [1,t(i),t(i)^2,t(i)^3]*tc1;
(xli) th2_desired = [1,t(i),t(i)^2,t(i)^3]*tc2;
(xlii) q_desired(:,i) = [th1_desired;th2_desired];
(xliii)
(xliv) %% Desired Joint-space velocites
(xlv) th1_dot_desired = [0,1,2*t(i),3*t(i)^2]*tc1;
(xlvi) th2_dot_desired = [0,1,2*t(i),3*t(i)^2]*tc2;

45
(xlvii) q_dot_desired(:,i) = [th1_dot_desired;th2_dot_desired];
(xlviii)
(xlix) %% Inverse differential kinematic model
(l) q_dot(:,i) = q_dot_desired(:,i);
(li) q(:,i+1) = q(:,i)+q_dot(:,i)*dt;

(lii) %% Forward kinematics

(liii) th1(i) = q(1,i); th2(i) = q(2,i);
(liv) [x(i),y(i)] = FK2R(th1(i),th2(i));
(lv) [xd(i),yd(i)] = FK2R(th1_desired,th2_desired);
(lvi) end
(lvii)
(lviii) figure % manipulator motion animation
(lix) for i=1:length(t)
(lx) plot ([0,L1*cos(th1(i)),x(i)], [0,
L1*sin(th1(i)),y(i)],'r-o','linewidth',2)
(lxi) hold on
(lxii) plot (xd,yd, 'k--', 'linewidth',1) % trajectory
(lxiii) plot (x(1:i),y(1:i), 'b-', 'linewidth',1) % trajectory
(lxiv) plot (xd(1),yd(1), 'rs', 'markersize',10) % starting
point
(lxv) plot(xd(length(t)),yd(length(t)),'gp', 'markersize',10)
%final point
(lxvi) set(gca,'fontsize',12, 'fontname', 'Times');
(lxvii) xlabel('x, [units]');ylabel('y, [units]');
(lxviii) axis([-(L1+L2)-0.1 (L1+L2)+0.1 -(L1+L2)-0.1
(L1+L2)+0.1]);
(lxix) grid on, axis square, pause(0.01), hold off
(lxx) end

46
OUTPUT

MATLAB CODE( Kinematic control)

• With proportional controller

% Start
clear all; close all; clc;

tf = 8; % trajectory duration
dt = 0.1; % step size

t = 0:dt:tf; % time span

global L1 L2
L1 =1; L2 = 1;

%% Joint positions using inverse kinematics

x0 = 1; y0 = 1;

47
xf = -1; yf = 1.5;

xdot0 = 0; ydot0 = 0;
xdotf = 0; ydotf = 0;

[th10,th20] = IK2R(x0,y0);
[th1f,th2f] = IK2R(xf,yf);

J0 = Jaco2R(th10,th20);
Jf = Jaco2R(th1f,th2f);

qdot0 = inv(J0)*[xdot0;ydot0];
qdotf = inv(Jf)*[xdotf;ydotf];
th1dot0 = qdot0(1); th1dotf = qdotf(1);
th2dot0 = qdot0(2); th2dotf = qdotf(2);

tc1 = Cubic_TR(th10, th1dot0, th1f, th1dotf,tf);

tc2 = Cubic_TR(th20,th2dot0, th2f, th2dotf, tf);
%% Initial conditions

q(:,1) = [th10-0.1;th20-0.1];
%% control parameter
lambda = 4;
%% Numerical Integration starts here
t = 0:dt:tf;
for i=1:length(t)
%% Desired Joint-space positions
th1_desired = [1,t(i),t(i)^2,t(i)^3]*tc1;
th2_desired = [1,t(i),t(i)^2,t(i)^3]*tc2;
q_desired(:,i) = [th1_desired;th2_desired];

%% Desired Joint-space velocites

th1_dot_desired = [0,1,2*t(i),3*t(i)^2]*tc1;
th2_dot_desired = [0,1,2*t(i),3*t(i)^2]*tc2;
q_dot_desired(:,i) = [th1_dot_desired;th2_dot_desired];

%% Joint posotion errors

q_tilda(:,i) = q_desired(:,i)-q(:,i);

%% kinematic control based on computing velocity control

q_dot(:,i) = (q_dot_desired(:,i)+lambda*q_tilda(:,i));
q(:,i+1) = q(:,i)+q_dot(:,i)*dt;

%% Forward kinematics

48
 th1(i) = q(1,i); th2(i) = q(2,i);
[x(i),y(i)] = FK2R(th1(i),th2(i));
[xd(i),yd(i)] = FK2R(th1_desired,th2_desired);
end

figure % manipulator motion animation

for i=1:length(t)
plot ([0,L1*cos(th1(i)),x(i)], [0, L1*sin(th1(i)),y(i)],'r-
o','linewidth',2)
hold on
plot (xd,yd, 'k--', 'linewidth',1) % trajectory
plot (x(1:i),y(1:i), 'b-', 'linewidth',1) % trajectory
plot (xd(1),yd(1), 'rs', 'markersize',10) % starting point
plot(xd(length(t)),yd(length(t)),'gp', 'markersize',10) %final
point
set(gca,'fontsize',12, 'fontname', 'Times');
xlabel('x, [units]');ylabel('y, [units]');
axis([-(L1+L2)-0.1 (L1+L2)+0.1 -(L1+L2)-0.1 (L1+L2)+0.1]);
grid on, axis square, pause(0.01), hold off
end

Feedforward control (Inverse dynamics)

MATLAB CODE

%% Inverse dynamic simulation of a RR planar robot

clear all; close all; clc;

%% Simulation parameters

dt = 0.01; % stepsize
ts = 30; % total simulation time
t = 0:dt:ts; % time span
global a1 a2 m1 m2 g

%% System parameters

m1 = 2; m2 = 1; % link massess
a1 = 0.5; a2 = 0.4; % link lengths
g = 0*9.81; % gravity

49
%% Initial conditions
q = [0;pi/3-pi/4]; % initial joint positions
q_dot = [0.2*pi/2;0]; % initial joint velocities

%% Numerical integration starts here

for i = 1:length(t)
%% Desired values

q_desired(:,i) = [pi/2*sin(0.2*t(i)); pi/3-pi/4*cos(0.5*t(i))];

q_dot_desired(:,i) = [0.2*pi/2*cos(0.2*t(i));...
0.5*pi/4*sin(0.5*t(i))];
q_double_dot_desired(:,i) = [-0.2^2*pi/2*sin(0.2*t(i));...
0.5^2*pi/4*cos(0.5*t(i))];

Md = inertia2R(q_desired(:,i));
oe_vd = other_effects2R(q_desired(:,i),q_dot_desired(:,i));
g_vd = gravity_effects2R(q_desired(:,i));
M = inertia2R(q(:,i));
oe_v = other_effects2R(q(:,i),q_dot(:,i));
g_v = gravity_effects2R(q(:,i));

%% Inverse dynamics
%input vector
tau(:,i) = Md*(q_double_dot_desired(:,i))...
+ oe_vd + g_vd;

% acceleration vector
q_double_dot(:,i) = inv(M)*(tau(:,i)-(oe_v+g_v));

% velocity propogation
q_dot(:,i+1) = q_dot(:,i) + q_double_dot(:,i)*dt;

% position update
q(:,i+1) = q(:,i) +q_dot(:,i)*dt + 1/2*q_double_dot(:,i)*dt^2;
[x(i),y(i)] = FK2R(q(1,i),q(2,i));

end

%% Animation
for i=1:10:length(t)
x1 = a1*cos(q(1,i));
y1 = a1*sin(q(1,i));
x2 = x1+a2*cos(q(1,i)+q(2,i));

50
 y2 = y1+a2*sin(q(1,i)+q(2,i));
plot([0,x1,x2],[0,y1,y2],'r-o','linewidth',2)
grid on, set(gca,'fontsize',20)
hold on
plot(x(1:i),y(1:i),'m-')
axis([-(a1+a2)-0.1, (a1+a2)+0.1,-(a1+a2)-0.1,(a1+a2)+0.1]);
axis square, xlabel('t,[s]'),ylabel('q, [units]')
hold off, pause(0.001)
end

%% Plotting functions
plot(t,q(1,1:i),'r-.',t,q(2,1:i),'b-','linewidth',2)
legend('\theta_1', '\theta_2')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q, [units]')

%% Creating functions for the same

function M = inertia2R(q)
global a1 a2 m1 m2
th1 = q(1); th2 = q(2);

% Inertia matrix
m11 = a1^2*m1 + a1^2*m2 + a2^2*m2 + 2*a1*a2*m2*cos(th2);
m21 = a2*m2*(a2 + a1*cos(th2));
m12 = a2*m2*(a2 + a1*cos(th2));
m22 = a2^2*m2;
M =[m11,m12;m21,m22] ;

end

function oe_v = other_effects2R(q,qdot)

global a1 a2 m1 m2
th1 = q(1); th2 = q(2);
th1dot = qdot(1); th2dot = qdot(2);
%other effects
oe_v1 = -a1*a2*m2*th2dot*sin(th2)*(2*th1dot + th2dot);
oe_v2 = a1*a2*m2*th1dot^2*sin(th2);
oe_v=[oe_v1;oe_v2];
end

51
function g_v = gravity_effects2R(q)
global a1 a2 m1 m2 g
th1 = q(1); th2 = q(2);
%gravity effects
g1 = a2*m2*cos(th1 + th2) + a1*m1*cos(th1) + a1*m2*cos(th1);
g2 = a2*m2*cos(th1 + th2);
g_v=g*[g1;g2];
end

function J = Jaco2R(th1,th2)
global a1 a2

J = [-a1*sin(th1)-a2*sin(th1+th2),-a2*sin(th1+th2);
+a1*cos(th1)+a2*cos(th1+th2),+a2*cos(th1+th2);];
end

function [x,y] = FK2R(th1,th2)

global a1 a2
x = a1*cos(th1)+a2*cos(th1+th2);
y = a1*sin(th1)+a2*sin(th1+th2);
end

function [th1,th2] = IK2R(x,y)

global a1 a2
c2 = (x^2+y^2-a1^2-a2^2)/(2*a1*a2);
s2 = sqrt(1-c2^2);
th1 = atan2(y,x)-atan2(a2*s2,a1+a2*c2);
th2 = atan2(s2,c2);
end

function tc = Cubic_TR(x0,xdot0,xf,xdotf,tf)
A = [1,0,0,0;
0,1,0,0;
1,tf,tf^2,tf^3;
0,1,2^tf ,3*tf^2];
b = [x0;xdot0;xf;xdotf];
tc = inv(A)*b;
end

52
MATLAB CODE(Computed torque control):

%% Inverse dynamic simulation of a RR planar robot

clear all; close all; clc;

%% Simulation parameters

dt = 0.01; % stepsize
ts = 30; % total simulation time
t = 0:dt:ts; % time span
global a1 a2 m1 m2 g b1 b2 c1 c2

%% System parameters

m1 = 2; m2 = 1; % link massess
a1 = 0.5; a2 = 0.4; % link lengths
g = 9.81; % gravity
b1 = 0.5;b2 = 0.5;c1 = 0*0.5;c2 = 0*0.5;

%% Initial conditions

q = [0;pi/12]; % initial joint positions

q_dot = [0;0]; % initial joint velocities

%% Control parameter
Kp = 4;Kd = 4;

%% Numerical integration starts here

for i = 1:length(t)
%% Desired values
q_desired(:,i) = [pi/2*sin(0.2*t(i));...
pi/3-pi/4*cos(0.5*t(i))];
q_dot_desired(:,i) = [0.2*pi/2*cos(0.2*t(i));...
0.5*pi/4*sin(0.5*t(i))];
q_double_dot_desired(:,i) = [-0.2^2*pi/2*sin(0.2*t(i));...
0.5^2*pi/4*cos(0.5*t(i))];

%Md = inertia2R(q(:,i));
%oe_vd = other_effects2R(q(:,i),q_dot_desired(:,i));
%g_vd = gravity_effects2R(q(:,i));
M = inertia2R(q(:,i));
oe_v = other_effects2R(q(:,i),q_dot(:,i));
g_v = gravity_effects2R(q(:,i));

%% Errors

53
 q_tilda(:,i) = q_desired(:,i) - q(:,i);
q_dot_tilda(:,i) = q_dot_desired(:,i) - q_dot(:,i);
%% Computed-torque control
%input vector
tau(:,i) = M*(q_double_dot_desired(:,i) ...
+ Kp*q_tilda(:,i) + Kd*q_dot_tilda(:,i))...
+ oe_v + g_v;

% acceleration vector
q_double_dot(:,i) = inv(M)*(tau(:,i)-(oe_v+g_v));

% velocity propogation
q_dot(:,i+1) = q_dot(:,i) + q_double_dot(:,i)*dt;

% position update
q(:,i*1) = q(:,i) +q_dot(:,i)*dt + 1/2*q_double_dot(:,i)*dt^2;
[x(i),y(i)] = FK2R(q(1,i),q(2,i));

end

%% Animation
for i=1:10:length(t)
x1 = a1*cos(q(1,i));
y1 = a1*sin(q(1,i));
x2 = x1+a2*cos(q(1,i)+q(2,i));
y2 = y1+a2*sin(q(1,i)+q(2,i));
plot([0,x1,x2],[0,y1,y2],'r-o','linewidth',2)
grid on, set(gca,'fontsize',20)
hold on
plot(x(1:i),y(1:i),'m-')
axis([-(a1+a2)-0.1, (a1+a2)+0.1,-(a1+a2)-0.1,(a1+a2)+0.1]);
axis square, xlabel('t,[s]'),ylabel('q, [units]')
hold off, pause(0.001)
end

%% Plotting functions
plot(t,q(1,1:i),'r-.',t,q(2,1:i),'b-','linewidth',2)
legend('\theta_1', '\theta_2')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q, [units]')

54
figure
plot(t,q_tilda(1,1:i),'r-.',t,q_tilda(2,1:i),'b-','linewidth',2)
legend('\theta_1_e', '\theta_2_e')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('{q},[units]')

MATLAB CODE(PID Control)

%% Inverse dynamic simulation of a RR planar robot

clear all; close all; clc;

%% Simulation parameters

dt = 0.01; % stepsize
ts = 15; % total simulation time
t = 0:dt:ts; % time span
global a1 a2 m1 m2 g b1 b2 c1 c2

%% System parameters

m1 = 2; m2 = 1; % link massess
a1 = 0.5; a2 = 0.4; % link lengths
g = 9.81; % gravity
b1 = 0.5;b2 = 0.5;c1 = 0*0.5;c2 = 0*0.5;

%% Initial conditions

q = [0;pi/3-pi/12]; % initial joint positions

q_dot = [0;0]; % initial joint velocities

%% Control parameter
Kp = 4;Kd = 4;Ki = 4;ei = [0;0];

55
%% Numerical integration starts here

for i = 1:length(t)
%% Desired values
q_desired(:,i) = [pi/2*sin(0.2*t(i));...
pi/3-pi/4*cos(0.5*t(i))];
q_dot_desired(:,i) = [0.2*pi/2*cos(0.2*t(i));...
0.5*pi/4*sin(0.5*t(i))];
q_double_dot_desired(:,i) = [-0.2^2*pi/2*sin(0.2*t(i));...
0.5^2*pi/4*cos(0.5*t(i))];

%Md = inertia2R(q(:,i));
%oe_vd = other_effects2R(q(:,i),q_dot_desired(:,i));
%g_vd = gravity_effects2R(q(:,i));
M = inertia2R(q(:,i));
oe_v = other_effects2R(q(:,i),q_dot(:,i));
g_v = gravity_effects2R(q(:,i));

%% Errors
q_tilda(:,i) = q_desired(:,i) - q(:,i);
q_dot_tilda(:,i) = q_dot_desired(:,i) - q_dot(:,i);
ei = ei + q_tilda(:,i)*dt;
%% PID control
%input vector
tau(:,i) = (Kp*q_tilda(:,i) + Kd*q_dot_tilda(:,i)+Ki*ei)+ g_v;

% acceleration vector
q_double_dot(:,i) = inv(M)*(tau(:,i)-(oe_v+g_v));

% velocity propogation
q_dot(:,i+1) = q_dot(:,i) + q_double_dot(:,i)*dt;

% position update
q(:,i+1) = q(:,i) +q_dot(:,i)*dt + 1/2*q_double_dot(:,i)*dt^2;
[x(i),y(i)] = FK2R(q(1,i),q(2,i));

end

%% Animation
for i=1:10:length(t)
x1 = a1*cos(q(1,i));
y1 = a1*sin(q(1,i));
x2 = x1+a2*cos(q(1,i)+q(2,i));

56
 y2 = y1+a2*sin(q(1,i)+q(2,i));
plot([0,x1,x2],[0,y1,y2],'r-o','linewidth',2)
grid on, set(gca,'fontsize',20)
hold on
plot(x(1:i),y(1:i),'m-')
axis([-(a1+a2)-0.1, (a1+a2)+0.1,-(a1+a2)-0.1,(a1+a2)+0.1]);
axis square, xlabel('t,[s]'),ylabel('q, [units]')
hold off, pause(0.001)
end

%% Plotting functions
plot(t,q(1,1:i),'r-.',t,q(2,1:i),'b-','linewidth',2)
legend('\theta_1', '\theta_2')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q, [units]')

figure
plot(t,q_tilda(1,1:i),'r-.',t,q_tilda(2,1:i),'b-','linewidth',2)
legend('\theta_1_e', '\theta_2_e')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q,[units]')

MATLAB code( Inverse Dynamics in task space)

%% Inverse dynamic simulation of a RR planar robot

clear all; close all; clc;

%% Simulation parameters

dt = 0.01; % stepsize
ts = 30; % total simulation time
t = 0:dt:ts; % time span
global a1 a2 m1 m2 g

57
%% System parameters

m1 = 2; m2 = 1; % link massess
a1 = 0.5; a2 = 0.4; % link lengths
g = 0*9.81; % gravity
b1 = 0.5;b2 = 0.5;c1 = 0*0.5;c2 = 0*0.5;

%% Initial conditions

%q = [-pi/4;pi/3]; % initial joint positions

%q_dot = [0;0]; % initial joint velocities

%% Numerical integration starts here

for i = 1:length(t)
%% Desired values
mu_desired(:,i) = [0.4+0.3*sin(0.2*t(i));...
0.4-0.3*cos(0.2*t(i))];
mu_dot_desired(:,i) = [0.3*0.2*cos(0.2*t(i));...
0.3*0.2*sin(0.2*t(i))];
mu_double_dot_desired(:,i) = [-0.3^0.2^2*sin(0.2*t(i));...
0.3*0.2^2*cos(0.2*t(i))];

[q_desired(1,i),q_desired(2,i)] =
IK2R(mu_desired(1,i),mu_desired(2,i));
Jd = Jaco2R(q_desired(1,i),q_desired(2,i));
q_dot_desired(:,i) = inv(Jd)*mu_dot_desired(:,i);

Md = inertia2R(q_desired(:,i));
oe_vd = other_effects2R(q_desired(:,i),q_dot_desired(:,i));
g_vd = gravity_effects2R(q_desired(:,i));
Jdotd = Jacodot2R(q_desired(:,i),q_dot_desired(:,i));

M_mu_d = inv(Jd')*Md*inv(Jd);
n_mu_d = inv(Jd')*(oe_vd+g_vd...
-Md*inv(Jd)*Jdotd*q_dot_desired(:,i));

%% Initial conditions
%q = [-pi/4;pi/3]; % initial joint positions
%q_dot = [0;0]; % initial joint velocities
q(:,1) = q_desired(:,1);
q_dot(:,1) = q_dot_desired(:,1);

58
 M = inertia2R(q(:,i));
oe_v = other_effects2R(q(:,i),q_dot(:,i));
g_v = gravity_effects2R(q(:,i));

%% Errors
%q_tilda(:,i) = q_desired(:,i) - q(:,i);
%q_dot_tilda(:,i) = q_dot_desired(:,i) - q_dot(:,i);
%% Computed-torque control
%input vector
%tau(:,i) = M*(mu_double_dot_desired(:,i) ...
%+ Kp*q_tilda(:,i) + Kd*q_dot_tilda(:,i))...
%+ oe_v + g_v;

%% Inverse dynamics
%input vector
F(:,i) = M_mu_d*(mu_double_dot_desired(:,i))...
+n_mu_d;
tau(:,i) = Jd'*F(:,i);

% acceleration vector
q_double_dot(:,i) = inv(M)*(tau(:,i)-(oe_v+g_v));

% velocity propogation
q_dot(:,i+1) = q_dot(:,i) + q_double_dot(:,i)*dt;

% position update
q(:,i+1) = q(:,i) +q_dot(:,i)*dt...
+ 1/2*q_double_dot(:,i)*dt^2;

[x(i),y(i)] = FK2R(q(1,i),q(2,i));

end

%% Animation
for i=1:10:length(t)
x1 = a1*cos(q(1,i));
y1 = a1*sin(q(1,i));
x2 = x1+a2*cos(q(1,i)+q(2,i));
y2 = y1+a2*sin(q(1,i)+q(2,i));
plot([0,x1,x2],[0,y1,y2],'r-o','linewidth',2)
grid on, set(gca,'fontsize',20)
hold on
plot(x(1:i),y(1:i),'m-')
axis([-(a1+a2)-0.1, (a1+a2)+0.1,-(a1+a2)-0.1,(a1+a2)+0.1]);

59
 axis square, xlabel('t,[s]'),ylabel('q, [units]')
hold off, pause(0.001)
end

%% Plotting functions
plot(t,q(1,1:i),'r-.',t,q(2,1:i),'b-','linewidth',2)
legend('\theta_1', '\theta_2')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q, [units]')

MATLAB Code( For motion control)

PD controller

%% Inverse dynamic simulation of a RR planar robot

clear all; close all; clc;

%% Simulation parameters

dt = 0.01; % stepsize
ts = 30; % total simulation time
t = 0:dt:ts; % time span
global a1 a2 m1 m2 g

%% System parameters

m1 = 2; m2 = 1; % link massess
a1 = 0.5; a2 = 0.4; % link lengths
g = 0*9.81; % gravity
b1 = 0.5;b2 = 0.5;c1 = 0*0.5;c2 = 0*0.5;

%% Initial conditions

q = [-pi/4;pi/3]; % initial joint positions

q_dot = [0;0]; % initial joint velocities

%% Control parameters
Kp = 4; Kd = 4;

60
%% Numerical integration starts here

for i = 1:length(t)
%% Desired values
mu_desired(:,i) = [0.4+0.3*sin(0.2*t(i));...
0.4-0.3*cos(0.2*t(i))];
mu_dot_desired(:,i) = [0.3*0.2*cos(0.2*t(i));...
0.3*0.2*sin(0.2*t(i))];
mu_double_dot_desired(:,i) = [-0.3^0.2^2*sin(0.2*t(i));...
0.3*0.2^2*cos(0.2*t(i))];

[q_desired(1,i),q_desired(2,i)] =
IK2R(mu_desired(1,i),mu_desired(2,i));
Jd = Jaco2R(q_desired(1,i),q_desired(2,i));
q_dot_desired(:,i) = inv(Jd)*mu_dot_desired(:,i);

Md = inertia2R(q_desired(:,i));
oe_vd = other_effects2R(q_desired(:,i),q_dot_desired(:,i));
g_vd = gravity_effects2R(q_desired(:,i));
Jdotd = Jacodot2R(q_desired(:,i),q_dot_desired(:,i));

M_mu_d = inv(Jd')*Md*inv(Jd);
n_mu_d = inv(Jd')*(oe_vd+g_vd...
-Md*inv(Jd)*Jdotd*q_dot_desired(:,i));

%% Initial conditions
%q(:,1) = q_desired(:,1);
%q_dot(:,1) = q_dot_desired(:,1);

M = inertia2R(q(:,i));
oe_v = other_effects2R(q(:,i),q_dot(:,i));
g_v = gravity_effects2R(q(:,i));

%% Errors
mu_tilda(:,i) = mu_desired(:,i) - mu(:,i);
mu_dot_tilda(:,i) = mu_dot_desired(:,i) - J*q_dot(:,i);
%% Computed-torque control
%input vector
F(:,i) = M_mu_d*(mu_double_dot_desired(:,i) ...
+ Kp*mu_tilda(:,i) + Kd*mu_dot_tilda(:,i))...
+ n_mu_d;

tau(:,i) = Jd'*F(:,i);

61
 %% Inverse dynamics
%input vector
tau(:,i) = Md*(inv(Jd)*(mu_double_dot_desired(:,i)...
-Jdotd*q_dot_desired(:,i)))...
+oe_vd+g_vd;
% acceleration vector
q_double_dot(:,i) = inv(M)*(tau(:,i)-(oe_v+g_v));

% velocity propogation
q_dot(:,i+1) = q_dot(:,i) + q_double_dot(:,i)*dt;

% position update
q(:,i*1) = q(:,i) +q_dot(:,i)*dt + 1/2*q_double_dot(:,i)*dt^2;
[x(i),y(i)] = FK2R(q(1,i),q(2,i));

end

%% Animation
for i=1:10:length(t)
x1 = a1*cos(q(1,i));
y1 = a1*sin(q(1,i));
x2 = x1+a2*cos(q(1,i)+q(2,i));
y2 = y1+a2*sin(q(1,i)+q(2,i));
plot([0,x1,x2],[0,y1,y2],'r-o','linewidth',2)
grid on, set(gca,'fontsize',20)
hold on
plot(x(1:i),y(1:i),'m-')
axis([-(a1+a2)-0.1, (a1+a2)+0.1,-(a1+a2)-0.1,(a1+a2)+0.1]);
axis square, xlabel('t,[s]'),ylabel('q, [units]')
hold off, pause(0.001)
end

%% Plotting functions
plot(t,q(1,1:i),'r-.',t,q(2,1:i),'b-','linewidth',2)
legend('\theta_1', '\theta_2')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q, [units]')

Adding this to previous code in error section.

%% Errors

62
 mu_tilda(:,i) = mu_desired(:,i) - mu(:,i);
mu_dot_tilda(:,i) = mu_dot_desired(:,i) - J*q_dot(:,i);
%% Computed-torque control
%input vector
F(:,i) = Kp*mu_tilda(:,i) + Kd*mu_dot_tilda(:,i);
tau(:,i) = Jd'*F(:,i)+g_v;

PID Control

%% Inverse dynamic simulation of a RR planar robot

clear all; close all; clc;

%% Simulation parameters

dt = 0.01; % stepsize
ts = 30; % total simulation time
t = 0:dt:ts; % time span
global a1 a2 m1 m2 g

%% System parameters

m1 = 2; m2 = 1; % link massess
a1 = 0.5; a2 = 0.4; % link lengths
g = 9.81; % gravity
b1 = 0.5;b2 = 0.5;c1 = 0*0.5;c2 = 0*0.5;

%% Initial conditions

q = [-pi/4;pi/3]; % initial joint positions

q_dot = [0;0]; % initial joint velocities

%% Control parameters
Kp = 16; Kd = 8; Ki = 0.5; ei = [0:0];

%% Numerical integration starts here

for i = 1:length(t)

63
 %% Desired values
mu_desired(:,i) = [0.4+0.3*sin(0.2*t(i));...
0.4-0.3*cos(0.2*t(i))];
mu_dot_desired(:,i) = [0.3*0.2*cos(0.2*t(i));...
0.3*0.2*sin(0.2*t(i))];
mu_double_dot_desired(:,i) = [-0.3^0.2^2*sin(0.2*t(i));...
0.3*0.2^2*cos(0.2*t(i))];

[q_desired(1,i),q_desired(2,i)] =
IK2R(mu_desired(1,i),mu_desired(2,i));
Jd = Jaco2R(q_desired(1,i),q_desired(2,i));
q_dot_desired(:,i) = inv(Jd)*mu_dot_desired(:,i);

Md = inertia2R(q_desired(:,i));
oe_vd = other_effects2R(q_desired(:,i),q_dot_desired(:,i));
g_vd = gravity_effects2R(q_desired(:,i));
Jdotd = Jacodot2R(q_desired(:,i),q_dot_desired(:,i));

M_mu_d = inv(Jd')*Md*inv(Jd);
n_mu_d = inv(Jd')*(oe_vd+g_vd...
-Md*inv(Jd)*Jdotd*q_dot_desired(:,i));

%% Initial conditions
%q(:,1) = q_desired(:,1);
%q_dot(:,1) = q_dot_desired(:,1);

M = inertia2R(q(:,i));
oe_v = other_effects2R(q(:,i),q_dot(:,i));
g_v = gravity_effects2R(q(:,i));

%% Errors %
mu_tilda(:,i) = mu_desired(:,i) - mu(:,i);
mu_dot_tilda(:,i) = mu_dot_desired(:,i)...
- Jxq_dot(:,i);
ei = ei + mu_tilda(:,i)*dt;
%% PID control
%input vector in task-space %
F(:,1) = Kp*mu_tilda(:,i)+Ki*ei+Kd*mu_dot_tilda(:,i) ;
% input vector in joint-space
tau(:,i) = JA'*F(:,i)+g_v;

%% Inverse dynamics
%input vector

64
 tau(:,i) = Md*(inv(Jd)*(mu_double_dot_desired(:,i)...
-Jdotd*q_dot_desired(:,i)))...
+oe_vd+g_vd;
% acceleration vector
q_double_dot(:,i) = inv(M)*(tau(:,i)-(oe_v+g_v));

% velocity propogation
q_dot(:,i+1) = q_dot(:,i) + q_double_dot(:,i)*dt;

% position update
q(:,i*1) = q(:,i) +q_dot(:,i)*dt + 1/2*q_double_dot(:,i)*dt^2;
[x(i),y(i)] = FK2R(q(1,i),q(2,i));

end

%% Animation
for i=1:10:length(t)
x1 = a1*cos(q(1,i));
y1 = a1*sin(q(1,i));
x2 = x1+a2*cos(q(1,i)+q(2,i));
y2 = y1+a2*sin(q(1,i)+q(2,i));
plot([0,x1,x2],[0,y1,y2],'r-o','linewidth',2)
grid on, set(gca,'fontsize',20)
hold on
plot(x(1:i),y(1:i),'m-')
axis([-(a1+a2)-0.1, (a1+a2)+0.1,-(a1+a2)-0.1,(a1+a2)+0.1]);
axis square, xlabel('t,[s]'),ylabel('q, [units]')
hold off, pause(0.001)
end

%% Plotting functions
plot(t,q(1,1:i),'r-.',t,q(2,1:i),'b-','linewidth',2)
legend('\theta_1', '\theta_2')
grid on
set(gca,'fontsize',20)
xlabel('t, [s]')
ylabel ('q, [units]')

%% Errors %
mu_tilda(:,i) = mu_desired(:,i) - mu(:,i);
mu_dot_tilda(:,i) = mu_dot_desired(:,i)...
- Jxq_dot(:,i);
ei = ei + mu_tilda(:,i)*dt;

65
%% PID control
%input vector in task-space %
F(:,1) = Kp*mu_tilda(:,i)+Ki*ei+Kd*mu_dot_tilda(:,i) ;
% input vector in joint-space
tau(:,i) = JA'*F(:,i)+g_v;

66
 CHAPTER-7

CONCLUSION:

Using a PID (Proportional-Integral-Derivative) controller to simulate a two degrees of freedom

(DOF) robot manipulator might offer many insightful possibilities. Here are some of the key
takeaways from such a simulation

1. Control System design: We can develop and fine-tune a PID controller specifically for
the robot manipulator system using the simulation to design your control system. To
accomplish desired performance parameters like stability, response time, and accuracy,
you can experiment with various PID gains (proportional, integral, and derivative). This
makes it easier to comprehend how each PID term affects how the system behaves.

2. Trajectory Tracking: We can test a PID controller's ability to reliably follow the desired
path by simulating different reference trajectories, such as simple curves or straight lines.
The robot's reaction to changes in direction can be tracked.

3. Stability Analysis: The simulation enables us to examine the system's stability at various
PID settings. By analysing the response to disturbances or changes in the system
characteristics, we can evaluate the stability margins. It aids in comprehending how PID
gains affect system stability and the likelihood of oscillations or instability.

4. Sensitivity Analysis: Using simulations, we can assess how sensitive the system's
performance is to variations in PID gains. We can assess resilience and find the right gain
levels that reduce sensitivity to system uncertainties or disruptions by evaluating the
reaction to parameter adjustments.

5. Error Analysis: Using the simulation, we can measure and analyse any discrepancies
between the intended robot manipulator trajectory and the actual trajectory it took. We
can evaluate the control system's overall accuracy, steady-state inaccuracy, and transient
response. The PID gains can be improved upon using this information as a guide.

67
➢ Limitation

• PID control's drawbacks can be shown while modelling a 2 DOF robot manipulator with
a PID controller, despite the fact that it is widely used and effective in many
applications.
• For extremely dynamic or nonlinear systems, we could see problems like overshoot,
oscillations, or trouble obtaining desired performance. If necessary, knowing these
restrictions can let us explore more sophisticated control methods.

68
 CHAPTER-8

FUTURE SCOPE

A 2-degree-of-freedom robotic manipulator with PID control has a very broad potential in the
future. Here are some potential directions for research and use:

PID control is extensively utilised and efficient, however there are also more advanced control
methods that can be investigated. To improve the performance and resilience of the manipulator,
future research can concentrate on creating sophisticated control systems such model predictive
control (MPC), adaptive control, or fuzzy control.

Planning and Optimization of Trajectories: Effective trajectory planning and optimization

techniques can increase the manipulator's capacity for motion. Future research can concentrate
on incorporating optimization algorithms to create the best possible trajectories, taking into
account constraints unique to each task, energy efficiency, joint limits, and obstacle avoidance.

Integration of modern sensing technologies: It can improve the manipulator's capacity for
perception and control. The manipulator can interact with the environment more intelligently and
carry out complex tasks by integrating vision systems, depth sensors, force/torque sensors, or
touch sensors.

Real-Time Adaptive Control: Conventional PID control uses set control gains, which may not
be the best option in dynamic and unpredictable environments. The performance and flexibility
of the manipulator are increased by real-time adaptive control algorithms that can modify the
control gains in response to the current operating circumstances.

Robotic Manipulation in Unstructured situations: One of the main directions for future study
is to increase the ability of the manipulator to work in unstructured or unfamiliar situations.

Cooperative Manipulation: Researching methods for collaborative manipulation, in which a

group of robotic manipulators cooperates to complete a job, has the potential to produce
considerable progress. Heavy object lifting, assembly work, and collaborative manufacturing
processes are all examples of jobs that

Learning-Based Control: The incorporation of deep learning and machine learning techniques
in control systems is a promising field of study for the future. The manipulator can enhance its
control strategy and adapt to various conditions by utilizing data-driven techniques, which allow
it to learn from prior experience or from data that has already been acquired.

69
 ➢ Reference:
[1] Baccouch, M. and Dodds, S.A., “Two-Link robot manipulator: simulation and control
design”, International Journal of Robotic Engineering, 5 (2), pp. 1-17 (2020)

[2] Qiang, Mi, et al. "Compare PID and PDF Control in 2 Degree of Freedom Robotic Manipulator." 2019
International Conference on Precision Machining, Non-Traditional Machining and Intelligent
Manufacturing (PNTIM 2019). Atlantis Press, 2019.

[3] Sciavicco, Lorenzo, and Bruno Siciliano. Modelling and control of robot manipulators. Springer
Science & Business Media, 2001.

[4] Su, Yuxin, Peter C. Müller, and Chunhong Zheng. "Global asymptotic saturated PID control for robot
manipulators." IEEE Transactions on Control systems technology 18.6 (2009): 1280-1288.

[5] Li, Yanan, Shengtao Xiao, and Shuzhi Sam Ge. "Reinforcement learning control for a robotic
manipulator with unknown deadzone." Proceeding of the 11th World Congress on Intelligent Control and
Automation. IEEE, 2014.

[6] NPTEL lecture on Mechanics and Control of Robotic Manipulators by Prof. Santhakumar Mohan

[7] Parra-Vega, Vicente, et al. "Dynamic sliding PID control for tracking of robot manipulators: Theory and
experiments." IEEE Transactions on Robotics and Automation 19.6 (2003): 967-976.

[8] Kasac, Josip, et al. "Global positioning of robot manipulators with mixed revolute and prismatic
joints." IEEE transactions on automatic control 51.6 (2006): 1035-1040.

[9] Rocco, Paolo. "Stability of PID control for industrial robot arms." IEEE transactions on robotics and
automation 12.4 (1996): 606-614.

[10] Hong, Yun, and Bin Yao. "A globally stable saturated desired compensation adaptive robust control
for linear motor systems with comparative experiments." Automatica 43.10 (2007): 1840-1848.

[11] Shah, Jolly Atit, and S. S. Rattan. "Dynamic analysis of two link robot manipulator for control design
using PID computed torque control." International Journal of Robotics and Automation 5.4 (2016): 277-
283.

[12] Nguyen-Tuong, Duy, and Jan Peters. "Learning robot dynamics for computed torque control using
local Gaussian processes regression." 2008 ECSIS Symposium on Learning and Adaptive Behaviors for
Robotic Systems (LAB-RS). IEEE, 2008.

[13] Mohammed, Amin A., and Ahmed Eltayeb. "Dynamics and control of a two-link manipulator using PID
and sliding mode control." 2018 International Conference on Computer, Control, Electrical, and
Electronics Engineering (ICCCEEE). IEEE, 2018.

[14] Nandy, Gourab, Basukinath Chatterjee, and Amartya Mukherjee. "Dynamic analysis of two-link robot
manipulator for control design." Advances in Communication, Devices and Networking: Proceedings of
ICCDN 2017. Springer Singapore, 2018.

70
[15] Kumar, J. Senthil, and E. Karthigai Amutha. "Control and tracking of robotic manipulator using PID
controller and hardware in Loop Simulation." 2014 International Conference on Communication and
Network Technologies. IEEE, 2014.

[16] David, I., and G. Robles. "PID control dynamics of a Robotic arm manipulator with two degrees of
Freedom." Control de Processos y Robotica (2012): 3-7.

[17] Mohammad, Ataei, and Shafiei S. Ehsan. "Sliding mode PID-controller design for robot manipulators
by using fuzzy tuning approach." 2008 27th Chinese Control Conference. IEEE, 2008.

[18] Nguyen-Tuong, Duy, Matthias Seeger, and Jan Peters. "Computed torque control with nonparametric
regression models." 2008 American Control Conference. IEEE, 2008.

[19] Spong, Mark W., Seth Hutchinson, and M. Vidyasagar. "Robot Dynamics and Control, chapter
Introduction." (2004).

[20] Kelly, Rafael. "Global positioning of robot manipulators via PD control plus a class of nonlinear
integral actions." IEEE Transactions on Automatic Control 43.7 (1998): 934-938.

[21] Zhang, Jing-Gang, Zhi-Yuan Liu, and Run Pei. "Two-degree-of-freedom PID control with fuzzy logic
compensation." Proceedings. International Conference on Machine Learning and Cybernetics. Vol. 3.
IEEE, 2002.

[22] Alfaro, V. M., Ramon Vilanova, and Orlando Arrieta. "Robust tuning of Two-Degree-of-Freedom (2-
DoF) PI/PID based cascade control systems." Journal of process control 19.10 (2009): 1658-1670.

[23] Araki, Mituhiko, and Hidefumi Taguchi. "Two-degree-of-freedom PID controllers." International Journal
of Control, Automation, and Systems 1.4 (2003): 401-411.

[24] Yukitomo, Masanori, et al. "A two degrees of freedom PID control system, its features and
applications." 2004 5th Asian Control Conference (IEEE Cat. No. 04EX904). Vol. 1. IEEE, 2004.

[25] Lochan, Kshetrimayum, and Binoy Krishna Roy. "Control of two-link 2-DOF robot manipulator using
fuzzy logic techniques: a review." Proceedings of Fourth International Conference on Soft Computing for
Problem Solving: SocProS 2014, Volume 1. New Delhi: Springer India, 2014.

71