Scissor Meachanism

This notebook models a scissor mechansim made from four rods, six hinges and two supports. as two indipendent rods joined by a hinge treated as constraint. The functions shown in the double pendulum notebook are used within the bar_joint_model class, which simplifies the construction of such mechanisms.

# Project and symbolic tools
import heyoka as hy
import quivira as qv

# Numerics and visualization
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML
from _animation import animate_bar_model

Initialization

Initialize the model and set the gravity constant.

model = qv.bar_joint_model()
model.set_g(9.81)

Building the model

The mechanism is being built. There are different ways the bars are added to the model, all highlighted here. `add_bar <../api.html#quivira.bar_joint_model.add_bar>`__ just adds the minimal information, length and mass. Theoretically, the equations of motion could be built from just this information. Eventually, it needs to be followed by `set_init_bar_attitude <../api.html#quivira.bar_joint_model.set_init_bar_attitude>`__ to add an initial position and rotation. `add_bar_at_attitude <../api.html#quivira.bar_joint_model.add_bar_at_attitude>`__ combines the two for simplicity. `add_bar_from_point_pair <../api.html#quivira.bar_joint_model.add_bar_from_point_pair>`__ uses the coordinates of a start and an end point.

Joints only require the indices of the bars they join together and the respective position along the bars. For a bar with midpoint position \((x, y)\) and angle \(\theta\), a point on the bar is described by

\[\begin{split}\mathbf{p}(s) = \begin{bmatrix} x \\ y \end{bmatrix} + s\,L \begin{bmatrix} \cos\theta \\ \sin\theta \end{bmatrix},\end{split}\]

where \(L\) is the bar length and \(s \in [-\tfrac{1}{2}, \tfrac{1}{2}]\) is the normalized position along the bar measured from its center. The endpoints correspond to \(s = -\tfrac{1}{2}\) and \(s = \tfrac{1}{2}\), while intermediate locations use any value in that interval.

Supports tie a point on a bar, defined with the \(s\) formulation from above, to a global position, and lock the dof in x and/or y. Thereofre, they are the only instance where both the topology and the initial state strictly have to be defined at the same time.

# First two bars
model.add_bar(length=np.sqrt(4**2 + 4**2), mass=1)
model.add_bar_at_attitude(x0=2, y0=2, theta0=-np.pi/4, length=np.sqrt(4**2 + 4**2), mass=1)
model.add_joint(bar_index=[0, 1], s=[0.0, 0.0])

# Second two bars
model.add_bar_from_point_pair(start=(4, 0), end=(8, 4), mass_per_length=(1/np.sqrt(4**2 + 4**2)))
model.add_bar_from_point_pair(start=(4, 4), end=(8, 0), mass_per_length=(1/np.sqrt(4**2 + 4**2)))
model.add_joint(bar_index=[2, 3], s=[0.0, 0.0])

# Connect them
model.add_joint(bar_index=[0, 3], s=[0.5, -0.5])
model.add_joint(bar_index=[1, 2], s=[0.5, -0.5])

# Make supports
model.add_support(bar_index=0, s=-0.5, dof_in_x=False, dof_in_y=False, x=0, y=0)

model.add_support(bar_index=1, s=-0.5, dof_in_x=False, dof_in_y=True, x=0, y=4)

Taylor integrator

The equations of motion are found, which we then use to instantiate heyoka Taylor adaptive integrator. We keep the Delassus matrix \(W\) to find singularities later.

ode_system, W = model.build_lagrange_eom(return_W=True)
ta = hy.taylor_adaptive(ode_system, compact_mode=True)

Initial state

Before we can integrate, we need to complete the setup of the initial state. While we set the initial positions and rotations of most bars when we added them to the model, we still need to assign an attitude to bar 0. All velocities are set to zero by default, which we can change for each bar with `set_init_bar_velocities <../api.html#quivira.bar_joint_model.set_init_bar_velocities>`__. Alternatively, we could also use `set_initial_state <../api.html#quivira.bar_joint_model.set_initial_state>`__ to set all initial states at the same time and disregard what we have initialized before, as we will see later in this notebook.

To check if we are satisfied with our model, we can print it. There should be no None values.

model.set_init_bar_attitude(bar_index=0, x0=2, y0=2, theta0=np.pi/4)
model.print_model()

