%
% [theta,thetad] = update(theta,thetad,tau,period)  Function UPDATE
% advances joint variables theta and thetad by time step "period."  The
% vector equations of motion are used, along with a integration time step
% dt.  Vector "tau" are the joint torques.  A simple Euler integration
% method is used.

function [theta,thetad] = update(theta,thetad,tau,period)

dt = 0.001; % Internal integration time step

N = period/dt;  % Number of steps in integration loop

% First define the manipulator constant parameters

m1 = 14.6;   % Mass m1 in kg
m2 = 2.3;   % Mass m2 in kg
m3 = 1.0;   % Mass m3 in kg
m23 = m2+m3;
m123 = m1+m23;

l1 = 0.5;   % Link length l1 in meters
l2 = 0.5;   % Link length l2 in meters

Izz = 0.1;  % Link 3 moment of inertia in kg-m^2

g = 9.8;    % Gravitational constant in m/sec^2

v = [5 5 5]';	% Viscous friction coefficient (N-m-sec)

for i = 1:N     % Main integration "sub-loop"

    c1 = cos(theta(1));
    c2 = cos(theta(2));
    s2 = sin(theta(2));
    c12 = cos(theta(1)+theta(2));

% Next evaluate the mass matrix, V vector, G vector, and F vector

    M = [m23*(l2^2+2*l1*l2*c2)+m123*l1^2  m23*(l2^2+l1*l2*c2)  Izz;
        m23*(l2^2+l1*l2*c2)              m23*l2^2+Izz         Izz;
        Izz                              Izz                  Izz];

    V = [-m23*l1*l2*s2*thetad(2)^2-2*m23*l1*l2*s2*thetad(1)*thetad(2);
        m23*l1*l2*s2*thetad(1)^2;
        0];

    G = [m23*g*l2*c12+m123*g*l1*c1;
        m23*g*l2*c12;
        0];

    F = [v(1)*thetad(1);
         v(2)*thetad(2);
         v(3)*thetad(3)];

    thetadd = inv(M)*(tau-V-G-F);   % Compute joint accelerations

    thetad_new = thetad+thetadd*dt;     % Integrate variables
    theta_new = theta+thetad*dt+(1/2)*thetadd*dt^2;

    thetad = thetad_new;    % Update joint variables
    theta = theta_new;
end;