Trebuchet Motion Constraint Equations

This short notebook derives the motion equations (using langrange multipliers) for several trebuchet constraints.

Using langrange multipliers, the motion equations and any relevant constraints can be written in the form:

\[\mathbf{M} \times \mathbf{\ddot{q}} + \frac{\partial \mathbf{f}^T}{\partial \mathbf{q}}\mathbf{\lambda}= \mathbf{g + h}\]

Here, \(\mathbf{M}\) is the inertia matrix of the system. It multiplies with \(\mathbf{\ddot{q}}\) the vector of angular accelerations. The Jacobian matrix of the constraint equations \(\frac{\partial \mathbf{f}^T}{\partial \mathbf{q}}\) multiplies with the vector of langrange multipliers \(\mathbf{\lambda}\). The remaining terms are put into the right-hand side of the equation, denoted as \(\mathbf{g} + \mathbf{h}\).

1. Setup

[1]:

from sympy import Eq, Function, diff, simplify, sin, symbols

# Define symbols
t = symbols("t")
l1, l2, l3, l4 = symbols(
    "l1 l2 l3 l4"
)  # Length of weight/projectile arm segments, and projectile/weight sling
h0 = symbols("h0")  # pivot height
theta, phi, psi = (
    Function("theta")(t),
    Function("phi")(t),
    Function("psi")(t),
)  # Arm angle, Weight angle, Projectile angle
dtheta_dt, dphi_dt, dpsi_dt = symbols(
    "dtheta_dt dphi_dt dpsi_dt"
)  # Time derivatives of angles

constant = symbols("constant")  # Constant

variables = [theta, phi, psi]
dvars_dt = [dtheta_dt, dphi_dt, dpsi_dt]


def compute_jacobian(constraint_eq: Eq) -> list:
    """Compute the Jacobian vector of the constraint equation.

    Compute with respect to the given variables.
    """
    J = [diff(constraint_eq.lhs, var) for var in variables]

    print("\nJacobian Vector:")
    for i, deriv in enumerate(J):
        print(f"d(constraint)/d{variables[i]}: {simplify(deriv)}")
    return J


def compute_rhs(J: list) -> None:
    """Compute the right-hand side of the motion equation."""
    # Multiply each jacobian component by their angle derivatives respectively
    J_times_dvars = [J[i] * var for i, var in enumerate(dvars_dt)]

    # Differentiate each jacobian component with respect to the angle variable,
    # respectively
    J_dot = [diff(J_times_dvars[i], var) for i, var in enumerate(variables)]

    # Multiply each differentiated component by their angle derivatives respectively
    J_dot = [J_dot[i] * dvars_dt[i] for i in range(len(dvars_dt))]

    # Sum J_dot components
    J_dot_sum = sum(J_dot)

    print("\nRight-hand side")
    print(simplify(-J_dot_sum))

2. Sliding constraint (hinged counterweight trebuchet)

We now derive the constraint equation, the matrix \(\frac{\partial \mathbf{f}^T}{\partial \mathbf{q}}\), and the term of the matrix \(\mathbf{g + h}\) corresponding to the row of this constraint equation.

[2]:

# The y-coordinate of the projectile is zero
constraint_eq = Eq(h0 - l2 * sin(theta) + l3 * sin(psi), 0)
print("Constraint Equation:")
print(simplify(constraint_eq))

# Derive jacobian
jacobian = compute_jacobian(constraint_eq)
# Derive right-hand side
compute_rhs(jacobian)

Constraint Equation:
Eq(h0 - l2*sin(theta(t)) + l3*sin(psi(t)), 0)

Jacobian Vector:
d(constraint)/dtheta(t): -l2*cos(theta(t))
d(constraint)/dphi(t): 0
d(constraint)/dpsi(t): l3*cos(psi(t))

Right-hand side
dpsi_dt**2*l3*sin(psi(t)) - dtheta_dt**2*l2*sin(theta(t))

3. Whipper - constrained weight

[3]:

# The weight's movement is restricted by the arm and sling. Its angular position
# relative to the arm is constant.
constraint_eq = Eq(phi - theta, 0)
print("Constraint Equation:")
print(simplify(constraint_eq))

# Derive jacobian
jacobian = compute_jacobian(constraint_eq)
# Derive right-hand side
compute_rhs(jacobian)

Constraint Equation:
Eq(phi(t) - theta(t), 0)

Jacobian Vector:
d(constraint)/dtheta(t): -1
d(constraint)/dphi(t): 1
d(constraint)/dpsi(t): 0

Right-hand side
0

4. Whipper - constrained sling

[4]:

# The projectile's movement is restricted by the arm. Its angular position relative to
# the arm is constant.
constraint_eq = Eq(psi - theta, 0)
print("Constraint Equation:")
print(simplify(constraint_eq))

# Derive jacobian
jacobian = compute_jacobian(constraint_eq)
# Derive right-hand side
compute_rhs(jacobian)

Constraint Equation:
Eq(psi(t) - theta(t), 0)

Jacobian Vector:
d(constraint)/dtheta(t): -1
d(constraint)/dphi(t): 0
d(constraint)/dpsi(t): 1

Right-hand side
0