% % Chapter 8 section 1 Demos
% mdl_puma560
% T0 = p560.fkine(qn)
% R0 = T0(1:3,1:3)
% dq = 1e-6
% p560.plot(qn)
% 
% % Increment joint 1 and numerically estimate the corresponding column of
% % the Jacobian
% Tp = p560.fkine(qn + [1 0 0 0 0 0]*dq)
% dTdq1 = (Tp - T0)/dq   % Change in T is not in SE(3)
% dRdq1 = dTdq1(1:3,1:3)  % Change in R is not in SO(3)
% S = dRdq1 * R0'
% jac1 = [dTdq1(1:3,4); vex(S)]
% 
% p560
% T1 = trotz(qn(1))*trotx(pi/2);
% hold on
% trplot(T1)
% 
% % Increment joint 2
% Tp = p560.fkine(qn + [0 1 0 0 0 0]*dq);
% dTdq2 = (Tp - T0)/dq;
% dRdq2 = dTdq2(1:3,1:3);
% S = dRdq2 * R0';
% jac2 = [dTdq2(1:3,4); vex(S)]
% 
% T2 = T1 * trotz(qn(2))*transl(0.4318);
% trplot(T1)
% 
% % Increment joint 3
% Tp = p560.fkine(qn + [0 0 1 0 0 0]*dq);
% dTdq3 = (Tp - T0)/dq;
% dRdq3 = dTdq3(1:3,1:3);
% S = dRdq3 * R0';
% jac3 = [dTdq3(1:3,4); vex(S)]
% 
% % Increment joint 4
% Tp = p560.fkine(qn + [0 0 0 1 0 0]*dq);
% dTdq4 = (Tp - T0)/dq;
% dRdq4 = dTdq4(1:3,1:3);
% S = dRdq4 * R0';
% jac4 = [dTdq4(1:3,4); vex(S)]
% 
% % Increment joint 5
% Tp = p560.fkine(qn + [0 0 0 0 1 0]*dq);
% dTdq5 = (Tp - T0)/dq;
% dRdq5 = dTdq5(1:3,1:3);
% S = dRdq5 * R0';
% jac5 = [dTdq5(1:3,4); vex(S)]
% 
% % Increment joint 6
% Tp = p560.fkine(qn + [0 0 0 0 0 1]*dq);
% dTdq6 = (Tp - T0)/dq;
% dRdq6 = dTdq6(1:3,1:3);
% S = dRdq6 * R0';
% jac6 = [dTdq6(1:3,4); vex(S)]
% 
% % Notice, translation is affected by only first 3 joints.
% % Rotation is affected by all joints.
% jac = [jac1 jac2 jac3 jac4 jac5 jac6]
% jacPos = jac(1:3,:)
% jacPhi = jac(4:6,:)
% 
% % Compare with toolbox built-in Jacobian function
% norm(p560.jacob0(qn) - jac)

