%**************************************************************************
%       User documentation of any code that uses this code must cite the
%       Authors in following article:
%
%       M. H. Korayem and S. R. Nekoo, “Controller design of cooperative
%       manipulators using state-dependent Riccati equation,”
%       Robotica,....
%
%       Permission to modify this file (ONLY) and to distribute modified
%       code is granted, provided the above notices are retained, and a
%       notice that the code was modified is included with the above
%       statements.
%
%       NOTE: This MATLAB code is provided free of charge.
%       You may make copies of the codes, but this NOTICE must appear in
%       all copies.
%**************************************************************************
function Run
clc
clear
close all
global al1 al2 al3 ar1 ar2 ar3 acl1 acl2 acl3 acr1 acr2 acr3 ml1 ml2 ml3...
    mr1 mr2 mr3 mel mer Izzl1 Izzl2 Izzl3 Izzel Izzr1 Izzr2 Izzr3 Izzer...
    b_v_l1 b_v_l2 b_v_l3 b_d_l1 b_d_l2 b_d_l3 b_s_l1 b_s_l2 b_s_l3 epsilon...
    b_v_r1 b_v_r2 b_v_r3 b_d_r1 b_d_r2 b_d_r3 b_s_r1 b_s_r2 b_s_r3...
    Q_l R_l Q_r R_r n g0 ustall_l omeganl_l ustall_r omeganl_r...
    psi_i_l psi_i_r xradius_l yradius_l xradius_r yradius_r mp Izzp...
    Q_bar_l Q_bar_r
%Time----------------------------------------------------------------------
tf=pi;
N=999;
%Path----------------------------------------------------------------------
xradius_l=0.5;
yradius_l=0.5;
xcenter_l=0;
ycenter_l=0;

xradius_r=xradius_l;
yradius_r=yradius_l;
xcenter_r=xcenter_l;
ycenter_r=ycenter_l;
%Initial points------------------------------------------------------------
xi_l=xradius_l+xcenter_l;
yi_l=ycenter_l;
psi_i_l=0;                            %Defines orientation of object

xi_r=xradius_r+xcenter_r;
yi_r=ycenter_r;
psi_i_r=pi;
%Object dimension----------------------------------------------------------
b=0.2;
l=0.4;
mp=10.2;
Izzp=1/12*mp*(b^2+l^2);
mel=0;
mer=0;
Izzel=0;
Izzer=0;
%No. of links--------------------------------------------------------------
n=3;
%R and Q-------------------------------------------------------------------
Q_l=10000*diag([ones(1,n),zeros(1,n)]);
R_l=0.001*eye(n);

Q_r=10000*diag([ones(1,n),zeros(1,n)]);
R_r=0.001*eye(n);

Q_bar_l=0.49*eye(n);
Q_bar_r=eye(n);
%Parameters----------------------------------------------------------------
d=1.5;
al1=1.5;
al2=1;
al3=0.25;
acl1=al1/2;
acl2=al2/2;
acl3=al3/2;
ml1=2;
ml2=1.5;
ml3=1;
Izzl1=1/12*ml1*al1^2;
Izzl2=1/12*ml2*al2^2;
Izzl3=1/12*ml3*al3^2;
b_v_l1=0.05;
b_v_l2=0.04;
b_v_l3=0.03;
b_d_l1=0.1*b_v_l1;
b_d_l2=0.1*b_v_l2;
b_d_l3=0.1*b_v_l3;
b_s_l1=0.05*b_v_l1;
b_s_l2=0.05*b_v_l2;
b_s_l3=0.05*b_v_l3;

ar1=1.5;
ar2=1;
ar3=0.25;
acr1=ar1/2;
acr2=ar2/2;
acr3=ar3/2;
mr1=2;
mr2=1.5;
mr3=1;
Izzr1=1/12*mr1*ar1^2;
Izzr2=1/12*mr2*ar2^2;
Izzr3=1/12*mr3*ar3^2;
b_v_r1=0.05;
b_v_r2=0.04;
b_v_r3=0.03;
b_d_r1=0.1*b_v_r1;
b_d_r2=0.1*b_v_r2;
b_d_r3=0.1*b_v_r3;
b_s_r1=0.05*b_v_r1;
b_s_r2=0.05*b_v_r2;
b_s_r3=0.05*b_v_r3;

epsilon=0.1;
g0=9.81;
%Limits of motors----------------------------------------------------------
ustall_l=[100;85;70];
omeganl_l=[5;5.2;5.5];

ustall_r=[100;85;70];
omeganl_r=[5;5.2;5.5];
%Inverse kinematic---------------------------------------------------------
xib_l=d+xi_l;
xai_l=xib_l-al3*cos(psi_i_l);
yai_l=yi_l-al3*sin(psi_i_l);
qinl2=real(acos((xai_l^2+yai_l^2-al1^2-al2^2)/(2*al1*al2)));
qinl1=real(atan2(yai_l,xai_l))-real(atan2(al2*sin(qinl2),(al1+al2*cos(qinl2))));
qinl3=psi_i_l-qinl1-qinl2;
qinl4=0;
qinl5=0;
qinl6=0;

xib_r=-d+xi_r;
xai_r=xib_r-ar3*cos(psi_i_r);
yai_r=yi_r-ar3*sin(psi_i_r);
qinr2=real(-acos((xai_r^2+yai_r^2-ar1^2-ar2^2)/(2*ar1*ar2)));
qinr1=real(atan2(yai_r,xai_r))-real(atan2(ar2*sin(qinr2),(ar1+ar2*cos(qinr2))));
qinr3=psi_i_r-qinr1-qinr2;
qinr4=0;
qinr5=0;
qinr6=0;

qin=[qinl1;qinl2;qinl3;qinr1;qinr2;qinr3;...
    qinl4;qinl5;qinl6;qinr4;qinr5;qinr6;...
    qinl1;qinl2;qinl3;qinr1;qinr2;qinr3];
