%**************************************************************************
%       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 d1 a2 a3 dc1 ac2 ac3 m1 m2 m3 mpl mpr mp...
    Ixx1 Ixx2 Ixx3 Iyy1 Iyy2 Iyy3 Izz1 Izz2 Izz3...
    b_v b_d b_s epsilon...
    Q_l R_l Q_r R_r n g0 ustall omeganl Q_bar_l Q_bar_r J k_mad
%Time----------------------------------------------------------------------
tf=3;
N=999;
%Start point---------------------------------------------------------------
xi=0.1;
yi=0;
zi=0.15;
%No. of links--------------------------------------------------------------
n=3;
%R and Q-------------------------------------------------------------------
Q_l=100*diag([ones(1,2*n),zeros(1,2*n)]);
R_l=0.01*eye(n);

Q_r=100*diag([ones(1,2*n),zeros(1,2*n)]);
R_r=0.01*eye(n);

Q_bar_l=eye(n);
Q_bar_r=eye(n);
%Parameters----------------------------------------------------------------
d=0.08;
d1=0.06;
dc1=d1/2;
a2=0.1;
ac2=a2/2;
a3=0.21;
ac3=a3/2;
m1=0.028;
m2=0.1;
m3=0.231;

mpl=0;
mpr=mpl;

mp=0.5;

Ixx1=0;
Iyy1=0.00005;
Izz1=0;

Ixx2=0.000016;
Iyy2=0.000034;
Izz2=0.000021;

Ixx3=0.000923;
Iyy3=0.000915;
Izz3=0.000085;

b_v=[0.02;0.02;0.02];
b_d=[0.01;0.01;0.01];
b_s=[0.005;0.005;0.005];

epsilon=0.02;
g0=9.81;

J=[0.05;0.05;0.05];
k_mad=[100;100;100];
%Limits of motors----------------------------------------------------------
ustall=[0.55;0.96;0.96];
omeganl=[5.85;3;3];
%Inverse kinematics--------------------------------------------------------
yi_bl=-d+yi;
yi_br=d+yi;

xinl1=atan2((yi_bl),(xi));
xinl3=(acos((xi^2+yi_bl^2+(zi-d1)^2-a2^2-a3^2)/(2*a2*a3)));
xinl2=-atan2(zi-d1,sqrt(xi^2+yi_bl^2))-atan2(a3*sin(xinl3),(a2+a3*cos(xinl3)));

