clc; clear all;

syms m1 m2 l1 l2 g q1 q2 q1_dot q2_dot q1_2dot q2_2dot

c1=cos(q1);
c12=cos(q1+q2);
c2=cos(q2);
s2=sin(q2);
s1=sin(q1);
s12=sin(q1+q2);
F1=[(m1+m2)*l1^2+m2*l2^2+2*m2*l1*l2*c2]*q1_2dot+...
[m2*l2^2+m2*l1*c2]*q2_2dot-2*m2*l1*l2*s2*q1_dot*q2_dot+...
-m2*l1*l2*s2*q2_dot^2+(m1+m2)*g*l1*s1+m2*g*l2*s12
F2=(m2*l2^2+m2*l1*l2*c2)*q1_2dot+m2*l2^2*q2_2dot+...
m2*l1*l2*s2*q1_dot^2+m2*g*l2*s12
%%%%%%%%%%%%%%%%%ketqua:
F1 = - l1*l2*m2*sin(q2)*q2_dot^2 - 2*l1*l2*m2*q1_dot*sin(q2)*q2_dot + q1_2dot*((m1
+ m2)*l1^2 + 2*m2*cos(q2)*l1*l2 + m2*l2^2) + q2_2dot*(m2*l2^2 + l1*m2*cos(q2)) +
g*l1*sin(q1)*(m1 + m2) + g*l2*m2*sin(q1 + q2)
F2 = q1_2dot*(m2*l2^2 + l1*m2*cos(q2)*l2) + l2^2*m2*q2_2dot + g*l2*m2*sin(q1 + q2)
+ l1*l2*m2*q1_dot^2*sin(q2)
%%%%%%%%%%%%%%%%%doibien
%q1=x1
%q1_dot=x2
%q2=x3
%q2_dot=x4
clc; clear all;
syms m1 m2 l1 l2 g x1 x2 x3 x4 x1_2dot x3_2dot u1 u2
F1 = - l1*l2*m2*sin(x3)*x4^2 - 2*l1*l2*m2*x2*sin(x3)*x4 + x1_2dot*((m1 + m2)*l1^2 +
2*m2*cos(x3)*l1*l2 + m2*l2^2) + x3_2dot*(m2*l2^2 + l1*m2*cos(x3)) +
g*l1*sin(x1)*(m1 + m2) + g*l2*m2*sin(x1 + x3)
F2 = x1_2dot*(m2*l2^2 + l1*m2*cos(x3)*l2) + l2^2*m2*x3_2dot + g*l2*m2*sin(x1 + x3)
+ l1*l2*m2*x2^2*sin(x3)
ptth1=F1-u1
ptth2=F2-u2
%%%%%%%%%%%%%%%%
clc; clear all;
syms m1 m2 l1 l2 g x1 x2 x3 x4 x1_2dot x3_2dot u1 u2
ptth1 = -l1*l2*m2*sin(x3)*x4^2 - 2*l1*l2*m2*x2*sin(x3)*x4 - u1 + x1_2dot*((m1 +
m2)*l1^2 + 2*m2*cos(x3)*l1*l2 + m2*l2^2) + x3_2dot*(m2*l2^2 + l1*m2*cos(x3)) +
g*l1*sin(x1)*(m1 + m2) + g*l2*m2*sin(x1 + x3)
ptth2 = x1_2dot*(m2*l2^2 + l1*m2*cos(x3)*l2) - u2 + l2^2*m2*x3_2dot +
g*l2*m2*sin(x1 + x3) + l1*l2*m2*x2^2*sin(x3)
[x1_2dot , x3_2dot]=solve(ptth1,ptth2,x1_2dot,x3_2dot)
%%%%%%%%%%%%%ketqua

function [x1_dot,x2_dot,x3_dot,x4_dot] = fcn(x1,x2,x3,x4,u1,u2)

