%%

L(1).m=3;
L(2).m=269;
L(3).m=60;
L(4).m=30;
L(5).m=20;
L(6).m=1;
%%
L(1).r=[0 0.05 0];
L(2).r=[-0.550 0 0.020];
L(3).r=[0 0 0];
L(4).r=[0 -0.3775 0];
L(5).r=[0 0 0];
L(6).r=[0 -0.3775 0];
%%
L(1).I=[0 ,0.35 ,0];
L(2).I=[.13 .524 .539];
L(3).I=[.066 .086 .0125];
L(4).I=[1.8e-3 1.3e-3 1.8e-3];
L(5).I=[.3e-3 .4e-3 .3e-3];
L(6).I=[.15e-3 .15e-3 .04e-3];
%%
L(1).G=-125.070999;
L(2).G=171.000000;
L(3).G=142.929001;
L(4).G=-60.000000;
L(5).G=66.894699;
L(6).G=-50.000000;
%%
%Motor inertias are supposed
L(1).Jm=8.0700e-07;
L(2).Jm=8.0700e-07;
L(3).Jm=8.0700e-07;
L(4).Jm=8.0700e-07;
L(5).Jm=8.0700e-07;
L(6).Jm=8.0700e-07;%L(i)=Link([theta d a alpha])
L(1)=Link('d',0.495,'a',0.175,'alpha',pi/2);
L(2)=Link('d',0,'a',0.9,'alpha',0,'offset',-pi/2);
L(3)=Link('d',0,'a',0.175,'alpha',-pi/2);
L(4)=Link('d',0.96,'a',0,'alpha',pi/2);
L(5)=Link('d',0,'a',0,'alpha',pi/2,'offset',pi);
L(6)=Link('d',0,'a',0,'alpha',0);
ABB_IRB_4600=SerialLink(L)
ABB_IRB_4600.name='ABB IRB 4600'

%% trajectory generation
path = [7 7 0;7 25 0;7 7 0;15 15 0;23 7 0;23,25,0]

p = mstraj(path, [1 1 3], [], [1 1 1], 1, 1)

Tp = transl(0.05*p);
q = ABB_IRB_4600.ikine(Tp)
ABB_IRB_4600.plot(q,'trail','b*','top','workspace',[-3 3 -3 3 -3 3])
hold on;
qn=[120 90 90 -90 90 90]*pi/180
ABB_IRB_4600.plot(qn)
Tp = transl(0.1*p);
q = ABB_IRB_4600.ikine(Tp)
% Dynamic Analysis
t=1;
qd(numrows(p),6)=0;
qd(1,6)=0;
for i=2:1:numrows(p)-1
qd(i,:)=(q(i+1,:)-q(i,:))/0.2
end
for i=1:1:numrows(p)-1
qdd(i,:)=(qd(i+1,:)-qd(i,:))/0.2
end
qdd(numrows(p),6)=-1*qd(numrows(p)-1,6)/0.2

for i=1:1:numrows(p)
Q(i,:) = ABB_IRB_4600.rne(q(i,:), qd(i,:), qdd(i,:))
end

ABB_IRB = ABB_IRB_4600.nofriction()