% % 8.1.1 tr2jac(T) - Transforming twists
% close all;
% offset = [2; 0; 0];
% ATB = transl(offset);
% hndl = trplot(eye(4), 'axis', [-1 3 -2 2 -2 2]); hold on;
% trplot(ATB, 'color', 'r');
% BJnuA = tr2jac(ATB)
% scale = 0.2;
% % Choose twist to be translation only.
% Anu = [1;2;3;0;0;0];
% hvA = line([0 Anu(1)*scale], ...
%            [0 Anu(2)*scale], ...
%            [0 Anu(3)*scale], ...
%            'color', 'g', 'lineWidth', 3);
% Bnu = BJnuA * Anu;   % Velocity does not change
% hvB = line([offset(1) Bnu(1)*scale+offset(1)], ...
%           [offset(2) Bnu(2)*scale+offset(2)], ...
%           [offset(3) Bnu(3)*scale+offset(3)], ...
%           'color', 'g', 'lineWidth', 3);
%       
% % Add rotational velocity and notice the effect on translational velocity
% Anu = [1;2;3;0;0;4];
% hwA = line([0 Anu(4)*scale], ...
%            [0 Anu(5)*scale], ...
%            [0 Anu(6)*scale], ...
%            'color', 'm', 'lineWidth', 3);
% Bnu = BJnuA * Anu;   % Rotational velocity generally changes transl. vel.
% hwB = line([offset(1) Bnu(4)*scale+offset(1)], ...
%            [offset(2) Bnu(5)*scale+offset(2)], ...
%            [offset(3) Bnu(6)*scale+offset(3)], ...
%            'color', 'm', 'lineWidth', 3);
% set(hvB, 'XData', [offset(1) Bnu(1)*scale+offset(1)], ...
%          'YData', [offset(2) Bnu(2)*scale+offset(2)], ...
%          'ZData', [offset(3) Bnu(3)*scale+offset(3)]);
%  
% Anu = [1;2;3;4;0;0]
% set(hwA, 'XData', [0 Anu(4)*scale], ...
%          'YData', [0 Anu(5)*scale], ...
%          'ZData', [0 Anu(6)*scale]);
% Bnu = BJnuA * Anu;   % Rotational velocity doesn't always change transl. v
% set(hwB, 'XData', [offset(1) Bnu(4)*scale+offset(1)], ...
%          'YData', [offset(2) Bnu(5)*scale+offset(2)], ...
%          'ZData', [offset(3) Bnu(6)*scale+offset(3)]);
% set(hvB, 'XData', [offset(1) Bnu(1)*scale+offset(1)], ...
%          'YData', [offset(2) Bnu(2)*scale+offset(2)], ...
%          'ZData', [offset(3) Bnu(3)*scale+offset(3)]);
% 
% % Transform one Jacobian to another
% J0 = p560.jacob0(qn)  % Get J in {0}
% Jn = p560.jacobn(qn)  % Get J in {N}
% % Text error - I talked to Peter.  He'll fix it in the next edition.
% % Middle equation of the three just above start of section 8.1.3 (page 176)
% Jnu = tr2jac(T0);
% norm(Jnu * Jn - J0)  % norm not equal to zero!!
% % The correct transform
% RR = blkdiag(R0, R0)
% norm(RR*Jn - J0)

