close all
mdl_puma560
p560.trchain
n =100;
t1 = 0:1/n:4;
t2 = 0:1/n:2;
t3 = 0:1/n:4;
q1 = [0 0 -3.14 0 0 0];
[qtg1 qtg1D qtg1DD] = jtraj(qz, q1, t1);

q2 = [3.14/2 0 -3.14 0 0 0];
[qtg2 qtg2D qtg2DD] = jtraj(q1, q2, t2);

q3 = [3.14/2 0 0 0 0 0];
[qtg3 qtg3D qtg3DD] = jtraj(q2, q3, t3);

qtgFinal = [qtg1; qtg2; qtg3];
qtgDFinal = [qtg1D; qtg2D; qtg3D];
qtgDDFinal = [qtg1DD; qtg2DD; qtg3DD];
TAU = p560.rne(qtgFinal, qtgDFinal, qtgDDFinal);

for i = 1:6
    subplot(3,2,i);
    plot(TAU(:,i));
end

% figure(2)
% for q=qtgFinal'
%     p560.plot(q');
% end
%MOMENTY SUMA
M = p560.inertia(qtgFinal);
C = p560.coriolis(qtgFinal, qtgDFinal);
F = p560.friction(qtgDFinal);
G = p560.gravload(qtgFinal);
TauSum = M*qtgDDFinal + C*qtgDFinal + F + G;