%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matlab Code to solve symbolic
% the dynamic equations of 
% the double inverted pendulum (DIP)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial commands to clear memory
% and set format
clc;clear all;format compact;
% Definition of the system's parameters
syms m0 m1 m2 dtheta0 dtheta1 dtheta2 theta0 theta1 theta2
syms l1 l2 L1 I1 I2 g ddtheta0 ddtheta1 ddtheta2
% Calculate the 3 Kinetic Energies
% for the cart and the 2 pendulums
T0=1/2*m0*dtheta0^2;
T1=1/2*m1*dtheta0^2+1/2*(m1*l1^2+I1)*dtheta1^2+m1*l1*dtheta0*dtheta1*cos(theta1);
T2=1/2*m2*dtheta0^2+1/2*m2*L1^2*dtheta1^2+1/2*(m2*l2^2+I2)*dtheta2^2+m2*L1*dtheta0*dtheta1*cos(theta1)+m2*l2*dtheta0*dtheta2*cos(theta2)+m2*L1*l2*dtheta1*dtheta2*cos(theta1-theta2);
% Calculate the 3 Potential Energies
% for the cart and the 2 pendulums
P0=0;
P1=m1*g*l1*cos(theta1);
P2=m2*g*(L1*cos(theta1)+l2*cos(theta2));
% Calculate the totals of Kinetic
% and Potential Energies
T=T0+T1+T2;
P=P0+P1+P2;
% We can use the simplify command 
% to make simpler the long equations
% simplify(T)
% simplify(P)
% Euler-Lagrange Equations for
% the cart
% Partial derivative T for dtheta0
pTdtheta0=diff(T,dtheta0);
% Derivative of T for dt
dtpTdtheta0=diff(pTdtheta0,theta0)*dtheta0+diff(pTdtheta0,dtheta0)*ddtheta0+...
            diff(pTdtheta0,theta1)*dtheta1+diff(pTdtheta0,dtheta1)*ddtheta1+...
            diff(pTdtheta0,theta2)*dtheta2+diff(pTdtheta0,dtheta2)*ddtheta2;
% Partial derivative T for theta0
pTtheta0=diff(T,theta0);
% Partial derivative P for theta0
pPtheta0=diff(P,theta0)
% Equation 1
eq1=simplify(dtpTdtheta0-pTtheta0+pPtheta0);
% Euler-Lagrange Equations for
% the first pendulum
% Partial derivative T for dtheta1
pTdtheta1=diff(T,dtheta1);
% Derivative of T for dt
dtpTdtheta1=diff(pTdtheta1,theta0)*dtheta0+diff(pTdtheta1,dtheta0)*ddtheta0+...
            diff(pTdtheta1,theta1)*dtheta1+diff(pTdtheta1,dtheta1)*ddtheta1+...
            diff(pTdtheta1,theta2)*dtheta2+diff(pTdtheta1,dtheta2)*ddtheta2;
% Partial derivative T for theta1
pTtheta1=diff(T,theta1);
% Partial derivative P for theta1
pPtheta1=diff(P,theta1);
% Equation 2
eq2=simplify(dtpTdtheta1-pTtheta1+pPtheta1);
% Euler-Lagrange Equations for
% the second pendulum
% Partial derivative T for dtheta2
pTdtheta2=diff(T,dtheta2);
% Derivative of T for dt
dtpTdtheta2=diff(pTdtheta2,theta0)*dtheta0+diff(pTdtheta2,dtheta0)*ddtheta0+...
            diff(pTdtheta2,theta1)*dtheta1+diff(pTdtheta2,dtheta1)*ddtheta1+...
            diff(pTdtheta2,theta2)*dtheta2+diff(pTdtheta2,dtheta2)*ddtheta2;
% Partial derivative T for theta2
pTtheta2=diff(T,theta2);
% Partial derivative P for theta2
pPtheta2=diff(P,theta2);
% Equation 3
eq3=simplify(dtpTdtheta2-pTtheta2+pPtheta2)
% Sol=solve(eq1,eq2,eq3,ddtheta0,ddtheta1,ddtheta2)
% simplify(Sol.ddtheta0)
Sol=solve(eq1,ddtheta0);
%For ddtheta
diff(eq1,ddtheta0);
diff(eq1,ddtheta1);
diff(eq1,ddtheta2);
diff(eq2,ddtheta0);
diff(eq2,ddtheta1);
diff(eq2,ddtheta2);
diff(eq3,ddtheta0);
diff(eq3,ddtheta1);
diff(eq3,ddtheta2);
%For dtheta
diff(eq1,dtheta0);
...