Lagrange equations of motion of very unstable robot I'm trying to control a robot and I have somewhere bug and I can't found it. I would be grateful if is there anybody who would check my equations. The robot diagram can be found here 1.
I would like to obtain differential equations of the robot by using the Lagrange equation of motion. Here is what have I done. 
$$x_1 = X + L_2\cos\theta_2 + P_1\cos (\theta_1 + \theta_2) $$
$$  z_1 = Z + L_2\sin\theta_2 + P_1\sin (\theta_1 + \theta_2)$$
$$\dot{x}_1 = - L_2\sin{\theta}_2\dot{\theta}_2 - P_1\sin(\theta_1 + \theta_2)(\dot{\theta}_1 + \dot{\theta}_2) $$
$$  z_1 = L_2\cos{\theta_2}\dot{\theta_2} + P_1\cos(\theta_1 + \theta_2)(\dot{\theta}_1 + \dot{\theta}_2)$$
$$x_2 = X + P_2\cos\theta_2 $$
$$z_2 = Z + P_2\sin\theta_2 $$
$$\dot{x}_2 = -P_2\sin\theta_2\dot{\theta}_2 $$
$$\dot{z}_2 = P_2\cos\theta_2\dot{\theta}_2 $$
$$x_m = X + L_2\cos\theta_2 + P_m\cos (\theta_1 + \theta_2 + \theta_m) $$
$$  z_m = Z + L_2\sin\theta_2 + P_m\sin (\theta_1 + \theta_2 + \theta_m)$$
$$\dot{x}_m = - L_2\sin\theta_2\dot{\theta}_2 - P_m\sin(\theta_1 + \theta_2 + \theta_m)(\dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_m)$$
$$  \dot{z}_m = L_2\cos\theta_2\dot{\theta}_2 + P_m\cos(\theta_1 + \theta_2 + \theta_m)(\dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_m)$$
Potential energy:
$$T_1 = \frac{1}{2}M_1(\dot{x}_1^2 + \dot{y}_1^2) + \frac{1}{2}I_1(\dot{\theta_1} + \dot{\theta_2})^2$$
$$T_2 = \frac{1}{2}M_2(\dot{x}_2^2 + \dot{y}_2^2) + \frac{1}{2}I_2\dot{\theta}_2^2$$
$$T_m = \frac{1}{2}M_m(\dot{x}_m^2 + \dot{y}_m^2) + \frac{1}{2}I_m(\dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_m)^2$$
Kinetic energy:
Gravity which is pointing downward 
$$ U_1 = M_1gy_1 $$
$$ U_2 = M_2gy_2 $$
$$ U_m = M_mgy_m $$
Spring with stiffness coefficient K:
$$ \Delta L_p = \sqrt{L_m^2 + L_k^2 - 2L_kL_m\cos(\theta_m + \theta_1-\pi))} - L_{p0} $$
$$ Up  = 1/2K({\Delta L_p})^2 $$
Where coefficents are:


*

*$M_1$: Mass of link 1 in center of gravity 

*$M_2$: Mass of link 2 in center of gravity

*$M_m$: Mass of link m in center of gravity 

*$L_1$: Length of link 1 

*$L_2$: Length of link 2 

*$L_m$: Length of link m

*$P_1$: Position of center of mass of link 1 

*$P_2$: Position of center of mass of link 2

*$P_m$: Position of center of mass of link m

*$L_{p0}$: Initial lenght of the spring


