% Chapter 7 Section 2 Demos
close all; clear q L;
L(1) = Link([0, 0, 1, 0]);
L(2) = Link([0, 0, 1, 0]);
twoR = SerialLink(L, 'name', '2Rplanar')  % Instantiate 2R Planar robot

mdl_twolink  % This does the same thing as above statements.
twolink.links
twolink.fkine([0,0])  % Fwd kin map with joint displacements = 0
twolink.fkine([0.2, 1.1]) % Fwd kin map with joint displacements = nonzero
twolink.plot([0,0])    % Draw a link in a particular config
twolink.plot([0.2, 1.1])  % Draw a link in a particular config
q1 = [0 : 0.1 : 1]'    % Prepare to simulate
q2 = [0 : 0.1 : 1]' * 2   % Second joint moves twice as fast
q = [q1  q2]
plot(q)
figure;
twolink.plot(q)  % Animate
T = twolink.fkine(q)   % End effector configs during trajectory
about(T)
T5 = T(:,:,5)

% Create 3R planar robot
close all; clear q L;
L(1) = Link([0, 0, 1, 0]);
L(2) = Link([0, 0, 1, 0]);
L(3) = Link([0, 0, 1, 0]);
threeR = SerialLink(L, 'name', '3Rplanar')

q1 = [0 : 0.1 : 1]' * 0.5;
q2 = [0 : 0.1 : 1]' * 0.9;
q3 = [0 : 0.1 : 1]' * 1.3;
q = [q1 q2 q3];
plot(q)
figure;
threeR.plot(q)
T = threeR.fkine(q)  % End effector configs during trajectory

% Puma 560
close all; clear q L;
mdl_puma560
p560    % Notice that frames {4}, {5} and {6} have coincident origins
p560.plot(qz) % q = zeros(6,1)
p560.plot(qr) % q = [0, pi/2 -pi/2, 0, 0, 0]
p560.plot(qs)
p560.plot(qn)
p560.plot(rand(1,6))
t = (0:0.1:2)';
q1 = sin(pi*t);
q2 = cos(pi*t);
q3 = -sin(pi*t);
q4 = -cos(pi*t);
q5 = cos(pi/2*t);
q6 = sin(pi/2*t);
q = [q1 q2 q3 q4 q5 q6]
p560.plot(q)

% modular Schunk arm with gripper
close all; clear q L;
L(1) = Link([0, 0.00, 0.00, pi/2]);
L(2) = Link([0, 0.00, 0.27, 0]);
L(3) = Link([0, 0.00, 0.18, pi/2]);
L(4) = Link([0, 0.00, 0.00, pi/2]);
L(5) = Link([0, 0.30, 0.00, pi/2]);
L(6) = Link([0, 0.00, 0.00, pi/2]);
L(7) = Link([0, 0.38, 0.00, pi/2]);
Schunk = SerialLink(L, 'name', 'schunk') 
Schunk.plot(rand(1,7))
t = (0:0.1:2)';
q1 = sin(pi*t);
q2 = cos(pi*t);
q3 = -sin(pi*t);
q4 = -cos(pi*t);
q5 = cos(pi/2*t);
q6 = sin(pi/2*t);
q7 = sin(pi/2*t);
q = [q1 q2 q3 q4 q5 q6 q7]
Schunk.plot(q)
