%PROBLEMA N#1

%UNVERSIDAD NACIONEAL DE TRUJILLO- ING.MECATRONICA

%FECHA 27-06-2018
%ELABORADO POR GUTIERREZ TERRONES JUAN EDUARDO
%-------------------------------------------------
%DH
clc, clear all, close all
GL=6;
syms q l a alpha
tetha = sym('tetha%d',[1 GL]);
d = sym('d%d',[1 GL]);
a= sym('a%d',[1 GL]);
alpha = sym('alpha%d',[1 GL]);
A=1;
%T=1;

%recursuDAT
syms g Fx Fy Fz q1 q2 L1 L2 Mx My Mz q1 q2 q3 q4 q5 q6
gravedad=[0; 0; -g];
%Fext=[Fx; Fy; Fz];
%Mext=[Mx;My;Mz];
m = sym('m%d',[1 GL]);

%%VAlores numericos del examen

tetha = [q1 q2 q2 q3 q4 q5 q6]; d = [450 0 0 615 0 100]; a= [150 600 100
0 0 0]; alpha = [pi/2 0 -pi/2 -pi/2 pi/2 0];% 6GL
Fext=[0; 0; 0];
Mext=[0;0;0];

for i=1:GL

% X = ['T',num2str(i),' respecto a ',num2str(i-1) ];

%disp(X)

T(:,:,i)=EJER_DENAVIT(tetha(i), d(i), a(i), alpha(i));

R=simplify( T(:,:,i));
Rn(:,:,i)= simplify(R(1:3,1:3));

A=A*T(:,:,i);
A_aux(:,:,i)=A;
R0_aux(:,:,i)=A(1:3,1:3) ;
R0(:,:,i)=simplify(R0_aux(:,:,i)^(-1));

%Jw_aux(:,i)=A(:,3);
% Jw=Jw_aux(1:3,:); % jacobiano de velocidad angular

end
%Jw=simplify(Jw);
%Ti=simplify(A);
%%%%%SUBS%%%%

%%%
% calculo fuerza
j=GL;
bandera=0;
while bandera==0

if j==1,; F(:,:,j)=simplify(Rn(:,:,j)*Fext-m(j)*gravedad);
else; F(:,:,j)=simplify(Rn(:,:,j)*Fext-m(j)*R0(:,:,j-1)*gravedad);
Fext=F(:,:,j);
end
j=j-1;
if j==0; bandera=1; end
end

disp('FUERZAS: ')
simplify(F)
% calculo dl momento

k=GL;
flag=0;
% se encuentra estos valores por una inspección geometrica simple del
mecanismso
syms r1x r2x r1y r2y r1z r2y rc1x rc2x rc1y rc2y rc1z rc2y
rn=[150, 600, 100, 0, 0, 0;0 0 0 0 0 0; 450 0 0 615 0 100];
rc=[rn/2]; %valores para 6GL

while flag==0
if k==1;M(:,:,k)=
Rn(:,:,k)*Mext+cross((Rn(:,:,k)*rn(:,k)),(Rn(:,:,k)*F(:,:,k)))-
cross((Rn(:,:,k)*rc(:,k)),(m(k)*gravedad));
else M(:,:,k)=
Rn(:,:,k)*Mext+cross((Rn(:,:,k)*rn(:,k)),(Rn(:,:,k)*F(:,:,k)))-
cross((Rn(:,:,k)*rc(:,k)),(m(k)*R0(:,:,k-1)*gravedad));
Mext=M(:,:,k);
end
k=k-1;
if k==0; flag=1; end

end
disp('MOMENTO : ')
simplify(M)
%PROBLEMA N#3

%UNVERSIDAD NACIONEAL DE TRUJILLO- ING.MECATRONICA

%FECHA 27-06-2018
%ELABORADO POR GUTIERREZ TERRONES JUAN EDUARDO
%-------------------------------------------------
%NEWTON EULER-3GDL ARTICULARES
clc, clear all
syms g q1 q2 q3 dq1 dq2 dq3 ddq1 ddq2 ddq3 l1 l2 l3 l1c l2c l3c pi m1 m2
m3 Ixx1 Ixy1 Ixz1 Iyx1 Iyy1 Iyz1 Izx1 Izy1 Izz1 Ixx2 Ixy2 Ixz2 Iyx2 Iyy2
Iyz2 Izx2 Izy2 Izz2 Ixx3 Ixy3 Ixz3 Iyx3 Iyy3 Iyz3 Izx3 Izy3 Izz3
syms Fx Fy Fz Nx Ny Nz L1 L2 L3 L4 d1
syms wp1 wp2 wp3 qp1 qp2 qp3 qpp1 qpp2 qpp3
%PARÁMETROS DE ENTRADA
Fext=[Fx ; Fy ; Fz];
Next=[Nx ; Ny ; Nz];
g0=[0;0;-g];

%DENAVIT
%CONSTANTES
theta= [0;q2;q3];
d=[d1;L2;L3];
a=[L1; 0; 0];
alpha= [pi/2; 0; 0];

%MATRIZ DE INERCIAS
I1=[Ixx1 Ixy1 Ixz1; Iyx1 Iyy1 Iyz1; Izx1 Izy1 Izz1];
I2=[Ixx2 Ixy2 Ixz2; Iyx2 Iyy2 Iyz2; Izx2 Izy2 Izz2];
I3=[Ixx3 Ixy3 Ixz3; Iyx3 Iyy3 Iyz3; Izx3 Izy3 Izz3];

%MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

