{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0",
   "metadata": {},
   "source": [
    "# Trebuchet Motion Constraint Equations\n",
    "\n",
    "This short notebook derives the motion equations (using langrange multipliers) for several trebuchet constraints.\n",
    "\n",
    "Using langrange multipliers, the motion equations and any relevant constraints can be written in the form:\n",
    "$$\n",
    "\\mathbf{M} \\times \\mathbf{\\ddot{q}} + \\frac{\\partial \\mathbf{f}^T}{\\partial \\mathbf{q}}\\mathbf{\\lambda}= \\mathbf{g + h}\n",
    "$$\n",
    "\n",
    "Here, $\\mathbf{M}$ is the inertia matrix of the system. It multiplies with $\\mathbf{\\ddot{q}}$ the vector of angular accelerations.\n",
    "The Jacobian matrix of the constraint equations $\\frac{\\partial \\mathbf{f}^T}{\\partial \\mathbf{q}}$ multiplies with the vector of langrange multipliers $\\mathbf{\\lambda}$. \n",
    "The remaining terms are put into the right-hand side of the equation, denoted as $\\mathbf{g} + \\mathbf{h}$. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1",
   "metadata": {},
   "source": [
    "## 1. Setup"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import Eq, Function, diff, simplify, sin, symbols\n",
    "\n",
    "# Define symbols\n",
    "t = symbols(\"t\")\n",
    "l1, l2, l3, l4 = symbols(\n",
    "    \"l1 l2 l3 l4\"\n",
    ")  # Length of weight/projectile arm segments, and projectile/weight sling\n",
    "h0 = symbols(\"h0\")  # pivot height\n",
    "theta, phi, psi = (\n",
    "    Function(\"theta\")(t),\n",
    "    Function(\"phi\")(t),\n",
    "    Function(\"psi\")(t),\n",
    ")  # Arm angle, Weight angle, Projectile angle\n",
    "dtheta_dt, dphi_dt, dpsi_dt = symbols(\n",
    "    \"dtheta_dt dphi_dt dpsi_dt\"\n",
    ")  # Time derivatives of angles\n",
    "\n",
    "constant = symbols(\"constant\")  # Constant\n",
    "\n",
    "variables = [theta, phi, psi]\n",
    "dvars_dt = [dtheta_dt, dphi_dt, dpsi_dt]\n",
    "\n",
    "\n",
    "def compute_jacobian(constraint_eq: Eq) -> list:\n",
    "    \"\"\"Compute the Jacobian vector of the constraint equation.\n",
    "\n",
    "    Compute with respect to the given variables.\n",
    "    \"\"\"\n",
    "    J = [diff(constraint_eq.lhs, var) for var in variables]\n",
    "\n",
    "    print(\"\\nJacobian Vector:\")\n",
    "    for i, deriv in enumerate(J):\n",
    "        print(f\"d(constraint)/d{variables[i]}: {simplify(deriv)}\")\n",
    "    return J\n",
    "\n",
    "\n",
    "def compute_rhs(J: list) -> None:\n",
    "    \"\"\"Compute the right-hand side of the motion equation.\"\"\"\n",
    "    # Multiply each jacobian component by their angle derivatives respectively\n",
    "    J_times_dvars = [J[i] * var for i, var in enumerate(dvars_dt)]\n",
    "\n",
    "    # Differentiate each jacobian component with respect to the angle variable,\n",
    "    # respectively\n",
    "    J_dot = [diff(J_times_dvars[i], var) for i, var in enumerate(variables)]\n",
    "\n",
    "    # Multiply each differentiated component by their angle derivatives respectively\n",
    "    J_dot = [J_dot[i] * dvars_dt[i] for i in range(len(dvars_dt))]\n",
    "\n",
    "    # Sum J_dot components\n",
    "    J_dot_sum = sum(J_dot)\n",
    "\n",
    "    print(\"\\nRight-hand side\")\n",
    "    print(simplify(-J_dot_sum))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3",
   "metadata": {},
   "source": [
    "## 2. Sliding constraint (hinged counterweight trebuchet)\n",
    "\n",
    "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."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# The y-coordinate of the projectile is zero\n",
    "constraint_eq = Eq(h0 - l2 * sin(theta) + l3 * sin(psi), 0)\n",
    "print(\"Constraint Equation:\")\n",
    "print(simplify(constraint_eq))\n",
    "\n",
    "# Derive jacobian\n",
    "jacobian = compute_jacobian(constraint_eq)\n",
    "# Derive right-hand side\n",
    "compute_rhs(jacobian)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5",
   "metadata": {},
   "source": [
    "## 3. Whipper - constrained weight"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# The weight's movement is restricted by the arm and sling. Its angular position\n",
    "# relative to the arm is constant.\n",
    "constraint_eq = Eq(phi - theta, 0)\n",
    "print(\"Constraint Equation:\")\n",
    "print(simplify(constraint_eq))\n",
    "\n",
    "# Derive jacobian\n",
    "jacobian = compute_jacobian(constraint_eq)\n",
    "# Derive right-hand side\n",
    "compute_rhs(jacobian)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7",
   "metadata": {},
   "source": [
    "## 4. Whipper - constrained sling"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8",
   "metadata": {},
   "outputs": [],
   "source": [
    "# The projectile's movement is restricted by the arm. Its angular position relative to\n",
    "# the arm is constant.\n",
    "constraint_eq = Eq(psi - theta, 0)\n",
    "print(\"Constraint Equation:\")\n",
    "print(simplify(constraint_eq))\n",
    "\n",
    "# Derive jacobian\n",
    "jacobian = compute_jacobian(constraint_eq)\n",
    "# Derive right-hand side\n",
    "compute_rhs(jacobian)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv (3.13.9)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.13.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}