Bars:
  Bar 0: length=5.656854249492381, mass=1
  Bar 1: length=5.656854249492381, mass=1
  Bar 2: length=5.656854249492381, mass=1.0
  Bar 3: length=5.656854249492381, mass=1.0

Joints:
  Joint 0: bar 0 at s=0.0, bar 1 at s=0.0
  Joint 1: bar 2 at s=0.0, bar 3 at s=0.0
  Joint 2: bar 0 at s=0.5, bar 3 at s=-0.5
  Joint 3: bar 1 at s=0.5, bar 2 at s=-0.5

Supports:
  Support 0: bar 0 at s=-0.5, position=(0, 0), dof_in_x=False, dof_in_y=False
  Support 1: bar 1 at s=-0.5, position=(0, 4), dof_in_x=False, dof_in_y=True

Initial state:
  Bar 0: x=2, y=2, theta=0.7853981633974483, xd=0, yd=0, thetad=0
  Bar 1: x=2, y=2, theta=-0.7853981633974483, xd=0, yd=0, thetad=0
  Bar 2: x=6.0, y=2.0, theta=0.7853981633974483, xd=0, yd=0, thetad=0
  Bar 3: x=6.0, y=2.0, theta=-0.7853981633974483, xd=0, yd=0, thetad=0

We can now extract that initial state as a list that will be passed to the integrator. We then use `check_constraint_fulfillment <../api.html#quivira.bar_joint_model.check_constraint_fulfillment>`__ to ensure that the initial state is possible with the topological constraints we defined in the beginning.

state0 = model.get_initial_state()
model.check_constraint_fulfillment(state0)

Simulation

Now we can integrate, sampling 401 times and for a duration of 1.2 seconds.

ta.time = 0.0
ta.state[:] = state0

t_end = 1
tgrid = np.linspace(0.0, t_end, 401)
solution = ta.propagate_grid(tgrid)[-1]

Animate the solution

animate_bar_model(model, solution)

Once Loop Reflect

New state as initial state and find singularities

We can now extract the final state of the integration, and set it as the initial state for a new integration.

As the structure collapses, we expect singularities in the Delassius Matrix \(W\). We find them using the reciprocal condition number rcon. Using a callback with a tolerance threshold of 1e-6, we monitor rcon after each integration step. So finding the singularity depends on the threshold and the integration step, whcih, if too big, can lead to us not finding the singularity. The advantage of this approach is that it is general for any topology, with the major disadvantage that finding the singularity depends on the step size, depreciating the advantage of choosing an adaptive step size.

# Prepare second integration
final_state = solution[-1]
model.set_initial_state(final_state)
state1 = model.get_initial_state()

ta.time = 0.0
ta.state[:] = state1

# Compile the symbolic Delassus matrix W
W_array = np.asarray(W, dtype=object)
n = W_array.shape[0]
state_vars = [var for var, rhs in ode_system]

W_func = hy.cfunc(W_array.flatten().tolist(), state_vars, compact_mode=True)

# Stop when cond(W) exceeds 10^6
threshold = 1e-6

def get_rcond(state):
    W_num = np.asarray(W_func(state)).reshape(n, n)
    W_num = 0.5 * (W_num + W_num.T)

    if not np.all(np.isfinite(W_num)):
        return np.nan

    eigvals = np.linalg.eigvalsh(W_num)

    if eigvals[-1] <= 0:
        return 0.0

    return eigvals[0] / eigvals[-1]


def singularity_callback(ta):
    rcond = get_rcond(ta.state)

    if not np.isfinite(rcond) or rcond < threshold:
        print(f"Near-singularity detected at t = {ta.time:.8f}")
        print(f"rcond(W) = {rcond:.3e}")
        return False  # Stop integration

    return True  # Continue integration


# Second integration with singularity detection
t_end = 1.0
tgrid = np.linspace(0.0, t_end, 401)

if not np.isfinite(get_rcond(ta.state)) or get_rcond(ta.state) < threshold:
    raise ValueError("Second integration starts near a singularity")

result = ta.propagate_grid(
    tgrid,
    callback=singularity_callback,
    max_delta_t=0.001
)

solution2 = result[-1]
print(f"Integration ended at t = {ta.time:.8f}")

Near-singularity detected at t = 0.18400000
rcond(W) = 7.422e-07
Integration ended at t = 0.18400000

Animate again

animate_bar_model(model, solution2)

Once Loop Reflect