% Chapter 7 Section 4 Demos
close all

% 7.4.1 Joint-Space Motion
mdl_puma560
T1 = transl(0.4,  0.2, 0) * trotx(pi);
T2 = transl(0.4, -0.2, 0) * trotx(pi/2);
% Get inv. kin. solution for the start and end poses
q1 = p560.ikine6s(T1);
q2 = p560.ikine6s(T2);
% Execute motion over 2 seconds in 50 millisecond intervals
t = [0 : 0.05 : 2]';
% Get multi-dimensional trajectories for all joints at once.
q = mtraj(@tpoly, q1, q2, t); % t can be a time vector or number of steps
q = mtraj(@lspb, q1, q2, t);
[q, qd, qdd] = jtraj(q1, q2, t); % Short-hand for mtraj(@tpoly, q1, q2, t)
figure;
plot(t, q);
title('Joint Positions vs Time');
legend('q1', 'q2', 'q3', 'q4', 'q5', 'q6');
figure;
plot(t, qd);
title('Joint Velocities vs Time');
legend('qd1', 'qd2', 'qd3', 'qd4', 'qd5', 'qd6');
figure;
plot(t, qdd);
title('Joint Accelerations vs Time');
legend('qdd1', 'qdd2', 'qdd3', 'qdd4', 'qdd5', 'qdd6');

% More concise statement...
% Not same as last.  x-motion was added.
T2 = transl(-0.4, -0.2, 0.7) * trotx(-pi/2);
q = p560.jtraj(T1, T2, t); % Specify start and end poses
figure;
plot(t, q);
title('Joint Positions vs Time');
legend('q1', 'q2', 'q3', 'q4', 'q5', 'q6');
p560.plot(q);


% 7.4.2 Cartesian-Space Motion
T2 = transl(-0.4, -0.2, 0.7) * trotx(-pi/2);
Ts = ctraj(T1, T2, length(t)); % generate Cartesian trajectory
figure;
p = transl(Ts);
plot3(p(:,1), p(:,2), p(:,3));
title('Path of End-Effector');
figure;
plot(t, p);
title('Cartesian Position vs Time');
legend('x', 'y', 'z');
figure;
plot(t, tr2rpy(Ts));
title('Cartesian Orientation Using RPY vs Time');
legend('roll', 'pitch', 'yaw');
figure;
plot(t, tr2eul(Ts));
title('Cartesian Orientation Using ZYZ vs Time');
legend('rotz', 'roty', 'rotz2');
% Use inv kin to generate a joint space trajectory
% This motion is impossible - note imaginary numbers
qc = p560.ikine6s(Ts); 

close all
T2 = transl(0.5, 0.2, 0.65) * trotx(pi/2) * troty(pi/2);
Ts = ctraj(T1, T2, length(t)); % generate Cartesian trajectory
qc = p560.ikine6s(Ts);
p560.plot(qc);
figure;
p = transl(Ts);
plot3(p(:,1), p(:,2), p(:,3));
title('Path of End-Effector');
figure;
plot(t, p);
title('Cartesian Position vs Time');
legend('x', 'y', 'z');
figure;
plot(t, tr2rpy(Ts));
title('Cartesian Orientation Using RPY vs Time');
legend('roll', 'pitch', 'yaw');


% 7.4.3 Cartesian-Space Motion through a Singularity
close all
T1 = transl(0.5,  0.57, 0.44) * troty(pi/2);
T2 = transl(0.5, -0.57, 0.44) * troty(pi/2);
t = [0 : 0.01 : 2]';
Ts = ctraj(T1, T2, length(t)); % generate Cartesian trajectory
qc = p560.ikine6s(Ts);
p560.plot(qc);
figure;
p = transl(Ts);
plot3(p(:,1), p(:,2), p(:,3));
title('Path of End-Effector');
figure;
plot(t, p);
title('Cartesian Position vs Time');
legend('x', 'y', 'z');
figure;
plot(t, tr2rpy(Ts));
title('RPY Orientation vs Time');
legend('roll', 'pitch', 'yaw');
% Everything looks fine, but what about in joint space?
plot(t,qc)
title('Joint Positions through a Wrist Singularity vs Time');
legend('q1', 'q2', 'q3', 'q4', 'q5', 'q6');