xinr1=atan2((yi_br),(xi));
xinr3=(acos((xi^2+yi_br^2+(zi-d1)^2-a2^2-a3^2)/(2*a2*a3)));
xinr2=-atan2(zi-d1,sqrt(xi^2+yi_br^2))-atan2(a3*sin(xinr3),(a2+a3*cos(xinr3)));
%Initial condition---------------------------------------------------------
qlin=[xinl1;xinl2;xinl3];
qrin=[xinr1;xinr2;xinr3];
qin=[qlin;qrin;qlin;qrin;zeros(4*n,1);qlin;qrin];
%ODE-----------------------------------------------------------------------
[t,x]=ode45(@sys,0:tf/N:tf,qin);
save matlab
afFigurePosition=[100,100,700,500];
figure(1)
subplot(3,2,1,'Parent',figure(1),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('a');grid on;box on;hold all;
plot(t,x(:,1),'k','Linewidth',1.5)
plot(t,x(:,7),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_l_1(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Link','Motor')
subplot(3,2,2,'Parent',figure(1),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('b');grid on;box on;hold all;
plot(t,x(:,2),'k','Linewidth',1.5)
plot(t,x(:,8),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_l_2(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,3,'Parent',figure(1),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('c');grid on;box on;hold all;
plot(t,x(:,3),'k','Linewidth',1.5)
plot(t,x(:,9),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_l_3(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,4,'Parent',figure(1),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('d');grid on;box on;hold all;
plot(t,x(:,4),'k','Linewidth',1.5)
plot(t,x(:,10),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_r_1(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,5,'Parent',figure(1),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('e');grid on;box on;hold all;
plot(t,x(:,5),'k','Linewidth',1.5)
plot(t,x(:,11),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_r_2(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,6,'Parent',figure(1),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('f');grid on;box on;hold all;
plot(t,x(:,6),'k','Linewidth',1.5)
plot(t,x(:,12),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('q_r_3(t)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)

figure(2)
subplot(3,2,1,'Parent',figure(2),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('a');grid on;box on;hold all;
plot(t,x(:,13),'k','Linewidth',1.5)
plot(t,x(:,19),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_l_1(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Link','Motor')
subplot(3,2,2,'Parent',figure(2),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('b');grid on;box on;hold all;
plot(t,x(:,14),'k','Linewidth',1.5)
plot(t,x(:,20),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_l_2(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,3,'Parent',figure(2),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('c');grid on;box on;hold all;
plot(t,x(:,15),'k','Linewidth',1.5)
plot(t,x(:,21),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_l_3(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,4,'Parent',figure(2),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('d');grid on;box on;hold all;
plot(t,x(:,16),'k','Linewidth',1.5)
plot(t,x(:,22),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_r_1(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,5,'Parent',figure(2),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('e');grid on;box on;hold all;
plot(t,x(:,17),'k','Linewidth',1.5)
plot(t,x(:,23),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_r_2(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,6,'Parent',figure(2),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('f');grid on;box on;hold all;
plot(t,x(:,18),'k','Linewidth',1.5)
plot(t,x(:,24),'--b','Linewidth',1.5)
xlabel('t (s)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('dq_r_3(t)/dt','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
%Plot path-----------------------------------------------------------------
afFigurePosition=[200,150,380,220];

qld1=x(:,25);
qld2=x(:,26);
qld3=x(:,27);

qrd1=x(:,28);
qrd2=x(:,29);
qrd3=x(:,30);

xle_d=cos(qld1).*cos(qld2).*a3.*cos(qld3)-cos(qld1).*sin(qld2).*a3.*sin(qld3)+cos(qld1).*a2.*cos(qld2);
yle_d=d+sin(qld1).*cos(qld2).*a3.*cos(qld3)-sin(qld1).*sin(qld2).*a3.*sin(qld3)+sin(qld1).*a2.*cos(qld2);
zle_d=-sin(qld2).*a3.*cos(qld3)-cos(qld2).*a3.*sin(qld3)-a2.*sin(qld2)+d1;

xre_d=cos(qrd1).*cos(qrd2).*a3.*cos(qrd3)-cos(qrd1).*sin(qrd2).*a3.*sin(qrd3)+cos(qrd1).*a2.*cos(qrd2);
yre_d=-d+sin(qrd1).*cos(qrd2).*a3.*cos(qrd3)-sin(qrd1).*sin(qrd2).*a3.*sin(qrd3)+sin(qrd1).*a2.*cos(qrd2);
zre_d=-sin(qrd2).*a3.*cos(qrd3)-cos(qrd2).*a3.*sin(qrd3)-a2.*sin(qrd2)+d1;

ql1=x(:,1);
ql2=x(:,2);
ql3=x(:,3);

qr1=x(:,4);
qr2=x(:,5);
qr3=x(:,6);

xle=cos(ql1).*cos(ql2).*a3.*cos(ql3)-cos(ql1).*sin(ql2).*a3.*sin(ql3)+cos(ql1).*a2.*cos(ql2);
yle=d+sin(ql1).*cos(ql2).*a3.*cos(ql3)-sin(ql1).*sin(ql2).*a3.*sin(ql3)+sin(ql1).*a2.*cos(ql2);
zle=-sin(ql2).*a3.*cos(ql3)-cos(ql2).*a3.*sin(ql3)-a2.*sin(ql2)+d1;
xle_1=0.*ones(length(t),1);
yle_1=d+0.*ones(length(t),1);
zle_1=d1.*ones(length(t),1);
xle_2=cos(ql1).*a2.*cos(ql2);
yle_2=d+sin(ql1).*a2.*cos(ql2);
zle_2=-a2.*sin(ql2)+d1;

xre=cos(qr1).*cos(qr2).*a3.*cos(qr3)-cos(qr1).*sin(qr2).*a3.*sin(qr3)+cos(qr1).*a2.*cos(qr2);
yre=-d+sin(qr1).*cos(qr2).*a3.*cos(qr3)-sin(qr1).*sin(qr2).*a3.*sin(qr3)+sin(qr1).*a2.*cos(qr2);
zre=-sin(qr2).*a3.*cos(qr3)-cos(qr2).*a3.*sin(qr3)-a2.*sin(qr2)+d1;
xre_1=0.*ones(length(t),1);
yre_1=-d+0.*ones(length(t),1);
zre_1=d1.*ones(length(t),1);
xre_2=cos(qr1).*a2.*cos(qr2);
yre_2=-d+sin(qr1).*a2.*cos(qr2);
zre_2=-a2.*sin(qr2)+d1;

el=sqrt((xle-xle_d).^2+(yle-yle_d).^2+(zle-zle_d).^2)*1000;
er=sqrt((xre-xre_d).^2+(yre-yre_d).^2+(zre-zre_d).^2)*1000;

figure(5)
axesi=axes('Parent',figure(5),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
hold(axesi,'all');grid on;box on;
plot(t,el,'b','Linewidth',1.5)
plot(t,er,'--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')

figure(6)
axesi=axes('Parent',figure(6),'FontWeight','bold',...
    'FontSize',10,'FontName','Cambria');
view([-1,-1,1]);hold(axesi,'all');grid on;
plot3(xi,yi,zi,'ok')
plot3(xle,yle,zle,'b','Linewidth',1.5)
plot3(xre,yre,zre,'--r','Linewidth',1.5)
plot3(xle_d,yle_d,zle_d,'--k','Linewidth',1)
for i=1:100:length(t)
    line([0,xle_1(i)],[d,yle_1(i)],[0,zle_1(i)],'color',[0.53,0.78,0.99],'LineStyle','-','Linewidth',1)
    line([xle_1(i),xle_2(i)],[yle_1(i),yle_2(i)],[zle_1(i),zle_2(i)],'color',[0.21,0.38,0.81],'LineStyle','-','Linewidth',1)
    line([xle_2(i),xle(i)],[yle_2(i),yle(i)],[zle_2(i),zle(i)],'color',[0.53,0.78,0.99],'LineStyle','-','Linewidth',1)
    line([0,xre_1(i)],[-d,yre_1(i)],[0,zre_1(i)],'color',[0.98 0.71 0.71],'LineStyle','-','Linewidth',1.5)
    line([xre_1(i),xre_2(i)],[yre_1(i),yre_2(i)],[zre_1(i),zre_2(i)],'color',[0.90 0.41 0.41],'LineStyle','-','Linewidth',1)
    line([xre_2(i),xre(i)],[yre_2(i),yre(i)],[zre_2(i),zre(i)],'color',[0.98 0.71 0.71],'LineStyle','-','Linewidth',1)
end
xlabel('x (m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
ylabel('y (m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
zlabel('z (m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
afFigurePosition=[200,150,380,280];
set(gcf,'Position',afFigurePosition)
axis equal;
legend('Start Point','Left Arm Path','Right Arm Path','Desired Path')
%Plot inputs---------------------------------------------------------------
for i=1:length(t)
    %States----------------------------------------------------------------
    ql1=x(i,1);
    ql2=x(i,2);
    ql3=x(i,3);
    qr1=x(i,4);
    qr2=x(i,5);
    qr3=x(i,6);

    ql4=x(i,7);
    ql5=x(i,8);
    ql6=x(i,9);
    qr4=x(i,10);
    qr5=x(i,11);
    qr6=x(i,12);

    dql1=x(i,13);
    dql2=x(i,14);
    dql3=x(i,15);
    dqr1=x(i,16);
    dqr2=x(i,17);
    dqr3=x(i,18);

    dql4=x(i,19);
    dql5=x(i,20);
    dql6=x(i,21);
    dqr4=x(i,22);
    dqr5=x(i,23);
    dqr6=x(i,24);

    qdl1=x(i,25);
    qdl2=x(i,26);
    qdl3=x(i,27);
    qdr1=x(i,28);
    qdr2=x(i,29);
    qdr3=x(i,30);

    q_l_l=[ql1;ql2;ql3];
    q_l_r=[qr1;qr2;qr3];
    q_m_l=[ql4;ql5;ql6];
    q_m_r=[qr4;qr5;qr6];

    dq_l_l=[dql1;dql2;dql3];
    dq_l_r=[dqr1;dqr2;dqr3];
    dq_m_l=[dql4;dql5;dql6];
    dq_m_r=[dqr4;dqr5;dqr6];

    q_l=[q_l_l;q_m_l];
    q_r=[q_l_r;q_m_r];
    dq_l=[dq_l_l;dq_m_l];
    dq_r=[dq_l_r;dq_m_r];
    %Left arm matrices-----------------------------------------------------
    M_l=[-2*mpl*cos(ql2)*a3^2*cos(ql3)*sin(ql2)*sin(ql3)+2*mpl*cos(ql2)^2*a3*cos(ql3)*a2-2*m3*cos(ql2)*ac3^2*cos(ql3)*sin(ql2)*sin(ql3)+2*m3*cos(ql2)^2*ac3*cos(ql3)*a2-2*m3*sin(ql2)*ac3*sin(ql3)*a2*cos(ql2)-2*mpl*sin(ql2)*a3*sin(ql3)*a2*cos(ql2)+mpl*a2^2*cos(ql2)^2-2*Ixx3*cos(ql3)^2*cos(ql2)^2+m3*a2^2*cos(ql2)^2-m3*ac3^2*cos(ql3)^2+Iyy1+Ixx2+Iyy3-Ixx2*cos(ql2)^2-Iyy3*cos(ql3)^2-Iyy3*cos(ql2)^2+Ixx3*cos(ql2)^2+Ixx3*cos(ql3)^2+m3*ac3^2+mpl*a3^2-m3*ac3^2*cos(ql2)^2-mpl*a3^2*cos(ql3)^2-mpl*a3^2*cos(ql2)^2+2*mpl*cos(ql2)^2*a3^2*cos(ql3)^2+2*m3*cos(ql2)^2*ac3^2*cos(ql3)^2+cos(ql2)^2*Iyy2+m2*ac2^2*cos(ql2)^2+2*Ixx3*sin(ql2)*cos(ql3)*cos(ql2)*sin(ql3)-2*Iyy3*sin(ql2)*cos(ql3)*cos(ql2)*sin(ql3)+2*Iyy3*cos(ql2)^2*cos(ql3)^2,0,0;...
        0,Izz2+Izz3+mpl*a2^2+m3*a2^2+m2*ac2^2+2*mpl*a3*cos(ql3)*a2+2*m3*ac3*cos(ql3)*a2+m3*ac3^2+mpl*a3^2,Izz3+mpl*a3*cos(ql3)*a2+m3*ac3*cos(ql3)*a2+m3*ac3^2+mpl*a3^2;...
        0,Izz3+mpl*a3*cos(ql3)*a2+m3*ac3*cos(ql3)*a2+m3*ac3^2+mpl*a3^2,Izz3+m3*ac3^2+mpl*a3^2];

    g_l=[0;...
        -g0*(m2*ac2*cos(ql2)+m3*cos(ql2)*ac3*cos(ql3)-m3*sin(ql2)*ac3*sin(ql3)+m3*a2*cos(ql2)+mpl*cos(ql2)*a3*cos(ql3)-mpl*sin(ql2)*a3*sin(ql3)+mpl*a2*cos(ql2));...
        g0*(m3*sin(ql2)*ac3*sin(ql3)-m3*cos(ql2)*ac3*cos(ql3)+mpl*sin(ql2)*a3*sin(ql3)-mpl*cos(ql2)*a3*cos(ql3))];

    c_l=[-2*dql1*(-2*dql2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)+dql3*m3*ac3*sin(ql3)*a2*cos(ql2)^2+dql3*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)+dql3*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)-dql3*mpl*sin(ql2)*a3^2*cos(ql2)-dql3*mpl*a3^2*sin(ql3)*cos(ql3)+dql2*mpl*a2^2*cos(ql2)*sin(ql2)-dql2*mpl*a3*sin(ql3)*a2+2*dql2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*dql2*mpl*a2*cos(ql2)^2*a3*sin(ql3)+2*dql2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*dql2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2-2*dql2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+2*dql2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*dql2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)+2*dql3*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*dql3*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql3*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql3*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2-2*dql3*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql3*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql3*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql3*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-dql2*m3*ac3*sin(ql3)*a2-dql3*m3*ac3^2*sin(ql3)*cos(ql3)-dql3*m3*sin(ql2)*ac3^2*cos(ql2)+dql2*m2*ac2^2*cos(ql2)*sin(ql2)-dql2*mpl*sin(ql2)*a3^2*cos(ql2)-dql2*mpl*a3^2*sin(ql3)*cos(ql3)+dql3*mpl*a2*cos(ql2)^2*a3*sin(ql3)+dql2*m3*a2^2*cos(ql2)*sin(ql2)-dql2*m3*ac3^2*sin(ql3)*cos(ql3)-dql2*m3*sin(ql2)*ac3^2*cos(ql2)+dql2*Ixx3*cos(ql3)*sin(ql3)+dql2*Ixx3*cos(ql2)*sin(ql2)-dql2*Iyy3*cos(ql3)*sin(ql3)-dql2*Iyy3*cos(ql2)*sin(ql2)-dql2*sin(ql2)*Ixx2*cos(ql2)+dql2*cos(ql2)*Iyy2*sin(ql2)+dql3*Ixx3*cos(ql3)*sin(ql3)+dql3*Ixx3*cos(ql2)*sin(ql2)-dql3*Iyy3*cos(ql2)*sin(ql2)-dql3*Iyy3*cos(ql3)*sin(ql3));...
        2*dql1^2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*dql1^2*mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*dql1^2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql1^2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*sin(ql3)*a2*dql3*dql2*m3*ac3-2*sin(ql3)*a2*dql3*dql2*mpl*a3-dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)-dql1^2*mpl*a3*sin(ql3)*a2-dql1^2*m3*sin(ql2)*ac3^2*cos(ql2)-sin(ql3)*a2*dql3^2*mpl*a3+2*dql1^2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*dql1^2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-sin(ql3)*a2*dql3^2*m3*ac3+dql1^2*mpl*a2^2*cos(ql2)*sin(ql2)-dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)+dql1^2*m3*a2^2*cos(ql2)*sin(ql2)+dql1^2*m2*ac2^2*cos(ql2)*sin(ql2)-dql1^2*m3*ac3*sin(ql3)*a2-dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)+dql1^2*Ixx3*cos(ql2)*sin(ql2)-dql1^2*sin(ql2)*Ixx2*cos(ql2)-dql1^2*Iyy3*cos(ql3)*sin(ql3)-dql1^2*Iyy3*cos(ql2)*sin(ql2)+dql1^2*cos(ql2)*Iyy2*sin(ql2)+dql1^2*Ixx3*cos(ql3)*sin(ql3);...
        -dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)-dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)+dql1^2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+dql1^2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-dql1^2*m3*sin(ql2)*ac3^2*cos(ql2)-dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)+dql1^2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+dql1^2*mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*dql1^2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql1^2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+dql1^2*Ixx3*cos(ql3)*sin(ql3)+dql1^2*Ixx3*cos(ql2)*sin(ql2)-dql1^2*Iyy3*cos(ql2)*sin(ql2)-dql1^2*Iyy3*cos(ql3)*sin(ql3)+sin(ql3)*a2*dql2^2*m3*ac3+sin(ql3)*a2*dql2^2*mpl*a3];

    c_bar_l=[4*dql2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql3*m3*ac3*sin(ql3)*a2*cos(ql2)^2-2*dql3*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-2*dql3*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*dql3*mpl*sin(ql2)*a3^2*cos(ql2)+2*dql3*mpl*a3^2*sin(ql3)*cos(ql3)-2*dql2*mpl*a2^2*cos(ql2)*sin(ql2)+2*dql2*mpl*a3*sin(ql3)*a2-4*dql2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)-4*dql2*mpl*a2*cos(ql2)^2*a3*sin(ql3)-4*dql2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)-4*dql2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+4*dql2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-4*dql2*m3*ac3*sin(ql3)*a2*cos(ql2)^2-4*dql2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-4*dql3*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)-4*dql3*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql3*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql3*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+4*dql3*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)+4*dql3*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql3*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql3*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+2*dql2*m3*ac3*sin(ql3)*a2+2*dql3*m3*ac3^2*sin(ql3)*cos(ql3)+2*dql3*m3*sin(ql2)*ac3^2*cos(ql2)-2*dql2*m2*ac2^2*cos(ql2)*sin(ql2)+2*dql2*mpl*sin(ql2)*a3^2*cos(ql2)+2*dql2*mpl*a3^2*sin(ql3)*cos(ql3)-2*dql3*mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*dql2*m3*a2^2*cos(ql2)*sin(ql2)+2*dql2*m3*ac3^2*sin(ql3)*cos(ql3)+2*dql2*m3*sin(ql2)*ac3^2*cos(ql2)-2*dql2*Ixx3*cos(ql3)*sin(ql3)-2*dql2*Ixx3*cos(ql2)*sin(ql2)+2*dql2*Iyy3*cos(ql3)*sin(ql3)+2*dql2*Iyy3*cos(ql2)*sin(ql2)+2*dql2*sin(ql2)*Ixx2*cos(ql2)-2*dql2*cos(ql2)*Iyy2*sin(ql2)-2*dql3*Ixx3*cos(ql3)*sin(ql3)-2*dql3*Ixx3*cos(ql2)*sin(ql2)+2*dql3*Iyy3*cos(ql2)*sin(ql2)+2*dql3*Iyy3*cos(ql3)*sin(ql3),0,0;...
        (Ixx3*cos(ql2)*sin(ql2)+cos(ql2)*Iyy2*sin(ql2)-sin(ql2)*Ixx2*cos(ql2)+Ixx3*cos(ql3)*sin(ql3)-Iyy3*cos(ql3)*sin(ql3)-2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-Iyy3*cos(ql2)*sin(ql2)+2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)+2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*mpl*a2*cos(ql2)^2*a3*sin(ql3)+mpl*a2^2*cos(ql2)*sin(ql2)-mpl*a3*sin(ql3)*a2-m3*ac3^2*sin(ql3)*cos(ql3)-m3*sin(ql2)*ac3^2*cos(ql2)+m2*ac2^2*cos(ql2)*sin(ql2)-mpl*sin(ql2)*a3^2*cos(ql2)+m3*a2^2*cos(ql2)*sin(ql2)-mpl*a3^2*sin(ql3)*cos(ql3)-m3*ac3*sin(ql3)*a2)*dql1,-2*sin(ql3)*a2*(m3*ac3+mpl*a3)*dql3,-sin(ql3)*a2*(m3*ac3+mpl*a3)*dql3;...
        (-mpl*a3^2*sin(ql3)*cos(ql3)-mpl*sin(ql2)*a3^2*cos(ql2)+m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-m3*sin(ql2)*ac3^2*cos(ql2)-m3*ac3^2*sin(ql3)*cos(ql3)+m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+Ixx3*cos(ql3)*sin(ql3)+Ixx3*cos(ql2)*sin(ql2)-Iyy3*cos(ql2)*sin(ql2)-Iyy3*cos(ql3)*sin(ql3))*dql1,1/2*sin(ql3)*a2*(m3*ac3+mpl*a3)*(2*dql2-dql3),1/2*sin(ql3)*a2*(m3*ac3+mpl*a3)*dql2];

    b_l=[b_v(1)*dql1+sign(dql1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dql1)/epsilon));...
        b_v(2)*dql2+sign(dql2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dql2)/epsilon));...
        b_v(3)*dql3+sign(dql3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dql3)/epsilon))];

    b_SDC_l=[sign(dql1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dql1)/epsilon));...
        sign(dql2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dql2)/epsilon));...
        sign(dql3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dql3)/epsilon))];
    %Right arm matrices----------------------------------------------------
    M_r=[-2*mpr*cos(qr2)*a3^2*cos(qr3)*sin(qr2)*sin(qr3)+2*mpr*cos(qr2)^2*a3*cos(qr3)*a2-2*m3*cos(qr2)*ac3^2*cos(qr3)*sin(qr2)*sin(qr3)+2*m3*cos(qr2)^2*ac3*cos(qr3)*a2-2*m3*sin(qr2)*ac3*sin(qr3)*a2*cos(qr2)-2*mpr*sin(qr2)*a3*sin(qr3)*a2*cos(qr2)+mpr*a2^2*cos(qr2)^2-2*Ixx3*cos(qr3)^2*cos(qr2)^2+m3*a2^2*cos(qr2)^2-m3*ac3^2*cos(qr3)^2+Iyy1+Ixx2+Iyy3-Ixx2*cos(qr2)^2-Iyy3*cos(qr3)^2-Iyy3*cos(qr2)^2+Ixx3*cos(qr2)^2+Ixx3*cos(qr3)^2+m3*ac3^2+mpr*a3^2-m3*ac3^2*cos(qr2)^2-mpr*a3^2*cos(qr3)^2-mpr*a3^2*cos(qr2)^2+2*mpr*cos(qr2)^2*a3^2*cos(qr3)^2+2*m3*cos(qr2)^2*ac3^2*cos(qr3)^2+cos(qr2)^2*Iyy2+m2*ac2^2*cos(qr2)^2+2*Ixx3*sin(qr2)*cos(qr3)*cos(qr2)*sin(qr3)-2*Iyy3*sin(qr2)*cos(qr3)*cos(qr2)*sin(qr3)+2*Iyy3*cos(qr2)^2*cos(qr3)^2,0,0;...
        0,Izz2+Izz3+mpr*a2^2+m3*a2^2+m2*ac2^2+2*mpr*a3*cos(qr3)*a2+2*m3*ac3*cos(qr3)*a2+m3*ac3^2+mpr*a3^2,Izz3+mpr*a3*cos(qr3)*a2+m3*ac3*cos(qr3)*a2+m3*ac3^2+mpr*a3^2;...
        0,Izz3+mpr*a3*cos(qr3)*a2+m3*ac3*cos(qr3)*a2+m3*ac3^2+mpr*a3^2,Izz3+m3*ac3^2+mpr*a3^2];

    g_r=[0;...
        -g0*(m2*ac2*cos(qr2)+m3*cos(qr2)*ac3*cos(qr3)-m3*sin(qr2)*ac3*sin(qr3)+m3*a2*cos(qr2)+mpr*cos(qr2)*a3*cos(qr3)-mpr*sin(qr2)*a3*sin(qr3)+mpr*a2*cos(qr2));...
        g0*(m3*sin(qr2)*ac3*sin(qr3)-m3*cos(qr2)*ac3*cos(qr3)+mpr*sin(qr2)*a3*sin(qr3)-mpr*cos(qr2)*a3*cos(qr3))];

    c_r=[-2*dqr1*(-2*dqr2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)+dqr3*m3*ac3*sin(qr3)*a2*cos(qr2)^2+dqr3*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)+dqr3*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)-dqr3*mpr*sin(qr2)*a3^2*cos(qr2)-dqr3*mpr*a3^2*sin(qr3)*cos(qr3)+dqr2*mpr*a2^2*cos(qr2)*sin(qr2)-dqr2*mpr*a3*sin(qr3)*a2+2*dqr2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*dqr2*mpr*a2*cos(qr2)^2*a3*sin(qr3)+2*dqr2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*dqr2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2-2*dqr2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+2*dqr2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*dqr2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)+2*dqr3*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*dqr3*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr3*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr3*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2-2*dqr3*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr3*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr3*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr3*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-dqr2*m3*ac3*sin(qr3)*a2-dqr3*m3*ac3^2*sin(qr3)*cos(qr3)-dqr3*m3*sin(qr2)*ac3^2*cos(qr2)+dqr2*m2*ac2^2*cos(qr2)*sin(qr2)-dqr2*mpr*sin(qr2)*a3^2*cos(qr2)-dqr2*mpr*a3^2*sin(qr3)*cos(qr3)+dqr3*mpr*a2*cos(qr2)^2*a3*sin(qr3)+dqr2*m3*a2^2*cos(qr2)*sin(qr2)-dqr2*m3*ac3^2*sin(qr3)*cos(qr3)-dqr2*m3*sin(qr2)*ac3^2*cos(qr2)+dqr2*Ixx3*cos(qr3)*sin(qr3)+dqr2*Ixx3*cos(qr2)*sin(qr2)-dqr2*Iyy3*cos(qr3)*sin(qr3)-dqr2*Iyy3*cos(qr2)*sin(qr2)-dqr2*sin(qr2)*Ixx2*cos(qr2)+dqr2*cos(qr2)*Iyy2*sin(qr2)+dqr3*Ixx3*cos(qr3)*sin(qr3)+dqr3*Ixx3*cos(qr2)*sin(qr2)-dqr3*Iyy3*cos(qr2)*sin(qr2)-dqr3*Iyy3*cos(qr3)*sin(qr3));...
        2*dqr1^2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*dqr1^2*mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*dqr1^2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr1^2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*sin(qr3)*a2*dqr3*dqr2*m3*ac3-2*sin(qr3)*a2*dqr3*dqr2*mpr*a3-dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)-dqr1^2*mpr*a3*sin(qr3)*a2-dqr1^2*m3*sin(qr2)*ac3^2*cos(qr2)-sin(qr3)*a2*dqr3^2*mpr*a3+2*dqr1^2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*dqr1^2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-sin(qr3)*a2*dqr3^2*m3*ac3+dqr1^2*mpr*a2^2*cos(qr2)*sin(qr2)-dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)+dqr1^2*m3*a2^2*cos(qr2)*sin(qr2)+dqr1^2*m2*ac2^2*cos(qr2)*sin(qr2)-dqr1^2*m3*ac3*sin(qr3)*a2-dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)+dqr1^2*Ixx3*cos(qr2)*sin(qr2)-dqr1^2*sin(qr2)*Ixx2*cos(qr2)-dqr1^2*Iyy3*cos(qr3)*sin(qr3)-dqr1^2*Iyy3*cos(qr2)*sin(qr2)+dqr1^2*cos(qr2)*Iyy2*sin(qr2)+dqr1^2*Ixx3*cos(qr3)*sin(qr3);...
        -dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)-dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)+dqr1^2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+dqr1^2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-dqr1^2*m3*sin(qr2)*ac3^2*cos(qr2)-dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)+dqr1^2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+dqr1^2*mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*dqr1^2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr1^2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+dqr1^2*Ixx3*cos(qr3)*sin(qr3)+dqr1^2*Ixx3*cos(qr2)*sin(qr2)-dqr1^2*Iyy3*cos(qr2)*sin(qr2)-dqr1^2*Iyy3*cos(qr3)*sin(qr3)+sin(qr3)*a2*dqr2^2*m3*ac3+sin(qr3)*a2*dqr2^2*mpr*a3];

    c_bar_r=[4*dqr2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr3*m3*ac3*sin(qr3)*a2*cos(qr2)^2-2*dqr3*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-2*dqr3*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*dqr3*mpr*sin(qr2)*a3^2*cos(qr2)+2*dqr3*mpr*a3^2*sin(qr3)*cos(qr3)-2*dqr2*mpr*a2^2*cos(qr2)*sin(qr2)+2*dqr2*mpr*a3*sin(qr3)*a2-4*dqr2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)-4*dqr2*mpr*a2*cos(qr2)^2*a3*sin(qr3)-4*dqr2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)-4*dqr2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+4*dqr2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-4*dqr2*m3*ac3*sin(qr3)*a2*cos(qr2)^2-4*dqr2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-4*dqr3*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)-4*dqr3*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr3*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr3*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+4*dqr3*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)+4*dqr3*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr3*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr3*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+2*dqr2*m3*ac3*sin(qr3)*a2+2*dqr3*m3*ac3^2*sin(qr3)*cos(qr3)+2*dqr3*m3*sin(qr2)*ac3^2*cos(qr2)-2*dqr2*m2*ac2^2*cos(qr2)*sin(qr2)+2*dqr2*mpr*sin(qr2)*a3^2*cos(qr2)+2*dqr2*mpr*a3^2*sin(qr3)*cos(qr3)-2*dqr3*mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*dqr2*m3*a2^2*cos(qr2)*sin(qr2)+2*dqr2*m3*ac3^2*sin(qr3)*cos(qr3)+2*dqr2*m3*sin(qr2)*ac3^2*cos(qr2)-2*dqr2*Ixx3*cos(qr3)*sin(qr3)-2*dqr2*Ixx3*cos(qr2)*sin(qr2)+2*dqr2*Iyy3*cos(qr3)*sin(qr3)+2*dqr2*Iyy3*cos(qr2)*sin(qr2)+2*dqr2*sin(qr2)*Ixx2*cos(qr2)-2*dqr2*cos(qr2)*Iyy2*sin(qr2)-2*dqr3*Ixx3*cos(qr3)*sin(qr3)-2*dqr3*Ixx3*cos(qr2)*sin(qr2)+2*dqr3*Iyy3*cos(qr2)*sin(qr2)+2*dqr3*Iyy3*cos(qr3)*sin(qr3),0,0;...
        (Ixx3*cos(qr2)*sin(qr2)+cos(qr2)*Iyy2*sin(qr2)-sin(qr2)*Ixx2*cos(qr2)+Ixx3*cos(qr3)*sin(qr3)-Iyy3*cos(qr3)*sin(qr3)-2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-Iyy3*cos(qr2)*sin(qr2)+2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)+2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*mpr*a2*cos(qr2)^2*a3*sin(qr3)+mpr*a2^2*cos(qr2)*sin(qr2)-mpr*a3*sin(qr3)*a2-m3*ac3^2*sin(qr3)*cos(qr3)-m3*sin(qr2)*ac3^2*cos(qr2)+m2*ac2^2*cos(qr2)*sin(qr2)-mpr*sin(qr2)*a3^2*cos(qr2)+m3*a2^2*cos(qr2)*sin(qr2)-mpr*a3^2*sin(qr3)*cos(qr3)-m3*ac3*sin(qr3)*a2)*dqr1,-2*sin(qr3)*a2*(m3*ac3+mpr*a3)*dqr3,-sin(qr3)*a2*(m3*ac3+mpr*a3)*dqr3;...
        (-mpr*a3^2*sin(qr3)*cos(qr3)-mpr*sin(qr2)*a3^2*cos(qr2)+m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-m3*sin(qr2)*ac3^2*cos(qr2)-m3*ac3^2*sin(qr3)*cos(qr3)+m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+Ixx3*cos(qr3)*sin(qr3)+Ixx3*cos(qr2)*sin(qr2)-Iyy3*cos(qr2)*sin(qr2)-Iyy3*cos(qr3)*sin(qr3))*dqr1,1/2*sin(qr3)*a2*(m3*ac3+mpr*a3)*(2*dqr2-dqr3),1/2*sin(qr3)*a2*(m3*ac3+mpr*a3)*dqr2];

    b_r=[b_v(1)*dqr1+sign(dqr1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dqr1)/epsilon));...
        b_v(2)*dqr2+sign(dqr2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dqr2)/epsilon));...
        b_v(3)*dqr3+sign(dqr3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dqr3)/epsilon))];

    b_SDC_r=[sign(dqr1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dqr1)/epsilon));...
        sign(dqr2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dqr2)/epsilon));...
        sign(dqr3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dqr3)/epsilon))];
    %External force--------------------------------------------------------
    J_l=[-sin(ql1)*(cos(ql2)*a3*cos(ql3)-sin(ql2)*a3*sin(ql3)+a2*cos(ql2)),-cos(ql1)*(sin(ql2)*a3*cos(ql3)+cos(ql2)*a3*sin(ql3)+a2*sin(ql2)),-cos(ql1)*a3*(cos(ql2)*sin(ql3)+sin(ql2)*cos(ql3));...
        cos(ql1)*(cos(ql2)*a3*cos(ql3)-sin(ql2)*a3*sin(ql3)+a2*cos(ql2)),-sin(ql1)*(sin(ql2)*a3*cos(ql3)+cos(ql2)*a3*sin(ql3)+a2*sin(ql2)),-sin(ql1)*a3*(cos(ql2)*sin(ql3)+sin(ql2)*cos(ql3));...
        0,-cos(ql2)*a3*cos(ql3)+sin(ql2)*a3*sin(ql3)-a2*cos(ql2),a3*(sin(ql2)*sin(ql3)-cos(ql2)*cos(ql3))];

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

    J_r=[-sin(qr1)*(cos(qr2)*a3*cos(qr3)-sin(qr2)*a3*sin(qr3)+a2*cos(qr2)),-cos(qr1)*(sin(qr2)*a3*cos(qr3)+cos(qr2)*a3*sin(qr3)+a2*sin(qr2)),-cos(qr1)*a3*(cos(qr2)*sin(qr3)+sin(qr2)*cos(qr3));...
        cos(qr1)*(cos(qr2)*a3*cos(qr3)-sin(qr2)*a3*sin(qr3)+a2*cos(qr2)),-sin(qr1)*(sin(qr2)*a3*cos(qr3)+cos(qr2)*a3*sin(qr3)+a2*sin(qr2)),-sin(qr1)*a3*(cos(qr2)*sin(qr3)+sin(qr2)*cos(qr3));...
        0,-cos(qr2)*a3*cos(qr3)+sin(qr2)*a3*sin(qr3)-a2*cos(qr2),a3*(sin(qr2)*sin(qr3)-cos(qr2)*cos(qr3))];

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

    d2xe_d=-2/81*t(i)^3+1/9*t(i)^2-1/9*t(i);
    d2ye_d=0;
    d2ze_d=2/27*t(i)^3-1/3*t(i)^2+1/3*t(i);

    fe=-[mp*d2xe_d;...
        mp*d2ye_d;...
        mp*d2ze_d+mp*g0];

    d2pe=[d2xe_d;d2ye_d;d2ze_d];

    d2q_l=inv(J_l)*(d2pe-J_l_dot*dq_l_l);
    d2q_r=inv(J_r)*(d2pe-J_r_dot*dq_l_r);

    d2q_m_l=inv(J_l)*(d2pe-J_l_dot*dq_m_l);
    d2q_m_r=inv(J_r)*(d2pe-J_r_dot*dq_m_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+diag(J)*d2q_m_l)+...
        inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r+diag(J)*d2q_m_r)-fe);

    fe_l=inv(J_l')*(M_l*d2q_l+c_l+g_l+b_l+diag(J)*d2q_m_l-inv(Q_bar_l)*inv(J_l')'*gama);
    fe_r=inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r+diag(J)*d2q_m_r-inv(Q_bar_r)*inv(J_r')'*gama);
    %SDC-------------------------------------------------------------------
    W_f1_l=[-inv(M_l)*diag(k_mad),inv(M_l)*diag(k_mad);...
        inv(diag(J))*diag(k_mad),-inv(diag(J))*diag(k_mad)];
    W_f2_l=[-inv(M_l)*(c_bar_l+diag(b_v)),zeros(n,n);...
        zeros(n,n),zeros(n,n)];
    A_l=[zeros(2*n,2*n),eye(2*n);...
        W_f1_l,W_f2_l];
    B_l=[zeros(3*n,n);inv(diag(J))];
    u_add_l=g_l+b_SDC_l-J_l'*fe_l;

    W_f1_r=[-inv(M_r)*diag(k_mad),inv(M_r)*diag(k_mad);...
        inv(diag(J))*diag(k_mad),-inv(diag(J))*diag(k_mad)];
    W_f2_r=[-inv(M_r)*(c_bar_r+diag(b_v)),zeros(n,n);...
        zeros(n,n),zeros(n,n)];
    A_r=[zeros(2*n,2*n),eye(2*n);...
        W_f1_r,W_f2_r];
    B_r=[zeros(3*n,n);inv(diag(J))];
    u_add_r=g_r+b_SDC_r-J_r'*fe_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;qdl1;qdl2;qdl3];dq_l]+u_add_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;qdr1;qdr2;qdr3];dq_r]+u_add_r;

    if u_l(1) > (ustall(1)-ustall(1)/omeganl(1)*dql4)
        u_l(1)=ustall(1)-ustall(1)/omeganl(1)*dql4;
    end
    if u_l(1) < (-ustall(1)-ustall(1)/omeganl(1)*dql4)
        u_l(1)=-ustall(1)-ustall(1)/omeganl(1)*dql4;
    end
    if u_l(2) > (ustall(2)-ustall(2)/omeganl(2)*dql5)
        u_l(2)=ustall(2)-ustall(2)/omeganl(2)*dql5;
    end
    if u_l(2) < (-ustall(2)-ustall(2)/omeganl(2)*dql5)
        u_l(2)=-ustall(2)-ustall(2)/omeganl(2)*dql5;
    end
    if u_l(3) > (ustall(3)-ustall(3)/omeganl(3)*dql6)
        u_l(3)=ustall(3)-ustall(3)/omeganl(3)*dql6;
    end
    if u_l(3) < (-ustall(3)-ustall(3)/omeganl(3)*dql6)
        u_l(3)=-ustall(3)-ustall(3)/omeganl(3)*dql6;
    end

    if u_r(1) > (ustall(1)-ustall(1)/omeganl(1)*dqr4)
        u_r(1)=ustall(1)-ustall(1)/omeganl(1)*dqr4;
    end
    if u_r(1) < (-ustall(1)-ustall(1)/omeganl(1)*dqr4)
        u_r(1)=-ustall(1)-ustall(1)/omeganl(1)*dqr4;
    end
    if u_r(2) > (ustall(2)-ustall(2)/omeganl(2)*dqr5)
        u_r(2)=ustall(2)-ustall(2)/omeganl(2)*dqr5;
    end
    if u_r(2) < (-ustall(2)-ustall(2)/omeganl(2)*dqr5)
        u_r(2)=-ustall(2)-ustall(2)/omeganl(2)*dqr5;
    end
    if u_r(3) > (ustall(3)-ustall(3)/omeganl(3)*dqr6)
        u_r(3)=ustall(3)-ustall(3)/omeganl(3)*dqr6;
    end
    if u_r(3) < (-ustall(3)-ustall(3)/omeganl(3)*dqr6)
        u_r(3)=-ustall(3)-ustall(3)/omeganl(3)*dqr6;
    end
    uss_l(1:n,i)=u_l;
    norm_u_l(i)=norm(u_l);
    uss_r(1:n,i)=u_r;
    norm_u_r(i)=norm(u_r);
