Computed Torque Control of Robot Manipulator

Martino Ojwok Ajangnay

Juba University
College of Engineering and Architecture
Electrical Engineering Department
Juba, Republic of South Sudan
martino.ajangnay16@gmail.com

Abstract: This paper proposed a computed Torque Control for robot manipulator. The significant of
the proposed control is in tuning of controller parameters based on the desired time domain
requirements. The control design exploits compensation of inverse dynamic of robot manipulator to
linearization and controller parameters selection to decouple the manipulator dynamics. The main
objective of the proposed control approach is to ensure the robot manipulator motion of joint position
follow the desired joint position value closely. The performance of the selected optimal control
parameters was verified through MATLAB/SIMULINK software.

Key word: inverse dynamics, computed torque control.

1. Introduction adaptive control law has been applied many

Recently robotics manipulator application in
manufacturing had revolutionized variety of
advanced technology industry. In such industrial,
accuracy and precision is paramount to produce
high quality products and processes. In order to
achieve high quality performance there is a need
to compute accurate model of the robot and the
environment it is operating precisely. The model
of robot dynamics can be deduced or computed
from the knowledge of structure of robot
mechanical structure. The kinematic of the robot
can be computed using direct and indirect
kinematic methods. Using Lagarange approach or
Newton-Euler approach, the dynamic of the robot
manipulator can be derived [1],[2],[3]. Different
control approach has been applied to control the
robot to perform tasks in unstructured
environment. The design of the control to meet
such objective is crucial and is tedious task in
robotic applications. Different control algorithm
ranging from classical PD, PID, robust and
advance control technology [4],[2]. Advance
control approach such as neural network, fuzzy
logic has also been extensively researched and
their application had revolutionized the robot
application in various industrial field [3],
[6],[7],[8],[9].
Dynamic of robot manipulator are characterized
by a system of highly coupled and nonlinear [1-
5]. Due to this nonlinearity it is crucial to devised
control techniques that decoupled and ensure the
control objective by linearizing and decoupling
the dynamic of robot manipulator.
More advance control techniques have also been
applied in robotic field, such as adaptive, robust
and variable structure in combination with neural
network, adaptive and robust control as well as
neuro-fuzzy logic controllers [9]-[12].
 2. Robot Manipulator Dynamic
