% Chapter 8 section 2 Demos

% 8.2 Resolved rate motion control
% Straight-line motion by resolved rate motion control.  It will exhibit
% drift in Cartesian space.
% This controller assumes that discrete integration of
%          qdot = J^{-1} nu_des
% will have no drift, but it will.
mdl_puma560
sl_rrmc
r = sim('sl_rrmc');
t = r.find('tout');
q = r.find('yout');
T = p560.fkine(q);
xyz = transl(T);
% mplot did not work for me
figure;
plot(t, xyz(:,1:3));  % These plots would be linear under perfect control 
figure;
plot(t, q(:,1:3));    % Joint-space plots should curve and then do

% Circle motion using forward kinematics and a virtual spring to correct
% drift, but NOT inv. kin.
sl_rrmc2

% 8.2.1 Singular Jacobian
% If J is singular use a "damped" Jacobian or the pseudo inverse
J = p560.jacob0(qr)
inv(J)
Jinv_d = inv(J + eye(6,6) * 1e-2);
nu_des = ones(6,1);
qdot = Jinv_d * nu_des
% Compare with the minimum error solution using pinv
qdot_pinv = pinv(J) * nu_des

