Equations of motions for 3D pendulum using spherical joint

[zoufaond]

does anybody have experience with obtaining equations of motions in sympy for 3D pendulum using sympy mechanics ? I tried to do it the easiest way as possible, but the resulting mass matrix has NaNs in it, does anybody know how to fix the problem ?

Here's the code I'm using:

from sympy import symbols, trigsimp, simplify
from sympy.physics.mechanics import Body, PinJoint, JointsMethod, ReferenceFrame, dynamicsymbols, mechanics_printing, mprint, SphericalJoint
mechanics_printing()

l, m, g = symbols('l m g')
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
u1d, u2d, u3d = dynamicsymbols('u1 u2 u3', 1)

ceiling = Body('C')
bob = Body('U', mass=m)
joint = SphericalJoint('P1', ceiling, bob, child_point=-l*bob.x, coordinates = [q1, q2, q3], speeds = [u1, u2, u3])

bob.apply_force(-ceiling.y*bob.mass*g)

method = JointsMethod(ceiling,joint)
method.form_eoms()
mm = method.mass_matrix_full
fo = method.forcing_full
mmi = simplify(mm).inv()
qudots = mmi*fo
mprint(simplify(qudots))

[oscarbenjamin]

It is good to include cross references: https://stackoverflow.com/questions/77629242/equations-of-motions-for-3d-pendulum-in-sympy?noredirect=1#comment136860754_77629242

[moorepants]

Here is an example of a pendulum with spherical joints:

https://pydy.readthedocs.io/en/stable/examples/three-link-conical-pendulum.html

[pelotonlopezmuro]

Thanks for your reply @moorepants ! I will detail what I am modeling; hopefully, you can give me further specifics/insights. I am studying the dynamics of a tower (long beam) fixed to the ground. My goal is to model the long beam as a sequence of interconnected rigid bodies and lumped linear and rotational stiffness/damping at the rigid bodies' interfaces.

To get started, I assumed one single rigid body and considered the rotations only. Then, it is a 3dof rigid body, with stiffness and damping torques applied at its base and a force due to the effect of gravity.
Please see my code:
https://github.com/pelotonlopezmuro/sympy-problems/blob/aff10d8bb8dcb470e1835af0d2509aefa29a585c/RogidBody1.py

I am using psi-theta-phi (z-y-x) Euler angles to describe the orientation of the rigid body. To start simple, the stiffness torques will be proportional to the Euler rotation, and the damping torques will be proportional to the Euler rotation rates. I am using Lagrange's method. I know that the solutions I obtain are consistent with my problem formulation, particularly consistent with how I model the stiffnesses/damping. I also know that the main problem with this model ( despite being oversimplified lumped parameters moded) is that the stiffnesses/damping torques are proportional to the Euler angles/rates.

In my simulation, theta converges to zero and phi to a final value different from zero. This can vary depending on the initial conditions, but one coordinate converges to zero at the end, and the other to a final value different from zero. However, in reality, one would expect phi and theta to converge to values different from zero. In summary, I am trying to build a more realistic model but still figuring out how. Any help or light you can throw on the issue will be greatly appreciated!

[moorepants]

and the damping torques will be proportional to the Euler rotation rates

Do you me the time derivative of the coordinate or the generalized speed associated with the body fixed angular velocity?

[pelotonlopezmuro]

I mean, the stiffness torques are proportional to psi, theta, and phi; and the damping torques are proportional to psi_dot, theta_dot, and phi_dot. These last ones are not the body-fixed angular velocity "p q r" but are related, if that makes sense.

[moorepants]

The only issue I can think of is that if you use three stiffnesses each proportional to one of the Euler angles, then for an arbitrary orientation you have some sum of those stiffnesses that may not be the effect that you actually want. It would probably be better to imagine a real spring and how that would be attached to your system, then model it.

[pelotonlopezmuro]

Yes, I agree with you. I thought it appropriate to comment in this thread about "3D pendulum using spherical joint" because, in my own words, I want to model an inverted 3D pendulum with a "spherical stiffness". I hope I well explained what I am trying to do. The example I gave is a very naive approach that results in a behavior I do not want to see. I am trying a few other methods, yet without great success.

[moorepants]

It is generally best not to invert the mass matrix symbolically. You can introduce divide-by-zeros by doing so, depending on your choice of coordinates.

[oscarbenjamin]

In fact the matrix is not invertible:

>>> mm.det()
0

[moorepants]

Also, make sure you set rot_type and rot_order such that you choose the best coordinates for the system you are modeling. You can also get singularities due to gimbal lock.

[moorepants]

A conical pendulum has only 2 generalized coordinates. You are formulating it with 3. My guess is the singularity is associated with having an acceleration equation for a mass-less/inertia-less state.

[zoufaond]

That was it, I added inertia tensor and it works, thank you :)