%ODE-----------------------------------------------------------------------
[t,x]=ode45(@sys,0:tf/N:tf,qin);
save matlab
afFigurePosition=[200,150,380,220];
figure(1)
axesi=axes('Parent',figure(1),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,x(:,1),'k','Linewidth',1.5)
plot(t,x(:,2),'--b','Linewidth',1.5)
plot(t,x(:,3),'-.r','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_l(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('\theta_l_1(t)','\theta_l_2(t)','\theta_l_3(t)')

figure(2)
axesi=axes('Parent',figure(2),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,x(:,4),'k','Linewidth',1.5)
plot(t,x(:,5),'--b','Linewidth',1.5)
plot(t,x(:,6),'-.r','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_r(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('\theta_r_1(t)','\theta_r_2(t)','\theta_r_3(t)')

figure(3)
axesi=axes('Parent',figure(3),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,x(:,7),'k','Linewidth',1.5)
plot(t,x(:,8),'--b','Linewidth',1.5)
plot(t,x(:,9),'-.r','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_l(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('\omega_l_1(t)','\omega_l_2(t)','\omega_l_3(t)')

figure(4)
axesi=axes('Parent',figure(4),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,x(:,10),'k','Linewidth',1.5)
plot(t,x(:,11),'--b','Linewidth',1.5)
plot(t,x(:,12),'-.r','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_r(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('\omega_r_1(t)','\omega_r_2(t)','\omega_r_3(t)')
%Plot path-----------------------------------------------------------------
figure(5)
axesi=axes('Parent',figure(5),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid off;box on;
plot(xi_l,yi_l,'om')
xe_d_l=xradius_l*cos(t)+xcenter_l;
ye_d_l=yradius_l*sin(t)+ycenter_l;
xe_d_r=xradius_r*cos(t)+xcenter_r;
ye_d_r=yradius_r*sin(t)+ycenter_r;
plot(xe_d_l,ye_d_l,':k','Linewidth',1)
xe_l=-d+al2*cos(x(:,1)+x(:,2))+al1*cos(x(:,1))+al3*cos(x(:,1)+x(:,2)+x(:,3));
ye_l=al2*sin(x(:,1)+x(:,2))+al1*sin(x(:,1))+al3*sin(x(:,1)+x(:,2)+x(:,3));
xe_r=d+ar2*cos(x(:,4)+x(:,5))+ar1*cos(x(:,4))+ar3*cos(x(:,4)+x(:,5)+x(:,6));
ye_r=ar2*sin(x(:,4)+x(:,5))+ar1*sin(x(:,4))+ar3*sin(x(:,4)+x(:,5)+x(:,6));
plot(xe_l,ye_l,'b','Linewidth',1.5)
plot(xe_r,ye_r,'--r','Linewidth',1.5)
for i=1:15:60
    xel1=-d+al1*cos(x(i,1));
    yel1=al1*sin(x(i,1));
    xel2=-d+al1*cos(x(i,1))+al2*cos(x(i,1)+x(i,2));
    yel2=al1*sin(x(i,1))+al2*sin(x(i,1)+x(i,2));
    xel3=-d+al1*cos(x(i,1))+al2*cos(x(i,1)+x(i,2))+al3*cos(x(i,1)+x(i,2)+x(i,3));
    yel3=al1*sin(x(i,1))+al2*sin(x(i,1)+x(i,2))+al3*sin(x(i,1)+x(i,2)+x(i,3));
    line([-d,xel1],[0,yel1],'color',[0.537254901960784 0.784313725490196 0.996078431372549],'LineStyle','-','Linewidth',1)
    line([xel1,xel2],[yel1,yel2],'color',[0.537254901960784 0.784313725490196 0.996078431372549],'LineStyle','-','Linewidth',1)
    line([xel2,xel3],[yel2,yel3],'color',[0.537254901960784 0.784313725490196 0.996078431372549],'LineStyle','-','Linewidth',1)
    %Right
    xer1=d+ar1*cos(x(i,4));
    yer1=ar1*sin(x(i,4));
    xer2=d+ar1*cos(x(i,4))+ar2*cos(x(i,4)+x(i,5));
    yer2=ar1*sin(x(i,4))+ar2*sin(x(i,4)+x(i,5));
    xer3=d+ar1*cos(x(i,4))+ar2*cos(x(i,4)+x(i,5))+ar3*cos(x(i,4)+x(i,5)+x(i,6));
    yer3=ar1*sin(x(i,4))+ar2*sin(x(i,4)+x(i,5))+ar3*sin(x(i,4)+x(i,5)+x(i,6));
    line([d,xer1],[0,yer1],'color',[0.9608 0.6353 0.6353],'LineStyle','-','Linewidth',1)
    line([xer1,xer2],[yer1,yer2],'color',[0.9608 0.6353 0.6353],'LineStyle','-','Linewidth',1)
    line([xer2,xer3],[yer2,yer3],'color',[0.9608 0.6353 0.6353],'LineStyle','-','Linewidth',1)
end
for i=1:100:length(t)
    %Rectangle
    xe_d_lo=xradius_l*cos(t(i))+xcenter_l;
    ye_d_lo=yradius_l*sin(t(i))+ycenter_l;
    xob1=xe_d_lo+b/2*cos(x(i,13)+x(i,14)+x(i,15)+pi/2);
    yob1=ye_d_lo+b/2*sin(x(i,13)+x(i,14)+x(i,15)+pi/2);
    xob2=xob1+l/2*cos(x(i,13)+x(i,14)+x(i,15));
    yob2=yob1+l/2*sin(x(i,13)+x(i,14)+x(i,15));
    xob3=xob2+b*cos(x(i,13)+x(i,14)+x(i,15)-pi/2);
    yob3=yob2+b*sin(x(i,13)+x(i,14)+x(i,15)-pi/2);
    xob4=xob3-l*cos(x(i,13)+x(i,14)+x(i,15));
    yob4=yob3-l*sin(x(i,13)+x(i,14)+x(i,15));
    xob5=xob4+b*cos(x(i,13)+x(i,14)+x(i,15)+pi/2);
    yob5=yob4+b*sin(x(i,13)+x(i,14)+x(i,15)+pi/2);
    line([xob1,xob2],[yob1,yob2],'color',[0 0 0],'LineStyle','-','Linewidth',1)
    line([xob2,xob3],[yob2,yob3],'color',[0 0 0],'LineStyle','-','Linewidth',1)
    line([xob3,xob4],[yob3,yob4],'color',[0 0 0],'LineStyle','-','Linewidth',1)
    line([xob4,xob5],[yob4,yob5],'color',[0 0 0],'LineStyle','-','Linewidth',1)
    line([xob5,xob1],[yob5,yob1],'color',[0 0 0],'LineStyle','-','Linewidth',1)
end
xlabel('x (m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('y (m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
afFigurePosition=[200,150,380,280];
set(gcf,'Position',afFigurePosition)
axis equal;
legend('Start Point','Desired','Arm Path L','Arm Path R')

Error_L=sqrt((xe_l-xe_d_l).^2+(ye_l-ye_d_l).^2);
Error_R=sqrt((xe_r-xe_d_r).^2+(ye_r-ye_d_r).^2);
afFigurePosition=[200,150,380,220];
figure(14)
axesi=axes('Parent',figure(14),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,Error_L,'b','Linewidth',1.5)
plot(t,Error_R,'--r','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('|Error|','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Left','Right')
%Plot inputs---------------------------------------------------------------
for i=1:length(t)
    ql1=x(i,1);
    ql2=x(i,2);
    ql3=x(i,3);
    qr1=x(i,4);
    qr2=x(i,5);
    qr3=x(i,6);
    dql1=x(i,7);
    dql2=x(i,8);
    dql3=x(i,9);
    dqr1=x(i,10);
    dqr2=x(i,11);
    dqr3=x(i,12);
    qdl1=x(i,13);
    qdl2=x(i,14);
    qdl3=x(i,15);
    qdr1=x(i,16);
    qdr2=x(i,17);
    qdr3=x(i,18);
    q_l=[ql1;ql2;ql3];
    q_r=[qr1;qr2;qr3];
    dq_l=[dql1;dql2;dql3];
    dq_r=[dqr1;dqr2;dqr3];
    %Left arm matrices-----------------------------------------------------
    ml11=mel*al2^2+Izzl1+Izzl2+Izzl3+Izzel+mel*al1^2+ml2*al1^2+mel*al3^2+ml3*al2^2+ml3*acl3^2+ml2*acl2^2+2*ml2*al1*sin(ql1)*acl2*sin(ql1+ql2)+2*ml3*al1*sin(ql1)*al2*sin(ql1+ql2)+2*ml3*al1*sin(ql1)*acl3*sin(ql1+ql2+ql3)+2*ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+2*ml3*al1*cos(ql1)*al2*cos(ql1+ql2)+2*ml2*al1*cos(ql1)*acl2*cos(ql1+ql2)+ml3*al1^2+ml1*acl1^2+2*ml3*al1*cos(ql1)*acl3*cos(ql1+ql2+ql3)+2*ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+2*mel*al1*sin(ql1)*al2*sin(ql1+ql2)+2*mel*al1*cos(ql1)*al2*cos(ql1+ql2)+2*mel*al1*sin(ql1)*al3*sin(ql1+ql2+ql3)+2*mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+2*mel*al1*cos(ql1)*al3*cos(ql1+ql2+ql3)+2*mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3);
    ml12=Izzl2+Izzel+mel*al2^2+ml2*acl2^2+ml3*al2^2+ml3*acl3^2+mel*al3^2+Izzl3+ml2*al1*sin(ql1)*acl2*sin(ql1+ql2)+ml2*al1*cos(ql1)*acl2*cos(ql1+ql2)+ml3*al1*sin(ql1)*al2*sin(ql1+ql2)+ml3*al1*sin(ql1)*acl3*sin(ql1+ql2+ql3)+2*ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+ml3*al1*cos(ql1)*al2*cos(ql1+ql2)+ml3*al1*cos(ql1)*acl3*cos(ql1+ql2+ql3)+2*ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+mel*al1*sin(ql1)*al2*sin(ql1+ql2)+mel*al1*cos(ql1)*al2*cos(ql1+ql2)+mel*al1*sin(ql1)*al3*sin(ql1+ql2+ql3)+2*mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+mel*al1*cos(ql1)*al3*cos(ql1+ql2+ql3)+2*mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3);
    ml13=ml3*al1*sin(ql1)*acl3*sin(ql1+ql2+ql3)+ml3*acl3^2+mel*al3^2+ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+mel*al1*cos(ql1)*al3*cos(ql1+ql2+ql3)+Izzel+mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3)+mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+ml3*al1*cos(ql1)*acl3*cos(ql1+ql2+ql3)+Izzl3+mel*al1*sin(ql1)*al3*sin(ql1+ql2+ql3);
    ml22=2*mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3)+ml2*acl2^2+mel*al3^2+ml3*acl3^2+Izzel+Izzl2+mel*al2^2+Izzl3+ml3*al2^2+2*mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+2*ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+2*ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3);
    ml23=Izzel+mel*al3^2+ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+Izzl3+ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3)+mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*acl3^2;
    ml33=Izzel+ml3*acl3^2+mel*al3^2+Izzl3;

    gl1=g0*(ml2*al1*cos(ql1)+ml2*acl2*cos(ql1+ql2)+mel*al1*cos(ql1)+mel*al3*cos(ql1+ql2+ql3)+ml3*al2*cos(ql1+ql2)+ml1*acl1*cos(ql1)+mel*al2*cos(ql1+ql2)+ml3*al1*cos(ql1)+ml3*acl3*cos(ql1+ql2+ql3));
    gl2=g0*(mel*al2*cos(ql1+ql2)+ml3*acl3*cos(ql1+ql2+ql3)+ml3*al2*cos(ql1+ql2)+ml2*acl2*cos(ql1+ql2)+mel*al3*cos(ql1+ql2+ql3));
    gl3=g0*cos(ql1+ql2+ql3)*(mel*al3+ml3*acl3);

    cl1=2*dql3*mel*al2*sin(ql1+ql2)*dql2*al3*cos(ql1+ql2+ql3)+2*dql3*mel*dql1*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)+2*dql3*mel*dql1*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)-ml3*al1*cos(ql1)*dql2^2*al2*sin(ql1+ql2)+ml3*al1*sin(ql1)*dql2^2*acl3*cos(ql1+ql2+ql3)+ml2*al1*sin(ql1)*acl2*cos(ql1+ql2)*dql2^2+mel*al1*sin(ql1)*dql2^2*al3*cos(ql1+ql2+ql3)-mel*al1*cos(ql1)*dql2^2*al3*sin(ql1+ql2+ql3)+ml3*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)*dql3^2+ml3*al1*sin(ql1)*dql2^2*al2*cos(ql1+ql2)-ml3*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*dql1*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)+ml3*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)*dql3^2-ml3*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*dql1*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)-2*dql2*ml3*dql1*al1*cos(ql1)*al2*sin(ql1+ql2)-ml2*al1*cos(ql1)*acl2*sin(ql1+ql2)*dql2^2+mel*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)*dql3^2+mel*al1*sin(ql1)*dql2^2*al2*cos(ql1+ql2)-mel*al1*cos(ql1)*dql2^2*al2*sin(ql1+ql2)-ml3*al1*cos(ql1)*dql2^2*acl3*sin(ql1+ql2+ql3)+2*dql2*ml3*dql1*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)-mel*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)*dql3^2+2*dql2*ml2*dql1*al1*sin(ql1)*acl2*cos(ql1+ql2)+2*dql2*ml3*dql1*al1*sin(ql1)*al2*cos(ql1+ql2)+mel*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)*dql3^2-mel*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)*dql3^2-2*dql2*ml3*dql1*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)-2*dql3*mel*al2*cos(ql1+ql2)*dql2*al3*sin(ql1+ql2+ql3)-2*dql2*ml2*dql1*al1*cos(ql1)*acl2*sin(ql1+ql2)+2*dql2*mel*dql1*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)-2*dql2*ml3*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)*dql3+2*dql3*ml3*dql1*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+2*dql3*ml3*dql1*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)-2*dql2*mel*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)*dql3-2*dql2*mel*dql1*al1*cos(ql1)*al2*sin(ql1+ql2)+2*dql2*ml3*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)*dql3-2*dql2*mel*dql1*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)+2*dql2*mel*dql1*al1*sin(ql1)*al2*cos(ql1+ql2)+2*dql2*mel*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)*dql3+2*dql3*ml3*al2*sin(ql1+ql2)*dql2*acl3*cos(ql1+ql2+ql3)-2*dql3*ml3*dql1*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)-2*dql3*ml3*dql1*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)-2*dql3*ml3*al2*cos(ql1+ql2)*dql2*acl3*sin(ql1+ql2+ql3);
    cl2=mel*dql1^2*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)-ml3*dql1^2*al1*sin(ql1)*al2*cos(ql1+ql2)+2*dql3*mel*al2*sin(ql1+ql2)*dql2*al3*cos(ql1+ql2+ql3)+2*dql3*mel*dql1*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)-ml3*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*dql1*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*dql1^2*al1*cos(ql1)*al2*sin(ql1+ql2)+ml2*dql1^2*al1*cos(ql1)*acl2*sin(ql1+ql2)-mel*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*al2*cos(ql1+ql2)*dql2*al3*sin(ql1+ql2+ql3)+2*dql3*ml3*dql1*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)-ml3*dql1^2*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)+2*dql3*ml3*al2*sin(ql1+ql2)*dql2*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al1*sin(ql1)*al2*cos(ql1+ql2)-ml2*dql1^2*al1*sin(ql1)*acl2*cos(ql1+ql2)+ml3*dql1^2*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)+mel*dql1^2*al1*cos(ql1)*al2*sin(ql1+ql2)-2*dql3*ml3*dql1*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)-2*dql3*ml3*al2*cos(ql1+ql2)*dql2*acl3*sin(ql1+ql2+ql3)+ml3*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)*dql3^2+mel*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)*dql3^2;
    cl3=mel*dql1^2*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)-2*ml3*dql1*al2*sin(ql1+ql2)*dql2*acl3*cos(ql1+ql2+ql3)-ml3*dql1^2*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)+2*mel*dql1*al2*cos(ql1+ql2)*dql2*al3*sin(ql1+ql2+ql3)-2*mel*dql1*al2*sin(ql1+ql2)*dql2*al3*cos(ql1+ql2+ql3)+2*ml3*dql1*al2*cos(ql1+ql2)*dql2*acl3*sin(ql1+ql2+ql3)-mel*dql2^2*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)-ml3*dql2^2*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)-ml3*dql1^2*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)+ml3*dql1^2*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)+ml3*dql2^2*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+mel*dql1^2*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)+mel*dql2^2*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*dql1^2*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3);

    cl11=-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql2*ml3*al1*al2*sin(ql2)-2*dql3*ml3*al1*acl3*sin(ql2+ql3)-2*dql2*ml2*al1*acl2*sin(ql2)-2*dql3*mel*al1*al3*sin(ql2+ql3)-2*dql2*mel*al1*al2*sin(ql2)-2*dql2*mel*al1*al3*sin(ql2+ql3)-2*dql2*ml3*al1*acl3*sin(ql2+ql3)-2*dql3*mel*al2*al3*sin(ql3);
    cl12=-dql2*mel*al1*al2*sin(ql2)-dql2*mel*al1*al3*sin(ql2+ql3)-dql2*ml2*al1*acl2*sin(ql2)-dql2*ml3*al1*acl3*sin(ql2+ql3)-dql2*ml3*al1*al2*sin(ql2)-2*dql3*mel*al2*al3*sin(ql3)-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql3*ml3*al1*acl3*sin(ql2+ql3)-2*dql3*mel*al1*al3*sin(ql2+ql3);
    cl13=-dql3*mel*al1*al3*sin(ql2+ql3)-dql3*ml3*al2*acl3*sin(ql3)-dql3*mel*al2*al3*sin(ql3)-dql3*ml3*al1*acl3*sin(ql2+ql3);
    cl21=al1*dql1*ml3*acl3*sin(ql2+ql3)+al1*dql1*ml3*al2*sin(ql2)+al1*dql1*mel*al2*sin(ql2)+al1*dql1*mel*al3*sin(ql2+ql3)+al1*dql1*ml2*acl2*sin(ql2)-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql3*mel*al2*al3*sin(ql3);
    cl22=-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql3*mel*al2*al3*sin(ql3);
    cl23=-dql3*mel*al2*al3*sin(ql3)-dql3*ml3*al2*acl3*sin(ql3);
    cl31=2*ml3*al2*dql2*acl3*sin(ql3)+2*mel*al2*dql2*al3*sin(ql3)+dql1*al2*mel*al3*sin(ql3)+dql1*mel*al1*al3*sin(ql2+ql3)+dql1*ml3*al2*acl3*sin(ql3)+dql1*ml3*al1*acl3*sin(ql2+ql3);
    cl32=ml3*al2*dql2*acl3*sin(ql3)+mel*al2*dql2*al3*sin(ql3);
    cl33=0;

    M_l=[ml11,ml12,ml13;...
        ml12,ml22,ml23;...
        ml13,ml23,ml33];

    g_l=[gl1;gl2;gl3];

    c_l=[cl1;cl2;cl3];

    C_bar_l=[cl11,cl12,cl13;...
        cl21,cl22,cl23;...
        cl31,cl32,cl33];

    b_l=[b_v_l1*dql1+sign(dql1)*(b_d_l1+(b_s_l1-b_d_l1)*exp(-abs(dql1)/epsilon));...
        b_v_l2*dql2+sign(dql2)*(b_d_l2+(b_s_l2-b_d_l2)*exp(-abs(dql2)/epsilon));...
        b_v_l3*dql3+sign(dql3)*(b_d_l3+(b_s_l3-b_d_l3)*exp(-abs(dql3)/epsilon))];

    b_SDC_l=[sign(dql1)*(b_d_l1+(b_s_l1-b_d_l1)*exp(-abs(dql1)/epsilon));...
        sign(dql2)*(b_d_l2+(b_s_l2-b_d_l2)*exp(-abs(dql2)/epsilon));...
        sign(dql3)*(b_d_l3+(b_s_l3-b_d_l3)*exp(-abs(dql3)/epsilon))];
    %Right arm matrices----------------------------------------------------
    mr11=mer*ar2^2+Izzr1+Izzr2+Izzr3+Izzer+mer*ar1^2+mr2*ar1^2+mer*ar3^2+mr3*ar2^2+mr3*acr3^2+mr2*acr2^2+2*mr2*ar1*sin(qr1)*acr2*sin(qr1+qr2)+2*mr3*ar1*sin(qr1)*ar2*sin(qr1+qr2)+2*mr3*ar1*sin(qr1)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar1*cos(qr1)*ar2*cos(qr1+qr2)+2*mr2*ar1*cos(qr1)*acr2*cos(qr1+qr2)+mr3*ar1^2+mr1*acr1^2+2*mr3*ar1*cos(qr1)*acr3*cos(qr1+qr2+qr3)+2*mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+2*mer*ar1*sin(qr1)*ar2*sin(qr1+qr2)+2*mer*ar1*cos(qr1)*ar2*cos(qr1+qr2)+2*mer*ar1*sin(qr1)*ar3*sin(qr1+qr2+qr3)+2*mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+2*mer*ar1*cos(qr1)*ar3*cos(qr1+qr2+qr3)+2*mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3);
    mr12=Izzr2+Izzer+mer*ar2^2+mr2*acr2^2+mr3*ar2^2+mr3*acr3^2+mer*ar3^2+Izzr3+mr2*ar1*sin(qr1)*acr2*sin(qr1+qr2)+mr2*ar1*cos(qr1)*acr2*cos(qr1+qr2)+mr3*ar1*sin(qr1)*ar2*sin(qr1+qr2)+mr3*ar1*sin(qr1)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+mr3*ar1*cos(qr1)*ar2*cos(qr1+qr2)+mr3*ar1*cos(qr1)*acr3*cos(qr1+qr2+qr3)+2*mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+mer*ar1*sin(qr1)*ar2*sin(qr1+qr2)+mer*ar1*cos(qr1)*ar2*cos(qr1+qr2)+mer*ar1*sin(qr1)*ar3*sin(qr1+qr2+qr3)+2*mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mer*ar1*cos(qr1)*ar3*cos(qr1+qr2+qr3)+2*mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3);
    mr13=mr3*ar1*sin(qr1)*acr3*sin(qr1+qr2+qr3)+mr3*acr3^2+mer*ar3^2+mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+mer*ar1*cos(qr1)*ar3*cos(qr1+qr2+qr3)+Izzer+mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+mr3*ar1*cos(qr1)*acr3*cos(qr1+qr2+qr3)+Izzr3+mer*ar1*sin(qr1)*ar3*sin(qr1+qr2+qr3);
    mr22=2*mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+mr2*acr2^2+mer*ar3^2+mr3*acr3^2+Izzer+Izzr2+mer*ar2^2+Izzr3+mr3*ar2^2+2*mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+2*mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3);
    mr23=Izzer+mer*ar3^2+mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+Izzr3+mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*acr3^2;
    mr33=Izzer+mr3*acr3^2+mer*ar3^2+Izzr3;

    gr1=g0*(mr2*ar1*cos(qr1)+mr2*acr2*cos(qr1+qr2)+mer*ar1*cos(qr1)+mer*ar3*cos(qr1+qr2+qr3)+mr3*ar2*cos(qr1+qr2)+mr1*acr1*cos(qr1)+mer*ar2*cos(qr1+qr2)+mr3*ar1*cos(qr1)+mr3*acr3*cos(qr1+qr2+qr3));
    gr2=g0*(mer*ar2*cos(qr1+qr2)+mr3*acr3*cos(qr1+qr2+qr3)+mr3*ar2*cos(qr1+qr2)+mr2*acr2*cos(qr1+qr2)+mer*ar3*cos(qr1+qr2+qr3));
    gr3=g0*cos(qr1+qr2+qr3)*(mer*ar3+mr3*acr3);

    cr1=2*dqr3*mer*ar2*sin(qr1+qr2)*dqr2*ar3*cos(qr1+qr2+qr3)+2*dqr3*mer*dqr1*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)+2*dqr3*mer*dqr1*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)-mr3*ar1*cos(qr1)*dqr2^2*ar2*sin(qr1+qr2)+mr3*ar1*sin(qr1)*dqr2^2*acr3*cos(qr1+qr2+qr3)+mr2*ar1*sin(qr1)*acr2*cos(qr1+qr2)*dqr2^2+mer*ar1*sin(qr1)*dqr2^2*ar3*cos(qr1+qr2+qr3)-mer*ar1*cos(qr1)*dqr2^2*ar3*sin(qr1+qr2+qr3)+mr3*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)*dqr3^2+mr3*ar1*sin(qr1)*dqr2^2*ar2*cos(qr1+qr2)-mr3*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*dqr1*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)+mr3*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)*dqr3^2-mr3*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*dqr1*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)-2*dqr2*mr3*dqr1*ar1*cos(qr1)*ar2*sin(qr1+qr2)-mr2*ar1*cos(qr1)*acr2*sin(qr1+qr2)*dqr2^2+mer*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)*dqr3^2+mer*ar1*sin(qr1)*dqr2^2*ar2*cos(qr1+qr2)-mer*ar1*cos(qr1)*dqr2^2*ar2*sin(qr1+qr2)-mr3*ar1*cos(qr1)*dqr2^2*acr3*sin(qr1+qr2+qr3)+2*dqr2*mr3*dqr1*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)-mer*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)*dqr3^2+2*dqr2*mr2*dqr1*ar1*sin(qr1)*acr2*cos(qr1+qr2)+2*dqr2*mr3*dqr1*ar1*sin(qr1)*ar2*cos(qr1+qr2)+mer*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)*dqr3^2-mer*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr2*mr3*dqr1*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mer*ar2*cos(qr1+qr2)*dqr2*ar3*sin(qr1+qr2+qr3)-2*dqr2*mr2*dqr1*ar1*cos(qr1)*acr2*sin(qr1+qr2)+2*dqr2*mer*dqr1*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)-2*dqr2*mr3*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)*dqr3+2*dqr3*mr3*dqr1*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+2*dqr3*mr3*dqr1*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)-2*dqr2*mer*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)*dqr3-2*dqr2*mer*dqr1*ar1*cos(qr1)*ar2*sin(qr1+qr2)+2*dqr2*mr3*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)*dqr3-2*dqr2*mer*dqr1*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)+2*dqr2*mer*dqr1*ar1*sin(qr1)*ar2*cos(qr1+qr2)+2*dqr2*mer*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)*dqr3+2*dqr3*mr3*ar2*sin(qr1+qr2)*dqr2*acr3*cos(qr1+qr2+qr3)-2*dqr3*mr3*dqr1*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mr3*dqr1*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mr3*ar2*cos(qr1+qr2)*dqr2*acr3*sin(qr1+qr2+qr3);
    cr2=mer*dqr1^2*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)-mr3*dqr1^2*ar1*sin(qr1)*ar2*cos(qr1+qr2)+2*dqr3*mer*ar2*sin(qr1+qr2)*dqr2*ar3*cos(qr1+qr2+qr3)+2*dqr3*mer*dqr1*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)-mr3*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*dqr1*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*dqr1^2*ar1*cos(qr1)*ar2*sin(qr1+qr2)+mr2*dqr1^2*ar1*cos(qr1)*acr2*sin(qr1+qr2)-mer*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*ar2*cos(qr1+qr2)*dqr2*ar3*sin(qr1+qr2+qr3)+2*dqr3*mr3*dqr1*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)-mr3*dqr1^2*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)+2*dqr3*mr3*ar2*sin(qr1+qr2)*dqr2*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar1*sin(qr1)*ar2*cos(qr1+qr2)-mr2*dqr1^2*ar1*sin(qr1)*acr2*cos(qr1+qr2)+mr3*dqr1^2*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)+mer*dqr1^2*ar1*cos(qr1)*ar2*sin(qr1+qr2)-2*dqr3*mr3*dqr1*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mr3*ar2*cos(qr1+qr2)*dqr2*acr3*sin(qr1+qr2+qr3)+mr3*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)*dqr3^2+mer*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)*dqr3^2;
    cr3=mer*dqr1^2*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)-2*mr3*dqr1*ar2*sin(qr1+qr2)*dqr2*acr3*cos(qr1+qr2+qr3)-mr3*dqr1^2*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+2*mer*dqr1*ar2*cos(qr1+qr2)*dqr2*ar3*sin(qr1+qr2+qr3)-2*mer*dqr1*ar2*sin(qr1+qr2)*dqr2*ar3*cos(qr1+qr2+qr3)+2*mr3*dqr1*ar2*cos(qr1+qr2)*dqr2*acr3*sin(qr1+qr2+qr3)-mer*dqr2^2*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)-mr3*dqr2^2*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)-mr3*dqr1^2*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)+mr3*dqr1^2*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)+mr3*dqr2^2*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+mer*dqr1^2*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mer*dqr2^2*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*dqr1^2*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3);

    cr11=-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr2*mr3*ar1*ar2*sin(qr2)-2*dqr3*mr3*ar1*acr3*sin(qr2+qr3)-2*dqr2*mr2*ar1*acr2*sin(qr2)-2*dqr3*mer*ar1*ar3*sin(qr2+qr3)-2*dqr2*mer*ar1*ar2*sin(qr2)-2*dqr2*mer*ar1*ar3*sin(qr2+qr3)-2*dqr2*mr3*ar1*acr3*sin(qr2+qr3)-2*dqr3*mer*ar2*ar3*sin(qr3);
    cr12=-dqr2*mer*ar1*ar2*sin(qr2)-dqr2*mer*ar1*ar3*sin(qr2+qr3)-dqr2*mr2*ar1*acr2*sin(qr2)-dqr2*mr3*ar1*acr3*sin(qr2+qr3)-dqr2*mr3*ar1*ar2*sin(qr2)-2*dqr3*mer*ar2*ar3*sin(qr3)-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr3*mr3*ar1*acr3*sin(qr2+qr3)-2*dqr3*mer*ar1*ar3*sin(qr2+qr3);
    cr13=-dqr3*mer*ar1*ar3*sin(qr2+qr3)-dqr3*mr3*ar2*acr3*sin(qr3)-dqr3*mer*ar2*ar3*sin(qr3)-dqr3*mr3*ar1*acr3*sin(qr2+qr3);
    cr21=ar1*dqr1*mr3*acr3*sin(qr2+qr3)+ar1*dqr1*mr3*ar2*sin(qr2)+ar1*dqr1*mer*ar2*sin(qr2)+ar1*dqr1*mer*ar3*sin(qr2+qr3)+ar1*dqr1*mr2*acr2*sin(qr2)-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr3*mer*ar2*ar3*sin(qr3);
    cr22=-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr3*mer*ar2*ar3*sin(qr3);
    cr23=-dqr3*mer*ar2*ar3*sin(qr3)-dqr3*mr3*ar2*acr3*sin(qr3);
    cr31=2*mr3*ar2*dqr2*acr3*sin(qr3)+2*mer*ar2*dqr2*ar3*sin(qr3)+dqr1*ar2*mer*ar3*sin(qr3)+dqr1*mer*ar1*ar3*sin(qr2+qr3)+dqr1*mr3*ar2*acr3*sin(qr3)+dqr1*mr3*ar1*acr3*sin(qr2+qr3);
    cr32=mr3*ar2*dqr2*acr3*sin(qr3)+mer*ar2*dqr2*ar3*sin(qr3);
    cr33=0;

    M_r=[mr11,mr12,mr13;...
        mr12,mr22,mr23;...
        mr13,mr23,mr33];

    g_r=[gr1;gr2;gr3];

    c_r=[cr1;cr2;cr3];

    C_bar_r=[cr11,cr12,cr13;...
        cr21,cr22,cr23;...
        cr31,cr32,cr33];

    b_r=[b_v_r1*dqr1+sign(dqr1)*(b_d_r1+(b_s_r1-b_d_r1)*exp(-abs(dqr1)/epsilon));...
        b_v_r2*dqr2+sign(dqr2)*(b_d_r2+(b_s_r2-b_d_r2)*exp(-abs(dqr2)/epsilon));...
        b_v_r3*dqr3+sign(dqr3)*(b_d_r3+(b_s_r3-b_d_r3)*exp(-abs(dqr3)/epsilon))];

    b_SDC_r=[sign(dqr1)*(b_d_r1+(b_s_r1-b_d_r1)*exp(-abs(dqr1)/epsilon));...
        sign(dqr2)*(b_d_r2+(b_s_r2-b_d_r2)*exp(-abs(dqr2)/epsilon));...
        sign(dqr3)*(b_d_r3+(b_s_r3-b_d_r3)*exp(-abs(dqr3)/epsilon))];
    %External force--------------------------------------------------------
    J_l=[-al2*sin(ql1+ql2)-al1*sin(ql1)-al3*sin(ql1+ql2+ql3),-al2*sin(ql1+ql2)-al3*sin(ql1+ql2+ql3),-al3*sin(ql1+ql2+ql3);...
        al2*cos(ql1+ql2)+al1*cos(ql1)+al3*cos(ql1+ql2+ql3),al2*cos(ql1+ql2)+al3*cos(ql1+ql2+ql3),al3*cos(ql1+ql2+ql3);...
        1,1,1];

    J_l_dot=[-al2*cos(ql1+ql2)*(dql1+dql2)-al1*cos(ql1)*dql1-al3*cos(ql1+ql2+ql3)*(dql1+dql2+dql3),-al2*cos(ql1+ql2)*(dql1+dql2)-al3*cos(ql1+ql2+ql3)*(dql1+dql2+dql3),-al3*cos(ql1+ql2+ql3)*(dql1+dql2+dql3);...
        -al2*sin(ql1+ql2)*(dql1+dql2)-al1*sin(ql1)*dql1-al3*sin(ql1+ql2+ql3)*(dql1+dql2+dql3),-al2*sin(ql1+ql2)*(dql1+dql2)-al3*sin(ql1+ql2+ql3)*(dql1+dql2+dql3),-al3*sin(ql1+ql2+ql3)*(dql1+dql2+dql3);...
        0,0,0];

    J_r=[-ar2*sin(qr1+qr2)-ar1*sin(qr1)-ar3*sin(qr1+qr2+qr3),-ar2*sin(qr1+qr2)-ar3*sin(qr1+qr2+qr3),-ar3*sin(qr1+qr2+qr3);...
        ar2*cos(qr1+qr2)+ar1*cos(qr1)+ar3*cos(qr1+qr2+qr3),ar2*cos(qr1+qr2)+ar3*cos(qr1+qr2+qr3),ar3*cos(qr1+qr2+qr3);...
        1,1,1];

    J_r_dot=[-ar2*cos(qr1+qr2)*(dqr1+dqr2)-ar1*cos(qr1)*dqr1-ar3*cos(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar2*cos(qr1+qr2)*(dqr1+dqr2)-ar3*cos(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar3*cos(qr1+qr2+qr3)*(dqr1+dqr2+dqr3);...
        -ar2*sin(qr1+qr2)*(dqr1+dqr2)-ar1*sin(qr1)*dqr1-ar3*sin(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar2*sin(qr1+qr2)*(dqr1+dqr2)-ar3*sin(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar3*sin(qr1+qr2+qr3)*(dqr1+dqr2+dqr3);...
        0,0,0];

    d2xe_d_l=-xradius_l*cos(t(i));
    d2ye_d_l=-yradius_l*sin(t(i));

    omega_e=730609364650485/36028797018963968*t(i)^4-3442915518944805/18014398509481984*t(i)^3+4056089287967733/9007199254740992*t(i)^2;
    domega_e=730609364650485/9007199254740992*t(i)^3-10328746556834415/18014398509481984*t(i)^2+4056089287967733/4503599627370496*t(i);

    d2pe=[d2xe_d_l;d2ye_d_l;omega_e];

    fe=-[mp*d2xe_d_l;...
        mp*d2ye_d_l+mp*g0;...
        Izzp*domega_e+Izzp*omega_e^2];

    d2q_l=inv(J_l)*(d2pe-J_l_dot*dq_l);
    d2q_r=inv(J_r)*(d2pe-J_r_dot*dq_r);

    SS_l=inv(J_l')*inv(Q_bar_l)*inv(J_l')';
    SS_r=inv(J_r')*inv(Q_bar_r)*inv(J_r')';
    gama=inv(SS_l+SS_r)*(inv(J_l')*(M_l*d2q_l+c_l+g_l+b_l)+...
        inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r)-fe);

    fe_l=inv(J_l')*(M_l*d2q_l+c_l+g_l+b_l-inv(Q_bar_l)*inv(J_l')'*gama);
    fe_r=inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r-inv(Q_bar_r)*inv(J_r')'*gama);
    %SDC-------------------------------------------------------------------
    u_add_l=g_l+b_SDC_l-J_l'*fe_l;
    u_add_r=g_r+b_SDC_r-J_r'*fe_r;

    OMEGA_l=diag([u_add_l(1)/ql1,u_add_l(2)/ql2,u_add_l(3)/ql3]);
    OMEGA_r=diag([u_add_r(1)/qr1,u_add_r(2)/qr2,u_add_r(3)/qr3]);

    A_l=[zeros(n,n),eye(n);...
        OMEGA_l,-inv(M_l)*(C_bar_l+diag([b_v_l1,b_v_l2,b_v_l3]))];
    B_l=[zeros(n,n);inv(M_l)];

    A_r=[zeros(n,n),eye(n);...
        OMEGA_r,-inv(M_r)*(C_bar_r+diag([b_v_r1,b_v_r2,b_v_r3]))];
    B_r=[zeros(n,n);inv(M_r)];
    %Control---------------------------------------------------------------
    K_l=care(A_l,B_l,Q_l,R_l);
    u_l=-inv(R_l)*B_l'*K_l*[q_l-[qdl1;qdl2;qdl3];dq_l];

    K_r=care(A_r,B_r,Q_r,R_r);
    u_r=-inv(R_r)*B_r'*K_r*[q_r-[qdr1;qdr2;qdr3];dq_r];

    if u_l(1) > (ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1)
        u_l(1)=ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1;
    end
    if u_l(1) < (-ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1)
        u_l(1)=-ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1;
    end
    if u_l(2) > (ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2)
        u_l(2)=ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2;
    end
    if u_l(2) < (-ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2)
        u_l(2)=-ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2;
    end
    if u_l(3) > (ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3)
        u_l(3)=ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3;
    end
    if u_l(3) < (-ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3)
        u_l(3)=-ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3;
    end

    if u_r(1) > (ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1)
        u_r(1)=ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1;
    end
    if u_r(1) < (-ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1)
        u_r(1)=-ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1;
    end
    if u_r(2) > (ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2)
        u_r(2)=ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2;
    end
    if u_r(2) < (-ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2)
        u_r(2)=-ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2;
    end
    if u_r(3) > (ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3)
        u_r(3)=ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3;
    end
    if u_r(3) < (-ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3)
        u_r(3)=-ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3;
    end

    uss_l(1:n,i)=u_l;
    uss_r(1:n,i)=u_r;
    norm_u_l(i)=norm(u_l);
    norm_u_r(i)=norm(u_r);
end
figure(6)
axesi=axes('Parent',figure(6),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,ustall_l(1)-ustall_l(1)/omeganl_l(1)*x(:,7),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall_l(1)-ustall_l(1)/omeganl_l(1)*x(:,7),'-.','linewidth',1.5,'Color',[0 0.498 0])
plot(t,uss_l(1,:),'b','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('u_l_1(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_l_1(t)')
figure(7)
axesi=axes('Parent',figure(7),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,ustall_l(2)-ustall_l(2)/omeganl_l(2)*x(:,8),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall_l(2)-ustall_l(2)/omeganl_l(2)*x(:,8),'-.','linewidth',1.5,'Color',[0 0.498 0])
plot(t,uss_l(2,:),'b','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('u_l_2(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_l_2(t)')
figure(8)
axesi=axes('Parent',figure(8),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,ustall_l(3)-ustall_l(3)/omeganl_l(3)*x(:,9),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall_l(3)-ustall_l(3)/omeganl_l(3)*x(:,9),'-.','linewidth',1.5,'Color',[0 0.498 0])
plot(t,uss_l(3,:),'b','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('u_l_3(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_l_3(t)')
figure(10)
axesi=axes('Parent',figure(10),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,ustall_r(1)-ustall_r(1)/omeganl_r(1)*x(:,10),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall_r(1)-ustall_r(1)/omeganl_r(1)*x(:,10),'-.','linewidth',1.5,'Color',[0 0.498 0])
plot(t,uss_r(1,:),'b','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('u_r_1(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_r_1(t)')
figure(11)
axesi=axes('Parent',figure(11),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,ustall_r(2)-ustall_r(2)/omeganl_r(2)*x(:,11),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall_r(2)-ustall_r(2)/omeganl_r(2)*x(:,11),'-.','linewidth',1.5,'Color',[0 0.498 0])
plot(t,uss_r(2,:),'b','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('u_r_2(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_r_2(t)')
figure(12)
axesi=axes('Parent',figure(12),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,ustall_r(3)-ustall_r(3)/omeganl_r(3)*x(:,12),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall_r(3)-ustall_r(3)/omeganl_r(3)*x(:,12),'-.','linewidth',1.5,'Color',[0 0.498 0])
plot(t,uss_r(3,:),'b','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('u_r_3(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_r_3(t)')
figure(13)
axesi=axes('Parent',figure(13),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,norm_u_l,'b','linewidth',1.5)
plot(t,norm_u_r,'--r','linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('|u(t)|','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Left','Right')
%System--------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xdot=sys(t,x)
global al1 al2 al3 ar1 ar2 ar3 acl1 acl2 acl3 acr1 acr2 acr3 ml1 ml2 ml3...
    mr1 mr2 mr3 mel mer Izzl1 Izzl2 Izzl3 Izzel Izzr1 Izzr2 Izzr3 Izzer...
    b_v_l1 b_v_l2 b_v_l3 b_d_l1 b_d_l2 b_d_l3 b_s_l1 b_s_l2 b_s_l3 epsilon...
    b_v_r1 b_v_r2 b_v_r3 b_d_r1 b_d_r2 b_d_r3 b_s_r1 b_s_r2 b_s_r3...
    Q_l R_l Q_r R_r n g0 ustall_l omeganl_l ustall_r omeganl_r...
    xradius_l yradius_l xradius_r yradius_r mp Izzp...
    Q_bar_l Q_bar_r
Time=t
%States--------------------------------------------------------------------
ql1=x(1);
ql2=x(2);
ql3=x(3);
qr1=x(4);
qr2=x(5);
qr3=x(6);
dql1=x(7);
dql2=x(8);
dql3=x(9);
dqr1=x(10);
dqr2=x(11);
dqr3=x(12);
qdl1=x(13);
qdl2=x(14);
qdl3=x(15);
qdr1=x(16);
qdr2=x(17);
qdr3=x(18);
q_l=[ql1;ql2;ql3];
q_r=[qr1;qr2;qr3];
dq_l=[dql1;dql2;dql3];
dq_r=[dqr1;dqr2;dqr3];
%Path----------------------------------------------------------------------
dxe_d_l=-xradius_l*sin(t);
dye_d_l=yradius_l*cos(t);

dxe_d_r=-xradius_r*sin(t);
dye_d_r=yradius_r*cos(t);

omega_e=730609364650485/36028797018963968*t^4-3442915518944805/18014398509481984*t^3+4056089287967733/9007199254740992*t^2;

V_l=[dxe_d_l;...
    dye_d_l;...
    0;...
    0;...
    0;...
    omega_e];

V_r=[dxe_d_r;...
    dye_d_r;...
    0;...
    0;...
    0;...
    omega_e];
%Jacobian desired----------------------------------------------------------
Jd_l=[-al2*sin(qdl1+qdl2)-al1*sin(qdl1)-al3*sin(qdl1+qdl2+qdl3),-al2*sin(qdl1+qdl2)-al3*sin(qdl1+qdl2+qdl3),-al3*sin(qdl1+qdl2+qdl3);...
    al2*cos(qdl1+qdl2)+al1*cos(qdl1)+al3*cos(qdl1+qdl2+qdl3),al2*cos(qdl1+qdl2)+al3*cos(qdl1+qdl2+qdl3),al3*cos(qdl1+qdl2+qdl3);...
    0,0,0;...
    0,0,0;...
    0,0,0;...
    1,1,1];

Jd_r=[-ar2*sin(qdr1+qdr2)-ar1*sin(qdr1)-ar3*sin(qdr1+qdr2+qdr3),-ar2*sin(qdr1+qdr2)-ar3*sin(qdr1+qdr2+qdr3),-ar3*sin(qdr1+qdr2+qdr3);...
    ar2*cos(qdr1+qdr2)+ar1*cos(qdr1)+ar3*cos(qdr1+qdr2+qdr3),ar2*cos(qdr1+qdr2)+ar3*cos(qdr1+qdr2+qdr3),ar3*cos(qdr1+qdr2+qdr3);...
    0,0,0;...
    0,0,0;...
    0,0,0;...
    1,1,1];
%Left arm matrices---------------------------------------------------------
ml11=mel*al2^2+Izzl1+Izzl2+Izzl3+Izzel+mel*al1^2+ml2*al1^2+mel*al3^2+ml3*al2^2+ml3*acl3^2+ml2*acl2^2+2*ml2*al1*sin(ql1)*acl2*sin(ql1+ql2)+2*ml3*al1*sin(ql1)*al2*sin(ql1+ql2)+2*ml3*al1*sin(ql1)*acl3*sin(ql1+ql2+ql3)+2*ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+2*ml3*al1*cos(ql1)*al2*cos(ql1+ql2)+2*ml2*al1*cos(ql1)*acl2*cos(ql1+ql2)+ml3*al1^2+ml1*acl1^2+2*ml3*al1*cos(ql1)*acl3*cos(ql1+ql2+ql3)+2*ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+2*mel*al1*sin(ql1)*al2*sin(ql1+ql2)+2*mel*al1*cos(ql1)*al2*cos(ql1+ql2)+2*mel*al1*sin(ql1)*al3*sin(ql1+ql2+ql3)+2*mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+2*mel*al1*cos(ql1)*al3*cos(ql1+ql2+ql3)+2*mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3);
ml12=Izzl2+Izzel+mel*al2^2+ml2*acl2^2+ml3*al2^2+ml3*acl3^2+mel*al3^2+Izzl3+ml2*al1*sin(ql1)*acl2*sin(ql1+ql2)+ml2*al1*cos(ql1)*acl2*cos(ql1+ql2)+ml3*al1*sin(ql1)*al2*sin(ql1+ql2)+ml3*al1*sin(ql1)*acl3*sin(ql1+ql2+ql3)+2*ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+ml3*al1*cos(ql1)*al2*cos(ql1+ql2)+ml3*al1*cos(ql1)*acl3*cos(ql1+ql2+ql3)+2*ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+mel*al1*sin(ql1)*al2*sin(ql1+ql2)+mel*al1*cos(ql1)*al2*cos(ql1+ql2)+mel*al1*sin(ql1)*al3*sin(ql1+ql2+ql3)+2*mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+mel*al1*cos(ql1)*al3*cos(ql1+ql2+ql3)+2*mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3);
ml13=ml3*al1*sin(ql1)*acl3*sin(ql1+ql2+ql3)+ml3*acl3^2+mel*al3^2+ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+mel*al1*cos(ql1)*al3*cos(ql1+ql2+ql3)+Izzel+mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3)+mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+ml3*al1*cos(ql1)*acl3*cos(ql1+ql2+ql3)+Izzl3+mel*al1*sin(ql1)*al3*sin(ql1+ql2+ql3);
ml22=2*mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3)+ml2*acl2^2+mel*al3^2+ml3*acl3^2+Izzel+Izzl2+mel*al2^2+Izzl3+ml3*al2^2+2*mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+2*ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+2*ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3);
ml23=Izzel+mel*al3^2+ml3*al2*sin(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+Izzl3+ml3*al2*cos(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+mel*al2*cos(ql1+ql2)*al3*cos(ql1+ql2+ql3)+mel*al2*sin(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*acl3^2;
ml33=Izzel+ml3*acl3^2+mel*al3^2+Izzl3;

gl1=g0*(ml2*al1*cos(ql1)+ml2*acl2*cos(ql1+ql2)+mel*al1*cos(ql1)+mel*al3*cos(ql1+ql2+ql3)+ml3*al2*cos(ql1+ql2)+ml1*acl1*cos(ql1)+mel*al2*cos(ql1+ql2)+ml3*al1*cos(ql1)+ml3*acl3*cos(ql1+ql2+ql3));
gl2=g0*(mel*al2*cos(ql1+ql2)+ml3*acl3*cos(ql1+ql2+ql3)+ml3*al2*cos(ql1+ql2)+ml2*acl2*cos(ql1+ql2)+mel*al3*cos(ql1+ql2+ql3));
gl3=g0*cos(ql1+ql2+ql3)*(mel*al3+ml3*acl3);

cl1=2*dql3*mel*al2*sin(ql1+ql2)*dql2*al3*cos(ql1+ql2+ql3)+2*dql3*mel*dql1*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)+2*dql3*mel*dql1*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)-ml3*al1*cos(ql1)*dql2^2*al2*sin(ql1+ql2)+ml3*al1*sin(ql1)*dql2^2*acl3*cos(ql1+ql2+ql3)+ml2*al1*sin(ql1)*acl2*cos(ql1+ql2)*dql2^2+mel*al1*sin(ql1)*dql2^2*al3*cos(ql1+ql2+ql3)-mel*al1*cos(ql1)*dql2^2*al3*sin(ql1+ql2+ql3)+ml3*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)*dql3^2+ml3*al1*sin(ql1)*dql2^2*al2*cos(ql1+ql2)-ml3*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*dql1*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)+ml3*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)*dql3^2-ml3*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*dql1*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)-2*dql2*ml3*dql1*al1*cos(ql1)*al2*sin(ql1+ql2)-ml2*al1*cos(ql1)*acl2*sin(ql1+ql2)*dql2^2+mel*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)*dql3^2+mel*al1*sin(ql1)*dql2^2*al2*cos(ql1+ql2)-mel*al1*cos(ql1)*dql2^2*al2*sin(ql1+ql2)-ml3*al1*cos(ql1)*dql2^2*acl3*sin(ql1+ql2+ql3)+2*dql2*ml3*dql1*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)-mel*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)*dql3^2+2*dql2*ml2*dql1*al1*sin(ql1)*acl2*cos(ql1+ql2)+2*dql2*ml3*dql1*al1*sin(ql1)*al2*cos(ql1+ql2)+mel*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)*dql3^2-mel*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)*dql3^2-2*dql2*ml3*dql1*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)-2*dql3*mel*al2*cos(ql1+ql2)*dql2*al3*sin(ql1+ql2+ql3)-2*dql2*ml2*dql1*al1*cos(ql1)*acl2*sin(ql1+ql2)+2*dql2*mel*dql1*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)-2*dql2*ml3*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)*dql3+2*dql3*ml3*dql1*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)+2*dql3*ml3*dql1*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)-2*dql2*mel*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)*dql3-2*dql2*mel*dql1*al1*cos(ql1)*al2*sin(ql1+ql2)+2*dql2*ml3*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)*dql3-2*dql2*mel*dql1*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)+2*dql2*mel*dql1*al1*sin(ql1)*al2*cos(ql1+ql2)+2*dql2*mel*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)*dql3+2*dql3*ml3*al2*sin(ql1+ql2)*dql2*acl3*cos(ql1+ql2+ql3)-2*dql3*ml3*dql1*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)-2*dql3*ml3*dql1*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)-2*dql3*ml3*al2*cos(ql1+ql2)*dql2*acl3*sin(ql1+ql2+ql3);
cl2=mel*dql1^2*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)-ml3*dql1^2*al1*sin(ql1)*al2*cos(ql1+ql2)+2*dql3*mel*al2*sin(ql1+ql2)*dql2*al3*cos(ql1+ql2+ql3)+2*dql3*mel*dql1*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)-ml3*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*dql1*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*dql1^2*al1*cos(ql1)*al2*sin(ql1+ql2)+ml2*dql1^2*al1*cos(ql1)*acl2*sin(ql1+ql2)-mel*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)*dql3^2-2*dql3*mel*al2*cos(ql1+ql2)*dql2*al3*sin(ql1+ql2+ql3)+2*dql3*ml3*dql1*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)-ml3*dql1^2*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)+2*dql3*ml3*al2*sin(ql1+ql2)*dql2*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al1*sin(ql1)*al2*cos(ql1+ql2)-ml2*dql1^2*al1*sin(ql1)*acl2*cos(ql1+ql2)+ml3*dql1^2*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)+mel*dql1^2*al1*cos(ql1)*al2*sin(ql1+ql2)-2*dql3*ml3*dql1*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)-2*dql3*ml3*al2*cos(ql1+ql2)*dql2*acl3*sin(ql1+ql2+ql3)+ml3*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)*dql3^2+mel*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)*dql3^2;
cl3=mel*dql1^2*al1*cos(ql1)*al3*sin(ql1+ql2+ql3)-2*ml3*dql1*al2*sin(ql1+ql2)*dql2*acl3*cos(ql1+ql2+ql3)-ml3*dql1^2*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)+2*mel*dql1*al2*cos(ql1+ql2)*dql2*al3*sin(ql1+ql2+ql3)-2*mel*dql1*al2*sin(ql1+ql2)*dql2*al3*cos(ql1+ql2+ql3)+2*ml3*dql1*al2*cos(ql1+ql2)*dql2*acl3*sin(ql1+ql2+ql3)-mel*dql2^2*al2*sin(ql1+ql2)*al3*cos(ql1+ql2+ql3)-ml3*dql2^2*al2*sin(ql1+ql2)*acl3*cos(ql1+ql2+ql3)-mel*dql1^2*al1*sin(ql1)*al3*cos(ql1+ql2+ql3)-ml3*dql1^2*al1*sin(ql1)*acl3*cos(ql1+ql2+ql3)+ml3*dql1^2*al1*cos(ql1)*acl3*sin(ql1+ql2+ql3)+ml3*dql2^2*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3)+mel*dql1^2*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)+mel*dql2^2*al2*cos(ql1+ql2)*al3*sin(ql1+ql2+ql3)+ml3*dql1^2*al2*cos(ql1+ql2)*acl3*sin(ql1+ql2+ql3);