end

afFigurePosition=[100,100,700,500];
figure(3)
subplot(3,2,1,'Parent',figure(3),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('a');grid on;box on;hold all;
plot(t,ustall(1)-ustall(1)/omeganl(1)*x(:,19),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall(1)-ustall(1)/omeganl(1)*x(:,19),'-.','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) (N.m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Upper Bound','Lower Bound','\tau_i_,_j(t)')
subplot(3,2,2,'Parent',figure(3),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('b');grid on;box on;hold all;
plot(t,ustall(2)-ustall(2)/omeganl(2)*x(:,20),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall(2)-ustall(2)/omeganl(2)*x(:,20),'-.','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) (N.m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,3,'Parent',figure(3),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('c');grid on;box on;hold all;
plot(t,ustall(3)-ustall(3)/omeganl(3)*x(:,21),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall(3)-ustall(3)/omeganl(3)*x(:,21),'-.','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) (N.m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,4,'Parent',figure(3),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('d');grid on;box on;hold all;
plot(t,ustall(1)-ustall(1)/omeganl(1)*x(:,22),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall(1)-ustall(1)/omeganl(1)*x(:,22),'-.','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) (N.m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,5,'Parent',figure(3),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('e');grid on;box on;hold all;
plot(t,ustall(2)-ustall(2)/omeganl(2)*x(:,23),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall(2)-ustall(2)/omeganl(2)*x(:,23),'-.','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) (N.m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
subplot(3,2,6,'Parent',figure(3),'FontWeight','bold','FontSize',10,'FontName','Cambria')
title('f');grid on;box on;hold all;
plot(t,ustall(3)-ustall(3)/omeganl(3)*x(:,24),'--','linewidth',1.5,'Color',[0 0.498 0])
plot(t,-ustall(3)-ustall(3)/omeganl(3)*x(:,24),'-.','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) (N.m)','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)

afFigurePosition=[200,150,380,220];
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|','FontWeight','bold','FontSize',10,'FontName','Cambria')
xlim([0,tf]);set(gcf,'Position',afFigurePosition)
legend('Left','Right')
%System--------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xdot=sys(t,x)
global d1 a2 a3 dc1 ac2 ac3 m1 m2 m3 mpl mpr mp...
    Ixx1 Ixx2 Ixx3 Iyy1 Iyy2 Iyy3 Izz1 Izz2 Izz3...
    b_v b_d b_s epsilon...
    Q_l R_l Q_r R_r n g0 ustall omeganl Q_bar_l Q_bar_r J k_mad
Time=t
%States--------------------------------------------------------------------
ql1=x(1);
ql2=x(2);
ql3=x(3);
qr1=x(4);
qr2=x(5);
qr3=x(6);

ql4=x(7);
ql5=x(8);
ql6=x(9);
qr4=x(10);
qr5=x(11);
qr6=x(12);

dql1=x(13);
dql2=x(14);
dql3=x(15);
dqr1=x(16);
dqr2=x(17);
dqr3=x(18);

dql4=x(19);
dql5=x(20);
dql6=x(21);
dqr4=x(22);
dqr5=x(23);
dqr6=x(24);

qdl1=x(25);
qdl2=x(26);
qdl3=x(27);
qdr1=x(28);
qdr2=x(29);
qdr3=x(30);

q_l_l=[ql1;ql2;ql3];
q_l_r=[qr1;qr2;qr3];
q_m_l=[ql4;ql5;ql6];
q_m_r=[qr4;qr5;qr6];

dq_l_l=[dql1;dql2;dql3];
dq_l_r=[dqr1;dqr2;dqr3];
dq_m_l=[dql4;dql5;dql6];
dq_m_r=[dqr4;dqr5;dqr6];

q_l=[q_l_l;q_m_l];
q_r=[q_l_r;q_m_r];
dq_l=[dq_l_l;dq_m_l];
dq_r=[dq_l_r;dq_m_r];
%Path----------------------------------------------------------------------
dxe_d=-1/162*t^4+1/27*t^3-1/18*t^2;
dye_d=0;
dze_d=1/54*t^4-1/9*t^3+1/6*t^2;

V=[dxe_d;...
    dye_d;...
    dze_d];
%Jacobian desired----------------------------------------------------------
Jd_l=[-sin(qdl1)*(cos(qdl2)*a3*cos(qdl3)-sin(qdl2)*a3*sin(qdl3)+a2*cos(qdl2)),-cos(qdl1)*(sin(qdl2)*a3*cos(qdl3)+cos(qdl2)*a3*sin(qdl3)+a2*sin(qdl2)),-cos(qdl1)*a3*(cos(qdl2)*sin(qdl3)+sin(qdl2)*cos(qdl3));...
    cos(qdl1)*(cos(qdl2)*a3*cos(qdl3)-sin(qdl2)*a3*sin(qdl3)+a2*cos(qdl2)),-sin(qdl1)*(sin(qdl2)*a3*cos(qdl3)+cos(qdl2)*a3*sin(qdl3)+a2*sin(qdl2)),-sin(qdl1)*a3*(cos(qdl2)*sin(qdl3)+sin(qdl2)*cos(qdl3));...
    0,-cos(qdl2)*a3*cos(qdl3)+sin(qdl2)*a3*sin(qdl3)-a2*cos(qdl2),a3*(sin(qdl2)*sin(qdl3)-cos(qdl2)*cos(qdl3))];

Jd_r=[-sin(qdr1)*(cos(qdr2)*a3*cos(qdr3)-sin(qdr2)*a3*sin(qdr3)+a2*cos(qdr2)),-cos(qdr1)*(sin(qdr2)*a3*cos(qdr3)+cos(qdr2)*a3*sin(qdr3)+a2*sin(qdr2)),-cos(qdr1)*a3*(cos(qdr2)*sin(qdr3)+sin(qdr2)*cos(qdr3));...
    cos(qdr1)*(cos(qdr2)*a3*cos(qdr3)-sin(qdr2)*a3*sin(qdr3)+a2*cos(qdr2)),-sin(qdr1)*(sin(qdr2)*a3*cos(qdr3)+cos(qdr2)*a3*sin(qdr3)+a2*sin(qdr2)),-sin(qdr1)*a3*(cos(qdr2)*sin(qdr3)+sin(qdr2)*cos(qdr3));...
    0,-cos(qdr2)*a3*cos(qdr3)+sin(qdr2)*a3*sin(qdr3)-a2*cos(qdr2),a3*(sin(qdr2)*sin(qdr3)-cos(qdr2)*cos(qdr3))];
%Left arm matrices---------------------------------------------------------
M_l=[-2*mpl*cos(ql2)*a3^2*cos(ql3)*sin(ql2)*sin(ql3)+2*mpl*cos(ql2)^2*a3*cos(ql3)*a2-2*m3*cos(ql2)*ac3^2*cos(ql3)*sin(ql2)*sin(ql3)+2*m3*cos(ql2)^2*ac3*cos(ql3)*a2-2*m3*sin(ql2)*ac3*sin(ql3)*a2*cos(ql2)-2*mpl*sin(ql2)*a3*sin(ql3)*a2*cos(ql2)+mpl*a2^2*cos(ql2)^2-2*Ixx3*cos(ql3)^2*cos(ql2)^2+m3*a2^2*cos(ql2)^2-m3*ac3^2*cos(ql3)^2+Iyy1+Ixx2+Iyy3-Ixx2*cos(ql2)^2-Iyy3*cos(ql3)^2-Iyy3*cos(ql2)^2+Ixx3*cos(ql2)^2+Ixx3*cos(ql3)^2+m3*ac3^2+mpl*a3^2-m3*ac3^2*cos(ql2)^2-mpl*a3^2*cos(ql3)^2-mpl*a3^2*cos(ql2)^2+2*mpl*cos(ql2)^2*a3^2*cos(ql3)^2+2*m3*cos(ql2)^2*ac3^2*cos(ql3)^2+cos(ql2)^2*Iyy2+m2*ac2^2*cos(ql2)^2+2*Ixx3*sin(ql2)*cos(ql3)*cos(ql2)*sin(ql3)-2*Iyy3*sin(ql2)*cos(ql3)*cos(ql2)*sin(ql3)+2*Iyy3*cos(ql2)^2*cos(ql3)^2,0,0;...
    0,Izz2+Izz3+mpl*a2^2+m3*a2^2+m2*ac2^2+2*mpl*a3*cos(ql3)*a2+2*m3*ac3*cos(ql3)*a2+m3*ac3^2+mpl*a3^2,Izz3+mpl*a3*cos(ql3)*a2+m3*ac3*cos(ql3)*a2+m3*ac3^2+mpl*a3^2;...
    0,Izz3+mpl*a3*cos(ql3)*a2+m3*ac3*cos(ql3)*a2+m3*ac3^2+mpl*a3^2,Izz3+m3*ac3^2+mpl*a3^2];

g_l=[0;...
    -g0*(m2*ac2*cos(ql2)+m3*cos(ql2)*ac3*cos(ql3)-m3*sin(ql2)*ac3*sin(ql3)+m3*a2*cos(ql2)+mpl*cos(ql2)*a3*cos(ql3)-mpl*sin(ql2)*a3*sin(ql3)+mpl*a2*cos(ql2));...
    g0*(m3*sin(ql2)*ac3*sin(ql3)-m3*cos(ql2)*ac3*cos(ql3)+mpl*sin(ql2)*a3*sin(ql3)-mpl*cos(ql2)*a3*cos(ql3))];

c_l=[-2*dql1*(-2*dql2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)+dql3*m3*ac3*sin(ql3)*a2*cos(ql2)^2+dql3*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)+dql3*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)-dql3*mpl*sin(ql2)*a3^2*cos(ql2)-dql3*mpl*a3^2*sin(ql3)*cos(ql3)+dql2*mpl*a2^2*cos(ql2)*sin(ql2)-dql2*mpl*a3*sin(ql3)*a2+2*dql2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*dql2*mpl*a2*cos(ql2)^2*a3*sin(ql3)+2*dql2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*dql2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2-2*dql2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+2*dql2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*dql2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)+2*dql3*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*dql3*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql3*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql3*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2-2*dql3*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql3*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql3*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql3*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-dql2*m3*ac3*sin(ql3)*a2-dql3*m3*ac3^2*sin(ql3)*cos(ql3)-dql3*m3*sin(ql2)*ac3^2*cos(ql2)+dql2*m2*ac2^2*cos(ql2)*sin(ql2)-dql2*mpl*sin(ql2)*a3^2*cos(ql2)-dql2*mpl*a3^2*sin(ql3)*cos(ql3)+dql3*mpl*a2*cos(ql2)^2*a3*sin(ql3)+dql2*m3*a2^2*cos(ql2)*sin(ql2)-dql2*m3*ac3^2*sin(ql3)*cos(ql3)-dql2*m3*sin(ql2)*ac3^2*cos(ql2)+dql2*Ixx3*cos(ql3)*sin(ql3)+dql2*Ixx3*cos(ql2)*sin(ql2)-dql2*Iyy3*cos(ql3)*sin(ql3)-dql2*Iyy3*cos(ql2)*sin(ql2)-dql2*sin(ql2)*Ixx2*cos(ql2)+dql2*cos(ql2)*Iyy2*sin(ql2)+dql3*Ixx3*cos(ql3)*sin(ql3)+dql3*Ixx3*cos(ql2)*sin(ql2)-dql3*Iyy3*cos(ql2)*sin(ql2)-dql3*Iyy3*cos(ql3)*sin(ql3));...
    2*dql1^2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*dql1^2*mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*dql1^2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql1^2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*sin(ql3)*a2*dql3*dql2*m3*ac3-2*sin(ql3)*a2*dql3*dql2*mpl*a3-dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)-dql1^2*mpl*a3*sin(ql3)*a2-dql1^2*m3*sin(ql2)*ac3^2*cos(ql2)-sin(ql3)*a2*dql3^2*mpl*a3+2*dql1^2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*dql1^2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-sin(ql3)*a2*dql3^2*m3*ac3+dql1^2*mpl*a2^2*cos(ql2)*sin(ql2)-dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)+dql1^2*m3*a2^2*cos(ql2)*sin(ql2)+dql1^2*m2*ac2^2*cos(ql2)*sin(ql2)-dql1^2*m3*ac3*sin(ql3)*a2-dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)+dql1^2*Ixx3*cos(ql2)*sin(ql2)-dql1^2*sin(ql2)*Ixx2*cos(ql2)-dql1^2*Iyy3*cos(ql3)*sin(ql3)-dql1^2*Iyy3*cos(ql2)*sin(ql2)+dql1^2*cos(ql2)*Iyy2*sin(ql2)+dql1^2*Ixx3*cos(ql3)*sin(ql3);...
    -dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)-dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)+dql1^2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+dql1^2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-dql1^2*m3*sin(ql2)*ac3^2*cos(ql2)-dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)+dql1^2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*dql1^2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*dql1^2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*dql1^2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+dql1^2*mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*dql1^2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql1^2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*dql1^2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+dql1^2*Ixx3*cos(ql3)*sin(ql3)+dql1^2*Ixx3*cos(ql2)*sin(ql2)-dql1^2*Iyy3*cos(ql2)*sin(ql2)-dql1^2*Iyy3*cos(ql3)*sin(ql3)+sin(ql3)*a2*dql2^2*m3*ac3+sin(ql3)*a2*dql2^2*mpl*a3];

