219 lines
16 KiB
Plaintext
219 lines
16 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 4,
|
||
|
|
"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": [
|
||
|
|
"# 建立坐标系"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 7,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"def smll_rot_trans(theta1, theta2, theta3):\n",
|
||
|
|
" theta11 = theta1 * theta1\n",
|
||
|
|
" theta22 = theta2 * theta2\n",
|
||
|
|
" theta33 = theta3 * theta3\n",
|
||
|
|
" sqrd_sum = theta11 + theta22 + theta33\n",
|
||
|
|
" sqrt1_sqrd_sum = (1.0 + sqrd_sum)**0.5\n",
|
||
|
|
" com_denom = sqrd_sum * sqrt1_sqrd_sum\n",
|
||
|
|
" theta12s = theta1 * theta2 * (sqrt1_sqrd_sum - 1.0)\n",
|
||
|
|
" theta13s = theta1 * theta3 * (sqrt1_sqrd_sum - 1.0)\n",
|
||
|
|
" theta23s = theta2 * theta3 * (sqrt1_sqrd_sum - 1.0)\n",
|
||
|
|
" \n",
|
||
|
|
" if com_denom == 0.0:\n",
|
||
|
|
" trans_mat = sm.Matrix([[1.0, 0.0, 0.0],\n",
|
||
|
|
" [0.0, 1.0, 0.0],\n",
|
||
|
|
" [0.0, 0.0, 1.0]])\n",
|
||
|
|
" \n",
|
||
|
|
" else:\n",
|
||
|
|
" trans_mat = sm.Matrix([[(theta11 * sqrt1_sqrd_sum + theta22 + theta33) / com_denom, (theta3 * sqrd_sum + theta12s) / com_denom, (-theta2 * sqrd_sum + theta13s) / com_denom],\n",
|
||
|
|
" [(-theta3 * sqrd_sum + theta12s) / com_denom, (theta11 + theta22 * sqrt1_sqrd_sum + theta33) / com_denom, (theta1 * sqrd_sum + theta23s) / com_denom],\n",
|
||
|
|
" [(theta2 * sqrd_sum + theta13s) / com_denom, (-theta1 * sqrd_sum + theta23s) / com_denom, (theta11 + theta22 + theta33 * sqrt1_sqrd_sum) / com_denom]])\n",
|
||
|
|
" return trans_mat"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 31,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/latex": [
|
||
|
|
"$\\displaystyle \\left[\\begin{matrix}\\frac{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} \\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}} & \\frac{\\left(\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} - 1.0\\right) \\theta_{1} \\theta_{2} + \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\theta_{3}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}} & \\frac{\\left(\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} - 1.0\\right) \\theta_{1} \\theta_{3} - \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\theta_{2}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}}\\\\\\frac{\\left(\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} - 1.0\\right) \\theta_{1} \\theta_{2} - \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\theta_{3}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}} & \\frac{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} \\theta_{2}^{2} + \\theta_{1}^{2} + \\theta_{3}^{2}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}} & \\frac{\\left(\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} - 1.0\\right) \\theta_{2} \\theta_{3} + \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\theta_{1}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}}\\\\\\frac{\\left(\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} - 1.0\\right) \\theta_{1} \\theta_{3} + \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\theta_{2}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}} & \\frac{\\left(\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} - 1.0\\right) \\theta_{2} \\theta_{3} - \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\theta_{1}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}} & \\frac{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5} \\theta_{3}^{2} + \\theta_{1}^{2} + \\theta_{2}^{2}}{\\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2}\\right) \\left(\\theta_{1}^{2} + \\theta_{2}^{2} + \\theta_{3}^{2} + 1.0\\right)^{0.5}}\\end{matrix}\\right]$"
|
||
|
|
],
|
||
|
|
"text/plain": [
|
||
|
|
"⎡ ⎛ 0.5 ⎞ ↪\n",
|
||
|
|
"⎢ ⎜⎛ 2 2 2 ⎞ 2 2 2⎟ ⎛ 2 2 2 ↪\n",
|
||
|
|
"⎢ ⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⋅θ₁ + θ₂ + θ₃ ⎠⋅⎝θ₁ + θ₂ + θ₃ + 1.0 ↪\n",
|
||
|
|
"⎢ ─────────────────────────────────────────────────────────────────── ↪\n",
|
||
|
|
"⎢ 2 2 2 ↪\n",
|
||
|
|
"⎢ θ₁ + θ₂ + θ₃ ↪\n",
|
||
|
|
"⎢ ↪\n",
|
||
|
|
"⎢⎛⎛ 0.5 ⎞ ⎞ ↪\n",
|
||
|
|
"⎢⎜⎜⎛ 2 2 2 ⎞ ⎟ ⎛ 2 2 2⎞ ⎟ ⎛ 2 2 ↪\n",
|
||
|
|
"⎢⎝⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ - 1.0⎠⋅θ₁⋅θ₂ - ⎝θ₁ + θ₂ + θ₃ ⎠⋅θ₃⎠⋅⎝θ₁ + θ₂ ↪\n",
|
||
|
|
"⎢───────────────────────────────────────────────────────────────────────────── ↪\n",
|
||
|
|
"⎢ 2 2 2 ↪\n",
|
||
|
|
"⎢ θ₁ + θ₂ + θ₃ ↪\n",
|
||
|
|
"⎢ ↪\n",
|
||
|
|
"⎢⎛⎛ 0.5 ⎞ ⎞ ↪\n",
|
||
|
|
"⎢⎜⎜⎛ 2 2 2 ⎞ ⎟ ⎛ 2 2 2⎞ ⎟ ⎛ 2 2 ↪\n",
|
||
|
|
"⎢⎝⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ - 1.0⎠⋅θ₁⋅θ₃ + ⎝θ₁ + θ₂ + θ₃ ⎠⋅θ₂⎠⋅⎝θ₁ + θ₂ ↪\n",
|
||
|
|
"⎢───────────────────────────────────────────────────────────────────────────── ↪\n",
|
||
|
|
"⎢ 2 2 2 ↪\n",
|
||
|
|
"⎣ θ₁ + θ₂ + θ₃ ↪\n",
|
||
|
|
"\n",
|
||
|
|
"↪ -0.5 ⎛⎛ 0.5 ⎞ ↪\n",
|
||
|
|
"↪ ⎞ ⎜⎜⎛ 2 2 2 ⎞ ⎟ ⎛ 2 2 ↪\n",
|
||
|
|
"↪ ⎠ ⎝⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ - 1.0⎠⋅θ₁⋅θ₂ + ⎝θ₁ + θ₂ + θ ↪\n",
|
||
|
|
"↪ ───── ────────────────────────────────────────────────────────── ↪\n",
|
||
|
|
"↪ 2 2 2 ↪\n",
|
||
|
|
"↪ θ₁ + θ₂ + θ₃ ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ -0.5 ⎛ 0.5 ⎞ ↪\n",
|
||
|
|
"↪ 2 ⎞ ⎜⎛ 2 2 2 ⎞ 2 2 2⎟ ⎛ ↪\n",
|
||
|
|
"↪ + θ₃ + 1.0⎠ ⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⋅θ₂ + θ₁ + θ₃ ⎠⋅⎝θ₁ ↪\n",
|
||
|
|
"↪ ──────────────── ──────────────────────────────────────────────── ↪\n",
|
||
|
|
"↪ 2 2 2 ↪\n",
|
||
|
|
"↪ θ₁ + θ₂ + θ₃ ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ -0.5 ⎛⎛ 0.5 ⎞ ↪\n",
|
||
|
|
"↪ 2 ⎞ ⎜⎜⎛ 2 2 2 ⎞ ⎟ ⎛ 2 2 ↪\n",
|
||
|
|
"↪ + θ₃ + 1.0⎠ ⎝⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ - 1.0⎠⋅θ₂⋅θ₃ - ⎝θ₁ + θ₂ + θ ↪\n",
|
||
|
|
"↪ ──────────────── ────────────────────────────────────────────────────────── ↪\n",
|
||
|
|
"↪ 2 2 2 ↪\n",
|
||
|
|
"↪ θ₁ + θ₂ + θ₃ ↪\n",
|
||
|
|
"\n",
|
||
|
|
"↪ ⎞ -0.5 ⎛⎛ 0.5 ⎞ ↪\n",
|
||
|
|
"↪ 2⎞ ⎟ ⎛ 2 2 2 ⎞ ⎜⎜⎛ 2 2 2 ⎞ ⎟ ↪\n",
|
||
|
|
"↪ ₃ ⎠⋅θ₃⎠⋅⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⎝⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ - 1.0⎠⋅θ₁⋅ ↪\n",
|
||
|
|
"↪ ─────────────────────────────────── ─────────────────────────────────────── ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ -0.5 ⎛⎛ 0.5 ⎞ ↪\n",
|
||
|
|
"↪ 2 2 2 ⎞ ⎜⎜⎛ 2 2 2 ⎞ ⎟ ↪\n",
|
||
|
|
"↪ + θ₂ + θ₃ + 1.0⎠ ⎝⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ - 1.0⎠⋅θ₂⋅ ↪\n",
|
||
|
|
"↪ ──────────────────────── ─────────────────────────────────────── ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ ⎞ -0.5 ⎛ 0.5 ↪\n",
|
||
|
|
"↪ 2⎞ ⎟ ⎛ 2 2 2 ⎞ ⎜⎛ 2 2 2 ⎞ ↪\n",
|
||
|
|
"↪ ₃ ⎠⋅θ₁⎠⋅⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⎝⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⋅θ ↪\n",
|
||
|
|
"↪ ─────────────────────────────────── ───────────────────────────── ↪\n",
|
||
|
|
"↪ ↪\n",
|
||
|
|
"↪ θ ↪\n",
|
||
|
|
"\n",
|
||
|
|
"↪ ⎞ -0.5⎤\n",
|
||
|
|
"↪ ⎛ 2 2 2⎞ ⎟ ⎛ 2 2 2 ⎞ ⎥\n",
|
||
|
|
"↪ θ₃ - ⎝θ₁ + θ₂ + θ₃ ⎠⋅θ₂⎠⋅⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⎥\n",
|
||
|
|
"↪ ──────────────────────────────────────────────────────⎥\n",
|
||
|
|
"↪ 2 2 2 ⎥\n",
|
||
|
|
"↪ θ₁ + θ₂ + θ₃ ⎥\n",
|
||
|
|
"↪ ⎥\n",
|
||
|
|
"↪ ⎞ -0.5⎥\n",
|
||
|
|
"↪ ⎛ 2 2 2⎞ ⎟ ⎛ 2 2 2 ⎞ ⎥\n",
|
||
|
|
"↪ θ₃ + ⎝θ₁ + θ₂ + θ₃ ⎠⋅θ₁⎠⋅⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⎥\n",
|
||
|
|
"↪ ──────────────────────────────────────────────────────⎥\n",
|
||
|
|
"↪ 2 2 2 ⎥\n",
|
||
|
|
"↪ θ₁ + θ₂ + θ₃ ⎥\n",
|
||
|
|
"↪ ⎥\n",
|
||
|
|
"↪ ⎞ -0.5 ⎥\n",
|
||
|
|
"↪ 2 2 2⎟ ⎛ 2 2 2 ⎞ ⎥\n",
|
||
|
|
"↪ ₃ + θ₁ + θ₂ ⎠⋅⎝θ₁ + θ₂ + θ₃ + 1.0⎠ ⎥\n",
|
||
|
|
"↪ ─────────────────────────────────────────── ⎥\n",
|
||
|
|
"↪ 2 2 2 ⎥\n",
|
||
|
|
"↪ ₁ + θ₂ + θ₃ ⎦"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 31,
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "execute_result"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"A = me.ReferenceFrame('A')\n",
|
||
|
|
"B = me.ReferenceFrame('B')\n",
|
||
|
|
"D = me.ReferenceFrame('D')\n",
|
||
|
|
"C = me.ReferenceFrame('C')\n",
|
||
|
|
"G = me.ReferenceFrame('G')\n",
|
||
|
|
"G_prime1 = me.ReferenceFrame('G_prime1')\n",
|
||
|
|
"G_prime2 = me.ReferenceFrame('G_prime2')\n",
|
||
|
|
"G_prime3 = me.ReferenceFrame('G_prime3')\n",
|
||
|
|
"# fast x-1, -y-3, z-2\n",
|
||
|
|
"\n",
|
||
|
|
"# 广义坐标\n",
|
||
|
|
"q1, q2, q3, q4, q_yaw, q_drtr, q_geaz, q_1f1, q_1f2, q_1e1, q_2f1, q_2f2, q_2e1, q_3f1, q_3f2, q_3e1 = me.dynamicsymbols('q1, q2, q3, q4, q_yaw, q_drtr, q_geaz, q_1f1, q_1f2, q_1e1, q_2f1, q_2f2, q_2e1, q_3f1, q_3f2, q_3e1')\n",
|
||
|
|
"# tower top\n",
|
||
|
|
"theta1, theta2, theta3 = me.dynamicsymbols(\"theta1, theta2, theta3\")\n",
|
||
|
|
"dcm = smll_rot_trans(theta1, theta2, theta3)\n",
|
||
|
|
"B.orient_dcm(A, dcm)\n",
|
||
|
|
"\n",
|
||
|
|
"# nacelle\n",
|
||
|
|
"D.orient_axis(B, q_yaw, B.y)\n",
|
||
|
|
"\n",
|
||
|
|
"# shaft C \n",
|
||
|
|
"theta_tilt = sm.symbols(\"theta_tilt\")\n",
|
||
|
|
"C.orient_axis(D, theta_tilt, D.z)\n",
|
||
|
|
"\n",
|
||
|
|
"# Hub G = Azimuth E\n",
|
||
|
|
"Azimuth = q_drtr + q_geaz\n",
|
||
|
|
"G.orient_axis(C, Azimuth, C.x)\n",
|
||
|
|
"\n",
|
||
|
|
"# 到每只叶片\n",
|
||
|
|
"angle = 2*sm.pi/3\n",
|
||
|
|
"G_prime1.orient_axis(G, angle * 0, G.x)\n",
|
||
|
|
"G_prime2.orient_axis(G, angle * 1, G.x)\n",
|
||
|
|
"G_prime3.orient_axis(G, angle * 2, G.x)\n",
|
||
|
|
"\n",
|
||
|
|
"B.dcm(A)\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"# 速度"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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
|
||
|
|
}
|