Generalized coordiantes are angles $\vec{q} = [\theta_1, \theta_2, \theta_m]$, $\vec{\dot{q}} = [\dot{\theta}_1, \dot{\theta}_2, \dot{\theta}_m]$. 
Generalize forces $Q = [0 0 \tau_m]$ is torque on motor which is connected in $\theta_m$ angle.
Lagrange equation of motion:
$$L = T - U$$
$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i} \right) - \frac{\partial L}{\partial q_i} = Q $$ 
from this i will obtain 3 very long equations, so I will not include them. I done this in Matlab. After that I used substitution:
$$\dot{\Theta}_1 = \ddot{\theta}_1$$
$$\Theta_1 = \dot{\theta}_1$$
$$\dot{\Theta}_2 = \ddot{\theta}_2$$
$$\Theta_2 = \dot{\theta}_2$$
$$\dot{\Theta}_m = \ddot{\theta}_m$$
$$\Theta_m = \dot{\theta}_m$$
and solved them to receive:
$$\dot{\theta}_1 = \Theta_1$$
$$\dot{\theta}_2 = \Theta_2$$
$$\dot{\theta}_m = \Theta_m$$
$$\dot{\Theta}_1 = f_1(\theta_1, \theta_2, \theta_m, \Theta_1, \Theta_2, \Theta_m, \tau_m)$$
$$\dot{\Theta}_2 = f_1(\theta_1, \theta_2, \theta_m, \Theta_1, \Theta_2, \Theta_m, \tau_m)$$
$$\dot{\Theta}_m = f_1(\theta_1, \theta_2, \theta_m, \Theta_1, \Theta_2, \Theta_m, \tau_m)$$
When I try to solve this in ode45 in Matlab it acts unrealistically. When I try linearization is an arbitrary working point then matrix A and B have numbers in 1e4. I tried many types of control but all failed. I check everything but I'm unable to say whenever are the equations correct.
Thank you for your help.
 A: Follows a MATHEMATICA script which simulates the movement equations for a set of parameters specified in parms
p0 = {X, Z};
p1 = p0 + L2 {Cos[theta2[t]], Sin[theta2[t]]} + P1 {Cos[theta1[t] + theta2[t]], Sin[theta1[t] + theta2[t]]};
p2 = p0 + P2 {Cos[theta2[t]], Sin[theta2[t]]};
pm = p0 + L2 {Cos[theta2[t]], Sin[theta2[t]]} + Pm {Cos[theta1[t] + theta2[t] + thetam[t]], Sin[theta1[t] + theta2[t] + thetam[t]]};
v1 = D[p1, t];
v2 = D[p2, t];
vm = D[pm, t];
T1 = 1/2 M1 p1.p1 + 1/2 I1 D[theta1[t] + theta2[t], t]^2;
T2 = 1/2 M2 p2.p2 + 1/2 I2 D[theta2[t], t]^2;
Tm = 1/2 Mm pm.pm + 1/2 Im D[theta1[t] + theta2[t] + thetam[t], t]^2;
T = T1 + T2 + Tm;
delLp = Sqrt[Lm^2 + Lk^2 - 2 Lm Lk Cos[thetam[t] + theta1[t] - Pi]] - Lp0;
V = g (M1 p1 + M2 p2 + Mm pm).{0, 1} + 1/2 K0 delLp^2;
L = T - V;
vars = {theta1[t], theta2[t], thetam[t]};
torque = {0, 0, tau};
equsmov = D[Grad[L, D[vars, t]], t] - Grad[L, vars] - torque;

parms = {L2 -> 1, P2 -> 1/2, L1 -> 1/2, P1 -> 0.5, Lm -> 1/4, Pm -> 1/8, Lk -> 3/4, I1 -> 1, I2 -> 2, Im -> 1/2, K0 -> 1000, Lp0 -> 1/2, g -> 10, M1 -> 1, M2 -> 2, Mm -> 1/2, X -> 0, Z -> 0};

ode = Thread[(equsmov /. parms /. tau -> 0) == 0];

tmax = 10;
cinits = {theta1[0] == 0.01, theta2[0] == -Pi/2, thetam[0] == 0, theta1'[0] == 0, theta2'[0] == 0, thetam'[0] == 0};
solthetas = NDSolve[Join[ode, cinits], vars, {t, 0, tmax}][[1]];

ParametricPlot[Evaluate[{p1, p2, pm} /. parms /. solthetas], {t, 0, tmax}]

Follows a simulation plot showing in abscense of input, and hanging, the pendular behavior.