c_bar_l=[4*dql2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*dql3*m3*ac3*sin(ql3)*a2*cos(ql2)^2-2*dql3*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-2*dql3*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*dql3*mpl*sin(ql2)*a3^2*cos(ql2)+2*dql3*mpl*a3^2*sin(ql3)*cos(ql3)-2*dql2*mpl*a2^2*cos(ql2)*sin(ql2)+2*dql2*mpl*a3*sin(ql3)*a2-4*dql2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)-4*dql2*mpl*a2*cos(ql2)^2*a3*sin(ql3)-4*dql2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)-4*dql2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+4*dql2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-4*dql2*m3*ac3*sin(ql3)*a2*cos(ql2)^2-4*dql2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-4*dql3*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)-4*dql3*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql3*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2-4*dql3*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+4*dql3*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)+4*dql3*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql3*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)-4*dql3*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+2*dql2*m3*ac3*sin(ql3)*a2+2*dql3*m3*ac3^2*sin(ql3)*cos(ql3)+2*dql3*m3*sin(ql2)*ac3^2*cos(ql2)-2*dql2*m2*ac2^2*cos(ql2)*sin(ql2)+2*dql2*mpl*sin(ql2)*a3^2*cos(ql2)+2*dql2*mpl*a3^2*sin(ql3)*cos(ql3)-2*dql3*mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*dql2*m3*a2^2*cos(ql2)*sin(ql2)+2*dql2*m3*ac3^2*sin(ql3)*cos(ql3)+2*dql2*m3*sin(ql2)*ac3^2*cos(ql2)-2*dql2*Ixx3*cos(ql3)*sin(ql3)-2*dql2*Ixx3*cos(ql2)*sin(ql2)+2*dql2*Iyy3*cos(ql3)*sin(ql3)+2*dql2*Iyy3*cos(ql2)*sin(ql2)+2*dql2*sin(ql2)*Ixx2*cos(ql2)-2*dql2*cos(ql2)*Iyy2*sin(ql2)-2*dql3*Ixx3*cos(ql3)*sin(ql3)-2*dql3*Ixx3*cos(ql2)*sin(ql2)+2*dql3*Iyy3*cos(ql2)*sin(ql2)+2*dql3*Iyy3*cos(ql3)*sin(ql3),0,0;...
    (Ixx3*cos(ql2)*sin(ql2)+cos(ql2)*Iyy2*sin(ql2)-sin(ql2)*Ixx2*cos(ql2)+Ixx3*cos(ql3)*sin(ql3)-Iyy3*cos(ql3)*sin(ql3)-2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)-Iyy3*cos(ql2)*sin(ql2)+2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)+2*m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+2*mpl*a2*cos(ql2)^2*a3*sin(ql3)+mpl*a2^2*cos(ql2)*sin(ql2)-mpl*a3*sin(ql3)*a2-m3*ac3^2*sin(ql3)*cos(ql3)-m3*sin(ql2)*ac3^2*cos(ql2)+m2*ac2^2*cos(ql2)*sin(ql2)-mpl*sin(ql2)*a3^2*cos(ql2)+m3*a2^2*cos(ql2)*sin(ql2)-mpl*a3^2*sin(ql3)*cos(ql3)-m3*ac3*sin(ql3)*a2)*dql1,-2*sin(ql3)*a2*(m3*ac3+mpl*a3)*dql3,-sin(ql3)*a2*(m3*ac3+mpl*a3)*dql3;...
    (-mpl*a3^2*sin(ql3)*cos(ql3)-mpl*sin(ql2)*a3^2*cos(ql2)+m3*cos(ql2)*ac3*cos(ql3)*a2*sin(ql2)+mpl*cos(ql2)*a3*cos(ql3)*a2*sin(ql2)-m3*sin(ql2)*ac3^2*cos(ql2)-m3*ac3^2*sin(ql3)*cos(ql3)+m3*ac3*sin(ql3)*a2*cos(ql2)^2+2*mpl*sin(ql2)*a3^2*cos(ql2)*cos(ql3)^2+2*mpl*a3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*m3*ac3^2*sin(ql3)*cos(ql3)*cos(ql2)^2+2*m3*cos(ql2)*ac3^2*cos(ql3)^2*sin(ql2)+mpl*a2*cos(ql2)^2*a3*sin(ql3)-2*Ixx3*sin(ql2)*cos(ql3)^2*cos(ql2)-2*Ixx3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*cos(ql2)^2*sin(ql3)*cos(ql3)+2*Iyy3*sin(ql2)*cos(ql3)^2*cos(ql2)+Ixx3*cos(ql3)*sin(ql3)+Ixx3*cos(ql2)*sin(ql2)-Iyy3*cos(ql2)*sin(ql2)-Iyy3*cos(ql3)*sin(ql3))*dql1,1/2*sin(ql3)*a2*(m3*ac3+mpl*a3)*(2*dql2-dql3),1/2*sin(ql3)*a2*(m3*ac3+mpl*a3)*dql2];

