Understanding how to use joints in sympy.physics.mechanics

[Davide-sd]

@moorepants @tjstienstra @sidhu1012 I'm tagging all of you because you are experts on the topic, hopefully you can help me...

I'm exploring the sympy.physics.mechanics module for the generation of equations of motion. So far I've been able to study mechanisms by creating frames, vectors, points (following the procedure of the rolling disk tutorial)

Now, I'm trying to use joints in order to understand their pros and cons. I followed the four-bar linkage tutorial and I'm attempting to create a simple crank-slider mechanism. In the following picture, the slider is constrained to move in the horizontal guide (gray dotted line). so far no luck: I don't understand what I'm doing wrong...

Would you please take the time to look into this and share your findings? Thanks.

from sympy import *
import sympy.physics.mechanics as me
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from pydy.codegen.ode_function_generators import generate_ode_function
from scipy.integrate import odeint
me.init_vprinting()

t = symbols("t")
# r: length of the crank
# L: length of the rod
r, L = symbols("r, L", positive=True)
q1, q2, q3 = me.dynamicsymbols("q1, q2, q3")
q1d, q2d, q3d = me.dynamicsymbols("q1, q2, q3", 1)
u1, u2, u3 = me.dynamicsymbols("u1, u2, u3")

N, A, B, S, G = symbols("N, A, B, S, G", cls=me.ReferenceFrame)

m_slider, rho1, rho2, g = symbols("m_slider, rho1, rho2, g")
inertias = [me.inertia(N, 0, 0, rho * l**3 / 12) for rho, l in zip([rho1, rho2], [r, L])]

wall = me.Body("Wall", frame=N)
guide = me.Body("Guide", frame=G, mass=0, central_inertia=me.inertia(N, 0, 0, 0))
crank = me.Body("Crank", frame=A, mass=rho1*r, central_inertia=inertias[0])
rod = me.Body("Rod", frame=B, mass=rho2*L, central_inertia=inertias[1])
slider = me.Body("Slider", frame=S, mass=m_slider)

crank.apply_force(-rho1 * r * g * N.y)
rod.apply_force(-rho2 * L * g * N.y)
slider.apply_force(-m_slider * g * N.y)

joint1 = me.PinJoint(
    "J1", wall, crank, coordinates=q1, speeds=u1,
    child_point=-r / 2 * crank.x,
    joint_axis=wall.z
)
joint2 = me.PinJoint(
    "J2", crank, rod, coordinates=q2, speeds=u2,
    parent_point=r / 2 * crank.x,
    child_point=-L / 2 * rod.x,
    joint_axis=crank.z
)
joint3 = me.PrismaticJoint(
    "J3", guide, rod, coordinates=q3, speeds=u3,
    child_point=L/2*rod.x
)

O = crank.masscenter.locatenew("O", -r/2*crank.x)
loop = guide.masscenter.pos_from(O) + q3 * N.x
fh = Matrix([loop & N.x, loop & N.y])
fh.simplify()

method = me.JointsMethod(wall, joint1, joint2, joint3)
qdots = solve(method.kdes, [q1d, q2d, q3d])
fhd = fh.diff(t).subs(qdots)

kane = me.KanesMethod(
    wall.frame,
    q_ind=[q1],
    u_ind=[u1],
    q_dependent=[q2, q3],
    u_dependent=[u2, u3],
    kd_eqs=method.kdes,
    configuration_constraints=fh,
    velocity_constraints=fhd,
    forcelist=method.loads, bodies=method.bodies)

fr, frstar = kane.kanes_equations()

# given an initial value of q1, find q2, q3 initial values
sd = {r: 1, L: 2.5, q1: pi/4}
res = nsolve(fh.subs(sd), (q2, q3), (2.5, 0.8))

coordinates = [q1, q2, q3]
speeds = [u1, u2, u3]
constants = [r, L, rho1, rho2, m_slider, g]
num_constants = np.array([1, 2.5, 1, 1, 0.25, 9.81])
constants_dict = dict(zip(constants, num_constants))
mm = kane.mass_matrix_full
fm = kane.forcing_full

x0 = [
    np.pi/4, *res, #2.7902129952052, 0.81849473032788,
    0, 0, 0
]
fps = 60
t0, tf = 0, 15
n = int(fps * (tf - t0))
t = np.linspace(t0, tf, n)
rhs = generate_ode_function(fm, coordinates, speeds, constants, mass_matrix=mm)
y = odeint(rhs, x0, t, args=(num_constants,))

fig, ax = plt.subplots(2, 1, sharex=True)
ax[0].plot(t, y[:, :3])
ax[0].legend(["$%s$" % latex(c) for c in coordinates])
ax[0].set_ylabel("Coordinates")
ax[1].plot(t, y[:, 3:])
ax[1].legend(["$%s$" % latex(c) for c in speeds])
ax[1].set_xlabel("Time [s]")
ax[1].set_ylabel("Velocities")

