%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Peppermill Carrier: Inverse dynamics
% Peyman Karimi Eskandary, PhD, Bruno Belzile, PhD, eng. jr.
% McGill University, Centre for Intelligent Machines
% Robotic Mechanical Systems Laboratory
% May 2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear
close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%% Parameters %%%%%%%%%%%%%%%%%%%%
p=60/1000;
pp=20/1000;
r=300/1000;    %m
% l=0.6-r;
l=300/1000;    %m
Test_Cycle=2;  %sec

g=9.81;        %m/s^2
rc= 35/1000;   %m
lc=150/1000;   %m
GR=1/22.5;     %Gear Ratio of the C-drive
ma=4.72;       %Kg
mf=0.134*2;    %Kg
mn=0.59;       %Kg
mp=1.294;      %Kg


Ia=0.0313;     %Kg.m^2
If=0.002*2;      %Kg.m^2
Ipep=0.0006;    %Kg.m^2
Is=6.24e-7 * 400;  %Kg.m^2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Data Loading %%%%%%%%%%%%%%
load('PMCdata2KHz1000ms_NEW.mat')
T=PMCdata(1,:)/max(PMCdata(1,:)) * Test_Cycle;
X=PMCdata(2,:)/1000;
X=X+r-max(X)/2;
% X=(2*r)-X;         %%%% rotate the path 90 deg
Y=PMCdata(3,:)/1000;
Y=Y+r-max(Y)/2;
Z0=300;
Z=PMCdata(4,:)/1000 - 325/1000 + Z0/1000;
Phi=PMCdata(5,:) - pi/2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Joint Variables %%%%%%%%%%%%%%
J_c=[2*pi/p  1/GR    0      0 ;
    -2*pi/p  1/GR    0      0 ;
        0     0   2*pi/p  1/GR;
        0     0  -2*pi/p  1/GR];

% h1=Z(length(T)/4)-(pp/(2*pi))*Phi(length(T)/4); 
% h2=Z(length(T)/4)+(pp/(2*pi))*Phi(length(T)/4);
h1=-l;
h2=-l;
k1=sqrt(r^2 + h1^2);
k2=sqrt(r^2 + h2^2);
d=[ r ; acos((r^2-l^2+k1^2)/(2*r*k1))+atan(h1/r) ; r ; 2*pi-acos((r^2-l^2+k2^2)/(2*r*k2))-atan(h2/r) ];
psi_mid=J_c*d;

H1=Z-(pp/(2*pi))*Phi; 
H2=Z+(pp/(2*pi))*Phi;
K1=sqrt(Y.^2 + H1.^2);
K2=sqrt(X.^2 + H2.^2);
Theta1=acos((r^2-l^2+K1.^2)./(2*r*K1))+atan(H1./Y);
Theta2=2*pi-acos((r^2-l^2+K2.^2)./(2*r*K2))-atan(H2./X);
Lambda1=2*pi-acos((l^2-r^2+K1.^2)./(2*l*K1))+atan(H1./Y);
Lambda2=acos((l^2-r^2+K2.^2)./(2*l*K2))-atan(H2./X);
lambda1=2*pi-acos((Y-r*cos(Theta1))/l);
lambda2=acos((X-r*cos(Theta2))/l);
D=[ X ; Theta1 ; Y ; Theta2 ]; 
Psi=J_c*D;
Psi(1,:) = Psi(1,:)-psi_mid(1);
Psi(2,:) = Psi(2,:)-psi_mid(2);
Psi(3,:) = Psi(3,:)-psi_mid(3);
Psi(4,:) = Psi(4,:)-psi_mid(4);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%% Joint Deriviteves %%%%%%%%%%%%%%
Psi_dot = diff(Psi,1,2) / (Test_Cycle/length(T));
Psi_dot(:,length(Psi_dot)+1)=Psi_dot(:,length(Psi_dot));

Psi_dotdot = diff(Psi,2,2) / ((Test_Cycle/length(T))^2);
Psi_dotdot(:,length(Psi_dotdot)+1)=Psi_dotdot(:,length(Psi_dotdot));
Psi_dotdot(:,length(Psi_dotdot)+1)=Psi_dotdot(:,length(Psi_dotdot));

Theta1_dot = diff(Theta1) / (Test_Cycle/length(T));
Theta1_dot(:,length(Theta1_dot)+1)=Theta1_dot(:,length(Theta1_dot));
Theta2_dot = diff(Theta2) / (Test_Cycle/length(T));
Theta2_dot(:,length(Theta2_dot)+1)=Theta2_dot(:,length(Theta2_dot));

Lambda1_dot = diff(Lambda1) / (Test_Cycle/length(T));
Lambda1_dot(:,length(Lambda1_dot)+1)=Lambda1_dot(:,length(Lambda1_dot));
Lambda2_dot = diff(Lambda2) / (Test_Cycle/length(T));
Lambda2_dot(:,length(Lambda2_dot)+1)=Lambda2_dot(:,length(Lambda2_dot));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Jacobian %%%%%%%%%%%%%%%%%%%%%
for i=1:length(T)
    B1=(r^2 - l^2 +K1(i)^2)/(2*r*K1(i));
    B2=(r^2 - l^2 +K2(i)^2)/(2*r*K2(i));
    A1=(B1*r-K1(i))/(r*sqrt(1-B1^2));
    A2=(K2(i)-B2*r)/(r*sqrt(1-B2^2));
    F1=(H1(i)*A1+Y(i))/(K1(i)^2);
    F2=(H2(i)*A2-X(i))/(K2(i)^2);
    G1=(Y(i)*A1-H1(i))/(K1(i)^2);
    G2=(X(i)*A2+H2(i))/(K2(i)^2);

J(:,:,i) = [ 2*pi/p   G1     F1    -pp*F1/(2*pi) ;
            -2*pi/p   G1     F1    -pp*F1/(2*pi) ;
              G2    2*pi/p   F2     pp*F2/(2*pi) ;
              G2   -2*pi/p   F2     pp*F2/(2*pi) ];
end

X_dot  =  diff(X) / (Test_Cycle/length(T)); X_dot(length(X_dot)+1)=X_dot(length(X_dot));
Y_dot  =  diff(Y) / (Test_Cycle/length(T)); Y_dot(length(Y_dot)+1)=Y_dot(length(Y_dot));
Z_dot  =  diff(Z) / (Test_Cycle/length(T)); Z_dot(length(Z_dot)+1)=Z_dot(length(Z_dot));
Phi_dot = diff(Phi) / (Test_Cycle/length(T)); Phi_dot(length(Phi_dot)+1)=Phi_dot(length(Phi_dot));

for i=1:length(T)
   PSI_dot(:,i) = J(:,:,i) * [X_dot(i) ; Y_dot(i) ; Z_dot(i) ; Phi_dot(i)];   
end    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%% NOC Constants %%%%%%%%%%%%%%%%%%%%%%%
Ms1=diag([Is j  j  j  j  j ]);
Ms2=diag([j  Is j  j  j  j ]);
Ma1=diag([Ia j  j  ma ma ma]);
Ma2=diag([j  Ia j  ma ma ma]);
Mf1=diag([If j  j  mf mf mf]);
Mf2=diag([j  If j  mf mf mf]);
Mn1=diag([j  j  j  mn mn mn]);
Mn2=diag([j  j  j  mn mn mn]);
Mp =diag([j  j  Ipep mp mp mp]);

