clc;clear;
m1=0.5;m2=0.4;m3=0.3;pi=3.14;
a1=0.3;a2=0.25;a3=0.22;i=1;
for t=0:0.1:10;
th1=pi*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
dth1=pi*(1-cos(2*pi*t/10))/10;
ddth1=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
th2=pi*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
dth2=pi*(1-cos(2*pi*t/10))/10;
ddth2=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
th3=pi*(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;
% Coordinate frames
i1=[1;0;0];
j1=[0;1;0];
e1=[0;0;1];e2=[0;0;1];e3=[0;0;1];e5=[0;0;1];
Izz1=1/3*m1*a1^2;Izz2=1/3*m2*a2^2;Izz3=1/3*m3*a3^2;
% Rotation matrices
Q1 =[cos(th1) -sin(th1) 0
sin(th1) cos(th1) 0
0 0 1];
Q2 =[cos(th2) -sin(th2) 0
sin(th2) cos(th2) 0
0 0 1];
Q3 =[cos(th3) -sin(th3) 0
sin(th3) cos(th3) 0
0 0 1];
a12=[a1;0;0];a23=[a2;0;0];a34=[a3;0;0]; %aij (i-link , j-frame)
a11=Q1*a12;
a21=Q1*Q2*a23;
a31=Q1*Q2*Q3*a34;
C31=-(a21+a31);
% 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)];
I3=[Izz3*eye(3,3) zeros(3,3)
zeros(3,3) m3*eye(3,3)];
Ms1 =[ I1 zeros(6,6) zeros(6,6)
zeros(6,6) I2 zeros(6,6)
zeros(6,6) zeros(6,6) I3 ];
% pi, Joint-rate propogation matrices (motion of ith joint)
p1=[e1
cross(e1,a11)];
p2=[e2
cross(e2,a21)];
p3=[e3
cross(e3,a31)];
% Bij, Twist propogation matrices (propogates the twist of #j to #i)
scewC21=[0 a21(3) -a21(2)
-a21(3) 0 a21(1)
a21(2) -a21(1) 0 ];
scewC32=[0 a31(3) -a31(2)
-a31(3) 0 a31(1)
a31(2) -a31(1) 0 ];
scewC31=[0 -C31(3) C31(2)
C31(3) 0 -C31(1)
-C31(2) C31(1) 0 ];
B21=[eye(3,3) zeros(3,3)
scewC21 eye(3,3) ];
B32=[eye(3,3) zeros(3,3)
scewC32 eye(3,3) ];
B31=[eye(3,3) zeros(3,3)
scewC31 eye(3,3) ];
% De-NOC matrices
Nl =[eye(6,6) zeros(6,6) zeros(6,6)
B21 eye(6,6) zeros(6,6)
B31 B32 eye(6,6) ];
Nd =[p1 zeros(6,1) zeros(6,1)
zeros(6,1) p2 zeros(6,1)
zeros(6,1) zeros(6,1) p3 ];
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=-(dth1+dth2)*cross(e2,a21);
dC32=-(dth1+dth2+dth3)*cross(e3,a31);
dC31=-((dth1+dth2)*cross(e2,a21)+(dth1+dth2+dth3)*cross(e3,a31));
scewdC21=[0 -dC21(3) dC21(2)
dC21(3) 0 -dC21(1)
-dC21(2) dC21(1) 0 ];
scewdC32=[0 -dC32(3) dC32(2)
dC32(3) 0 -dC32(1)
-dC32(2) dC32(1) 0 ];
scewdC31=[0 -dC31(3) dC31(2)
dC31(3) 0 -dC31(1)
-dC31(2) dC31(1) 0 ];
dB21 =[zeros(3,3) zeros(3,3)
scewdC21 zeros(3,3)];
dB31 =[zeros(3,3) zeros(3,3)
scewdC31 zeros(3,3)];
dB32 =[zeros(3,3) zeros(3,3)
scewdC32 zeros(3,3)];
dNl=[zeros(6,6) zeros(6,6) zeros(6,6)
dB21 zeros(6,6) zeros(6,6)
dB31 dB32 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 =[dth1*scewde1 zeros(3,3) zeros(3,3) 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)
zeros(3,3) zeros(3,3) dth2*scewde1 zeros(3,3) zeros(3,3) zeros(3,3)
zeros(3,3) zeros(3,3) zeros(3,3) zeros(3,3) 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) zeros(3,3) zeros(3,3)];
Met=transpose(Nd)*transpose(Nl)*Ws1*Ms1*(Nl)*Nd
dp1= [zeros(3,1)
dth1*cross(e1,cross(e1,a11))];
dp2= [zeros(3,1)
(dth1+dth2)*cross(e2,cross(e2,a21))];
dp3= [zeros(3,1)
(dth1+dth2+dth3)*cross(e3,cross(e3,a31))];
dNd=[dp1 zeros(6,1) zeros(6,1)
zeros(6,1) dp2 zeros(6,1)
zeros(6,1) zeros(6,1) dp3 ];
Mwt=transpose(Nd)*Mt*dNd
Cs1 =Mlt+Met+Mwt;
dths1 =[dth1
dth2
dth3];
h=Cs1*dths1
% External wrench
g=9.81*[0;-1;0];f3=0;
f3e=f3*[0;-1;0];
% Wes1 =[tue1
% m1*g
% zeros(3,1)
% m2*g
% zeros(3,1)
% Q1*Q2*Q3*f3e+m3*g];
% % Constrained wrench
% %lmd =[lmx;lmy;0];
% scewa31=[0 -a31(3) a31(2)
% a31(3) 0 -a31(1)
% -a31(2) a31(1) 0 ];
% wlms1 =[zeros(3,3)
% zeros(3,3)
% zeros(3,3)
% zeros(3,3)
% scewa31
% eye(3,3) ];
% wlms1(9:18,1:3)
% tlms1=transpose(N)*wlms1;
% tolms1=[tlms1(1,1) tlms1(1,2);tlms1(2,1) tlms1(2,2)];
dths1 =[dth1
dth2
dth3];
We=[m1*cross(g,a11/2);m1*g;m2*cross(g,a21/2);m2*g;m3*cross(g,a31/2);m3*g]
C1=(Is1*[ddth1;ddth2;ddth3]+Cs1*dths1)-transpose(Nd)*transpose(Nl)*We;
to1(i)=C1(1,1);to2(i)=C1(2,1);to3(i)=C1(3,1);
% torque1=[C1(3,1);C1(9,1);C1(15,1)]
i=i+1;
% pinv(transpose(Nd)*transpose(Nl))*
end
figure(2)
t= 0:0.1:10;
plot(t,to1,'r',t,to2,'b',t,to3,'g','Linewidth',2);
title('Joint cons force')
xlabel('Time (sec)');
ylabel('force (N) ');
grid on
figure(3)
t= 0:0.1:10;
plot(t,th1,'r',t,th2,'b',t,th3,'g','Linewidth',2);
title('Joint cons force')
xlabel('Time (sec)');
ylabel('force (N) ');
grid on
