%% =========================================================================
%% EXAMEN OFICIAL ROBOTICA - CONVOCATORIA 9 DE JUNIO DE 2025 (PROBLEMA B)
%% RESOLUCION MEDIANTE EL ALGORITMO RECURSIVO DE NEWTON-EULER (NE_R3GDL.m)
%% Grado en Ingenieria de Tecnologias Industriales - E.S.I. Sevilla
%% =========================================================================
% Este script adapta y ejecuta la plantilla oficial de clase NE_R3GDL.m
% para resolver el Problema B de Junio de 2025 (Robot RR de accionamiento directo).
%
% Contenido:
% 1. Definicion de parametros cinematicos y dinamicos segun el enunciado.
% 2. Algoritmo recursivo hacia afuera (cinematica: w, wd, vd, a_c).
% 3. Algoritmo recursivo hacia adentro (dinamica: f, n, Tau).
% 4. Extraccion de matrices M(q), V(q, qd) y G(q).
% 5. Inclusion de motores de accionamiento directo y senales de control por intensidad (i1, i2).
% =========================================================================

clear; clc; close all;

fprintf('=========================================================================\n');
fprintf('  RESOLUCION JUNIO 2025 P3 USANDO EL ALGORITMO NEWTON-EULER (NE_R3GDL)  \n');
fprintf('=========================================================================\n\n');

%% 1. TIPO DE ARTICULACIONES
Tipo_Q1 = 'R'; % Rotacion
Tipo_Q2 = 'R'; % Rotacion
Tipo_Q3 = 'R'; % 3a articulacion nula (robot 2 GDL)

%% 2. DEFINICION DE VARIABLES SIMBOLICAS
syms q1 q2 q3 qd1 qd2 qd3 qdd1 qdd2 qdd3 real
syms i1 i2 g real
PI = sym(pi);

%% 3. DATOS CINEMATICOS DEL BRAZO (Denavit-Hartenberg)
% Dimensiones de los eslabones: L1 = L2 = 2L = 1.0 m (L = 0.5 m)
L1 = 1.0;
L2 = 1.0;

% Parametros DH:
% Eslabon 1:
theta1 = q1;   d1 = 0;   a1 = L1;   alpha1 = 0; % Plano de rotacion
% Eslabon 2:
theta2 = q2;   d2 = 0;   a2 = L2;   alpha2 = 0;
% Eslabon 3 (Nulo para robot de 2 GDL):
theta3 = 0;    d3 = 0;   a3 = 0;    alpha3 = 0;
% Marco final:
theta4 = 0;    d4 = 0;   a4 = 0;    alpha4 = 0;

%% 4. DATOS DINAMICOS DE LOS ESLABONES
% Eslabon 1 (Pieza A: longitud 1.0 m, masa 1.0 kg):
m1  = 1.0;                    % kg
s11 = [-L1/2, 0, 0]';         % CoM en el punto medio [-0.5, 0, 0] m
I11 = [ 0.00021, 0,   0; ...  % ^B I_22 = 0.00021 kg*m^2 (eje longitudinal X)
        0,       0.5, 0; ...  % ^B I_11 = 0.5 kg*m^2     (eje transversal Y)
        0,       0,   0.5 ];  % ^B I_33 = 0.5 kg*m^2     (eje normal Z)

% Eslabon 2 (Pieza B: longitud 1.0 m, masa 1.0 kg):
m2  = 1.0;                    % kg
s22 = [-L2/2, 0, 0]';         % CoM en el punto medio [-0.5, 0, 0] m
I22 = [ 0.00021, 0,   0; ...
        0,       0.5, 0; ...
        0,       0,   0.5 ];

% Eslabon 3 (Nulo):
m3  = 0;
s33 = [0, 0, 0]';
I33 = zeros(3, 3);

%% 5. DATOS DE LOS MOTORES DE ACCIONAMIENTO DIRECTO (r = 1)
Jm1 = 0.025;   Jm2 = 0.025;   Jm3 = 0;   % kg*m^2
Bm1 = 2e-5;    Bm2 = 2e-5;    Bm3 = 0;   % N*m/(rad/s)
R1  = 1;       R2  = 1;       R3  = 0;   % Accionamiento directo: r = 1
Kt1 = 10.0;    Kt2 = 10.0;    Kt3 = 1;   % N*m/A