% 8.1.3 Symbolic derivation of d/dt(RPY) to ang velocity
close all
syms thr    thp    thy    % theta_roll, theta_pitch, and theta_yaw
syms thrdot thpdot thydot
R = rotx(thr) * roty(thp) * rotz(thy)
R1dot = diff(R, thr) * thrdot
R2dot = diff(R, thp) * thpdot
R3dot = diff(R, thy) * thydot
Rdot = R1dot + R2dot + R3dot
S = simple(Rdot * R.');  % " .' " is the non-congugate transpose operator 
omegax = S(3,2)
omegay = S(1,3)
omegaz = S(2,1)

% %8.1.4 Jacobian Condition and Manipulability
% mdl_puma560
% J = p560.jacob0(qr)    %qr reaches out far
% p560.plot(qr);
% detJ = det(J)  % qr is a singular configuration
% fprintf('Singluar --> not all twists are possible.\n')
% rankJ = rank(J) % at qr, p560 loses one degree of freedom
% fprintf('Since rank is 5, only a 5D subspace of twists is possible.\n')
% % Examine J
% J
% fprintf('Interpret the nonzero values in J:\n')
% fprintf('   omega_x = 0.\n')
% fprintf('   joint 2 controls v_y, but row is almost 0.  Need big q2dot!\n')
% fprintf('   joints 2,3 control v_z, again, row is almost zero.\n')
% fprintf('   joints 1,2,3 control v_x.\n')
% fprintf('   joints 2,3,5 control omega_y.\n')
% fprintf('   joints 1,4,6 control omega_z.\n\n')
% fprintf('Two columns of J are identical!\n')
% fprintf('Rotations about these joints both affect z-angular velocity.\n\n')
% % Examine pinv(J)
% pinvJ = pinv(J) 
% fprintf('Interpret pinv(J):\n')
% fprintf('   large values in cols 2,3 --> desired v_y and v_z require large joint rates.\n')
% fprintf('   demanded motions in v_x, omega_y and omega_z are easy to achieve.\n')
% fprintf('   4th col of pinv(J) is zero --> demanded omega_x is not obeyed.\n');
% % Examine null(J)
% nullJ = null(J)
% fprintf('Interpret null(J):\n')
% fprintf('Null space is 1D.  A 1D subspace in joint space causes no motion of {E}.\n')
% fprintf('Move jnts 4 and 6 at same rate in opposite directions --> {E} is constant.\n')
% q = qr;
% q5 = pi/180 * 1;  % 1 degree away from the singularity
% q(5) = q5;
% J = p560.jacob0(q)
% rankJ = rank(J)
% detJ = det(J)
% condJ = cond(J)
% Jinv = inv(J)
% % Rotate tool about y world axis - no problem.
% nu = [0; 0; 0; 0; 1; 0]/100
% qdot = Jinv * nu
% % Translate tool in y world direction - problem, but there are some offsets
% % away from the wrist that allow y translation even though large joint
% % rates are required.
% nu = [0; 1; 0; 0; 0; 0]/100
% qdot = Jinv * nu
% % Rotate tool about x world axis - problem!  But problem is same magnitude
% % as previous.
% nu = [0; 0; 0; 1; 0; 0]/100
% qdot = Jinv * nu
% 
% % Interestingly, the problem about the x-axis increases, but the others
% % don't
% q5 = pi/180 * 0.01;  % 0.01 degree away from the singularity
% q(5) = q5;
% J = p560.jacob0(q)
% rankJ = rank(J)
% detJ = det(J)
% condJ = cond(J)
% Jinv = inv(J)
% % Rotate tool about y world axis - no problem.
% nu = [0; 0; 0; 0; 1; 0]/100
% qdot = Jinv * nu
% % Translate tool in y world direction - same joint speeds as needed above
% nu = [0; 1; 0; 0; 0; 0]/100
% qdot = Jinv * nu
% % Rotate tool about x world axis - bigger problem!  Need much bigger joint
% % rates.  Note that there is no axis pointing largely in the x-direction!
% nu = [0; 0; 0; 1; 0; 0]/100
% qdot = Jinv * nu

% % Manipulability ellipsoid in a pretty good configuration
% mdl_puma560
% J = p560.jacob0(qn);
% p560.plot(qn)
% hold on; axis equal;
% T = p560.fkine(qn);
% center = T(1:3,4);
% Jpos = J(1:3,:);
% plot_ellipse(Jpos*Jpos'/50, center);
% Jorient = J(4:6,:);
% figure
% p560.plot(qn)
% hold on; axis equal;
% plot_ellipse(Jorient*Jorient'/500, center);
% 
% % Manipulability ellipsoids in a nearly singular configuration
% q = qr;
% q(5) = pi/180;
% qbad = q;
% J = p560.jacob0(qbad);
% T = p560.fkine(qbad);
% Jpos = J(1:3,:);
% Jorient = J(4:6,:);
% center = T(1:3,4);
% figure
% p560.plot(qbad)
% hold on; axis equal;
% plot_ellipse(Jorient*Jorient'/500, center);
% fprintf('Angular velocity is degenerate in one direction. \n');
% figure
% p560.plot(qbad)
% hold on; axis equal;
% plot_ellipse(Jpos*Jpos'/50, center);
% fprintf('Translational velocity is degenerate in two directions. \n');
% 
% % Toolbox manipulability measure is Yoshikawa's
% p560.maniplty(qn,'yoshikawa')
% p560.maniplty(qr,'yoshikawa')
% p560.maniplty(qz,'yoshikawa')


% % 2R Planar Manipulator
% clear all; close all;
% syms q1 q2 l1 l2
% 
% fKin = [l1*cos(q1) + l2*cos(q1+q2);
%         l1*sin(q1) + l2*sin(q1+q2)]
% % Jacobian of end effector in {W}
% JW = [-l1*sin(q1) - l2*sin(q1+q2)     -l2*sin(q1+q2);
%        l1*cos(q1) + l2*cos(q1+q2)      l2*cos(q1+q2)]
% singW = simple(det(JW))  
% 
% R = [cos(q1+q2)  -sin(q1+q2);
%      sin(q1+q2)   cos(q1+q2)]
% JE = R * JW
% singE = simple(det(JE))  

% Some numerical examples
clear all; close all;
mdl_twolink
twolink.plot(qz)
l1 = 1; l2 = 1; q1 = -pi/4;

q2 = 0 : 0.01 : pi;
manipW = zeros(length(q2), 1);
manipE = zeros(length(q2), 1);
eigBig = zeros(length(q2), 1);
eigLow = zeros(length(q2), 1);
eigRatio = zeros(length(q2), 1);
condJWinv = zeros(length(q2), 1);
for i = 1 : length(q2)
    fKin = [l1*cos(q1) + l2*cos(q1+q2(i));
            l1*sin(q1) + l2*sin(q1+q2(i))];
    JW = [-l1*sin(q1) - l2*sin(q1+q2(i))     -l2*sin(q1+q2(i));
           l1*cos(q1) + l2*cos(q1+q2(i))      l2*cos(q1+q2(i))];
    JE = [l1*sin(q2(i))          0;
          l1*cos(q2(i)) + l2    l2];
    [eVecW, eValW] = eig(JW*JW');
    manipW(i) = sqrt(det(JW*JW'));
    eigBig(i) = max(eValW(1,1), eValW(2,2));
    eigLow(i) = min(eValW(1,1), eValW(2,2));
    eigRatio(i) = eigLow(i) / eigBig(i);
    [eVecE, eValE] = eig(JE*JE');
    manipE(i) = sqrt(det(JE*JE'));
%     if q2(i) > 1.565 && q2(i) < 1.575
%         pause;
%     end
end    
figure;
plot(q2, manipE)
figure;
plot(q2, manipW, q2, eigBig, q2, eigLow, q2, eigRatio)
legend('manipW', 'eigBig', 'eigLow', 'Low/Big');
title('Manipulability and Eigenvalues vs Elbow Angle');
[val, ind] = max(eigRatio);
twolink.plot([1 q2(ind)]);


% l1 = 0.2;
% for i = 1 : length(q2)
%     JW = [-l1*sin(q1) - l2*sin(q1+q2(i))     -l2*sin(q1+q2(i));
%            l1*cos(q1) + l2*cos(q1+q2(i))      l2*cos(q1+q2(i))];
%     JE = [l1*sin(q2(i))          0;
%           l1*cos(q2(i)) + l2    l2];
%     [eVecW, eValW] = eig(JW*JW');
%     manipW(i) = sqrt(det(JW*JW'));
%     eigBig(i) = max(eValW(1,1), eValW(2,2));
%     eigLow(i) = min(eValW(1,1), eValW(2,2));
%     eigRatio(i) = eigLow(i) / eigBig(i);
%     condJWinv(i) = 1 / cond(JW);
% end    
% figure;
% plot(q2, manipW, q2, eigBig, q2, eigLow, q2, eigRatio, q2, condJWinv)
% legend('manipW', 'eigBig', 'eigLow', 'Low/Big', 'condJWinv');
% title('Manipulability and Eigenvalues vs Elbow Angle');

% RPR Planar Manipulator
close all; clear all;
d = 2;  f = 0.5;
L(1) = Link([0, 0, 0, 0, 0]);
L(2) = Link([0, 0, d, -pi/2, 0]);
L(3) = Link([0, 0, 0,  pi/2, 1]);
L(4) = Link([0, 0, f, 0, 0]);
rpr = SerialLink(L, 'name', 'rpr')

q0 = [0 : 0.05 : 1]' * 0.0;   % I put this in only because first link doesn't render
q1 = [0 : 0.05 : 1]' * 5 - 2.5;
q2 = [0 : 0.05 : 1]' * 4 - 2;
q3 = [0 : 0.05 : 1]' * 5 - 2.5;
figure;
rpr.plot([q0 q1 q0 q0]);
rpr.plot([q0 q0 q2 q0]);
rpr.plot([q0 q0 q0 q3]);

% Derive some things symbolically
syms q1 q2 q3 d f

% Don't know if I can do this symbolically with the toolkit
fKin = [cos(q1+q3),   -sin(q1+q3),   d*cos(q1) - q2*sin(q1) + f*cos(q1+q3);
        sin(q1+q3),    cos(q1+q3),   d*sin(q1) + q2*cos(q1) - f*sin(q1+q3);
           q1,              0,                        q3]
JW = [-d*sin(q1) - q2*cos(q1) - f*sin(q1+q3),  -sin(q1),  -f*sin(q1+q3);
       d*cos(q1) - q2*sin(q1) - f*cos(q1+q3),   cos(q1),  -f*cos(q1+q3);
                1,                                 0,           1]
            
JWp = JW(1:2, :)
JWp - jacobian(fKin(1:2,3),[q1 q2 q3])  % Just check my math

simple(det(JW))  % = -q2

% Now back to numerical calculations
d = 2;
f = 0.5;
q1 = [0 : 0.1 : 1]' * 2.5;
q2 = [0 : 0.1 : 1]' * 2;
q3 = 0;
num = length(q1);
rpr.plot([0, q1(1),q2(1),q3]);
for i = 1:num
    for j = 1:num
        fKin = [cos(q1(i)+q3),   -sin(q1(i)+q3),  d*cos(q1(i)) - q2(j)*sin(q1(i)) + f*cos(q1(i)+q3);
                sin(q1(i)+q3),    cos(q1(i)+q3),  d*sin(q1(i)) + q2(j)*cos(q1(i)) - f*sin(q1(i)+q3);
                   q1(i),              0,                        q3];
        JW = [-d*sin(q1(i)) - q2(j)*cos(q1(i)) - f*sin(q1(i)+q3),  -sin(q1(i)),  -f*sin(q1(i)+q3);
               d*cos(q1(i)) - q2(j)*sin(q1(i)) - f*cos(q1(i)+q3),   cos(q1(i)),  -f*cos(q1(i)+q3);
                            1,                       0,           1];
        JWp = JW(1:2, :);
        center = fKin(1:2,3);
        plot_ellipse(JWp*JWp'/400, center);
    end
end

% Derive Jacobian for a Kuka arm
close all; clear all;
a1 = 0.2
a2 = 1.0;
d4 = 0.7;
alpha1 = -pi/2;
alpha3 = pi/2;
alpha4 = pi/2;
alpha5 = pi/2;

L(1) = Link([0, 0, a1, -pi/2]);  
L(2) = Link([0, 0, a2,  0]);
L(3) = Link([0, 0,  0,  pi/2]);
L(4) = Link([0, d4, 0,  pi/2]);
L(5) = Link([0,  0, 0, -pi/2]);
L(6) = Link([0,  0, 0,  0]);
kuka = SerialLink(L, 'name', 'Kuka')
kuka.plot(zeros(1,6)); hold on;

% Generate a trajectory using Jacobian
% get starting configurations
pos = [0.7, 0.0, 1.5];
q0 = kuka.ikine(transl(pos));
dir = [-1,1,-1];
dir = dir/norm(dir);
step = 0.2;
% Notice the large non-linear affect near the end of the trajectory where
% the arm is nearly singular.  The 3rd joint over-shoots a couple times at
% the end of the trajectory.  The ellipsoid gets thin in the direction of
% the workspace boundary.
q = q0;
for i = 1:20
    kuka.plot(q);
    J = kuka.jacob0(q);
    Jpos = J(1:3, :);
    cond(Jpos)
    T = kuka.fkine(q);
    plot_ellipse(Jpos*Jpos'/50, T(1:3, 4));
    delq = J \ [dir * step  0 0 0]';
    q = q + delq';
end
% Now go through the same thing with the orienation singularities.  Note
% orientation manipulability is bad in the first config and gets bad again
% in the center.
figure;
q = q0;
for i = 1:20
    kuka.plot(q);
    J = kuka.jacob0(q);
    Jorient = J(4:6, :);
    cond(Jorient)
    T = kuka.fkine(q);
    plot_ellipse(Jorient*Jorient'/50, T(1:3, 4));
    delq = J \ [dir * step  0 0 0]';
    q = q + delq';
end

