Chapter-24-Part-7-Pendulum-with-Plate.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "aae02f6b",
   "metadata": {},
   "source": [
    "## Square plate attached at the end of a pendulum"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c2691f30",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "import sympy.physics.mechanics as me\n",
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "67bbffd4",
   "metadata": {},
   "outputs": [],
   "source": [
    "a, L, g, m_rod, m_plate = symbols(\"a, L, g, m, M\")\n",
    "q1, q2 = me.dynamicsymbols(\"q1, q2\")\n",
    "q1d, q2d = me.dynamicsymbols(\"q1, q2\", 1)\n",
    "omega1, omega2 = me.dynamicsymbols(\"omega_1, omega_2\")\n",
    "\n",
    "# reaction forces acting on point O\n",
    "Rx, Ry = me.dynamicsymbols(\"R_x, R_y\")\n",
    "# auxiliary velocities needed to compute the reaction forces\n",
    "u1_aux, u2_aux = me.dynamicsymbols(\"u_aux^((1:3))\")\n",
    "\n",
    "N, A, B = symbols(\"N, A, B\", cls=me.ReferenceFrame)\n",
    "A.orient_axis(N, N.z, q1)\n",
    "B.orient_axis(N, N.z, q2)\n",
    "\n",
    "O, P, G, C, R = symbols(\"O, P, G, C, R\", cls=me.Point)\n",
    "P.set_pos(O, -L * A.y)\n",
    "G.set_pos(O, -L / 2 * A.y)\n",
    "C.set_pos(P, a / 2 * (B.x - B.y))\n",
    "R.set_pos(P, -a * B.y)\n",
    "\n",
    "O.set_vel(N, u1_aux * N.x + u2_aux * N.y)\n",
    "P.v2pt_theory(O, N, A)\n",
    "P.a2pt_theory(O, N, A)\n",
    "G.v2pt_theory(O, N, A)\n",
    "G.a2pt_theory(O, N, A)\n",
    "C.v2pt_theory(P, N, B)\n",
    "C.a2pt_theory(P, N, B)\n",
    "R.v2pt_theory(P, N, B)\n",
    "R.a2pt_theory(P, N, B)\n",
    "\n",
    "coordinates = [q1, q2]\n",
    "velocities = [omega1, omega2]\n",
    "kde = [q1d - omega1, q2d - omega2]\n",
    "kane = me.KanesMethod(\n",
    "    N,\n",
    "    q_ind=coordinates, \n",
    "    u_ind=velocities,\n",
    "    kd_eqs=kde,\n",
    "    u_auxiliary=[u1_aux, u2_aux]\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "723fa189-ad52-41ae-ae77-0665844b7711",
   "metadata": {},
   "source": [
    "Number of operations to compute the DCM from B to N:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16558843-884a-4609-b7b6-5d7ebd5476f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "dcm_B_N = B.dcm(N)\n",
    "display(\n",
    "    dcm_B_N,\n",
    "    count_ops(dcm_B_N),\n",
    "    count_ops(dcm_B_N, visual=True)\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7b524132",
   "metadata": {},
   "outputs": [],
   "source": [
    "id_rod = me.inertia(A, m_rod * L**2 / 12, 0, m_rod * L**2 / 12)\n",
    "id_plate = me.inertia(B, m_plate * a**2 / 12, m_plate * a**2 / 12, m_plate * a**2 / 6)\n",
    "inertia_rod = (id_rod, G)\n",
    "inertia_plate = (id_plate, C)\n",
    "\n",
    "rod = me.RigidBody(\"rod\", G, A, m_rod, inertia_rod)\n",
    "plate = me.RigidBody(\"plate\", C, B, m_plate, inertia_plate)\n",
    "bodies = [rod, plate]\n",
    "\n",
    "loads = [\n",
    "    (G, -m_rod * g * N.y),\n",
    "    (C, -m_plate * g * N.y),\n",
    "    (O, Rx * N.x + Ry * N.y)\n",
    "]\n",
    "\n",
    "fr, frstar = kane.kanes_equations(bodies, loads)\n",
    "trigsimp(fr + frstar)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a943d30c-b86a-4e57-8853-ac14c1e44280",
   "metadata": {},
   "source": [
    "### Numerical results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "186948c5",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydy.system import System\n",
    "\n",
    "sys = System(kane)\n",
    "sys.constants = {\n",
    "    m_rod: 0,\n",
    "    m_plate: 10,\n",
    "    a: 6,\n",
    "    L: 12,\n",
    "    g: 9.81\n",
    "}\n",
    "sys.initial_conditions = {\n",
    "    q1: 0, q2: 0,\n",
    "    omega1: 0, omega2: 0\n",
    "}\n",
    "\n",
    "fps = 60\n",
    "t0, tf = 0, 15\n",
    "n = int(fps * (tf - t0))\n",
    "sys.times = np.linspace(t0, tf, n)\n",
    "\n",
    "results = sys.integrate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "689c3b30",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "results_deg = np.rad2deg(results)\n",
    "fig, ax = plt.subplots(2, 1, sharex=True)\n",
    "ax[0].plot(sys.times, results_deg[:, 0], label=\"$q_{1}$\")\n",
    "ax[0].plot(sys.times, results_deg[:, 1], label=\"$q_{2}$\")\n",
    "ax[0].set_ylabel(r\"Angle [deg]\")\n",
    "ax[0].legend()\n",
    "ax[1].plot(sys.times, results_deg[:, 2], label=r\"$\\omega_{1}$\")\n",
    "ax[1].plot(sys.times, results_deg[:, 3], label=r\"$\\omega_{2}$\")\n",
    "ax[1].set_ylabel(r\"Velocity [deg / s]\")\n",
    "ax[1].set_xlabel(\"Time [s]\")\n",
    "ax[1].legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2aa8d470-18d0-475b-a315-b1bab7077ccd",
   "metadata": {},
   "source": [
    "### Compute reaction forces at O"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "67fde014-d3ab-4fba-9e9f-5102e5e8d6b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "R_eqns = solve(kane.auxiliary_eqs, [Rx, Ry])\n",
    "R_eqns = Matrix(list(R_eqns.values())).T\n",
    "R_eqns.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fd19a227-0919-436b-8dee-a3ad57ebbf0a",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = symbols(\"t\")\n",
    "arguments_1 = [q1, q2, omega1, omega2] + list(sys.constants.keys())\n",
    "arguments_2 = (\n",
    "    [q1, q2, omega1, omega2] +\n",
    "    [q1d, q2d, omega1.diff(t), omega2.diff(t)] +\n",
    "    list(sys.constants.keys())\n",
    ")\n",
    "\n",
    "mm_func = lambdify(arguments_1, kane.mass_matrix_full)\n",
    "fm_func = lambdify(arguments_1, kane.forcing_full)\n",
    "R_eqns_func = lambdify(arguments_2, R_eqns)\n",
    "\n",
    "states_dot = np.zeros_like(results)\n",
    "reactions = np.zeros((results.shape[0], 2))\n",
    "for i in range(results.shape[0]):\n",
    "    args_1 = list(results[i, :]) + list(sys.constants.values())\n",
    "    states_dot[i, :] = np.linalg.solve(\n",
    "        mm_func(*args_1),\n",
    "        fm_func(*args_1)\n",
    "    ).flatten()\n",
    "    args_2 = list(results[i, :]) + list(states_dot[i, :]) + list(sys.constants.values())\n",
    "    reactions[i, :] = R_eqns_func(*args_2)\n",
    "\n",
    "fig, ax = plt.subplots()\n",
    "ax.plot(sys.times, reactions[:, 0], label=\"$R_{x}$\")\n",
    "ax.plot(sys.times, reactions[:, 1], label=\"$R_{y}$\")\n",
    "ax.set_xlabel(\"time [s]\")\n",
    "ax.set_ylabel(\"force [N]\")\n",
    "ax.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "028a5b32-9566-4eae-ac17-b6f7142e3d70",
   "metadata": {},
   "source": [
    "### Compute velocity of point P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c00a8294-f6fb-4cfd-ab33-08ea177d628a",
   "metadata": {},
   "outputs": [],
   "source": [
    "v_P = P.vel(N).to_matrix(N)\n",
    "kindiffdict = kane.kindiffdict()\n",
    "aux_dict = {aux_speed: 0 for aux_speed in [u1_aux, u2_aux]}\n",
    "v_P = v_P.subs(kindiffdict).subs(aux_dict)\n",
    "v_P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0a3674c-68e5-47fc-8f68-b497c22bdf8e",
   "metadata": {},
   "outputs": [],
   "source": [
    "v_P_x_func = lambdify([L, q1, q2, omega1, omega2], v_P[0])\n",
    "v_P_y_func = lambdify([L, q1, q2, omega1, omega2], v_P[1])\n",
    "v_P_x_res = v_P_x_func(sys.constants[L], results[:, 0], results[:, 1], results[:, 2], results[:, 3])\n",
    "v_P_y_res = v_P_y_func(sys.constants[L], results[:, 0], results[:, 1], results[:, 2], results[:, 3])\n",
    "\n",
    "fig, ax = plt.subplots()\n",
    "ax.plot(sys.times, v_P_x_res, label=r\"$\\vec{v}_{P} \\cdot \\hat{n}_{x}$\")\n",
    "ax.plot(sys.times, v_P_y_res, label=r\"$\\vec{v}_{P} \\cdot \\hat{n}_{y}$\")\n",
    "ax.legend()\n",
    "ax.set_xlabel(\"Time [s]\")\n",
    "ax.set_ylabel(\"Velocity [m/s]\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "696b78b7",
   "metadata": {},
   "source": [
    "### Animation with PyDy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "20c01901",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydy.viz import Scene, Cylinder, Box, VisualizationFrame\n",
    "\n",
    "rod_geom = Cylinder(length=L, radius=0.5, color='grey', name=\"rod\")\n",
    "plate_geom = Box(a, a, 0.5, color=\"orange\", name=\"plate\")\n",
    "\n",
    "rod_vf = VisualizationFrame('rod vf', A, G, rod_geom)\n",
    "plate_vf = VisualizationFrame('plate vf', B, C, plate_geom)\n",
    "\n",
    "# arguments to Scene: reference frame, origin, visual frames, system\n",
    "scene = Scene(N, O, rod_vf, plate_vf, system=sys)\n",
    "scene.display_jupyter(axes_arrow_length=5)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d08665a7",
   "metadata": {},
   "source": [
    "### Animation with Matplotlib"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e00486b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib.animation import FuncAnimation\n",
    "from IPython.display import HTML\n",
    "\n",
    "fig, ax = plt.subplots()\n",
    "\n",
    "def get_line_coords(results, i):\n",
    "    q1_val = results[i, 0]\n",
    "    L_val = sys.constants[L]\n",
    "    y = [0, -L_val * np.cos(q1_val)]\n",
    "    x = [0, L_val * np.sin(q1_val)]\n",
    "    return x, y\n",
    "\n",
    "R = lambda angle: np.array([\n",
    "    [np.cos(angle), np.sin(angle), 0],\n",
    "    [-np.sin(angle), np.cos(angle), 0],\n",
    "    [0, 0, 1],\n",
    "])\n",
    "\n",
    "standard_plate = np.array([\n",
    "    [0, 0, 1],\n",
    "    [1, 0, 1],\n",
    "    [1, 1, 1],\n",
    "    [0, 1, 1],\n",
    "    [0, 0, 1],\n",
    "])\n",
    "\n",
    "def get_plate_coords(results, i):\n",
    "    q1_val = results[i, 0]\n",
    "    q2_val = results[i, 1]\n",
    "    a_val = sys.constants[a]\n",
    "    L_val = sys.constants[L]\n",
    "    rotated_plate = (a_val * standard_plate) @ R(q2_val)\n",
    "    y = -L_val * np.cos(q1_val) - rotated_plate[:, 0]\n",
    "    x = L_val * np.sin(q1_val) + rotated_plate[:, 1]\n",
    "    return x, y\n",
    "\n",
    "idx = 0\n",
    "line, = ax.plot(*get_line_coords(results, idx))\n",
    "plate, = ax.plot(*get_plate_coords(results, idx))\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.axis([-15, 15, -25, 5])\n",
    "\n",
    "def update(idx):\n",
    "    line.set_data(*get_line_coords(results, idx))\n",
    "    plate.set_data(*get_plate_coords(results, idx))\n",
    "    ax.set_title(\"Time = {:.2f} s\".format(sys.times[idx]))\n",
    "\n",
    "ani = FuncAnimation(fig, func=update, frames=len(sys.times))\n",
    "HTML(ani.to_jshtml(fps=fps))\n",
    "# ani.save(\"pendulum-square-plate.mp4\", fps=60)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "321bc156",
   "metadata": {},
   "source": [
    "## With the Joints Framework"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ba3ad76",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "import sympy.physics.mechanics as me\n",
    "import numpy as np\n",
    "from pydy.system import System\n",
    "from pydy.viz import Scene, Cylinder, Box, VisualizationFrame, Sphere\n",
    "me.init_vprinting()\n",
    "\n",
    "a, L, g, m_rod, m_plate = symbols(\"a, L, g, m, M\")\n",
    "q1, q2 = me.dynamicsymbols(\"q1, q2\")\n",
    "omega1, omega2 = me.dynamicsymbols(\"omega_1, omega_2\")\n",
    "M, N, A, B = symbols(\"M, N, A, B\", cls=me.ReferenceFrame)\n",
    "N.orient_axis(M, M.z, pi)\n",
    "\n",
    "G, C = symbols(\"G, C\", cls=me.Point)\n",
    "\n",
    "wall = me.RigidBody(\"Wall\", frame=N, mass=0)\n",
    "rod = me.RigidBody(\n",
    "    \"Rod\", G, A, m_rod,\n",
    "    (me.inertia(A, 0, 0, m_rod * L**2 / 12), G)\n",
    ")\n",
    "plate = me.RigidBody(\n",
    "    \"Plate\", C, B, m_plate,\n",
    "    (me.inertia(B, m_plate * a**2 / 12, m_plate * a**2 / 12, m_plate * a**2 / 6), C)\n",
    ")\n",
    "\n",
    "system = me.System.from_newtonian(wall)\n",
    "system.add_joints(\n",
    "    me.PinJoint(\n",
    "        \"Joint 1\", wall, rod, coordinates=q1, speeds=omega1,\n",
    "        child_point=-L / 2 * rod.frame.y,\n",
    "        joint_axis=wall.frame.z\n",
    "    ),\n",
    "    me.PinJoint(\n",
    "        \"Joint 2\", rod, plate, coordinates=q2, speeds=omega2,\n",
    "        parent_point=L / 2 * rod.frame.y,\n",
    "        child_point=a / 2 * (B.x - B.y),\n",
    "        joint_axis=rod.frame.z\n",
    "    )\n",
    ")\n",
    "\n",
    "system.apply_uniform_gravity(g * system.y)\n",
    "\n",
    "system.validate_system()\n",
    "eoms = system.form_eoms()\n",
    "eoms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "11d0be23-e8b8-47d4-a12d-7218599dcbf3",
   "metadata": {},
   "outputs": [],
   "source": [
    "dcm_B_N = B.dcm(N)\n",
    "display(\n",
    "    dcm_B_N,\n",
    "    count_ops(dcm_B_N),\n",
    "    count_ops(dcm_B_N, visual=True)\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ccb947fb",
   "metadata": {},
   "outputs": [],
   "source": [
    "sys = System(kane)\n",
    "sys.constants = {\n",
    "    m_rod: 0,\n",
    "    m_plate: 10,\n",
    "    a: 6,\n",
    "    L: 12,\n",
    "    g: 9.81\n",
    "}\n",
    "sys.initial_conditions = {\n",
    "    q1: 0, q2: pi / 2,\n",
    "    omega1: 0, omega2: 0\n",
    "}\n",
    "\n",
    "fps = 60\n",
    "t0, tf = 0, 15\n",
    "n = int(fps * (tf - t0))\n",
    "sys.times = np.linspace(t0, tf, n)\n",
    "\n",
    "results = sys.integrate()\n",
    "\n",
    "rod_geom = Cylinder(length=L, radius=0.5, color='grey', name=\"rod\")\n",
    "plate_geom = Box(a, a, 0.5, color=\"orange\", name=\"plate\")\n",
    "\n",
    "rod_vf = VisualizationFrame('rod vf', A, rod.masscenter, rod_geom)\n",
    "plate_vf = VisualizationFrame('plate vf', B, plate.masscenter, plate_geom)\n",
    "\n",
    "# arguments to Scene: reference frame, origin, visual frames, system\n",
    "O = rod.masscenter.locatenew(\"O\", -L/2 * rod.frame.y)\n",
    "scene = Scene(M, O, rod_vf, plate_vf, system=sys)\n",
    "scene.display_jupyter(axes_arrow_length=5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "34670727-8c88-442b-adf1-394014d8f747",
   "metadata": {},
   "outputs": [],
   "source": [
    "B.dcm(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ed40198e-0447-4df1-a848-bdc0a6107099",
   "metadata": {},
   "outputs": [],
   "source": [
    "count_ops(B.dcm(N))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bfd18e49-3e5e-4a6c-af9d-2c189a114a34",
   "metadata": {},
   "outputs": [],
   "source": [
    "count_ops(B.dcm(N), visual=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "daf6c7d0-4d73-44d7-af3c-a7b741bb91e0",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}