% CINEMÁTICA DIRECTA ROBOT PLANO 3GL RRP

clc,clear,close ;
% Características del Robot
syms q1 q2 q3 L1 L2 q3
H01=[cos(q1) -sin(q1) 0 0;sin(q1) cos(q1) 0 0;0 0 1 0;0 0 0 1];%Matriz de rotación
en Z theta 1
H12=[1 0 0 L1;0 1 0 0;0 0 1 0;0 0 0 1];% Matriz de traslación en X L1
H02=H01*H12; % Coordenadas de la junta 2 respecto a la base
H23=[cos(q2) -sin(q2) 0 0;sin(q2) cos(q2) 0 0;0 0 1 0;0 0 0 1];%Matriz de rotación
en Z theta 2
H34=[1 0 0 L2;0 1 0 0;0 0 1 0;0 0 0 1];% Matriz de traslación en X L2
H04=H01*H12*H23*H34;
H45=[1 0 0 q3;0 1 0 0;0 0 1 0;0 0 0 1];
H05=H01*H12*H23*H34*H45;% Coordenadas del efector final respecto a la base
H05=simplify(H05)
%Asigno Valores
q1=35;%Ángulo Theta 1 en grados
q2=20;%Ángulo Tetha 2 en grados
L1=1;% Longitud eslabón 1 en metros
L2=0.8;% Longitud eslabón 2 en metros
q3=0.4;
q1=q1*pi/180;%convierto grados a radianes
q2=q2*pi/180;
H02=eval(H02); %Evalúo el punto de la junta 2
H04=eval(H04); % Evaluó el punto del efector final
H05=eval(H05);
%Puntos
x0=0;y0=0; % Coordenadas de la Base
x1=H02(1,4);y1=H02(2,4); % Coordenadas de la junta 2
x2=H04(1,4);y2=H04(2,4); % Coordenadas del efector final
x3=H05(1,4);y3=H05(2,4);
x=[x0,x1,x2,x3];
y=[y0,y1,y2,y3];
%Gráfica
figure
line(x,y)
hold on
plot(x1,y1,'ro')
hold on
plot(x2,y2,'ro')
hold on
plot(x3,y3,'ro')
title('Robot Plano 3GL RRP')