feat: add quaternion calculations notebook for rotation transformations

Author: marc-hanheide

src/cmp3103m_ros2_code_fragments/cmp3103m_ros2_code_fragments/ipynb/quaternion.ipynb

@@ -0,0 +1,185 @@
+{
+  "nbformat": 4,
+  "nbformat_minor": 0,
+  "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.2"
+    },
+    "colab": {
+      "name": "quaternion.ipynb",
+      "provenance": []
+    }
+  },
+  "cells": [
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "gcpyFv6whLyc"
+      },
+      "source": [
+        "from math import sin, cos, atan2, pi\n",
+        "from scipy.spatial.transform import Rotation as R"
+      ],
+      "execution_count": null,
+      "outputs": []
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "ebPe4SwqhLyg"
+      },
+      "source": [
+        "# some angle:\n",
+        "\n",
+        "angle = 30 * pi / 180"
+      ],
+      "execution_count": 52,
+      "outputs": []
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "ExPk-YxRhLyk"
+      },
+      "source": [
+        "# definition of a quaternion rotation around the z axis alone\n",
+        "q = {\n",
+        "    'x': 0.0,\n",
+        "    'y': 0.0,\n",
+        "    'z': sin(angle/2),\n",
+        "    'w': cos(angle/2)\n",
+        "}"
+      ],
+      "execution_count": 53,
+      "outputs": []
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "MqACWNyahLym",
+        "outputId": "6a9d7e41-c10d-4b79-881f-049b71555e9f",
+        "colab": {
+          "base_uri": "https://localhost:8080/",
+          "height": 34
+        }
+      },
+      "source": [
+        "# show it\n",
+        "print('quaternion is %s' % q)"
+      ],
+      "execution_count": 54,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "text": [
+            "quaternion is {'x': 0.0, 'y': 0.0, 'z': 0.25881904510252074, 'w': 0.9659258262890683}\n"
+          ],
+          "name": "stdout"
+        }
+      ]
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "pbvMLBpihLyq",
+        "outputId": "16b869fe-f8fc-4c2f-a4fc-2802645983bb",
+        "colab": {
+          "base_uri": "https://localhost:8080/",
+          "height": 34
+        }
+      },
+      "source": [
+        "recomputeangle = atan2(q['z'], q['w'])*2\n",
+        "print('yaw angle extracted from quaternion is %s' % (recomputeangle * 180 / pi))"
+      ],
+      "execution_count": 55,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "text": [
+            "yaw angle extracted from quaternion is 29.999999999999996\n"
+          ],
+          "name": "stdout"
+        }
+      ]
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "fRzioAQvhLyt",
+        "outputId": "f61521de-16a4-4595-9673-db042a97857f",
+        "colab": {
+          "base_uri": "https://localhost:8080/",
+          "height": 68
+        }
+      },
+      "source": [
+        "r = R.from_quat([q['x'], q['y'], q['z'], q['w']])\n",
+        "print(r.as_matrix())"
+      ],
+      "execution_count": 56,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "text": [
+            "[[ 0.8660254 -0.5        0.       ]\n",
+            " [ 0.5        0.8660254  0.       ]\n",
+            " [ 0.         0.         1.       ]]\n"
+          ],
+          "name": "stdout"
+        }
+      ]
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "nQKvK6tPhdAw",
+        "outputId": "ea3e71db-2248-41ee-e583-0ea9cb6a91a0",
+        "colab": {
+          "base_uri": "https://localhost:8080/",
+          "height": 51
+        }
+      },
+      "source": [
+        "print(r.as_euler('zyx', degrees=True))\n",
+        "print(r.as_rotvec())"
+      ],
+      "execution_count": 57,
+      "outputs": [
+        {
+          "output_type": "stream",
+          "text": [
+            "[30.  0.  0.]\n",
+            "[0.         0.         0.52359878]\n"
+          ],
+          "name": "stdout"
+        }
+      ]
+    },
+    {
+      "cell_type": "code",
+      "metadata": {
+        "id": "9AlK5IbjhvqE"
+      },
+      "source": [
+        ""
+      ],
+      "execution_count": null,
+      "outputs": []
+    }
+  ]
+}