%% Simulación Puma 560: Pick-and-Place con plot3d
clear; clc; close all;

% 1. Cargar el modelo del Puma 560
mdl_puma560;

% 2. Definir la posición de la pelotita (en metros)
pos_pelota = [0.5, 0.4, 0.2]; 

% 3. Crear la figura y graficar la pelotita
figure;
hold on;
view(3);
grid on;
axis([-1 1 -1 1 -0.5 1.5]);

% Dibujar una esfera simple para representar la pelotita
[sx, sy, sz] = sphere(20);
radius = 0.03;
h_ball = surf(sx*radius + pos_pelota(1), sy*radius + pos_pelota(2), sz*radius + pos_pelota(3));
set(h_ball, 'FaceColor', 'yellow', 'EdgeColor', 'none');
camlight; lighting gouraud;

% 4. Definir trayectorias
% T1: Matriz de transformación para la posición de la pelota (orientación hacia abajo)
T_pick = transl(pos_pelota) * troty(pi); 

% q_start: posición inicial (reposo)
q_start = qz; 

% q_pick: cinemática inversa para llegar a la pelota
q_pick = p560.ikine6s(T_pick);

% Generar trayectoria suave (50 pasos)
t = [0:0.05:2]';
q_path = jtraj(q_start, q_pick, t);

% 5. Animación con plot3d
% Nota: plot3d requiere que el toolbox tenga instalados los archivos de malla (STL)
try
    p560.plot3d(q_path, 'view', 'iso', 'fps', 30, 'noshadow');
catch
    fprintf('Asegúrate de tener instaladas las mallas para plot3d. Usando plot() normal.\n');
    p560.plot(q_path);
end

% 6. Movimiento de retorno con la pelotita (Simulación visual)
% Calculamos una trayectoria de regreso
q_path_back = jtraj(q_pick, q_start, t);

for i=1:length(q_path_back)
    % Actualizar robot
    p560.animate(q_path_back(i,:));
    
    % Actualizar posición de la pelota para que siga al efector final
    T_end = p560.fkine(q_path_back(i,:));
    pos_actual = T_end.t; % Extraer traslación
    
    set(h_ball, 'XData', sx*radius + pos_actual(1), ...
                'YData', sy*radius + pos_actual(2), ...
                'ZData', sz*radius + pos_actual(3));
    pause(0.01);
end