% Chapter 7 Section 3 Demos

% 7.3.1 Closed form solutions
mdl_puma560
p560   
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) % q = [0, pi/4, -pi, 0, pi/4, 0]

% Solve the inverse kinematics problem
p560_2 = SerialLink(p560, 'name', 'puma#2');
Target = p560_2.fkine(qn)
qTarget = qn
qout = p560_2.ikine6s(Target)
figure;
p560_2.plot(qout)  
% Note that this solution is not = qn!!

% Force the solver to return a particular solution
qout = p560_2.ikine6s(Target, 'rdn');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'rdf');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'run');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'ruf');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'ldn');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'ldf');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'lun');
p560_2.plot(qout);

qout = p560_2.ikine6s(Target, 'luf');
p560_2.plot(qout)

% No solution
p560_2.ikine6s(transl(3,0,0))
p560_2.ikine6s(transl(0,0,0))


% 7.3.2 Numerical solutions
% Numerical solution is slow, but is more flexible
fprintf('\nTime for 10 numerical solutions is:  ')
tic;
for i = 1:10
    Target = p560.fkine(qn + 0.1 * (rand(1,6)-0.5));
    p560.ikine(Target);
end
toc;

fprintf('\nTime for 10 closed-form solutins is:  ')
tic;
for i = 1:10
    Target = p560.fkine(qn + 0.9 * (rand(1,6)-0.5));
    p560.ikine6s(Target);
end
toc;

% Guess affects solution time
Target = p560.fkine(qn)
q_guess = qn;
fprintf('\n\nTime with error = 1e-9 is:  ')
tic;
for i = 1:10
    p560.ikine(Target, q_guess + rand(1,6) * 1e-9);
end
toc;

fprintf('\nTime with error = 1e-7 is:  ')
tic;
for i = 1:10
    p560.ikine(Target, q_guess + rand(1,6) * 1e-7);
end
toc;

fprintf('\nTime with error = 1e-5 is:  ')
tic;
for i = 1:10
    p560.ikine(Target, q_guess + rand(1,6) * 1e-5);
end
toc;

fprintf('\nTime with error = 1e-3 is:  ')
tic;
for i = 1:10
    p560.ikine(Target, q_guess + rand(1,6) * 1e-3);
end
toc;
% Still 200 times slower even with a guess that is within 0.1%


% 7.3.3 Inv. Kin. for Under-Actuated Manipulators
close all;
mdl_twolink
twolink.display
Target = transl([1,0.4,3]);
q_guess = [0 0];
target_mask = [1 1 0 0 0 0];  % [x, y, z, thx, thy, thz]
q = twolink.ikine(Target, q_guess, target_mask)
twolink.plot(q)

Target = transl([1,2,3]);  % Point outside workspace
target_mask = [1 1 0 0 0 0];  % [x, y, z, thx, thy, thz]
q = twolink.ikine(Target, q_guess, target_mask)
twolink.plot(q)

Target = transl([1e-8, 1e-8, 0]);  % Point at origin - non unique soln
target_mask = [1 1 0 0 0 0];  % [x, y, z, thx, thy, thz]
q = twolink.ikine(Target, zeros(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, [pi/4 + .1  pi + .1], target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask) % Sometimes get warning
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)

Target = transl([1,0.4,1]) * trotz(pi/4);
target_mask = [1 0 0 0 0 1];  % [x, y, z, thx, thy, thz]
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)
q = twolink.ikine(Target, rand(1,2), target_mask)
twolink.plot(q)


% 7.3.4 Inv. Kin. for Redundant manipulators
close all;
mdl_puma560
platform = SerialLink( [0 0 0 -pi/2 1;  -pi/2 0 0 pi/2 1], ...
    'base', troty(pi/2), 'name', 'platform' );
platform.fkine([1 2])  % The platform is in the (x,y)-plane

% Mount Puma on (x,y)-platform
p8 = SerialLink( [platform,  p560] )  % But the shoulder is too low
p560.links(1).d = 0.3   % Lift the p560 30 cm above the base
p8 = SerialLink( [platform,  p560], 'name', 'P8' )

Target = transl(0.5, 0.4, 0.5) * rpy2tr(0, 3*pi/4, 0)
qout = p560.ikine(Target)  % p560 without base cannot reach achieve the pose
figure;
p8.plot([0 0 qout])
figure;
qout = p8.ikine(Target)   % p8 can reach it
p8.plot(qout)
tform_norm = norm(Target - p8.fkine(qout))
% Make target unreachable by the p560
Target = transl(0.5, 1.4, 0.5) * rpy2tr(0, 3*pi/4, 0)
qout = p560.ikine(Target)  % p560 without base cannot reach achieve the pose
qout = p8.ikine(Target)   % p8 can reach it
p8.plot(qout)





