[Python] My code keeps hanging in the step where it is supposed to solve the linear equations

Here is my code:

import numpy as np
import sympy as smp
import matplotlib.pyplot as plt
from scipy.integrate import odeint
import plotly.graph_objects as go
from IPython.display import HTML
import pandas as pd
print("code started")

#to find the inertia tensor of the object we are going to try to draw out the shape of the top as a function of theta, for the purposes of testing this shape is going to be a cube
#defining the function for against the z axis in the domin pi/4 to pi/2

#now we want numpy to generate points of that function in our first domain for theta
#this test is for a cube and we have aldready computed to MOI tensor by hand in book so
# defining M and r
M = 0.1
r = 5
scale = M * r**2
#I = scale * np.array([
# [2/3, -1/4, -1/4],
# [-1/4, 2/3, -1/4],
# [-1/4, -1/4, 2/3]
#])
#here we have defined our inertia tensor, now to move on to the motion itself
t, h, g = smp.symbols('t h g', real=True)
#the above defines time height of center of mass and gravity as symbols and says that they are real
the, phi, psi = smp.symbols(r'\theta \phi \psi', cls=smp.Function)
#this defines the euler angles as functions of time
print("defining functions")
the = the(t)
phi = phi(t)
psi = psi(t)
#now we can define their derivatives and Second derivatives w.r.t time
print("finding derivatives")
the_d = smp.diff(the,t)
phi_d = smp.diff(phi,t)
psi_d = smp.diff(psi,t)
the_dd = smp.diff(the_d,t)
phi_dd = smp.diff(phi_d,t)
psi_dd = smp.diff(psi_d,t)
#now we define the transformation matrices of the rotation to be able to transform the vectors to the cartesian coordinate system in terms of z y and x
#since the vectors themselves are being rotatend instead of the coordinate axes the sign convention for sin is opposite
print("defining transformation matrices")
R3 = smp.Matrix([[smp.cos(psi), -smp.sin(psi), 0],
[smp.sin(psi), smp.cos(psi), 0],
[0, 0, 1]])
R2 = smp.Matrix([[1,0,0],
[0,smp.cos(the),-smp.sin(the)],
[0,smp.sin(the),smp.cos(the)]])

R1 = smp.Matrix([[smp.cos(phi),-smp.sin(phi),0],
[smp.sin(phi),smp.cos(phi),0],
[0,0,1]])

# and then we find the full rotation matrix as R(product of all)
R = R1*R2*R3
#now we define angular momentum
print("defining angular momentum")
omega = smp.Matrix([phi_d*smp.sin(the)*smp.sin(psi)+the_d*smp.cos(psi),
phi_d*smp.sin(the)*smp.cos(psi)-the_d*smp.sin(psi),
phi_d*smp.cos(the)+psi_d])
#although we have the numerical tensor now we need to define symbolic elements to perform symbolic integration
Ixx, Iyy, Izz, Ixy, Iyz, Ixz = smp.symbols('I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{yz}, I_{xz}', real=True)
I = smp.Matrix([[Ixx,Ixy,Ixz],[Ixy,Iyy,Iyz],[Ixz,Iyz,Izz]])
#now we move on to defining the lagrangian using the simplified one where we divide by mL^2 sicne if you multiply the lagrangian by a constant the equations remain the same
print("defining lagrangian")
T = smp.Rational(1,2)*omega.T.dot(I*omega).simplify()
V = g*h*smp.cos(the)
L = T-V
#now to obtain the lagrangian equations for all three angles
print("defining lagrangian equatiosn")
LE1 = smp.diff(L, the) - smp.diff(smp.diff(L, the_d),t)
LE1 = LE1.simplify()
LE2 = smp.diff(L, phi) - smp.diff(smp.diff(L, phi_d),t)
LE2 = LE2.simplify()
LE3 = smp.diff(L, psi) - smp.diff(smp.diff(L, psi_d),t)
LE3 = LE3.simplify()
#now since all of their secod derivatives are linear we can solve for them using the LEs
print("solving lagrangian ")
sols=smp.solve([LE1, LE2, LE3], (the_dd, phi_dd, psi_dd),
simplify=False, rational=False)
#however since python can only work with first order derivatives we need to change our variable definitions and we change the sympy symbolic functions into numerical python functions
print("Converting to numerical functions")
dz1dt_f = smp.lambdify((g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz, the, phi, psi, the_d, phi_d, psi_d), sols[the_dd])
dthedt_f = smp.lambdify(the_d, the_d)
dz2dt_f = smp.lambdify((g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz, the, phi, psi, the_d, phi_d, psi_d), sols[phi_dd])
dphidt_f = smp.lambdify(phi_d, phi_d)
dz3dt_f = smp.lambdify((g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz, the, psi, phi, the_d, phi_d, psi_d), sols[psi_dd])

dpsidt_f = smp.lambdify(psi_d, psi_d)
#now given the arguements we will be able to get the vales of the second order differential equations
#now we write as a system of equatins that takes in all the angles and their first derivatives and returns their time derivative
def dSdt(s, t):
the, z1, phi, z2, psi, z3 = s
return [
dthedt_f(z1),
dz1dt_f(g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz,the,phi,psi,z1,z2,z3),
dphidt_f(z2),
dz2dt_f(g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz,the,phi,psi,z1,z2,z3),
dpsidt_f(z3),
dz3dt_f(g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz,the,phi,psi,z1,z2,z3),
]
Ixx = smp.Rational(2,3) * scale
Iyy = smp.Rational(2,3) * scale
Izz = smp.Rational(2,3) * scale
Ixy = smp.Rational(-1,4) * scale
Iyz = smp.Rational(-1,4) * scale
Ixz = smp.Rational(-1,4) * scale
#I = np.array([[Ixx, Ixy, Ixz],[Ixy, Iyy, Iyz],[Ixz, Iyz, Izz]])
g = 9.8/0.05
#in our current case 2.5
h = 2.5
#now we define our initial conditions and solve the differential equations for hte first 2 seconds
t = np.linspace(0, 2, 10000)
print("Starting integration...")
ans = odeint(dSdt, y0=[np.pi/4, 0, 0, 10, 0, 120*np.pi], t=t)
print("Integration done!")

print(ans.T)


Everytime I try to run the code it works fine and prints all the statements including solving lagrangian, but it goes no further and hangs at the solving line.

I thought at first that there was an error in the function but all references I can find use the same function and it seems to work for them.

Continue reading...