b_l=[b_v(1)*dql1+sign(dql1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dql1)/epsilon));...
    b_v(2)*dql2+sign(dql2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dql2)/epsilon));...
    b_v(3)*dql3+sign(dql3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dql3)/epsilon))];

b_SDC_l=[sign(dql1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dql1)/epsilon));...
    sign(dql2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dql2)/epsilon));...
    sign(dql3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dql3)/epsilon))];

k_bar_l=[k_mad(1)*(ql1-ql4);...
    k_mad(2)*(ql2-ql5);...
    k_mad(3)*(ql3-ql6)];
%Right arm matrices--------------------------------------------------------
M_r=[-2*mpr*cos(qr2)*a3^2*cos(qr3)*sin(qr2)*sin(qr3)+2*mpr*cos(qr2)^2*a3*cos(qr3)*a2-2*m3*cos(qr2)*ac3^2*cos(qr3)*sin(qr2)*sin(qr3)+2*m3*cos(qr2)^2*ac3*cos(qr3)*a2-2*m3*sin(qr2)*ac3*sin(qr3)*a2*cos(qr2)-2*mpr*sin(qr2)*a3*sin(qr3)*a2*cos(qr2)+mpr*a2^2*cos(qr2)^2-2*Ixx3*cos(qr3)^2*cos(qr2)^2+m3*a2^2*cos(qr2)^2-m3*ac3^2*cos(qr3)^2+Iyy1+Ixx2+Iyy3-Ixx2*cos(qr2)^2-Iyy3*cos(qr3)^2-Iyy3*cos(qr2)^2+Ixx3*cos(qr2)^2+Ixx3*cos(qr3)^2+m3*ac3^2+mpr*a3^2-m3*ac3^2*cos(qr2)^2-mpr*a3^2*cos(qr3)^2-mpr*a3^2*cos(qr2)^2+2*mpr*cos(qr2)^2*a3^2*cos(qr3)^2+2*m3*cos(qr2)^2*ac3^2*cos(qr3)^2+cos(qr2)^2*Iyy2+m2*ac2^2*cos(qr2)^2+2*Ixx3*sin(qr2)*cos(qr3)*cos(qr2)*sin(qr3)-2*Iyy3*sin(qr2)*cos(qr3)*cos(qr2)*sin(qr3)+2*Iyy3*cos(qr2)^2*cos(qr3)^2,0,0;...
    0,Izz2+Izz3+mpr*a2^2+m3*a2^2+m2*ac2^2+2*mpr*a3*cos(qr3)*a2+2*m3*ac3*cos(qr3)*a2+m3*ac3^2+mpr*a3^2,Izz3+mpr*a3*cos(qr3)*a2+m3*ac3*cos(qr3)*a2+m3*ac3^2+mpr*a3^2;...
    0,Izz3+mpr*a3*cos(qr3)*a2+m3*ac3*cos(qr3)*a2+m3*ac3^2+mpr*a3^2,Izz3+m3*ac3^2+mpr*a3^2];