m1=5;m2=3; l1= 0,5;l2=0,34;g=9.81;
x1_dot = x2;
x2_dot = (l2^2*u1 - l2^2*u2 - l1*u2*cos(x3) - g*l1*l2^2*m1*sin(x1) -
g*l1*l2^2*m2*sin(x1) + l1*l2^3*m2*x2^2*sin(x3) + l1*l2^3*m2*x4^2*sin(x3) +
l1^2*l2*m2*x2^2*cos(x3)*sin(x3) + g*l1*l2*m2*sin(x1 + x3)*cos(x3) +
2*l1*l2^3*m2*x2*x4*sin(x3))/(l1^2*l2^2*m1 + l1^2*l2^2*m2 - l1^2*l2*m2*cos(x3)^2 -
l1*l2^2*m2*cos(x3) + l1*l2^3*m2*cos(x3)) ;
x3_2dot = x4;
x4_dot = -(l2^2*m2*u1 - l1^2*m2*u2 - l1^2*m1*u2 - l2^2*m2*u2 +
l1*l2^3*m2^2*x2^2*sin(x3) + l1^3*l2*m2^2*x2^2*sin(x3) + l1*l2^3*m2^2*x4^2*sin(x3) +
g*l1^2*l2*m2^2*sin(x1 + x3) - g*l1*l2^2*m2^2*sin(x1) + l1*l2*m2*u1*cos(x3) -
2*l1*l2*m2*u2*cos(x3) + l1^3*l2*m1*m2*x2^2*sin(x3) + 2*l1*l2^3*m2^2*x2*x4*sin(x3) +
2*l1^2*l2^2*m2^2*x2^2*cos(x3)*sin(x3) + l1^2*l2^2*m2^2*x4^2*cos(x3)*sin(x3) +
g*l1*l2^2*m2^2*sin(x1 + x3)*cos(x3) - g*l1^2*l2*m2^2*cos(x3)*sin(x1) +
g*l1^2*l2*m1*m2*sin(x1 + x3) - g*l1*l2^2*m1*m2*sin(x1) +
2*l1^2*l2^2*m2^2*x2*x4*cos(x3)*sin(x3) -
g*l1^2*l2*m1*m2*cos(x3)*sin(x1))/(l1^2*l2^2*m2^2 + m1*l1^2*l2^2*m2 -
l1^2*l2*m2^2*cos(x3)^2 + l1*l2^3*m2^2*cos(x3) - l1*l2^2*m2^2*cos(x3));

function [u1,u2] = fcn(x1,x2,x3,x4,c1,c2,c3,c4,nuy1,nuy2)

m1=5;m2=3; l1= 0,5;l2=0,34;g=9.81;
x1_dot = x2;
x2_dot = (l2^2*u1 - l2^2*u2 - l1*u2*cos(x3) - g*l1*l2^2*m1*sin(x1) -
g*l1*l2^2*m2*sin(x1) + l1*l2^3*m2*x2^2*sin(x3) + l1*l2^3*m2*x4^2*sin(x3) +
l1^2*l2*m2*x2^2*cos(x3)*sin(x3) + g*l1*l2*m2*sin(x1 + x3)*cos(x3) +
2*l1*l2^3*m2*x2*x4*sin(x3))/(l1^2*l2^2*m1 + l1^2*l2^2*m2 - l1^2*l2*m2*cos(x3)^2 -
l1*l2^2*m2*cos(x3) + l1*l2^3*m2*cos(x3)) ;
x3_2dot = x4;
x4_dot = -(l2^2*m2*u1 - l1^2*m2*u2 - l1^2*m1*u2 - l2^2*m2*u2 +
l1*l2^3*m2^2*x2^2*sin(x3) + l1^3*l2*m2^2*x2^2*sin(x3) + l1*l2^3*m2^2*x4^2*sin(x3) +
g*l1^2*l2*m2^2*sin(x1 + x3) - g*l1*l2^2*m2^2*sin(x1) + l1*l2*m2*u1*cos(x3) -
2*l1*l2*m2*u2*cos(x3) + l1^3*l2*m1*m2*x2^2*sin(x3) + 2*l1*l2^3*m2^2*x2*x4*sin(x3) +
2*l1^2*l2^2*m2^2*x2^2*cos(x3)*sin(x3) + l1^2*l2^2*m2^2*x4^2*cos(x3)*sin(x3) +
g*l1*l2^2*m2^2*sin(x1 + x3)*cos(x3) - g*l1^2*l2*m2^2*cos(x3)*sin(x1) +
g*l1^2*l2*m1*m2*sin(x1 + x3) - g*l1*l2^2*m1*m2*sin(x1) +
2*l1^2*l2^2*m2^2*x2*x4*cos(x3)*sin(x3) -
g*l1^2*l2*m1*m2*cos(x3)*sin(x1))/(l1^2*l2^2*m2^2 + m1*l1^2*l2^2*m2 -
l1^2*l2*m2^2*cos(x3)^2 + l1*l2^3*m2^2*cos(x3) - l1*l2^2*m2^2*cos(x3));

