mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
483 lines
30 KiB
Plaintext
483 lines
30 KiB
Plaintext
{
|
||
"cells": [
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"### Simulation"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 16,
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/html": [
|
||
"\n",
|
||
"<style>\n",
|
||
" div#notebook-container { width: 95%; }\n",
|
||
" div#menubar-container { width: 65%; }\n",
|
||
" div#maintoolbar-container { width: 99%; }\n",
|
||
"</style>\n"
|
||
],
|
||
"text/plain": [
|
||
"<IPython.core.display.HTML object>"
|
||
]
|
||
},
|
||
"metadata": {},
|
||
"output_type": "display_data"
|
||
}
|
||
],
|
||
"source": [
|
||
"from IPython.display import Image\n",
|
||
"Image(filename=\"figure.png\",width=600)\n",
|
||
"from IPython.display import display, HTML\n",
|
||
"\n",
|
||
"display(HTML(data=\"\"\"\n",
|
||
"<style>\n",
|
||
" div#notebook-container { width: 95%; }\n",
|
||
" div#menubar-container { width: 65%; }\n",
|
||
" div#maintoolbar-container { width: 99%; }\n",
|
||
"</style>\n",
|
||
"\"\"\"))"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
""
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"### Equation generation"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 5,
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"import sympy as sp\n",
|
||
"import numpy as np\n",
|
||
"from IPython.display import display\n",
|
||
"sp.init_printing(use_latex='mathjax')"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 6,
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# parameters\n",
|
||
"# Angular moment of inertia\n",
|
||
"J_B = 1e-2 * np.diag([1., 1., 1.])\n",
|
||
"\n",
|
||
"# Gravity\n",
|
||
"g_I = np.array((-1, 0., 0.))\n",
|
||
"\n",
|
||
"# Fuel consumption\n",
|
||
"alpha_m = 0.01\n",
|
||
"\n",
|
||
"# Vector from thrust point to CoM\n",
|
||
"r_T_B = np.array([-1e-2, 0., 0.])\n",
|
||
"\n",
|
||
"\n",
|
||
"def dir_cosine(q):\n",
|
||
" return np.matrix([\n",
|
||
" [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +\n",
|
||
" q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],\n",
|
||
" [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *\n",
|
||
" (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],\n",
|
||
" [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -\n",
|
||
" q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]\n",
|
||
" ])\n",
|
||
"\n",
|
||
"def omega(w):\n",
|
||
" return np.matrix([\n",
|
||
" [0, -w[0], -w[1], -w[2]],\n",
|
||
" [w[0], 0, w[2], -w[1]],\n",
|
||
" [w[1], -w[2], 0, w[0]],\n",
|
||
" [w[2], w[1], -w[0], 0],\n",
|
||
" ])\n",
|
||
"\n",
|
||
"def skew(v):\n",
|
||
" return np.matrix([\n",
|
||
" [0, -v[2], v[1]],\n",
|
||
" [v[2], 0, -v[0]],\n",
|
||
" [-v[1], v[0], 0]\n",
|
||
" ])\n",
|
||
"\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 7,
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"f = sp.zeros(14, 1)\n",
|
||
"\n",
|
||
"x = sp.Matrix(sp.symbols(\n",
|
||
" 'm rx ry rz vx vy vz q0 q1 q2 q3 wx wy wz', real=True))\n",
|
||
"u = sp.Matrix(sp.symbols('ux uy uz', real=True))\n",
|
||
"\n",
|
||
"g_I = sp.Matrix(g_I)\n",
|
||
"r_T_B = sp.Matrix(r_T_B)\n",
|
||
"J_B = sp.Matrix(J_B)\n",
|
||
"\n",
|
||
"C_B_I = dir_cosine(x[7:11, 0])\n",
|
||
"C_I_B = C_B_I.transpose()\n",
|
||
"\n",
|
||
"f[0, 0] = - alpha_m * u.norm()\n",
|
||
"f[1:4, 0] = x[4:7, 0]\n",
|
||
"f[4:7, 0] = 1 / x[0, 0] * C_I_B * u + g_I\n",
|
||
"f[7:11, 0] = 1 / 2 * omega(x[11:14, 0]) * x[7: 11, 0]\n",
|
||
"f[11:14, 0] = J_B ** -1 * \\\n",
|
||
" (skew(r_T_B) * u - skew(x[11:14, 0]) * J_B * x[11:14, 0])\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 8,
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/latex": [
|
||
"$$\\left[\\begin{matrix}- 0.01 \\sqrt{ux^{2} + uy^{2} + uz^{2}}\\\\vx\\\\vy\\\\vz\\\\\\frac{- 1.0 m - ux \\left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\\right) - 2 uy \\left(q_{0} q_{3} - q_{1} q_{2}\\right) + 2 uz \\left(q_{0} q_{2} + q_{1} q_{3}\\right)}{m}\\\\\\frac{2 ux \\left(q_{0} q_{3} + q_{1} q_{2}\\right) - uy \\left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\\right) - 2 uz \\left(q_{0} q_{1} - q_{2} q_{3}\\right)}{m}\\\\\\frac{- 2 ux \\left(q_{0} q_{2} - q_{1} q_{3}\\right) + 2 uy \\left(q_{0} q_{1} + q_{2} q_{3}\\right) - uz \\left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\\right)}{m}\\\\- 0.5 q_{1} wx - 0.5 q_{2} wy - 0.5 q_{3} wz\\\\0.5 q_{0} wx + 0.5 q_{2} wz - 0.5 q_{3} wy\\\\0.5 q_{0} wy - 0.5 q_{1} wz + 0.5 q_{3} wx\\\\0.5 q_{0} wz + 0.5 q_{1} wy - 0.5 q_{2} wx\\\\0\\\\1.0 uz\\\\- 1.0 uy\\end{matrix}\\right]$$"
|
||
],
|
||
"text/plain": [
|
||
"⎡ _________________ \n",
|
||
"⎢ ╱ 2 2 2 \n",
|
||
"⎢ -0.01⋅╲╱ ux + uy + uz \n",
|
||
"⎢ \n",
|
||
"⎢ vx \n",
|
||
"⎢ \n",
|
||
"⎢ vy \n",
|
||
"⎢ \n",
|
||
"⎢ vz \n",
|
||
"⎢ \n",
|
||
"⎢ ⎛ 2 2 ⎞ \n",
|
||
"⎢-1.0⋅m - ux⋅⎝2⋅q₂ + 2⋅q₃ - 1⎠ - 2⋅uy⋅(q₀⋅q₃ - q₁⋅q₂) + 2⋅uz⋅(q₀⋅q₂ + q₁⋅q₃)\n",
|
||
"⎢─────────────────────────────────────────────────────────────────────────────\n",
|
||
"⎢ m \n",
|
||
"⎢ \n",
|
||
"⎢ ⎛ 2 2 ⎞ \n",
|
||
"⎢ 2⋅ux⋅(q₀⋅q₃ + q₁⋅q₂) - uy⋅⎝2⋅q₁ + 2⋅q₃ - 1⎠ - 2⋅uz⋅(q₀⋅q₁ - q₂⋅q₃) \n",
|
||
"⎢ ──────────────────────────────────────────────────────────────────── \n",
|
||
"⎢ m \n",
|
||
"⎢ \n",
|
||
"⎢ ⎛ 2 2 ⎞ \n",
|
||
"⎢ -2⋅ux⋅(q₀⋅q₂ - q₁⋅q₃) + 2⋅uy⋅(q₀⋅q₁ + q₂⋅q₃) - uz⋅⎝2⋅q₁ + 2⋅q₂ - 1⎠ \n",
|
||
"⎢ ───────────────────────────────────────────────────────────────────── \n",
|
||
"⎢ m \n",
|
||
"⎢ \n",
|
||
"⎢ -0.5⋅q₁⋅wx - 0.5⋅q₂⋅wy - 0.5⋅q₃⋅wz \n",
|
||
"⎢ \n",
|
||
"⎢ 0.5⋅q₀⋅wx + 0.5⋅q₂⋅wz - 0.5⋅q₃⋅wy \n",
|
||
"⎢ \n",
|
||
"⎢ 0.5⋅q₀⋅wy - 0.5⋅q₁⋅wz + 0.5⋅q₃⋅wx \n",
|
||
"⎢ \n",
|
||
"⎢ 0.5⋅q₀⋅wz + 0.5⋅q₁⋅wy - 0.5⋅q₂⋅wx \n",
|
||
"⎢ \n",
|
||
"⎢ 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 1.0⋅uz \n",
|
||
"⎢ \n",
|
||
"⎣ -1.0⋅uy \n",
|
||
"\n",
|
||
"⎤\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎥\n",
|
||
"⎦"
|
||
]
|
||
},
|
||
"metadata": {},
|
||
"output_type": "display_data"
|
||
}
|
||
],
|
||
"source": [
|
||
"display(sp.simplify(f)) # f"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 12,
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/latex": [
|
||
"$$\\left[\\begin{array}{cccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\\\frac{ux \\left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\\right) + 2 uy \\left(q_{0} q_{3} - q_{1} q_{2}\\right) - 2 uz \\left(q_{0} q_{2} + q_{1} q_{3}\\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \\frac{2 \\left(q_{2} uz - q_{3} uy\\right)}{m} & \\frac{2 \\left(q_{2} uy + q_{3} uz\\right)}{m} & \\frac{2 \\left(q_{0} uz + q_{1} uy - 2 q_{2} ux\\right)}{m} & \\frac{2 \\left(- q_{0} uy + q_{1} uz - 2 q_{3} ux\\right)}{m} & 0 & 0 & 0\\\\\\frac{- 2 ux \\left(q_{0} q_{3} + q_{1} q_{2}\\right) + uy \\left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\\right) + 2 uz \\left(q_{0} q_{1} - q_{2} q_{3}\\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \\frac{2 \\left(- q_{1} uz + q_{3} ux\\right)}{m} & \\frac{2 \\left(- q_{0} uz - 2 q_{1} uy + q_{2} ux\\right)}{m} & \\frac{2 \\left(q_{1} ux + q_{3} uz\\right)}{m} & \\frac{2 \\left(q_{0} ux + q_{2} uz - 2 q_{3} uy\\right)}{m} & 0 & 0 & 0\\\\\\frac{2 ux \\left(q_{0} q_{2} - q_{1} q_{3}\\right) - 2 uy \\left(q_{0} q_{1} + q_{2} q_{3}\\right) + uz \\left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \\frac{2 \\left(q_{1} uy - q_{2} ux\\right)}{m} & \\frac{2 \\left(q_{0} uy - 2 q_{1} uz + q_{3} ux\\right)}{m} & \\frac{2 \\left(- q_{0} ux - 2 q_{2} uz + q_{3} uy\\right)}{m} & \\frac{2 \\left(q_{1} ux + q_{2} uy\\right)}{m} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 0.5 wx & - 0.5 wy & - 0.5 wz & - 0.5 q_{1} & - 0.5 q_{2} & - 0.5 q_{3}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wx & 0 & 0.5 wz & - 0.5 wy & 0.5 q_{0} & - 0.5 q_{3} & 0.5 q_{2}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wy & - 0.5 wz & 0 & 0.5 wx & 0.5 q_{3} & 0.5 q_{0} & - 0.5 q_{1}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wz & 0.5 wy & - 0.5 wx & 0 & - 0.5 q_{2} & 0.5 q_{1} & 0.5 q_{0}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\end{array}\\right]$$"
|
||
],
|
||
"text/plain": [
|
||
"⎡ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ ⎛ 2 2 ⎞ \n",
|
||
"⎢ux⋅⎝2⋅q₂ + 2⋅q₃ - 1⎠ + 2⋅uy⋅(q₀⋅q₃ - q₁⋅q₂) - 2⋅uz⋅(q₀⋅q₂ + q₁⋅q₃) \n",
|
||
"⎢──────────────────────────────────────────────────────────────────── 0 0 \n",
|
||
"⎢ 2 \n",
|
||
"⎢ m \n",
|
||
"⎢ \n",
|
||
"⎢ ⎛ 2 2 ⎞ \n",
|
||
"⎢-2⋅ux⋅(q₀⋅q₃ + q₁⋅q₂) + uy⋅⎝2⋅q₁ + 2⋅q₃ - 1⎠ + 2⋅uz⋅(q₀⋅q₁ - q₂⋅q₃) \n",
|
||
"⎢───────────────────────────────────────────────────────────────────── 0 0 \n",
|
||
"⎢ 2 \n",
|
||
"⎢ m \n",
|
||
"⎢ \n",
|
||
"⎢ ⎛ 2 2 ⎞ \n",
|
||
"⎢2⋅ux⋅(q₀⋅q₂ - q₁⋅q₃) - 2⋅uy⋅(q₀⋅q₁ + q₂⋅q₃) + uz⋅⎝2⋅q₁ + 2⋅q₂ - 1⎠ \n",
|
||
"⎢──────────────────────────────────────────────────────────────────── 0 0 \n",
|
||
"⎢ 2 \n",
|
||
"⎢ m \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎢ 0 0 0 \n",
|
||
"⎢ \n",
|
||
"⎣ 0 0 0 \n",
|
||
"\n",
|
||
"0 0 0 0 0 0 0 \n",
|
||
" \n",
|
||
"0 1 0 0 0 0 0 \n",
|
||
" \n",
|
||
"0 0 1 0 0 0 0 \n",
|
||
" \n",
|
||
"0 0 0 1 0 0 0 \n",
|
||
" \n",
|
||
" \n",
|
||
" 2⋅(q₂⋅uz - q₃⋅uy) 2⋅(q₂⋅uy + q₃⋅uz) 2⋅(q₀⋅uz + q₁⋅uy\n",
|
||
"0 0 0 0 ───────────────── ───────────────── ────────────────\n",
|
||
" m m m \n",
|
||
" \n",
|
||
" \n",
|
||
" \n",
|
||
" 2⋅(-q₁⋅uz + q₃⋅ux) 2⋅(-q₀⋅uz - 2⋅q₁⋅uy + q₂⋅ux) 2⋅(q₁⋅ux + \n",
|
||
"0 0 0 0 ────────────────── ──────────────────────────── ───────────\n",
|
||
" m m m \n",
|
||
" \n",
|
||
" \n",
|
||
" \n",
|
||
" 2⋅(q₁⋅uy - q₂⋅ux) 2⋅(q₀⋅uy - 2⋅q₁⋅uz + q₃⋅ux) 2⋅(-q₀⋅ux - 2⋅q₂\n",
|
||
"0 0 0 0 ───────────────── ─────────────────────────── ────────────────\n",
|
||
" m m m \n",
|
||
" \n",
|
||
" \n",
|
||
"0 0 0 0 0 -0.5⋅wx -0.5⋅w\n",
|
||
" \n",
|
||
"0 0 0 0 0.5⋅wx 0 0.5⋅w\n",
|
||
" \n",
|
||
"0 0 0 0 0.5⋅wy -0.5⋅wz 0 \n",
|
||
" \n",
|
||
"0 0 0 0 0.5⋅wz 0.5⋅wy -0.5⋅w\n",
|
||
" \n",
|
||
"0 0 0 0 0 0 0 \n",
|
||
" \n",
|
||
"0 0 0 0 0 0 0 \n",
|
||
" \n",
|
||
"0 0 0 0 0 0 0 \n",
|
||
"\n",
|
||
" 0 0 0 0 ⎤\n",
|
||
" ⎥\n",
|
||
" 0 0 0 0 ⎥\n",
|
||
" ⎥\n",
|
||
" 0 0 0 0 ⎥\n",
|
||
" ⎥\n",
|
||
" 0 0 0 0 ⎥\n",
|
||
" ⎥\n",
|
||
" ⎥\n",
|
||
" - 2⋅q₂⋅ux) 2⋅(-q₀⋅uy + q₁⋅uz - 2⋅q₃⋅ux) ⎥\n",
|
||
"─────────── ──────────────────────────── 0 0 0 ⎥\n",
|
||
" m ⎥\n",
|
||
" ⎥\n",
|
||
" ⎥\n",
|
||
" ⎥\n",
|
||
"q₃⋅uz) 2⋅(q₀⋅ux + q₂⋅uz - 2⋅q₃⋅uy) ⎥\n",
|
||
"────── ─────────────────────────── 0 0 0 ⎥\n",
|
||
" m ⎥\n",
|
||
" ⎥\n",
|
||
" ⎥\n",
|
||
" ⎥\n",
|
||
"⋅uz + q₃⋅uy) 2⋅(q₁⋅ux + q₂⋅uy) ⎥\n",
|
||
"──────────── ───────────────── 0 0 0 ⎥\n",
|
||
" m ⎥\n",
|
||
" ⎥\n",
|
||
" ⎥\n",
|
||
"y -0.5⋅wz -0.5⋅q₁ -0.5⋅q₂ -0.5⋅q₃⎥\n",
|
||
" ⎥\n",
|
||
"z -0.5⋅wy 0.5⋅q₀ -0.5⋅q₃ 0.5⋅q₂ ⎥\n",
|
||
" ⎥\n",
|
||
" 0.5⋅wx 0.5⋅q₃ 0.5⋅q₀ -0.5⋅q₁⎥\n",
|
||
" ⎥\n",
|
||
"x 0 -0.5⋅q₂ 0.5⋅q₁ 0.5⋅q₀ ⎥\n",
|
||
" ⎥\n",
|
||
" 0 0 0 0 ⎥\n",
|
||
" ⎥\n",
|
||
" 0 0 0 0 ⎥\n",
|
||
" ⎥\n",
|
||
" 0 0 0 0 ⎦"
|
||
]
|
||
},
|
||
"metadata": {},
|
||
"output_type": "display_data"
|
||
}
|
||
],
|
||
"source": [
|
||
"display(sp.simplify(f.jacobian(x)))# A "
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 10,
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/latex": [
|
||
"$$\\left[\\begin{matrix}- \\frac{0.01 ux}{\\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \\frac{0.01 uy}{\\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \\frac{0.01 uz}{\\sqrt{ux^{2} + uy^{2} + uz^{2}}}\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\\\frac{- 2 q_{2}^{2} - 2 q_{3}^{2} + 1}{m} & \\frac{2 \\left(- q_{0} q_{3} + q_{1} q_{2}\\right)}{m} & \\frac{2 \\left(q_{0} q_{2} + q_{1} q_{3}\\right)}{m}\\\\\\frac{2 \\left(q_{0} q_{3} + q_{1} q_{2}\\right)}{m} & \\frac{- 2 q_{1}^{2} - 2 q_{3}^{2} + 1}{m} & \\frac{2 \\left(- q_{0} q_{1} + q_{2} q_{3}\\right)}{m}\\\\\\frac{2 \\left(- q_{0} q_{2} + q_{1} q_{3}\\right)}{m} & \\frac{2 \\left(q_{0} q_{1} + q_{2} q_{3}\\right)}{m} & \\frac{- 2 q_{1}^{2} - 2 q_{2}^{2} + 1}{m}\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 1.0\\\\0 & -1.0 & 0\\end{matrix}\\right]$$"
|
||
],
|
||
"text/plain": [
|
||
"⎡ -0.01⋅ux -0.01⋅uy -0.01⋅uz ⎤\n",
|
||
"⎢──────────────────── ──────────────────── ────────────────────⎥\n",
|
||
"⎢ _________________ _________________ _________________⎥\n",
|
||
"⎢ ╱ 2 2 2 ╱ 2 2 2 ╱ 2 2 2 ⎥\n",
|
||
"⎢╲╱ ux + uy + uz ╲╱ ux + uy + uz ╲╱ ux + uy + uz ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 2 2 ⎥\n",
|
||
"⎢- 2⋅q₂ - 2⋅q₃ + 1 2⋅(-q₀⋅q₃ + q₁⋅q₂) 2⋅(q₀⋅q₂ + q₁⋅q₃) ⎥\n",
|
||
"⎢─────────────────── ────────────────── ───────────────── ⎥\n",
|
||
"⎢ m m m ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 2 2 ⎥\n",
|
||
"⎢ 2⋅(q₀⋅q₃ + q₁⋅q₂) - 2⋅q₁ - 2⋅q₃ + 1 2⋅(-q₀⋅q₁ + q₂⋅q₃) ⎥\n",
|
||
"⎢ ───────────────── ─────────────────── ────────────────── ⎥\n",
|
||
"⎢ m m m ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 2 2 ⎥\n",
|
||
"⎢ 2⋅(-q₀⋅q₂ + q₁⋅q₃) 2⋅(q₀⋅q₁ + q₂⋅q₃) - 2⋅q₁ - 2⋅q₂ + 1 ⎥\n",
|
||
"⎢ ────────────────── ───────────────── ─────────────────── ⎥\n",
|
||
"⎢ m m m ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎢ 0 0 1.0 ⎥\n",
|
||
"⎢ ⎥\n",
|
||
"⎣ 0 -1.0 0 ⎦"
|
||
]
|
||
},
|
||
"execution_count": 10,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"sp.simplify(f.jacobian(u)) # B"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"### Ref\n",
|
||
"\n",
|
||
"- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper\n",
|
||
"by Michael Szmuk and Behçet Açıkmeşe.\n",
|
||
"\n",
|
||
"- inspired by EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of \"Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time\" https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime\n",
|
||
"\n"
|
||
]
|
||
}
|
||
],
|
||
"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.6.8"
|
||
}
|
||
},
|
||
"nbformat": 4,
|
||
"nbformat_minor": 2
|
||
}
|