{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "9XPaBeu3rBQH"
      },
      "source": [
        "## トラ技連載「ロボット現代制御の理論と実装」\n",
        "トラ技2022年４月号からはじまった「ロボット現代制御の理論と実装」の倒立振子のモデルをPythonの\n",
        "sympyを使って解いてみます。\n",
        "\n",
        "倒立振子の座標を、トラ技2022年４月号p160の図２より引用します。\n",
        "\n",
        "<img src=\"images/J4/Fig2.png\" width=\"400\">\n",
        "\n",
        "今月号はオイラー・ラグランジュの式(1)を使って、運動方程式から倒立振子の微分方程式を求め、それを状態方程式に変換するまでをsympyを使って求めてみます。\n",
        "$$\n",
        "\\frac{d}{dt}\\left ( \\frac{\\partial L}{\\partial \\dot{x}} \\right ) - \\frac{\\partial L}{\\partial x} = 0\n",
        "\\tag{1}\n",
        "$$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "EHFB2TDPv308"
      },
      "source": [
        "### sympyの準備\n",
        "最初にsympyを使うためのにインポートと初期化をします。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "S9AcD2Axq5Pn"
      },
      "outputs": [],
      "source": [
        "import sympy as sp\n",
        "\n",
        "sp.init_printing()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "_ZHBJEtkBCNO"
      },
      "source": [
        "次に変数を定義します。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "K0YYAfQLA6uI"
      },
      "outputs": [],
      "source": [
        "# 変数の定義\n",
        "l, r, m, M, J_b, J_w, g, t = sp.var(\"l r m M J_b J_w g t\")\n",
        "thF = sp.Function(\"th\")\n",
        "psiF = sp.Function(\"psi\")\n",
        "psi, dpsi, ddpsi, th, dth, ddth, tau = sp.var(\"psi dpsi ddpsi th dth ddth tau\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "kDPk1luBCAic"
      },
      "source": [
        "状態方程式に使用する変数をq, dq, ddq配列にセットします。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "w3me_B_-B1gV"
      },
      "outputs": [],
      "source": [
        "q = [th, psi]\n",
        "dq = [dth, dpsi]\n",
        "ddq = [ddth, ddpsi]"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "MXd1-F7pCZLr"
      },
      "source": [
        "### 運動エネルギー\n",
        "倒立振子の運動エネルギーは、車体の回転、車輪の回転、車体の並進の運動エネルギーを使って以下のように表されます。\n",
        "\n",
        "$$\n",
        "K = \\frac{1}{2}M(\\dot{x}_b^2 + \\dot{y}_b^2) + \\frac{1}{2} J_b\\dot{\\psi}^2 + \\frac{1}{2}m \\dot{x}_w^2 + \\frac{1}{2} J_w(\\dot{\\psi} + \\dot{\\theta})^2\n",
        "\\tag{2}\n",
        "$$\n",
        "\n",
        "ここで定数\n",
        "- $l$ : 車輪の中心から車体の重心までの長さ\n",
        "- $r$ : 車輪の半径\n",
        "- $J_b$ : 車体の重心周りの慣性モーメント\n",
        "- $J_w$ : 車輪の中心周りの慣性モーメント\n",
        "- $m$ : 車輪の重量\n",
        "- $M$ : 車体の重量\n",
        "\n",
        "各項\n",
        "- $\\frac{1}{2}M(\\dot{x}_b^2 + \\dot{y}_b^2)$ : 車体の並進の運動エネルギー\n",
        "- $\\frac{1}{2} J_w(\\dot{\\psi}^2)$ : 車体の重心周りの回転の運動エネルギー\n",
        "- $\\frac{1}{2}m \\dot{x}_w^2$ : 車輪の並進の運動エネルギー\n",
        "- $\\frac{1}{2} J_w(\\dot{\\psi} + \\dot{\\theta})^2$ : 車輪の中心周りの回転の運動エネルギー"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ffbuIdb7HbYY"
      },
      "source": [
        "### 位置エネルギー\n",
        "位置エネルギーUは、車輪と車体の関係から以下のように表されます。\n",
        "\n",
        "$$\n",
        "U = M g l cos(\\psi)\n",
        "\\tag{3}\n",
        "$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 38
        },
        "id": "2KLqdiJ5Hgcs",
        "outputId": "c3602a10-b47c-4e8d-d299-d06b7e67abd0"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle M g l \\cos{\\left(\\psi \\right)}$"
            ],
            "text/plain": [
              "M⋅g⋅l⋅cos(ψ)"
            ]
          },
          "execution_count": 4,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "U = M*g*l*sp.cos(psi)\n",
        "U"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "i5z-506BLAlq"
      },
      "source": [
        "### 幾何的な関係\n",
        "幾何的な関係から$x_w, x_b, y_b$は、以下のように表されます。\n",
        "$$\n",
        "x_w = r (\\theta + \\psi )\n",
        "\\tag{4}\n",
        "$$\n",
        "\n",
        "$$\n",
        "x_b = r(\\theta + \\psi) + l sin(\\psi)\n",
        "\\tag{5}\n",
        "$$\n",
        "$$\n",
        "y_b = l cos \\psi\n",
        "\\tag{6}\n",
        "$$\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 38
        },
        "id": "SuL5t3m2OLoh",
        "outputId": "9f5feb6c-8f33-453e-f8dd-8d2c23520d10"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle \\left( r \\left(\\psi{\\left(t \\right)} + \\operatorname{th}{\\left(t \\right)}\\right), \\  l \\sin{\\left(\\psi{\\left(t \\right)} \\right)} + r \\left(\\psi{\\left(t \\right)} + \\operatorname{th}{\\left(t \\right)}\\right), \\  l \\cos{\\left(\\psi{\\left(t \\right)} \\right)}\\right)$"
            ],
            "text/plain": [
              "(r⋅(ψ(t) + th(t)), l⋅sin(ψ(t)) + r⋅(ψ(t) + th(t)), l⋅cos(ψ(t)))"
            ]
          },
          "execution_count": 5,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "x_w = r*(thF(t) + psiF(t))\n",
        "x_b = r*(thF(t) + psiF(t)) + l*sp.sin(psiF(t))\n",
        "y_b = l*sp.cos(psiF(t))\n",
        "\n",
        "x_w, x_b, y_b"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 58
        },
        "id": "5L4xa7zArW1H",
        "outputId": "1cd76c28-375b-4c97-c6af-b6be52b5cadf"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle \\left( r \\left(\\frac{d}{d t} \\psi{\\left(t \\right)} + \\frac{d}{d t} \\operatorname{th}{\\left(t \\right)}\\right), \\  l \\cos{\\left(\\psi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\psi{\\left(t \\right)} + r \\left(\\frac{d}{d t} \\psi{\\left(t \\right)} + \\frac{d}{d t} \\operatorname{th}{\\left(t \\right)}\\right), \\  - l \\sin{\\left(\\psi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\psi{\\left(t \\right)}\\right)$"
            ],
            "text/plain": [
              "⎛  ⎛d          d        ⎞              d            ⎛d          d        ⎞    \n",
              "⎜r⋅⎜──(ψ(t)) + ──(th(t))⎟, l⋅cos(ψ(t))⋅──(ψ(t)) + r⋅⎜──(ψ(t)) + ──(th(t))⎟, -l\n",
              "⎝  ⎝dt         dt       ⎠              dt           ⎝dt         dt       ⎠    \n",
              "\n",
              "           d       ⎞\n",
              "⋅sin(ψ(t))⋅──(ψ(t))⎟\n",
              "           dt      ⎠"
            ]
          },
          "execution_count": 6,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# x_w, x_b, y_bを微分\n",
        "dx_w = sp.diff(x_w, t)\n",
        "dx_b = sp.diff(x_b, t)\n",
        "dy_b = sp.diff(y_b, t)\n",
        "\n",
        "dx_w, dx_b, dy_b"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "BSdDtcL8Ov36"
      },
      "source": [
        "これを使って運動エネルギー、位置エネルギーを表すと以下のようになります。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 134
        },
        "id": "hgg1GU8wO50v",
        "outputId": "5ea6c09f-69b1-49ee-b3f3-10ab41bdbbb1"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle 0.5 J_{b} dpsi^{2} + 0.5 J_{w} \\left(dpsi + dth\\right)^{2} + 0.5 M \\left(l^{2} \\sin^{2}{\\left(\\psi{\\left(t \\right)} \\right)} \\left(\\frac{d}{d t} \\psi{\\left(t \\right)}\\right)^{2} + \\left(l \\cos{\\left(\\psi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\psi{\\left(t \\right)} + r \\left(\\frac{d}{d t} \\psi{\\left(t \\right)} + \\frac{d}{d t} \\operatorname{th}{\\left(t \\right)}\\right)\\right)^{2}\\right) + 0.5 m r^{2} \\left(\\frac{d}{d t} \\psi{\\left(t \\right)} + \\frac{d}{d t} \\operatorname{th}{\\left(t \\right)}\\right)^{2}$"
            ],
            "text/plain": [
              "                                              ⎛                        2      \n",
              "            2                       2         ⎜ 2    2       ⎛d       ⎞    ⎛  \n",
              "0.5⋅J_b⋅dpsi  + 0.5⋅J_w⋅(dpsi + dth)  + 0.5⋅M⋅⎜l ⋅sin (ψ(t))⋅⎜──(ψ(t))⎟  + ⎜l⋅\n",
              "                                              ⎝              ⎝dt      ⎠    ⎝  \n",
              "\n",
              "                                              2⎞                              \n",
              "          d            ⎛d          d        ⎞⎞ ⎟          2 ⎛d          d     \n",
              "cos(ψ(t))⋅──(ψ(t)) + r⋅⎜──(ψ(t)) + ──(th(t))⎟⎟ ⎟ + 0.5⋅m⋅r ⋅⎜──(ψ(t)) + ──(th(\n",
              "          dt           ⎝dt         dt       ⎠⎠ ⎠            ⎝dt         dt    \n",
              "\n",
              "    2\n",
              "   ⎞ \n",
              "t))⎟ \n",
              "   ⎠ "
            ]
          },
          "execution_count": 7,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "K = 1/2*M*(dx_b**2 + dy_b**2) + 1/2*J_b*dpsi**2 + 1/2*m*dx_w**2 + 1/2*J_w*(dpsi + dth)**2\n",
        "K"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "I0wqdPErZ1BL"
      },
      "source": [
        "ここで、時間の関数として定義していた$\\theta(t), \\psi(t)$とその微分を変数$th, psi, dth, dpsi$に置換えます。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 70
        },
        "id": "0wNcYvBLT_Oq",
        "outputId": "8515c5d2-208a-462d-f6db-b4aac5469728"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle 0.5 J_{b} dpsi^{2} + 0.5 J_{w} \\left(dpsi + dth\\right)^{2} + 0.5 M \\left(dpsi^{2} l^{2} \\sin^{2}{\\left(\\psi \\right)} + \\left(dpsi l \\cos{\\left(\\psi \\right)} + r \\left(dpsi + dth\\right)\\right)^{2}\\right) + 0.5 m r^{2} \\left(dpsi + dth\\right)^{2}$"
            ],
            "text/plain": [
              "            2                       2         ⎛    2  2    2                  \n",
              "0.5⋅J_b⋅dpsi  + 0.5⋅J_w⋅(dpsi + dth)  + 0.5⋅M⋅⎝dpsi ⋅l ⋅sin (ψ) + (dpsi⋅l⋅cos(\n",
              "\n",
              "                    2⎞          2             2\n",
              "ψ) + r⋅(dpsi + dth)) ⎠ + 0.5⋅m⋅r ⋅(dpsi + dth) "
            ]
          },
          "execution_count": 8,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "K = K.subs({sp.diff(psiF(t), t): dpsi, sp.diff(thF(t), t): dth})\n",
        "K = K.subs({psiF(t): psi, thF: th})\n",
        "K"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Z5NMFw6jL-gz"
      },
      "source": [
        "### オイラー・ラグランジュL\n",
        "オイラー・ラグランジュの$L = K - U$に、幾何的な関係を代入すると以下のようになります。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 70
        },
        "id": "aC8ydLk4JeKe",
        "outputId": "99496e22-ff52-44ec-f50f-4894d218dd87"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle 0.5 J_{b} dpsi^{2} + 0.5 J_{w} \\left(dpsi + dth\\right)^{2} - M g l \\cos{\\left(\\psi \\right)} + 0.5 M \\left(dpsi^{2} l^{2} \\sin^{2}{\\left(\\psi \\right)} + \\left(dpsi l \\cos{\\left(\\psi \\right)} + r \\left(dpsi + dth\\right)\\right)^{2}\\right) + 0.5 m r^{2} \\left(dpsi + dth\\right)^{2}$"
            ],
            "text/plain": [
              "            2                       2                        ⎛    2  2    2   \n",
              "0.5⋅J_b⋅dpsi  + 0.5⋅J_w⋅(dpsi + dth)  - M⋅g⋅l⋅cos(ψ) + 0.5⋅M⋅⎝dpsi ⋅l ⋅sin (ψ)\n",
              "\n",
              "                                   2⎞          2             2\n",
              " + (dpsi⋅l⋅cos(ψ) + r⋅(dpsi + dth)) ⎠ + 0.5⋅m⋅r ⋅(dpsi + dth) "
            ]
          },
          "execution_count": 9,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "L = K - U\n",
        "L.simplify()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Z6TZBEB70W4p"
      },
      "source": [
        "### オイラー・ラグランジュの微分\n",
        "ここで、$\\frac{d}{dt}(\\frac{\\partial L}{\\partial \\dot{q_i}})$を求めるために、$L_{q_i} = \\frac{\\partial L}{\\partial \\dot{q}_i}$として、以下の公式を使います。\n",
        "\n",
        "$$\n",
        "\\frac{d}{dt} \\left (\\frac{\\partial L}{\\partial \\dot{q_i}} \\right ) = \\Sigma_{j = 1}^N\\left ( \\frac{\\partial L_{q_i}}{\\partial \\dot{q_i}} \\ddot{q}_j + \\frac{\\partial L_{q_i}}{\\partial q_j} \\dot{q}_j \\right)\n",
        "$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {
        "id": "Bcv4QPjfMSVu"
      },
      "outputs": [],
      "source": [
        "N = len(q)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "metadata": {
        "id": "i35I_FNhbUD8"
      },
      "outputs": [],
      "source": [
        "dLq = []\n",
        "ddLq = []\n",
        "eq = []\n",
        "for i in range(N):\n",
        "  dLq.append(sp.diff(L, dq[i]))\n",
        "  temp = 0\n",
        "  for j in range(N):\n",
        "    temp = temp + sp.diff(dLq[i], dq[j])*ddq[j] + sp.diff(dLq[i], q[j])*dq[j]\n",
        "  ddLq.append(temp)\n",
        "  eq.append(ddLq[i] - sp.diff(L, q[i]))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "metadata": {
        "id": "nSkn0rgKbXA4"
      },
      "outputs": [],
      "source": [
        "for i in range(N):\n",
        "  eq[i] = eq[i].simplify()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "aT3VQrRTblPS",
        "outputId": "bdc32d79-8090-4b28-a2eb-6d833177b960"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "-1.0*M*dpsi**2*l*r*sin(psi) + 1.0*ddpsi*(J_w + M*r*(l*cos(psi) + r) + m*r**2) + 1.0*ddth*(J_w + M*r**2 + m*r**2)\n"
          ]
        }
      ],
      "source": [
        "print(eq[0])"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 14,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Jhw9DmDWbnlS",
        "outputId": "0491d81d-da3d-438c-dae3-ac5ce4abc297"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "1.0*J_b*ddpsi + 1.0*J_w*ddpsi + 1.0*J_w*ddth + 1.0*M*ddpsi*l**2 + 2.0*M*ddpsi*l*r*cos(psi) + 1.0*M*ddpsi*r**2 + 1.0*M*ddth*l*r*cos(psi) + 1.0*M*ddth*r**2 - 1.0*M*dpsi**2*l*r*sin(psi) - 1.0*M*g*l*sin(psi) + 1.0*ddpsi*m*r**2 + 1.0*ddth*m*r**2\n"
          ]
        }
      ],
      "source": [
        "print(eq[1])"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 15,
      "metadata": {
        "id": "xP7Vl8zYb419"
      },
      "outputs": [],
      "source": [
        "add_eq = eq[0] + eq[1]\n",
        "add_eq = add_eq.simplify()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 16,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 60
        },
        "id": "FeIqGhbad9JH",
        "outputId": "664b3c69-447b-4e43-f52a-2c54f35c9b40"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/latex": [
              "$\\displaystyle 1.0 J_{b} ddpsi + 2.0 J_{w} ddpsi + 2.0 J_{w} ddth + 1.0 M ddpsi l^{2} + 3.0 M ddpsi l r + 2.0 M ddpsi r^{2} + 1.0 M ddth l r + 2.0 M ddth r^{2} - 1.0 M g l \\psi + 2.0 ddpsi m r^{2} + 2.0 ddth m r^{2}$"
            ],
            "text/plain": [
              "                                                            2                 \n",
              "1.0⋅J_b⋅ddpsi + 2.0⋅J_w⋅ddpsi + 2.0⋅J_w⋅ddth + 1.0⋅M⋅ddpsi⋅l  + 3.0⋅M⋅ddpsi⋅l⋅\n",
              "\n",
              "                 2                                2                          2\n",
              "r + 2.0⋅M⋅ddpsi⋅r  + 1.0⋅M⋅ddth⋅l⋅r + 2.0⋅M⋅ddth⋅r  - M⋅g⋅l⋅ψ + 2.0⋅ddpsi⋅m⋅r \n",
              "\n",
              "               2\n",
              " + 2.0⋅ddth⋅m⋅r "
            ]
          },
          "execution_count": 16,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "add_eq.subs({sp.cos(psi): 1, sp.sin(psi): psi, dpsi**2: 0, psi*dpsi: 0})"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "u1SBKEoOryYl"
      },
      "source": [
        "上記の結果を式で表すと以下のようになります。\n",
        "$$\n",
        "J_b \\ddot{\\psi} + 2 J_w \\ddot{\\psi} + 2 J_w \\ddot{\\theta} + M l^2 \\ddot{\\psi} + 3 M l r \\ddot{\\psi} + 2 M r^2 \\ddot{\\psi} + M l r \\ddot{\\theta} + 2 M r^2 \\ddot{\\theta} - M g l \\psi + 2 r^2 \\ddot{\\psi} + 2 m r^2 \\ddot{\\theta} \n",
        "$$\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8UeJermouWav"
      },
      "source": [
        "これを$\\ddot{\\theta}, \\ddot{\\psi}$について整理すると求める式(12)が求まります。\n",
        "\n",
        "$$\n",
        "( J_b + 2 J_w + M l^2 + 3 M l r + 2 M r^2 + 2 m r^2 )\\ddot{\\psi}\n",
        "+ (2 J_w + M l r + 2 M r^2 + 2 m r^2)\\ddot{\\theta} - M l g \\psi = 0\n",
        "\\tag{12}\n",
        "$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "rzd3_51ujFbP"
      },
      "outputs": [],
      "source": []
    }
  ],
  "metadata": {
    "colab": {
      "collapsed_sections": [],
      "name": "toragi_202205.ipynb",
      "provenance": []
    },
    "kernelspec": {
      "display_name": "Python 3",
      "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.8.8"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}