g1=diff(x2_dot,u1);g1=simplify(g1)
h1=diff(x2_dot,u2);h1=simplify(h1)
f1 = x2_dot-g1*u1-h1*u2;f1=simplify(f1)
g2=diff(x4_dot,u1);g2=simplify(g2)
h2=diff(x4_dot,u2);h2=simplify(h2)
f2 = x4_dot-g2*u1-h2*u2;f2=simplify(f2)
%%%%%%%%%%%%ketqua
clc; clear all;
syms m1 m2 l1 l2 g x1 x2 x3 x4 x1d x1d_dot x1d_2dot x3d x3d_dot x3d_2dot
g1 =l2/(l1*(l2^2*m2*cos(x3) - l1*m2*cos(x3)^2 - l2*m2*cos(x3) + l1*l2*m1 +
l1*l2*m2))
h1 =-(l2^2 + l1*cos(x3))/(l1*l2*(l2^2*m2*cos(x3) - l1*m2*cos(x3)^2 - l2*m2*cos(x3)
+ l1*l2*m1 + l1*l2*m2))
f1 =(g*m2*sin(x1) + g*m2*sin(x1 + 2*x3) + l1*m2*x2^2*sin(2*x3) +
2*l2^2*m2*x2^2*sin(x3) + 2*l2^2*m2*x4^2*sin(x3) - 2*g*l2*m1*sin(x1) -
2*g*l2*m2*sin(x1) + 4*l2^2*m2*x2*x4*sin(x3))/(2*l2^2*m2*cos(x3) - 2*l1*m2*cos(x3)^2
- 2*l2*m2*cos(x3) + 2*l1*l2*m1 + 2*l1*l2*m2)
g2 =-(l2 + l1*cos(x3))/(l1*(l2^2*m2*cos(x3) - l1*m2*cos(x3)^2 - l2*m2*cos(x3) +
l1*l2*m1 + l1*l2*m2))
h2 =(l1^2*m1 + l1^2*m2 + l2^2*m2 + 2*l1*l2*m2*cos(x3))/(l1*l2*m2*(l2^2*m2*cos(x3) -
l1*m2*cos(x3)^2 - l2*m2*cos(x3) + l1*l2*m1 + l1*l2*m2))
f2 =-(l1^2*m1*x2^2*sin(x3) + l1^2*m2*x2^2*sin(x3) + l2^2*m2*x2^2*sin(x3) +
l2^2*m2*x4^2*sin(x3) - g*l2*m1*sin(x1) - g*l2*m2*sin(x1) + g*l1*m1*cos(x1)*sin(x3)
+ g*l1*m2*cos(x1)*sin(x3) + 2*l2^2*m2*x2*x4*sin(x3) + g*l2*m2*cos(x3)^2*sin(x1) +
l1*l2*m2*x2^2*sin(2*x3) + (l1*l2*m2*x4^2*sin(2*x3))/2 +
g*l2*m2*cos(x1)*cos(x3)*sin(x3) + l1*l2*m2*x2*x4*sin(2*x3))/(l2^2*m2*cos(x3) -
l1*m2*cos(x3)^2 - l2*m2*cos(x3) + l1*l2*m1 + l1*l2*m2)
 clc; clear all;