cl11=-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql2*ml3*al1*al2*sin(ql2)-2*dql3*ml3*al1*acl3*sin(ql2+ql3)-2*dql2*ml2*al1*acl2*sin(ql2)-2*dql3*mel*al1*al3*sin(ql2+ql3)-2*dql2*mel*al1*al2*sin(ql2)-2*dql2*mel*al1*al3*sin(ql2+ql3)-2*dql2*ml3*al1*acl3*sin(ql2+ql3)-2*dql3*mel*al2*al3*sin(ql3);
cl12=-dql2*mel*al1*al2*sin(ql2)-dql2*mel*al1*al3*sin(ql2+ql3)-dql2*ml2*al1*acl2*sin(ql2)-dql2*ml3*al1*acl3*sin(ql2+ql3)-dql2*ml3*al1*al2*sin(ql2)-2*dql3*mel*al2*al3*sin(ql3)-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql3*ml3*al1*acl3*sin(ql2+ql3)-2*dql3*mel*al1*al3*sin(ql2+ql3);
cl13=-dql3*mel*al1*al3*sin(ql2+ql3)-dql3*ml3*al2*acl3*sin(ql3)-dql3*mel*al2*al3*sin(ql3)-dql3*ml3*al1*acl3*sin(ql2+ql3);
cl21=al1*dql1*ml3*acl3*sin(ql2+ql3)+al1*dql1*ml3*al2*sin(ql2)+al1*dql1*mel*al2*sin(ql2)+al1*dql1*mel*al3*sin(ql2+ql3)+al1*dql1*ml2*acl2*sin(ql2)-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql3*mel*al2*al3*sin(ql3);
cl22=-2*dql3*ml3*al2*acl3*sin(ql3)-2*dql3*mel*al2*al3*sin(ql3);
cl23=-dql3*mel*al2*al3*sin(ql3)-dql3*ml3*al2*acl3*sin(ql3);
cl31=2*ml3*al2*dql2*acl3*sin(ql3)+2*mel*al2*dql2*al3*sin(ql3)+dql1*al2*mel*al3*sin(ql3)+dql1*mel*al1*al3*sin(ql2+ql3)+dql1*ml3*al2*acl3*sin(ql3)+dql1*ml3*al1*acl3*sin(ql2+ql3);
cl32=ml3*al2*dql2*acl3*sin(ql3)+mel*al2*dql2*al3*sin(ql3);
cl33=0;

