Last active
August 21, 2019 21:19
-
-
Save Stapelzeiger/8637ebe2f8d3dd452b3de0ab1af63a64 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
{ | |
"cells": [ | |
{ | |
"cell_type": "code", | |
"execution_count": 19, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"import sympy as sp\n", | |
"sp.init_printing()\n", | |
"from sympy import sin, cos, Matrix" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 22, | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"def radians(deg):\n", | |
" return sp.pi*deg/180" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 145, | |
"metadata": {}, | |
"outputs": [ | |
{ | |
"data": { | |
"text/latex": [ | |
"$$\\left[\\begin{matrix}- \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(- l_{2} \\cos{\\left (\\alpha \\right )} - \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) & - \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(- l_{2} \\cos{\\left (\\alpha \\right )} - \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) & \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(l_{2} \\cos{\\left (\\alpha \\right )} + \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) & \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(l_{2} \\cos{\\left (\\alpha \\right )} + \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right)\\\\- \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(l_{1} \\cos{\\left (\\alpha \\right )} + \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) & \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(- l_{1} \\cos{\\left (\\alpha \\right )} - \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) & - \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(l_{1} \\cos{\\left (\\alpha \\right )} + \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) & \\frac{\\sqrt{2} CQ \\sin{\\left (\\alpha \\right )}}{2} + CT \\left(- l_{1} \\cos{\\left (\\alpha \\right )} - \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right)\\\\- CQ \\cos{\\left (\\alpha \\right )} + CT \\left(- \\frac{\\sqrt{2} l_{1} \\sin{\\left (\\alpha \\right )}}{2} + \\frac{\\sqrt{2} l_{2} \\sin{\\left (\\alpha \\right )}}{2}\\right) & CQ \\cos{\\left (\\alpha \\right )} + CT \\left(\\frac{\\sqrt{2} l_{1} \\sin{\\left (\\alpha \\right )}}{2} - \\frac{\\sqrt{2} l_{2} \\sin{\\left (\\alpha \\right )}}{2}\\right) & CQ \\cos{\\left (\\alpha \\right )} + CT \\left(\\frac{\\sqrt{2} l_{1} \\sin{\\left (\\alpha \\right )}}{2} - \\frac{\\sqrt{2} l_{2} \\sin{\\left (\\alpha \\right )}}{2}\\right) & - CQ \\cos{\\left (\\alpha \\right )} + CT \\left(- \\frac{\\sqrt{2} l_{1} \\sin{\\left (\\alpha \\right )}}{2} + \\frac{\\sqrt{2} l_{2} \\sin{\\left (\\alpha \\right )}}{2}\\right)\\end{matrix}\\right]$$" | |
], | |
"text/plain": [ | |
"⎡ √2⋅CQ⋅sin(α) ⎛ √2⋅l₃⋅sin(α)⎞ √2⋅CQ⋅sin(α) ⎛ \n", | |
"⎢- ──────────── + CT⋅⎜-l₂⋅cos(α) - ────────────⎟ - ──────────── + CT⋅⎜-l₂⋅cos\n", | |
"⎢ 2 ⎝ 2 ⎠ 2 ⎝ \n", | |
"⎢ \n", | |
"⎢ √2⋅CQ⋅sin(α) ⎛ √2⋅l₃⋅sin(α)⎞ √2⋅CQ⋅sin(α) ⎛ \n", | |
"⎢- ──────────── + CT⋅⎜l₁⋅cos(α) + ────────────⎟ ──────────── + CT⋅⎜-l₁⋅cos(\n", | |
"⎢ 2 ⎝ 2 ⎠ 2 ⎝ \n", | |
"⎢ \n", | |
"⎢ ⎛ √2⋅l₁⋅sin(α) √2⋅l₂⋅sin(α)⎞ ⎛√2⋅l₁⋅sin(α\n", | |
"⎢-CQ⋅cos(α) + CT⋅⎜- ──────────── + ────────────⎟ CQ⋅cos(α) + CT⋅⎜───────────\n", | |
"⎣ ⎝ 2 2 ⎠ ⎝ 2 \n", | |
"\n", | |
" √2⋅l₃⋅sin(α)⎞ √2⋅CQ⋅sin(α) ⎛ √2⋅l₃⋅sin(α)⎞ √2⋅CQ⋅si\n", | |
"(α) - ────────────⎟ ──────────── + CT⋅⎜l₂⋅cos(α) + ────────────⎟ ────────\n", | |
" 2 ⎠ 2 ⎝ 2 ⎠ 2 \n", | |
" \n", | |
" √2⋅l₃⋅sin(α)⎞ √2⋅CQ⋅sin(α) ⎛ √2⋅l₃⋅sin(α)⎞ √2⋅CQ⋅si\n", | |
"α) - ────────────⎟ - ──────────── + CT⋅⎜l₁⋅cos(α) + ────────────⎟ ────────\n", | |
" 2 ⎠ 2 ⎝ 2 ⎠ 2 \n", | |
" \n", | |
") √2⋅l₂⋅sin(α)⎞ ⎛√2⋅l₁⋅sin(α) √2⋅l₂⋅sin(α)⎞ \n", | |
"─ - ────────────⎟ CQ⋅cos(α) + CT⋅⎜──────────── - ────────────⎟ -CQ⋅cos(α\n", | |
" 2 ⎠ ⎝ 2 2 ⎠ \n", | |
"\n", | |
"n(α) ⎛ √2⋅l₃⋅sin(α)⎞ ⎤\n", | |
"──── + CT⋅⎜l₂⋅cos(α) + ────────────⎟ ⎥\n", | |
" ⎝ 2 ⎠ ⎥\n", | |
" ⎥\n", | |
"n(α) ⎛ √2⋅l₃⋅sin(α)⎞ ⎥\n", | |
"──── + CT⋅⎜-l₁⋅cos(α) - ────────────⎟ ⎥\n", | |
" ⎝ 2 ⎠ ⎥\n", | |
" ⎥\n", | |
" ⎛ √2⋅l₁⋅sin(α) √2⋅l₂⋅sin(α)⎞⎥\n", | |
") + CT⋅⎜- ──────────── + ────────────⎟⎥\n", | |
" ⎝ 2 2 ⎠⎦" | |
] | |
}, | |
"execution_count": 145, | |
"metadata": {}, | |
"output_type": "execute_result" | |
} | |
], | |
"source": [ | |
"\"\"\"\n", | |
"n3 ^x n1\n", | |
" |___>y\n", | |
"n4 n2\n", | |
"\"\"\"\n", | |
"\n", | |
"## Prop Position\n", | |
"# props are as in fig above, CG is below prop plane\n", | |
"l1, l2, l3 =sp.symbols('l1, l2, l3') # l1, l2, l3 positive distances prop-CG along x,y,z\n", | |
"p1 = Matrix([l1, l2, -l3])\n", | |
"p2 = Matrix([-l1, l2, -l3])\n", | |
"p3 = Matrix([l1, -l2, -l3])\n", | |
"p4 = Matrix([-l1, -l2, -l3])\n", | |
"\n", | |
"## Prop Direction\n", | |
"# tilt in pitch, then yaw around for each prop\n", | |
"prp_tilt = sp.symbols('alpha') # props are tilted in -> alpha is positive\n", | |
"R_prop_pitch = Matrix([\n", | |
" [cos(prp_tilt), 0, sin(prp_tilt)],\n", | |
" [0, 1, 0],\n", | |
" [-sin(prp_tilt), 0, cos(prp_tilt)],\n", | |
"])\n", | |
"\n", | |
"R_prop_yaw = lambda prp_y: Matrix([\n", | |
" [cos(prp_y), -sin(prp_y), 0],\n", | |
" [sin(prp_y), cos(prp_y), 0],\n", | |
" [0, 0, 1],\n", | |
"])\n", | |
"up = Matrix([0, 0, -1])\n", | |
"d1 = R_prop_yaw(radians(45)) * R_prop_pitch * up\n", | |
"d2 = R_prop_yaw(radians(45+90)) * R_prop_pitch * up\n", | |
"d3 = R_prop_yaw(radians(45+90*3)) * R_prop_pitch * up\n", | |
"d4 = R_prop_yaw(radians(45+90*2)) * R_prop_pitch * up\n", | |
"\n", | |
"## Prop\n", | |
"# model:\n", | |
"# Thrust: T = CT * n^2\n", | |
"# Torque: Q = CQ * n^2\n", | |
"CT, CQ = sp.symbols('CT, CQ')\n", | |
"\n", | |
"#spin direction (along di)\n", | |
"s1 = +1\n", | |
"s2 = -1\n", | |
"s3 = -1\n", | |
"s4 = +1\n", | |
"\n", | |
"# torque B matrix\n", | |
"B = Matrix([\n", | |
" (CT*p1.cross(d1) + s1*CQ*d1).T,\n", | |
" (CT*p2.cross(d2) + s2*CQ*d2).T,\n", | |
" (CT*p3.cross(d3) + s3*CQ*d3).T,\n", | |
" (CT*p4.cross(d4) + s4*CQ*d4).T\n", | |
"]).T\n", | |
"B" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 162, | |
"metadata": { | |
"scrolled": true | |
}, | |
"outputs": [ | |
{ | |
"data": { | |
"text/latex": [ | |
"$$\\left[\\begin{matrix}-1 & 1 & 1\\\\-1 & -1 & -1\\\\1 & 1 & -1\\\\1 & -1 & 1\\end{matrix}\\right]$$" | |
], | |
"text/plain": [ | |
"⎡-1 1 1 ⎤\n", | |
"⎢ ⎥\n", | |
"⎢-1 -1 -1⎥\n", | |
"⎢ ⎥\n", | |
"⎢1 1 -1⎥\n", | |
"⎢ ⎥\n", | |
"⎣1 -1 1 ⎦" | |
] | |
}, | |
"execution_count": 162, | |
"metadata": {}, | |
"output_type": "execute_result" | |
} | |
], | |
"source": [ | |
"# Binv as used in current torque to thruster mapping\n", | |
"Binv = Matrix([\n", | |
" [-1, -1, 1, 1],\n", | |
" [1, -1, 1, -1],\n", | |
" [1, -1, -1, 1]\n", | |
"]).T\n", | |
"Binv" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 159, | |
"metadata": {}, | |
"outputs": [ | |
{ | |
"name": "stdout", | |
"output_type": "stream", | |
"text": [ | |
"⎡-1⎤\n", | |
"⎢ ⎥\n", | |
"⎢-1⎥\n", | |
"⎢ ⎥\n", | |
"⎢1 ⎥\n", | |
"⎢ ⎥\n", | |
"⎣1 ⎦\n" | |
] | |
}, | |
{ | |
"data": { | |
"text/latex": [ | |
"$$\\left[\\begin{matrix}2 \\sqrt{2} CQ \\sin{\\left (\\alpha \\right )} - 2 CT \\left(- l_{2} \\cos{\\left (\\alpha \\right )} - \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) + 2 CT \\left(l_{2} \\cos{\\left (\\alpha \\right )} + \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right)\\\\0\\\\0\\end{matrix}\\right]$$" | |
], | |
"text/plain": [ | |
"⎡ ⎛ √2⋅l₃⋅sin(α)⎞ ⎛ √2⋅l₃⋅s\n", | |
"⎢2⋅√2⋅CQ⋅sin(α) - 2⋅CT⋅⎜-l₂⋅cos(α) - ────────────⎟ + 2⋅CT⋅⎜l₂⋅cos(α) + ───────\n", | |
"⎢ ⎝ 2 ⎠ ⎝ 2 \n", | |
"⎢ \n", | |
"⎢ 0 \n", | |
"⎢ \n", | |
"⎣ 0 \n", | |
"\n", | |
"in(α)⎞⎤\n", | |
"─────⎟⎥\n", | |
" ⎠⎥\n", | |
" ⎥\n", | |
" ⎥\n", | |
" ⎥\n", | |
" ⎦" | |
] | |
}, | |
"execution_count": 159, | |
"metadata": {}, | |
"output_type": "execute_result" | |
} | |
], | |
"source": [ | |
"n2 = Binv*Matrix([1, 0, 0])\n", | |
"sp.pprint(n2)\n", | |
"B*n2" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 161, | |
"metadata": {}, | |
"outputs": [ | |
{ | |
"name": "stdout", | |
"output_type": "stream", | |
"text": [ | |
"⎡1 ⎤\n", | |
"⎢ ⎥\n", | |
"⎢-1⎥\n", | |
"⎢ ⎥\n", | |
"⎢1 ⎥\n", | |
"⎢ ⎥\n", | |
"⎣-1⎦\n" | |
] | |
}, | |
{ | |
"data": { | |
"text/latex": [ | |
"$$\\left[\\begin{matrix}0\\\\- 2 \\sqrt{2} CQ \\sin{\\left (\\alpha \\right )} - 2 CT \\left(- l_{1} \\cos{\\left (\\alpha \\right )} - \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right) + 2 CT \\left(l_{1} \\cos{\\left (\\alpha \\right )} + \\frac{\\sqrt{2} l_{3} \\sin{\\left (\\alpha \\right )}}{2}\\right)\\\\0\\end{matrix}\\right]$$" | |
], | |
"text/plain": [ | |
"⎡ 0 \n", | |
"⎢ \n", | |
"⎢ ⎛ √2⋅l₃⋅sin(α)⎞ ⎛ √2⋅l₃⋅\n", | |
"⎢-2⋅√2⋅CQ⋅sin(α) - 2⋅CT⋅⎜-l₁⋅cos(α) - ────────────⎟ + 2⋅CT⋅⎜l₁⋅cos(α) + ──────\n", | |
"⎢ ⎝ 2 ⎠ ⎝ 2\n", | |
"⎢ \n", | |
"⎣ 0 \n", | |
"\n", | |
" ⎤\n", | |
" ⎥\n", | |
"sin(α)⎞⎥\n", | |
"──────⎟⎥\n", | |
" ⎠⎥\n", | |
" ⎥\n", | |
" ⎦" | |
] | |
}, | |
"execution_count": 161, | |
"metadata": {}, | |
"output_type": "execute_result" | |
} | |
], | |
"source": [ | |
"n2 = Binv*Matrix([0, 1, 0])\n", | |
"sp.pprint(n2)\n", | |
"B*n2" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 160, | |
"metadata": {}, | |
"outputs": [ | |
{ | |
"name": "stdout", | |
"output_type": "stream", | |
"text": [ | |
"⎡1 ⎤\n", | |
"⎢ ⎥\n", | |
"⎢-1⎥\n", | |
"⎢ ⎥\n", | |
"⎢-1⎥\n", | |
"⎢ ⎥\n", | |
"⎣1 ⎦\n" | |
] | |
}, | |
{ | |
"data": { | |
"text/latex": [ | |
"$$\\left[\\begin{matrix}0\\\\0\\\\- 4 CQ \\cos{\\left (\\alpha \\right )} + 2 CT \\left(- \\frac{\\sqrt{2} l_{1} \\sin{\\left (\\alpha \\right )}}{2} + \\frac{\\sqrt{2} l_{2} \\sin{\\left (\\alpha \\right )}}{2}\\right) - 2 CT \\left(\\frac{\\sqrt{2} l_{1} \\sin{\\left (\\alpha \\right )}}{2} - \\frac{\\sqrt{2} l_{2} \\sin{\\left (\\alpha \\right )}}{2}\\right)\\end{matrix}\\right]$$" | |
], | |
"text/plain": [ | |
"⎡ 0 \n", | |
"⎢ \n", | |
"⎢ 0 \n", | |
"⎢ \n", | |
"⎢ ⎛ √2⋅l₁⋅sin(α) √2⋅l₂⋅sin(α)⎞ ⎛√2⋅l₁⋅sin(α) √2\n", | |
"⎢-4⋅CQ⋅cos(α) + 2⋅CT⋅⎜- ──────────── + ────────────⎟ - 2⋅CT⋅⎜──────────── - ──\n", | |
"⎣ ⎝ 2 2 ⎠ ⎝ 2 \n", | |
"\n", | |
" ⎤\n", | |
" ⎥\n", | |
" ⎥\n", | |
" ⎥\n", | |
"⋅l₂⋅sin(α)⎞⎥\n", | |
"──────────⎟⎥\n", | |
" 2 ⎠⎦" | |
] | |
}, | |
"execution_count": 160, | |
"metadata": {}, | |
"output_type": "execute_result" | |
} | |
], | |
"source": [ | |
"n2 = Binv*Matrix([0, 0, 1])\n", | |
"sp.pprint(n2)\n", | |
"B*n2" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": null, | |
"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.7.4" | |
} | |
}, | |
"nbformat": 4, | |
"nbformat_minor": 2 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment