function [dXout] = cg_eom(tin,Xin)

% Equations of motion for the CONTINUOUS dynamics of a compass
% gait walker.  (Dynamics are identical to a double pendulum;
% e.g., the acrobot or pendubot when input torque is zero.)
% Katie Byl, 1/19/2010.  ECE594D, Winter 2010 (UCSB)
%
%    mh
%    |\   b
%    | \         L = a+b = 1 (unit length leg)
%   m   m
%   |    \  a         point masses: m=5; mh=1; a=.7; b=.3;
% _|_     \
%    ----____    <-- gam_slope = ground slope
%----------------
%
% State definition, "qin" (4x1)
% "Goswami" convention for the 4 states:
%      Xin(1) = angle of non-stance (ns) foot, wrt +y axis (CCW)
%      Xin(2) = angle of stance (s) foot, wrt +y axis (CCW)
%      Xin(3) = dXin(1)/dt (ns foot, absolute ang. vel.)
%      Xin(4) = dXin(2)/dt (s foot, absolute ang. vel.)

% Constants defining geometry and mass distribution:
cg_params  % <-- look inside script "cg_params.m"

% Input states
qns = Xin(1);
qs = Xin(2);
dqns = Xin(3);
dqs = Xin(4);

M = [m*b^2             ,      -m*L*b*cos(qs-qns)
    -m*L*b*cos(qs-qns) ,       mh*L^2 + m*(L^2+a^2)];
N = [0                      ,       m*L*b*dqs*sin(qs-qns)
    -m*L*b*dqns*sin(qs-qns) ,       0];
g = [ m*g*b*sin(qns)
    -(mh*L+m*(a+L))*g*sin(qs)];

dq = [dqns; dqs];
d2qout = -M^-1 * (N*dq + g);
dXout = [dq; d2qout];  % instantaneous derivatives of all 4 states

%------------------------end of code------------------------------------
