Cinemática Direta Robô Scara(Fanuc)

Esta é uma pré-visualização de arquivo. Entre para ver o arquivo original

clear all
clc
syms theta_1 theta_2 theta_3 real
%theta_1 = 0*pi/180;
%theta_2 = 0*pi/180;
%theta_3 = 90*pi/180;
d = 0;
P_A_B = [0, 0, 160]';
R_B_A = [cos(theta_1), -sin(theta_1), 0;
 sin(theta_1), cos(theta_1) 0;
 0, 0, 1];
T_B_A = [R_B_A, P_A_B; zeros(1,3), 1];
P_B_C = [0, 350, 0]';
R_C_B = [cos(theta_2), -sin(theta_2), 0;
 sin(theta_2), cos(theta_2), 0;
 0, 0, 1];
T_C_B = [R_C_B, P_B_C; zeros(1,3), 1];
P_C_D = [0, 300, -d]';
R_D_C = eye(3,3);
T_D_C = [R_D_C, P_C_D; zeros(1,3), 1];
P_D_E = [0, 0, 0]';
R_E_D = [cos(theta_3), -sin(theta_3), 0;
 sin(theta_3), cos(theta_3), 0;
 0, 0, 1];
T_E_D = [R_E_D, P_D_E; zeros(1,3), 1];
D_A_T=T_B_A*(T_C_B*(T_D_C*T_E_D))
%D_A_T=T_B_A*(T_C_B*(T_D_C*[0, 0, 0, 1]'))