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

function [thetamat, dthetamat] ...
         = ForwardDynamicsTrajectory(thetalist,dthetalist,taumat,g, ...
                                     Ftipmat,Mlist,Glist,Slist,dt,intRes)
% Takes thetalist: n-vector of initial joint variables,
%       dthetalist: n-vector of initial joint rates,
%       taumat: An N x n matrix of joint forces/torques, where each row is 
%               the joint effort at any time step,
%       g: Gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the 
%                end-effector (If there are no tip forces, the user should 
%                input a zero and a zero matrix will be used),
%       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,
%       dt: The timestep between consecutive joint forces/torques,
%       intRes: Integration resolution is the number of times integration
%               (Euler) takes places between each time step. Must be an 
%               integer value greater than or equal to 1.
% Returns thetamat: The N x n matrix of robot joint angles resulting from 
%                   the specified joint forces/torques,
%         dthetamat: The N x n matrix of robot joint velocities.
% This function simulates the motion of a serial chain given an open-loop 
% history of joint forces/torques. It calls a numerical integration 
% procedure that uses ForwardDynamics.
% Example Inputs (3 Link Robot):
%{
  clc;clear;
  thetalist = [0.1; 0.1; 0.1];
  dthetalist = [0.1; 0.2; 0.3];
  taumat = [[3.63, -6.58, -5.57]; [3.74, -5.55, -5.5]; ...
            [4.31, -0.68, -5.19]; [5.18, 5.63, -4.31]; ...
            [5.85, 8.17, -2.59]; [5.78, 2.79, -1.7]; ...
            [4.99, -5.3, -1.19]; [4.08, -9.41, 0.07]; ...
            [3.56, -10.1, 0.97]; [3.49, -9.41, 1.23]];
  %Initialise robot description (Example with 3 links)
  g = [0;0;-9.8];
  Ftipmat = ones(size(taumat,1),6);
  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]];
  dt = 0.1;
  intRes = 8;
  [thetamat,dthetamat] ...
  = ForwardDynamicsTrajectory(thetalist,dthetalist,taumat,g,Ftipmat, ...
                              Mlist,Glist,Slist,dt,intRes);
  %Output using matplotlib to plot the joint forces/torques
  Tf = size(taumat,1);
  time=0:(Tf/size(thetamat,1)):(Tf-(Tf/size(thetamat,1)));
  plot(time,thetamat(:,1),'b')
  hold on
  plot(time,thetamat(:,2),'g')
  plot(time,thetamat(:,3),'r')
  plot(time,dthetamat(:,1),'c')
  plot(time,dthetamat(:,2),'m')
  plot(time,dthetamat(:,3),'y')
  title('Plot of Joint Angles and Joint Velocities')
  xlabel('Time')
  ylabel('Joint Angles/Velocities')
  legend('Theta1','Theta2','Theta3','DTheta1','DTheta2','DTheta3')
%}

taumat = taumat';
Ftipmat = Ftipmat';
thetamat = taumat;
thetamat(:,1) = thetalist;
dthetamat = taumat;
dthetamat(:,1) = dthetalist;
for i = 1:size(taumat,2) - 1
    for j = 1:intRes
       ddthetalist = ForwardDynamics(thetalist,dthetalist,taumat(:,i), ...
                                     g,Ftipmat(:,i),Mlist,Glist,Slist);     
       [thetalist, dthetalist] = EulerStep(thetalist,dthetalist, ...
                                           ddthetalist,dt / intRes);
    end
    thetamat(:,i + 1) = thetalist;
    dthetamat(:,i + 1) = dthetalist;
end
thetamat = thetamat';
dthetamat = dthetamat';
end