%Dynamics of 2-link torque-controlled arm
%
% USAGE: [xdot, xdot_x, xdot_u] = arm_dyn(x, u)
%
% 4-dim state x contains: joint angles (2); joint velocities (2)

function [xdot, xdot_x, xdot_u] = arm_dyn(x, u)

% arm model parameters
m1_   = 1.4;    % segment mass
m2_   = 1.1;
l1_   = 0.3;    % segment length
l2_   = 0.33;
s1_   = 0.11;   % segment center of mass
s2_   = 0.16;
i1_   = 0.025;  % segment moment of inertia
i2_   = 0.045;
b11_  = 0.5;    % joint friction
b22_  = 0.5;  
b12_  = 0.1; 
b21_  = 0.1; 

EPS = 1E-5;     % finite difference epsilon


%------------------------ compute inertia I and extra torque H --------
% temp vars
mls = m2_*l1_*s2_;
iml = i1_ + i2_ + m2_*l1_^2;
dd = i2_*iml-i2_^2;
sy = sin(x(2,:));
cy = cos(x(2,:));

% inertia
I_11 = iml+2*mls*cy;
I_12 = i2_+mls*cy;
I_22 = i2_*ones(size(cy));

% determinant
dt = dd-mls^2*cy.^2;

% inverse inertia I1
I1_11 = i2_./dt;
I1_12 = (-i2_-mls*cy)./dt;
I1_22 = (iml+2*mls*cy)./dt;

% temp vars
sw = sin(x(2,:));
cw = cos(x(2,:));
y = x(3,:);
z = x(4,:);

% extra torque H (Coriolis, centripetal, friction)
H = [-mls*(2*y+z).*z.*sw + b11_*y + b12_*z;...
     mls*y.^2.*sw + b22_*z + b12_*y];


%------------- compute xdot = inv(I) * (torque - H) ------------ 
torque = u - H;
xdot = [x(3:4,:); ...
        I1_11.*torque(1,:) + I1_12.*torque(2,:); ...
        I1_12.*torque(1,:) + I1_22.*torque(2,:)];

    
%----------- compute xdot_x using finite differences ------------
if nargout>1,
    x1 = repmat(x, [1,4]) + eye(4)*EPS;
    x2 = repmat(x, [1,4]) - eye(4)*EPS;
    uu = repmat(u,[1,4]);
    f1 = arm_dyn(x1,uu);
    f2 = arm_dyn(x2,uu);
    xdot_x = (f1-f2)/2/EPS;
    
    xdot_u = zeros(4,2);
    xdot_u(3:4,:) = [I1_11 I1_12; I1_12 I1_22];
end