function [ dx ] = dynamics(x,u)
    % 2-link planar manipulator dynamics
    th1     = x(1); % Joint angle 1
    th1d    = x(2); % Joint angle 1 derivative
    th2     = x(3); % Joint angle 2 
    th2d    = x(4); % Joint angle 2 derivative
    tau1    = x(5); % Torque 1 
    tau2    = x(6); % Torque 2

    tau1r = u(1);
    tau2r = u(2);
    mM1 	= 0.3;
    mM2 	= 0.3;
    kr1 	= 1.0;
    kr2 	= 1.0;
    rM1  = 0.05;
    rM2  = 0.05;
    Im1 	= 1/2*mM1*rM1*rM1;
    Im2 	= 1/2*mM2*rM2*rM2;

    ml1 	= 3.0;
    ml2 	= 3.0;
    l1 	= 1.0;
    l2 	= 1.0;
    Il1	= 1.0/12.0*ml1*l1*l1;
    Il2	= 1.0/12.0*ml2*l2*l2;
    a1 	= l1/2.0;
    %a2 	= l2/2.0;

    g = 9.81;

    alpha_1	= Il1 + ml1*l1*l1 + kr1*kr1*Im1 + Il2 + ml2*(a1*a1 + l2*l2 + 2*a1*l2*cos(th2) + Im2 + mM2*a1*a1);
    alpha_2	= Il2 + ml2*(l2*l2 + a1*l2*cos(th2)) + kr2*Im2;
    alpha_3 = -2.0*ml2*a1*l2*sin(th2);
    alpha_4 = -ml2*a1*l2*sin(th2);
    K_alpha = (ml1*l1 + mM2*a1 + ml2*a1)*g*cos(th1) + ml2*l2*g*cos(th1 + th2);

    beta_1 	= Il2 + ml2*(l2*l2 + a1*l2*cos(th2)) + kr2*Im2;
    beta_2 	= Il2 + ml2*l2*l2 + kr2*kr2*Im2;
    beta_3 	= ml2*a1*l2*sin(th2);
    K_beta 	= ml2*l2*g*cos(th1 + th2);

    theta1dd_exp = 1.0/(alpha_1 - alpha_2*beta_1/beta_2) 	* (alpha_2/beta_2 * (K_beta + beta_3*th1d*th1d - tau2) - alpha_3*th1d*th2d - alpha_4*th2d - K_alpha + tau1);

    dx = [  th1d;
            theta1dd_exp;
            th2d;
            1.0/beta_2*(tau2 - beta_1*theta1dd_exp - beta_3*th1d*th1d - K_beta);
            tau1r;
            tau2r ];
end