fprintf('--- PARAMETROS CARGADOS EN LA PLANTILLA NE_R3GDL ---\n');
fprintf('  m1 = %.1f kg, m2 = %.1f kg, L1 = %.1f m, L2 = %.1f m\n', m1, m2, L1, L2);
fprintf('  I_long = %.5f kg*m^2, I_perp = %.1f kg*m^2\n', I11(1,1), I11(2,2));
fprintf('  Jm1 = Jm2 = %.3f kg*m^2, Bm1 = Bm2 = %.1e N*m/(rad/s)\n', Jm1, Bm1);
fprintf('  Accionamiento directo: R1 = R2 = 1, Kt1 = Kt2 = %.1f N*m/A\n\n', Kt1);

%% 6. ALGORITMO RECURSIVO DE NEWTON-EULER (PASOS N-E 1 A 10)

% N-E 1: Vectores de posicion entre origenes p_ii
p11 = [a1, d1*sin(alpha1), d1*cos(alpha1)]';
p22 = [a2, d2*sin(alpha2), d2*cos(alpha2)]';
p33 = [a3, d3*sin(alpha3), d3*cos(alpha3)]';
p44 = [a4, d4*sin(alpha4), d4*cos(alpha4)]';

% N-E 2: Condiciones iniciales de la base
w00  = [0 0 0]';
wd00 = [0 0 0]';
v00  = [0 0 0]';
% En robot planar horizontal g=0 en plano, o [0; g; 0] si gravedad en plano:
vd00 = [0 g 0]';

% Condiciones en el extremo (sin cargas externas aplicadas):
f44 = [0 0 0]';
n44 = [0 0 0]';
Z   = [0 0 1]';

% N-E 3: Matrices de rotacion relativas
R01 = [cos(theta1) -cos(alpha1)*sin(theta1)  sin(alpha1)*sin(theta1);
       sin(theta1)  cos(alpha1)*cos(theta1) -sin(alpha1)*cos(theta1);
       0            sin(alpha1)              cos(alpha1)           ];
R10 = R01';

R12 = [cos(theta2) -cos(alpha2)*sin(theta2)  sin(alpha2)*sin(theta2);
       sin(theta2)  cos(alpha2)*cos(theta2) -sin(alpha2)*cos(theta2);
       0            sin(alpha2)              cos(alpha2)           ];
R21 = R12';

R23 = [cos(theta3) -cos(alpha3)*sin(theta3)  sin(alpha3)*sin(theta3);
       sin(theta3)  cos(alpha3)*cos(theta3) -sin(alpha3)*cos(theta3);
       0            sin(alpha3)              cos(alpha3)           ];
R32 = R23';

R34 = eye(3);
R43 = eye(3);

% N-E 4: Velocidades angulares
w11 = R10*w00 + Z*qd1;
w22 = R21*w11 + Z*qd2;
w33 = R32*w22 + Z*qd3;

% N-E 5: Aceleraciones angulares
wd11 = R10*wd00 + Z*qdd1 + cross(w11, Z*qd1);
wd22 = R21*wd11 + Z*qdd2 + cross(w22, Z*qd2);
wd33 = R32*wd22 + Z*qdd3 + cross(w33, Z*qd3);

% N-E 6: Aceleraciones lineales de los origenes
vd11 = cross(wd11, p11) + cross(w11, cross(w11, p11)) + R10*vd00;
vd22 = cross(wd22, p22) + cross(w22, cross(w22, p22)) + R21*vd11;
vd33 = cross(wd33, p33) + cross(w33, cross(w33, p33)) + R32*vd22;

% N-E 7: Aceleraciones lineales de los centros de gravedad
a11 = cross(wd11, s11) + cross(w11, cross(w11, s11)) + vd11;
a22 = cross(wd22, s22) + cross(w22, cross(w22, s22)) + vd22;
a33 = cross(wd33, s33) + cross(w33, cross(w33, s33)) + vd33;

