roboticatp4_ex2.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 2 ----------------------------------------------------------------

syms t1 t2 t3 b c

PJ_DH=[t1 50 -pi/2 0;
       t2 0  pi/2  0;
       0  b  -pi/2 0;
       t3 0  pi/2  0;
       0  c  0     0];

oTn=eye(4);
for i=1:5
   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
% oTn=simple(oTn)

% Links
% L =LINK([alpha A theta D sigma offset])   sigma [0-rotacao 1-translacao]
L1 = link([-pi/2  0 0    50 0 0   ]);
L2 = link([ pi/2  0 0    0  0 0   ]);
L3 = link([-pi/2  0 0    0  1 0   ]);
L4 = link([ pi/2  0 0    0  0 0   ]);
L5 = link([0      0 0    0  1 0   ]);

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

% Matrizes de Rotacao utilizadas no calculo do Jacobiano
oR1 = [cos(t1) 0  -sin(t1);
       sin(t1) 0  cos(t1);
       0       -1 0];

oR3 =[cos(t1)*cos(t2) -cos(t1)*sin(t2) -sin(t1);
      sin(t1)*cos(t2) -sin(t1)*sin(t2) cos(t1);
      -sin(t2)        -cos(t2)         0];

% Calculo da Matriz Jacobiana
Jo=[diff(oTn(1,4),t1) diff(oTn(1,4),t2) diff(oTn(1,4),b) diff(oTn(1,4),t3) diff(oTn(1,4),c);    % Vx
    diff(oTn(2,4),t1) diff(oTn(2,4),t2) diff(oTn(2,4),b) diff(oTn(2,4),t3) diff(oTn(2,4),c);    % Vy
    diff(oTn(3,4),t1) diff(oTn(3,4),t2) diff(oTn(3,4),b) diff(oTn(3,4),t3) diff(oTn(3,4),c);    % Vz
  [0;0;1] oR1*[0;0;1] [0;0;0] oR3*[0;0;1] [0;0;0]]

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

% Matriz Jacobiana para a junta teta1 e b e restantes constantes
Jo_b=[Jo(1,1) Jo(1,3);
    Jo(2,1) Jo(2,3)]

% Vector de velocidade das juntas para Vy=20 cm/s
w=inv(Jo_b)*[0;20]

teta2=pi/2;
teta3=pi/2;
lc=30;

tx=40;
i=1;
for ty=-20:0.1:20
   % Calculo dos valores das juntas com base na cinematica inversa
   teta1=atan2(ty,tx);
   lb=sqrt(tx^2+ty^2);
   % Calculo das velocidades com base nos valores de teta1 e b
   w1= 20*cos(teta1)/(sin(teta1)^2*lc*cos(teta2)*sin(teta3)+sin(teta1)^2*sin(teta2)*lc*cos(teta3)+sin(teta1)^2*sin(teta2)*lb+cos(teta1)^2*lc*cos(teta2)*sin(teta3)+cos(teta1)^2*sin(teta2)*lc*cos(teta3)+cos(teta1)^2*sin(teta2)*lb);
   w2=20*sin(teta1)/(sin(teta1)^2+cos(teta1)^2)/sin(teta2);
   
   tab(i,1)=ty;
   tab(i,2)=teta1;
   tab(i,3)=lb;
   tab(i,4)=w1;
   tab(i,5)=w2;
   i=i+1;    
end

% Tabela com os parametros das juntas [teta1 teta2 b teta3 c]
x=size(tab);
for i=1:x(1,1)    
    tab2(i,1)=tab(i,2);  % teta1
    tab2(i,2)=teta2;     % teta2
    tab2(i,3)=tab(i,3);  % b
    tab2(i,4)=teta3;     % teta3
    tab2(i,5)=lc;        % c    
end

% Plot do Robo com Px constante e Py a variar de -20 a 20
subplot(2,3,1)
plot(R,tab2,'workspace',[-10 50 -30 30 -10 60])
% Plot da velocidade de teta1 em funcao da posicao Py
subplot(2,3,2)
plot(tab(:,1),tab(:,4),'-r')
grid
xlabel('Py[cm]');
ylabel('W1[rad/s]')
legend('w1')
% Plot da velocidade de b em fucao da posicao Py
subplot(2,3,3)
plot(tab(:,1),tab(:,5),'-g')
grid
xlabel('Py[cm]');
ylabel('W2[rad/s]')
legend('w2')
% Plot do angulo de teta1 em funcao da posicao Py
subplot(2,3,5)
plot(tab(:,1),tab(:,2),'-r')
grid
xlabel('Py[cm]');
ylabel('teta1[º]')
legend('teta1')
% Plot do comprimento de b em funcao da posicao Py
subplot(2,3,6)
plot(tab(:,1),tab(:,3),'-g')
grid
xlabel('Py[cm]');
ylabel('b[cm]')
legend('b')

% Tabela para os pontos [-20 -10 0 10 20]
%   y     teta1     b     w1        w2
% [-20  -0.4636  44.7213  0.4     -8.9442]
% [-10  -0.2449  41.2310  0.4705  -4.8507]
% [0    0        40       0.5     0      ]
% [10   0.2449   41.2310  0.4705  4.8507 ]
% [20   0.4636   44.7213  0.4     8.9442 ]