M_l=[ml11,ml12,ml13;...
    ml12,ml22,ml23;...
    ml13,ml23,ml33];

g_l=[gl1;gl2;gl3];

c_l=[cl1;cl2;cl3];

C_bar_l=[cl11,cl12,cl13;...
    cl21,cl22,cl23;...
    cl31,cl32,cl33];

b_l=[b_v_l1*dql1+sign(dql1)*(b_d_l1+(b_s_l1-b_d_l1)*exp(-abs(dql1)/epsilon));...
    b_v_l2*dql2+sign(dql2)*(b_d_l2+(b_s_l2-b_d_l2)*exp(-abs(dql2)/epsilon));...
    b_v_l3*dql3+sign(dql3)*(b_d_l3+(b_s_l3-b_d_l3)*exp(-abs(dql3)/epsilon))];

b_SDC_l=[sign(dql1)*(b_d_l1+(b_s_l1-b_d_l1)*exp(-abs(dql1)/epsilon));...
    sign(dql2)*(b_d_l2+(b_s_l2-b_d_l2)*exp(-abs(dql2)/epsilon));...
    sign(dql3)*(b_d_l3+(b_s_l3-b_d_l3)*exp(-abs(dql3)/epsilon))];
%Right arm matrices--------------------------------------------------------
mr11=mer*ar2^2+Izzr1+Izzr2+Izzr3+Izzer+mer*ar1^2+mr2*ar1^2+mer*ar3^2+mr3*ar2^2+mr3*acr3^2+mr2*acr2^2+2*mr2*ar1*sin(qr1)*acr2*sin(qr1+qr2)+2*mr3*ar1*sin(qr1)*ar2*sin(qr1+qr2)+2*mr3*ar1*sin(qr1)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar1*cos(qr1)*ar2*cos(qr1+qr2)+2*mr2*ar1*cos(qr1)*acr2*cos(qr1+qr2)+mr3*ar1^2+mr1*acr1^2+2*mr3*ar1*cos(qr1)*acr3*cos(qr1+qr2+qr3)+2*mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+2*mer*ar1*sin(qr1)*ar2*sin(qr1+qr2)+2*mer*ar1*cos(qr1)*ar2*cos(qr1+qr2)+2*mer*ar1*sin(qr1)*ar3*sin(qr1+qr2+qr3)+2*mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+2*mer*ar1*cos(qr1)*ar3*cos(qr1+qr2+qr3)+2*mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3);
mr12=Izzr2+Izzer+mer*ar2^2+mr2*acr2^2+mr3*ar2^2+mr3*acr3^2+mer*ar3^2+Izzr3+mr2*ar1*sin(qr1)*acr2*sin(qr1+qr2)+mr2*ar1*cos(qr1)*acr2*cos(qr1+qr2)+mr3*ar1*sin(qr1)*ar2*sin(qr1+qr2)+mr3*ar1*sin(qr1)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+mr3*ar1*cos(qr1)*ar2*cos(qr1+qr2)+mr3*ar1*cos(qr1)*acr3*cos(qr1+qr2+qr3)+2*mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+mer*ar1*sin(qr1)*ar2*sin(qr1+qr2)+mer*ar1*cos(qr1)*ar2*cos(qr1+qr2)+mer*ar1*sin(qr1)*ar3*sin(qr1+qr2+qr3)+2*mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mer*ar1*cos(qr1)*ar3*cos(qr1+qr2+qr3)+2*mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3);
mr13=mr3*ar1*sin(qr1)*acr3*sin(qr1+qr2+qr3)+mr3*acr3^2+mer*ar3^2+mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+mer*ar1*cos(qr1)*ar3*cos(qr1+qr2+qr3)+Izzer+mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+mr3*ar1*cos(qr1)*acr3*cos(qr1+qr2+qr3)+Izzr3+mer*ar1*sin(qr1)*ar3*sin(qr1+qr2+qr3);
mr22=2*mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+mr2*acr2^2+mer*ar3^2+mr3*acr3^2+Izzer+Izzr2+mer*ar2^2+Izzr3+mr3*ar2^2+2*mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+2*mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+2*mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3);
mr23=Izzer+mer*ar3^2+mr3*ar2*sin(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+Izzr3+mr3*ar2*cos(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+mer*ar2*cos(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+mer*ar2*sin(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*acr3^2;
mr33=Izzer+mr3*acr3^2+mer*ar3^2+Izzr3;

gr1=g0*(mr2*ar1*cos(qr1)+mr2*acr2*cos(qr1+qr2)+mer*ar1*cos(qr1)+mer*ar3*cos(qr1+qr2+qr3)+mr3*ar2*cos(qr1+qr2)+mr1*acr1*cos(qr1)+mer*ar2*cos(qr1+qr2)+mr3*ar1*cos(qr1)+mr3*acr3*cos(qr1+qr2+qr3));
gr2=g0*(mer*ar2*cos(qr1+qr2)+mr3*acr3*cos(qr1+qr2+qr3)+mr3*ar2*cos(qr1+qr2)+mr2*acr2*cos(qr1+qr2)+mer*ar3*cos(qr1+qr2+qr3));
gr3=g0*cos(qr1+qr2+qr3)*(mer*ar3+mr3*acr3);

cr1=2*dqr3*mer*ar2*sin(qr1+qr2)*dqr2*ar3*cos(qr1+qr2+qr3)+2*dqr3*mer*dqr1*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)+2*dqr3*mer*dqr1*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)-mr3*ar1*cos(qr1)*dqr2^2*ar2*sin(qr1+qr2)+mr3*ar1*sin(qr1)*dqr2^2*acr3*cos(qr1+qr2+qr3)+mr2*ar1*sin(qr1)*acr2*cos(qr1+qr2)*dqr2^2+mer*ar1*sin(qr1)*dqr2^2*ar3*cos(qr1+qr2+qr3)-mer*ar1*cos(qr1)*dqr2^2*ar3*sin(qr1+qr2+qr3)+mr3*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)*dqr3^2+mr3*ar1*sin(qr1)*dqr2^2*ar2*cos(qr1+qr2)-mr3*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*dqr1*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)+mr3*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)*dqr3^2-mr3*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*dqr1*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)-2*dqr2*mr3*dqr1*ar1*cos(qr1)*ar2*sin(qr1+qr2)-mr2*ar1*cos(qr1)*acr2*sin(qr1+qr2)*dqr2^2+mer*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)*dqr3^2+mer*ar1*sin(qr1)*dqr2^2*ar2*cos(qr1+qr2)-mer*ar1*cos(qr1)*dqr2^2*ar2*sin(qr1+qr2)-mr3*ar1*cos(qr1)*dqr2^2*acr3*sin(qr1+qr2+qr3)+2*dqr2*mr3*dqr1*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)-mer*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)*dqr3^2+2*dqr2*mr2*dqr1*ar1*sin(qr1)*acr2*cos(qr1+qr2)+2*dqr2*mr3*dqr1*ar1*sin(qr1)*ar2*cos(qr1+qr2)+mer*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)*dqr3^2-mer*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr2*mr3*dqr1*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mer*ar2*cos(qr1+qr2)*dqr2*ar3*sin(qr1+qr2+qr3)-2*dqr2*mr2*dqr1*ar1*cos(qr1)*acr2*sin(qr1+qr2)+2*dqr2*mer*dqr1*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)-2*dqr2*mr3*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)*dqr3+2*dqr3*mr3*dqr1*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)+2*dqr3*mr3*dqr1*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)-2*dqr2*mer*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)*dqr3-2*dqr2*mer*dqr1*ar1*cos(qr1)*ar2*sin(qr1+qr2)+2*dqr2*mr3*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)*dqr3-2*dqr2*mer*dqr1*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)+2*dqr2*mer*dqr1*ar1*sin(qr1)*ar2*cos(qr1+qr2)+2*dqr2*mer*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)*dqr3+2*dqr3*mr3*ar2*sin(qr1+qr2)*dqr2*acr3*cos(qr1+qr2+qr3)-2*dqr3*mr3*dqr1*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mr3*dqr1*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mr3*ar2*cos(qr1+qr2)*dqr2*acr3*sin(qr1+qr2+qr3);
cr2=mer*dqr1^2*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)-mr3*dqr1^2*ar1*sin(qr1)*ar2*cos(qr1+qr2)+2*dqr3*mer*ar2*sin(qr1+qr2)*dqr2*ar3*cos(qr1+qr2+qr3)+2*dqr3*mer*dqr1*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)-mr3*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*dqr1*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*dqr1^2*ar1*cos(qr1)*ar2*sin(qr1+qr2)+mr2*dqr1^2*ar1*cos(qr1)*acr2*sin(qr1+qr2)-mer*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)*dqr3^2-2*dqr3*mer*ar2*cos(qr1+qr2)*dqr2*ar3*sin(qr1+qr2+qr3)+2*dqr3*mr3*dqr1*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)-mr3*dqr1^2*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)+2*dqr3*mr3*ar2*sin(qr1+qr2)*dqr2*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar1*sin(qr1)*ar2*cos(qr1+qr2)-mr2*dqr1^2*ar1*sin(qr1)*acr2*cos(qr1+qr2)+mr3*dqr1^2*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)+mer*dqr1^2*ar1*cos(qr1)*ar2*sin(qr1+qr2)-2*dqr3*mr3*dqr1*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)-2*dqr3*mr3*ar2*cos(qr1+qr2)*dqr2*acr3*sin(qr1+qr2+qr3)+mr3*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)*dqr3^2+mer*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)*dqr3^2;
cr3=mer*dqr1^2*ar1*cos(qr1)*ar3*sin(qr1+qr2+qr3)-2*mr3*dqr1*ar2*sin(qr1+qr2)*dqr2*acr3*cos(qr1+qr2+qr3)-mr3*dqr1^2*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)+2*mer*dqr1*ar2*cos(qr1+qr2)*dqr2*ar3*sin(qr1+qr2+qr3)-2*mer*dqr1*ar2*sin(qr1+qr2)*dqr2*ar3*cos(qr1+qr2+qr3)+2*mr3*dqr1*ar2*cos(qr1+qr2)*dqr2*acr3*sin(qr1+qr2+qr3)-mer*dqr2^2*ar2*sin(qr1+qr2)*ar3*cos(qr1+qr2+qr3)-mr3*dqr2^2*ar2*sin(qr1+qr2)*acr3*cos(qr1+qr2+qr3)-mer*dqr1^2*ar1*sin(qr1)*ar3*cos(qr1+qr2+qr3)-mr3*dqr1^2*ar1*sin(qr1)*acr3*cos(qr1+qr2+qr3)+mr3*dqr1^2*ar1*cos(qr1)*acr3*sin(qr1+qr2+qr3)+mr3*dqr2^2*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3)+mer*dqr1^2*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mer*dqr2^2*ar2*cos(qr1+qr2)*ar3*sin(qr1+qr2+qr3)+mr3*dqr1^2*ar2*cos(qr1+qr2)*acr3*sin(qr1+qr2+qr3);

