Chapter-24-Part-9-Quick-Return-Mechanism.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "e873cdf5",
   "metadata": {},
   "source": [
    "## Quick Return Mechanism"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2198b2ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "import sympy.physics.mechanics as me\n",
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "39a110ff",
   "metadata": {},
   "source": [
    "### EoM generation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5d66483f",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3, q4, q5 = me.dynamicsymbols(\"q1, q2, q3, q4, q5\")\n",
    "q1d, q2d, q3d, q4d, q5d = me.dynamicsymbols(\"q1, q2, q3, q4, q5\", 1)\n",
    "u1, u2, u3, u4, u5 = me.dynamicsymbols(\"u1, u2, u3, u4, u5\")\n",
    "T, F = me.dynamicsymbols(\"T, F\") # Torque and Force\n",
    "d1, d2, r1, r2, L = symbols(\"d1, d2, r1, r2, L\")\n",
    "\n",
    "# reaction forces\n",
    "RxO, RyO, RyR2 = me.dynamicsymbols(\"R_xO, R_yO, R_yR2\")\n",
    "# auxiliary velocities\n",
    "u1_aux, u2_aux, u3_aux = u_aux = me.dynamicsymbols(\"u_aux^((1:4))\")\n",
    "\n",
    "N, A, B, C = symbols(\"N, A, B, C\", cls=me.ReferenceFrame)\n",
    "A.orient_axis(N, N.z, q1)\n",
    "B.orient_axis(N, N.z, q2)\n",
    "C.orient_axis(N, N.z, q3)\n",
    "\n",
    "O, P1, P2, Q, R1, R2, M = symbols(\"O, P1, P2, Q, R1, R2, M\", cls=me.Point)\n",
    "M.set_pos(O, -d1 * N.y)\n",
    "P1.set_pos(O, r1 * A.x)\n",
    "P2.set_pos(M, q4 * B.x)\n",
    "Q.set_pos(M, L * B.x)\n",
    "R1.set_pos(Q, r2 * C.x)\n",
    "R2.set_pos(O, q5 * N.x + d2 * N.y)\n",
    "\n",
    "G1 = O.locatenew(\"G1\", r1 / 2 * A.x)\n",
    "G2 = M.locatenew(\"G2\", L / 2 * B.x)\n",
    "G3 = Q.locatenew(\"G3\", r2 / 2 * C.x)\n",
    "\n",
    "O.set_vel(N, u1_aux * N.x + u2_aux * N.y)\n",
    "M.set_vel(N, 0)\n",
    "P1.v2pt_theory(O, N, A)\n",
    "P1.a2pt_theory(O, N, A)\n",
    "Q.v2pt_theory(M, N, B)\n",
    "Q.a2pt_theory(M, N, B)\n",
    "R1.v2pt_theory(Q, N, C)\n",
    "R1.a2pt_theory(Q, N, C)\n",
    "G1.v2pt_theory(O, N, A)\n",
    "G1.a2pt_theory(O, N, A)\n",
    "G2.v2pt_theory(M, N, B)\n",
    "G2.a2pt_theory(M, N, B)\n",
    "G3.v2pt_theory(Q, N, C)\n",
    "G3.a2pt_theory(Q, N, C)\n",
    "R2.set_vel(N, R2.vel(N) + u3_aux * N.y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "655b4ee5",
   "metadata": {},
   "outputs": [],
   "source": [
    "# constraints\n",
    "zero1 = P1.pos_from(O) + O.pos_from(P2)\n",
    "zero2 = R1.pos_from(O) + O.pos_from(R2)\n",
    "f_c = [zero1 & N.x, zero1 & N.y, zero2 & N.x, zero2 & N.y]\n",
    "\n",
    "t = symbols(\"t\")\n",
    "dzero1 = zero1.diff(t, N)\n",
    "dzero2 = zero2.diff(t, N)\n",
    "f_v = [dzero1 & N.x, dzero1 & N.y, dzero2 & N.x, dzero2 & N.y]\n",
    "\n",
    "q_ind = [q1]\n",
    "u_ind = [u1]\n",
    "q_dep = [q2, q3, q4, q5]\n",
    "u_dep = [u2, u3, u4, u5]\n",
    "\n",
    "kde = [\n",
    "    q1d - u1,\n",
    "    q2d - u2,\n",
    "    q3d - u3,\n",
    "    q4d - u4,\n",
    "    q5d - u5\n",
    "]\n",
    "\n",
    "kane = me.KanesMethod(\n",
    "    N,\n",
    "    q_ind=q_ind,\n",
    "    u_ind=u_ind,\n",
    "    q_dependent=q_dep,\n",
    "    u_dependent=u_dep,\n",
    "    kd_eqs=kde,\n",
    "    configuration_constraints=f_c,\n",
    "    velocity_constraints=f_v,\n",
    "    u_auxiliary=u_aux\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8aacca2",
   "metadata": {},
   "outputs": [],
   "source": [
    "m, ms = symbols(\"m, m_s\")\n",
    "inertia_dyac_crank = me.inertia(A, 0, 0, m * r1**2 / 12)\n",
    "inertia_dyac_rod_MQ = me.inertia(A, 0, 0, m * L**2 / 12)\n",
    "inertia_dyac_rod_QR1 = me.inertia(A, 0, 0, m * r2**2 / 12)\n",
    "\n",
    "crank = me.RigidBody(\"Crank\", G1, A, m, (inertia_dyac_crank, G1))\n",
    "rod_MQ = me.RigidBody(\"Rod MQ\", G2, B, m, (inertia_dyac_rod_MQ, G2))\n",
    "rod_QR1 = me.RigidBody(\"Rod QR1\", G3, C, m, (inertia_dyac_rod_QR1, G3))\n",
    "slider1 = me.Particle(\"Slider 1\", P2, ms)\n",
    "slider2 = me.Particle(\"Slider 2\", R2, ms)\n",
    "\n",
    "bodies = [crank, rod_MQ, rod_QR1, slider1, slider2]\n",
    "\n",
    "g = symbols(\"g\")\n",
    "loads = [\n",
    "    (G1, -m * g * N.y),\n",
    "    (G2, -m * g * N.y),\n",
    "    (G3, -m * g * N.y),\n",
    "    (P2, -ms * g * N.y),\n",
    "    (R2, -ms * g * N.y),\n",
    "    (R2, F * N.x),\n",
    "    (A, T * A.z),\n",
    "    (O, RxO * N.x + RyO * N.y),\n",
    "    (R2, RyR2 * N.y),\n",
    "]\n",
    "\n",
    "fr, frstar = kane.kanes_equations(bodies, loads)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a133b86c",
   "metadata": {},
   "outputs": [],
   "source": [
    "trigsimp(fr + frstar)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "561ad5a4-4e15-4580-80a0-040237d4c8da",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "9852830b",
   "metadata": {},
   "source": [
    "### Find Initial Conditions for the configuration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a7b99587",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from spb import plot_contour\n",
    "\n",
    "constants = [r1, r2, L, m, ms, g, d1, d2]\n",
    "values = [5, 5, 15, 12, 12, 9.81, 8, 8]\n",
    "constants = dict(zip(constants, values))\n",
    "\n",
    "# substitution dictionary\n",
    "sd = {\n",
    "    q1: np.deg2rad(165)\n",
    "}\n",
    "sd.update(constants)\n",
    "\n",
    "# visualize proper initial guesses for solving q2, q4\n",
    "q2q4_eq = [c.subs(sd) for c in f_c[:2]]\n",
    "plot_contour(*q2q4_eq, (q2, 0, 2*pi), (q4, 0, sd[L]), {\"levels\": [0]}, is_filled=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62487492",
   "metadata": {},
   "outputs": [],
   "source": [
    "nsolve(q2q4_eq, [q2, q4], [1.8, 12])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "251b4156",
   "metadata": {},
   "outputs": [],
   "source": [
    "# update the substitution dictionary with the newly found values\n",
    "sd.update({q2: 2.05003605544231, q4: 10.47404046241})\n",
    "\n",
    "# visualize proper initial guesses for solving q3, q5\n",
    "q3q5_eq = [c.subs(sd) for c in f_c[2:]]\n",
    "plot_contour(*q3q5_eq, (q3, 0, 2*pi), (q5, -10, 10), {\"levels\": [0]}, is_filled=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "872cf7bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "nsolve(q3q5_eq, [q3, q5], [0.5, -2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c16c80a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "sd.update({q3: 0.568018505594562, q5: -2.70172728198401})"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "743fc991",
   "metadata": {},
   "source": [
    "### Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d18fbe6",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydy.system import System\n",
    "\n",
    "def force_func(state, time):\n",
    "    return 15\n",
    "\n",
    "def torque_func(state, time):\n",
    "    return 1000 * np.exp(-time)\n",
    "\n",
    "# remove unused symbols, otherwise Pydy raises error\n",
    "constants.pop(d1, None)\n",
    "constants.pop(d2, None)\n",
    "\n",
    "sys = System(kane)\n",
    "sys.constants = constants\n",
    "sys.initial_conditions = {\n",
    "    q1: sd[q1],\n",
    "    q2: sd[q2],\n",
    "    q3: sd[q3],\n",
    "    q4: sd[q4],\n",
    "    q5: sd[q5],\n",
    "    u1: 0, u2: 0, u3: 0, u4: 0, u5: 0\n",
    "}\n",
    "sys.specifieds = {\n",
    "    F: force_func,\n",
    "    T: torque_func\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": "092a8729",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "coordinates = [q1, q2, q3, q4, q5]\n",
    "speeds = [u1, u2, u3, u4, u5]\n",
    "\n",
    "fig, ax = plt.subplots(2, 1, sharex=True)\n",
    "ax[0].plot(sys.times, results[:, :5])\n",
    "ax[0].set_ylabel('Coordinates')\n",
    "ax[0].legend([\"${}$\".format(latex(c)) for c in coordinates])\n",
    "ax[1].plot(sys.times, results[:, 5], label=\"$u_{1}(t)$ [rad / s]\")\n",
    "ax[1].plot(sys.times, results[:, 9], label=\"$u_{5}(t)$ [m / s]\")\n",
    "ax[1].legend()\n",
    "ax[1].set_xlabel('Time [s]')\n",
    "ax[1].set_ylabel('Velocities')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "523c3f7f",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydy.viz import Scene, VisualizationFrame, Cylinder, Cube\n",
    "\n",
    "radius = 0.2\n",
    "crank_geom = Cylinder(r1, radius, name=\"crank geom\", color=\"gray\")\n",
    "rod_MQ_geom = Cylinder(L, radius, name=\"rod MQ geom\", color=\"gray\")\n",
    "rod_QR1_geom = Cylinder(r2, radius, name=\"rod QR1 geom\", color=\"gray\")\n",
    "slider_q4 = Cube(3 * radius, name=\"Slider q4\", color=\"red\")\n",
    "slider_q5 = Cube(3 * radius, name=\"Slider q5\", color=\"red\")\n",
    "\n",
    "rf1, rf2, rf3 = symbols(\"rf1, rf2, rf3\", cls=me.ReferenceFrame)\n",
    "rf1.orient_axis(A, A.z, -pi / 2)\n",
    "rf2.orient_axis(B, B.z, -pi / 2)\n",
    "rf3.orient_axis(C, C.z, -pi / 2)\n",
    "\n",
    "crank_vf = VisualizationFrame(\"Crank VF\", rf1, G1, crank_geom)\n",
    "rod_MQ_vf = VisualizationFrame(\"Rod MQ VF\", rf2, G2, rod_MQ_geom)\n",
    "rod_QR1_vf = VisualizationFrame(\"Rod QR1 VF\", rf3, G3, rod_QR1_geom)\n",
    "slider_q4 = VisualizationFrame(\"Slider q4\", rf2, P2, slider_q4)\n",
    "slider_q5 = VisualizationFrame(\"Slider q5\", N, R2, slider_q5)\n",
    "\n",
    "# reinsert the previously removed values in order to visualize the system\n",
    "sys.constants.update({d1: 8, d2: 8})\n",
    "\n",
    "scene = Scene(N, O, crank_vf, rod_MQ_vf, rod_QR1_vf, slider_q4, slider_q5, times=sys.times)\n",
    "scene.states_symbols = coordinates + speeds\n",
    "scene.constants = sys.constants\n",
    "scene.states_trajectories = results\n",
    "scene.display_jupyter(axes_arrow_length=5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb5af65d",
   "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
}