Deriving TWIP dynamics

% This version uses the wheel angle relative to the body as a state variable
% (which we measure directly), rather than the wheel angle relative to
% vertical (which is not measured directly).
% It also includes wheel friction

clc;clear;close all;
%% Define generalized system variables:

syms wh dwh th dth torque friction 'real'
syms g m_p m_w l_p r_w I_p I_w 'real'

s = [wh; th];
ds = [dwh; dth];

% kinematics: wh rolls forward, th (body angle wrt vertical) leans forward,
% which, since wheel angle is with respect to body, rolls the wheel forward
x = (wh + th)*r_w;
dx = (dwh + dth)*r_w;

% Locate center of mass of pendulum
% Pendulum 'arm' is a function of the pendulum angle, theta, and the cart
% position, x.
cm_p = [x + l_p*sin(th); l_p*cos(th)];

% Linear velocity of the pendulum
v_p = jacobian(cm_p, s)*ds;

% Angular velocity of the wheel
omega_w = dx/r_w;

% Kinetic energies
k_w = simplify( expand( 1/2 * m_w * dx^2 + 1/2 * I_w * omega_w^2 ) );
k_p = simplify( expand( 1/2 * m_p * (v_p)' * v_p + 1/2 * I_p * dth^2 ) );

% Potential energies
p_w = 0;
p_p = m_p * g * l_p * cos(th);

% Lagrangian
L = k_w + k_p - p_w - p_p;

% External Forces (like joint torques and friction)
% Positive motor torque moves the body to learn forward, and rotates the wheel
% backwards. Friction shows the wheel down.
% Moving just the body does no work with respect to the torque, so the
% second entry in Q is zero.
% There is a fictional joint of the body with the "world", which exerts no
% torque and is frictionless.
Q = [ -torque - friction*dwh, 0 ];

% Compute Euler-Lagrange:
EQ = EulerLagrange(s,ds,L,Q,1); 

% Equations that result:
1. ddwh = -(torque + I_w*ddth + dwh*friction + ddth*m_p*r_w^2 + ddth*m_w*r_w^2 - dth^2*l_p*m_p*r_w*sin(th) + ddth*l_p*m_p*r_w*cos(th))/(I_w + m_p*r_w^2 + m_w*r_w^2) 
2. ddth = -(I_w*ddwh + ddwh*m_p*r_w^2 + ddwh*m_w*r_w^2 - g*l_p*m_p*sin(th) - dth^2*l_p*m_p*r_w*sin(th) + ddwh*l_p*m_p*r_w*cos(th))/(I_p + I_w + l_p^2*m_p + m_p*r_w^2 + m_w*r_w^2 + 2*l_p*m_p*r_w*cos(th)) 

% From here on I am doing this manually.

Equation 1
(I_w + m_p*r_w^2 + m_w*r_w^2)*ddwh = -torque - I_w*ddth - dwh*friction - ddth*m_p*r_w^2 - ddth*m_w*r_w^2 + dth^2*l_p*m_p*r_w*sin(th) - ddth*l_p*m_p*r_w*cos(th)

A = (I_w + (m_p + m_w)*r_w^2)
B = l_p*m_p*r_w
D = dth^2*l_p*m_p*r_w*sin(th) - Centripetal force
so
A*ddwh = -torque - friction*dwh - A*ddth + D - B*cos(th)*ddth
so
A*(ddwh + ddth) + B*cos(th)*ddth 
  = -torque - friction*dwh + D

Equation 2
(I_p + I_w + l_p^2*m_p + m_p*r_w^2 + m_w*r_w^2 + 2*l_p*m_p*r_w*cos(th))*ddth
= -I_w*ddwh - ddwh*m_p*r_w^2 - ddwh*m_w*r_w^2 + g*l_p*m_p*sin(th) + dth^2*l_p*m_p*r_w*sin(th) - ddwh*l_p*m_p*r_w*cos(th))

C = I_p + l_p^2*m_p
G = l_p*m_p
so
(A + C + 2*B*cos(th))*ddth + (A + B*cos(th))*ddwh
 = D + G*sin(th)*g
       Gravitational term

Dynamics Equation
( A           A+B*cos(th)     )( ddwh ) = ( -torque - friction*dwh + D )
( A+B*cos(th) A+2*B*cos(th)+C )( ddth )   (  D + G*sin(th)*g           )
Mass Matrix

Inertial parameters (constants) A, B, and C appear linearly in the dynamics
and are thus easy to identify using linear regression
Same for viscous friction coefficient f

************************************************************************
// compare with version that is in terms of x and th
e1 = I_w/r_w^2 + m_p + m_w;   e1 = A/(r_w^2)
e2 = l_p*m_p*cos(th);         e2 = B*cos(th)/r_w
e3 = (I_p + l_p^2*m_p);       e3 = C
v1 = (-trq/r_w + thd^2*l_p*m_p*sin(th))
v2 = (trq + g*l_p*m_p*sin(th))

e1*ddx + e2*ddth = v1
e2*ddx + e3*ddth = v2

ddx = (ddww + ddth)*r_w
so
A*(ddww + ddth)/r_w + (B*cos(th)/r_w)*ddth = -trq/r_w + D/r_w
(B*cos(th)/r_w)*(ddww + ddth)*r_w + C*ddth = trq + G*sin(th)*g

so

A*ddww + (A + B*cos(th))*ddth = -trq + D
B*cos(th)*ddww + (B*cos(th) + C)*ddth = trq + G*sin(th)*g

Add equation 1 to equation 2 to get new equation 2

(A + B*cos(th))*ddwh + (A + 2*B*cos(th) + C)*ddth = D + G*sin(th)*g