{ "cells": [ { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "import sympy as sm\n", "import sympy.physics.mechanics as me\n", "me.init_vprinting(use_latex='mathjax')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Body Fixed Newton-Euler Equations" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [], "source": [ "m, Ixx, Iyy, Izz = sm.symbols('m, I_{xx}, I_{yy}, I_{zz}')\n", "Ixy, Iyz, Ixz = sm.symbols('I_{xy}, I_{yz}, I_{xz}')\n", "Fx, Fy, Fz, Mx, My, Mz = me.dynamicsymbols('F_x, F_y, F_z, M_x, M_y, M_z')\n", "u1, u2, u3, u4, u5, u6 = me.dynamicsymbols('u1, u2, u3, u4, u5, u6')\n", "\n", "A = me.ReferenceFrame('A')\n", "B = me.ReferenceFrame('B')\n", "\n", "Bo = me.Point('Bo')" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "A_w_B = u4*B.x + u5*B.y + u6*B.z\n", "B.set_ang_vel(A, A_w_B)\n", "\n", "A_v_Bo = u1*B.x + u2*B.y + u3*B.z\n", "Bo.set_vel(A, A_v_Bo)" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( \\mathbf{\\hat{b}_x}, \\ \\mathbf{\\hat{b}_y}, \\ \\mathbf{\\hat{b}_z}, \\ 0, \\ 0, \\ 0\\right)$" ], "text/plain": [ "(b_x, b_y, b_z, 0, 0, 0)" ] }, "execution_count": 5, "metadata": {}, "output_type": "execute_result" } ], "source": [ "v_Bo_1 = A_v_Bo.diff(u1, A, var_in_dcm=False)\n", "v_Bo_2 = A_v_Bo.diff(u2, A, var_in_dcm=False)\n", "v_Bo_3 = A_v_Bo.diff(u3, A, var_in_dcm=False)\n", "v_Bo_4 = A_v_Bo.diff(u4, A, var_in_dcm=False)\n", "v_Bo_5 = A_v_Bo.diff(u5, A, var_in_dcm=False)\n", "v_Bo_6 = A_v_Bo.diff(u6, A, var_in_dcm=False)\n", "\n", "v_Bo_1, v_Bo_2, v_Bo_3, v_Bo_4, v_Bo_5, v_Bo_6" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( 0, \\ 0, \\ 0, \\ \\mathbf{\\hat{b}_x}, \\ \\mathbf{\\hat{b}_y}, \\ \\mathbf{\\hat{b}_z}\\right)$" ], "text/plain": [ "(0, 0, 0, b_x, b_y, b_z)" ] }, "execution_count": 6, "metadata": {}, "output_type": "execute_result" } ], "source": [ "w_B_1 = A_w_B.diff(u1, A, var_in_dcm=False)\n", "w_B_2 = A_w_B.diff(u2, A, var_in_dcm=False)\n", "w_B_3 = A_w_B.diff(u3, A, var_in_dcm=False)\n", "w_B_4 = A_w_B.diff(u4, A, var_in_dcm=False)\n", "w_B_5 = A_w_B.diff(u5, A, var_in_dcm=False)\n", "w_B_6 = A_w_B.diff(u6, A, var_in_dcm=False)\n", "\n", "w_B_1, w_B_2, w_B_3, w_B_4, w_B_5, w_B_6" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[ \\left[ \\mathbf{\\hat{b}_x}, \\ \\mathbf{\\hat{b}_y}, \\ \\mathbf{\\hat{b}_z}, \\ 0, \\ 0, \\ 0\\right], \\ \\left[ 0, \\ 0, \\ 0, \\ \\mathbf{\\hat{b}_x}, \\ \\mathbf{\\hat{b}_y}, \\ \\mathbf{\\hat{b}_z}\\right]\\right]$" ], "text/plain": [ "[[b_x, b_y, b_z, 0, 0, 0], [0, 0, 0, b_x, b_y, b_z]]" ] }, "execution_count": 7, "metadata": {}, "output_type": "execute_result" } ], "source": [ "par_vels = me.partial_velocity([A_v_Bo, A_w_B], [u1, u2, u3, u4, u5, u6], A)\n", "\n", "par_vels" ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}F_{x}\\\\F_{y}\\\\F_{z}\\\\M_{x}\\\\M_{x}\\\\M_{z}\\end{matrix}\\right]$" ], "text/plain": [ "⎡Fₓ ⎤\n", "⎢ ⎥\n", "⎢F_y⎥\n", "⎢ ⎥\n", "⎢F_z⎥\n", "⎢ ⎥\n", "⎢Mₓ ⎥\n", "⎢ ⎥\n", "⎢Mₓ ⎥\n", "⎢ ⎥\n", "⎣M_z⎦" ] }, "execution_count": 8, "metadata": {}, "output_type": "execute_result" } ], "source": [ "T = Mx*B.x + My*B.y + Mz*B.z\n", "R = Fx*B.x + Fy*B.y + Fz*B.z\n", "\n", "F1 = v_Bo_1.dot(R) + w_B_1.dot(T)\n", "F2 = v_Bo_2.dot(R) + w_B_2.dot(T)\n", "F3 = v_Bo_3.dot(R) + w_B_3.dot(T)\n", "F4 = v_Bo_4.dot(R) + w_B_4.dot(T)\n", "F5 = v_Bo_5.dot(R) + w_B_5.dot(T)\n", "F6 = v_Bo_6.dot(R) + w_B_6.dot(T)\n", "\n", "Fr = sm.Matrix([F1, F2, F3, F4, F4, F6])\n", "Fr" ] }, { "cell_type": "code", "execution_count": 12, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle I_{xx}\\mathbf{\\hat{b}_x}\\otimes \\mathbf{\\hat{b}_x} + I_{xy}\\mathbf{\\hat{b}_x}\\otimes \\mathbf{\\hat{b}_y} + I_{xz}\\mathbf{\\hat{b}_x}\\otimes \\mathbf{\\hat{b}_z} + I_{xy}\\mathbf{\\hat{b}_y}\\otimes \\mathbf{\\hat{b}_x} + I_{yy}\\mathbf{\\hat{b}_y}\\otimes \\mathbf{\\hat{b}_y} + I_{yz}\\mathbf{\\hat{b}_y}\\otimes \\mathbf{\\hat{b}_z} + I_{xz}\\mathbf{\\hat{b}_z}\\otimes \\mathbf{\\hat{b}_x} + I_{yz}\\mathbf{\\hat{b}_z}\\otimes \\mathbf{\\hat{b}_y} + I_{zz}\\mathbf{\\hat{b}_z}\\otimes \\mathbf{\\hat{b}_z}$" ], "text/plain": [ "I_{xx} b_x⊗b_x + I_{xy} b_x⊗b_y + I_{xz} b_x⊗b_z + I_{xy} b_y⊗b_x + I_{yy} b_y⊗b_y + I_{yz} b_y⊗b_z + I_{xz} b_z⊗b_x + I_{yz} b_z⊗b_y + I_{zz} b_z⊗b_z" ] }, "execution_count": 12, "metadata": {}, "output_type": "execute_result" } ], "source": [ "I = me.inertia(B, Ixx, Iyy, Izz, Ixy, Iyz, Ixz)\n", "I" ] }, { "cell_type": "code", "execution_count": 13, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- m \\left(- u_{2} u_{6} + u_{3} u_{5} + \\dot{u}_{1}\\right)\\\\- m \\left(u_{1} u_{6} - u_{3} u_{4} + \\dot{u}_{2}\\right)\\\\- m \\left(- u_{1} u_{5} + u_{2} u_{4} + \\dot{u}_{3}\\right)\\\\- I_{xx} \\dot{u}_{4} - I_{xy} \\dot{u}_{5} - I_{xz} \\dot{u}_{6} - \\left(- I_{xy} u_{6} + I_{xz} u_{5}\\right) u_{4} - \\left(- I_{yy} u_{6} + I_{yz} u_{5}\\right) u_{5} - \\left(- I_{yz} u_{6} + I_{zz} u_{5}\\right) u_{6}\\\\- I_{xy} \\dot{u}_{4} - I_{yy} \\dot{u}_{5} - I_{yz} \\dot{u}_{6} - \\left(I_{xx} u_{6} - I_{xz} u_{4}\\right) u_{4} - \\left(I_{xy} u_{6} - I_{yz} u_{4}\\right) u_{5} - \\left(I_{xz} u_{6} - I_{zz} u_{4}\\right) u_{6}\\\\- I_{xz} \\dot{u}_{4} - I_{yz} \\dot{u}_{5} - I_{zz} \\dot{u}_{6} - \\left(- I_{xx} u_{5} + I_{xy} u_{4}\\right) u_{4} - \\left(- I_{xy} u_{5} + I_{yy} u_{4}\\right) u_{5} - \\left(- I_{xz} u_{5} + I_{yz} u_{4}\\right) u_{6}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ -m⋅(-u₂⋅u₆ + u₃⋅u₅ + u₁̇) ↪\n", "⎢ ↪\n", "⎢ -m⋅(u₁⋅u₆ - u₃⋅u₄ + u₂̇) ↪\n", "⎢ ↪\n", "⎢ -m⋅(-u₁⋅u₅ + u₂⋅u₄ + u₃̇) ↪\n", "⎢ ↪\n", "⎢-I_{xx}⋅u₄̇ - I_{xy}⋅u₅̇ - I_{xz}⋅u₆̇ - (-I_{xy}⋅u₆ + I_{xz}⋅u₅)⋅u₄ - (-I_{yy}⋅u ↪\n", "⎢ ↪\n", "⎢ -I_{xy}⋅u₄̇ - I_{yy}⋅u₅̇ - I_{yz}⋅u₆̇ - (I_{xx}⋅u₆ - I_{xz}⋅u₄)⋅u₄ - (I_{xy}⋅u₆ ↪\n", "⎢ ↪\n", "⎣-I_{xz}⋅u₄̇ - I_{yz}⋅u₅̇ - I_{zz}⋅u₆̇ - (-I_{xx}⋅u₅ + I_{xy}⋅u₄)⋅u₄ - (-I_{xy}⋅u ↪\n", "\n", "↪ ⎤\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ₆ + I_{yz}⋅u₅)⋅u₅ - (-I_{yz}⋅u₆ + I_{zz}⋅u₅)⋅u₆⎥\n", "↪ ⎥\n", "↪ - I_{yz}⋅u₄)⋅u₅ - (I_{xz}⋅u₆ - I_{zz}⋅u₄)⋅u₆ ⎥\n", "↪ ⎥\n", "↪ ₅ + I_{yy}⋅u₄)⋅u₅ - (-I_{xz}⋅u₅ + I_{yz}⋅u₄)⋅u₆⎦" ] }, "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Rs = -m*Bo.acc(A)\n", "Ts = -(B.ang_acc_in(A).dot(I) + me.cross(A_w_B, I).dot(A_w_B))\n", "\n", "F1s = v_Bo_1.dot(Rs) + w_B_1.dot(Ts)\n", "F2s = v_Bo_2.dot(Rs) + w_B_2.dot(Ts)\n", "F3s = v_Bo_3.dot(Rs) + w_B_3.dot(Ts)\n", "F4s = v_Bo_4.dot(Rs) + w_B_4.dot(Ts)\n", "F5s = v_Bo_5.dot(Rs) + w_B_5.dot(Ts)\n", "F6s = v_Bo_6.dot(Rs) + w_B_6.dot(Ts)\n", "\n", "Frs = sm.Matrix([F1s, F2s, F3s, F4s, F5s, F6s])\n", "Frs" ] }, { "cell_type": "code", "execution_count": 14, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- m \\left(- u_{2} u_{6} + u_{3} u_{5} + \\dot{u}_{1}\\right) + F_{x}\\\\- m \\left(u_{1} u_{6} - u_{3} u_{4} + \\dot{u}_{2}\\right) + F_{y}\\\\- m \\left(- u_{1} u_{5} + u_{2} u_{4} + \\dot{u}_{3}\\right) + F_{z}\\\\- I_{xx} \\dot{u}_{4} - I_{xy} \\dot{u}_{5} - I_{xz} \\dot{u}_{6} - \\left(- I_{xy} u_{6} + I_{xz} u_{5}\\right) u_{4} - \\left(- I_{yy} u_{6} + I_{yz} u_{5}\\right) u_{5} - \\left(- I_{yz} u_{6} + I_{zz} u_{5}\\right) u_{6} + M_{x}\\\\- I_{xy} \\dot{u}_{4} - I_{yy} \\dot{u}_{5} - I_{yz} \\dot{u}_{6} - \\left(I_{xx} u_{6} - I_{xz} u_{4}\\right) u_{4} - \\left(I_{xy} u_{6} - I_{yz} u_{4}\\right) u_{5} - \\left(I_{xz} u_{6} - I_{zz} u_{4}\\right) u_{6} + M_{x}\\\\- I_{xz} \\dot{u}_{4} - I_{yz} \\dot{u}_{5} - I_{zz} \\dot{u}_{6} - \\left(- I_{xx} u_{5} + I_{xy} u_{4}\\right) u_{4} - \\left(- I_{xy} u_{5} + I_{yy} u_{4}\\right) u_{5} - \\left(- I_{xz} u_{5} + I_{yz} u_{4}\\right) u_{6} + M_{z}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ -m⋅(-u₂⋅u₆ + u₃⋅u₅ + u₁̇) + ↪\n", "⎢ ↪\n", "⎢ -m⋅(u₁⋅u₆ - u₃⋅u₄ + u₂̇) + F ↪\n", "⎢ ↪\n", "⎢ -m⋅(-u₁⋅u₅ + u₂⋅u₄ + u₃̇) + ↪\n", "⎢ ↪\n", "⎢-I_{xx}⋅u₄̇ - I_{xy}⋅u₅̇ - I_{xz}⋅u₆̇ - (-I_{xy}⋅u₆ + I_{xz}⋅u₅)⋅u₄ - (-I_{yy}⋅u ↪\n", "⎢ ↪\n", "⎢ -I_{xy}⋅u₄̇ - I_{yy}⋅u₅̇ - I_{yz}⋅u₆̇ - (I_{xx}⋅u₆ - I_{xz}⋅u₄)⋅u₄ - (I_{xy}⋅u ↪\n", "⎢ ↪\n", "⎣-I_{xz}⋅u₄̇ - I_{yz}⋅u₅̇ - I_{zz}⋅u₆̇ - (-I_{xx}⋅u₅ + I_{xy}⋅u₄)⋅u₄ - (-I_{xy}⋅u ↪\n", "\n", "↪ Fₓ ⎤\n", "↪ ⎥\n", "↪ _y ⎥\n", "↪ ⎥\n", "↪ F_z ⎥\n", "↪ ⎥\n", "↪ ₆ + I_{yz}⋅u₅)⋅u₅ - (-I_{yz}⋅u₆ + I_{zz}⋅u₅)⋅u₆ + Mₓ ⎥\n", "↪ ⎥\n", "↪ ₆ - I_{yz}⋅u₄)⋅u₅ - (I_{xz}⋅u₆ - I_{zz}⋅u₄)⋅u₆ + Mₓ ⎥\n", "↪ ⎥\n", "↪ ₅ + I_{yy}⋅u₄)⋅u₅ - (-I_{xz}⋅u₅ + I_{yz}⋅u₄)⋅u₆ + M_z⎦" ] }, "execution_count": 14, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Fr + Frs" ] }, { "cell_type": "code", "execution_count": 15, "metadata": {}, "outputs": [], "source": [ "u = sm.Matrix([u1, u2, u3, u4, u5, u6])\n", "t = me.dynamicsymbols._t\n", "ud = u.diff(t)" ] }, { "cell_type": "code", "execution_count": 16, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}m & 0 & 0 & 0 & 0 & 0\\\\0 & m & 0 & 0 & 0 & 0\\\\0 & 0 & m & 0 & 0 & 0\\\\0 & 0 & 0 & I_{xx} & I_{xy} & I_{xz}\\\\0 & 0 & 0 & I_{xy} & I_{yy} & I_{yz}\\\\0 & 0 & 0 & I_{xz} & I_{yz} & I_{zz}\\end{matrix}\\right]$" ], "text/plain": [ "⎡m 0 0 0 0 0 ⎤\n", "⎢ ⎥\n", "⎢0 m 0 0 0 0 ⎥\n", "⎢ ⎥\n", "⎢0 0 m 0 0 0 ⎥\n", "⎢ ⎥\n", "⎢0 0 0 I_{xx} I_{xy} I_{xz}⎥\n", "⎢ ⎥\n", "⎢0 0 0 I_{xy} I_{yy} I_{yz}⎥\n", "⎢ ⎥\n", "⎣0 0 0 I_{xz} I_{yz} I_{zz}⎦" ] }, "execution_count": 16, "metadata": {}, "output_type": "execute_result" } ], "source": [ "M = -Frs.jacobian(ud)\n", "M" ] }, { "cell_type": "code", "execution_count": 17, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}m \\left(- u_{2} u_{6} + u_{3} u_{5}\\right)\\\\m \\left(u_{1} u_{6} - u_{3} u_{4}\\right)\\\\m \\left(- u_{1} u_{5} + u_{2} u_{4}\\right)\\\\\\left(- I_{xy} u_{6} + I_{xz} u_{5}\\right) u_{4} + \\left(- I_{yy} u_{6} + I_{yz} u_{5}\\right) u_{5} + \\left(- I_{yz} u_{6} + I_{zz} u_{5}\\right) u_{6}\\\\\\left(I_{xx} u_{6} - I_{xz} u_{4}\\right) u_{4} + \\left(I_{xy} u_{6} - I_{yz} u_{4}\\right) u_{5} + \\left(I_{xz} u_{6} - I_{zz} u_{4}\\right) u_{6}\\\\\\left(- I_{xx} u_{5} + I_{xy} u_{4}\\right) u_{4} + \\left(- I_{xy} u_{5} + I_{yy} u_{4}\\right) u_{5} + \\left(- I_{xz} u_{5} + I_{yz} u_{4}\\right) u_{6}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ m⋅(-u₂⋅u₆ + u₃⋅u₅) ↪\n", "⎢ ↪\n", "⎢ m⋅(u₁⋅u₆ - u₃⋅u₄) ↪\n", "⎢ ↪\n", "⎢ m⋅(-u₁⋅u₅ + u₂⋅u₄) ↪\n", "⎢ ↪\n", "⎢(-I_{xy}⋅u₆ + I_{xz}⋅u₅)⋅u₄ + (-I_{yy}⋅u₆ + I_{yz}⋅u₅)⋅u₅ + (-I_{yz}⋅u₆ + I_{ ↪\n", "⎢ ↪\n", "⎢ (I_{xx}⋅u₆ - I_{xz}⋅u₄)⋅u₄ + (I_{xy}⋅u₆ - I_{yz}⋅u₄)⋅u₅ + (I_{xz}⋅u₆ - I_{zz ↪\n", "⎢ ↪\n", "⎣(-I_{xx}⋅u₅ + I_{xy}⋅u₄)⋅u₄ + (-I_{xy}⋅u₅ + I_{yy}⋅u₄)⋅u₅ + (-I_{xz}⋅u₅ + I_{ ↪\n", "\n", "↪ ⎤\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ zz}⋅u₅)⋅u₆⎥\n", "↪ ⎥\n", "↪ }⋅u₄)⋅u₆ ⎥\n", "↪ ⎥\n", "↪ yz}⋅u₄)⋅u₆⎦" ] }, "execution_count": 17, "metadata": {}, "output_type": "execute_result" } ], "source": [ "C = -Frs.xreplace({udi: 0 for udi in ud})\n", "C" ] }, { "cell_type": "code", "execution_count": 18, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}F_{x}\\\\F_{y}\\\\F_{z}\\\\M_{x}\\\\M_{x}\\\\M_{z}\\end{matrix}\\right]$" ], "text/plain": [ "⎡Fₓ ⎤\n", "⎢ ⎥\n", "⎢F_y⎥\n", "⎢ ⎥\n", "⎢F_z⎥\n", "⎢ ⎥\n", "⎢Mₓ ⎥\n", "⎢ ⎥\n", "⎢Mₓ ⎥\n", "⎢ ⎥\n", "⎣M_z⎦" ] }, "execution_count": 18, "metadata": {}, "output_type": "execute_result" } ], "source": [ "F = Fr\n", "F" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# 两杆算例" ] }, { "cell_type": "code", "execution_count": 19, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( \\frac{l u_{1}}{2}\\mathbf{\\hat{a}_y}, \\ l u_{1}\\mathbf{\\hat{a}_y}, \\ u_{1}\\mathbf{\\hat{n}_z}, \\ u_{2}\\mathbf{\\hat{a}_x} + u_{1}\\mathbf{\\hat{n}_z}\\right)$" ], "text/plain": [ "⎛l⋅u₁ ⎞\n", "⎜──── a_y, l⋅u₁ a_y, u₁ n_z, u₂ a_x + u₁ n_z⎟\n", "⎝ 2 ⎠" ] }, "execution_count": 19, "metadata": {}, "output_type": "execute_result" } ], "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')\n", "\n", "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)\n", "\n", "O = me.Point('O')\n", "Ao = me.Point('A_O')\n", "Bo = me.Point('B_O')\n", "\n", "Ao.set_pos(O, l/2*A.x)\n", "Bo.set_pos(O, l*A.x)\n", "\n", "O.set_vel(N, 0)\n", "Ao.v2pt_theory(O, N, A)\n", "Bo.v2pt_theory(O, N, A)\n", "\n", "Ao.vel(N), Bo.vel(N), A.ang_vel_in(N), B.ang_vel_in(N)" ] }, { "cell_type": "code", "execution_count": 20, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle - q_{3} u_{1} \\cos{\\left(q_{2} \\right)}\\mathbf{\\hat{b}_x} + u_{3}\\mathbf{\\hat{b}_y} + q_{3} u_{2}\\mathbf{\\hat{b}_z} + l u_{1}\\mathbf{\\hat{a}_y}$" ], "text/plain": [ "-q₃⋅u₁⋅cos(q₂) b_x + u₃ b_y + q₃⋅u₂ b_z + l⋅u₁ a_y" ] }, "execution_count": 20, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Q = me.Point('Q')\n", "Q.set_pos(Bo, q3*B.y)\n", "Q.set_vel(B, u3*B.y)\n", "Q.v1pt_theory(Bo, N, B)\n", "\n", "Q.vel(N)" ] }, { "cell_type": "code", "execution_count": 21, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left( - \\frac{l u_{1}^{2}}{2}\\mathbf{\\hat{a}_x} + \\frac{l \\dot{u}_{1}}{2}\\mathbf{\\hat{a}_y}, \\ - l u_{1}^{2}\\mathbf{\\hat{a}_x} + l \\dot{u}_{1}\\mathbf{\\hat{a}_y}, \\ \\dot{u}_{1}\\mathbf{\\hat{n}_z}, \\ \\dot{u}_{2}\\mathbf{\\hat{a}_x} + u_{1} u_{2}\\mathbf{\\hat{a}_y} + \\dot{u}_{1}\\mathbf{\\hat{n}_z}\\right)$" ], "text/plain": [ "⎛ 2 ↪\n", "⎜-l⋅u₁ l⋅u₁̇ 2 ↪\n", "⎜─────── a_x + ──── a_y, -l⋅u₁ a_x + l⋅u₁̇ a_y, u₁̇ n_z, u₂̇ a_x + u₁⋅u₂ a_y + u ↪\n", "⎝ 2 2 ↪\n", "\n", "↪ ⎞\n", "↪ ⎟\n", "↪ ₁̇ n_z⎟\n", "↪ ⎠" ] }, "execution_count": 21, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Ao.acc(N), Bo.acc(N), A.ang_acc_in(N), B.ang_acc_in(N)" ] }, { "cell_type": "code", "execution_count": 22, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle (q_{3} u_{1} u_{2} \\sin{\\left(q_{2} \\right)} + q_{3} u_{1} \\sin{\\left(q_{2} \\right)} \\dot{q}_{2} - q_{3} \\cos{\\left(q_{2} \\right)} \\dot{u}_{1} - u_{1} u_{3} \\cos{\\left(q_{2} \\right)} - u_{1} \\cos{\\left(q_{2} \\right)} \\dot{q}_{3})\\mathbf{\\hat{b}_x} + (- q_{3} u_{1}^{2} \\cos^{2}{\\left(q_{2} \\right)} - q_{3} u_{2}^{2} + \\dot{u}_{3})\\mathbf{\\hat{b}_y} + (q_{3} u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)} + q_{3} \\dot{u}_{2} + u_{2} u_{3} + u_{2} \\dot{q}_{3})\\mathbf{\\hat{b}_z} - l u_{1}^{2}\\mathbf{\\hat{a}_x} + l \\dot{u}_{1}\\mathbf{\\hat{a}_y}$" ], "text/plain": [ " ↪\n", "(q₃⋅u₁⋅u₂⋅sin(q₂) + q₃⋅u₁⋅sin(q₂)⋅q₂̇ - q₃⋅cos(q₂)⋅u₁̇ - u₁⋅u₃⋅cos(q₂) - u₁⋅cos( ↪\n", "\n", "↪ ⎛ 2 2 2 ⎞ ⎛ 2 ↪\n", "↪ q₂)⋅q₃̇) b_x + ⎝- q₃⋅u₁ ⋅cos (q₂) - q₃⋅u₂ + u₃̇⎠ b_y + ⎝q₃⋅u₁ ⋅sin(q₂)⋅cos(q₂ ↪\n", "\n", "↪ ⎞ 2 \n", "↪ ) + q₃⋅u₂̇ + u₂⋅u₃ + u₂⋅q₃̇⎠ b_z + -l⋅u₁ a_x + l⋅u₁̇ a_y" ] }, "execution_count": 22, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Q.acc(N)" ] }, { "cell_type": "code", "execution_count": 23, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle (2 q_{3} u_{1} u_{2} \\sin{\\left(q_{2} \\right)} - q_{3} \\cos{\\left(q_{2} \\right)} \\dot{u}_{1} - 2 u_{1} u_{3} \\cos{\\left(q_{2} \\right)})\\mathbf{\\hat{b}_x} + (- q_{3} u_{1}^{2} \\cos^{2}{\\left(q_{2} \\right)} - q_{3} u_{2}^{2} + \\dot{u}_{3})\\mathbf{\\hat{b}_y} + (q_{3} u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)} + q_{3} \\dot{u}_{2} + 2 u_{2} u_{3})\\mathbf{\\hat{b}_z} - l u_{1}^{2}\\mathbf{\\hat{a}_x} + l \\dot{u}_{1}\\mathbf{\\hat{a}_y}$" ], "text/plain": [ " ⎛ 2 2 ↪\n", "(2⋅q₃⋅u₁⋅u₂⋅sin(q₂) - q₃⋅cos(q₂)⋅u₁̇ - 2⋅u₁⋅u₃⋅cos(q₂)) b_x + ⎝- q₃⋅u₁ ⋅cos (q₂ ↪\n", "\n", "↪ 2 ⎞ ⎛ 2 ⎞ ↪\n", "↪ ) - q₃⋅u₂ + u₃̇⎠ b_y + ⎝q₃⋅u₁ ⋅sin(q₂)⋅cos(q₂) + q₃⋅u₂̇ + 2⋅u₂⋅u₃⎠ b_z + -l⋅u ↪\n", "\n", "↪ 2 \n", "↪ ₁ a_x + l⋅u₁̇ a_y" ] }, "execution_count": 23, "metadata": {}, "output_type": "execute_result" } ], "source": [ "t = me.dynamicsymbols._t\n", "\n", "qdot_repl = {q1.diff(t): u1,\n", " q2.diff(t): u2,\n", " q3.diff(t): u3}\n", "\n", "Q.set_acc(N, Q.acc(N).xreplace(qdot_repl))\n", "Q.acc(N)" ] }, { "cell_type": "code", "execution_count": 24, "metadata": {}, "outputs": [], "source": [ "R_Ao = m*g*N.x\n", "R_Bo = m*g*N.x + kl*q3*B.y\n", "R_Q = m/4*g*N.x - kl*q3*B.y\n", "T_A = -kt*q1*N.z + kt*q2*A.x\n", "T_B = -kt*q2*A.x" ] }, { "cell_type": "code", "execution_count": 25, "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)" ] }, { "cell_type": "code", "execution_count": 26, "metadata": {}, "outputs": [], "source": [ "v_Ao_1 = Ao.vel(N).diff(u1, N)\n", "v_Bo_1 = Bo.vel(N).diff(u1, N)\n", "v_Q_1 = Q.vel(N).diff(u1, N)\n", "\n", "v_Ao_2 = Ao.vel(N).diff(u2, N)\n", "v_Bo_2 = Bo.vel(N).diff(u2, N)\n", "v_Q_2 = Q.vel(N).diff(u2, N)\n", "\n", "v_Ao_3 = Ao.vel(N).diff(u3, N)\n", "v_Bo_3 = Bo.vel(N).diff(u3, N)\n", "v_Q_3 = Q.vel(N).diff(u3, N)\n", "\n", "w_A_1 = A.ang_vel_in(N).diff(u1, N)\n", "w_B_1 = B.ang_vel_in(N).diff(u1, N)\n", "\n", "w_A_2 = A.ang_vel_in(N).diff(u2, N)\n", "w_B_2 = B.ang_vel_in(N).diff(u2, N)\n", "\n", "w_A_3 = A.ang_vel_in(N).diff(u3, N)\n", "w_B_3 = B.ang_vel_in(N).diff(u3, N)" ] }, { "cell_type": "code", "execution_count": 27, "metadata": {}, "outputs": [], "source": [ "F1 = v_Ao_1.dot(R_Ao) + v_Bo_1.dot(R_Bo) + v_Q_1.dot(R_Q) + w_A_1.dot(T_A) + w_B_1.dot(T_B)\n", "F2 = v_Ao_2.dot(R_Ao) + v_Bo_2.dot(R_Bo) + v_Q_2.dot(R_Q) + w_A_2.dot(T_A) + w_B_2.dot(T_B)\n", "F3 = v_Ao_3.dot(R_Ao) + v_Bo_3.dot(R_Bo) + v_Q_3.dot(R_Q) + w_A_3.dot(T_A) + w_B_3.dot(T_B)" ] }, { "cell_type": "code", "execution_count": 28, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- \\frac{7 g l m \\sin{\\left(q_{1} \\right)}}{4} - \\frac{g m q_{3} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}}{4} - k_{t} q_{1}\\\\\\frac{g m q_{3} \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)}}{4} - k_{t} q_{2}\\\\- \\frac{g m \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}}{4} - k_{l} q_{3}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 7⋅g⋅l⋅m⋅sin(q₁) g⋅m⋅q₃⋅cos(q₁)⋅cos(q₂) ⎤\n", "⎢- ─────────────── - ────────────────────── - kₜ⋅q₁⎥\n", "⎢ 4 4 ⎥\n", "⎢ ⎥\n", "⎢ g⋅m⋅q₃⋅sin(q₁)⋅sin(q₂) ⎥\n", "⎢ ────────────────────── - kₜ⋅q₂ ⎥\n", "⎢ 4 ⎥\n", "⎢ ⎥\n", "⎢ g⋅m⋅sin(q₁)⋅cos(q₂) ⎥\n", "⎢ - ─────────────────── - kₗ⋅q₃ ⎥\n", "⎣ 4 ⎦" ] }, "execution_count": 28, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Fr = sm.Matrix([F1, F2, F3])\n", "Fr" ] }, { "cell_type": "code", "execution_count": 29, "metadata": {}, "outputs": [], "source": [ "TAs = -(A.ang_acc_in(N).dot(I_A_Ao) + me.cross(A.ang_vel_in(N), I_A_Ao).dot(A.ang_vel_in(N)))\n", "TBs = -(B.ang_acc_in(N).dot(I_B_Bo) + me.cross(B.ang_vel_in(N), I_B_Bo).dot(B.ang_vel_in(N)))\n", "\n", "F1s = v_Ao_1.dot(-m*Ao.acc(N)) + v_Bo_1.dot(-m*Bo.acc(N)) + v_Q_1.dot(-m/4*Q.acc(N))\n", "F1s += w_A_1.dot(TAs) + w_B_1.dot(TBs)\n", "\n", "F2s = v_Ao_2.dot(-m*Ao.acc(N)) + v_Bo_2.dot(-m*Bo.acc(N)) + v_Q_2.dot(-m/4*Q.acc(N))\n", "F2s += w_A_2.dot(TAs) + w_B_2.dot(TBs)\n", "\n", "F3s = v_Ao_3.dot(-m*Ao.acc(N)) + v_Bo_3.dot(-m*Bo.acc(N)) + v_Q_3.dot(-m/4*Q.acc(N))\n", "F3s += w_A_3.dot(TAs) + w_B_3.dot(TBs)" ] }, { "cell_type": "code", "execution_count": 30, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- \\frac{19 l^{2} m \\dot{u}_{1}}{12} - \\frac{l m q_{3} u_{1}^{2} \\cos{\\left(q_{2} \\right)}}{4} + l \\left(- \\frac{m \\left(- q_{3} u_{1}^{2} \\cos^{2}{\\left(q_{2} \\right)} - q_{3} u_{2}^{2} + \\dot{u}_{3}\\right) \\cos{\\left(q_{2} \\right)}}{4} + \\frac{m \\left(q_{3} u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)} + q_{3} \\dot{u}_{2} + 2 u_{2} u_{3}\\right) \\sin{\\left(q_{2} \\right)}}{4}\\right) + \\frac{m \\left(2 q_{3} u_{1} u_{2} \\sin{\\left(q_{2} \\right)} - q_{3} \\cos{\\left(q_{2} \\right)} \\dot{u}_{1} - 2 u_{1} u_{3} \\cos{\\left(q_{2} \\right)}\\right) q_{3} \\cos{\\left(q_{2} \\right)}}{4} + \\left(- \\frac{l^{2} m \\left(- u_{1} u_{2} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{2} \\right)} \\dot{u}_{1}\\right)}{12} + \\frac{l^{2} m u_{1} u_{2} \\sin{\\left(q_{2} \\right)}}{12}\\right) \\cos{\\left(q_{2} \\right)}\\\\- \\frac{l^{2} m u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)}}{12} - \\frac{l^{2} m \\dot{u}_{2}}{12} + \\frac{l m q_{3} \\sin{\\left(q_{2} \\right)} \\dot{u}_{1}}{4} - \\frac{m \\left(q_{3} u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)} + q_{3} \\dot{u}_{2} + 2 u_{2} u_{3}\\right) q_{3}}{4}\\\\- \\frac{l m \\cos{\\left(q_{2} \\right)} \\dot{u}_{1}}{4} - \\frac{m \\left(- q_{3} u_{1}^{2} \\cos^{2}{\\left(q_{2} \\right)} - q_{3} u_{2}^{2} + \\dot{u}_{3}\\right)}{4}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 2 2 ⎛ ⎛ 2 2 2 ⎞ ↪\n", "⎢ 19⋅l ⋅m⋅u₁̇ l⋅m⋅q₃⋅u₁ ⋅cos(q₂) ⎜ m⋅⎝- q₃⋅u₁ ⋅cos (q₂) - q₃⋅u₂ + u₃̇⎠⋅ ↪\n", "⎢- ────────── - ────────────────── + l⋅⎜- ──────────────────────────────────── ↪\n", "⎢ 12 4 ⎝ 4 ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎣ ↪\n", "\n", "↪ ⎛ 2 ⎞ ⎞ ↪\n", "↪ cos(q₂) m⋅⎝q₃⋅u₁ ⋅sin(q₂)⋅cos(q₂) + q₃⋅u₂̇ + 2⋅u₂⋅u₃⎠⋅sin(q₂)⎟ m⋅(2⋅q₃⋅u₁ ↪\n", "↪ ─────── + ────────────────────────────────────────────────────⎟ + ────────── ↪\n", "↪ 4 ⎠ ↪\n", "↪ ↪\n", "↪ 2 2 2 ⎛ ↪\n", "↪ l ⋅m⋅u₁ ⋅sin(q₂)⋅cos(q₂) l ⋅m⋅u₂̇ l⋅m⋅q₃⋅sin(q₂)⋅u₁̇ m⋅⎝q₃⋅u ↪\n", "↪ - ──────────────────────── - ─────── + ───────────────── - ─────── ↪\n", "↪ 12 12 4 ↪\n", "↪ ↪\n", "↪ ⎛ 2 2 ↪\n", "↪ l⋅m⋅cos(q₂)⋅u₁̇ m⋅⎝- q₃⋅u₁ ⋅cos (q₂) ↪\n", "↪ - ────────────── - ───────────────────── ↪\n", "↪ 4 4 ↪\n", "\n", "↪ ⎛ 2 ↪\n", "↪ ⋅u₂⋅sin(q₂) - q₃⋅cos(q₂)⋅u₁̇ - 2⋅u₁⋅u₃⋅cos(q₂))⋅q₃⋅cos(q₂) ⎜ l ⋅m⋅(-u₁⋅u₂⋅ ↪\n", "↪ ───────────────────────────────────────────────────────── + ⎜- ───────────── ↪\n", "↪ 4 ⎝ ↪\n", "↪ ↪\n", "↪ 2 ⎞ ↪\n", "↪ ₁ ⋅sin(q₂)⋅cos(q₂) + q₃⋅u₂̇ + 2⋅u₂⋅u₃⎠⋅q₃ ↪\n", "↪ ──────────────────────────────────────── ↪\n", "↪ 4 ↪\n", "↪ ↪\n", "↪ 2 ⎞ ↪\n", "↪ - q₃⋅u₂ + u₃̇⎠ ↪\n", "↪ ────────────── ↪\n", "↪ ↪\n", "\n", "↪ 2 ⎞ ⎤\n", "↪ sin(q₂) + cos(q₂)⋅u₁̇) l ⋅m⋅u₁⋅u₂⋅sin(q₂)⎟ ⎥\n", "↪ ───────────────────── + ──────────────────⎟⋅cos(q₂)⎥\n", "↪ 12 12 ⎠ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎦" ] }, "execution_count": 30, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Frs = sm.Matrix([F1s, F2s, F3s])\n", "Frs" ] }, { "cell_type": "code", "execution_count": 31, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left\\{q_{1}, q_{2}, q_{3}\\right\\}$" ], "text/plain": [ "{q₁, q₂, q₃}" ] }, "execution_count": 31, "metadata": {}, "output_type": "execute_result" } ], "source": [ "me.find_dynamicsymbols(Fr)" ] }, { "cell_type": "code", "execution_count": 32, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left\\{q_{2}, q_{3}, u_{1}, u_{2}, u_{3}, \\dot{u}_{1}, \\dot{u}_{2}, \\dot{u}_{3}\\right\\}$" ], "text/plain": [ "{q₂, q₃, u₁, u₂, u₃, u₁̇, u₂̇, u₃̇}" ] }, "execution_count": 32, "metadata": {}, "output_type": "execute_result" } ], "source": [ "me.find_dynamicsymbols(Frs)" ] }, { "cell_type": "code", "execution_count": 33, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}\\dot{u}_{1}\\\\\\dot{u}_{2}\\\\\\dot{u}_{3}\\end{matrix}\\right]$" ], "text/plain": [ "⎡u₁̇⎤\n", "⎢ ⎥\n", "⎢u₂̇⎥\n", "⎢ ⎥\n", "⎣u₃̇⎦" ] }, "execution_count": 33, "metadata": {}, "output_type": "execute_result" } ], "source": [ "u = sm.Matrix([u1, u2, u3])\n", "ud = u.diff(t)\n", "ud" ] }, { "cell_type": "code", "execution_count": 34, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- \\frac{l^{2} m \\cos^{2}{\\left(q_{2} \\right)}}{12} - \\frac{19 l^{2} m}{12} - \\frac{m q_{3}^{2} \\cos^{2}{\\left(q_{2} \\right)}}{4} & \\frac{l m q_{3} \\sin{\\left(q_{2} \\right)}}{4} & - \\frac{l m \\cos{\\left(q_{2} \\right)}}{4}\\\\\\frac{l m q_{3} \\sin{\\left(q_{2} \\right)}}{4} & - \\frac{l^{2} m}{12} - \\frac{m q_{3}^{2}}{4} & 0\\\\- \\frac{l m \\cos{\\left(q_{2} \\right)}}{4} & 0 & - \\frac{m}{4}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 2 2 2 2 2 ⎤\n", "⎢ l ⋅m⋅cos (q₂) 19⋅l ⋅m m⋅q₃ ⋅cos (q₂) l⋅m⋅q₃⋅sin(q₂) -l⋅m⋅cos(q₂) ⎥\n", "⎢- ───────────── - ─────── - ────────────── ────────────── ─────────────⎥\n", "⎢ 12 12 4 4 4 ⎥\n", "⎢ ⎥\n", "⎢ 2 2 ⎥\n", "⎢ l⋅m⋅q₃⋅sin(q₂) l ⋅m m⋅q₃ ⎥\n", "⎢ ────────────── - ──── - ───── 0 ⎥\n", "⎢ 4 12 4 ⎥\n", "⎢ ⎥\n", "⎢ -l⋅m⋅cos(q₂) -m ⎥\n", "⎢ ───────────── 0 ─── ⎥\n", "⎣ 4 4 ⎦" ] }, "execution_count": 34, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Md = Frs.jacobian(ud)\n", "Md" ] }, { "cell_type": "code", "execution_count": 35, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left\\{ \\dot{u}_{1} : 0, \\ \\dot{u}_{2} : 0, \\ \\dot{u}_{3} : 0\\right\\}$" ], "text/plain": [ "{u₁̇: 0, u₂̇: 0, u₃̇: 0}" ] }, "execution_count": 35, "metadata": {}, "output_type": "execute_result" } ], "source": [ "ud_zerod = {udr: 0 for udr in ud}\n", "ud_zerod\n" ] }, { "cell_type": "code", "execution_count": 36, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left[\\begin{matrix}- \\frac{7 g l m \\sin{\\left(q_{1} \\right)}}{4} - \\frac{g m q_{3} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}}{4} - k_{t} q_{1} + \\frac{l^{2} m u_{1} u_{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)}}{6} - \\frac{l m q_{3} u_{1}^{2} \\cos{\\left(q_{2} \\right)}}{4} + l \\left(- \\frac{m \\left(- q_{3} u_{1}^{2} \\cos^{2}{\\left(q_{2} \\right)} - q_{3} u_{2}^{2}\\right) \\cos{\\left(q_{2} \\right)}}{4} + \\frac{m \\left(q_{3} u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)} + 2 u_{2} u_{3}\\right) \\sin{\\left(q_{2} \\right)}}{4}\\right) + \\frac{m \\left(2 q_{3} u_{1} u_{2} \\sin{\\left(q_{2} \\right)} - 2 u_{1} u_{3} \\cos{\\left(q_{2} \\right)}\\right) q_{3} \\cos{\\left(q_{2} \\right)}}{4}\\\\\\frac{g m q_{3} \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)}}{4} - k_{t} q_{2} - \\frac{l^{2} m u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)}}{12} - \\frac{m \\left(q_{3} u_{1}^{2} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{2} \\right)} + 2 u_{2} u_{3}\\right) q_{3}}{4}\\\\- \\frac{g m \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}}{4} - k_{l} q_{3} - \\frac{m \\left(- q_{3} u_{1}^{2} \\cos^{2}{\\left(q_{2} \\right)} - q_{3} u_{2}^{2}\\right)}{4}\\end{matrix}\\right]$" ], "text/plain": [ "⎡ 2 ↪\n", "⎢ 7⋅g⋅l⋅m⋅sin(q₁) g⋅m⋅q₃⋅cos(q₁)⋅cos(q₂) l ⋅m⋅u₁⋅u₂⋅sin(q₂)⋅cos(q ↪\n", "⎢- ─────────────── - ────────────────────── - kₜ⋅q₁ + ──────────────────────── ↪\n", "⎢ 4 4 6 ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ g⋅m ↪\n", "⎢ ─── ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎢ ↪\n", "⎣ ↪\n", "\n", "↪ 2 ⎛ ⎛ 2 2 2⎞ ⎛ ↪\n", "↪ ₂) l⋅m⋅q₃⋅u₁ ⋅cos(q₂) ⎜ m⋅⎝- q₃⋅u₁ ⋅cos (q₂) - q₃⋅u₂ ⎠⋅cos(q₂) m⋅⎝q ↪\n", "↪ ── - ────────────────── + l⋅⎜- ────────────────────────────────────── + ──── ↪\n", "↪ 4 ⎝ 4 ↪\n", "↪ ↪\n", "↪ 2 2 ⎛ 2 ↪\n", "↪ ⋅q₃⋅sin(q₁)⋅sin(q₂) l ⋅m⋅u₁ ⋅sin(q₂)⋅cos(q₂) m⋅⎝q₃⋅u₁ ⋅sin(q₂)⋅c ↪\n", "↪ ─────────────────── - kₜ⋅q₂ - ──────────────────────── - ─────────────────── ↪\n", "↪ 4 12 ↪\n", "↪ ↪\n", "↪ ⎛ 2 2 ↪\n", "↪ g⋅m⋅sin(q₁)⋅cos(q₂) m⋅⎝- q₃⋅u₁ ⋅cos (q₂) - q₃⋅u₂ ↪\n", "↪ - ─────────────────── - kₗ⋅q₃ - ──────────────────────────── ↪\n", "↪ 4 4 ↪\n", "\n", "↪ 2 ⎞ ⎞ ↪\n", "↪ ₃⋅u₁ ⋅sin(q₂)⋅cos(q₂) + 2⋅u₂⋅u₃⎠⋅sin(q₂)⎟ m⋅(2⋅q₃⋅u₁⋅u₂⋅sin(q₂) - 2⋅u₁⋅u₃⋅ ↪\n", "↪ ────────────────────────────────────────⎟ + ──────────────────────────────── ↪\n", "↪ 4 ⎠ 4 ↪\n", "↪ ↪\n", "↪ ⎞ ↪\n", "↪ os(q₂) + 2⋅u₂⋅u₃⎠⋅q₃ ↪\n", "↪ ──────────────────── ↪\n", "↪ 4 ↪\n", "↪ ↪\n", "↪ 2⎞ ↪\n", "↪ ⎠ ↪\n", "↪ ── ↪\n", "↪ ↪\n", "\n", "↪ ⎤\n", "↪ cos(q₂))⋅q₃⋅cos(q₂)⎥\n", "↪ ───────────────────⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎥\n", "↪ ⎦" ] }, "execution_count": 36, "metadata": {}, "output_type": "execute_result" } ], "source": [ "gd = Frs.xreplace(ud_zerod) + Fr\n", "gd" ] }, { "cell_type": "code", "execution_count": 37, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left\\{q_{2}, q_{3}\\right\\}$" ], "text/plain": [ "{q₂, q₃}" ] }, "execution_count": 37, "metadata": {}, "output_type": "execute_result" } ], "source": [ "me.find_dynamicsymbols(Md)" ] }, { "cell_type": "code", "execution_count": 38, "metadata": {}, "outputs": [ { "data": { "text/latex": [ "$\\displaystyle \\left\\{q_{1}, q_{2}, q_{3}, u_{1}, u_{2}, u_{3}\\right\\}$" ], "text/plain": [ "{q₁, q₂, q₃, u₁, u₂, u₃}" ] }, "execution_count": 38, "metadata": {}, "output_type": "execute_result" } ], "source": [ "me.find_dynamicsymbols(gd)" ] } ], "metadata": { "kernelspec": { "display_name": "MinerU", "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.10.16" } }, "nbformat": 4, "nbformat_minor": 2 }