# 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.

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};