{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "### Simulation" ] }, { "cell_type": "code", "execution_count": 16, "metadata": {}, "outputs": [ { "data": { "text/html": [ "\n", "\n" ], "text/plain": [ "" ] }, "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", "\n", "\"\"\"))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "![gif](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/rocket_powered_landing/animation.gif)" ] }, { "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 }