wGa1=[0;0;0;0;0;-ma*g];
wGa2=[0;0;0;0;0;-ma*g];
wGf1=[0;0;0;0;0;-mf*g];
wGf2=[0;0;0;0;0;-mf*g];
wGn1=[0;0;0;0;0;-mn*g];
wGn2=[0;0;0;0;0;-mn*g];
wGp =[0;0;0;0;0;-mp*g];
wEp =[0;0;0;0;0;0];   %Cable force 9999
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%% Twist-Shaping Matrices %%%%%%%%%%%%%%%%%%%%
for i=1:length(T)
TsL1(:,:,i)=[1 0 0 0;
             0 0 0 0;
             zeros(4)];

TsR1(:,:,i)=[0 1 0 0;
             0 0 0 0;
             zeros(4)];
         
TsL2(:,:,i)=[0 0 0 0;
             0 0 1 0;
             zeros(4)];

TsR2(:,:,i)=[0 0 0 0;
             0 0 0 1;
             zeros(4)];
         
Ta1(:,:,i)= 0.5*[GR GR 0 0;
          0 0 0 0;
          0 0 0 0;
          p/(2*pi)  -p/(2*pi)  0 0;
         -rc*GR*sin(Theta1(i))  -rc*GR*sin(Theta1(i))  0 0;
          rc*GR*cos(Theta1(i))   rc*GR*cos(Theta1(i))  0 0];
      
Ta2(:,:,i)= 0.5*[0 0 0 0;
          0 0 GR GR;
          0 0 0 0;      
          0 0 -rc*GR*sin(Theta2(i))  -rc*GR*sin(Theta2(i));
          0 0  p/(2*pi)   -p/(2*pi);
          0 0 -rc*GR*cos(Theta2(i))  -rc*GR*cos(Theta2(i))];

Tf1(:,:,i)= 0.5*[(-r*GR*sin(Theta1(i)))/(l*sin(Lambda1(i)))  (-r*GR*sin(Theta1(i)))/(l*sin(Lambda1(i)))  -p/(2*pi*l*sin(Lambda1(i)))  p/(2*pi*l*sin(Lambda1(i)));
          0 0 0 0;
          0 0 0 0;
          p/(2*pi)  -p/(2*pi) 0 0;
          (r/l)*GR*(lc-l)*sin(Theta1(i))  (r/l)*GR*(lc-l)*sin(Theta1(i)) (p*lc)/(2*pi*l)  -(p*lc)/(2*pi*l);
          r*GR*cos(Theta1(i))-(r*GR*lc*sin(Theta1(i)))/(l*tan(Lambda1(i)))   r*GR*cos(Theta1(i))-(r*GR*lc*sin(Theta1(i)))/(l*tan(Lambda1(i)))  -(p*lc)/(2*pi*l*tan(Lambda1(i)))  (p*lc)/(2*pi*l*tan(Lambda1(i)))];
      
Tf2(:,:,i)= 0.5*[0 0 0 0;
          -p/(2*pi*l*sin(Lambda2(i)))  p/(2*pi*l*sin(Lambda2(i)))   (-r*GR*sin(Theta2(i)))/(l*sin(Lambda2(i)))  (-r*GR*sin(Theta2(i)))/(l*sin(Lambda2(i)));
          0 0 0 0;      
          (p*lc)/(2*pi*l)  -(p*lc)/(2*pi*l)  (r/l)*GR*(lc-l)*sin(Theta2(i))  (r/l)*GR*(lc-l)*sin(Theta2(i));
          0 0 p/(2*pi)   -p/(2*pi);
          (p*lc)/(2*pi*l*tan(Lambda2(i)))  -(p*lc)/(2*pi*l*tan(Lambda2(i)))   (r*GR*lc*sin(Theta2(i)))/(l*tan(Lambda2(i))) - r*GR*cos(Theta2(i))   (r*GR*lc*sin(Theta2(i)))/(l*tan(Lambda2(i))) - r*GR*cos(Theta2(i)) ];
      
Tn1(:,:,i)= 0.5*[0 0 0 0;
          0 0 0 0;
          0 0 0 0;
          p/(2*pi)  -p/(2*pi) 0 0;
          0 0 p/(2*pi)  -p/(2*pi);
          r*GR*cos(Theta1(i))-(r*GR*sin(Theta1(i)))/tan(Lambda1(i))   r*GR*cos(Theta1(i))-(r*GR*sin(Theta1(i)))/tan(Lambda1(i))  -p/(2*pi*tan(Lambda1(i)))  p/(2*pi*tan(Lambda1(i)))];
      
Tn2(:,:,i)= 0.5*[0 0 0 0;
          0 0 0 0;
          0 0 0 0;
          p/(2*pi)  -p/(2*pi) 0 0;
          0 0 p/(2*pi)  -p/(2*pi);
          p/(2*pi*tan(Lambda2(i)))  -p/(2*pi*tan(Lambda2(i)))   (r*GR*sin(Theta2(i)))/tan(Lambda2(i)) - r*GR*cos(Theta2(i))   (r*GR*sin(Theta2(i)))/tan(Lambda2(i)) - r*GR*cos(Theta2(i)) ];      

Tp(:,:,i) = 0.5*[0 0 0 0;
          0 0 0 0;
          (pi/pp)*(Tn2(6,:,i)-Tn1(6,:,i));
          p/(2*pi)  -p/(2*pi) 0 0;
          0 0 p/(2*pi)  -p/(2*pi);
          0.5*(Tn2(6,:,i)+Tn1(6,:,i))];   