cr11=-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr2*mr3*ar1*ar2*sin(qr2)-2*dqr3*mr3*ar1*acr3*sin(qr2+qr3)-2*dqr2*mr2*ar1*acr2*sin(qr2)-2*dqr3*mer*ar1*ar3*sin(qr2+qr3)-2*dqr2*mer*ar1*ar2*sin(qr2)-2*dqr2*mer*ar1*ar3*sin(qr2+qr3)-2*dqr2*mr3*ar1*acr3*sin(qr2+qr3)-2*dqr3*mer*ar2*ar3*sin(qr3);
cr12=-dqr2*mer*ar1*ar2*sin(qr2)-dqr2*mer*ar1*ar3*sin(qr2+qr3)-dqr2*mr2*ar1*acr2*sin(qr2)-dqr2*mr3*ar1*acr3*sin(qr2+qr3)-dqr2*mr3*ar1*ar2*sin(qr2)-2*dqr3*mer*ar2*ar3*sin(qr3)-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr3*mr3*ar1*acr3*sin(qr2+qr3)-2*dqr3*mer*ar1*ar3*sin(qr2+qr3);
cr13=-dqr3*mer*ar1*ar3*sin(qr2+qr3)-dqr3*mr3*ar2*acr3*sin(qr3)-dqr3*mer*ar2*ar3*sin(qr3)-dqr3*mr3*ar1*acr3*sin(qr2+qr3);
cr21=ar1*dqr1*mr3*acr3*sin(qr2+qr3)+ar1*dqr1*mr3*ar2*sin(qr2)+ar1*dqr1*mer*ar2*sin(qr2)+ar1*dqr1*mer*ar3*sin(qr2+qr3)+ar1*dqr1*mr2*acr2*sin(qr2)-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr3*mer*ar2*ar3*sin(qr3);
cr22=-2*dqr3*mr3*ar2*acr3*sin(qr3)-2*dqr3*mer*ar2*ar3*sin(qr3);
cr23=-dqr3*mer*ar2*ar3*sin(qr3)-dqr3*mr3*ar2*acr3*sin(qr3);
cr31=2*mr3*ar2*dqr2*acr3*sin(qr3)+2*mer*ar2*dqr2*ar3*sin(qr3)+dqr1*ar2*mer*ar3*sin(qr3)+dqr1*mer*ar1*ar3*sin(qr2+qr3)+dqr1*mr3*ar2*acr3*sin(qr3)+dqr1*mr3*ar1*acr3*sin(qr2+qr3);
cr32=mr3*ar2*dqr2*acr3*sin(qr3)+mer*ar2*dqr2*ar3*sin(qr3);
cr33=0;

