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

function taulist = InverseDynamics(thetalist,dthetalist,ddthetalist,g, ...
                                   Ftip,Mlist,Glist,Slist)
% Takes thetalist: n-vector of joint variables,
%       dthetalist: n-vector of joint rates,
%       ddthetalist: n-vector of joint accelerations,
%       g: Gravity vector g,
%       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 taulist: The n-vector of required joint forces/torques.
% This function uses forward-backward Newton-Euler iterations to solve the 
% equation:
% taulist = Mlist(thetalist)ddthetalist + c(thetalist,dthetalist) ...
%           + g(thetalist) + Jtr(thetalist)Ftip
% Example Input (3 Link Robot):
%{ 
  clear;clc;
  thetalist = [0.1; 0.1; 0.1];
  dthetalist = [0.1; 0.2; 0.3];
  ddthetalist = [2; 1.5; 1];
  g = [0; 0; -9.8];
  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]];
  taulist = InverseDynamics(thetalist,dthetalist,ddthetalist,g,Ftip, ...
                            Mlist,Glist,Slist)
%}
% Output:
% taulist =
%   74.6962
%  -33.0677
%   -3.2306

n = size(thetalist,1);
Mi = eye(4);
Ai = zeros(6,n);
AdTi = zeros(6,6,n + 1);
Vi = zeros(6,n + 1);
Vdi = zeros(6,n + 1);
Vdi(4:6,1) = -g;
AdTi(:,:,n + 1) = Adjoint(TransInv(Mlist(:,:,n + 1)));
Fi = Ftip;
taulist = zeros(n,1);
for i=1:n    
    Mi = Mi * Mlist(:,:,i);
    Ai(:,i) = Adjoint(TransInv(Mi)) * Slist(:,i);    
    AdTi(:,:,i) = Adjoint(MatrixExp6(VecTose3(Ai(:,i) * -thetalist(i))) ...
                      * TransInv(Mlist(:,:,i)));    
    Vi(:,i + 1) = AdTi(:,:,i) * Vi(:,i) + Ai(:,i) * dthetalist(i);
    Vdi(:,i + 1) = AdTi(:,:,i) * Vdi(:,i) + Ai(:,i) * ddthetalist(i) ...
                   + ad(Vi(:,i + 1)) * Ai(:,i) * dthetalist(i) ;    
end
for i = n:-1:1
    Fi = AdTi(:,:,i + 1)'* Fi + Glist(:,:,i) * Vdi(:,i + 1) ...
         - ad(Vi(:,i + 1))' * (Glist(:,:,i) * Vi(:,i + 1));
    taulist(i) = Fi' * Ai(:,i);
end
end