g_r=[0;...
    -g0*(m2*ac2*cos(qr2)+m3*cos(qr2)*ac3*cos(qr3)-m3*sin(qr2)*ac3*sin(qr3)+m3*a2*cos(qr2)+mpr*cos(qr2)*a3*cos(qr3)-mpr*sin(qr2)*a3*sin(qr3)+mpr*a2*cos(qr2));...
    g0*(m3*sin(qr2)*ac3*sin(qr3)-m3*cos(qr2)*ac3*cos(qr3)+mpr*sin(qr2)*a3*sin(qr3)-mpr*cos(qr2)*a3*cos(qr3))];

c_r=[-2*dqr1*(-2*dqr2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)+dqr3*m3*ac3*sin(qr3)*a2*cos(qr2)^2+dqr3*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)+dqr3*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)-dqr3*mpr*sin(qr2)*a3^2*cos(qr2)-dqr3*mpr*a3^2*sin(qr3)*cos(qr3)+dqr2*mpr*a2^2*cos(qr2)*sin(qr2)-dqr2*mpr*a3*sin(qr3)*a2+2*dqr2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*dqr2*mpr*a2*cos(qr2)^2*a3*sin(qr3)+2*dqr2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*dqr2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2-2*dqr2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+2*dqr2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*dqr2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)+2*dqr3*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*dqr3*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr3*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr3*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2-2*dqr3*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr3*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr3*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr3*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-dqr2*m3*ac3*sin(qr3)*a2-dqr3*m3*ac3^2*sin(qr3)*cos(qr3)-dqr3*m3*sin(qr2)*ac3^2*cos(qr2)+dqr2*m2*ac2^2*cos(qr2)*sin(qr2)-dqr2*mpr*sin(qr2)*a3^2*cos(qr2)-dqr2*mpr*a3^2*sin(qr3)*cos(qr3)+dqr3*mpr*a2*cos(qr2)^2*a3*sin(qr3)+dqr2*m3*a2^2*cos(qr2)*sin(qr2)-dqr2*m3*ac3^2*sin(qr3)*cos(qr3)-dqr2*m3*sin(qr2)*ac3^2*cos(qr2)+dqr2*Ixx3*cos(qr3)*sin(qr3)+dqr2*Ixx3*cos(qr2)*sin(qr2)-dqr2*Iyy3*cos(qr3)*sin(qr3)-dqr2*Iyy3*cos(qr2)*sin(qr2)-dqr2*sin(qr2)*Ixx2*cos(qr2)+dqr2*cos(qr2)*Iyy2*sin(qr2)+dqr3*Ixx3*cos(qr3)*sin(qr3)+dqr3*Ixx3*cos(qr2)*sin(qr2)-dqr3*Iyy3*cos(qr2)*sin(qr2)-dqr3*Iyy3*cos(qr3)*sin(qr3));...
    2*dqr1^2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*dqr1^2*mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*dqr1^2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr1^2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*sin(qr3)*a2*dqr3*dqr2*m3*ac3-2*sin(qr3)*a2*dqr3*dqr2*mpr*a3-dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)-dqr1^2*mpr*a3*sin(qr3)*a2-dqr1^2*m3*sin(qr2)*ac3^2*cos(qr2)-sin(qr3)*a2*dqr3^2*mpr*a3+2*dqr1^2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*dqr1^2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-sin(qr3)*a2*dqr3^2*m3*ac3+dqr1^2*mpr*a2^2*cos(qr2)*sin(qr2)-dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)+dqr1^2*m3*a2^2*cos(qr2)*sin(qr2)+dqr1^2*m2*ac2^2*cos(qr2)*sin(qr2)-dqr1^2*m3*ac3*sin(qr3)*a2-dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)+dqr1^2*Ixx3*cos(qr2)*sin(qr2)-dqr1^2*sin(qr2)*Ixx2*cos(qr2)-dqr1^2*Iyy3*cos(qr3)*sin(qr3)-dqr1^2*Iyy3*cos(qr2)*sin(qr2)+dqr1^2*cos(qr2)*Iyy2*sin(qr2)+dqr1^2*Ixx3*cos(qr3)*sin(qr3);...
    -dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)-dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)+dqr1^2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+dqr1^2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-dqr1^2*m3*sin(qr2)*ac3^2*cos(qr2)-dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)+dqr1^2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*dqr1^2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*dqr1^2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*dqr1^2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+dqr1^2*mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*dqr1^2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr1^2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*dqr1^2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+dqr1^2*Ixx3*cos(qr3)*sin(qr3)+dqr1^2*Ixx3*cos(qr2)*sin(qr2)-dqr1^2*Iyy3*cos(qr2)*sin(qr2)-dqr1^2*Iyy3*cos(qr3)*sin(qr3)+sin(qr3)*a2*dqr2^2*m3*ac3+sin(qr3)*a2*dqr2^2*mpr*a3];