description
The robot manipulator dynamic equation of the n
degree of freedom can be represented in joint
space as [1], [2].
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝐹(𝑞̇ ) + 𝐺(𝑞) + 𝜏𝑑 = 𝜏
Where 𝑞, 𝑞̇ , 𝑞̈ ∈ 𝑅 𝑛 are the vectors of joint
position, velocity and acceleration respectively;
𝑀(𝑞) ∈ 𝑅 𝑛𝑥𝑛 is a symmetric positive definite
inertia matrix; 𝐶(𝑞, 𝑞̇ )𝑞̇ ∈ 𝑅 𝑛 is a vector of
Coriolis and centripetal forces; 𝐹(𝑞̇ ) are friction
terms; 𝐺(𝑞) ∈ 𝑅 𝑛 denotes a gravity vector; 𝜏𝑑
represent disturbance; and 𝜏 ∈ 𝑅 𝑛 represents a
vector of torque exerted on joints.
The dynamic model given in equation (1) has
properties that are crucial for control design
2.1 The inertia matrix 𝑀(𝑞) is symmetric,
positive definite, and bounded so that
𝜇1 𝐼 ≤ 𝑀(𝑞) ≤ 𝜇2 𝐼 for all 𝑞(𝑡). For
revolute joints, the only occurrence of the
joints variable 𝑞𝑖 are as sin(𝑞𝑖 ), cos(𝑞𝑖 ).
for arm with prismatic joints, the bounds
𝜇1 , 𝜇2 are constants.
2.2 The Coriolis/centripetal vector 𝐶(𝑞, 𝑞̇ )𝑞̇
is quadratic in 𝑞̇ . 𝐶(𝑞, 𝑞̇ ) is bounded so
that ‖𝐶(𝑞, 𝑞̇ ‖ ≤ 𝑐𝐵 ‖𝑞̇ ‖, or equivalently
‖𝐶(𝑞, 𝑞̇ )𝑞̇ ‖ ≤ 𝑐𝐵 ‖𝑞̇ ‖2.
2.3 Coriolis/centripetal matrix can always be
selected so that the matrix 𝑆(𝑞, 𝑞̇ ) ≡
𝑀̇(𝑞) − 2𝐶(𝑞, 𝑞̇ ) 𝑖𝑠 𝑠𝑘𝑒𝑤 𝑠𝑦𝑚𝑚𝑒𝑡𝑟𝑖𝑐.
Therefore, 𝑥 𝑇 𝑆(𝑞, 𝑞̇ )𝑥 = 0 for all vector
𝑥. This is a statement of the fact that the
function forces in the robot system do not
work.
2.4 The friction terms have the approximate
form 𝐹(𝑞̇ ) = 𝐹𝑣 𝑞̇ + 𝐹𝑑 (𝑞̇ ), with 𝐹𝑣 is a
diagonal matrix constant coefficients
representing the viscous friction, and
𝐹𝑑 (. ) a vector with entries like 𝐾𝑑𝑖 the
coefficients of dynamic friction. These
friction terms are bounded so that
‖𝐹(𝑞̇ )‖ ≤ 𝑓𝐵 ‖𝑞̇ ‖ + 𝑘𝐵 for constants
𝑓𝐵 , 𝑘𝐵 .
2.5 The gravity vector is bounded so that
‖𝐺(𝑞)‖ ≤ 𝑔𝐵 . For revolute joints, the
only occurrences of the joint variables 𝑞𝑖
are as sin(𝑞𝑖 ) , cos(𝑞𝑖 ). For revolute joint
(1) arms the bound 𝑔𝐵 is a constant.
2.6 The disturbances are bounded so that
‖𝜏𝑑 ‖ ≤ 𝑑𝐵 .
In addition to the above mentioned properties, the
dynamic model in equation (1) can be defined in
terms of certain parameters, such as the link mass,
and moment of inertia, friction coefficient, etc.
usually a priori knowledge of these parameters is
not complete and therefore, they may be
determined in order to modify the dynamic model
or tune a model-based controller. Fortunately, it
can be shown that the nonlinear dynamic model
presented in Eq. (1) is linear in terms of the above
mentioned parameters in the following sense.
There is exist 𝑛 𝑥 𝑝 matrix function (𝑞, 𝑞̇ , 𝑞̈ ) ,
which is called the regressor, and a 𝑝 𝑥 1 vector
Φ such that the dynamic model can be written as
𝜏 = 𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝐹(𝑞, 𝑞̇ ) + 𝐺(𝑞)
= 𝑌(𝑞, 𝑞̇ , 𝑞̈ )Φ
𝜏 = 𝑌(𝑞, 𝑞̇ , 𝑞̈ )Φ (1.a)
Where Φ is a 𝑝 𝑥 1 vector of dynamic parameters
and 𝑝 is the number of parameters. Linearity in
the dynamic parameters property is a base for
traditional adaptive control design and on-line
parameter estimation method.
3. Computed-Torque Control
The Computed Torque Control aimed at
linearizing and decoupling robot manipulator
dynamics; nonlinearities such as Coriolis and
centrifugal terms as well as gravity terms can be
simply compensated by adding these forces to the
control input.
The control torque input to manipulator can be
consider as
𝜏 = 𝑀(𝑞)𝑦 + 𝐶(𝑞, 𝑞̇ ) + 𝐺(𝑞) (2)
The above control law is a cascaded control,
consists of an inner nonlinear compensation loop
and an outer-loop controller with exogenous
control signal y. Substituting equation (2) into
equation (1) yield
𝑞̈ = 𝑦 (3)
This control input converts a nonlinear controller
design problem into a simple design problem for
a linear system consisting of n decoupled
subsystem. Outer-loop control y is usually chosen
as[4]
𝑦 = 𝑞𝑑̈ + 𝐾𝑑 𝑒̇ + 𝐾𝑝 𝑒 (4)
Where 𝑒 = 𝑞𝑑 − 𝑞 is a joint position error, 𝑒̇ =
𝑞𝑑̇ − 𝑞̇ is a joint velocity error, 𝑞𝑑 , 𝑞𝑑̇ are desired
joint position and desired velocity respectively.
Substituting equation (4), (3), (2) into equation
(1), result to linear error dynamics given by
𝑒̈ + 𝐾𝑑 𝑒̇ + 𝐾𝑝 𝑒 = 0 (5)
Equation (5) is of second order system whose
convergence of the tracking error to zero is
guaranteed by choosing appropriate values of 𝐾𝑑
and 𝐾𝑝 based on second-order system time
domain characteristics (bandwidth, rise time,
time to overshoot, steady state error etc.).
The values of 𝐾𝑑 and 𝐾𝑝 determine the
performance of error signal such as bandwidth of
error signal and damping ratio. Usually the values
of 𝐾𝑑 and 𝐾𝑝 are chosen as a diagonal positive
definite matrix gain to ensure decoupling and to
guarantee the stability of the error system.
The selection of controller parameters in Eq. (5)
are chosen such that the robot position output
tracks the desired position output closely. As it is
seen that equation (5) is of second-order system,
whose behavior can be characterized by selection
of gain 𝐾𝑑 and 𝐾𝑝 to meet certain time-domain
characteristic requirements. Thus, their values
can be selected as a function of the desired closed
loop bandwidth 𝜔𝑛 and damping ratio 𝜁. i.e
𝐾𝑝 = 𝜔𝑛2 , 𝐾𝑑 = 2𝜔𝑛 𝜁 (6)
On assumption of positive definite matrices
𝐾𝑝 𝑎𝑛𝑑 𝐾𝑑 is asymptotically stable. Thus the
definite matrices 𝐾𝑝 𝑎𝑛𝑑 𝐾𝑑 are chosen as
2 2 2 }
𝐾𝑝 = 𝑑𝑖𝑎𝑔{𝜔𝑛1 , 𝜔𝑛2 , … , 𝜔𝑛𝑛 𝑎𝑛𝑑 𝐾𝑑 =
{2𝜁1 𝜔𝑛1 , 2𝜁2 𝜔𝑛2 , … , 2𝜁𝑛 𝜔𝑛𝑛 } (7)
The stability of the closed-loop system is
investigated by choosing the Lyapunov function
candidate, and by introducing a small constant
𝜀 staisfying
𝜆𝑚𝑖𝑛 {𝐾𝑑 } > 𝜀 > 0 (8)
For any x∈ 𝑅 2𝑛 is any nonzero vector yields
𝜆𝑚𝑖𝑛 {𝐾𝑑 }𝑥 𝑇 𝑥 > 𝜀𝑥 𝑇 𝑥.
Because 𝐾𝑑 is a symmetric and positive definite
matrix, 𝑥 𝑇 𝐾𝑑 𝑥 ≥ 𝜆𝑚𝑖𝑛 {𝐾𝑑 }𝑥 𝑇 𝑥 and therefore,
𝑥 𝑇 [𝐾𝑑 − 𝜀𝐼]𝑧 > 0 ∀𝑥 ≠ 0 ∈ 𝑅 𝑛 (9)
This means that the matrix 𝐾𝑑 − 𝜀𝐼 is positive
definite. Given the above consideration, we can
conclude that
𝐾𝑝 + 𝜀𝐾𝑑 − 𝜀 2 𝐼 > 0. (10)
Let us choose the total energy of the system as
Lyapunov function
1 1
𝑉(𝑒, 𝑒̇ ) = 2 𝑒̇ 𝑇 𝑒̇ + 2 𝑒 𝑇 [𝐾𝑝 + 𝜀𝐾𝑑 ]𝑒 +
𝜀𝑒 𝑇 𝑒̇ (11)
Taking the derivative of Eq. (11):
𝑉̇ (𝑒, 𝑒̇ ) = 𝑒̇ 𝑇 𝑒̈ + 𝑒 𝑇 (𝐾𝑝 + 𝜀𝐾𝑑 )𝑒̇ + 𝜀𝑒 𝑇 𝑒̈ +
𝜀𝑒̇ 𝑇 𝑒̇ (12)
Using Eq. (5) in Eq. (12) yields 𝐶21 (𝑞) = 𝑚2 𝑙1 𝑙𝑐2 𝑞1̇ sin(𝑞2 )

𝑉̇ (𝑒, 𝑒̇ ) = −𝑒̇ 𝑇 [𝐾𝑣 − 𝜀𝐼]𝑒̇ − 𝜀𝑒 𝑇 𝐾𝑝 𝑒 (13) 𝐶22 (𝑞) = 0

Since 𝐾𝑑 − 𝜀𝐼 is positive definite eq. (10) the

function 𝑉̇ (𝑒, 𝑒̇ ) is globally negative definite.
𝐺1 (𝑞) = (𝑚1 𝑙𝑐1 + 𝑚2 𝑙1 )𝑔𝑠𝑖𝑛(𝑞1 )
Thus we can conclude that the origin [𝑒 𝑇 𝑒̇ 𝑇 ]𝑇 of
+ 𝑚2 𝑙𝑐2 𝑔𝑠𝑖𝑛(𝑞1 + 𝑞2 )
the closed-loop equation is globally uniformly
asymptotically stable. 𝐺2 (𝑞) = 𝑚2 𝑙𝑐2 𝑔𝑠𝑖𝑛(𝑞1 + 𝑞2 )

4.0 Simulation and Controller performance

evaluation

The control performance was evaluated using

Two-Link rigid robot manipulator through l2

MATLAB/SIMULINK software. Diagram m2, I2

shown in figure (5) was used to simulate the robot lc2

model and controller design performance.

q2

Two link manipulator dynamic model derived

by using Lagrange method or Newton-Euler l1

method is given by following equations

lc1 m1, I1
𝑀 (𝑞) 𝑀12 (𝑞) 𝑞1̈
[ 11 ][ ]
𝑀21 (𝑞) 𝑀22 (𝑞) 𝑞2̈
q1
𝐶 (𝑞, 𝑞̇ ) 𝐶12 (𝑞, 𝑞̇ ) 𝑞1̇
+ [ 11 ][ ]
𝐶21 (𝑞, 𝑞̇ ) 𝐶22 (𝑞, 𝑞̇ ) 𝑞2̇
𝐺 (𝑞) 𝜏1
+ [ 1 ] = [𝜏 ] Figure (1) two link manipulator
𝐺2 (𝑞) 2

Robot model, 𝑀(𝑞), 𝐶(𝑞, 𝑞̇ ) 𝑎𝑛𝑑 𝐺(𝑞) are

programmed matlab m-files that masked to form
2 the SIMULINK blocks shown in figure (5).
𝑀11 (𝑞) = 𝑚1 𝑙𝑐1
+ 𝑚2 (𝑙12 + 𝑙𝑐2
2
The controller parameters are chosen such that
+ 2𝑙1 𝑙𝑐2 cos(𝑞2 )) + 𝐼1 + 𝐼2
the closed-loop system has bandwidth of 5.6 rad/s
2
𝑀12 (𝑞) = 𝑚2 (𝑙𝑐2 + 𝑙1 𝑙𝑐2 cos(𝑞2 )) + 𝐼2 and damping ratio of 1. Thus the gain 𝐾𝑝 =
𝑑𝑖𝑎𝑔{40,40} 𝑎𝑛𝑑 𝐾𝑑 = 𝑑𝑖𝑎𝑔{12.6,12.6}
2
𝑀12 (𝑞) = 𝑚2 (𝑙𝑐2 + 𝑙1 𝑙𝑐2 cos(𝑞2 )) + 𝐼2
Robot Manipulator parameters are shown in table
2
𝑀22 (𝑞) = 𝑚2 𝑙𝑐2 + 𝐼2 (1).

5. Result

𝐶11 (𝑞) = −𝑚2 𝑙1 𝑙𝑐2 𝑞2̇ sin(𝑞2 ) Figure (2), illustrates time response of the actual
joint positions against the desired joint positions.
𝐶12 (𝑞) = −𝑚2 𝑙1 𝑙𝑐2 (𝑞1̇ + 𝑞2̇ )sin(𝑞2 )
The figure shows that the actual joint position 2
Angular velocity: q1ddot & q1dot

tracks the desired joint positions closely.

Angular velocity [rad]

q1ddot
1 q1dot

0
Figure (3) compares the actual joint velocities
-1
and desired joint velocities. The figure shows that -2
0 1 2 3 4 5 6 7 8 9 10
the actual velocities converged to the desired Time [sec]
Angular position: q2ddot & q2dot
joint velocities perfectly. 4

Angular velocity [rad]

q2ddot
q2dot
2
Figure (4) shows the joint position error of two
0
links. The tracking errors are small which show
that the controller was able to drive the actual -2
0 1 2 3 4 5 6 7 8 9 10
Time [sec]
joint positions toward desired joint positions.
This shows that the control objective has been
Figure 3: First and second link desired and actual
met. That to say, that the position error converge
angular velocity signal
asymptotically toward zero as time converge to
infinity. x 10
-3
Position Error: eq1 & eq2
2
eq1
Figure (5) show input torque which bounded as eq2
1
desired joint position, velocity and acceleration is
bounded. postion [rad] 0

-1
6. Conclusion
-2
Choosing the controller gains 𝐾𝑑 𝑎𝑛𝑑 𝐾𝑝 as
diagonal matrices results in decoupled -3

multivariable linear closed loop system.

-4
Therefore its dynamics behavior of the tracking 0 1 2 3 4 5
Time [sec]
6 7 8 9 10

error of each joint position is governed by

second-order differential equations which can be Figure 4: Position tracking error for first and
independently control of each other. second link

Position: q1d & q1 Torque: tau1 & tau2

1.5 10
tau1
q1d 9 tau2
q1
postion [rad]

1
8

7
0.5
6
Torque [Nm]

0
0 1 2 3 4 5 6 7 8 9 10 5
Time [sec]
4
Position: q2d & q2
2
3
q2d
1.5 q2 2
postion [rad]

1 1

0.5 0
0 1 2 3 4 5 6 7 8 9 10
Time [sec]
0
0 1 2 3 4 5 6 7 8 9 10
Time [sec]
Figure 4: Input torque
Figure 2: First and second link desired and
actual position signal
 2. E. Margrini, F. Flacco and A. De Luca,
desired and actual psoition
“Control of Generalized Contact Motion
input torque
desired position,velocity and acceleration

desired and actual velocity

and Force in Physical Human-Robot
position error Interaction”, IEEE International
qd actual position and
qd_dot

qd_2dot
Add
K*uvec

Kp
angular velocity
Conference on Robotics and Automation
q
q
desired signal

K*uvec
u_PD
M(q)

M(q)
u
q_dot
(ICRA), Seattle, Washington, 2015, pp.
Add3 Robot Manipulator
Add2 Kd Add1
2298-2304.
g(q)

g(q) q
3. N. Kumar,. Panwar, N. Sukavanam, S. P.
C(q)
q
Sharma and J Borm, “Neural Network
C(q,qdot)
q_dot
Based Hybrid Force/Position Control for
Robot Manipulator, International Journal
of Precision Engineering and
Manufacturing, Vol. 12 No. 3, pp. 419-
Figure 5: Computed-Torque control Simulink 426.
block diagram 4. R. Kelly, V. Santibanez and A. Loria,
“Control of Robot Manipulators in Joint
Space”, Springer-Verlag London
Limited, 2005.
5. Z. Jiang and T. Ishita, “A Neural
Table (1): Two link robot manipulator Network Controller for Trajectory
parameters. Control of Industrial Robot
Manipulators”, Journal of Computers,
Description Paramete Value Unit Vol. 3, NO. 8 August 2008.
r 6. V. Panwar and N. Sukavanam, “Design
Length of Link 1 l1 0.26 m of Optimal Hybrid Position/Force
Length of Link 2 l2 0.26 m
Controller for a Robot Manipulator using
Distance of centre of lc1 0.0983 m
mass of link 1 Neural Network”, Mathematical
Distance of centre of lc2 0.0229 m Problems in Engineering, Vol. 2007
mass of link 2 7. L. F. Baptista, J. M. C. Sousa and J. M.
Mass of link 1 m1 6.5225 kg G.Sa da Costa, “Predictive Force Control
Mass of link 2 m2 2.0458 kg
Inertia rel. to centre of I1 0.1213 Kg of Robot Manipulators in Nonrigid
mass of link 1 m2 Environments”, Industrial Robotics:
Inertia rel. to centre of I2 0.0116 Kgm2 Theory, Modeling and Control, p. 964,
mass of link 2 Dec 2006.
8. H. Lee, D. Nam and C. H. Park, “A
Sliding Mode Controller using Neural
Networks for Robot Manipulator”,
REFERENCES European Symposium on Artificial
Neural Networks, Belgium, 28-30 April,
1. B. Siciliano, L. Sciavicco, L. Villani and pp. 193-198.
G. Oriolo, “Robotics: Modeling, 9. L. Huang, S. S. Ge and T. H. Lee, “Fuzzy
Planning and Control”, Springer-Vertag Unidirectional Force Control of
London Limited, 2009. Constrained robotic Manipulators”,
 Fuzzy Sets and Systems 134, Elsevier, He was working with Sudan University of
(2003), pp. 135-146. Science and Technology from 1992 to 2011. He
10. H. D. Patino, R. Carelli and B. R. was Assistant Professor and Head of Electrical
Kuchen, “Neural Networks for Engineering at College of Engineering, Sudan
Advanced Control of Robot University of Science and technology. He worked
Manipulators”, IEEE Transactions on as Manager of Project and Facilities at Dar
Neural Networks, Vol. 13, No. 2 March Petroleum Operating Company (Oil and Gas
2002. company) since 2011 to 2017. Currently he works
11. S. Chiaverini, B. Sicilino and L. Villani, as Assistant Professor with Juba University,
“A Survey of Robot Interaction Control College of Engineering and Architecture,
Schemes with Experimental Electrical Engineering Department, South Sudan.
Comparison”, IEEE/ASME Transactions
on Mechatronics, Vol. 4, No. 3, Sep
1999.
12. S. S. Ge, C. C. Hang and C. L.oon,
“Adaptive Neural Network Control of Appendix
Robot Manipulators in Task Space”,
% Robot model
IEEE Transactions on Industrial function[sys,x0]=martino_model(t,x,u,flag,Link1,Link2,xx
Electronics, Vol. 44, No. 6, December 0)
m1=Link1(1);
1997. l1=Link1(2);
lc1=Link1(3);
I1=Link1(4);
m2=Link2(1);
l2=Link2(2);
lc2=Link2(3);
I2=Link2(4);

if flag==0
x0=xx0;
sys=[4,0,4,2,0,1];
elseif flag==1

M11=m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(x(2)))+I1
+I2;
M12=m2*(lc2^2+l1*lc2*cos(x(2)))+I2;
Dr Martino Ojwok Ajangnay obtained a Bachelor M21=M12;
M22=m2*lc2^2+I2;
of Science in Electrical Engineering (BSc., Mq=[M11 M12;M21 M22];
honours, 1st Class, “Control”) in 1992 from C11=-m2*l1*lc2*sin(x(2))*x(4);
C12=-m2*l1*lc2*sin(x(2))*(x(3)+x(4));
Electrical Engineering, Sudan University of C21=m2*l1*lc2*sin(x(2))*x(3);
Science and Technology, Khartoum, Sudan C22=0;
Cq=[C11 C12;C21 C22];
He obtained PhD and MSc in Digital System
G1=(m1*lc1+m2*l1)*9.8*sin(x(1))+m2*lc2*9.8*sin(x(1)+
Engineering from Computing and Electrical x(2));
Engineering, Heriot-Watt University- Edinburgh, G2=m2*lc2*9.8*sin(x(1)+x(2));
Gq=[G1;G2];
United Kingdom in 2004 and 2000 respectively. dx1=x(3);
His research interest is in digital control systems dx2=x(4);
dx34=inv(Mq)*([u(1);u(2)]-Cq*[x(3);x(4)]-Gq);
and its application in robotic manipulators
dx3=dx34(1);
modeling, mobile robot modeling and control. dx4=dx34(2);
sys=[dx1;dx2;dx3;dx4];
elseif flag==3 m1=Link1(1);
sys=[x(1);x(2);x(3);x(4)]; l1=Link1(2);
else lc1=Link1(3);
sys=[]; I1=Link1(4);
end m2=Link2(1);
l2=Link2(2);
lc2=Link2(3);
% Coriolis and Centripetal force I2=Link2(4);

