clear all, clc, close all

% Definición de eslabones: Link([theta d a alpha])
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');
robot_miqueas.plot([0 0 0]); % Muestra el robot en posición inicial

% Definir puntos de inicio y fin (en radianes)
q_inicio = [0, 0, 0];             % Posición de reposo
q_final = [pi/4, -pi/6, pi/3];    % Un punto dentro del espacio de trabajo

% 3. Generar la trayectoria (50 pasos de tiempo)
t = 0:0.05:2;                     % De 0 a 2 segundos
[q, qd, qdd] = jtraj(q_inicio, q_final, t);

% 4. Ejecutar la animación
figure;
robot_miqueas.plot(q, 'trail', 'r-'); % 'trail' dibuja la línea del movimiento

robot_miqueas.teach();