syms m1 m2 l1 l2 g x1 x2 x3 x4 x1d x1d_dot x1d_2dot x3d x3d_dot x3d_2dot k u1 u2
nuy1 nuy2
g1 =l2/(l1*(l2^2*m2*cos(x3) - l1*m2*cos(x3)^2 - l2*m2*cos(x3) + l1*l2*m1 +
l1*l2*m2))
h1 =-(l2^2 + l1*cos(x3))/(l1*l2*(l2^2*m2*cos(x3) - l1*m2*cos(x3)^2 - l2*m2*cos(x3)
+ l1*l2*m1 + l1*l2*m2))
f1 =(g*m2*sin(x1) + g*m2*sin(x1 + 2*x3) + l1*m2*x2^2*sin(2*x3) +
2*l2^2*m2*x2^2*sin(x3) + 2*l2^2*m2*x4^2*sin(x3) - 2*g*l2*m1*sin(x1) -
2*g*l2*m2*sin(x1) + 4*l2^2*m2*x2*x4*sin(x3))/(2*l2^2*m2*cos(x3) - 2*l1*m2*cos(x3)^2
- 2*l2*m2*cos(x3) + 2*l1*l2*m1 + 2*l1*l2*m2)
g2 =-(l2 + l1*cos(x3))/(l1*(l2^2*m2*cos(x3) - l1*m2*cos(x3)^2 - l2*m2*cos(x3) +
l1*l2*m1 + l1*l2*m2))
h2 =(l1^2*m1 + l1^2*m2 + l2^2*m2 + 2*l1*l2*m2*cos(x3))/(l1*l2*m2*(l2^2*m2*cos(x3) -
l1*m2*cos(x3)^2 - l2*m2*cos(x3) + l1*l2*m1 + l1*l2*m2))
f2 =-(l1^2*m1*x2^2*sin(x3) + l1^2*m2*x2^2*sin(x3) + l2^2*m2*x2^2*sin(x3) +
l2^2*m2*x4^2*sin(x3) - g*l2*m1*sin(x1) - g*l2*m2*sin(x1) + g*l1*m1*cos(x1)*sin(x3)
+ g*l1*m2*cos(x1)*sin(x3) + 2*l2^2*m2*x2*x4*sin(x3) + g*l2*m2*cos(x3)^2*sin(x1) +
l1*l2*m2*x2^2*sin(2*x3) + (l1*l2*m2*x4^2*sin(2*x3))/2 +
g*l2*m2*cos(x1)*cos(x3)*sin(x3) + l1*l2*m2*x2*x4*sin(2*x3))/(l2^2*m2*cos(x3) -
l1*m2*cos(x3)^2 - l2*m2*cos(x3) + l1*l2*m1 + l1*l2*m2)
e1 = x1 - x1d; e3=x3-x3d; e1_dot=x2-x1d_dot;e3_dot =x4-x3d_dot;
s1=e1+lamda*e1_dot;s3 = e3+lamda*e3_dot;
S1 = s1 ; S2 = s3;
%%%%%%%%%%%
pt_truot_1=c1*x2-c1*x1d_dot+c2*[f1+g1*u1+h1*u2]-c2*x1d_2dot+nuy1*sign(S1)
pt_truot_2=c3*x4-c3*x3d_dot+c4*[f2+g2*u1+h2*u2]-c4*x3d_2dot+nuy2*sign(S2)
[u1,u2]=solve(pt_truot_1 ,pt_truot_2,u1,u2)
u1=simplify(u1)
u2=simplify(u2)
%%%%%%%%%%%%%%%%%ketqua
u1 = (c1*c4*l1^2*m1*x1d_dot - c4*l1^2*m2*nuy1*sign(c1*x1 + c2*x2 - c1*x1d -
c2*x1d_dot) - c4*l2^2*m2*nuy1*sign(c1*x1 + c2*x2 - c1*x1d - c2*x1d_dot) -
c2*l2^2*m2*nuy2*sign(c3*x3 + c4*x4 - c3*x3d - c4*x3d_dot) - c1*c4*l1^2*m1*x2 -
c1*c4*l1^2*m2*x2 - c1*c4*l2^2*m2*x2 - c2*c3*l2^2*m2*x4 - c4*l1^2*m1*nuy1*sign(c1*x1
+ c2*x2 - c1*x1d - c2*x1d_dot) + c1*c4*l1^2*m2*x1d_dot + c1*c4*l2^2*m2*x1d_dot +
c2*c3*l2^2*m2*x3d_dot + c2*c4*l1^2*m1*x1d_2dot + c2*c4*l1^2*m2*x1d_2dot +
c2*c4*l2^2*m2*x1d_2dot + c2*c4*l2^2*m2*x3d_2dot + c2*c4*g*l2*m2*sin(x1 + x3) +
c2*c4*g*l1*m1*sin(x1) + c2*c4*g*l1*m2*sin(x1) - c2*c3*l1*m2*x4*cos(x3) +
c2*c3*l1*m2*x3d_dot*cos(x3) + c2*c4*l1*m2*x3d_2dot*cos(x3) -
c2*l1*m2*nuy2*sign(c3*x3 + c4*x4 - c3*x3d - c4*x3d_dot)*cos(x3) -
2*c4*l1*l2*m2*nuy1*sign(c1*x1 + c2*x2 - c1*x1d - c2*x1d_dot)*cos(x3) -
c2*c4*l1*l2*m2*x4^2*sin(x3) - 2*c1*c4*l1*l2*m2*x2*cos(x3) +
2*c1*c4*l1*l2*m2*x1d_dot*cos(x3) + 2*c2*c4*l1*l2*m2*x1d_2dot*cos(x3) -
2*c2*c4*l1*l2*m2*x2*x4*sin(x3))/(c2*c4)
u2 = (l2*m2*(2*c1*c4*l2*x1d_dot - 2*c2*c3*l2*x4 - 2*c1*c4*l2*x2 +
2*c2*c3*l2*x3d_dot + 2*c2*c4*l2*x1d_2dot + 2*c2*c4*l2*x3d_2dot + 2*c2*c4*g*sin(x1 +
x3) - 2*c4*l2*nuy1*sign(c1*x1 + c2*x2 - c1*x1d - c2*x1d_dot) -
2*c2*l2*nuy2*sign(c3*x3 + c4*x4 - c3*x3d - c4*x3d_dot) - 2*c4*l1*nuy1*sign(c1*x1 +
c2*x2 - c1*x1d - c2*x1d_dot)*cos(x3) + 2*c2*c4*l1*x2^2*sin(x3) -
2*c1*c4*l1*x2*cos(x3) + 2*c1*c4*l1*x1d_dot*cos(x3) +
2*c2*c4*l1*x1d_2dot*cos(x3)))/(2*c2*c4)