M_r=[mr11,mr12,mr13;...
    mr12,mr22,mr23;...
    mr13,mr23,mr33];

g_r=[gr1;gr2;gr3];

c_r=[cr1;cr2;cr3];

C_bar_r=[cr11,cr12,cr13;...
    cr21,cr22,cr23;...
    cr31,cr32,cr33];

b_r=[b_v_r1*dqr1+sign(dqr1)*(b_d_r1+(b_s_r1-b_d_r1)*exp(-abs(dqr1)/epsilon));...
    b_v_r2*dqr2+sign(dqr2)*(b_d_r2+(b_s_r2-b_d_r2)*exp(-abs(dqr2)/epsilon));...
    b_v_r3*dqr3+sign(dqr3)*(b_d_r3+(b_s_r3-b_d_r3)*exp(-abs(dqr3)/epsilon))];

b_SDC_r=[sign(dqr1)*(b_d_r1+(b_s_r1-b_d_r1)*exp(-abs(dqr1)/epsilon));...
    sign(dqr2)*(b_d_r2+(b_s_r2-b_d_r2)*exp(-abs(dqr2)/epsilon));...
    sign(dqr3)*(b_d_r3+(b_s_r3-b_d_r3)*exp(-abs(dqr3)/epsilon))];
%External force------------------------------------------------------------
J_l=[-al2*sin(ql1+ql2)-al1*sin(ql1)-al3*sin(ql1+ql2+ql3),-al2*sin(ql1+ql2)-al3*sin(ql1+ql2+ql3),-al3*sin(ql1+ql2+ql3);...
    al2*cos(ql1+ql2)+al1*cos(ql1)+al3*cos(ql1+ql2+ql3),al2*cos(ql1+ql2)+al3*cos(ql1+ql2+ql3),al3*cos(ql1+ql2+ql3);...
    1,1,1];

