% Script rmrc.m computes the required joint rates (thetadots) to execute
% the desired Cartesian path specified in MATLAB Exercise 5.  It uses
% function jacob.m to find the Jacobian in frame {0}.  The main portion of
% this script is a loop that runs over the 5 seconds of the trajectory
% using a stepsize of 0.1 second.
%
% In accordance with part (a) of the exercise, the main loop calculates and
% saves (1) [thetadots], (2) [thetas], (3) [x y phi], (4) |J|, and (5)
% [torques] at each time step for subsequent plotting.  A very simple
% integration method (newtheta = oldtheta + thetadot*dt) is adequate.
%
% The saved [thetas} can be used with a line-drawing simulation provided by
% Starr to check your motion path.

RAD = pi/180;   % Conversion factor from degrees->radians

l1 = 4;     % Link 1 length (m)
l2 = 3;     % Link 2 length (m)
l3 = 2;     % Link 3 length (m)

%tf = 5;     % Motion duration (s) This is for the text trajectory
tf = 35;     % Motion duration (s) This is for the "demo"
%dt = 0.1;   % Time step (s)
dt = 0.02;   % Time step (s)  This one gives slower, smoother motion

%V = [0.2; -0.3; -0.2];   % Text Cartesian velocity (m/s), (rad/s)
V = [-0.2; -0.1; -0.2];   % "Demo" Cartesian velocity (m/s), (rad/s)
W = [1; 2; 3];    % Desired Cartesian wrench (N), (N-m)

N = tf/dt+1;  % Number of steps in motion (add one so we start at zero)

% Allocate data arrays for all computed quantities

theta = zeros(N,3);     % Array to hold joint angles (rad)
thetad = zeros(N,3);    % Array to hold joint rates (rad/s)
cart = zeros(N,3);      % Array to hold Cartesian [x y phi]
Jdet = zeros(N,1);      % Array to hold determinant of Jacobian
torque = zeros(N,3);    % Array to hold joint torques (N-m)
condition = zeros(N,1); % Array to hold condition numbers of J

theta(1,:) = [10*RAD 20*RAD 30*RAD];   % Initial joint angles in radians

for i = 1:N     % Main loop over Cartesian motion
    cart(i,:) = kin(theta(i,:));    % Get current Cartesian position
    J = jacob(theta(i,:));  % Compute Jacobian at current position
    Jdet(i) = det(J);       % Compute determinant of Jacobian
    condition(i) = cond(J); % Compute condition number of Jacobian
    thetad(i,:) = inv(J)*V;    % Find joint rates
    torque(i,:) = J'*W;     % Find static joint torques
    theta(i+1,:) = theta(i,:)+thetad(i,:)*dt;   % Update joint angles
end

theta = theta(1:N,:);   % Eliminate last theta (one too far)
t = [0:dt:tf]';     % Time vector for subsequent plotting