Chapter-24-Part-8-Crank-Slider-Mechanism.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "3d5337a1",
   "metadata": {},
   "source": [
    "## Crank-Slider Mechanism"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e40540e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "import sympy.physics.mechanics as me\n",
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "23e163a4",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3 = me.dynamicsymbols(\"q1, q2, q3\")\n",
    "q1d, q2d, q3d = me.dynamicsymbols(\"q1, q2, q3\", 1)\n",
    "u1, u2, u3 = me.dynamicsymbols(\"u1, u2, u3\")\n",
    "T, F = me.dynamicsymbols(\"T, F\") # Torque and Force\n",
    "L1, L2 = symbols(\"L1, L2\")\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",
    "# G1: center of mass of the crank, OP\n",
    "# G2: center of mass of the rod, PQ\n",
    "O, P, Q, R, G1, G2 = symbols(\"O, P, Q, R, G1, G2\", cls=me.Point)\n",
    "P.set_pos(O, L1 * A.x)\n",
    "Q.set_pos(P, L2 * B.x)\n",
    "R.set_pos(O, q3 * N.x)\n",
    "G1.set_pos(O, L1 / 2 * A.x)\n",
    "G2.set_pos(P, L2 / 2 * B.x)\n",
    "\n",
    "O.set_vel(N, 0)\n",
    "P.v2pt_theory(O, N, A)\n",
    "P.a2pt_theory(O, N, A)\n",
    "G1.v2pt_theory(O, N, A)\n",
    "G1.a2pt_theory(O, N, A)\n",
    "Q.v2pt_theory(P, N, B)\n",
    "Q.a2pt_theory(P, N, B)\n",
    "G2.v2pt_theory(P, N, B)\n",
    "G2.a2pt_theory(P, N, B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "765ccade",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = symbols(\"t\")\n",
    "# configuration constraints: OR + (QP + QO) = 0\n",
    "zero = R.pos_from(O) + O.pos_from(Q)\n",
    "f_c = ImmutableDenseMatrix([zero & N.x, zero & N.y])\n",
    "display(zero, f_c.simplify())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ba3d4885",
   "metadata": {},
   "outputs": [],
   "source": [
    "# velocity constraints\n",
    "dzero = zero.diff(t, N)\n",
    "f_v = ImmutableDenseMatrix([dzero & N.x, dzero & N.y])\n",
    "display(f_v.simplify())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5c9a8f95",
   "metadata": {},
   "outputs": [],
   "source": [
    "q_ind = [q1]\n",
    "u_ind = [u1]\n",
    "q_dep = [q2, q3]\n",
    "u_dep = [u2, u3]\n",
    "kde = [q1d - u1, q2d - u2, q3d - u3]\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",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "adf60374",
   "metadata": {},
   "outputs": [],
   "source": [
    "g, m_crank, m_rod, m_slider = symbols(\"g, m_c, m_r, m_s\")\n",
    "id_crank = me.inertia(A, m_crank * L1**2 / 12, 0, m_crank * L1**2 / 12)\n",
    "id_rod = me.inertia(B, m_rod * L2**2 / 12, 0, m_rod * L2**2 / 12)\n",
    "inertia_crank = (id_crank, G1)\n",
    "inertia_rod = (id_rod, G2)\n",
    "\n",
    "crank = me.RigidBody(\"crank\", G1, A, m_crank, inertia_crank)\n",
    "rod = me.RigidBody(\"rod\", G2, B, m_rod, inertia_rod)\n",
    "slider = me.Particle(\"slider\", R, m_slider)\n",
    "bodies = [crank, rod, slider]\n",
    "\n",
    "loads = [\n",
    "    (G1, -m_crank * g * N.y),\n",
    "    (G2, -m_rod * g * N.y),\n",
    "    (R, -m_slider * g * N.y),\n",
    "    (R, F * N.x),\n",
    "    (A, T * A.z),\n",
    "]\n",
    "\n",
    "fr, frstar = kane.kanes_equations(bodies, loads)\n",
    "eoms = fr + frstar\n",
    "eoms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d586fe3a",
   "metadata": {},
   "outputs": [],
   "source": [
    "from spb import *\n",
    "\n",
    "constants = {\n",
    "    m_crank: 5,\n",
    "    m_rod: 10,\n",
    "    m_slider: 1,\n",
    "    L1: 1,\n",
    "    L2: 2.5,\n",
    "    g: 9.81\n",
    "}\n",
    "\n",
    "q2q3_eq = [c.subs(constants).subs(q1, pi/4) for c in f_c]\n",
    "plot_contour(*q2q3_eq, (q2, 0, 2*pi), (q3, 0, 5), {\"levels\": [0]}, is_filled=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "26cfed0f",
   "metadata": {},
   "outputs": [],
   "source": [
    "res = nsolve(q2q3_eq, [q2, q3], (6, 3))\n",
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50f61658",
   "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 100 * np.exp(-time)\n",
    "\n",
    "sys = System(kane)\n",
    "sys.constants = constants\n",
    "sys.initial_conditions = {\n",
    "    q1: pi / 4, q2: res[0], q3: res[1],\n",
    "    u1: 0, u2: 0, u3: 0\n",
    "}\n",
    "sys.specifieds = {\n",
    "    F: force_func,\n",
    "    T: torque_func,\n",
    "}\n",
    "\n",
    "fps = 60\n",
    "t0, tf = 0, 10\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": "667bb878-ddaf-4730-a0d3-85991cd8ce25",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "n_cols = results.shape[1]\n",
    "labels_1 = [\"$q_{%s}$\" % (i + 1) for i in range(int(n_cols / 2))]\n",
    "labels_2 = [\"$u_{%s}$\" % (i + 1) for i in range(int(n_cols / 2))]\n",
    "\n",
    "fig, ax = plt.subplots(2, 1, sharex=True, layout=\"constrained\")\n",
    "ax[0].plot(sys.times, results[:, :3])\n",
    "ax[0].set_ylabel(r\"Coordinates\")\n",
    "ax[0].legend(labels_1)\n",
    "ax[1].plot(sys.times, results[:, 3:])\n",
    "ax[1].legend(labels_2)\n",
    "ax[1].set_ylabel(r\"Velocities\")\n",
    "ax[1].set_xlabel(\"Time [s]\")\n",
    "fig.suptitle(\"States of the system\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a2bea877",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydy.viz import Scene, Cylinder, Cube, VisualizationFrame\n",
    "\n",
    "crank_geom = Cylinder(length=L1, radius=0.05, color='grey', name=\"crank\")\n",
    "rod_geom = Cylinder(length=L2, radius=0.05, color='grey', name=\"rod\")\n",
    "slider_geom = Cube(0.2, color=\"orange\", name=\"slider\")\n",
    "\n",
    "crank_frame, rod_frame = symbols(\"crank_frame, rod_frame\", cls=me.ReferenceFrame)\n",
    "crank_frame.orient_axis(A, A.z, -pi / 2)\n",
    "rod_frame.orient_axis(B, B.z, -pi / 2)\n",
    "crank_vf = VisualizationFrame('crank vf', crank_frame, G1, crank_geom)\n",
    "rod_vf = VisualizationFrame('rod vf', rod_frame, G2, rod_geom)\n",
    "slider_vf = VisualizationFrame('slider vf', N, R, slider_geom)\n",
    "\n",
    "scene = Scene(N, O, crank_vf, rod_vf, slider_vf, system=sys)\n",
    "scene.display_jupyter(axes_arrow_length=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "51f90168",
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib.animation import FuncAnimation\n",
    "from IPython.display import HTML\n",
    "\n",
    "def _crank(y, idx, r):\n",
    "    q1 = y[idx, 0]\n",
    "    x = [0, r * np.cos(q1)]\n",
    "    y = [0, r * np.sin(q1)]\n",
    "    return x, y\n",
    "\n",
    "def _rod(y, idx, r, L):\n",
    "    q1 = y[idx, 0]\n",
    "    q2 = y[idx, 1]\n",
    "    x = [r * np.cos(q1), r * np.cos(q1) + L * np.cos(q2)]\n",
    "    y = [r * np.sin(q1), r * np.sin(q1) + L * np.sin(q2)]\n",
    "    return x, y\n",
    "\n",
    "idx = 0\n",
    "fig, ax = plt.subplots()\n",
    "ax.axhline(0, linestyle=\":\", color=\"darkgray\")\n",
    "crank, = ax.plot(*_crank(results, idx, constants[L1]), label=\"crank\")\n",
    "rod, = ax.plot(*_rod(results, idx, constants[L1], constants[L2]), label=\"rod\")\n",
    "slider = ax.scatter(results[idx, 2], 0, color=\"r\", label=\"slider\")\n",
    "ax.legend()\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.axis([-1.5, 4, -2, 2])\n",
    "\n",
    "def update(idx):\n",
    "    crank.set_data(*_crank(results, idx, constants[L1]))\n",
    "    rod.set_data(*_rod(results, idx, constants[L1], constants[L2]))\n",
    "    slider.set_offsets([results[idx, 2], 0])\n",
    "    ax.set_title(\"t = {:.2f} s\".format(sys.times[idx]))\n",
    "\n",
    "ani = FuncAnimation(fig, update, frames=len(sys.times))\n",
    "HTML(ani.to_jshtml(fps=fps))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "73dab878-f77e-46f9-9884-07efc37243ed",
   "metadata": {},
   "source": [
    "### About constraints and accuracy of numerical results."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e6e36d5-b717-4057-8ba4-d2390dcb8224",
   "metadata": {},
   "outputs": [],
   "source": [
    "kane_without_fc = me.KanesMethod(\n",
    "    N,\n",
    "    q_ind=[q1, q2, q3],\n",
    "    u_ind=u_ind,\n",
    "    u_dependent=u_dep,\n",
    "    kd_eqs=kde,\n",
    "    velocity_constraints=f_v\n",
    ")\n",
    "fr_without_fc, frstar_without_fc = kane.kanes_equations(bodies, loads)\n",
    "eoms_without_fc = fr_without_fc + frstar_without_fc\n",
    "eoms_without_fc - eoms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e59a80d9-c3d9-4e7d-8e43-ec4d319894ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "f_c_func = lambdify(list(sys.constants.keys()) + sys.states, f_c.subs(kane.kindiffdict()))\n",
    "f_v_func = lambdify(list(sys.constants.keys()) + sys.states, f_v.subs(kane.kindiffdict()))\n",
    "f_c_num_res = f_c_func(*list(sys.constants.values()), *results.T).squeeze().T\n",
    "f_v_num_res = f_v_func(*list(sys.constants.values()), *results.T).squeeze().T\n",
    "\n",
    "fig, ax = plt.subplots(1, 2, layout=\"constrained\")\n",
    "ax[0].plot(sys.times, f_c_num_res[:, 0], label=\"$f_{c}[0]$\")\n",
    "ax[0].plot(sys.times, f_c_num_res[:, 1], label=\"$f_{c}[1]$\")\n",
    "ax[0].legend()\n",
    "ax[0].set_xlabel(\"Time [s]\")\n",
    "ax[0].set_ylabel(\"Error\")\n",
    "ax[0].set_title(\"Configuration Constraints\")\n",
    "\n",
    "ax[1].plot(sys.times, f_v_num_res[:, 0], label=\"$f_{v}[0]$\")\n",
    "ax[1].plot(sys.times, f_v_num_res[:, 1], label=\"$f_{v}[1]$\")\n",
    "ax[1].legend()\n",
    "ax[1].set_xlabel(\"Time [s]\")\n",
    "ax[1].set_ylabel(\"Error\")\n",
    "ax[1].set_title(\"Velocity Constraints\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3ac460a-658c-4786-b542-c01f2c190788",
   "metadata": {},
   "source": [
    "### Reaction forces and reaction torques."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "58ebd918-b428-4804-a783-e1115534a9f9",
   "metadata": {},
   "source": [
    "#### Computing *incorrect* reaction forces whe using this configuration constraint: $\\vec{OR} + (\\vec{QP} + \\vec{PO}) = \\vec{0}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fdd2ee74-d1ba-4949-9568-7825fb163954",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3 = me.dynamicsymbols(\"q1, q2, q3\")\n",
    "q1d, q2d, q3d = me.dynamicsymbols(\"q1, q2, q3\", 1)\n",
    "u1, u2, u3 = me.dynamicsymbols(\"u1, u2, u3\")\n",
    "T, F = me.dynamicsymbols(\"T, F\") # Torque and Force\n",
    "L1, L2 = symbols(\"L1, L2\")\n",
    "t = symbols(\"t\")\n",
    "\n",
    "RxO, RyO, TO, RyR = Rf = me.dynamicsymbols(\"R_xO, R_yO, T_O, R_yR\")\n",
    "uxO, uyO, uzO, uyR = u_aux = me.dynamicsymbols(\"u_aux^xO u_aux^yO u_aux^zO u_aux^yR\")\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",
    "A.set_ang_vel(N, A.ang_vel_in(N) + uzO * N.z)\n",
    "\n",
    "# G1: center of mass of the crank, OP\n",
    "# G2: center of mass of the rod, PQ\n",
    "O, P, Q, R, G1, G2 = symbols(\"O, P, Q, R, G1, G2\", cls=me.Point)\n",
    "P.set_pos(O, L1 * A.x)\n",
    "Q.set_pos(P, L2 * B.x)\n",
    "R.set_pos(O, q3 * N.x)\n",
    "G1.set_pos(O, L1 / 2 * A.x)\n",
    "G2.set_pos(P, L2 / 2 * B.x)\n",
    "\n",
    "O.set_vel(N, uxO * N.x + uyO * N.y)\n",
    "R.set_vel(N, R.pos_from(O).diff(t, N) + uyR * N.y)\n",
    "P.v2pt_theory(O, N, A)\n",
    "P.a2pt_theory(O, N, A)\n",
    "G1.v2pt_theory(O, N, A)\n",
    "G1.a2pt_theory(O, N, A)\n",
    "Q.v2pt_theory(P, N, B)\n",
    "Q.a2pt_theory(P, N, B)\n",
    "G2.v2pt_theory(P, N, B)\n",
    "G2.a2pt_theory(P, N, B)\n",
    "\n",
    "# configuration constraints: OR + (QP + QO) = 0\n",
    "zero = R.pos_from(O) + O.pos_from(Q)\n",
    "f_c = ImmutableDenseMatrix([zero & N.x, zero & N.y])\n",
    "# velocity constraints\n",
    "dzero = zero.diff(t, N)\n",
    "f_v = ImmutableDenseMatrix([dzero & N.x, dzero & N.y])\n",
    "\n",
    "q_ind = [q1]\n",
    "u_ind = [u1]\n",
    "q_dep = [q2, q3]\n",
    "u_dep = [u2, u3]\n",
    "kde = [q1d - u1, q2d - u2, q3d - u3]\n",
    "\n",
    "kane_config_1 = 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",
    ")\n",
    "\n",
    "g, m_crank, m_rod, m_slider = symbols(\"g, m_c, m_r, m_s\")\n",
    "id_crank = me.inertia(A, m_crank * L1**2 / 12, 0, m_crank * L1**2 / 12)\n",
    "id_rod = me.inertia(B, m_rod * L2**2 / 12, 0, m_rod * L2**2 / 12)\n",
    "inertia_crank = (id_crank, G1)\n",
    "inertia_rod = (id_rod, G2)\n",
    "\n",
    "crank = me.RigidBody(\"crank\", G1, A, m_crank, inertia_crank)\n",
    "rod = me.RigidBody(\"rod\", G2, B, m_rod, inertia_rod)\n",
    "slider = me.Particle(\"slider\", R, m_slider)\n",
    "bodies = [crank, rod, slider]\n",
    "\n",
    "loads = [\n",
    "    (G1, -m_crank * g * N.y),\n",
    "    (G2, -m_rod * g * N.y),\n",
    "    (R, -m_slider * g * N.y),\n",
    "    (R, F * N.x),\n",
    "    (A, T * A.z ),\n",
    "    (O, RxO * N.x + RyO * N.y + TO * N.z),\n",
    "    (R, RyR * N.y),\n",
    "    (A, TO * N.z)\n",
    "]\n",
    "\n",
    "fr, frstar = kane_config_1.kanes_equations(bodies, loads)\n",
    "eom_config_1 = fr + frstar\n",
    "display(eom_config_1[0])\n",
    "\n",
    "reactions = solve(kane_config_1.auxiliary_eqs, Rf)\n",
    "display(*[Eq(k, v) for k, v in reactions.items()])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "29001637-c4a8-409d-9e53-c2157c0b484d",
   "metadata": {},
   "source": [
    "#### Computing *incorrect* reaction forces whe using this configuration constraint: $\\vec{OQ} \\cdot \\hat{n}_{y} = 0$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea0b9ec5-f604-4373-adde-9af0ab87283e",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "import sympy.physics.mechanics as me\n",
    "me.init_vprinting()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b152279e-1937-4b05-9589-f6f1c5afac5a",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2 = me.dynamicsymbols(\"q1, q2\")\n",
    "q1d, q2d = me.dynamicsymbols(\"q1, q2\", 1)\n",
    "u1, u2 = me.dynamicsymbols(\"u1, u2\")\n",
    "T, F = me.dynamicsymbols(\"T, F\") # Torque and Force\n",
    "L1, L2 = symbols(\"L1, L2\")\n",
    "t = symbols(\"t\")\n",
    "\n",
    "RxO, RyO = Rf = me.dynamicsymbols(\"R_xO, R_yO\")\n",
    "uxO_aux, uyO_aux = u_aux = me.dynamicsymbols(\"u_aux^xO u_aux^yO\")\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",
    "# G1: center of mass of the crank, OP\n",
    "# G2: center of mass of the rod, PQ\n",
    "O, P, Q, G1, G2 = symbols(\"O, P, Q, G1, G2\", cls=me.Point)\n",
    "P.set_pos(O, L1 * A.x)\n",
    "Q.set_pos(P, L2 * B.x)\n",
    "G1.set_pos(O, L1 / 2 * A.x)\n",
    "G2.set_pos(P, L2 / 2 * B.x)\n",
    "\n",
    "# O.set_vel(N, 0)\n",
    "O.set_vel(N, uxO_aux * N.x + uyO_aux * N.y)\n",
    "P.v2pt_theory(O, N, A)\n",
    "P.a2pt_theory(O, N, A)\n",
    "G1.v2pt_theory(O, N, A)\n",
    "G1.a2pt_theory(O, N, A)\n",
    "Q.v2pt_theory(P, N, B)\n",
    "Q.a2pt_theory(P, N, B)\n",
    "G2.v2pt_theory(P, N, B)\n",
    "G2.a2pt_theory(P, N, B)\n",
    "\n",
    "# configuration constraints: Q can't move along N.y\n",
    "zero = Q.pos_from(O)\n",
    "f_c = ImmutableDenseMatrix([zero & N.y])\n",
    "# velocity constraints\n",
    "dzero = zero.diff(t, N)\n",
    "f_v = ImmutableDenseMatrix([dzero & N.y])\n",
    "\n",
    "q_ind = [q1, q2]\n",
    "u_ind = [u1]\n",
    "u_dep = [u2]\n",
    "kde = [q1d - u1, q2d - u2]\n",
    "\n",
    "kane_config_2 = me.KanesMethod(\n",
    "    N,\n",
    "    q_ind=q_ind,\n",
    "    u_ind=u_ind,\n",
    "    u_dependent=u_dep,\n",
    "    kd_eqs=kde,\n",
    "    velocity_constraints=f_v,\n",
    "    u_auxiliary=u_aux\n",
    ")\n",
    "\n",
    "g, m_crank, m_rod, m_slider = symbols(\"g, m_c, m_r, m_s\")\n",
    "id_crank = me.inertia(A, m_crank * L1**2 / 12, 0, m_crank * L1**2 / 12)\n",
    "id_rod = me.inertia(B, m_rod * L2**2 / 12, 0, m_rod * L2**2 / 12)\n",
    "inertia_crank = (id_crank, G1)\n",
    "inertia_rod = (id_rod, G2)\n",
    "\n",
    "crank = me.RigidBody(\"crank\", G1, A, m_crank, inertia_crank)\n",
    "rod = me.RigidBody(\"rod\", G2, B, m_rod, inertia_rod)\n",
    "slider = me.Particle(\"slider\", Q, m_slider)\n",
    "bodies = [crank, rod, slider]\n",
    "\n",
    "loads = [\n",
    "    (G1, -m_crank * g * N.y),\n",
    "    (G2, -m_rod * g * N.y),\n",
    "    (Q, -m_slider * g * N.y),\n",
    "    (Q, F * N.x),\n",
    "    (A, T * A.z),\n",
    "    (O, RxO * N.x + RyO * N.y),\n",
    "]\n",
    "\n",
    "fr, frstar = kane_config_2.kanes_equations(bodies, loads)\n",
    "eom_config_2 = fr + frstar\n",
    "reactions = solve(kane_config_2.auxiliary_eqs, Rf)\n",
    "display(*[Eq(k, v) for k, v in reactions.items()])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4d85f11f",
   "metadata": {},
   "source": [
    "## With the Joints Framework"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d0c35035",
   "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, Cube, VisualizationFrame\n",
    "me.init_vprinting()\n",
    "\n",
    "q1, q2, q3 = me.dynamicsymbols(\"q1, q2, q3\")\n",
    "u1, u2, u3 = me.dynamicsymbols(\"u1, u2, u3\")\n",
    "T, F = me.dynamicsymbols(\"T, F\") # Torque and Force\n",
    "L1, L2, m_crank, m_rod, m_slider, g = symbols(\"L1, L2, m_c, m_r, m_s, g\")\n",
    "t = symbols(\"t\")\n",
    "\n",
    "N, A, B, S = symbols(\"N, A, B, S\", cls=me.ReferenceFrame)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "54a7a616",
   "metadata": {},
   "outputs": [],
   "source": [
    "O, G1, G2, R = symbols(\"O, G1, G2, R\", cls=me.Point)\n",
    "\n",
    "wall = me.RigidBody(\"Wall\", O, N, 0)\n",
    "crank = me.RigidBody(\n",
    "    \"Crank\", G1, A, m_crank,\n",
    "    (me.inertia(A, m_crank * L1 ** 2 / 12, 0, m_crank * L1 ** 2 / 12), G1))\n",
    "rod = me.RigidBody(\n",
    "    \"Rod\", G2, B, m_rod,\n",
    "    (me.inertia(A, 0, 0, m_rod * L2 ** 2 / 12), G2))\n",
    "slider = me.Particle(\"Slider\", R, m_slider)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "66efb2f0-a65f-4d74-b7f7-e9015b8d7404",
   "metadata": {},
   "outputs": [],
   "source": [
    "system = me.System.from_newtonian(wall)\n",
    "system.add_joints(\n",
    "    me.PinJoint(\n",
    "        \"j1\", wall, crank, coordinates=q1, speeds=u1,\n",
    "        child_point=-L1 / 2 * crank.x,\n",
    "        joint_axis=wall.z\n",
    "    ),\n",
    "    me.PinJoint(\n",
    "        \"j2\", crank, rod, coordinates=q2, speeds=u2,\n",
    "        parent_point=L1 / 2 * crank.x,\n",
    "        child_point=-L2 / 2 * rod.x,\n",
    "        joint_axis=crank.z\n",
    "    ),\n",
    "    me.PrismaticJoint(\n",
    "        \"j3\", wall, slider, coordinates=q3, speeds=u3\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "71a9c099-8cb0-4f9c-aa55-1ec5a58189b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "zero = rod.masscenter.pos_from(slider.masscenter) + L2 / 2 * rod.x\n",
    "f_c = Matrix([zero & N.x, zero & N.y])\n",
    "f_c.simplify()\n",
    "f_v = f_c.diff(t)\n",
    "f_v.simplify()\n",
    "display(zero, f_c, f_v)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "85c6548a-2abc-4e6c-9a11-8559d2c9b763",
   "metadata": {},
   "outputs": [],
   "source": [
    "system.add_holonomic_constraints(*f_c)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d312f9de-9377-4628-943f-c2541f126baf",
   "metadata": {},
   "outputs": [],
   "source": [
    "system.velocity_constraints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "afd0e382-b973-4d1d-9ced-c44209bfdb1e",
   "metadata": {},
   "outputs": [],
   "source": [
    "system.q_ind = system.get_joint(\"j1\").coordinates\n",
    "system.u_ind = system.get_joint(\"j1\").speeds\n",
    "q_dep, u_dep = [], []\n",
    "for j in [\"j2\", \"j3\"]:\n",
    "    q_dep.extend(list(system.get_joint(j).coordinates))\n",
    "    u_dep.extend(list(system.get_joint(j).speeds))\n",
    "system.q_dep = q_dep\n",
    "system.u_dep = u_dep"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f2705b3-b5fd-46e3-8645-aeec7197f70a",
   "metadata": {},
   "outputs": [],
   "source": [
    "system.apply_uniform_gravity(-g * system.y)\n",
    "\n",
    "system.add_loads(\n",
    "    me.Force(R, F * N.x),\n",
    "    me.Torque(A, T * A.z),\n",
    ")\n",
    "\n",
    "system.validate_system()\n",
    "eoms = system.form_eoms()\n",
    "eoms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d353868d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydy.system import System as PydySystem\n",
    "\n",
    "def force_func(state, time):\n",
    "    return 15\n",
    "\n",
    "def torque_func(state, time):\n",
    "    return 100 * np.exp(-time)\n",
    "\n",
    "sys = PydySystem(system.eom_method)\n",
    "sys.constants = {\n",
    "    m_crank: 5,\n",
    "    m_rod: 10,\n",
    "    m_slider: 1,\n",
    "    L1: 1,\n",
    "    L2: 2.5,\n",
    "    g: 9.81\n",
    "}\n",
    "sys.initial_conditions = {\n",
    "    q1: pi / 4, q2: 5.21103059157059, q3: 3.10502254284291,\n",
    "    u1: 0, u2: 0, u3: 0\n",
    "}\n",
    "sys.specifieds = {\n",
    "    F: force_func,\n",
    "    T: torque_func,\n",
    "}\n",
    "\n",
    "fps = 60\n",
    "t0, tf = 0, 10\n",
    "n = int(fps * (tf - t0))\n",
    "sys.times = np.linspace(t0, tf, n)\n",
    "results = sys.integrate()\n",
    "\n",
    "crank_geom = Cylinder(length=L1, radius=0.05, color='grey', name=\"crank\")\n",
    "rod_geom = Cylinder(length=L2, radius=0.05, color='grey', name=\"rod\")\n",
    "slider_geom = Cube(0.2, color=\"orange\", name=\"slider\")\n",
    "\n",
    "crank_frame, rod_frame = symbols(\"crank_frame, rod_frame\", cls=me.ReferenceFrame)\n",
    "crank_frame.orient_axis(A, A.z, -pi / 2)\n",
    "rod_frame.orient_axis(B, B.z, -pi / 2)\n",
    "\n",
    "crank_vf = VisualizationFrame('crank vf', crank_frame, crank.masscenter, crank_geom)\n",
    "rod_vf = VisualizationFrame('rod vf', rod_frame, rod.masscenter, rod_geom)\n",
    "slider_vf = VisualizationFrame('slider vf', N, slider.masscenter, slider_geom)\n",
    "\n",
    "# arguments to Scene: reference frame, origin, visual frames, system\n",
    "O = crank.masscenter.locatenew(\"O\", -L1/2 * crank.frame.x)\n",
    "scene = Scene(N, O, crank_vf, rod_vf, slider_vf, system=sys)\n",
    "scene.display_jupyter(axes_arrow_length=5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99055e65",
   "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
}