J_l_dot=[-al2*cos(ql1+ql2)*(dql1+dql2)-al1*cos(ql1)*dql1-al3*cos(ql1+ql2+ql3)*(dql1+dql2+dql3),-al2*cos(ql1+ql2)*(dql1+dql2)-al3*cos(ql1+ql2+ql3)*(dql1+dql2+dql3),-al3*cos(ql1+ql2+ql3)*(dql1+dql2+dql3);...
    -al2*sin(ql1+ql2)*(dql1+dql2)-al1*sin(ql1)*dql1-al3*sin(ql1+ql2+ql3)*(dql1+dql2+dql3),-al2*sin(ql1+ql2)*(dql1+dql2)-al3*sin(ql1+ql2+ql3)*(dql1+dql2+dql3),-al3*sin(ql1+ql2+ql3)*(dql1+dql2+dql3);...
    0,0,0];

J_r=[-ar2*sin(qr1+qr2)-ar1*sin(qr1)-ar3*sin(qr1+qr2+qr3),-ar2*sin(qr1+qr2)-ar3*sin(qr1+qr2+qr3),-ar3*sin(qr1+qr2+qr3);...
    ar2*cos(qr1+qr2)+ar1*cos(qr1)+ar3*cos(qr1+qr2+qr3),ar2*cos(qr1+qr2)+ar3*cos(qr1+qr2+qr3),ar3*cos(qr1+qr2+qr3);...
    1,1,1];

