% manipulator.m ... vypocet inverzni dynamicke ulohy

clear all
close all

% zadane hodnoty
l2=0.5;
l3=0.4;
a=0.3;
b=0.1;
m2=10;
m3=8;
m4=8;
g=9.81;

I2S2=1/12*m2*(2*l2)^2;
I3S3=1/12*m3*(2*l3)^2;
I4S4=0.8;

R=[];   % priprava pole pro ukladani vysledku
for t=0:0.02:5
    % predepsane pohony
    fi2=(110-12*t)*pi/180; fi2d=-12*pi/180; fi2dd=0;
    fi3=(-60+4.8*t^2)*pi/180; fi3d=9.6*t*pi/180; fi3dd=9.6*pi/180;
    fi4=0; fi4d=0; fi4dd=0;
    
    % substituce
    sfi2=sin(fi2); cfi2=cos(fi2);
    sfi3=sin(fi3); cfi3=cos(fi3);
    sfi4=sin(fi4); cfi4=cos(fi4);
    
    % kinematika
    x2dd=-l2*(cfi2*fi2d^2+sfi2*fi2dd);
    y2dd=l2*(-sfi2*fi2d^2+cfi2*fi2dd);
    x3dd=-2*l2*(cfi2*fi2d^2+sfi2*fi2dd)-l3*(cfi3*fi3d^2+sfi3*fi3dd);
    y3dd=2*l2*(-sfi2*fi2d^2+cfi2*fi2dd)+l3*(-sfi3*fi3d^2+cfi3*fi3dd);
    x4dd=-2*l2*(cfi2*fi2d^2+sfi2*fi2dd)-2*l3*(cfi3*fi3d^2+sfi3*fi3dd)...
         -a*(cfi4*fi4d^2+sfi4*fi4dd)-b*(-sfi4*fi4d^2+cfi4*fi4dd);
    y4dd=2*l2*(-sfi2*fi2d^2+cfi2*fi2dd)+2*l3*(-sfi3*fi3d^2+cfi3*fi3dd)...
         -a*(-sfi4*fi4d^2+cfi4*fi4dd)-b*(cfi4*fi4d^2+sfi4*fi4dd);

    % dynamika
    D=[      1,        0,        1,       0,              0,             0, 0, 0, 0;
             0,        1,        0,       1,              0,             0, 0, 0, 0;
       l2*sfi2, -l2*cfi2, -l2*sfi2, l2*cfi2,              0,             0, 1,-1, 0;
             0,        0,       -1,       0,              1,             0, 0, 0, 0;
             0,        0,        0,      -1,              0,             1, 0, 0, 0;
             0,        0, -l3*sfi3, l3*cfi3,       -l3*sfi3,       l3*cfi3, 0, 1,-1;
             0,        0,        0,       0,             -1,             0, 0, 0, 0;
             0,        0,        0,       0,              0,            -1, 0, 0, 0;
             0,        0,        0,       0, -a*sfi4-b*cfi4, a*cfi4-b*sfi4, 0, 0, 1];
     
    M=diag([m2,m2,I2S2,m3,m3,I3S3,m4,m4,I4S4]);
    A=[x2dd,y2dd,fi2dd,x3dd,y3dd,fi3dd,x4dd,y4dd,fi4dd]';
    Q=[0,-m2*g,0,0,-m3*g,0,0,-m4*g,0]';
    
    R=[R, inv(D)*(M*A-Q)];
    
    % animace
    O3=[2*l2*cfi2,2*l2*sfi2];
    O4=O3+[2*l3*cfi3,2*l3*sfi3];
    plot([0,O3(1),O4(1)],[0,O3(2),O4(2)],'b-o','Linewidth',4);hold on
    plot([O4(1),O4(1)+a*cfi4],[O4(2),O4(2)+a*sfi4],'r-','Linewidth',5)
    hold off
    axis equal
    axis([-0.5 2 -0.5 2])
    drawnow
end

% vykresleni hnacich momentu
figure
plot(R(7:9,:)')
legend('M2','M3','M4')
xlabel('cas [s]'); ylabel('[Nm]');
title('Prubeh hnacich momentu');

% vykresleni Lagr. multiplikatoru (tj. reakci ve vazbach)
figure
plot(R(1:6,:)')
legend('\lambda_1','\lambda_2','\lambda_3','\lambda_4','\lambda_5','\lambda_6')
xlabel('cas [s]'); ylabel('[N]');
title('Lagrangeovy multiplikatory');