% ITERACION HACIA EL INTERIOR (DINAMICA)

% N-E 8: Fuerzas ejercidas sobre los eslabones
f33 = R34*f44 + m3*a33;
f22 = R23*f33 + m2*a22;
f11 = R12*f22 + m1*a11;

% N-E 9: Pares ejercidos sobre los eslabones
n33 = R34*(n44 + cross(R43*p33, f44)) + cross(p33 + s33, m3*a33) + I33*wd33 + cross(w33, I33*w33);
n22 = R23*(n33 + cross(R32*p22, f33)) + cross(p22 + s22, m2*a22) + I22*wd22 + cross(w22, I22*w22);
n11 = R12*(n22 + cross(R21*p11, f22)) + cross(p11 + s11, m1*a11) + I11*wd11 + cross(w11, I11*w11);

% N-E 10: Pares articulares
T1 = n11'*R10*Z;
T2 = n22'*R21*Z;
T3 = n33'*R32*Z;

%% 7. EXTRACCION SIMBOLICA DE MATRICES M, V, G
% Ecuacion 1:
M11 = diff(T1, qdd1);
Taux = simplify(T1 - M11*qdd1);
M12 = diff(Taux, qdd2);
Taux = simplify(Taux - M12*qdd2);
G1 = diff(Taux, g)*g;
V1 = simplify(Taux - G1);

% Ecuacion 2:
M21 = diff(T2, qdd1);
Taux = simplify(T2 - M21*qdd1);
M22 = diff(T2, qdd2);
Taux = simplify(Taux - M22*qdd2);
G2 = diff(Taux, g)*g;
V2 = simplify(Taux - G2);

M_brazo = [simplify(M11), simplify(M12); simplify(M21), simplify(M22)];
V_brazo = [simplify(V1); simplify(V2)];
G_brazo = [simplify(G1); simplify(G2)];

fprintf('--- RESULTADOS DEL ALGORITMO NEWTON-EULER (SOLO BRAZO) ---\n');
fprintf('Matriz de Inercia del Brazo M(q):\n');
disp(M_brazo);

fprintf('Vector Coriolis y Centrifugo del Brazo V(q, qd):\n');
disp(V_brazo);

fprintf('Vector Gravitatorio del Brazo G(q):\n');
disp(G_brazo);

%% 8. INCLUSION DE LOS MOTORES DE ACCIONAMIENTO DIRECTO
R_mat  = diag([R1, R2]);
Jm_mat = diag([Jm1, Jm2]);
Bm_mat = diag([Bm1, Bm2]);
Kt_mat = diag([Kt1, Kt2]);

% Matriz de inercia total con motores:
Ma = M_brazo + R_mat * R_mat * Jm_mat;

% Terminos centrípetos, coriolis y friccion viscosa:
Va = V_brazo + R_mat * R_mat * Bm_mat * [qd1; qd2];

% Vector de pares gravitatorios:
Ga = G_brazo;

fprintf('\n--- MATRICES TOTALES CON MOTORES (Ma, Va, Ga) ---\n');
fprintf('Matriz de Inercia Total Ma(q):\n');
disp(Ma);

fprintf('Vector Dinamico Total Va(q, qd) con Friccion Viscosa:\n');
disp(Va);

fprintf('Vector Gravitatorio Total Ga(q):\n');
disp(Ga);

%% 9. MODELO DINAMICO DIRECTO EN INTENSIDADES DE MOTOR (i1, i2)
% En accionamiento directo (r=1): Tau = Kt * i = [Kt1*i1; Kt2*i2]
% Por tanto:
%   Ma(q)*qdd + Va(q, qd) + Ga(q) = Kt * i
%
% Despejando las aceleraciones articulares:
%   qdd = inv(Ma) * ( Kt * [i1; i2] - Va - Ga )

fprintf('\n=========================================================================\n');
fprintf('  MODELO DINAMICO DIRECTO EN INTENSIDADES DE MOTOR:\n');
fprintf('  [qdd1; qdd2] = inv(Ma(q)) * ( [10*i1; 10*i2] - Va(q,qd) - Ga(q) )\n');
fprintf('=========================================================================\n');