parte4y5.m

A robotic s subject work from the university. An example of Denavit-Hartemberg application over a

disp('Calcular una trayectoria en coordenadas articulares')
clear all

% Para probar con valores concretos:
t1=pi/2; t2=pi/2; t3=pi/2; t4=pi/2; % Estos valores son irrelevantes
l1=1.8; l2=0.8; l3=1.0; l4=0.8;

% Se crea la matriz dh con los par醡etros del manipulador
dh =    [0     0	t1    l1    0;
        -t2    l2	0     0     0;
		 0     0    pi/2  l3    0;
        -t3    0    0     l4    0;];
    
% Se establecen la tolerancia y el n鷐ero m醲imo de iteraciones
tol=1e-6; 
maxit=1000;

% Se introduce la trayectoria (circunferencia) y la orientaci髇 
% deseadas, se escogen 'x', 'y' y 'z' de manera que vayan variando 
% al generar la trayectoria de transformaciones

alpha=[0:0.2:2*pi];

x=2*cos(alpha);
y=2*sin(alpha);
z=2*ones(1,length(x));

phi=zeros(1,length(x));

% Se crea la trayectoria de transformaciones

for k=1:length(x)
   TG(:,:,k) = [cos(phi(k)) -sin(phi(k))    0   x(k);
                sin(phi(k)) cos(phi(k))     0   y(k);
                0           0               1   z(k);
                0           0               0   1];
end

% Se calcula el modelo inverso para cada uno de los puntos de la 
% trayectoria usando un vector inicial q0 y una m醩cara m (Para 
% un manipulador con menos de 6 DOF se debe pasar como par醡etro 
% un vector m醩cara)
q0=[0 0 0 0];
m=[1 1 1 0 0 0];
q=ikine(dh,tol,maxit,TG,q0,m)

disp('Simulacion del movimiento del robot')
% Se representa gr醘icamente el manipulador mientras recorre la trayectoria
plotbot(dh,q,'lfdw')