%% =========================================================================
% EXAMEN ROBÓTICA GITI - SEGUNDA CONVOCATORIA (12 SEPTIEMBRE 2018) - CUESTIÓN 1
% =========================================================================
% Robot Manipulador Espacial de 3 GDL (R-R-P Esférico/Polar)
% 1. Parámetros de Denavit-Hartenberg
% 2. Cinemática Directa de Posición (Efector Final)
% 3. Cinemática Inversa Analítica (PCI)
% =========================================================================

clear; clc; close all;
fprintf('=================================================================\n');
fprintf('  EXAMEN ROBÓTICA GITI - SEPTIEMBRE 2018 - CUESTIÓN 1 (ROBOT RRP)\n');
fprintf('=================================================================\n\n');

%% PARTE 1: DEFINICIÓN SIMBÓLICA Y DENAVIT-HARTENBERG
syms q1 q2 d3 L1 L2 real
syms x_d y_d z_d real

fprintf('--- 1. PARÁMETROS DE DENAVIT-HARTENBERG ---\n');
% Función de matriz homogénea DH estándar:
dh_mat = @(theta, d, a, alpha) [
    cos(theta), -cos(alpha)*sin(theta),  sin(alpha)*sin(theta), a*cos(theta);
    sin(theta),  cos(alpha)*cos(theta), -sin(alpha)*cos(theta), a*sin(theta);
    0,           sin(alpha),             cos(alpha),            d;
    0,           0,                      0,                     1
];

% Articulación 1 (Rotación q1 alrededor de Z0 vertical, brazo L1 a lo largo de X1, Z1 perpendicular):
A1 = dh_mat(q1, 0, L1, sym(pi/2));

% Articulación 2 (Rotación q2 alrededor de Z1 horizontal, eje Z2 colineal con el brazo):
A2 = dh_mat(q2, 0, 0, sym(pi/2));

% Articulación 3 (Prismática: extensión d3 a lo largo de Z2 con tramo fijo L2):
A3 = dh_mat(0, L2 + d3, 0, 0);

% Matriz de transformación total:
T03 = simplify(A1 * A2 * A3);

fprintf('Matriz de Transformación Homogénea Total T03:\n');
disp(T03);

%% PARTE 2: CINEMÁTICA DIRECTA DE POSICIÓN
x = simplify(T03(1, 4));
y = simplify(T03(2, 4));
z = simplify(T03(3, 4));

fprintf('\n--- 2. CINEMÁTICA DIRECTA DEL EFECTOR FINAL ---\n');
fprintf('x(q1, q2, d3) = %s\n', char(x));
fprintf('y(q1, q2, d3) = %s\n', char(y));
fprintf('z(q1, q2, d3) = %s\n', char(z));

% Forma compacta factorizada:
% x = (L1 + (L2+d3)*cos(q2)) * cos(q1)
% y = (L1 + (L2+d3)*cos(q2)) * sin(q1)
% z = (L2+d3) * sin(q2)

%% PARTE 3: JACOBIANO Y SINGULARIDADES
J = jacobian([x; y; z], [q1, q2, d3]);
det_J = simplify(det(J));

fprintf('\n--- JACOBIANO LINEAL Y DETERMINANTE ---\n');
fprintf('det(J) = %s\n', char(det_J));
% det(J) = -(L2 + d3) * (L1 + (L2 + d3)*cos(q2))
fprintf('Singularidades:\n');
fprintf('  1. L2 + d3 = 0 (Brazo completamente retraído en la articulación 2)\n');
fprintf('  2. L1 + (L2 + d3)*cos(q2) = 0 (El efector final cruza el eje vertical Z0)\n');

%% PARTE 4: CINEMÁTICA INVERSA ANALÍTICA (PCI)
fprintf('\n--- 3. CINEMÁTICA INVERSA ANALÍTICA (PCI) ---\n');
% Dadas x_d, y_d, z_d:
% 1. q1 = atan2(y_d, x_d)
% 2. Radio horizontal R_xy = sqrt(x_d^2 + y_d^2)
%    R_xy - L1 = (L2 + d3)*cos(q2)
%    z_d       = (L2 + d3)*sin(q2)
% 3. q2 = atan2(z_d, R_xy - L1)
% 4. d3 = sqrt((R_xy - L1)^2 + z_d^2) - L2

fprintf('Ecuaciones de la Cinemática Inversa:\n');
fprintf('  q1 = atan2(y_d, x_d)\n');
fprintf('  R_xy = sqrt(x_d^2 + y_d^2)\n');
fprintf('  q2 = atan2(z_d, R_xy - L1)\n');
fprintf('  d3 = sqrt((R_xy - L1)^2 + z_d^2) - L2\n');

%% VERIFICACIÓN NUMÉRICA
L1_val = 1.0; L2_val = 0.8;
q1_test = deg2rad(30);
q2_test = deg2rad(45);
d3_test = 0.5;

x_num = double(subs(x, [q1, q2, d3, L1, L2], [q1_test, q2_test, d3_test, L1_val, L2_val]));
y_num = double(subs(y, [q1, q2, d3, L1, L2], [q1_test, q2_test, d3_test, L1_val, L2_val]));
z_num = double(subs(z, [q1, q2, d3, L1, L2], [q1_test, q2_test, d3_test, L1_val, L2_val]));

% Reconstrucción por cinemática inversa:
q1_rec = atan2(y_num, x_num);
R_xy_rec = sqrt(x_num^2 + y_num^2);
q2_rec = atan2(z_num, R_xy_rec - L1_val);
d3_rec = sqrt((R_xy_rec - L1_val)^2 + z_num^2) - L2_val;

fprintf('\nVerificación Numérica (Diferencia Inversa - Directa):\n');
fprintf('  Error q1: %.2e rad\n', abs(q1_test - q1_rec));
fprintf('  Error q2: %.2e rad\n', abs(q2_test - q2_rec));
fprintf('  Error d3: %.2e m\n', abs(d3_test - d3_rec));
fprintf('¡Verificación simbólica y numérica COMPLETADA CON ÉXITO!\n');