clear all, clc, close all

% 1. Configuración del Robot (tus parámetros actualizados)
L1 = Link([0, 0.104, 0, pi/2]); 
L2 = Link([0, 0, 0.120, 0]);
L3 = Link([0, 0, 0.235, 0]);
robot_miqueas = SerialLink([L1 L2 L3], 'name', 'Brazo Articulado');

% 2. Definir Puntos Cartesianos (en metros)
% Punto A: Estirado hacia adelante
T1 = transl(0.3, 0, 0.15); 

% Punto B: Desplazado lateralmente y más bajo
T2 = transl(0.1, 0.25, 0.05); 

% 3. Resolver Cinemática Inversa para los puntos extremos
% Usamos 'mask' porque tenemos 3 grados de libertad (solo controlamos X, Y, Z)
q1 = robot_miqueas.ikine(T1, 'mask', [1 1 1 0 0 0]);
q2 = robot_miqueas.ikine(T2, 'mask', [1 1 1 0 0 0]);

% 4. Generar Trayectoria Suave (Joint Space)
% 100 pasos en 4 segundos para que sea muy fluido
t = linspace(0, 4, 100); 
q_trayectoria = jtraj(q1, q2, t);

% 5. Animación con visualización de trayectoria
figure('Name', 'Simulación Cinemática Inversa');
robot_miqueas.plot(q_trayectoria, 'trail', 'b-', 'movie', 'robot_movimiento.mp4');

% Mostrar coordenadas reales inicial y final
T_inicial = robot_miqueas.fkine(q1);
T_final = robot_miqueas.fkine(q2);
fprintf('Coordenadas iniciales y finales alcanzadas:\n Xi: %.3f, Yi: %.3f, Zi: %.3f\n Xf: %.3f, Yf: %.3f, Zf: %.3f\n', T_inicial.t, T_final.t);