mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 15:48:13 -05:00
add notebook
This commit is contained in:
Atsushi Sakai
@@ -0,0 +1,451 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Equation generation"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from IPython.display import Image\n",
|
||||
"#Image(filename=\"hoge.png\",width=600)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"#"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 32,
|
||||
"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": 43,
|
||||
"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": 38,
|
||||
"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": 39,
|
||||
"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": 40,
|
||||
"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 ⎦"
|
||||
]
|
||||
},
|
||||
"execution_count": 40,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"sp.simplify(f.jacobian(x)) # A "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 42,
|
||||
"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": 42,
|
||||
"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.7"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
Reference in New Issue
Block a user