{ "cells": [ { "cell_type": "code", "execution_count": null, "id": "509a68dd", "metadata": {}, "outputs": [], "source": [ "import sympy as sm\n", "import sympy.physics.mechanics as me\n", "me.init_vprinting()" ] }, { "cell_type": "code", "execution_count": null, "id": "8610daa7", "metadata": {}, "outputs": [], "source": [ "m, g, kt, kl, l = sm.symbols('m, g, k_t, k_l, l')\n", "q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')\n", "u1, u2, u3 = me.dynamicsymbols('u1, u2, u3')" ] }, { "cell_type": "code", "execution_count": null, "id": "81a94348", "metadata": {}, "outputs": [], "source": [ "N = me.ReferenceFrame('N')\n", "A = me.ReferenceFrame('A')\n", "B = me.ReferenceFrame('B')\n", "\n", "A.orient_axis(N, q1, N.z)\n", "B.orient_axis(A, q2, A.x)\n", "\n", "A.set_ang_vel(N, u1*N.z)\n", "B.set_ang_vel(A, u2*A.x)" ] }, { "cell_type": "code", "execution_count": null, "id": "9ce6318f", "metadata": {}, "outputs": [], "source": [ "O = me.Point('O')\n", "Ao = me.Point('A_O')\n", "Bo = me.Point('B_O')\n", "Q = me.Point('Q')\n", "\n", "Ao.set_pos(O, l/2*A.x)\n", "Bo.set_pos(O, l*A.x)\n", "Q.set_pos(Bo, q3*B.y)\n", "\n", "O.set_vel(N, 0)\n", "Ao.v2pt_theory(O, N, A)\n", "Bo.v2pt_theory(O, N, A)\n", "Q.set_vel(B, u3*B.y)\n", "Q.v1pt_theory(Bo, N, B)" ] }, { "cell_type": "code", "execution_count": null, "id": "a625df8a", "metadata": {}, "outputs": [], "source": [ "qd_repl = {q1.diff(): u1, q2.diff(): u2, q3.diff(): u3}\n", "qd_repl" ] }, { "cell_type": "code", "execution_count": null, "id": "a360ddc4", "metadata": {}, "outputs": [], "source": [ "Q.set_acc(N, Q.acc(N).xreplace(qd_repl))\n", "Q.acc(N)" ] }, { "cell_type": "code", "execution_count": null, "id": "4065952d", "metadata": {}, "outputs": [], "source": [ "v_Ao_1 = Ao.vel(N).diff(u1, N)\n", "v_Ao_2 = Ao.vel(N).diff(u2, N)\n", "v_Ao_3 = Ao.vel(N).diff(u3, N)\n", "\n", "v_Ao_1, v_Ao_2, v_Ao_3" ] }, { "cell_type": "code", "execution_count": null, "id": "8bd31eee", "metadata": {}, "outputs": [], "source": [ "v_Bo_1 = Bo.vel(N).diff(u1, N)\n", "v_Bo_2 = Bo.vel(N).diff(u2, N)\n", "v_Bo_3 = Bo.vel(N).diff(u3, N)\n", "\n", "v_Bo_1, v_Bo_2, v_Bo_3" ] }, { "cell_type": "code", "execution_count": null, "id": "a4d9a507", "metadata": {}, "outputs": [], "source": [ "v_Q_1 = Q.vel(N).diff(u1, N)\n", "v_Q_2 = Q.vel(N).diff(u2, N)\n", "v_Q_3 = Q.vel(N).diff(u3, N)\n", "\n", "v_Q_1, v_Q_2, v_Q_3" ] }, { "cell_type": "code", "execution_count": null, "id": "074b1274", "metadata": {}, "outputs": [], "source": [ "w_A_1 = A.ang_vel_in(N).diff(u1, N)\n", "w_A_2 = A.ang_vel_in(N).diff(u2, N)\n", "w_A_3 = A.ang_vel_in(N).diff(u3, N)\n", "\n", "w_A_1, w_A_2, w_A_3" ] }, { "cell_type": "code", "execution_count": null, "id": "c18f2d80", "metadata": {}, "outputs": [], "source": [ "w_B_1 = B.ang_vel_in(N).diff(u1, N)\n", "w_B_2 = B.ang_vel_in(N).diff(u2, N)\n", "w_B_3 = B.ang_vel_in(N).diff(u3, N)\n", "\n", "w_B_1, w_B_2, w_B_3" ] }, { "cell_type": "code", "execution_count": null, "id": "fa515390", "metadata": {}, "outputs": [], "source": [ "I = m*l**2/12\n", "I_A_Ao = I*me.outer(A.y, A.y) + I*me.outer(A.z, A.z)\n", "I_B_Bo = I*me.outer(B.x, B.x) + I*me.outer(B.z, B.z)\n", "\n", "I_A_Ao, I_B_Bo" ] }, { "cell_type": "code", "execution_count": null, "id": "2e38939d", "metadata": {}, "outputs": [], "source": [ "F = me.dynamicsymbols('F')\n", "\n", "R_Ao = m*g*N.x\n", "R_Bo = m*g*N.x - F*B.y\n", "R_Q = m/4*g*N.x + F*B.y\n", "\n", "R_Ao, R_Bo, R_Q" ] }, { "cell_type": "code", "execution_count": null, "id": "e64f2d0e", "metadata": {}, "outputs": [], "source": [ "T_A = -kt*q1*N.z + kt*q2*B.x\n", "T_B = -kt*q2*B.x\n", "\n", "T_A, T_B" ] }, { "cell_type": "code", "execution_count": null, "id": "cebcbd08", "metadata": {}, "outputs": [], "source": [ "Rs_Ao = -m*Ao.acc(N)\n", "Rs_Bo = -m*Bo.acc(N)\n", "Rs_Q = -m/4*Q.acc(N)" ] }, { "cell_type": "code", "execution_count": null, "id": "6cc96540", "metadata": {}, "outputs": [], "source": [ "Ts_A = -(me.dot(A.ang_acc_in(N), I_A_Ao) + \n", " me.dot(me.cross(A.ang_vel_in(N), I_A_Ao), A.ang_vel_in(N)))\n", "Ts_A" ] }, { "cell_type": "code", "execution_count": null, "id": "d6014023", "metadata": {}, "outputs": [], "source": [ "Ts_B = -(me.dot(B.ang_acc_in(N), I_B_Bo) + \n", " me.dot(me.cross(B.ang_vel_in(N), I_B_Bo), B.ang_vel_in(N)))\n", "Ts_B" ] }, { "cell_type": "code", "execution_count": null, "id": "0dfb5828", "metadata": {}, "outputs": [], "source": [ "Fr = sm.Matrix([\n", " v_Ao_1.dot(R_Ao) + v_Bo_1.dot(R_Bo) + v_Q_1.dot(R_Q) +\n", " w_A_1.dot(T_A) + w_B_1.dot(T_B),\n", " v_Ao_2.dot(R_Ao) + v_Bo_2.dot(R_Bo) + v_Q_2.dot(R_Q) +\n", " w_A_2.dot(T_A) + w_B_2.dot(T_B),\n", " v_Ao_3.dot(R_Ao) + v_Bo_3.dot(R_Bo) + v_Q_3.dot(R_Q) +\n", " w_A_3.dot(T_A) + w_B_3.dot(T_B),\n", "])" ] }, { "cell_type": "code", "execution_count": null, "id": "495260dc", "metadata": {}, "outputs": [], "source": [ "Frs = sm.Matrix([\n", " v_Ao_1.dot(Rs_Ao) + v_Bo_1.dot(Rs_Bo) + v_Q_1.dot(Rs_Q) +\n", " w_A_1.dot(Ts_A) + w_B_1.dot(Ts_B),\n", " v_Ao_2.dot(Rs_Ao) + v_Bo_2.dot(Rs_Bo) + v_Q_2.dot(Rs_Q) +\n", " w_A_2.dot(Ts_A) + w_B_2.dot(Ts_B),\n", " v_Ao_3.dot(Rs_Ao) + v_Bo_3.dot(Rs_Bo) + v_Q_3.dot(Rs_Q) +\n", " w_A_3.dot(Ts_A) + w_B_3.dot(Ts_B),\n", "])" ] }, { "cell_type": "code", "execution_count": null, "id": "41ec1afe", "metadata": {}, "outputs": [], "source": [ "kanes_eqs = Fr + Frs\n", "kanes_eqs" ] }, { "cell_type": "code", "execution_count": null, "id": "06d3e714", "metadata": {}, "outputs": [], "source": [ "u = sm.Matrix([u1, u2, u3])\n", "t = me.dynamicsymbols._t\n", "ud = u.diff(t)" ] }, { "cell_type": "code", "execution_count": null, "id": "ae102805", "metadata": {}, "outputs": [], "source": [ "Md = Frs.jacobian(ud)\n", "Md" ] }, { "cell_type": "code", "execution_count": null, "id": "c5b94664", "metadata": {}, "outputs": [], "source": [ "ud_zero_repl = {u1.diff(): 0, u2.diff(): 0, u3.diff(): 0}" ] }, { "cell_type": "code", "execution_count": null, "id": "5e21c23e", "metadata": {}, "outputs": [], "source": [ "gd = Frs.xreplace(ud_zero_repl) + Fr\n", "gd" ] }, { "cell_type": "code", "execution_count": null, "id": "dff01c0c", "metadata": {}, "outputs": [], "source": [ "me.find_dynamicsymbols(Md)" ] }, { "cell_type": "code", "execution_count": null, "id": "207ab1ec", "metadata": {}, "outputs": [], "source": [ "me.find_dynamicsymbols(gd)" ] }, { "cell_type": "code", "execution_count": null, "id": "6528bf03", "metadata": {}, "outputs": [], "source": [ "Md.free_symbols | gd.free_symbols" ] }, { "cell_type": "code", "execution_count": null, "id": "60f98344", "metadata": {}, "outputs": [], "source": [ "q = sm.Matrix([q1, q2, q3])\n", "q" ] }, { "cell_type": "code", "execution_count": null, "id": "b8a04c01", "metadata": {}, "outputs": [], "source": [ "u" ] }, { "cell_type": "code", "execution_count": null, "id": "f7adf5f3", "metadata": {}, "outputs": [], "source": [ "p = sm.Matrix([g, kt, l, m])\n", "p" ] }, { "cell_type": "code", "execution_count": null, "id": "58fbd334", "metadata": {}, "outputs": [], "source": [ "# specified inputs\n", "r = sm.Matrix([F])\n", "r" ] }, { "cell_type": "code", "execution_count": null, "id": "d8700491", "metadata": {}, "outputs": [], "source": [ "eval_dyn = sm.lambdify((q, u, p, r), (Md, gd))" ] }, { "cell_type": "code", "execution_count": null, "id": "095dd8b9", "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "\n", "q_vals = np.array([\n", " np.deg2rad(25.0), # q1, rad\n", " np.deg2rad(5.0), # q2, rad\n", " 0.1, # q3, m\n", "])\n", "\n", "u_vals = np.array([\n", " 0.1, # u1, rad/s\n", " 2.2, # u2, rad/s\n", " 0.3, # u3, m/s\n", "])\n", "\n", "p_vals = np.array([\n", " 9.81, # g, m/s**2\n", " 0.01, # kt, Nm/rad\n", " 0.6, # l, m\n", " 1.0, # m, kg\n", "])\n", "\n", "r_vals = np.array([100.0]) # Newtons" ] }, { "cell_type": "code", "execution_count": null, "id": "97e842e7", "metadata": {}, "outputs": [], "source": [ "Md_vals, gd_vals = eval_dyn(q_vals, u_vals, p_vals, r_vals)" ] }, { "cell_type": "code", "execution_count": null, "id": "06bfa82d", "metadata": {}, "outputs": [], "source": [ "Md_vals" ] }, { "cell_type": "code", "execution_count": null, "id": "cbf98950", "metadata": {}, "outputs": [], "source": [ "gd_vals" ] }, { "cell_type": "code", "execution_count": null, "id": "997d348d", "metadata": {}, "outputs": [], "source": [ "np.linalg.solve?" ] }, { "cell_type": "code", "execution_count": null, "id": "8594796b", "metadata": {}, "outputs": [], "source": [ "ud_vals = np.linalg.solve(-Md_vals, np.squeeze(gd_vals))\n", "ud_vals" ] }, { "cell_type": "code", "execution_count": null, "id": "391282ac", "metadata": {}, "outputs": [], "source": [ "qd_vals = u_vals\n", "qd_vals" ] }, { "cell_type": "code", "execution_count": null, "id": "1d1c7925", "metadata": {}, "outputs": [], "source": [ "xd_vals = np.hstack((qd_vals, ud_vals))\n", "xd_vals" ] }, { "cell_type": "code", "execution_count": null, "id": "84500e0e", "metadata": {}, "outputs": [], "source": [ "def eval_rhs(t, x, p, r):\n", " q = x[:3]\n", " u = x[3:]\n", " Md, gd = eval_dyn(q, u, p, r)\n", " ud = np.linalg.solve(-Md, np.squeeze(gd))\n", " qd = u\n", " return np.hstack((qd, ud))" ] }, { "cell_type": "code", "execution_count": null, "id": "fda4f3c0", "metadata": {}, "outputs": [], "source": [ "eval_rhs(2.1, np.hstack((q_vals, u_vals)), p_vals, r_vals)" ] }, { "cell_type": "code", "execution_count": null, "id": "2c8590f7", "metadata": {}, "outputs": [], "source": [ "def eval_rhs(t, x, p):\n", " \n", " q = x[:3]\n", " u = x[3:]\n", " #r = np.array([1.0*np.sin(2*np.pi*t)])\n", " r = np.array([-2.0*q[2]])\n", " Md, gd = eval_dyn(q, u, p, r)\n", " ud = np.linalg.solve(-Md, np.squeeze(gd))\n", " qd = u\n", " return np.hstack((qd, ud))" ] }, { "cell_type": "code", "execution_count": null, "id": "3e90a1af", "metadata": {}, "outputs": [], "source": [ "eval_rhs(2.1, np.hstack((q_vals, u_vals)), p_vals)" ] }, { "cell_type": "code", "execution_count": null, "id": "d0dc296d", "metadata": {}, "outputs": [], "source": [ "from scipy.integrate import solve_ivp" ] }, { "cell_type": "code", "execution_count": null, "id": "b74edd07", "metadata": {}, "outputs": [], "source": [ "x0 = np.hstack((q_vals, u_vals))\n", "t0 = 0.0\n", "tf = 4.0\n", "\n", "sol = solve_ivp(eval_rhs, (t0, tf), x0, args=(p_vals, ))" ] }, { "cell_type": "code", "execution_count": null, "id": "7207f46f", "metadata": {}, "outputs": [], "source": [ "sol.t" ] }, { "cell_type": "code", "execution_count": null, "id": "317da4ea", "metadata": {}, "outputs": [], "source": [ "sol.y" ] }, { "cell_type": "code", "execution_count": null, "id": "17e766d4", "metadata": {}, "outputs": [], "source": [ "import matplotlib.pyplot as plt" ] }, { "cell_type": "code", "execution_count": null, "id": "c1edec0f", "metadata": {}, "outputs": [], "source": [ "sol.t.shape" ] }, { "cell_type": "code", "execution_count": null, "id": "ac21a099", "metadata": {}, "outputs": [], "source": [ "sol.y.shape" ] }, { "cell_type": "code", "execution_count": null, "id": "6b34f75d", "metadata": {}, "outputs": [], "source": [ "%matplotlib notebook" ] }, { "cell_type": "code", "execution_count": null, "id": "bbf6770d", "metadata": {}, "outputs": [], "source": [ "plt.plot(sol.t, np.transpose(sol.y))\n", "\n", "plt.xlabel('Time [s]')\n", "\n", "plt.legend(['q1', 'q2', 'q3', 'u1', 'u2', 'u3'])" ] }, { "cell_type": "code", "execution_count": null, "id": "bbbf07a1", "metadata": {}, "outputs": [], "source": [ "def plot_results(ts, xs):\n", " \"\"\"Returns the array of axes of a 4 panel plot of the state trajectory\n", " versus time.\n", "\n", " Parameters\n", " ==========\n", " ts : array_like, shape(m,)\n", " Values of time.\n", " xs : array_like, shape(m, 6)\n", " Values of the state trajectories corresponding to ``ts`` in order\n", " [q1, q2, q3, u1, u2, u3].\n", "\n", " Returns\n", " =======\n", " axes : ndarray, shape(4,)\n", " Matplotlib axes for each panel.\n", "\n", " \"\"\"\n", "\n", " fig, axes = plt.subplots(4, 1, sharex=True)\n", "\n", " fig.set_size_inches((10.0, 6.0))\n", "\n", " axes[0].plot(ts, np.rad2deg(xs[:, :2]), '.-')\n", " axes[1].plot(ts, xs[:, 2], '.-')\n", " axes[2].plot(ts, np.rad2deg(xs[:, 3:5]), '.-')\n", " axes[3].plot(ts, xs[:, 5], '.-')\n", "\n", " axes[0].legend([me.vlatex(q[0], mode='inline'),\n", " me.vlatex(q[1], mode='inline')])\n", " axes[1].legend([me.vlatex(q[2], mode='inline')])\n", " axes[2].legend([me.vlatex(u[0], mode='inline'),\n", " me.vlatex(u[1], mode='inline')])\n", " axes[3].legend([me.vlatex(u[2], mode='inline')])\n", "\n", " axes[0].set_ylabel('Angle [deg]')\n", " axes[1].set_ylabel('Distance [m]')\n", " axes[2].set_ylabel('Angular Rate [deg/s]')\n", " axes[3].set_ylabel('Speed [m/s]')\n", "\n", " axes[3].set_xlabel('Time [s]')\n", "\n", " fig.tight_layout()\n", "\n", " return axes" ] }, { "cell_type": "code", "execution_count": null, "id": "6e8de99e", "metadata": {}, "outputs": [], "source": [ "plot_results(sol.t, np.transpose(sol.y))" ] }, { "cell_type": "code", "execution_count": null, "id": "6d423433", "metadata": {}, "outputs": [], "source": [ "x0 = np.hstack((q_vals, u_vals))\n", "t0 = 0.0\n", "tf = 4.0\n", "\n", "ts = np.linspace(t0, tf, num=100)\n", "\n", "sol = solve_ivp(eval_rhs, (t0, tf), x0, args=(p_vals, ), t_eval=ts)" ] }, { "cell_type": "code", "execution_count": null, "id": "705d40c3", "metadata": {}, "outputs": [], "source": [ "plot_results(sol.t, np.transpose(sol.y))" ] }, { "cell_type": "code", "execution_count": null, "id": "bf67fa3b", "metadata": {}, "outputs": [], "source": [ "M = me.ReferenceFrame('M')\n", "M.orient_axis(N, sm.pi/2, N.z)" ] }, { "cell_type": "code", "execution_count": null, "id": "c379501e", "metadata": {}, "outputs": [], "source": [ "Bl = me.Point('B_l')\n", "Br = me.Point('B_r')\n", "Bl.set_pos(Bo, -l/2*B.y)\n", "Br.set_pos(Bo, l/2*B.y)" ] }, { "cell_type": "code", "execution_count": null, "id": "651258ba", "metadata": {}, "outputs": [], "source": [ "coordinates = O.pos_from(O).to_matrix(M)\n", "for point in [Bo, Q, Bl, Br]:\n", " coordinates = coordinates.row_join(point.pos_from(O).to_matrix(M))\n", "\n", "eval_point_coords = sm.lambdify((q, p), coordinates)\n", "eval_point_coords(q_vals, p_vals)" ] }, { "cell_type": "code", "execution_count": null, "id": "67302d69", "metadata": {}, "outputs": [], "source": [ "# initial configuration of the points\n", "x, y, z = eval_point_coords(q_vals, p_vals)\n", "\n", "# create a figure\n", "fig = plt.figure()\n", "fig.set_size_inches((10.0, 10.0))\n", "\n", "# setup the subplots\n", "ax_top = fig.add_subplot(2, 2, 1)\n", "ax_3d = fig.add_subplot(2, 2, 2, projection='3d')\n", "ax_front = fig.add_subplot(2, 2, 3)\n", "ax_right = fig.add_subplot(2, 2, 4)\n", "\n", "# common line and marker properties for each panel\n", "line_prop = {\n", " 'color': 'black',\n", " 'marker': 'o',\n", " 'markerfacecolor': 'blue',\n", " 'markersize': 10,\n", "}\n", "\n", "# top view\n", "lines_top, = ax_top.plot(x, z, **line_prop)\n", "ax_top.set_xlim((-0.5, 0.5))\n", "ax_top.set_ylim((0.5, -0.5))\n", "ax_top.set_title('Top View')\n", "ax_top.set_xlabel('x')\n", "ax_top.set_ylabel('z')\n", "ax_top.set_aspect('equal')\n", "\n", "# 3d view\n", "lines_3d, = ax_3d.plot(x, z, y, **line_prop)\n", "ax_3d.set_xlim((-0.5, 0.5))\n", "ax_3d.set_ylim((0.5, -0.5))\n", "ax_3d.set_zlim((-0.8, 0.2))\n", "ax_3d.set_xlabel('x')\n", "ax_3d.set_ylabel('z')\n", "ax_3d.set_zlabel('y')\n", "\n", "# front view\n", "lines_front, = ax_front.plot(x, y, **line_prop)\n", "ax_front.set_xlim((-0.5, 0.5))\n", "ax_front.set_ylim((-0.8, 0.2))\n", "ax_front.set_title('Front View')\n", "ax_front.set_xlabel('x')\n", "ax_front.set_ylabel('y')\n", "ax_front.set_aspect('equal')\n", "\n", "# right view\n", "lines_right, = ax_right.plot(z, y, **line_prop)\n", "ax_right.set_xlim((0.5, -0.5))\n", "ax_right.set_ylim((-0.8, 0.2))\n", "ax_right.set_title('Right View')\n", "ax_right.set_xlabel('z')\n", "ax_right.set_ylabel('y')\n", "ax_right.set_aspect('equal')\n", "\n", "fig.tight_layout()" ] }, { "cell_type": "code", "execution_count": null, "id": "a633812f", "metadata": {}, "outputs": [], "source": [ "from matplotlib.animation import FuncAnimation" ] }, { "cell_type": "code", "execution_count": null, "id": "13bebbee", "metadata": {}, "outputs": [], "source": [ "xs = np.transpose(sol.y)\n", "def animate(i):\n", " x, y, z = eval_point_coords(xs[i, :3], p_vals)\n", " lines_top.set_data(x, z)\n", " lines_3d.set_data_3d(x, z, y)\n", " lines_front.set_data(x, y)\n", " lines_right.set_data(z, y)" ] }, { "cell_type": "code", "execution_count": null, "id": "a8b054ce", "metadata": {}, "outputs": [], "source": [ "animate(20)" ] }, { "cell_type": "code", "execution_count": null, "id": "a57bc766", "metadata": {}, "outputs": [], "source": [ "ani = FuncAnimation(fig, animate, len(sol.t))" ] }, { "cell_type": "code", "execution_count": null, "id": "471a1072", "metadata": {}, "outputs": [], "source": [ "from IPython.display import HTML\n", "\n", "HTML(ani.to_html5_video())" ] }, { "cell_type": "code", "execution_count": null, "id": "b8b9b9b2", "metadata": {}, "outputs": [], "source": [ "HTML(ani.to_jshtml(fps=30))" ] }, { "cell_type": "code", "execution_count": null, "id": "7624e318", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "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.8.12" } }, "nbformat": 4, "nbformat_minor": 5 }