Tcbl(:,:,i) = 0.5*[0 0 0 0;
          0 0 0 0;
          0 0 0 0;
          0 0 0 0;
          0 0 0 0;
          0.5*(Tn2(6,:,i)+Tn1(6,:,i))];  
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%% Twist-Shaping Derivatives %%%%%%%%%%%%%%%%%
dTa1= zeros(6,4,length(T)-1); dTa1(1,1,:)=diff(Ta1(1,1,:)) / (Test_Cycle/length(T)); dTa1(1,2,:)=diff(Ta1(1,2,:)) / (Test_Cycle/length(T)); dTa1(1,3,:)=diff(Ta1(1,3,:)) / (Test_Cycle/length(T)); dTa1(1,4,:)=diff(Ta1(1,4,:)) / (Test_Cycle/length(T)); dTa1(2,1,:)=diff(Ta1(2,1,:)) / (Test_Cycle/length(T)); dTa1(2,2,:)=diff(Ta1(2,2,:)) / (Test_Cycle/length(T)); dTa1(2,3,:)=diff(Ta1(2,3,:)) / (Test_Cycle/length(T)); dTa1(2,4,:)=diff(Ta1(2,4,:)) / (Test_Cycle/length(T)); dTa1(3,1,:)=diff(Ta1(3,1,:)) / (Test_Cycle/length(T)); dTa1(3,2,:)=diff(Ta1(3,2,:)) / (Test_Cycle/length(T)); dTa1(3,3,:)=diff(Ta1(3,3,:)) / (Test_Cycle/length(T)); dTa1(3,4,:)=diff(Ta1(3,4,:)) / (Test_Cycle/length(T)); dTa1(4,1,:)=diff(Ta1(4,1,:)) / (Test_Cycle/length(T)); dTa1(4,2,:)=diff(Ta1(4,2,:)) / (Test_Cycle/length(T)); dTa1(4,3,:)=diff(Ta1(4,3,:)) / (Test_Cycle/length(T)); dTa1(4,4,:)=diff(Ta1(4,4,:)) / (Test_Cycle/length(T)); dTa1(5,1,:)=diff(Ta1(5,1,:)) / (Test_Cycle/length(T)); dTa1(5,2,:)=diff(Ta1(5,2,:)) / (Test_Cycle/length(T)); dTa1(5,3,:)=diff(Ta1(5,3,:)) / (Test_Cycle/length(T)); dTa1(5,4,:)=diff(Ta1(5,4,:)) / (Test_Cycle/length(T)); dTa1(6,1,:)=diff(Ta1(6,1,:)) / (Test_Cycle/length(T)); dTa1(6,2,:)=diff(Ta1(6,2,:)) / (Test_Cycle/length(T)); dTa1(6,3,:)=diff(Ta1(6,3,:)) / (Test_Cycle/length(T)); dTa1(6,4,:)=diff(Ta1(6,4,:)) / (Test_Cycle/length(T)); dTa1(:,:,length(T))=dTa1(:,:,length(T)-1);
dTa2= zeros(6,4,length(T)-1); dTa2(1,1,:)=diff(Ta2(1,1,:)) / (Test_Cycle/length(T)); dTa2(1,2,:)=diff(Ta2(1,2,:)) / (Test_Cycle/length(T)); dTa2(1,3,:)=diff(Ta2(1,3,:)) / (Test_Cycle/length(T)); dTa2(1,4,:)=diff(Ta2(1,4,:)) / (Test_Cycle/length(T)); dTa2(2,1,:)=diff(Ta2(2,1,:)) / (Test_Cycle/length(T)); dTa2(2,2,:)=diff(Ta2(2,2,:)) / (Test_Cycle/length(T)); dTa2(2,3,:)=diff(Ta2(2,3,:)) / (Test_Cycle/length(T)); dTa2(2,4,:)=diff(Ta2(2,4,:)) / (Test_Cycle/length(T)); dTa2(3,1,:)=diff(Ta2(3,1,:)) / (Test_Cycle/length(T)); dTa2(3,2,:)=diff(Ta2(3,2,:)) / (Test_Cycle/length(T)); dTa2(3,3,:)=diff(Ta2(3,3,:)) / (Test_Cycle/length(T)); dTa2(3,4,:)=diff(Ta2(3,4,:)) / (Test_Cycle/length(T)); dTa2(4,1,:)=diff(Ta2(4,1,:)) / (Test_Cycle/length(T)); dTa2(4,2,:)=diff(Ta2(4,2,:)) / (Test_Cycle/length(T)); dTa2(4,3,:)=diff(Ta2(4,3,:)) / (Test_Cycle/length(T)); dTa2(4,4,:)=diff(Ta2(4,4,:)) / (Test_Cycle/length(T)); dTa2(5,1,:)=diff(Ta2(5,1,:)) / (Test_Cycle/length(T)); dTa2(5,2,:)=diff(Ta2(5,2,:)) / (Test_Cycle/length(T)); dTa2(5,3,:)=diff(Ta2(5,3,:)) / (Test_Cycle/length(T)); dTa2(5,4,:)=diff(Ta2(5,4,:)) / (Test_Cycle/length(T)); dTa2(6,1,:)=diff(Ta2(6,1,:)) / (Test_Cycle/length(T)); dTa2(6,2,:)=diff(Ta2(6,2,:)) / (Test_Cycle/length(T)); dTa2(6,3,:)=diff(Ta2(6,3,:)) / (Test_Cycle/length(T)); dTa2(6,4,:)=diff(Ta2(6,4,:)) / (Test_Cycle/length(T)); dTa2(:,:,length(T))=dTa2(:,:,length(T)-1);
dTf1= zeros(6,4,length(T)-1); dTf1(1,1,:)=diff(Tf1(1,1,:)) / (Test_Cycle/length(T)); dTf1(1,2,:)=diff(Tf1(1,2,:)) / (Test_Cycle/length(T)); dTf1(1,3,:)=diff(Tf1(1,3,:)) / (Test_Cycle/length(T)); dTf1(1,4,:)=diff(Tf1(1,4,:)) / (Test_Cycle/length(T)); dTf1(2,1,:)=diff(Tf1(2,1,:)) / (Test_Cycle/length(T)); dTf1(2,2,:)=diff(Tf1(2,2,:)) / (Test_Cycle/length(T)); dTf1(2,3,:)=diff(Tf1(2,3,:)) / (Test_Cycle/length(T)); dTf1(2,4,:)=diff(Tf1(2,4,:)) / (Test_Cycle/length(T)); dTf1(3,1,:)=diff(Tf1(3,1,:)) / (Test_Cycle/length(T)); dTf1(3,2,:)=diff(Tf1(3,2,:)) / (Test_Cycle/length(T)); dTf1(3,3,:)=diff(Tf1(3,3,:)) / (Test_Cycle/length(T)); dTf1(3,4,:)=diff(Tf1(3,4,:)) / (Test_Cycle/length(T)); dTf1(4,1,:)=diff(Tf1(4,1,:)) / (Test_Cycle/length(T)); dTf1(4,2,:)=diff(Tf1(4,2,:)) / (Test_Cycle/length(T)); dTf1(4,3,:)=diff(Tf1(4,3,:)) / (Test_Cycle/length(T)); dTf1(4,4,:)=diff(Tf1(4,4,:)) / (Test_Cycle/length(T)); dTf1(5,1,:)=diff(Tf1(5,1,:)) / (Test_Cycle/length(T)); dTf1(5,2,:)=diff(Tf1(5,2,:)) / (Test_Cycle/length(T)); dTf1(5,3,:)=diff(Tf1(5,3,:)) / (Test_Cycle/length(T)); dTf1(5,4,:)=diff(Tf1(5,4,:)) / (Test_Cycle/length(T)); dTf1(6,1,:)=diff(Tf1(6,1,:)) / (Test_Cycle/length(T)); dTf1(6,2,:)=diff(Tf1(6,2,:)) / (Test_Cycle/length(T)); dTf1(6,3,:)=diff(Tf1(6,3,:)) / (Test_Cycle/length(T)); dTf1(6,4,:)=diff(Tf1(6,4,:)) / (Test_Cycle/length(T)); dTf1(:,:,length(T))=dTf1(:,:,length(T)-1);
dTf2= zeros(6,4,length(T)-1); dTf2(1,1,:)=diff(Tf2(1,1,:)) / (Test_Cycle/length(T)); dTf2(1,2,:)=diff(Tf2(1,2,:)) / (Test_Cycle/length(T)); dTf2(1,3,:)=diff(Tf2(1,3,:)) / (Test_Cycle/length(T)); dTf2(1,4,:)=diff(Tf2(1,4,:)) / (Test_Cycle/length(T)); dTf2(2,1,:)=diff(Tf2(2,1,:)) / (Test_Cycle/length(T)); dTf2(2,2,:)=diff(Tf2(2,2,:)) / (Test_Cycle/length(T)); dTf2(2,3,:)=diff(Tf2(2,3,:)) / (Test_Cycle/length(T)); dTf2(2,4,:)=diff(Tf2(2,4,:)) / (Test_Cycle/length(T)); dTf2(3,1,:)=diff(Tf2(3,1,:)) / (Test_Cycle/length(T)); dTf2(3,2,:)=diff(Tf2(3,2,:)) / (Test_Cycle/length(T)); dTf2(3,3,:)=diff(Tf2(3,3,:)) / (Test_Cycle/length(T)); dTf2(3,4,:)=diff(Tf2(3,4,:)) / (Test_Cycle/length(T)); dTf2(4,1,:)=diff(Tf2(4,1,:)) / (Test_Cycle/length(T)); dTf2(4,2,:)=diff(Tf2(4,2,:)) / (Test_Cycle/length(T)); dTf2(4,3,:)=diff(Tf2(4,3,:)) / (Test_Cycle/length(T)); dTf2(4,4,:)=diff(Tf2(4,4,:)) / (Test_Cycle/length(T)); dTf2(5,1,:)=diff(Tf2(5,1,:)) / (Test_Cycle/length(T)); dTf2(5,2,:)=diff(Tf2(5,2,:)) / (Test_Cycle/length(T)); dTf2(5,3,:)=diff(Tf2(5,3,:)) / (Test_Cycle/length(T)); dTf2(5,4,:)=diff(Tf2(5,4,:)) / (Test_Cycle/length(T)); dTf2(6,1,:)=diff(Tf2(6,1,:)) / (Test_Cycle/length(T)); dTf2(6,2,:)=diff(Tf2(6,2,:)) / (Test_Cycle/length(T)); dTf2(6,3,:)=diff(Tf2(6,3,:)) / (Test_Cycle/length(T)); dTf2(6,4,:)=diff(Tf2(6,4,:)) / (Test_Cycle/length(T)); dTf2(:,:,length(T))=dTf2(:,:,length(T)-1);
dTn1= zeros(6,4,length(T)-1); dTn1(1,1,:)=diff(Tn1(1,1,:)) / (Test_Cycle/length(T)); dTn1(1,2,:)=diff(Tn1(1,2,:)) / (Test_Cycle/length(T)); dTn1(1,3,:)=diff(Tn1(1,3,:)) / (Test_Cycle/length(T)); dTn1(1,4,:)=diff(Tn1(1,4,:)) / (Test_Cycle/length(T)); dTn1(2,1,:)=diff(Tn1(2,1,:)) / (Test_Cycle/length(T)); dTn1(2,2,:)=diff(Tn1(2,2,:)) / (Test_Cycle/length(T)); dTn1(2,3,:)=diff(Tn1(2,3,:)) / (Test_Cycle/length(T)); dTn1(2,4,:)=diff(Tn1(2,4,:)) / (Test_Cycle/length(T)); dTn1(3,1,:)=diff(Tn1(3,1,:)) / (Test_Cycle/length(T)); dTn1(3,2,:)=diff(Tn1(3,2,:)) / (Test_Cycle/length(T)); dTn1(3,3,:)=diff(Tn1(3,3,:)) / (Test_Cycle/length(T)); dTn1(3,4,:)=diff(Tn1(3,4,:)) / (Test_Cycle/length(T)); dTn1(4,1,:)=diff(Tn1(4,1,:)) / (Test_Cycle/length(T)); dTn1(4,2,:)=diff(Tn1(4,2,:)) / (Test_Cycle/length(T)); dTn1(4,3,:)=diff(Tn1(4,3,:)) / (Test_Cycle/length(T)); dTn1(4,4,:)=diff(Tn1(4,4,:)) / (Test_Cycle/length(T)); dTn1(5,1,:)=diff(Tn1(5,1,:)) / (Test_Cycle/length(T)); dTn1(5,2,:)=diff(Tn1(5,2,:)) / (Test_Cycle/length(T)); dTn1(5,3,:)=diff(Tn1(5,3,:)) / (Test_Cycle/length(T)); dTn1(5,4,:)=diff(Tn1(5,4,:)) / (Test_Cycle/length(T)); dTn1(6,1,:)=diff(Tn1(6,1,:)) / (Test_Cycle/length(T)); dTn1(6,2,:)=diff(Tn1(6,2,:)) / (Test_Cycle/length(T)); dTn1(6,3,:)=diff(Tn1(6,3,:)) / (Test_Cycle/length(T)); dTn1(6,4,:)=diff(Tn1(6,4,:)) / (Test_Cycle/length(T)); dTn1(:,:,length(T))=dTn1(:,:,length(T)-1);
dTn2= zeros(6,4,length(T)-1); dTn2(1,1,:)=diff(Tn2(1,1,:)) / (Test_Cycle/length(T)); dTn2(1,2,:)=diff(Tn2(1,2,:)) / (Test_Cycle/length(T)); dTn2(1,3,:)=diff(Tn2(1,3,:)) / (Test_Cycle/length(T)); dTn2(1,4,:)=diff(Tn2(1,4,:)) / (Test_Cycle/length(T)); dTn2(2,1,:)=diff(Tn2(2,1,:)) / (Test_Cycle/length(T)); dTn2(2,2,:)=diff(Tn2(2,2,:)) / (Test_Cycle/length(T)); dTn2(2,3,:)=diff(Tn2(2,3,:)) / (Test_Cycle/length(T)); dTn2(2,4,:)=diff(Tn2(2,4,:)) / (Test_Cycle/length(T)); dTn2(3,1,:)=diff(Tn2(3,1,:)) / (Test_Cycle/length(T)); dTn2(3,2,:)=diff(Tn2(3,2,:)) / (Test_Cycle/length(T)); dTn2(3,3,:)=diff(Tn2(3,3,:)) / (Test_Cycle/length(T)); dTn2(3,4,:)=diff(Tn2(3,4,:)) / (Test_Cycle/length(T)); dTn2(4,1,:)=diff(Tn2(4,1,:)) / (Test_Cycle/length(T)); dTn2(4,2,:)=diff(Tn2(4,2,:)) / (Test_Cycle/length(T)); dTn2(4,3,:)=diff(Tn2(4,3,:)) / (Test_Cycle/length(T)); dTn2(4,4,:)=diff(Tn2(4,4,:)) / (Test_Cycle/length(T)); dTn2(5,1,:)=diff(Tn2(5,1,:)) / (Test_Cycle/length(T)); dTn2(5,2,:)=diff(Tn2(5,2,:)) / (Test_Cycle/length(T)); dTn2(5,3,:)=diff(Tn2(5,3,:)) / (Test_Cycle/length(T)); dTn2(5,4,:)=diff(Tn2(5,4,:)) / (Test_Cycle/length(T)); dTn2(6,1,:)=diff(Tn2(6,1,:)) / (Test_Cycle/length(T)); dTn2(6,2,:)=diff(Tn2(6,2,:)) / (Test_Cycle/length(T)); dTn2(6,3,:)=diff(Tn2(6,3,:)) / (Test_Cycle/length(T)); dTn2(6,4,:)=diff(Tn2(6,4,:)) / (Test_Cycle/length(T)); dTn2(:,:,length(T))=dTn2(:,:,length(T)-1);
dTp = zeros(6,4,length(T)-1); dTp(1,1,:)= diff(Tp(1,1,:))  / (Test_Cycle/length(T)); dTp(1,2,:)= diff(Tp(1,2,:))  / (Test_Cycle/length(T)); dTp(1,3,:)= diff(Tp(1,3,:))  / (Test_Cycle/length(T)); dTp(1,4,:)= diff(Tp(1,4,:))  / (Test_Cycle/length(T)); dTp(2,1,:)= diff(Tp(2,1,:))  / (Test_Cycle/length(T)); dTp(2,2,:)= diff(Tp(2,2,:))  / (Test_Cycle/length(T)); dTp(2,3,:)= diff(Tp(2,3,:))  / (Test_Cycle/length(T)); dTp(2,4,:)= diff(Tp(2,4,:))  / (Test_Cycle/length(T)); dTp(3,1,:)= diff(Tp(3,1,:))  / (Test_Cycle/length(T)); dTp(3,2,:)= diff(Tp(3,2,:))  / (Test_Cycle/length(T)); dTp(3,3,:)= diff(Tp(3,3,:))  / (Test_Cycle/length(T)); dTp(3,4,:)= diff(Tp(3,4,:))  / (Test_Cycle/length(T)); dTp(4,1,:)= diff(Tp(4,1,:))  / (Test_Cycle/length(T)); dTp(4,2,:)= diff(Tp(4,2,:))  / (Test_Cycle/length(T)); dTp(4,3,:)= diff(Tp(4,3,:))  / (Test_Cycle/length(T)); dTp(4,4,:)= diff(Tp(4,4,:))  / (Test_Cycle/length(T)); dTp(5,1,:)= diff(Tp(5,1,:))  / (Test_Cycle/length(T)); dTp(5,2,:)= diff(Tp(5,2,:))  / (Test_Cycle/length(T)); dTp(5,3,:)= diff(Tp(5,3,:))  / (Test_Cycle/length(T)); dTp(5,4,:)= diff(Tp(5,4,:))  / (Test_Cycle/length(T)); dTp(6,1,:)= diff(Tp(6,1,:))  / (Test_Cycle/length(T)); dTp(6,2,:)= diff(Tp(6,2,:))  / (Test_Cycle/length(T)); dTp(6,3,:)= diff(Tp(6,3,:))  / (Test_Cycle/length(T)); dTp(6,4,:)= diff(Tp(6,4,:))  / (Test_Cycle/length(T)); dTp(:,:,length(T))= dTp(:,:,length(T)-1);
dTcbl = zeros(6,4,length(T)-1); dTcbl(1,1,:)= diff(Tcbl(1,1,:))  / (Test_Cycle/length(T)); dTcbl(1,2,:)= diff(Tcbl(1,2,:))  / (Test_Cycle/length(T)); dTcbl(1,3,:)= diff(Tcbl(1,3,:))  / (Test_Cycle/length(T)); dTcbl(1,4,:)= diff(Tcbl(1,4,:))  / (Test_Cycle/length(T)); dTcbl(2,1,:)= diff(Tcbl(2,1,:))  / (Test_Cycle/length(T)); dTcbl(2,2,:)= diff(Tcbl(2,2,:))  / (Test_Cycle/length(T)); dTcbl(2,3,:)= diff(Tcbl(2,3,:))  / (Test_Cycle/length(T)); dTcbl(2,4,:)= diff(Tcbl(2,4,:))  / (Test_Cycle/length(T)); dTcbl(3,1,:)= diff(Tcbl(3,1,:))  / (Test_Cycle/length(T)); dTcbl(3,2,:)= diff(Tcbl(3,2,:))  / (Test_Cycle/length(T)); dTcbl(3,3,:)= diff(Tcbl(3,3,:))  / (Test_Cycle/length(T)); dTcbl(3,4,:)= diff(Tcbl(3,4,:))  / (Test_Cycle/length(T)); dTcbl(4,1,:)= diff(Tcbl(4,1,:))  / (Test_Cycle/length(T)); dTcbl(4,2,:)= diff(Tcbl(4,2,:))  / (Test_Cycle/length(T)); dTcbl(4,3,:)= diff(Tcbl(4,3,:))  / (Test_Cycle/length(T)); dTcbl(4,4,:)= diff(Tcbl(4,4,:))  / (Test_Cycle/length(T)); dTcbl(5,1,:)= diff(Tcbl(5,1,:))  / (Test_Cycle/length(T)); dTcbl(5,2,:)= diff(Tcbl(5,2,:))  / (Test_Cycle/length(T)); dTcbl(5,3,:)= diff(Tcbl(5,3,:))  / (Test_Cycle/length(T)); dTcbl(5,4,:)= diff(Tcbl(5,4,:))  / (Test_Cycle/length(T)); dTcbl(6,1,:)= diff(Tcbl(6,1,:))  / (Test_Cycle/length(T)); dTcbl(6,2,:)= diff(Tcbl(6,2,:))  / (Test_Cycle/length(T)); dTcbl(6,3,:)= diff(Tcbl(6,3,:))  / (Test_Cycle/length(T)); dTcbl(6,4,:)= diff(Tcbl(6,4,:))  / (Test_Cycle/length(T)); dTcbl(:,:,length(T))= dTcbl(:,:,length(T)-1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%% Angular-Velocity & Inertia Dyad %%%%%%%%%%%%%%%
for i=1:length(T)
WsL1(:,:,i)=[ [0 0 0 ; 0 0 -Psi_dot(1,i) ; 0 Psi_dot(1,i) 0] zeros(3); zeros(3) zeros(3)]; 
WsR1(:,:,i)=[ [0 0 0 ; 0 0 -Psi_dot(2,i) ; 0 Psi_dot(2,i) 0] zeros(3); zeros(3) zeros(3)]; 
WsL2(:,:,i)=[ [0 0 Psi_dot(3,i) ; 0 0 0 ; -Psi_dot(3,i) 0 0] zeros(3); zeros(3) zeros(3)]; 
WsR2(:,:,i)=[ [0 0 Psi_dot(4,i) ; 0 0 0 ; -Psi_dot(4,i) 0 0] zeros(3); zeros(3) zeros(3)]; 
Wa1(:,:,i)=[ [0 0 0 ; 0 0 -Theta1_dot(i) ; 0 Theta1_dot(i) 0] zeros(3); zeros(3) zeros(3)];
Wa2(:,:,i)=[ [0 0 Theta2_dot(i) ; 0 0 0 ; -Theta2_dot(i) 0 0] zeros(3); zeros(3) zeros(3)];
Wf1(:,:,i)=[ [0 0 0 ; 0 0 -Lambda1_dot(i) ; 0 Lambda1_dot(i) 0] zeros(3); zeros(3) zeros(3)];
Wf2(:,:,i)=[ [0 0 Lambda2_dot(i) ; 0 0 0 ; -Lambda2_dot(i) 0 0] zeros(3); zeros(3) zeros(3)];
Wn1(:,:,i)=[ zeros(3) zeros(3); zeros(3) zeros(3)];
Wn2(:,:,i)=[ zeros(3) zeros(3); zeros(3) zeros(3)];
Wp (:,:,i)=[ [0 -Phi_dot(i) 0 ; Phi_dot(i) 0 0 ; 0 0 0] zeros(3); zeros(3) zeros(3)];

IsL1(:,:,i)=TsL1(:,:,i).' * Ms1 * TsL1(:,:,i);
IsR1(:,:,i)=TsR1(:,:,i).' * Ms1 * TsR1(:,:,i);
IsL2(:,:,i)=TsL2(:,:,i).' * Ms2 * TsL2(:,:,i);
IsR2(:,:,i)=TsR2(:,:,i).' * Ms2 * TsR2(:,:,i);
Ia1(:,:,i)=Ta1(:,:,i).' * Ma1 * Ta1(:,:,i);
Ia2(:,:,i)=Ta2(:,:,i).' * Ma2 * Ta2(:,:,i);
If1(:,:,i)=Tf1(:,:,i).' * Mf1 * Tf1(:,:,i);
If2(:,:,i)=Tf2(:,:,i).' * Mf2 * Tf2(:,:,i);
In1(:,:,i)=Tn1(:,:,i).' * Mn1 * Tn1(:,:,i);
In2(:,:,i)=Tn2(:,:,i).' * Mn2 * Tn2(:,:,i);
Ip(:,:,i) = Tp(:,:,i).' * Mp * Tp(:,:,i);

CsL1(:,:,i)=TsL1(:,:,i).' * WsL1(:,:,i) * Ms1 * TsL1(:,:,i);            %%%% dTsL1=0 %%%%
CsR1(:,:,i)=TsR1(:,:,i).' * WsR1(:,:,i) * Ms1 * TsR1(:,:,i);            %%%% dTsR1=0 %%%%
CsL2(:,:,i)=TsL2(:,:,i).' * WsL2(:,:,i) * Ms2 * TsL2(:,:,i);            %%%% dTsL2=0 %%%%
CsR2(:,:,i)=TsR2(:,:,i).' * WsR2(:,:,i) * Ms2 * TsR2(:,:,i);            %%%% dTsR2=0 %%%%
Ca1(:,:,i)=Ta1(:,:,i).' * Ma1 * dTa1(:,:,i) + Ta1(:,:,i).' * Wa1(:,:,i) * Ma1 * Ta1(:,:,i);
Ca2(:,:,i)=Ta2(:,:,i).' * Ma2 * dTa2(:,:,i) + Ta2(:,:,i).' * Wa2(:,:,i) * Ma2 * Ta2(:,:,i);
Cf1(:,:,i)=Tf1(:,:,i).' * Mf1 * dTf1(:,:,i) + Tf1(:,:,i).' * Wf1(:,:,i) * Mf1 * Tf1(:,:,i);
Cf2(:,:,i)=Tf2(:,:,i).' * Mf2 * dTf2(:,:,i) + Tf2(:,:,i).' * Wf2(:,:,i) * Mf2 * Tf2(:,:,i);
Cn1(:,:,i)=Tn1(:,:,i).' * Mn1 * dTn1(:,:,i) + Tn1(:,:,i).' * Wn1(:,:,i) * Mn1 * Tn1(:,:,i);
Cn2(:,:,i)=Tn2(:,:,i).' * Mn2 * dTn2(:,:,i) + Tn2(:,:,i).' * Wn2(:,:,i) * Mn2 * Tn2(:,:,i);
Cp(:,:,i) = Tp(:,:,i).' * Mp  * dTp(:,:,i)  +  Tp(:,:,i).' *  Wp(:,:,i) * Mp *  Tp(:,:,i);
end
Itot=IsL1+IsR1+IsL2+IsR2+Ia1+Ia2+If1+If2+In1+In2+Ip;
Ctot=CsL1+CsR1+CsL2+CsR2+Ca1+Ca2+Cf1+Cf2+Cn1+Cn2+Cp;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%% Torque NOC %%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:length(T)
    TorqueS(:,i) = -1*( Ta1(:,:,i).' * wGa1 + Ta2(:,:,i).' * wGa2 + Tf1(:,:,i).' * wGf1 + Tf2(:,:,i).' * wGf2 + Tn1(:,:,i).' * wGn1 + Tn2(:,:,i).' * wGn2 + Tp(:,:,i).' * wGp);
    TorqueD(:,i) =  Itot(:,:,i) * Psi_dotdot(:,i) + Ctot(:,:,i) * Psi_dot(:,i);
    Torque(:,i) = TorqueS(:,i) + TorqueD(:,i); 
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%% WORK & ENERGY %%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:length(T)
    
    Ks = 0.5*Is*(Psi_dot(:,i).' * Psi_dot(:,i));
    Kp = 0.5*Ipep*(Phi_dot(i)^2) + 0.5*mp*(X_dot(i)^2 + Y_dot(i)^2 + Z_dot(i)^2);
    Kn1 = 0.5*mn*(X_dot(i)^2 + Y_dot(i)^2 + (r^2)*((Theta1_dot(i)*cos(Theta1(i))+Lambda1_dot(i)*cos(Lambda1(i)))^2));
    Kn2 = 0.5*mn*(X_dot(i)^2 + Y_dot(i)^2 + (r^2)*((Theta2_dot(i)*cos(Theta2(i))+Lambda2_dot(i)*cos(Lambda2(i)))^2));
    Ka1 = 0.5*Ia*(Theta1_dot(i)^2) + 0.5*ma*(X_dot(i)^2 + (rc^2)*(Theta1_dot(i)^2));
    Ka2 = 0.5*Ia*(Theta2_dot(i)^2) + 0.5*ma*(Y_dot(i)^2 + (rc^2)*(Theta2_dot(i)^2));
    Kf1 = 0.5*If*(Lambda1_dot(i)^2) + 0.5*mf*(X_dot(i)^2 + (r*Theta1_dot(i)*sin(Theta1(i))+lc*Lambda1_dot(i)*sin(Lambda1(i)))^2 + (r*Theta1_dot(i)*cos(Theta1(i))+lc*Lambda1_dot(i)*cos(Lambda1(i)))^2 );
    Kf2 = 0.5*If*(Lambda2_dot(i)^2) + 0.5*mf*(Y_dot(i)^2 + (r*Theta2_dot(i)*sin(Theta2(i))+lc*Lambda2_dot(i)*sin(Lambda2(i)))^2 + (r*Theta2_dot(i)*cos(Theta2(i))+lc*Lambda2_dot(i)*cos(Lambda2(i)))^2 );
    
    Up = mp*g*(Z(i)-Z(1));
    Un = 2*mn*g*(Z(i)-Z(1));
    Ua1 = ma*g*rc*(sin(Theta1(i))-sin(Theta1(1)));
    Ua2 = ma*g*rc*(-sin(Theta2(i))+sin(Theta2(1)));
    Uf1 = mf*g*(r*sin(Theta1(i)) + lc*sin(Lambda1(i)) - r*sin(Theta1(1)) - lc*sin(Lambda1(1)));
    Uf2 = mf*g*(-r*sin(Theta2(i)) - lc*sin(Lambda2(i)) + r*sin(Theta2(1)) + lc*sin(Lambda2(1)));
    
    U(i) = Up+Un+Ua1+Ua2+Uf1+Uf2;
    K(i) = Ks+Kp+Kn1+Kn2+Ka1+Ka2+Kf1+Kf2;
       
    if i==1
        W(i) = Torque(:,i).' * (Psi(:,i+1)-Psi(:,i));
    elseif i==length(T)
        W(i)=0;
    else
        W(i) = W(i-1) + 0.5*(Torque(:,i).'+Torque(:,i-1).') * (Psi(:,i+1)-Psi(:,i));  % + Fcbl(i) * (Z(i+1)-Z(i));
    end  
end

VelABS = sqrt(X_dot.^2 + Y_dot.^2 + Z_dot.^2); 
AccABS  =  diff(VelABS) / (Test_Cycle/length(T)); AccABS(length(AccABS)+1)=AccABS(length(AccABS));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%   Plotting %%%%%%%%%%%%%%%%%%%%%

figure(12)          %%%%%% Total Velocity %%%%%%%
plot(T,VelABS, 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Velocity (m/s)','FontSize',14,'FontWeight','bold');
title('EE Total Velocity','FontSize',16,'FontWeight','bold')
grid on;




figure(10)          %%%%%% WORK & ENERGY %%%%%%%
plot(T,U+K , T,W,'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Energy (j)','FontSize',14,'FontWeight','bold');
title('Work and Energy','FontSize',16,'FontWeight','bold')
legend({'Energy','Work'},'FontSize',14,'FontWeight','bold');
grid on;

hold on;
figure(10)          %%%%%% Motor Torques %%%%%%%
plot(T,Torque(1,:) , T,Torque(2,:),'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',22,'interpreter','latex');
ylabel('Torque (N.m)','FontSize',22,'interpreter','latex');
title('Motor torques','FontSize',22,'FontWeight','normal','interpreter','latex')
legend({'$\tau_{1L}$','$\tau_{1R}$'},'interpreter','latex','FontSize',20,'FontWeight','bold');
grid on;


figure(9)          %%%%%% Motor Torques %%%%%%%
set(figure(9), 'Position', [0 600 500 500])
subplot(2,1,1)
hold on;
plot(T,Torque(1,:) , T,Torque(2,:),'-red' , 'LineWidth',2);
plot(T,TorqueS(1,:),'--blue', T,TorqueS(2,:),'--red' , 'LineWidth',1);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Torque (N.m)','FontSize',14,'FontWeight','bold');
hold off;
title('Motor Torques','FontSize',16,'FontWeight','bold')
legend({'$\tau_{1L}$','$\tau_{1R}$'},'interpreter','latex','FontSize',14,'FontWeight','bold');
grid on;
subplot(2,1,2)
hold on;
plot(T,Torque(3,:)*(-1), T,Torque(4,:)*(-1),'-red' , 'LineWidth',2);
plot(T,TorqueS(3,:)*(-1),'--blue', T,TorqueS(4,:)*(-1),'--red' , 'LineWidth',1);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Torque (N.m)','FontSize',14,'FontWeight','bold');
hold off;
legend({'$-\tau_{2L}$','$-\tau_{2R}$'},'interpreter','latex','FontSize',14,'FontWeight','bold');
grid on;

figure(99)          %%%%%% Motor Torques (max abs) %%%%%%%
set(figure(99), 'Position', [0 600 500 500])
hold on;
plot(T,max(abs(Torque)), 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Torque (N.m)','FontSize',14,'FontWeight','bold');
hold off;
title('Motor Torques (max abs)','FontSize',16,'FontWeight','bold')
grid on;

figure(8)          %%%%%% Motor Static Torques %%%%%%%
set(figure(8), 'Position', [0 50 500 500])
subplot(2,1,1)
plot(T,TorqueS(1,:), T,TorqueS(2,:),'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Torque (N.m)','FontSize',14,'FontWeight','bold');
title('Motor Static Torques','FontSize',16,'FontWeight','bold');
legend({'$\tau_{1L}$','$\tau_{1R}$'},'interpreter','latex','FontSize',14,'FontWeight','bold');
grid on;
subplot(2,1,2)
plot(T,TorqueS(3,:)*(-1), T,TorqueS(4,:)*(-1),'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Torque (N.m)','FontSize',14,'FontWeight','bold');
legend({'$-\tau_{2L}$','$-\tau_{2R}$'},'interpreter','latex','FontSize',14,'FontWeight','bold');
grid on;


figure(7)          %%%%%% Joint-Accelerations %%%%%%%
%subplot(2,1,1)
plot(T,Psi_dotdot(1,:) , T,Psi_dotdot(2,:),'--red' ,T,Psi_dotdot(3,:) ,':', T,Psi_dotdot(4,:),'-.', 'LineWidth',2);
xlabel('Time (s)','FontSize',14);
ylabel('Acceleration (rad/s^2)','FontSize',14);
% title('Joint-Accelerations','FontSize',16,'FontWeight','bold')
legend({'$\ddot{\psi}_{1L}$','$\ddot{\psi}_{1R}$','$\ddot{\psi}_{2L}$','$\ddot{\psi}_{2R}$'},'interpreter','latex','FontSize',14,'FontWeight','bold');
grid on;
%subplot(2,1,2)
%plot(T,Psi_dotdot(3,:) , T,Psi_dotdot(4,:),'-red' , 'LineWidth',2);
%grid on;
 

figure(6)          %%%%%% Joint-Rates %%%%%%%
subplot(2,1,1)
plot(T,Psi_dot(1,:) , T,Psi_dot(2,:) ,'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Angular velocity (rad/s)','FontSize',14,'FontWeight','bold');
title('Joint-Rates','FontSize',16,'FontWeight','bold')
legend({'$\dot{\psi}_{1L}$','$\dot{\psi}_{1R}$'},'interpreter','latex','FontSize',14,'FontWeight','bold');
grid on;
subplot(2,1,2)
plot(T,Psi_dot(3,:) , T,Psi_dot(4,:),'-red' , 'LineWidth',2);
grid on;


figure(5)          %%%%%% Link-Angles %%%%%%%
subplot(2,1,1)
plot(T,Theta1*180/pi , T,(2*pi-Theta2)*180/pi ,'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Angular position (deg)','FontSize',14,'FontWeight','bold');
title('Arm Angles','FontSize',16,'FontWeight','bold')
legend({'\theta_{1}','-\theta_{2}'},'FontSize',14,'FontWeight','bold');
grid on;
subplot(2,1,2)
plot(T,(2*pi-Lambda1)*180/pi , T,Lambda2*180/pi ,'-red' , 'LineWidth',2);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Angular position (deg)','FontSize',14,'FontWeight','bold');
title('Forearm Angles','FontSize',16,'FontWeight','bold');
legend({'-\lambda_{1}','\lambda_{2}'},'FontSize',14,'FontWeight','bold');
grid on;


figure(4)          %%%%%% Joint-Variables %%%%%%%
plot(T,Psi(1,:) , T,Psi(2,:), T,Psi(3,:) , T,Psi(4,:) , 'LineWidth',2.5);
xlabel('Time (sec)','FontSize',14,'FontWeight','bold');
ylabel('Angular Displacement (rad)','FontSize',14,'FontWeight','bold');
title('Joint-Variables','FontSize',16,'FontWeight','bold')
legend({'\psi_{1L}','\psi_{1R}','\psi_{2L}','\psi_{2R}'},'FontSize',14,'FontWeight','bold');
grid on;


figure(2)       %%%%%% PMC Posture %%%%%%%
hold on,
quiver3(0,0,0,  0,0,400,  1,'blue', 'LineWidth',10)
quiver3(0,0,0,  0,0,-275,  1,'blue', 'LineWidth',10)
quiver3(0,0,250,  0,500,0,  1,'blue', 'LineWidth',10);
quiver3(0,0,-250,  500,0,0,  1,'blue', 'LineWidth',10);

for i=1:(length(T)/4):(length(T)/2+1)

    quiver3(0,Y(i)*1000,250,  r*1000*cos(2*pi-Theta2(i)),0,r*1000*sin(2*pi-Theta2(i)),  1,'red', 'LineWidth',5);
    quiver3(X(i)*1000,0,-250,  0,r*1000*cos(Theta1(i)),r*1000*sin(Theta1(i)),  1,'red', 'LineWidth',5);
    quiver3(r*1000*cos(2*pi-Theta2(i)),Y(i)*1000,250+r*1000*sin(2*pi-Theta2(i)),  r*1000*cos(2*pi-Lambda2(i)),0,r*1000*sin(2*pi-Lambda2(i)),  1,'g', 'LineWidth',5);
    quiver3(X(i)*1000,r*1000*cos(Theta1(i)),r*1000*sin(Theta1(i))-250,  0,r*1000*cos(Lambda1(i)),r*1000*sin(Lambda1(i)),  1,'g', 'LineWidth',5);
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(Theta1(i))+r*1000*sin(Lambda1(i))-250,  0,0,240,  1,'k', 'LineWidth',10);
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(2*pi-Theta2(i))+r*1000*sin(2*pi-Lambda2(i))+250,  0,0,-240,  1,'k', 'LineWidth',10);
    
    %Nut
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(Theta1(i))+r*1000*sin(Lambda1(i))-250,  0,0,75,  1,'k', 'LineWidth',20);
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(2*pi-Theta2(i))+r*1000*sin(2*pi-Lambda2(i))+250,  0,0,-75,  1,'k', 'LineWidth',20);    
end    
title('PMC Posture','FontSize',16,'FontWeight','bold');
xlabel('X (mm)','FontSize',12,'FontWeight','bold');
ylabel('Y (mm)','FontSize',12,'FontWeight','bold');
zlabel('Z (mm)','FontSize',12,'FontWeight','bold');
axis('tight','equal')
%axis off;
view(45,20)
grid on;
hold off;


figure(1)       %%%%%% PMC Animation %%%%%%%
set(figure(1), 'Position', [1000 100 800 1000])
for i=1:100:length(T)
    clf;
    hold on,
    quiver3(0,0,0,  0,0,400,  1,'blue', 'LineWidth',10);
    quiver3(0,0,0,  0,0,-275,  1,'blue', 'LineWidth',10);
    quiver3(0,0,250,  0,500,0,  1,'blue', 'LineWidth',10);
    quiver3(0,0,-250,  500,0,0,  1,'blue', 'LineWidth',10);
    
    quiver3(0,Y(i)*1000,250,  r*1000*cos(2*pi-Theta2(i)),0,r*1000*sin(2*pi-Theta2(i)),  1,'red', 'LineWidth',5);
    quiver3(X(i)*1000,0,-250,  0,r*1000*cos(Theta1(i)),r*1000*sin(Theta1(i)),  1,'red', 'LineWidth',5);
    quiver3(r*1000*cos(2*pi-Theta2(i)),Y(i)*1000,250+r*1000*sin(2*pi-Theta2(i)),  r*1000*cos(2*pi-Lambda2(i)),0,r*1000*sin(2*pi-Lambda2(i)),  1,'g', 'LineWidth',5);
    quiver3(X(i)*1000,r*1000*cos(Theta1(i)),r*1000*sin(Theta1(i))-250,  0,r*1000*cos(Lambda1(i)),r*1000*sin(Lambda1(i)),  1,'g', 'LineWidth',5);
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(Theta1(i))+r*1000*sin(Lambda1(i))-250,  0,0,240+((Phi(i)+pi/2)/pi)*(pp*500),  1,'k', 'LineWidth',10);
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(2*pi-Theta2(i))+r*1000*sin(2*pi-Lambda2(i))+250,  0,0,-240-((Phi(i)+pi/2)/pi)*(pp*500),  1,'k', 'LineWidth',10);
    
    %Nut
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(Theta1(i))+r*1000*sin(Lambda1(i))-250,  0,0,75,  1,'k', 'LineWidth',30);
    quiver3(X(i)*1000,Y(i)*1000,r*1000*sin(2*pi-Theta2(i))+r*1000*sin(2*pi-Lambda2(i))+250,  0,0,-75,  1,'k', 'LineWidth',30);
    
    %gripper
    rg=75;
    quiver3(X(i)*1000,Y(i)*1000,r*500*(sin(Theta1(i))+sin(Lambda1(i))+sin(2*pi-Theta2(i))+sin(2*pi-Lambda2(i))),   rg*cos(Phi(i)), rg*sin(Phi(i)),0,  1,'k', 'LineWidth',5);
    quiver3(X(i)*1000,Y(i)*1000,r*500*(sin(Theta1(i))+sin(Lambda1(i))+sin(2*pi-Theta2(i))+sin(2*pi-Lambda2(i))),  -rg*cos(Phi(i)),-rg*sin(Phi(i)),0,  1,'k', 'LineWidth',5);
    quiver3(X(i)*1000,Y(i)*1000,r*500*(sin(Theta1(i))+sin(Lambda1(i))+sin(2*pi-Theta2(i))+sin(2*pi-Lambda2(i))),  -rg*sin(Phi(i)), rg*cos(Phi(i)),0,  1,'k', 'LineWidth',5);
    quiver3(X(i)*1000,Y(i)*1000,r*500*(sin(Theta1(i))+sin(Lambda1(i))+sin(2*pi-Theta2(i))+sin(2*pi-Lambda2(i))),   rg*sin(Phi(i)),-rg*cos(Phi(i)),0,  1,'k', 'LineWidth',5);
    
    quiver3(X(i)*1000,0,-250,  50,0,0, 1,'red', 'LineWidth',25);
    quiver3(X(i)*1000,0,-250, -50,0,0, 1,'red', 'LineWidth',25);
    quiver3(0,Y(i)*1000, 250,  0,50,0, 1,'red', 'LineWidth',25);
    quiver3(0,Y(i)*1000, 250, 0,-50,0, 1,'red', 'LineWidth',25);
    
    title('PMC Posture','FontSize',16,'FontWeight','bold');
    xlabel('X (mm)','FontSize',12,'FontWeight','bold');
    ylabel('Y (mm)','FontSize',12,'FontWeight','bold');
    zlabel('Z (mm)','FontSize',12,'FontWeight','bold');
    axis('equal');
    axis([0 500 0 500 -600+Z0 600]);    
    view(45,20)
    grid on;
    hold off;
    pause(0.1)
end