What I understood by the four-bar linkage example, is that q2 is the angle between two bodies that share a joint, in this case q2 should be the angle between crank and rod. However, by creating an animation I clearly get a wrong result.

def _crank(y, idx, r):
    q1 = y[idx, 0]
    x = [0, r * np.cos(q1)]
    y = [0, r * np.sin(q1)]
    return x, y

def _rod(y, idx, r, L):
    q1 = y[idx, 0]
    q2 = y[idx, 1]
    x = [r * np.cos(q1), r * np.cos(q1) + L * np.cos(q1 + q2)]
    y = [r * np.sin(q1), r * np.sin(q1) + L * np.sin(q1 + q2)]
    return x, y

def create_animation(filename):
    idx = 0
    fig, ax = plt.subplots()
    ax.axhline(0, linestyle=":", color="darkgray")
    crank, = ax.plot(*_crank(y, idx, constants_dict[r]), label="crank")
    rod, = ax.plot(*_rod(y, idx, constants_dict[r], constants_dict[L]), label="rod")
    slider = ax.scatter(y[idx, 2], 0, label="slider")
    ax.legend()
    ax.set_aspect("equal")
    ax.axis([-1.5, 4, -2, 2])

    def update(idx):
        crank.set_data(*_crank(y, idx, constants_dict[r]))
        rod.set_data(*_rod(y, idx, constants_dict[r], constants_dict[L]))
        slider.set_offsets([y[idx, 2], 0])
        ax.set_title("t = {:.2f} s".format(t[idx]))

    ani = FuncAnimation(fig, update, frames=len(t))
    ani.save(filename, fps=60)

create_animation("crank-slider-joints.mp4")

[tjstienstra]

Good morning,
Good to see that you are playing around with joints framework. I'll quickly start with what I see your currently doing (based on reading your code, I haven't ran it), after which I shortly mention what I would change to solve the problem.
You start with the creation of all bodies, which is sensible. Next you define three joints. The first two are pin joints. The third is the prismatic joint. This prismatic joint defines the guide to be move -q3 * rod.x with respect to the rod (x direction is the default choice, rod.x=guide.x). Next you compute the constraints by computing the loop.

This however leads to something as above. I hope that this rushed explanation makes it clear what is actually happening.

The actual problem is that you are defining the closing joint, namely the prismatic joint. What you could do is remove the third joint and define the loop as rod.masscenter.pos_from(O) + L/2*rod.x - q3 * N.x.

Is this explanation clear? If you have any other questions let me know.

P.S. you may also like this tutorial on creation animations.

[Davide-sd]

Unfortunately, this is wrong! You can verify that the equations of motion do not contain the mass of the slider: (fr + frstar).has(m_slider) returns False. That's because it was not added to the system. Which is precisely what I'm trying to figure out: how do we add the mass of the slider using the joints framework?

[tjstienstra]

Sorry, good find. Forgot to add the slider as body to KanesMethod: bodies=method.bodies + [slider]. Besides that, the bodies orientation and position must also be specified. Applied both changes in the code I've shared earlier. Hopefully it is correct now. At least frstar.has(m_slider) return True.

P.S. A currently experimental System class, which is the replacement of JointsMethod will hopefully soon be available in master. This class should make the book-keeping of these systems a bit easier :)

[Davide-sd]

Interesting: you didn't create a prismatic joint. Can you explain why? If I think about a slider, a prismatic joint is the first thing that comes to my mind...

[tjstienstra]

They reason why, was that this was the first I thought of. Though it is probably the most confusing solution, as it is also possible to actually use a pin joint or a prismatic joint instead. Analyzing the crank-slider gives the following visualization:

A first note to make is that the slider is connected via a pin joint to the rod and via a prismatic joint to the wall. Overall we see that a total of four joints create a loop. As sympy mechanics uses graphs for both the orientation between reference frames and the position of points, which must be both connected and acyclic, we may not create a loop. Instead we should replace one of the joints by holonomic constraints. If you were to replace the pin joint between the rod and the slider. Then you can use a prismatic joint as joint3.

joint3 = me.PrismaticJoint(
    "J3", wall, slider, coordinates=q3, speeds=u3
)

loop = rod.masscenter.pos_from(slider.masscenter) + L / 2 * rod.x  # Updated the loop
fh = Matrix([loop & N.x, loop & N.y])
fh.simplify()

method = me.JointsMethod(wall, joint1, joint2, joint3)
qdots = solve(method.kdes, [q1d, q2d, q3d])  # Use kdes instead of method.kdes
fhd = fh.diff(t).subs(qdots)

kane = me.KanesMethod(
    wall.frame,
    q_ind=[q1],
    u_ind=[u1],
    q_dependent=[q2, q3],
    u_dependent=[u2, u3],
    kd_eqs=method.kdes,  # Use kdes instead of method.kdes
    configuration_constraints=fh,
    velocity_constraints=fhd,
    forcelist=method.loads, bodies=method.bodies)

[Davide-sd]

Thank you very much for this explanation, everything is much clearer now :)