function[sys,x0]=martino_C(t,x,u,flag,Link1,Link2) if flag==0
m1=Link1(1); x0=[];
l1=Link1(2); sys=[0,0,2,4,0,1];
lc1=Link1(3); elseif flag==3
I1=Link1(4);
m2=Link2(1); M11=m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(u(2)))+I1
l2=Link2(2); +I2;
lc2=Link2(3); M12=m2*(lc2^2+l1*lc2*cos(u(2)))+I2;
I2=Link2(4); M21=M12;
M22=m2*lc2^2+I2;
if flag==0 Mq=[M11 M12;M21 M22];
x0=[]; uM=Mq*[u(3);u(4)];
sys=[0,0,2,4,0,1]; sys=[uM(1);uM(2)];
elseif flag==3 else
C11=-m2*l1*lc2*sin(u(2))*u(4); sys=[];
C12=-m2*l1*lc2*sin((2))*(u(3)+u(4)); end
C21=m2*l1*lc2*sin(u(2))*u(3);
C22=0;
Cq=[C11 C12;C21 C22];
uC=Cq*[u(3);u(4)];
sys=[uC(1);uC(2)];
else
sys=[];
end

% Gravitational forces

function[sys,x0]=martino_G(t,x,u,flag,Link1,Link2)
m1=Link1(1);
l1=Link1(2);
lc1=Link1(3);
I1=Link1(4);
m2=Link2(1);
l2=Link2(2);
lc2=Link2(3);
I2=Link2(4);

if flag==0
x0=[];
sys=[0,0,2,2,0,1];
elseif flag==3

G1=(m1*lc1+m2*l1)*9.8*sin(u(1))+m2*lc2*9.8*sin(u(1)+
u(2));
G2=m2*lc2*9.8*sin(u(1)+u(2));
sys=[G1;G2];
else
sys=[];
end

% Inertia Matrix

function[sys,x0]=martino_M(t,x,u,flag,Link1,Link2)