J_r_dot=[-ar2*cos(qr1+qr2)*(dqr1+dqr2)-ar1*cos(qr1)*dqr1-ar3*cos(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar2*cos(qr1+qr2)*(dqr1+dqr2)-ar3*cos(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar3*cos(qr1+qr2+qr3)*(dqr1+dqr2+dqr3);...
    -ar2*sin(qr1+qr2)*(dqr1+dqr2)-ar1*sin(qr1)*dqr1-ar3*sin(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar2*sin(qr1+qr2)*(dqr1+dqr2)-ar3*sin(qr1+qr2+qr3)*(dqr1+dqr2+dqr3),-ar3*sin(qr1+qr2+qr3)*(dqr1+dqr2+dqr3);...
    0,0,0];

d2xe_d_l=-xradius_l*cos(t);
d2ye_d_l=-yradius_l*sin(t);

domega_e=730609364650485/9007199254740992*t^3-10328746556834415/18014398509481984*t^2+4056089287967733/4503599627370496*t;

d2pe=[d2xe_d_l;d2ye_d_l;omega_e];

fe=-[mp*d2xe_d_l;...
    mp*d2ye_d_l+mp*g0;...
    Izzp*domega_e+Izzp*omega_e^2];

d2q_l=inv(J_l)*(d2pe-J_l_dot*dq_l);
d2q_r=inv(J_r)*(d2pe-J_r_dot*dq_r);

SS_l=inv(J_l')*inv(Q_bar_l)*inv(J_l')';
SS_r=inv(J_r')*inv(Q_bar_r)*inv(J_r')';
gama=inv(SS_l+SS_r)*(inv(J_l')*(M_l*d2q_l+c_l+g_l+b_l)+...
    inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r)-fe);

fe_l=inv(J_l')*(M_l*d2q_l+c_l+g_l+b_l-inv(Q_bar_l)*inv(J_l')'*gama);
fe_r=inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r-inv(Q_bar_r)*inv(J_r')'*gama);
%SDC-----------------------------------------------------------------------
u_add_l=g_l+b_SDC_l-J_l'*fe_l;
u_add_r=g_r+b_SDC_r-J_r'*fe_r;

OMEGA_l=diag([u_add_l(1)/ql1,u_add_l(2)/ql2,u_add_l(3)/ql3]);
OMEGA_r=diag([u_add_r(1)/qr1,u_add_r(2)/qr2,u_add_r(3)/qr3]);

A_l=[zeros(n,n),eye(n);...
    OMEGA_l,-inv(M_l)*(C_bar_l+diag([b_v_l1,b_v_l2,b_v_l3]))];
B_l=[zeros(n,n);inv(M_l)];

A_r=[zeros(n,n),eye(n);...
    OMEGA_r,-inv(M_r)*(C_bar_r+diag([b_v_r1,b_v_r2,b_v_r3]))];
B_r=[zeros(n,n);inv(M_r)];
%Control-------------------------------------------------------------------
K_l=care(A_l,B_l,Q_l,R_l);
u_l=-inv(R_l)*B_l'*K_l*[q_l-[qdl1;qdl2;qdl3];dq_l];

K_r=care(A_r,B_r,Q_r,R_r);
u_r=-inv(R_r)*B_r'*K_r*[q_r-[qdr1;qdr2;qdr3];dq_r];

if u_l(1) > (ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1)
    u_l(1)=ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1;
end
if u_l(1) < (-ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1)
    u_l(1)=-ustall_l(1)-ustall_l(1)/omeganl_l(1)*dql1;
end
if u_l(2) > (ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2)
    u_l(2)=ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2;
end
if u_l(2) < (-ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2)
    u_l(2)=-ustall_l(2)-ustall_l(2)/omeganl_l(2)*dql2;
end
if u_l(3) > (ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3)
    u_l(3)=ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3;
end
if u_l(3) < (-ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3)
    u_l(3)=-ustall_l(3)-ustall_l(3)/omeganl_l(3)*dql3;
end

if u_r(1) > (ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1)
    u_r(1)=ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1;
end
if u_r(1) < (-ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1)
    u_r(1)=-ustall_r(1)-ustall_r(1)/omeganl_r(1)*dqr1;
end
if u_r(2) > (ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2)
    u_r(2)=ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2;
end
if u_r(2) < (-ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2)
    u_r(2)=-ustall_r(2)-ustall_r(2)/omeganl_r(2)*dqr2;
end
if u_r(3) > (ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3)
    u_r(3)=ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3;
end
if u_r(3) < (-ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3)
    u_r(3)=-ustall_r(3)-ustall_r(3)/omeganl_r(3)*dqr3;
end
%System--------------------------------------------------------------------
xdot=[dq_l;...
    dq_r;...
    inv(M_l)*(u_l+J_l'*fe_l-c_l-g_l-b_l);...
    inv(M_r)*(u_r+J_r'*fe_r-c_r-g_r-b_r);...
    inv(Jd_l'*Jd_l)*Jd_l'*V_l;...
    inv(Jd_r'*Jd_r)*Jd_r'*V_r];