gyz/obsidian_backup
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
obsidian_backup/补课/多体动力学/17_Equations_of_Motions.ipynb

1097 lines
48 KiB
Plaintext
Raw Normal View History

vault backup: 2025-03-23 12:18:57
2025-03-23 12:18:58 +08:00
{
vault backup: 2025-03-23 12:40:05
2025-03-23 12:40:05 +08:00
"cells": [
{
"cell_type": "code",
vault backup: 2025-03-28 09:27:26
2025-03-28 09:27:28 +08:00
"execution_count": 2,
vault backup: 2025-03-23 12:40:05
2025-03-23 12:40:05 +08:00
"metadata": {},
"outputs": [],
"source": [
"import sympy as sm\n",
"import sympy.physics.mechanics as me\n",
"me.init_vprinting(use_latex='mathjax')"
]
vault backup: 2025-03-28 09:27:26
2025-03-28 09:27:28 +08:00
},
{
"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)"
]
vault backup: 2025-03-23 12:40:05
2025-03-23 12:40:05 +08:00
}
],
vault backup: 2025-03-23 12:18:57
2025-03-23 12:18:58 +08:00
"metadata": {
"kernelspec": {
vault backup: 2025-03-28 09:27:26
2025-03-28 09:27:28 +08:00
"display_name": "MinerU",
vault backup: 2025-03-23 12:18:57
2025-03-23 12:18:58 +08:00
"language": "python",
"name": "python3"
},
"language_info": {
vault backup: 2025-03-28 09:27:26
2025-03-28 09:27:28 +08:00
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
vault backup: 2025-03-23 12:18:57
2025-03-23 12:18:58 +08:00
"name": "python",
vault backup: 2025-03-28 09:27:26
2025-03-28 09:27:28 +08:00
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.16"
vault backup: 2025-03-23 12:18:57
2025-03-23 12:18:58 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 2
}
Reference in New Issue Copy Permalink