T01= EJER_DENAVIT(theta(1), d(1), a(1), alpha(1))
T12= EJER_DENAVIT(theta(2), d(2), a(2), alpha(2))
T23= EJER_DENAVIT(theta(3), d(3), a(3), alpha(3))

T02=simplify(T01*T12);
T03=simplify(T01*T12*T23);

%MATRICES DE ROTACIÓN
R01=[T01(1,1) T01(1,2) T01(1,3); T01(2,1) T01(2,2) T01(2,3); T01(3,1)
T01(3,2) T01(3,3) ];
R02=[T02(1,1) T02(1,2) T02(1,3); T02(2,1) T02(2,2) T02(2,3); T02(3,1)
T02(3,2) T02(3,3) ];
R03=[T03(1,1) T03(1,2) T03(1,3); T03(2,1) T03(2,2) T03(2,3); T03(3,1)
T03(3,2) T03(3,3) ];
R12=[T12(1,1) T12(1,2) T12(1,3); T12(2,1) T12(2,2) T12(2,3); T12(3,1)
T01(3,2) T12(3,3)];
R23=[T23(1,1) T23(1,2) T23(1,3); T23(2,1) T23(2,2) T23(2,3); T23(3,1)
T23(3,2) T23(3,3)];

%MATRICES DE ROTACIÓN TRANSPUESTAS

RT01=transpose(R01);
RT02=transpose(R02);
RT03=transpose(R03);
RT12=transpose(R12);
RT23=transpose(R23);

%MATRICES DE POSICIÓN P* (coordenadas i respecto a i-1)

%P*
p01= [T01(1,4);T01(2,4);T01(3,4)];
p12= [T12(1,4);T12(2,4);T12(3,4)];
p23= [T23(1,4);T23(2,4);T23(3,4)];

%POSICIÓN DEL CENTRO DE MASA i RESPECTO A LA BASE

r1C=[0;0;l1c];
r2C=[l2c*cos(q1)*cos(q2); l2c*cos(q2)*sin(q1); l1 + l2c*sin(q2)];
r3C=[cos(q1)*(l3c*cos(q2 + q3) + l2*cos(q2)); sin(q1)*(l3c*cos(q2 + q3) +
l2*cos(q2)); l1 + l3c*sin(q2 + q3) + l2*sin(q2)];

%POSICIÓN DE CENTROS DE MASA DEL ELEMENTO i DESDE SIST. COORDENADAS i+1.

lo
%mismo que r01c de estática
%Si
s01=[0;l1c;0];
s12=[l2c;0;0];
s23=[l3c;0;0];

Z0=[0;0;1];
zero=[0;0;0];

%........................................................................
...................

%ECUACIONES HACIA ADELANTE

%VELOCIDADES ANGULARES:
w1=simplify(RT01*(Z0*q1));
w2=simplify(RT12*(w1+Z0*q2));
w3=simplify(RT23*(w2+Z0*q3));

%ACELERACIONES ANGULARES:
wp1=simplify(RT01*(Z0*qpp1));
wp2=simplify(RT12*(wp1+Z0*qpp2+cross(w1,Z0*qp2)));
wp3=simplify(RT23*(wp2+Z0*qpp3+cross(w2,Z0*qp3)));

%ACELERACIONES LINEALES
vp1=simplify(cross(wp1,RT01*p01)+cross(wp1,cross(wp1,RT01*p01)));
vp2=simplify(cross(wp2,RT02*p12)+cross(wp2,cross(wp2,RT02*p12))+RT12*vp1)
;
vp3=simplify(cross(wp3,RT03*p23)+cross(wp3,cross(wp3,RT03*p23))+RT23*vp2)
;

%ACELERACIÓN FINAL EQUIS DÉ 100% REAL NO FAKE.

a1=simplify(cross(wp1,RT01*s01)+cross(w1,cross(w1,RT01*s01))+ vp1);
a2=simplify(cross(wp2,RT02*s12)+cross(w2,cross(w2,RT02*s12))+ vp2);
a3=simplify(cross(wp3,RT03*s23)+cross(w3,cross(w3,RT03*s23))+ vp3);

%........................................................................
...................
%ECUACIONES HACIA ATRÁS

%FUERZAS
%---> ESLABÓN 2-3 ejerce el eslabón 2 sobre el eslabón 3
F23=R23*Fext+m3*a3-m3*RT02*g0;
%--->ESLABÓN 1-2 ejerce el eslabón 1 sobre el eslabón 2
F12=R12*F23+m2*a2-m2*RT01*g0;
%--->ESLABÓN 0-1 ejerce la base sobre el eslabón 1.
F01=R01*F12+m1*a1-m1*g0;

%MOMENTOS
%---> ESLABÓN 2-3 ejerce el eslabón 2 sobre el eslabón 3

N23=simplify(R23*(Next+cross(RT03*p23,Fext))+cross(RT02*p23+RT02*s23,F23)
+ (RT03*I3*R03*wp3)+ cross(w3,RT03*I3*R03*w3));
%--->ESLABÓN 1-2 ejerce el eslabón 1 sobre el eslabón 2
N12=simplify(R12*(N23+cross(RT02*p12,
F23))+cross(RT01*p12+RT01*s12,F12)+
RT02*I2*R02*wp2+cross(w2,RT02*I2*R02*w2));
%--->ESLABÓN 0-1 ejerce la base sobre el eslabón 1.
N01=simplify(R01*(N12+cross(RT01*p01, F12))+cross(p01+s01,F01)+
RT01*I1*R01*wp1+cross(w1,RT01*I1*R01*w1));

%FINALMENTE, TORQUEs
tau1=transpose(N01)*Z0
tau2=transpose(N02)*Z0
tau3=transpose(N03)*Z0