%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist)
% Takes thetalist: A list of joint variables,
%       Ftip: Spatial force applied by the end-effector expressed in frame 
%             {n+1},
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns JTFtip: The joint forces and torques required only to create the 
%                 end-effector force Ftip.
% This function calls InverseDynamics with g = 0, dthetalist = 0, and 
% ddthetalist = 0.
% Example Input (3 Link Robot):
%{
  clear;clc;
  thetalist = [0.1; 0.1; 0.1];
  Ftip = [1; 1; 1; 1; 1; 1];
  M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
  M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
  M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
  M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
  G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
  G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
  G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
  Glist = cat(3,G1,G2,G3);
  Mlist = cat(4,M01,M12,M23,M34); 
  Slist = [[1; 0; 1;      0; 1;     0], ...
           [0; 1; 0; -0.089; 0;     0], ...
           [0; 1; 0; -0.089; 0; 0.425]];
  JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist)
%}
% Output:
% JTFtip =
%    1.4095
%    1.8577
%    1.3924

n = size(thetalist,1);
JTFtip = InverseDynamics(thetalist,zeros(n,1),zeros(n,1),[0; 0; 0], ...
                         Ftip,Mlist,Glist,Slist);
end