function [whdd thdd] = twip2( th, thd, trq, whd )
% compute a two wheeled inverted pendulum's forward dynamics
m_w = 0.173; % kg
m_p = 0.826-m_w; % kg
I_w = 0.0066;
l_p = 0.043; % m
I_p = 0.00084;
r_w = 0.0323; % m
g = 9.81; % m/s^2
e1 = I_w + (m_p + m_w)*r_w*r_w;
e2 = l_p*m_p*r_w*cos(th);
e3 = I_p + l_p*l_p*m_p;
M = [ e1 (e2+e1)
e2 (e3+e2) ];
Minv = inv(M);
e4 = thd*thd*l_p*m_p*r_w*sin(th);
e5 = g*l_p*m_p*sin(th);
scale1 = 0.004;
scale2 = 0.00063;
w_viscosity1 = 0.0095;
w_viscosity2 = 0.0095;
v = [ (-scale1*trq + e4 - w_viscosity1*whd ) (scale2*trq + e5 + w_viscosity2*whd) ];
rhs = transpose( v );
result = Minv*rhs;
whdd = result(1);
thdd = result(2);
end