%%%% MODELO DINAMICO ROBOT 4 GDL%%%% Ing.MAURICIO SARASTYclear
allclose allclc%%%%%%%%%%%%%%%%%%%%%%% DECLARACIN DE VARIABLES
%%%%%%%%%%%%%%%%%%%%%%%%%%a1 = 0; a2 = 0.3; a3 = 0.3; a4 = 0.1; a5
= 0; a6 = 0; d1 = 0.6; d2 = 0; d3 = 0; d4 = 0; d5 = 0; d6 =
0;alpha1 = pi/2; alpha2 = 0; alpha3 = 0; alpha4 = 0; alpha5 =
0;alpha6 = 0;theta1 = 0;theta2 = pi/2;theta3 = -2*pi/3;theta4 =
-pi/2;theta5 = 0;theta6 = 0;thpp1 = 0;thpp2 = 0;thpp3 = 0;thpp4 =
0;thpp5 = 0;thpp6 = 0;thp1 = 0;thp2 = 0;thp3 = 0;thp4 = 0;thp5 =
0;thp6 = 0;a = [a1 a2 a3 a4 a5 a6];alpha = [alpha1 alpha2 alpha3
alpha4 alpha5 alpha6];d = [d1 d2 d3 d4 d5 d6];thp = [thp1 thp2 thp3
thp4 thp5 thp6]; % VELOCIDADESthpp = [thpp1 thpp2 thpp3 thpp4 thpp5
thpp6]; % ACELERACIONEStheta = [theta1 theta2 theta3 theta4 theta5
theta6]; % ANGULO DE ARTICULACIONm = [12 6 6 2 0 0];Xm = [0 0.15
0.15 0.05 0 0];Ym = [0 0 0 0 0 0];Zm = [0.3 0 0 0 0 0];g = [0 0
-abs(9.8062) 0];I = eye(4);%%%%%%%%%%%%%%%% CALCULO DE LA MATRIZ DE
TRANSFORMACIN %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0A1 1A2 2A3 3A4 4A5
5A6 DE CADA ESLABON %%%%%%%%%%%%%%%%%%%% | c(t(i))
-c(al(i))*s(t(i)) s(al(i))*s(t(i)) a(i)*c(t(i)) | % i-1Ai = |
s(t(i)) c(al(i))*c(t(i)) -s(al(i))*c(t(i)) a(i)*s(t(i)) |% | 0
s(al(i)) c(al(i)) d(i) | % | 0 0 0 1 | for i=1:6 MxTr(:, :, i+1) =
MatrizTransformacion(a(i), alpha(i), ... % Llamar la funcion de
trans d(i), theta(i));endMxTr(:, :, 1) = I; %MxTr Transformacion de
cada eslabon%%%%% REFERENCIA AL ORIGEN 0A1 0A2 0A3 0A4 0A5 0A6
%%%%%%%%%%%%%%%%%%%%MxTrf(:, :, 1) = MxTr(:, :, 1);for i=2:7
MxTrf(:, :, i) = MxTrf(:, :, i-1) * MxTr(:, :, i); % MxTr
Transformacion respecto al origenend%%%%%%%%%%%%% CALCULO DE LA
MATRIZ DEL TENSOR DE INERCIA Ji %%%%%%%%%%%%%%%Ixx = (1/4) * m(1) *
(0.05^2) + (1/3)*m(1)*(0.3^2);Izz = Ixx;Iyy = 0.5 *
m(1)*(0.05^2);for i=1:6 J(:,:,i) = [ Ixx 0 0 m(i)*Xm(i); ... 0 Iyy
0 m(i)*Ym(i); ... 0 0 Izz m(i)*Zm(i); ... m(i)*Xm(i) m(i)*Ym(i)
m(i)*Zm(i) m(i) ];end%%%%%%%%% MATRIZ DE DERIVADAS PARA
ARTICULACIONES DE ROTACIN Qi%%%%%%%%%%%Q = [0 -1 0 0; 1 0 0 0; 0 0
0 0; 0 0 0 0];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MATRIZ U(i,j)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%k = 1;U = sym(zeros(4,4,21));for i=1:6
A1 = MxTrf(:,:,i); h = i+1; A2 = MxTr(:,:,h); U(:,:,k) = A1 * Q *
A2; for j=i+2:7 k = k + 1; A2 = A2 * MxTr(:,:,j); U(:,:,k) = A1 * Q
* A2; end k = k + 1;end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MATRIZ D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%h = 1;p = 1;D = sym(zeros(6));for
j=1:6 k = j; for y=j:6 sum = 0; for i=k:6 sum = vpa(sum +
trace(U(:,:,h)*J(:,:,i)*U(:,:,p)')); h = h + 1; p = p + 1; end
D(j,y) = sum; D(y,j) = D(j,y); k = k + 1; p = p - (6 - y); end h =
p;end%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MATRIZ U(i,j,k)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%U1 = sym(zeros(4,4,56));k = 1;for
n=1:6 for i=1:n M1 = MxTrf(:,:,i); M2 = MxTr(:,:,1); for j=i:n %%%
Calculo M3 %%% M3 = MxTr(:,:,j+1); for o=j+2:7 M3 = M3 *
MxTr(:,:,o); end %%%%%%%%%%%%%%%%%%% U1(:,:,k) = M1*Q*M2*Q*M3; %%%
Calculo M2 %%% M2 = MxTr(:,:,i+1); for o=i+1:j M2 = M2 *
MxTr(:,:,o+1); end %%%%%%%%%%%%%%%%%% k = k+1; end
endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MATRIZ H(i,j,k)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%H = sym(zeros(6,6,6));vp = [1 7 12 16
19 21];p1 = vp(1);for n=1:6 h = sym(zeros(6)); c = [1 2 5 11 21
36]; t = n; p = vp(n); p1 = vp(n); for i=1:6 for j=i:6 sum=0; if
j
