clear;clc;
% Defination
% Link properties
m1=1;m2=1;
a1=1;a2=1;

% Coordinate frames
i1=[1;0;0];
j1=[0;1;0];
e1=[0;0;1];e2=[0;0;1];

Izz1=1/12*m1*a1^2;Izz2=1/12*m2*a2^2;

% trajectory
pi=3.14;
i=0;

for t= 0:.1:10
    
% Trajectories
  th4=pi*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
  dth4=pi*(1-cos(2*pi*t/10))/10;
  ddth4=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
  th3=pi/2*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
  dth3=pi*(1-cos(2*pi*t/10))/10;
  ddth3=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
  % Rotation matrices

% Rotation matrices

Q1 =[cos(th4) -sin(th4)  0
     sin(th4)  cos(th4)  0
        0          0     1];
Q2 =[cos(th3) -sin(th3)  0
     sin(th3)  cos(th3)  0
        0          0     1];

     
% Link length vectors
a12=[a1;0;0];a23=[a2;0;0];               %aij (i-link , j-frame)
a11=Q1*a12;                            
a22=Q1*Q2*a23;

 

% d - distance from origin(first frame of link 'i') to cg
d12=[a1*0.5;0;0];d23=[a2*0.5;0;0];            %dij (i-link , j-frame)
d11=Q1*d12;                            
d22=Q1*Q2*d23;


% r - distance from cg to origin(first frame of link 'i')
r12=[(a1-a1*0.5);0;0];r23=[(a2-a2*0.5);0;0];           %rij (i-link , j-frame)
r11=Q1*d12;                            
r22=Q1*Q2*d23;


C21=-(r11+d22);

% SUBSYSTEM I

% Mass matrices
I1=[Izz1*eye(3,3)      zeros(3,3)             
    zeros(3,3)         m1*eye(3,3)];       
I2=[Izz2*eye(3,3)      zeros(3,3)            
    zeros(3,3)         m2*eye(3,3)];

Ms1 =[   I1        zeros(6,6)
      zeros(6,6)     I2      ];
  
% pi, Joint-rate propogation matrices (motion of ith joint)
p1=[e1
        cross(e1,d11)];
p2=[e2
        cross(e2,d22)];


% Bij, Twist propogation matrices (propogates the twist of #j to #i)
scewC21=[0      -C21(3)  C21(2)
         C21(3)    0    -C21(1)
        -C21(2)  C21(1)    0  ];
  
B21=[eye(3,3)      zeros(3,3)
     scewC21       eye(3,3) ];
 
% De-NOC matrices
Nl =[eye(6,6)         zeros(6,6)
     B21              eye(6,6) ];
Nd =[p1               zeros(6,1)
     zeros(6,1)          p2    ];
N=Nl*Nd;
% Genralised Inertia matrix (GIM)
% M telda
Mt =transpose(Nl)*Ms1*(Nl);
Is1=transpose(Nd)*Mt*Nd

% Matrix of convective inertia terms (MCI)
dC21=-(dth4*cross(e1,r11)+(dth4+dth3)*cross(e1,d22));
scewdC21=[0       -dC21(3)  dC21(2)
         dC21(3)    0      -dC21(1)
         -dC21(2)  dC21(1)     0   ];
dB21 =[zeros(3,3)        zeros(3,3)
               scewdC21           zeros(3,3)];

dNl=[zeros(6,6)         zeros(6,6)
     dB21              zeros(6,6)];
 
Mlt =transpose(Nd)*transpose(Nl)*Ms1*(dNl)*Nd
scewde1=[0       -e1(3)  e1(2)
        e1(3)    0      -e1(1)
        -e1(2)  e1(1)     0   ];
Ws1 =[dth4*scewde1         zeros(3,3)       zeros(3,3)      zeros(3,3)     % genralised matrix of angular velocities
         zeros(3,3)        zeros(3,3)       zeros(3,3)      zeros(3,3)
         zeros(3,3)        zeros(3,3)      dth3*scewde1     zeros(3,3)
         zeros(3,3)        zeros(3,3)       zeros(3,3)      zeros(3,3)];
V=[dth4*scewde1         zeros(3,3)       zeros(3,3)      zeros(3,3)     % genralised matrix of angular velocities
   zeros(3,3)          dth4*scewde1       zeros(3,3)      zeros(3,3)
   zeros(3,3)          zeros(3,3)      dth3*scewde1     zeros(3,3)
   zeros(3,3)          zeros(3,3)       zeros(3,3)      dth3*scewde1];
Met=(transpose(Nd)*transpose(Nl)*Ws1*Ms1*(Nl)*Nd)
Nd =[p1               zeros(6,1)
     zeros(6,1)          p2    ];
dp1= [zeros(3,1)                           
      dth4*cross(e1,cross(e1,d11))]; 
dp2= [zeros(3,1)                           
      (dth4+dth3)*cross(e1,cross(e1,d22))];       
dNd=[dp1                  zeros(6,1)                                              
     zeros(6,1)             dp2     ];
Mwt=transpose(Nd)*Mt*V*Nd

Cs1  =Mlt+Met+Mwt;
dths1 =[dth4
        dth3]; 
h=Cs1*dths1

% External wrench
g=9.81*[0;-1;0];f2=0;tu1=0;
f2e=f2*[0;-1;0];tue1=tu1*[0;0;0];
% Wes1 =[m1*cross(g,a11/2)
%        m1*g
%        m1*cross(g,a22/2)
%        m2*g];
Wes1 =[zeros(3,1)
       m1*g
       zeros(3,1)
       m2*g];
% Constrained wrench
%lmd =[lmx;lmy;0];


dths1 =[dth4
        dth3];
% lamda3=pinv(transpose(Nd)*transpose(Nl))*(Is1*[ddth4;ddth3]+h)-Wes1
torque=(Is1*[ddth4;ddth3]+h)-(transpose(Nd)*transpose(Nl))*Wes1
i=i+1
tor1(i)=torque(1,1);tor2(i)=torque(2,1);
y3(i)=th4;
 y5(i)=th3;
 

end
figure(2)
t= 0:0.1:10;
plot(t,tor1,'r',t,tor2,'b','Linewidth',2);
title('Joint cons force')
xlabel('Time (sec)');
ylabel('force (N) ');
grid on

 figure(4)
plot(t,y3*180/3.14,'g',t,y5*180/3.14,'k','Linewidth',2);
title('Joint Trajectories')
xlabel('time (sec)');
ylabel('Joint angles (deg) ');