roboticatp4_ex1.m

Speed control for a specific trajectory for a robotic manipulator

%--------------------------------------------------------------------------
% Robotica LabWork 4
%
% Ricardo Faria n.2007112135
% Luis Santos n.2007122309
%
%--------------------------------------------------------------------------

clear all
clc

%% Ponto 1 ----------------------------------------------------------------

syms t1 t2 t3

PJ_DH=[t1 0 0 35;
    t2 0 0 35;
    t3 10 0 0];

oTn=eye(4);
for i=1:3
   C=[cos(PJ_DH(i,1)) -sin(PJ_DH(i,1))*cos(PJ_DH(i,3)) sin(PJ_DH(i,1))*sin(PJ_DH(i,3)) PJ_DH(i,4)*cos(PJ_DH(i,1));
        sin(PJ_DH(i,1)) cos(PJ_DH(i,1))*cos(PJ_DH(i,3)) -cos(PJ_DH(i,1))*sin(PJ_DH(i,3)) PJ_DH(i,4)*sin(PJ_DH(i,1));
        0 sin(PJ_DH(i,3)) cos(PJ_DH(i,3)) PJ_DH(i,2);0 0 0 1];

   oTn=oTn*C;
end
% Matriz de Cinematica Directa
oTn=simple(oTn)

% Links
% L =LINK([alpha A theta D sigma offset])   sigma [0-rotacao 1-translacao]
L1 = link([0  35 0 0  0 0   ]);
L2 = link([0  35 0 0  0 0   ]);
L3 = link([0  0  0 10 0 0   ]);

% Robo
R=robot({L1 L2 L3});

% Movimento do Robo na posicao [30,30,10]
ty=30;

tab = zeros(1, 3);
i = 1;
for tx=-30:0.1:30
    
%     ty=ty+sin(tx);
    
    teta2=acos((tx^2+ty^2-2*(35^2))/(2*35^2));
    teta1=atan2(ty,tx)-atan2(sin(teta2),cos(teta2)+1);
    teta3=pi/2-teta1-teta2;

    tab(i,: ) = [teta1 teta2 teta3];
    tab2(i,: )= [tab(i,:) tx ty];
    i=i+1;
    
end

figure
subplot(2,2,[1 3])
plot(R,tab,'workspace',[-50 50 -50 50 -5 15])
subplot(2,2,2)
plot(tab)
legend('teta1','teta2','teta3')
grid

% Calculo do Jacobiano
J=[diff(oTn(1,4),t1) diff(oTn(1,4),t2) diff(oTn(1,4),t3);
    diff(oTn(2,4),t1) diff(oTn(2,4),t2) diff(oTn(2,4),t3);
    1 1 1]

% Matriz Jacobiana apartir da funcao jacobian da toolbox
J_toolbox=jacobian([oTn(1,4);oTn(2,4);oTn(3,4)],[t1 t2 t3])

inv_J=simple(inv(J));

vx=20;
vy=0;
wz=0;

% Vector de velocidades angulares
w=inv_J*[vx;vy;wz]

j=size(tab);

for i=1:j(1,1)
   
    te1=tab2(i,1);
    te2=tab2(i,2);
    te3=tab2(i,3);
    
    w1=4/7*cos(te1+te2)/sin(te2);
    w2=4/7*(-cos(te1+te2)-cos(te1))/sin(te2);
    w3=4/7*cos(te1)/sin(te2);
    
    %               px        py           t1              t2                 t3         w1 w2 w3
    tab3(i,:) = [tab2(i,4) tab2(i,5) tab2(i,1)*180/pi tab2(i,2)*180/pi  tab2(i,3)*180/pi w1 w2 w3];
    
end

subplot(2,2,4)
hold on
grid
xlabel('Px[cm]');
ylabel('W[rad/s]')
plot(tab3(:,1),tab3(:,6),'-r')
plot(tab3(:,1),tab3(:,7),'-g')
plot(tab3(:,1),tab3(:,8),'-b')
legend('w1','w2','w3')