c_bar_r=[4*dqr2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*dqr3*m3*ac3*sin(qr3)*a2*cos(qr2)^2-2*dqr3*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-2*dqr3*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*dqr3*mpr*sin(qr2)*a3^2*cos(qr2)+2*dqr3*mpr*a3^2*sin(qr3)*cos(qr3)-2*dqr2*mpr*a2^2*cos(qr2)*sin(qr2)+2*dqr2*mpr*a3*sin(qr3)*a2-4*dqr2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)-4*dqr2*mpr*a2*cos(qr2)^2*a3*sin(qr3)-4*dqr2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)-4*dqr2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+4*dqr2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-4*dqr2*m3*ac3*sin(qr3)*a2*cos(qr2)^2-4*dqr2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-4*dqr3*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)-4*dqr3*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr3*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2-4*dqr3*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+4*dqr3*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)+4*dqr3*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr3*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)-4*dqr3*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+2*dqr2*m3*ac3*sin(qr3)*a2+2*dqr3*m3*ac3^2*sin(qr3)*cos(qr3)+2*dqr3*m3*sin(qr2)*ac3^2*cos(qr2)-2*dqr2*m2*ac2^2*cos(qr2)*sin(qr2)+2*dqr2*mpr*sin(qr2)*a3^2*cos(qr2)+2*dqr2*mpr*a3^2*sin(qr3)*cos(qr3)-2*dqr3*mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*dqr2*m3*a2^2*cos(qr2)*sin(qr2)+2*dqr2*m3*ac3^2*sin(qr3)*cos(qr3)+2*dqr2*m3*sin(qr2)*ac3^2*cos(qr2)-2*dqr2*Ixx3*cos(qr3)*sin(qr3)-2*dqr2*Ixx3*cos(qr2)*sin(qr2)+2*dqr2*Iyy3*cos(qr3)*sin(qr3)+2*dqr2*Iyy3*cos(qr2)*sin(qr2)+2*dqr2*sin(qr2)*Ixx2*cos(qr2)-2*dqr2*cos(qr2)*Iyy2*sin(qr2)-2*dqr3*Ixx3*cos(qr3)*sin(qr3)-2*dqr3*Ixx3*cos(qr2)*sin(qr2)+2*dqr3*Iyy3*cos(qr2)*sin(qr2)+2*dqr3*Iyy3*cos(qr3)*sin(qr3),0,0;...
    (Ixx3*cos(qr2)*sin(qr2)+cos(qr2)*Iyy2*sin(qr2)-sin(qr2)*Ixx2*cos(qr2)+Ixx3*cos(qr3)*sin(qr3)-Iyy3*cos(qr3)*sin(qr3)-2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)-Iyy3*cos(qr2)*sin(qr2)+2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)+2*m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+2*mpr*a2*cos(qr2)^2*a3*sin(qr3)+mpr*a2^2*cos(qr2)*sin(qr2)-mpr*a3*sin(qr3)*a2-m3*ac3^2*sin(qr3)*cos(qr3)-m3*sin(qr2)*ac3^2*cos(qr2)+m2*ac2^2*cos(qr2)*sin(qr2)-mpr*sin(qr2)*a3^2*cos(qr2)+m3*a2^2*cos(qr2)*sin(qr2)-mpr*a3^2*sin(qr3)*cos(qr3)-m3*ac3*sin(qr3)*a2)*dqr1,-2*sin(qr3)*a2*(m3*ac3+mpr*a3)*dqr3,-sin(qr3)*a2*(m3*ac3+mpr*a3)*dqr3;...
    (-mpr*a3^2*sin(qr3)*cos(qr3)-mpr*sin(qr2)*a3^2*cos(qr2)+m3*cos(qr2)*ac3*cos(qr3)*a2*sin(qr2)+mpr*cos(qr2)*a3*cos(qr3)*a2*sin(qr2)-m3*sin(qr2)*ac3^2*cos(qr2)-m3*ac3^2*sin(qr3)*cos(qr3)+m3*ac3*sin(qr3)*a2*cos(qr2)^2+2*mpr*sin(qr2)*a3^2*cos(qr2)*cos(qr3)^2+2*mpr*a3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*m3*ac3^2*sin(qr3)*cos(qr3)*cos(qr2)^2+2*m3*cos(qr2)*ac3^2*cos(qr3)^2*sin(qr2)+mpr*a2*cos(qr2)^2*a3*sin(qr3)-2*Ixx3*sin(qr2)*cos(qr3)^2*cos(qr2)-2*Ixx3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*cos(qr2)^2*sin(qr3)*cos(qr3)+2*Iyy3*sin(qr2)*cos(qr3)^2*cos(qr2)+Ixx3*cos(qr3)*sin(qr3)+Ixx3*cos(qr2)*sin(qr2)-Iyy3*cos(qr2)*sin(qr2)-Iyy3*cos(qr3)*sin(qr3))*dqr1,1/2*sin(qr3)*a2*(m3*ac3+mpr*a3)*(2*dqr2-dqr3),1/2*sin(qr3)*a2*(m3*ac3+mpr*a3)*dqr2];

