Skip to content

Instantly share code, notes, and snippets.

@Stapelzeiger

Stapelzeiger/SALL-E_b_matrix.ipynb

Last active August 21, 2019 21:19
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save Stapelzeiger/8637ebe2f8d3dd452b3de0ab1af63a64 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save Stapelzeiger/8637ebe2f8d3dd452b3de0ab1af63a64 to your computer and use it in GitHub Desktop.
Download ZIP
Display the source blob
Display the rendered blob
Raw
SALL-E_b_matrix.ipynb
{
"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