2 Deep Dive into Orientation.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$\\large{\\text{Foundations of Kinematics and Algorithms in Robotics}} \\\\ \\large{\\text{Module 2: Deep Dive into Orientation}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "### Contents\n",
    "\n",
    "[1.0 Skew Symmetric Matrices](#1)  \n",
    "[2.0 Lie Groups](#2)  \n",
    "[3.0 Angle Axis Representation](#3)  \n",
    "[4.0 The Special Orthogonal Group in Three Dimensions](#4)  \n",
    "[5.0 Three Angle Representations](#5)  \n",
    "[6.0 Quaternions](#6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "import spatialmath.base as smb\n",
    "\n",
    "import spatialgeometry as sg\n",
    "\n",
    "# The matrix exponential method\n",
    "from scipy.linalg import expm, logm\n",
    "\n",
    "# sympy provides symbolic mathematic operations \n",
    "import sympy as sym\n",
    "\n",
    "# setting up sympy to print nicely\n",
    "from IPython import display as ds\n",
    "from sympy.physics.mechanics import init_vprinting\n",
    "from sympy.physics.vector import vlatex\n",
    "init_vprinting(use_latex='mathjax')"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='1'></a>\n",
    "# 1.0 Skew Symmetric Matrices\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will start this Module with some required knowledge on skew-symmetric matrices.\n",
    "\n",
    "## Skew Symmetric Matrices\n",
    "\n",
    "In robotics, we often use skew symmetric matrices. These are a special type of matrix that have the following property\n",
    "\n",
    "$$\n",
    "    -\\mathbf{A} = \\mathbf{A}^\\top.\n",
    "$$\n",
    "\n",
    "The negative of the matrix is equal to its transpose. This property means that the diagonal elements of the matrix are all zero due to the symmetry constraint, a skew-symmetric matrix $\\in \\mathbb{R}^{3 \\times 3}$ can only contain three unique values.\n",
    "\n",
    "If we have a vector $\\mathbf{a} \\in \\mathbb{R}^3$\n",
    "\n",
    "$$\n",
    "    \\mathbf{a} = (a_0,\\ a_1,\\ a_2)^\\top.\n",
    "$$\n",
    "\n",
    "then the skew-symmetric matrix of $\\mathbf{a}$ is\n",
    "\n",
    "$$\n",
    "    [\\mathbf{a}]_\\times =\n",
    "    \\begin{pmatrix}\n",
    "        0 & -a_2 & a_1 \\\\\n",
    "        a_2 & 0 & -a_0 \\\\\n",
    "        -a_1 & a_0 & 0\n",
    "    \\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "The skew symmetric matrix is a useful tool for representing the cross product of two vectors. The cross product of two vectors $\\mathbf{a}$ and $\\mathbf{b}$ is defined as\n",
    "\n",
    "$$\n",
    "    \\mathbf{a} \\times \\mathbf{b} = [\\mathbf{a}]_\\times \\mathbf{b}\n",
    "$$\n",
    "\n",
    "where $\\mathbf{b} = (b_0,\\ b_1,\\ b_2)^\\top$.\n",
    "\n",
    "\n",
    "\n",
    "The vex operator $\\vee_\\times(\\cdot)$ is the inverse of the skew operator $[\\cdot]_\\times$. It takes a skew-symmetric matrix and returns the corresponding vector\n",
    "\n",
    "$$\n",
    "    \\vee_\\times([\\mathbf{a}]_\\times) = \\mathbf{a}.\n",
    "$$"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Augmented Skew Symmetric Matrices\n",
    "\n",
    "In robotics, we also use augmented skew-symmetric matrices, which are matrices that contain both a skew-symmetric matrix and a vector -- somewhat similar to a homogeneous transformation matrix containing a rotation matrix and a translation vector. The diagonal of the matrix are all zeroes.\n",
    "\n",
    "An augmented skew-symmetric matrix $\\in \\mathbb{R}^{4 \\times 4}$ can contain six unique values.\n",
    "\n",
    "If we have a vector $\\mathbf{a} \\in \\mathbb{R}^6$\n",
    "\n",
    "$$\n",
    "    \\mathbf{a} = (a_0,\\ a_1,\\ a_2,\\ a_3,\\ a_4,\\ a_5)^\\top.\n",
    "$$\n",
    "\n",
    "then the augmented skew-symmetric matrix of $\\mathbf{a}$ is\n",
    "\n",
    "$$\n",
    "    [\\mathbf{a}] =\n",
    "    \\begin{pmatrix}\n",
    "        0 & -a_2 & a_1 & a_2 \\\\\n",
    "        a_2 & 0 & -a_0 & a_4 \\\\\n",
    "        -a_1 & a_0 & 0 & a_5 \\\\\n",
    "        0 & 0 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "The augmented vex operator $\\vee(\\cdot)$ is the inverse of the augmented skew operator $[\\cdot]$. It takes an augmented skew-symmetric matrix and returns the corresponding vector\n",
    "\n",
    "$$\n",
    "    \\vee([\\mathbf{a}]) = \\mathbf{a}.\n",
    "$$"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Conversions\n",
    "\n",
    "<img src=\"Figures/2/skew_dark.png\" alt=\"drawing\" width=\"1000\"/>\n",
    "\n",
    "Shown on the left is a vector $\\bf{s}\\in \\mathbb{R}^3$ along with its corresponding skew symmetric matrix $\\bf{S} \\in \\bf{so}(3) \\subset \\mathbb{R}^{3 \\times 3}$. Shown on the right is a vector $\\bf{\\hat{s}}\\in \\mathbb{R}^6$ along with its corresponding augmented skew symmetric matrix $\\bf{\\hat{S}} \\in \\bf{se}(3) \\subset \\mathbb{R}^{4 \\times 4}$. The skew functions $[\\cdot]_\\times : \\mathbb{R}^3 \\mapsto \\bf{so}(3)$ maps a vector to a skew symmetric matrix, and $[\\cdot] : \\mathbb{R}^6 \\mapsto \\bf{se}(3)$ maps a vector to an augmented skew symmetric matrix. The inverse skew functions $\\vee_\\times(\\cdot) : \\bf{so}(3) \\mapsto \\mathbb{R}^3$ maps a skew symmetric matrix to a vector and $\\vee(\\cdot) : \\bf{se}(3) \\mapsto \\mathbb{R}^6$ maps an augmented skew symmetric matrix to a vector."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "Write a Python method that takes a vector $\\mathbf{a} \\in \\mathbb{R}^3$ and returns the corresponding skew-symmetric matrix.\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "Write a Python method that takes a skew symmetric matrix $\\mathbf{A} \\in \\mathbb{R}^{3 \\times 3}$ and returns the corresponding vector.\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "Is the following a valid skew-symmetric matrix?\n",
    "\n",
    "$$\n",
    "    \\mathbf{A} =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 1 & 0 \\\\\n",
    "        1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "#### Question 4\n",
    "\n",
    "Is the following a valid skew-symmetric matrix?\n",
    "\n",
    "$$\n",
    "    \\mathbf{A} =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 1 & 0 \\\\\n",
    "        -1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "#### Question 5\n",
    "\n",
    "Is the following a valid skew-symmetric matrix?\n",
    "\n",
    "$$\n",
    "    \\mathbf{A} =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 1 & 2 \\\\\n",
    "        -1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "$$\n",
    "\n",
    "#### Question 6\n",
    "\n",
    "Write a Python method that can test if a matrix is a valid skew-symmetric matrix.\n",
    "\n",
    "#### Question 7\n",
    "\n",
    "Write a Python method that can test if a matrix is a valid augmented skew-symmetric matrix.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='2'></a>\n",
    "# 2.0 Lie Groups\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we learned in Module 1, we can represent\n",
    "a 2D rotation using the Special Orthogonal Group in two dimensions $\\mathbf{R} \\in \\mathbf{SO}(2) \\subset \\mathbb{R}^{2 \\times 2}$\n",
    "and\n",
    "a 3D rotation using the Special Orthogonal Group in three dimensions $\\mathbf{R} \\in \\mathbf{SO}(3) \\subset \\mathbb{R}^{3 \\times 3}$.\n",
    "\n",
    "These groups form __manifolds__. Think of a manifold as a surface that exists in $n$-dimensional space. For example, the points on a surface of a sphere form a manifold in 3D space. In the case of $\\mathbf{SO}(2)$ matrices, they form a  manifold in the 4-dimensional space of all $2 \\ \\times \\ 2$ matrices. An $\\mathbf{SO}(3)$ is a manifold that inhabits 9-dimensional space since the matrix has 9 elements. A __Lie Group__ is a group that is also a differentiable manifold -- this means that the surface is _smooth_.\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualise a Manifold with a Sphere\n",
    "\n",
    "Since 9-dimensional space is quite hard to visualise, let's look at a 2-dimensional smooth surface existing in 3-dimensional space. The surface of the sphere is the set of all points such\n",
    "\n",
    "\\begin{align*}\n",
    "    x^2 + y^2 + z^2 &= 1\n",
    "\\end{align*}\n",
    "\n",
    "where the radius of the sphere is 1.\n",
    "\n",
    "<img src=\"Figures/2/fig1-1.png\" alt=\"drawing\" max-width=\"700\"/>\n",
    "\n",
    "This Figure displays the unit sphere. The surface of the sphere is a smooth 2-dimensional differentiable surface.\n",
    "In this case, a subset of points in 3-dimensional space that satisfy the equation $x^2 + y^2 + z^2 = 1$ form the manifold. The same principle applies to the $\\mathbf{SO}(2)$ and $\\mathbf{SO}(3)$ groups. It is in essence a subset of all possible points in the space of $2 \\times 2$ or $3 \\times 3$ matrices."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Lie Algebra and the Tangent Space\n",
    "\n",
    "At any point on the manifold we can construct tangent vectors. The set of all tangent vectors at that point form the tangent space. This is the multidimensional equivalent to a tangent line on a curve, or a tangent plane on a sphere. We can think of this as the set of all possible derivatives of the manifold at that point.\n",
    "\n",
    "The tangent space of $\\bf{SO}(3)$ lies in $\\bf{so}(3)$ which are the Lie algebras."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The tangent space at the identity is described by the Lie algebra of the group, and the basis directions of the tangent space are called the generators of the group. There are three generators of $\\bf{SO}(3)$ each belonging to $\\bf{so}(3)$.\n",
    "\n",
    "The three generators each provide a basis for the group $\\bf{SO}(3)$.\n",
    "\n",
    "The generator for the x-axis is\n",
    "\\begin{equation*}\n",
    "    G_x =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 \\\\\n",
    "        0 & 0 & -1 \\\\\n",
    "        0 & 1 & 0\n",
    "    \\end{pmatrix}.\n",
    "\\end{equation*}\n",
    "\n",
    "The generator for the y-axis is\n",
    "\\begin{equation*}\n",
    "    G_y =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 \\\\\n",
    "        -1 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "\\end{equation*}\n",
    "\n",
    "The generator for the z-axis is\n",
    "\\begin{equation*}\n",
    "    G_z =\n",
    "    \\begin{pmatrix}\n",
    "        0 & -1 & 0 \\\\\n",
    "        1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "\\end{equation*}\n",
    "\n",
    "Every point in the tangent space $\\bf{so}(3)$ can be expressed as a linear combination of basis matrices\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\Omega(\\eta_1, \\ \\eta_2, \\ \\eta_3) =\n",
    "    \\eta_1 \\ G_x +\n",
    "    \\eta_2 \\ G_y +\n",
    "    \\eta_3 \\ G_z\n",
    "\\end{equation*}\n",
    "where $\\bf{\\eta} = (\\eta_1, \\ \\eta_2, \\ \\eta_3)$ are the exponential coordinates (also known as the Euler vector)."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These generator matrices are each skew symmetric matrices. A scaled generator matrix is also skew-symmetric. The sum of generator matrices are also skew-symmetric. Lets make each of these in Python using the `skew` method provided by `spatialmath`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle G_x = \\left[\\begin{matrix}0 & 0 & 0\\\\0 & 0 & -1\\\\0 & 1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle G_y = \\left[\\begin{matrix}0 & 0 & 1\\\\0 & 0 & 0\\\\-1 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle G_z = \\left[\\begin{matrix}0 & -1 & 0\\\\1 & 0 & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Lets make the basis matrices in Python\n",
    "Gx = smb.skew([1, 0, 0])\n",
    "Gy = smb.skew([0, 1, 0])\n",
    "Gz = smb.skew([0, 0, 1])\n",
    "\n",
    "# Visualise the results\n",
    "ds.display(ds.Math(f\"G_x = {sym.latex(sym.Matrix(Gx))}\"))\n",
    "ds.display(ds.Math(f\"G_y = {sym.latex(sym.Matrix(Gy))}\"))\n",
    "ds.display(ds.Math(f\"G_z = {sym.latex(sym.Matrix(Gz))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "How many numbers are in the Euler vector?\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "What does the Euler vector represent?\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "What does the tangent space represent?\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='3'></a>\n",
    "# 3.0 Angle Axis Representation\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Together, the exponential coordinates define an axis and an angle\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{\\eta} =\n",
    "    \\hat{\\eta} \\ \\theta\n",
    "\\end{equation*}\n",
    "\n",
    "where $\\hat{\\eta} \\in \\mathbb{R}^3$ is a unit vector which defines the axis and $\\theta$ defines the angle or rotation around the axis.\n",
    "\n",
    "Therefore $\\eta$ describes four numbers with\n",
    "\\begin{equation*}\n",
    "    \\theta = \\lvert\\lvert \\eta \\rvert\\rvert\n",
    "\\end{equation*}\n",
    "and \n",
    "\\begin{equation*}\n",
    "    \\hat{\\eta} = \\dfrac{\\eta}{\\lvert\\lvert \\eta \\rvert\\rvert}\n",
    "\\end{equation*}\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Figure below shows the axis and angle representation of a rotation\n",
    "\n",
    "<img src=\"Figures/2/fig1-2.png\" alt=\"drawing\" width=\"400\"/>"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have the following relationship between the Lie algebra $\\phi$ and Lie group $\\Phi$\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\forall \\bf{X} \\in \\bf{\\phi} \\Rightarrow e^{\\bf{X}} \\in \\bf{\\Phi}\n",
    "\\end{equation*}\n",
    "\n",
    "which says that the exponential of any member of $\\bf{so}(3)$ is a valid member of $\\bf{SO}(3)$\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\forall \\bf{X} \\in \\bf{so}(3) \\Rightarrow e^{\\bf{X}} \\in \\bf{SO}(3)\n",
    "\\end{equation*}\n",
    "\n",
    "Lets put this to the test in Python.\n",
    "\n",
    "We will use the Spatialmath Package to create an $\\bf{SO}(3)$ matrix that represents a rotation around the x-axis by an amount $\\pi$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_x(\\pi) = \\left[\\begin{matrix}1.0 & 0 & 0\\\\0 & -1.0 & 0\\\\0 & 0 & -1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "R_x = smb.rotx(np.pi)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R_x(\\pi) = {sym.latex(sym.Matrix(np.round(R_x, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we will compute the rotation matrix using the theory detailed above and compute\n",
    "\n",
    "\\begin{align*}\n",
    "    R_x(\\pi) &= e^{\\pi \\ g_x}\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_x(\\pi) = \\left[\\begin{matrix}1.0 & 0 & 0\\\\0 & -1.0 & 0\\\\0 & 0 & -1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "R_x = expm(np.pi * Gx)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R_x(\\pi) = {sym.latex(sym.Matrix(np.round(R_x, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which shows that we get the same result."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "Write a Python method that creates an $\\mathbf{SO}(3)$ matrix representing a rotation around the y-axis by an angle $\\theta$ using the `expm` method.\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "What is the difference between the Euler vector and the Angle Axis representation?\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "Can we achieve any orientation using the Angle Axis representation?\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='4'></a>\n",
    "# 4.0 The Special Orthogonal Group in Three Dimensions\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can create every rotation in $\\bf{SO}(3)$ with the generators\n",
    "\n",
    "\\begin{align*}\n",
    "    \\Omega(\\bf\\eta)\n",
    "    &= \n",
    "    \\begin{pmatrix}\n",
    "        0 & -\\eta_3 & \\eta_2 \\\\\n",
    "        \\eta_3 & 0 & -\\eta_1 \\\\\n",
    "        -\\eta_2 & \\eta_1 & 0\n",
    "    \\end{pmatrix} \\\\\n",
    "    &= \n",
    "    [\\eta]_\\times\n",
    "\\end{align*}\n",
    "where $[\\cdot]_\\times : \\mathbb{R}^3 \\mapsto \\bf{so}(3)$ maps a vector to a skew symmetric matrix.\n",
    "\n",
    "_Note that where we use the $\\eta$ symbol in this notebook, the Robotics Vision and Control textbook uses the $\\omega$ symbol._\n",
    "\n",
    "We then create a rotation with\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\bf\\eta)\n",
    "    &= e^{[\\eta]_\\times} \\\\\n",
    "    &= e^{[\\hat{\\eta}]_\\times \\ \\theta} \\\\\n",
    "\\end{align*}\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Coming up with the axis and angle to define $\\eta$ can be tricky in practice as it is difficult to interpret directly. Another strategy can be to construct rotations using a combination of the generators. For example, we can construct a rotation matrix from consecutive rotations about the $x$-, $y$-, and $z$-axis\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\alpha, \\ \\beta, \\ \\gamma)\n",
    "    &= \n",
    "    e^{\\gamma \\ g_x} \\ e^{\\beta \\ g_y} \\ e^{\\alpha \\ g_z}\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R = \\left[\\begin{matrix}0 & -0.71 & 0.71\\\\0 & -0.71 & -0.71\\\\1.0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "rot_rpy = sm.SO3.Rx(np.pi/2).A @ sm.SO3.Ry(np.pi/4).A @ sm.SO3.Rz(np.pi/2).A\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R = {sym.latex(sym.Matrix(np.round(rot_rpy, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now lets try using the generators"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R = \\left[\\begin{matrix}0 & -0.71 & 0.71\\\\0 & -0.71 & -0.71\\\\1.0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "rot_rpy = expm(np.pi/2 * Gx) @ expm(np.pi/4 * Gy) @ expm(np.pi/2 * Gz)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R = {sym.latex(sym.Matrix(np.round(rot_rpy, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also go in the reverse direction and decompose a rotation into a combination of the generators using the matrix logarithm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle [\\eta]_\\times = \\left[\\begin{matrix}0 & -1.76 & -0.73\\\\1.76 & 0 & -1.76\\\\0.73 & 1.76 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "sk_eta = logm(rot_rpy)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"[\\eta]_\\\\times = {sym.latex(sym.Matrix(np.round(sk_eta, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And using the vex method we can get the Euler vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\eta = \\left[\\begin{matrix}1.76\\\\-0.73\\\\1.76\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "eta = smb.vex(sk_eta)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\eta = {sym.latex(sym.Matrix(np.round(eta, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and finally, we can decompose the Euler vector into the angle-axis representation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\hat{\\eta} = \\left[\\begin{matrix}0.68\\\\-0.28\\\\0.68\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\theta = 2.59$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "theta = np.linalg.norm(eta)\n",
    "eta_hat = eta / np.linalg.norm(eta)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\hat{{\\eta}} = {sym.latex(sym.Matrix(np.round(eta_hat, 2)))}\"))\n",
    "ds.display(ds.Math(f\"\\\\theta = {sym.latex((np.round(theta, 2)))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The $\\mathbf{SO}(3)$ Elementary Rotations\n",
    "\n",
    "In Module 1, we introduced the following elementary rotations for $\\mathbf{SO}(3)$\n",
    "\n",
    "$$\n",
    "    \\bf{R}_x(\\theta) = \n",
    "    \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left(\\theta \\right)} & - \\sin{\\left(\\theta \\right)}\\\\0 & \\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)}\\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "$$\n",
    "    \\bf{R}_y(\\theta) = \n",
    "    \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0 & \\sin{\\left(\\theta \\right)}\\\\0 & 1 & 0\\\\- \\sin{\\left(\\theta \\right)} & 0 & \\cos{\\left(\\theta \\right)}\\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "$$    \n",
    "    \\bf{R}_z(\\theta) = \n",
    "    \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & - \\sin{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "However, we did not show how these were derived. Using `sympy`, we will create these elementary rotations using symbolic expressions.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_x(θ) = \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left(θ \\right)} & - \\sin{\\left(θ \\right)}\\\\0 & \\sin{\\left(θ \\right)} & \\cos{\\left(θ \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_y(θ) = \\left[\\begin{matrix}\\cos{\\left(θ \\right)} & 0 & \\sin{\\left(θ \\right)}\\\\0 & 1 & 0\\\\- \\sin{\\left(θ \\right)} & 0 & \\cos{\\left(θ \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_z(θ) = \\left[\\begin{matrix}\\cos{\\left(θ \\right)} & - \\sin{\\left(θ \\right)} & 0\\\\\\sin{\\left(θ \\right)} & \\cos{\\left(θ \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make symbolic variables for the rotation angle\n",
    "θ = sym.symbols('θ')\n",
    "\n",
    "# Perform the symbolic matrix exponential for θ * gx_x\n",
    "# This will create the R_x(θ) matrix\n",
    "Rx = sym.Matrix(θ * gx).exp()\n",
    "\n",
    "# Perform the symbolic matrix exponential for θ * gx_y\n",
    "# This will create the R_y(θ) matrix\n",
    "Ry = sym.Matrix(θ * gy).exp()\n",
    "\n",
    "# Perform the symbolic matrix exponential for θ * gx_z\n",
    "# This will create the R_z(θ) matrix\n",
    "Rz = sym.Matrix(θ * gz).exp()\n",
    "\n",
    "ds.display(ds.Math(f\"R_x(θ) = {sym.latex(sym.simplify(Rx))}\"))\n",
    "ds.display(ds.Math(f\"R_y(θ) = {sym.latex(sym.simplify(Ry))}\"))\n",
    "ds.display(ds.Math(f\"R_z(θ) = {sym.latex(sym.simplify(Rz))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What does a Rotation Matrix Represent?\n",
    "\n",
    "If we can create a rotation matrix $\\bf{R}$ using the matrix exponential of the angle-axis representation, then what does the resulting matrix represent?\n",
    "\n",
    "The columns of $\\bf{R}$ are the new basis vectors of the rotated frame. The rows of $\\bf{R}$ are the basis vectors of the original frame relative to the new frame.\n",
    "\n",
    "This means that the first column of $\\bf{R}$ is the new $x$-axis of the rotated frame, the second column is the new $y$-axis, and the third column is the new $z$-axis."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's consider a two dimensional rotation as it's much easier to visualise.\n",
    "\n",
    "We have a rotation matrix\n",
    "\n",
    "$$\n",
    "    \\bf{R} = \n",
    "    \\begin{pmatrix}\n",
    "        0.866 & -0.5 \\\\\n",
    "        0.5 & 0.866\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "that represents a rotation of $30^\\circ$.\n",
    "\n",
    "Looking at the first column of $\\bf{R}$, we can define the first basis vector as\n",
    "\n",
    "$$\n",
    "    \\hat{\\mathbf{x}} = \n",
    "    \\begin{pmatrix}\n",
    "        0.866 \\\\\n",
    "        0.5\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "and we can define the second basis vector as\n",
    "\n",
    "$$\n",
    "    \\hat{\\mathbf{y}} = \n",
    "    \\begin{pmatrix}\n",
    "        -0.5 \\\\\n",
    "        0.866\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "We can then plot these basis vectors to see what the rotated coordinate frame looks like.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"Figures/2/fig1-3.png\" alt=\"drawing\"/>"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "What is the advantage of a rotation matrix over an Euler vector or the angle-axis representation?\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "What is the relationship between the columns of a rotation matrix and the rows of the inverse of the rotation matrix?\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "What is the relationship between the columns of a rotation matrix and the rows of the transpose of the rotation matrix?\n",
    "\n",
    "#### Question 4\n",
    "\n",
    "Use `sympy` to generate the symbolic expression for the following rotation matrix (use the Python code above as a guide):\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\alpha, \\ \\beta, \\ \\gamma)\n",
    "    &= \n",
    "    e^{\\gamma \\ g_x} \\ e^{\\beta \\ g_y} \\ e^{\\alpha \\ g_z}\n",
    "\\end{align*}\n",
    "\n",
    "#### Question 5\n",
    "\n",
    "Why are the columns of a rotation to be unit length and orthogonal?\n",
    "\n",
    "#### Question 6\n",
    "\n",
    "Why is it important for a rotation matrix to have a determinant of $+1$?\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='5'></a>\n",
    "# 5.0 Three Angle Representations\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One of Euler’s rotation theories states:\n",
    "\n",
    "\n",
    "_Any two independent orthonormal coordinate frames can be related by a sequence of rotations (not more than three) about coordinate axes, where no two successive rotations may be about the same axis._"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Euler’s rotation theorem means that the rotation between any two coordinate frames in 3D can be represented by a sequence of three rotation angles, each associated with a particular coordinate frame axis. The rotations are applied consecutively: the first rotates the world frame {0} around some axis to create a new coordinate frame {1}; then a rotation about some axis of {1} results in frame {2}; and finally a rotation about some axis of {2} results in frame {3}."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are a total of twelve unique rotation sequences. Six involve repetition, but not successive, of rotations about one particular axis:\n",
    "* XYX\n",
    "* XZX\n",
    "* YXY\n",
    "* YZY\n",
    "* ZXZ\n",
    "* ZYZ\n",
    "\n",
    "Another six are characterized by rotations about all three axes:\n",
    "* XYZ\n",
    "* XZY\n",
    "* YZX\n",
    "* YXZ\n",
    "* ZXY\n",
    "* ZYX"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Euler Angles\n",
    "\n",
    "If there are twelve possible sequences of three angle representations, then what are Euler angles?\n",
    "\n",
    "Unfortunately, Euler angles are not a unique representation of a rotation and any of the twelve sequences may be referred to as Euler angles. Therefore, when specifying that Euler angles are used, it is important to also specify the sequence of rotations. Euler angles as a whole are not very useful to us, but two of the sequences are very useful."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Roll-Pitch-Yaw Angles"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You may be thinking that roll-pitch-yaw angles help us to specify the specific sequence of three-angle representation that are being used. However, this is not the case. Roll-pitch-yaw (RPY) angles may refer to either the XYZ or ZYX sequence depending on the field and application.\n",
    "\n",
    "For ZYX RPY angles, we have\n",
    "\n",
    "$$\n",
    "    \\mathbf{\\Gamma} =\n",
    "    \\begin{pmatrix}\n",
    "        \\alpha \\\\\n",
    "        \\beta \\\\\n",
    "        \\gamma\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "and the corresponding rotation matrix is\n",
    "\n",
    "$$\n",
    "    \\bf{R}(\\mathbf{\\Gamma})\n",
    "    = \n",
    "    \\bf{R}_z(\\gamma) \\ \\bf{R}_y(\\beta) \\ \\bf{R}_x(\\alpha)\n",
    "$$\n",
    "\n",
    "and note that the order we apply the angles is the reverse order as shown in the angle vector $\\mathbf{\\Gamma}$.\n",
    "\n",
    "For XYZ RPY angles, we have the same angle vector\n",
    "\n",
    "$$\n",
    "    \\mathbf{\\Gamma} =\n",
    "    \\begin{pmatrix}\n",
    "        \\alpha \\\\\n",
    "        \\beta \\\\\n",
    "        \\gamma\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "but a different equation to get the rotation matrix\n",
    "\n",
    "$$\n",
    "    \\bf{R}(\\mathbf{\\Gamma})\n",
    "    = \n",
    "    \\bf{R}_x(\\gamma) \\ \\bf{R}_y(\\beta) \\ \\bf{R}_z(\\alpha)\n",
    "$$"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Why have so many Angle Representations?\n",
    "\n",
    "Let's first consider a real-world application for RPY angles.\n",
    "\n",
    "When describing the attitude of vehicles such as ships, aircraft and cars, the convention is that the x-axis of the body frame points in the forward direction and its z-axis points either up or down. We start with the world reference frame and in order:\n",
    "\n",
    "* rotate about the world z-axis by the yaw angle, $\\gamma$, so that the x-axis points in the direction of travel, then\n",
    "* rotate about the y-axis of the frame above by the pitch angle, $\\beta$, which sets the angle of the vehicle’s longitudinal axis relative to the horizontal plane (pitching the nose up or nose down), and then finally\n",
    "* rotate about the x-axis of the frame above by the roll angle, $\\alpha$, so that the vehicle’s body rolls about its longitudinal axis.\n",
    "\n",
    "This is a very practical and achievable way of describing the attitude of a vehicle. A pilot can read the orientation of the aircraft through an instrument or report the orientation through comprehensible three numbers. This is much more convenient than reporting the nine numbers of a rotation matrix. Additionally, we can grasp the meaning of RPY angles immediately, whereas it is not so easy to understand the meaning of a rotation matrix, angle axis, or Euler vector without a visualisation tool or a conversion to Euler angles."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So you may wonder, why don't we just use RPY angles for everything?\n",
    "\n",
    "Firstly, there are inherent limitations with RPY angles, and Euler angles. They suffer from singularities and gimbal lock which will we discuss next.\n",
    "\n",
    "Secondly, a roll-pitch-yaw angle is just that, a vector of angles. These angles do not form a group and therefore do not have a group operation. This means that we cannot add, multiple, divide or use some other operation to any two RPY angles to get a new RPY angle. When we want to compose two sets of RPY angles, we will typically first convert them to rotation matrices and then multiply them together before converting back to RPY angles."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Singularities and Gimbal Lock"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A fundamental problem with all the three-angle representations just described is singularity. This is also known as gimbal lock, a geeky term made famous in the movie Apollo 13. The term is related to mechanical gimbal systems.\n",
    "\n",
    "Singularities occur with Euler angles when axes of rotation are aligned in a certain manner."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### What is a Singularity?\n",
    "\n",
    "In mathematics, a singularity is a point in a function where the function is not defined or the mathematical object ceases to be well defined.\n",
    "\n",
    "With a matrix, a singularity is a point where the matrix is not invertible, its determinant is zero, and its rank is less than the number of rows or columns."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Rank of A: 4\n",
      "Determinant of A: 1.0\n",
      "Rank of B: 2\n",
      "Determinant of B: 0.0\n"
     ]
    }
   ],
   "source": [
    "# Make a non-singular matrix\n",
    "A = np.eye(4)\n",
    "\n",
    "# Make a singular matrix\n",
    "B = np.array([\n",
    "    [1, 2, 3, 4],\n",
    "    [1, 4, 3, 8],\n",
    "    [1, 6, 3, 12],\n",
    "    [1, 8, 3, 16]\n",
    "])\n",
    "\n",
    "# Check if A is singular\n",
    "print(f\"Rank of A: {np.linalg.matrix_rank(A)}\")\n",
    "print(f\"Determinant of A: {np.linalg.det(A)}\")\n",
    "\n",
    "# Check if B is singular\n",
    "print(f\"Rank of B: {np.linalg.matrix_rank(B)}\")\n",
    "print(f\"Determinant of B: {np.linalg.det(B)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you recall the intuitive meaning of the determinant, it is a scalar that tells us how much the volume of a parallelepiped is scaled by when we apply a linear transformation to it. If the determinant is zero, then the volume of the parallelepiped is transformed to zero and therefore the transformation is not invertible. The singularities of Euler angles are of a different nature, where the function we have defined breaks down and fails to be well defined."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Making a Singularity with Euler Angles\n",
    "\n",
    "Let's consider the ZYZ sequence Euler angles. If we have the following angles\n",
    "\n",
    "$$\n",
    "    \\mathbf{\\Gamma}\n",
    "    =\n",
    "    \\begin{pmatrix}\n",
    "        30^\\circ \\\\\n",
    "        0 \\\\\n",
    "        30^\\circ\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "that will give us the following rotation matrix\n",
    "\n",
    "$$\n",
    "    \\bf{R}(\\mathbf{\\Gamma})\n",
    "    =\n",
    "    \\mathbf{R}_z(30^\\circ) \\ \\mathbf{R}_y(0) \\ \\mathbf{R}_z(30^\\circ)\n",
    "$$\n",
    "\n",
    "a rotation of $\\mathbf{R}_y(0)$ will give the identity matrix. Therefore, the rotation becomes\n",
    "\n",
    "$$\n",
    "    \\bf{R}(\\mathbf{\\Gamma})\n",
    "    =\n",
    "    \\mathbf{R}_z(30^\\circ) \\ \\mathbf{R}_z(30^\\circ)\n",
    "$$\n",
    "\n",
    "which is equivalent to a rotation of $\\mathbf{R}_z(60^\\circ)$. Therefore, no matter what the value of our first or third angle is, the rotation is always going to be around the z-axis. When the second angle is zero, this three-angle representation breaks down and we have a singularity."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Making a Singularity with RPY Angles\n",
    "\n",
    "Of course, this can occur with any of the three-angle representations. For example, if we have ZYX RPY angles\n",
    "\n",
    "$$\n",
    "    \\mathbf{\\Gamma}\n",
    "    =\n",
    "    \\begin{pmatrix}\n",
    "        \\alpha \\\\\n",
    "        \\beta \\\\\n",
    "        \\gamma\n",
    "    \\end{pmatrix}\n",
    "    =\n",
    "    \\begin{pmatrix}\n",
    "        30^\\circ \\\\\n",
    "        90^\\circ \\\\\n",
    "        60^\\circ\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "then the rotation matrix is\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\mathbf{\\Gamma})\n",
    "    &=\n",
    "    \\mathbf{R}_z(\\gamma) \\ \\mathbf{R}_y(\\beta) \\ \\mathbf{R}_x(\\alpha) \\\\\n",
    "    &=\n",
    "    \\mathbf{R}_z(60^\\circ) \\ \\mathbf{R}_y(90^\\circ) \\ \\mathbf{R}_x(30^\\circ)\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, it's best to follow this using a physical coordinate frame or use your right hand to visualise the rotation.\n",
    "\n",
    "* Our first rotation is about the z-axis -- this is your thumb. The amount of this rotation does not matter for the singularity to occur.\n",
    "* If we rotate about the y-axis by $90^\\circ$ -- this is your middle finger -- then your index finger will be pointing down (in the opposite direction your thumb was pointing before the rotation).\n",
    "\n",
    "What's the issue? \n",
    "\n",
    "Our last rotation is about the x-axis -- this is your index finger and this is now on the same axis as the z-axis in our first rotation. Therefore, no matter what the value of our first or third angle is, the rotation is always going to be around the original z-axis. Note that the same issue occurs when the second angle is $-90^\\circ$."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### When to use Euler Angles in Practice?\n",
    "\n",
    "In robotics, we use RPY angles less often unless we need to read or communicate orientations between humans. We do need to do a great deal of computations transforming orientations between various coordinate frames. For this use-case, Euler angles are inconvenient and inefficient. When orientations need to be read of be intuited, we can visualise the orientation using software, or simply convert to RPY angles to be read.\n",
    "\n",
    "However, there are some applications where Euler angles are useful such as in aerospace where orientations need to read and communicated between humans. In these scenarios, it is crucial to choose an angle sequence where the singularity is impossible or improbable to occur. With our ZYX RPY angles, the singularity occurs with a pitch angle of $\\pm 90^\\circ$. This is not a very likely scenario for plane, boat or car as they would have to be facing straight up or straight down."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test Your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "What is the difference between the Euler vector and Euler angles?\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "What do Euler angles refer to?\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "What is the difference between Euler angles and RPY angles?\n",
    "\n",
    "#### Question 4\n",
    "\n",
    "At what angle does the singularity occur for the YZY Euler angles?\n",
    "\n",
    "#### Question 5\n",
    "\n",
    "Where does the ZYX RPY angles be a good choice of angle representation for a rocket going into space, where the z-axis is aligned with the rocket's direction of travel?\n",
    "\n",
    "#### Question 6\n",
    "\n",
    "Write a Python method that returns a rotation matrix given a set of XYZ Euler angles. Construct the rotation matrix using the `expm` method.\n",
    "\n",
    "#### Question 7\n",
    "\n",
    "Can you convert between Euler angles and Euler vectors?\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='6'></a>\n",
    "# 6.0 Quaternions\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Quaternions are widely used today for robotics, computer vision, computer graphics and aerospace navigation systems. The quaternion was invented nearly 200 years ago as an extension of the complex number – a hypercomplex number – with a real part and three complex parts\n",
    "\n",
    "$$\n",
    "    \\check{q} = s + v_x i + v_y j + v_z k \\in \\mathbb{H}\n",
    "$$\n",
    "\n",
    "where the orthogonal complex numbers $i$, $j$ and $k$ are defined such that\n",
    "\n",
    "$$\n",
    "    i^2 = j^2 = k^2 = ijk = -1.\n",
    "$$\n",
    "\n",
    "_The set of $\\mathbb{H}$ (Hamilton) represents the set of all hypercomplex numbers that form a quaternion._\n",
    "\n",
    "It is more useful to think of a quaternion as a scalar value and a vector $\\check{q} = (s, \\mathbf{v})$ or in component form\n",
    "\n",
    "$$\n",
    "    \\check{q} = s \\langle v_x, \\ v_y, \\ v_z \\rangle\n",
    "$$\n",
    "\n",
    "The magnitude of a quaternion is\n",
    "\n",
    "\\begin{align*}\n",
    "    \\lvert\\lvert \\check{q} \\rvert\\rvert \n",
    "    &= \\sqrt{\\check{q} \\cdot \\check{q}} \\\\\n",
    "    &= \\sqrt{s^2 + v_x^2 + v_y^2 + v_z^2}.\n",
    "\\end{align*}\n",
    "\n",
    "To represent rotations, we use unit quaternions or versors, quaternions with unit magnitude $\\lvert\\lvert \\check{q} \\rvert\\rvert = 1$ and denoted by $\\mathring{q}$. They can be considered as a rotation of $\\theta$ about the unit vector $\\hat{\\mathbf{v}}$ which are related to the components of the unit quaternion by\n",
    "\n",
    "\\begin{align*}\n",
    "    \\mathring{q}\n",
    "    &=\n",
    "    \\cos\n",
    "    \\left(\n",
    "        \\frac{\\theta}{2}\n",
    "    \\right)\n",
    "    \\left\\langle\n",
    "        \\hat{\\mathbf{v}}\n",
    "        \\sin\n",
    "        \\left(\n",
    "            \\frac{\\theta}{2}\n",
    "        \\right)\n",
    "    \\right\\rangle\n",
    "    \\in \\mathbf{S}^3\n",
    "    \\subset \\mathbb{H}\n",
    "\\end{align*}\n",
    "\n",
    "_The set $\\mathbf{S}^3$ is the 3-sphere and represents angles in 4D space._\n",
    "\n",
    "where $\\hat{\\mathbf{v}}$ is a unit vector that describes the axis of rotation and $\\theta$ is the angle of rotation. These are the same four numbers that we used to describe the angle-axis representation of a rotation from earlier\n",
    "\n",
    "$$\n",
    "    \\hat{\\mathbf{\\eta}} \\theta = \\hat{\\mathbf{v}} \\theta\n",
    "$$\n",
    "\n",
    "Therefore, given an angle-axis representation of a rotation $\\hat{\\mathbf{\\eta}} \\theta$, we can convert it to a unit quaternion by\n",
    "\n",
    "\\begin{align*}\n",
    "    \\mathring{q}\n",
    "    &=\n",
    "    \\cos\n",
    "    \\left(\n",
    "        \\frac{\\theta}{2}\n",
    "    \\right)\n",
    "    \\left\\langle\n",
    "        \\hat{\\mathbf{\\eta}}\n",
    "        \\sin\n",
    "        \\left(\n",
    "            \\frac{\\theta}{2}\n",
    "        \\right)\n",
    "    \\right\\rangle\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is much better to think of unit quaternions or versors (i.e. the quaternions that represent rotations) as a slight variation to the angle-axis representation rather than as a hyper-complex number in 4D space. Similar to $\\mathbf{SO}(3)$, unit quaternions also form a Lie group."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "__Note: All Unit Quaternions are valid Quaternions and are also valid 3D rotations. However, not all Quaternions are Unit Quaternions or 3D Rotations.__\n",
    "\n",
    "Quaternions represent all four dimensional hyper-complex numbers and have many use cases mathematics not limited to representing orientation. However, and confusingly, the term quaternion and unit quaternion are often used interchangeably in the context of describing a rotation."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Composition of Quaternions\n",
    "\n",
    "The composition of two unit quaternions $\\mathring{q}_1$ and $\\mathring{q}_2$ is given by the Hamilton product or quaternion product\n",
    "\n",
    "$$\n",
    "    \\mathring{q}_1 \\circ \\mathring{q}_2 =\n",
    "    s_1 s_2 -\n",
    "    \\mathbf{v}_1 \\cdot \\mathbf{v}_2\n",
    "    \\langle\n",
    "        s_1 \\mathbf{v}_2 + s_2 \\mathbf{v}_1 +\n",
    "        \\mathbf{v}_1 \\times \\mathbf{v}_2\n",
    "    \\rangle\n",
    "$$\n",
    "\n",
    "We can also write this as a matrix multiplication\n",
    "\n",
    "$$\n",
    "    \\mathring{q}_1 \\circ \\mathring{q}_2 =\n",
    "    \\begin{pmatrix}\n",
    "        s_1 & -v_{1_x} & -v_{1_y} & -v_{1_z} \\\\\n",
    "        v_{1_x} & s_1 & v_{1_z} & -v_{1_y} \\\\\n",
    "        v_{1_y} & v_{1_z} & s_1 & v_{1_x} \\\\\n",
    "        v_{1_z} & v_{1_y} & -v_{1_x} & s_1\n",
    "    \\end{pmatrix}\n",
    "    \\begin{pmatrix}\n",
    "        s_2 \\\\\n",
    "        v_{2_x} \\\\\n",
    "        v_{2_y} \\\\\n",
    "        v_{2_z}\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the quaternion multiplication is not commutative, i.e. $\\mathring{q}_1 \\circ \\mathring{q}_2 \\neq \\mathring{q}_2 \\circ \\mathring{q}_1$. The quaternions $\\mathring{q}_1$ and $\\mathring{q}_2$ must follow our existing rules for composing rotations with the sub and superscript rules, for example if\n",
    "\n",
    "$$\n",
    "    \\mathring{q}_1 = {^a \\mathring{q}_b} \\quad \\text{and} \\quad \\mathring{q}_2 = {^b \\mathring{q}_c}\n",
    "$$\n",
    "\n",
    "then \n",
    "\n",
    "$$\n",
    "    \\mathring{q}_1 \\circ \\mathring{q}_2 = {^a \\mathring{q}_c}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\eta_1 = \\left[\\begin{matrix}1.57\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\eta_2 = \\left[\\begin{matrix}0\\\\1.57\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_1 = \\left[\\begin{matrix}1.0 & 0 & 0\\\\0 & 0 & -1.0\\\\0 & 1.0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_2 = \\left[\\begin{matrix}0 & 0 & 1.0\\\\0 & 1.0 & 0\\\\-1.0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q_1 = \\left[\\begin{matrix}0.71\\\\0.71\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q_2 = \\left[\\begin{matrix}0.71\\\\0\\\\0.71\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Let's define two angle axis rotations\n",
    "# The first is a rotation of 90 degrees about the x-axis\n",
    "# The second is a rotation of 90 degrees about the y-axis\n",
    "nhat1 = np.array([1, 0, 0])\n",
    "nhat2 = np.array([0, 1, 0])\n",
    "theta1 = np.pi/2\n",
    "theta2 = np.pi/2\n",
    "\n",
    "# Now convert this to the Euler vector (also the exponential coordinates)\n",
    "n1 = theta1 * nhat1\n",
    "n2 = theta2 * nhat2\n",
    "\n",
    "ds.display(ds.Math(f\"\\\\eta_1 = {sym.latex(sym.Matrix(np.round(n1, 2)))}\"))\n",
    "ds.display(ds.Math(f\"\\\\eta_2 = {sym.latex(sym.Matrix(np.round(n2, 2)))}\"))\n",
    "\n",
    "# Now convert this to an SO3 rotation matrix\n",
    "R1 = expm(smb.skew(n1))\n",
    "R2 = expm(smb.skew(n2))\n",
    "\n",
    "ds.display(ds.Math(f\"R_1 = {sym.latex(sym.Matrix(np.round(R1, 2)))}\"))\n",
    "ds.display(ds.Math(f\"R_2 = {sym.latex(sym.Matrix(np.round(R2, 2)))}\"))\n",
    "\n",
    "# Now let's convert the angle axis to a quaternion\n",
    "q1 = np.array([\n",
    "    np.cos(theta1/2),\n",
    "    np.sin(theta1/2) * nhat1[0],\n",
    "    np.sin(theta1/2) * nhat1[1],\n",
    "    np.sin(theta1/2) * nhat1[2]\n",
    "])\n",
    "\n",
    "q2 = np.array([\n",
    "    np.cos(theta2/2),\n",
    "    np.sin(theta2/2) * nhat2[0],\n",
    "    np.sin(theta2/2) * nhat2[1],\n",
    "    np.sin(theta2/2) * nhat2[2]\n",
    "])\n",
    "\n",
    "ds.display(ds.Math(f\"q_1 = {sym.latex(sym.Matrix(np.round(q1, 2)))}\"))\n",
    "ds.display(ds.Math(f\"q_2 = {sym.latex(sym.Matrix(np.round(q2, 2)))}\"))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_1 R_2 = \\left[\\begin{matrix}0 & 0 & 1.0\\\\1.0 & 0 & 0\\\\0 & 1.0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q_1 q_2 = \\left[\\begin{matrix}0.5\\\\0.5\\\\0.5\\\\0.5\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Lets compose these two matrices\n",
    "R3 = R1 @ R2\n",
    "\n",
    "ds.display(ds.Math(f\"R_1 R_2 = {sym.latex(sym.Matrix(np.round(R3)))}\"))\n",
    "\n",
    "# Now lets compose these two quaternions using the Hamilton product\n",
    "\n",
    "q3 = np.array([\n",
    "    q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3],\n",
    "    q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2],\n",
    "    q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1],\n",
    "    q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]\n",
    "])\n",
    "\n",
    "ds.display(ds.Math(f\"q_1 q_2 = {sym.latex(sym.Matrix(np.round(q3, 2)))}\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\eta_3 \\ \\text{(from quat)} = \\left[\\begin{matrix}1.21\\\\1.21\\\\1.21\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\eta_3 \\ \\text{(from rot)} = \\left[\\begin{matrix}1.21\\\\1.21\\\\1.21\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Now lets decompose this quaternion back into an angle axis representation\n",
    "\n",
    "# First let's get the angle\n",
    "theta3 = 2 * np.arccos(q3[0])\n",
    "\n",
    "# Now let's get the axis\n",
    "nhat3 = q3[1:] / np.sin(theta3/2)\n",
    "\n",
    "# and then the Euler vector\n",
    "n3 = theta3 * nhat3\n",
    "\n",
    "ds.display(ds.Math(f\"\\\\eta_3 \\ \\\\text{{(from quat)}} = {sym.latex(sym.Matrix(np.round(n3, 2)))}\"))\n",
    "\n",
    "# Now let's decompose the rotation matrix back to the Euler vector\n",
    "n3_from_R = smb.vex(logm(R3))\n",
    "\n",
    "ds.display(ds.Math(f\"\\\\eta_3 \\ \\\\text{{(from rot)}} = {sym.latex(sym.Matrix(np.round(n3_from_R, 2)))}\"))\n",
    "\n",
    "# Which shows that we get the same result with both methods"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Null Rotation\n",
    "\n",
    "The null rotation or identity for the unit quaternion is given by\n",
    "\n",
    "$$\n",
    "    \\mathring{q}_0 = 1 \\langle 0, \\ 0, \\ 0 \\rangle\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q = \\left[\\begin{matrix}1.0\\\\0\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\theta = 0$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\hat{\\eta} = \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R = \\left[\\begin{matrix}1.0 & 0 & 0\\\\0 & 1.0 & 0\\\\0 & 0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Let's make this quaternion using the UnitQuaternion class of spatialmath\n",
    "q = sm.UnitQuaternion([1.0, 0.0, 0.0, 0.0])\n",
    "\n",
    "ds.display(ds.Math(f\"q = {sym.latex(sym.Matrix(np.round(q.A, 2)))}\"))\n",
    "\n",
    "theta, nhat = q.angvec()\n",
    "\n",
    "ds.display(ds.Math(f\"\\\\theta = {theta}\"))\n",
    "ds.display(ds.Math(f\"\\hat{{\\\\eta}} = {sym.latex(sym.Matrix(np.round(nhat, 2)))}\"))\n",
    "\n",
    "# Now let's convert this to a rotation matrix\n",
    "R = expm(smb.skew(nhat * theta))\n",
    "\n",
    "ds.display(ds.Math(f\"R = {sym.latex(sym.Matrix(np.round(R, 2)))}\"))\n",
    "\n",
    "# Which shows that the quaterion [1, 0, 0, 0] is the identity rotation or null rotation"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Inverse Rotation\n",
    "\n",
    "We can take the conjugate of a quaternion $\\check{q}$\n",
    "\n",
    "$$\n",
    "    \\check{q}^* = s \\langle -v_x, \\ -v_y, \\ -v_z \\rangle \\in \\mathbb{H}\n",
    "$$\n",
    "\n",
    "with $\\check{q}^*$ providing the inverse rotation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q = \\left[\\begin{matrix}0.71\\\\0.71\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q^-1 = \\left[\\begin{matrix}0.71\\\\-0.71\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle q \\circ q^{-1} = \\left[\\begin{matrix}1.0\\\\0\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a Unit Quaternion\n",
    "# This time we will use the UnitQuaternion class from spatialmath\n",
    "q = sm.UnitQuaternion([1.0, 1.0, 0.0, 0.0])\n",
    "\n",
    "# Now let's invert this quaternion\n",
    "# We did this the mannual way, we could have also just done q.inv()\n",
    "q_inv = sm.UnitQuaternion([q.A[0], -q.A[1], -q.A[2], -q.A[3]])\n",
    "\n",
    "# Now let's multiply these two quaternions using the Hamilton product (spatialmath provides this for us)\n",
    "q3 = q * q_inv\n",
    "\n",
    "\n",
    "ds.display(ds.Math(f\"q = {sym.latex(sym.Matrix(np.round(q.A, 2)))}\"))\n",
    "ds.display(ds.Math(f\"q^{-1} = {sym.latex(sym.Matrix(np.round(q_inv, 2)))}\"))\n",
    "ds.display(ds.Math(f\"q \\\\circ q^{{-1}} = {sym.latex(sym.Matrix(np.round(q3.A, 2)))}\"))\n",
    "\n",
    "# Which shows that we get the identity quaternion, or null rotation, satisfying the inverse rule"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The Double Mapping\n",
    "\n",
    "An interesting property of quaternions is that they represent a _double covering_ of the $\\mathbf{SO}(3)$ group. This means that there are two unit quaternions to represent each rotation $\\in \\mathbf{SO}(3)$. We have the relationship that $\\mathring{q}$ and $-\\mathring{q}$ represents the same rotation."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Using Quaternions\n",
    "\n",
    "Unit quaternions represent yet another way to represent rotations in 3D space.\n",
    "\n",
    "Compared to other angle representations we have looked at, quaternions have some advantages and disadvantages\n",
    "\n",
    "* _Angle Axis_ -- Compared to the angle-axis representation, quaternions form a group closed under the Hamilton product. This means that we can compose rotations using the Hamilton product, while to compose rotations using the angle-axis representation we must first convert the angle-axis representation to a rotation matrix and then multiply the resulting rotation matrices.\n",
    "* $\\mathbf{SO}(3)$ -- Compared to the rotation matrix representation, quaternions use only four numbers instead of nine making them more compact and memory efficient. This can be useful when working on low memory or embedded devices. \n",
    "    * The Hamilton product uses 16 multiplications and 12 additions\n",
    "    * Composition of $\\mathbf{SO}(3)$ matrices use 27 multiplications and 18 additions\n",
    "    * Although note that to compute the hamilton product as a matrix multiplication (computers are highly optimised for matrix multiplication) requires the conversion of the first quaternion to a $4 \\times 4$ matrix, which will incur extra operations.\n",
    "* _Euler Angles_ -- Compared to Euler angles, quaternions form a group and are free from the gimbal lock issue.\n",
    "\n",
    "When implementing quaternions on a computer, they are typically represented as an array with four values. For example, `spatialmath` implements quaternions as a `NumPy` array with four values $s, \\ v_x, \\ v_y, \\ v_z$. Note that other software packages -- notable ROS -- represent quaternions with different ordering of the values, for example $v_x, \\ v_y, \\ v_z, \\ s$. There are also many naming conventions, ROS uses `x, y, z, w` where `w` is the scalar part of the quaternion and `x, y, z` are the vector or imaginary part of the quaternion.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test Your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "What is the difference between the Euler vector and a Unit Quaternion?\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "What is the difference between a Quaternion and a Versor?\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "What is the difference between a Quaternion and a Unit Quaternion?\n",
    "\n",
    "#### Question 4\n",
    "\n",
    "Show that $\\mathring{q}$ and $-\\mathring{q}$ both result in the same rotation matrix.\n",
    "\n",
    "#### Question 5\n",
    "\n",
    "Investigate the computational cost of the Hamilton product for quaternions, the matrix multiplication representation of the Hamilton product, and the composition of $\\mathbf{SO}(3)$ matrices. Which is the most efficient? (Hint: the `timeit` or `time` module in Python may be useful)\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "egh432",
   "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.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}