b_r=[b_v(1)*dqr1+sign(dqr1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dqr1)/epsilon));...
    b_v(2)*dqr2+sign(dqr2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dqr2)/epsilon));...
    b_v(3)*dqr3+sign(dqr3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dqr3)/epsilon))];

b_SDC_r=[sign(dqr1)*(b_d(1)+(b_s(1)-b_d(1))*exp(-abs(dqr1)/epsilon));...
    sign(dqr2)*(b_d(2)+(b_s(2)-b_d(2))*exp(-abs(dqr2)/epsilon));...
    sign(dqr3)*(b_d(3)+(b_s(3)-b_d(3))*exp(-abs(dqr3)/epsilon))];

k_bar_r=[k_mad(1)*(qr1-qr4);...
    k_mad(2)*(qr2-qr5);...
    k_mad(3)*(qr3-qr6)];
%External force------------------------------------------------------------
J_l=[-sin(ql1)*(cos(ql2)*a3*cos(ql3)-sin(ql2)*a3*sin(ql3)+a2*cos(ql2)),-cos(ql1)*(sin(ql2)*a3*cos(ql3)+cos(ql2)*a3*sin(ql3)+a2*sin(ql2)),-cos(ql1)*a3*(cos(ql2)*sin(ql3)+sin(ql2)*cos(ql3));...
    cos(ql1)*(cos(ql2)*a3*cos(ql3)-sin(ql2)*a3*sin(ql3)+a2*cos(ql2)),-sin(ql1)*(sin(ql2)*a3*cos(ql3)+cos(ql2)*a3*sin(ql3)+a2*sin(ql2)),-sin(ql1)*a3*(cos(ql2)*sin(ql3)+sin(ql2)*cos(ql3));...
    0,-cos(ql2)*a3*cos(ql3)+sin(ql2)*a3*sin(ql3)-a2*cos(ql2),a3*(sin(ql2)*sin(ql3)-cos(ql2)*cos(ql3))];

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

J_r=[-sin(qr1)*(cos(qr2)*a3*cos(qr3)-sin(qr2)*a3*sin(qr3)+a2*cos(qr2)),-cos(qr1)*(sin(qr2)*a3*cos(qr3)+cos(qr2)*a3*sin(qr3)+a2*sin(qr2)),-cos(qr1)*a3*(cos(qr2)*sin(qr3)+sin(qr2)*cos(qr3));...
    cos(qr1)*(cos(qr2)*a3*cos(qr3)-sin(qr2)*a3*sin(qr3)+a2*cos(qr2)),-sin(qr1)*(sin(qr2)*a3*cos(qr3)+cos(qr2)*a3*sin(qr3)+a2*sin(qr2)),-sin(qr1)*a3*(cos(qr2)*sin(qr3)+sin(qr2)*cos(qr3));...
    0,-cos(qr2)*a3*cos(qr3)+sin(qr2)*a3*sin(qr3)-a2*cos(qr2),a3*(sin(qr2)*sin(qr3)-cos(qr2)*cos(qr3))];

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

d2xe_d=-2/81*t^3+1/9*t^2-1/9*t;
d2ye_d=0;
d2ze_d=2/27*t^3-1/3*t^2+1/3*t;

fe=-[mp*d2xe_d;...
    mp*d2ye_d;...
    mp*d2ze_d+mp*g0];

d2pe=[d2xe_d;d2ye_d;d2ze_d];

d2q_l=inv(J_l)*(d2pe-J_l_dot*dq_l_l);
d2q_r=inv(J_r)*(d2pe-J_r_dot*dq_l_r);

d2q_m_l=inv(J_l)*(d2pe-J_l_dot*dq_m_l);
d2q_m_r=inv(J_r)*(d2pe-J_r_dot*dq_m_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+diag(J)*d2q_m_l)+...
    inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r+diag(J)*d2q_m_r)-fe);

fe_l=inv(J_l')*(M_l*d2q_l+c_l+g_l+b_l+diag(J)*d2q_m_l-inv(Q_bar_l)*inv(J_l')'*gama);
fe_r=inv(J_r')*(M_r*d2q_r+c_r+g_r+b_r+diag(J)*d2q_m_r-inv(Q_bar_r)*inv(J_r')'*gama);
%SDC-----------------------------------------------------------------------
W_f1_l=[-inv(M_l)*diag(k_mad),inv(M_l)*diag(k_mad);...
    inv(diag(J))*diag(k_mad),-inv(diag(J))*diag(k_mad)];
W_f2_l=[-inv(M_l)*(c_bar_l+diag(b_v)),zeros(n,n);...
    zeros(n,n),zeros(n,n)];
A_l=[zeros(2*n,2*n),eye(2*n);...
    W_f1_l,W_f2_l];
B_l=[zeros(3*n,n);inv(diag(J))];
u_add_l=g_l+b_SDC_l-J_l'*fe_l;

W_f1_r=[-inv(M_r)*diag(k_mad),inv(M_r)*diag(k_mad);...
    inv(diag(J))*diag(k_mad),-inv(diag(J))*diag(k_mad)];
W_f2_r=[-inv(M_r)*(c_bar_r+diag(b_v)),zeros(n,n);...
    zeros(n,n),zeros(n,n)];
A_r=[zeros(2*n,2*n),eye(2*n);...
    W_f1_r,W_f2_r];
B_r=[zeros(3*n,n);inv(diag(J))];
u_add_r=g_r+b_SDC_r-J_r'*fe_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;qdl1;qdl2;qdl3];dq_l]+u_add_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;qdr1;qdr2;qdr3];dq_r]+u_add_r;

if u_l(1) > (ustall(1)-ustall(1)/omeganl(1)*dql4)
    u_l(1)=ustall(1)-ustall(1)/omeganl(1)*dql4;
end
if u_l(1) < (-ustall(1)-ustall(1)/omeganl(1)*dql4)
    u_l(1)=-ustall(1)-ustall(1)/omeganl(1)*dql4;
end
if u_l(2) > (ustall(2)-ustall(2)/omeganl(2)*dql5)
    u_l(2)=ustall(2)-ustall(2)/omeganl(2)*dql5;
end
if u_l(2) < (-ustall(2)-ustall(2)/omeganl(2)*dql5)
    u_l(2)=-ustall(2)-ustall(2)/omeganl(2)*dql5;
end
if u_l(3) > (ustall(3)-ustall(3)/omeganl(3)*dql6)
    u_l(3)=ustall(3)-ustall(3)/omeganl(3)*dql6;
end
if u_l(3) < (-ustall(3)-ustall(3)/omeganl(3)*dql6)
    u_l(3)=-ustall(3)-ustall(3)/omeganl(3)*dql6;
end

if u_r(1) > (ustall(1)-ustall(1)/omeganl(1)*dqr4)
    u_r(1)=ustall(1)-ustall(1)/omeganl(1)*dqr4;
end
if u_r(1) < (-ustall(1)-ustall(1)/omeganl(1)*dqr4)
    u_r(1)=-ustall(1)-ustall(1)/omeganl(1)*dqr4;
end
if u_r(2) > (ustall(2)-ustall(2)/omeganl(2)*dqr5)
    u_r(2)=ustall(2)-ustall(2)/omeganl(2)*dqr5;
end
if u_r(2) < (-ustall(2)-ustall(2)/omeganl(2)*dqr5)
    u_r(2)=-ustall(2)-ustall(2)/omeganl(2)*dqr5;
end
if u_r(3) > (ustall(3)-ustall(3)/omeganl(3)*dqr6)
    u_r(3)=ustall(3)-ustall(3)/omeganl(3)*dqr6;
end
if u_r(3) < (-ustall(3)-ustall(3)/omeganl(3)*dqr6)
    u_r(3)=-ustall(3)-ustall(3)/omeganl(3)*dqr6;
end
%System--------------------------------------------------------------------
xdot=[dq_l_l;...
    dq_l_r;...
    dq_m_l;...
    dq_m_r;...
    inv(M_l)*(J_l'*fe_l-c_l-g_l-b_l-k_bar_l);...
    inv(M_r)*(J_r'*fe_r-c_r-g_r-b_r-k_bar_r);...
    inv(diag(J))*(u_l+k_bar_l);...
    inv(diag(J))*(u_r+k_bar_r);...
    inv(Jd_l)*V;...
    inv(Jd_r)*V];