1 Spatial Mathematics.ipynb

{
   "cells": [
      {
         "cell_type": "code",
         "execution_count": 1,
         "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": [
            "\n",
            "$\\large{\\text{Foundations of Kinematics and Algorithms in Robotics}} \\\\ \\large{\\text{Module 1: Spatial Mathematics}}$\n",
            "\n",
            "$\\text{By Jesse Haviland and Peter Corke}$\n",
            "\n",
            "<br>\n",
            "\n",
            "### Contents\n",
            "\n",
            "[1.0 What is Position?](#1)  \n",
            "[2.0 What is Orientation?](#2)  \n",
            "[3.0 Homogeneous Coordinates](#3)  \n",
            "[4.0 Pose Graphs](#4)  \n",
            "[5.0 Summary of Representations](#5)  "
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 2,
         "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",
            "\n",
            "import spatialgeometry as sg"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Why do we care about Spatial Mathematics?\n",
            "\n",
            "Knowing the position and orientation -- also referred to as the pose -- of a robot or key items in its environment is a very common requirement. \n",
            "\n",
            "For example:\n",
            "\n",
            "* An autonomous vehicle needs to know its 2D position on a map and its heading\n",
            "* A robot arm needs to know the position of its end-effector and the position of the manipulation target\n",
            "* An autonomous drone needs to know its 3D position and orientation\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<br>\n",
            "\n",
            "<a id='1'></a>\n",
            "# 1.0 What is Position?\n",
            "---\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "From high-school maths you will recall that the position of a point can be represented by a set of coordinates. For a 2-dimensional space\n",
            "\n",
            "<img src=\"Figures/1/fig1-1.png\" alt=\"drawing\" max-width=\"1000\"/>\n",
            "\n",
            "we can represent the position of the grey point by its $x$ and $y$ coordinates which we would write as $(1,\\ 2)$.\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "\n",
            "For a point in 3 dimensional space\n",
            "\n",
            "<img src=\"Figures/1/fig1-2.png\" alt=\"drawing\" max-width=\"5000\"/>\n",
            "\n",
            "the position of the point is described its $x$, $y$, and $z$ coordinates which we would write as $(3,\\ 2,\\ 4)$.\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Coordinate Frames\n",
            "\n",
            "There is no absolute way to describe position since everything in the universe is moving.  The best we can do is describe where something is with respect to some agreed local coordinate frame.\n",
            "\n",
            "Going back to our 2D point example above, we said that coordinates of the point were $(1,\\ 2)$, and we said that because those are the coordinates of the point relative to the origin of the coordinate frame defined by the $x$- and $y$-axis of the plot.\n",
            "\n",
            "In the figure below, we have plotted the original point $(1,\\ 2)$ and we have also explicitly plotted the coordinate frame $\\{0\\}$.  We will do this a lot in robotics.\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<img src=\"Figures/1/fig1-1b.png\" alt=\"drawing\" max-width=\"700\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "A coordinate frame is a set of orthogonal axes.  The __origin__ of a coordinate frame is the __point__ where the axes intersect.\n",
            "\n",
            "\n",
            "_Orthogonality is the generalisation of the geometric notion of perpendicularity. In the context of orthogonal axes, this means that each axis is perpendicular to each other axis._\n",
            "\n",
            "The basis vectors are unit vectors parallel to the axes and are denoted by $\\hat{\\mathbf{x}}$, $\\hat{\\mathbf{y}}$.  A point can also be represented by a vector\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = x \\hat{\\mathbf{x}} + y \\hat{\\mathbf{y}}\n",
            "$$\n",
            "\n",
            "from the origin of the coordinate frame to the point.  Such a vector is called a __coordinate vector__.  It is  a linear combination of the basis vectors.  In robotics we denote a coordinate vector by a column vector\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = \\begin{pmatrix} x \\\\ y \\end{pmatrix} \\in \\mathbb{R}^2\n",
            "$$\n",
            "\n",
            "or, in this particular case\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = \\begin{pmatrix} 1 \\\\ 2 \\end{pmatrix}.\n",
            "$$\n",
            "\n",
            "* _The $\\in$ symbol is used to denote that the element on the left is a member of a set on the right._\n",
            "* _The $\\mathbb{R}$ symbol denotes the real number space, the set of all real numbers._\n",
            "* _We use $\\mathbb{R}^n$ to denote the $n$-dimensional real number space, the set of all $n$ dimensional vectors with real valued elements._\n",
            "* _We use $\\mathbb{R}^{a \\times b}$ to represents set of all $a \\times b$ dimensional matrices with real valued elements._\n",
            "\n",
            "For a 3D coordinate system, there is an additional axis, the $z$-axis, and its basis vector is denoted by $\\hat{\\mathbf{z}}$.\n",
            "A point is represented by a coordinate vector\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = x \\hat{\\mathbf{x}} + y \\hat{\\mathbf{y}} + z \\hat{\\mathbf{z}}.\n",
            "$$\n",
            "\n",
            "which we write as a column vector\n",
            "$$\n",
            "    \\mathbf{p} = \\begin{pmatrix} x \\\\ y \\\\ z \\end{pmatrix} \\in \\mathbb{R}^3\n",
            "$$\n",
            "\n",
            "The specific 3D point given above is described by the column vector\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = \\begin{pmatrix} 3 \\\\ 2 \\\\ 4 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "We denote a coordinate frame as $\\{a\\}$, where $a$ is the name of the coordinate frame. There will always be an agreed local coordinate frame, and in robotics this is called the _world_ coordinate frame and is commonly denoted as $\\{0\\}$ or $\\{w\\}$.  If no explicit coordinate frame is given, we assume the world coordinate frame.  For a 2D problem like a mobile robot moving on a plane, the world coordinate frame is a 2D coordinate frame, for a 3D problem like a robot arm in a 3D workspace, the world coordinate frame is a 3D coordinate frame.\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "The color convention for coordinate frames is to use red for the $x$-axis, green for the $y$-axis, and blue for the $z$-axis.\n",
            "\n",
            "We use right-handed coordinate frames where your index/pointer finger points in the positive $x$-direction, your middle finger points in the positive $y$-direction, and your thumb points in the positive $z$-direction.\n",
            "\n",
            "<img src=\"Figures/1/box-righthand.png\" alt=\"drawing\" width=\"300\"/>\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Multiple Coordinate Frames\n",
            "\n",
            "Let's revisit our 2-dimensional position example and add another coordinate frame. In the below figure, the point is defined by the coordinate vector\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{p}} = \\begin{pmatrix} 1 \\\\ 2 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "which is represented in the world coordinate frame. We use the superscript notation ${^0(\\cdot)}$ to indicate that the position is relative to the $\\{0\\}$ coordinate frame. In this case the world coordinate frame, or the world frame, is the _reference_ coordinate frame.\n",
            "\n",
            "We have added a new coordinate frame $\\{a\\}$. The point is located at the same position relative to the __origin__ of the $\\{0\\}$ frame, but it is located at a different position relative to the __origin__ of the $\\{a\\}$ frame. The position of the point in the $\\{a\\}$ frame is\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{p}} = \\begin{pmatrix} -2 \\\\ 1 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "where we use the superscript notation ${^a(\\cdot)}$ to indicate that the position is relative to the $\\{a\\}$ coordinate frame."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<img src=\"Figures/1/fig1-3.png\" alt=\"drawing\" max-width=\"600\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "\n",
            "This leads us to a new problem. How can we represent the origin of one coordinate frame relative to another coordinate frame? For example, how can we represent the position of the origin of the $\\{a\\}$ frame relative to the origin of the $\\{0\\}$ frame?"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Motions\n",
            "\n",
            "We can think of the coordinate frame as a rigid body and the relative position between two coordinate frames as a motion.\n",
            "\n",
            "<img src=\"Figures/1/fig1-4.png\" alt=\"drawing\" max-width=\"600\"/>\n",
            "\n",
            "The dotted line shows the _motion_ from the $\\{0\\}$ to the $\\{a\\}$ frame. We can describe this motion as a translation of $x = 3$ and $y = 1$ which we can represent as a translation vector\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{t}_a} = \\begin{pmatrix} 3 \\\\ 1 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "where we use $\\mathbf{t}$ to indicate a translation vector, the superscript notation ${^0(\\cdot)}$ to indicate that the motion is relative to $\\{0\\}$ coordinate frame, the subscript notation ${(\\cdot)_a}$ to indicate that the motion is to the origin of the $\\{a\\}$ frame.\n",
            "\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "In most robotics problems we will multiple coordinate frames.  For example\n",
            "\n",
            "<img src=\"Figures/1/fig1-5.png\" alt=\"drawing\" width=\"500\"/>\n",
            "\n",
            "has three frames: $\\{0\\}$, $\\{a\\}$, and $\\{b\\}$. The motion from the $\\{0\\}$ to $\\{a\\}$ can be described by a translation vector ${^0\\mathbf{t}_a}$.\n",
            "\n",
            "The motion from the $\\{a\\}$ to $\\{b\\}$ can be described by a translation vector ${^a\\mathbf{t}_b}$.\n",
            "\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Important Semantics\n",
            "\n",
            "To be philosophical for a moment, a point in space has no position -- the concept is meaningless.  However we can introduce a coordinate frame into space, and then define the position of the point with respect to that coordinate frame in terms of a __coordinate vector__.  A point should always have a leading superscript to indicate the coordinate frame that the point is relative to. For example ${^a\\mathbf{p}}$ indicates that the point is relative to the $\\{a\\}$ coordinate frame.\n",
            "\n",
            "A motion or displacement from the origin of one coordinate frame to the origin of another can be described by a __translation__ vector.  A translation will always have a subscript to indicate the coordinate frame that the translation is relative to, and a subscript to indicate which frame the translation is to. For example ${^a\\mathbf{t}_b}$ indicates that the translation is from the origin of the $\\{a\\}$ coordinate frame to the origin of the $\\{b\\}$ coordinate frame.\n",
            "\n",
            "A coordinate vector $\\mathbf{p}$ and a translation vector $\\mathbf{t}$ have the same dimension or number of elements. However, they represent different things."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Test your Understanding\n",
            "\n",
            "#### Question 1.1\n",
            "If the coordinate vector of a point is\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{p}} = \\begin{pmatrix} 2 \\\\ 4 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "and the position of $\\{b\\}$ is\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{t}}_b = \\begin{pmatrix} 6 \\\\ 2 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "what is the position of $\\mathbf{p}$ relative to $\\{b\\}$?\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{p}} = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "#### Question 1.2\n",
            "If the position of the origin of $\\{a\\}$, or simply the position of $\\{a\\}$, is given by the coordinate vector\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{t}}_a = \\begin{pmatrix} 2 \\\\ 4 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "what is the relative position of $\\{0\\}$ with respect to $\\{a\\}$?\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{t}}_0 = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "#### Question 1.3\n",
            "If we define the relationship between the origin of the following frames\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{t}}_a = \\begin{pmatrix} 2 \\\\ 4 \\end{pmatrix}, \\quad\n",
            "    {^a\\mathbf{t}}_b = \\begin{pmatrix} 2 \\\\ 1 \\end{pmatrix}, \\quad\n",
            "    {^a\\mathbf{t}}_c = \\begin{pmatrix} 3 \\\\ -2 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "and a point\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{p}} = \\begin{pmatrix} -1 \\\\ 3 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "1. What is $^a\\mathbf{p}$?\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{p}} = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "2. What is $^b\\mathbf{p}$?\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{p}} = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "3. What is $^c\\mathbf{p}$?\n",
            "\n",
            "$$\n",
            "    {^c\\mathbf{p}} = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "4. What is $^b\\mathbf{t}_c$?\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{t}_c} = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "5. What is $^0\\mathbf{t}_c$?\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{t}_c} = \\begin{pmatrix} ? \\\\ ? \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "\n",
            "\n",
            "#### Question 1.4\n",
            "\n",
            "Find the relative translation of each __?__ in the following figure, using the information provided in 1.3 (take note of the direction the arrow is pointing)\n",
            "\n",
            "<img src=\"Figures/1/fig1-6.png\" alt=\"drawing\" width=\"500\"/>\n",
            "\n"
         ]
      },
      {
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<br>\n",
            "\n",
            "<a id='2'></a>\n",
            "# 2.0 What is Orientation?\n",
            "---"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "So far we have only discussed position. However, position describes only a subset of the spatial properties of an object. For most objects we have to deal with, its orientation is very important.  For a 2D problem we this might be the direction the robot is driving.  For a 3D problem, like a flying drone, orientation is more complex -- there are three axes to rotate around.\n",
            "\n",
            "As for position, there is no absolute way to describe the orientation of an object.  We can only describe its orientation with respect to some agreed coordinate frame.  Orientation is the relative orientation of one coordinate frame with respect to another.  The vertical, or gravity, axis is an important reference axis for humans when thinking about orientation.\n",
            "\n",
            "Consider the following figure. Frame $\\{a\\}$ is rotated 60 degrees relative to frame $\\{0\\}$\n",
            "\n",
            "<img src=\"Figures/1/fig2-1.png\" alt=\"drawing\" width=\"500\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "We use another right hand rule to define the positive direction of rotation. The thumb of the right hand points in the axis of rotation while the fingers wrap in the direction of positive rotation. In this case above, the positive direction of rotation is counterclockwise -- because the rotation is around an implicit $z$-axis out of the page.\n",
            "\n",
            "<img src=\"Figures/1/box-righthand-vector.png\" alt=\"drawing\" width=\"300\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "In this case the angle of rotation is simply a number: 60 degrees.  However in robotics, it is very common to represent a rotation by a rotation matrix. At first impression, most people think that this is overly cumbersome and complex, but bear with us because it has real advantages.\n",
            "\n",
            "For a 2D rotation, the rotation matrix is a $2 \\times 2$ matrix. For a 3D rotation, the rotation matrix is a $3 \\times 3$ matrix. However, not all $2 \\times 2$ and $3 \\times 3$ matrices are valid rotation matrices. Rotation matrices have the following properties:\n",
            "\n",
            "1. The columns are the basis vectors that define the axes of the rotated coordinate frame in the original coordinate frame, and therefore have unit length and are orthogonal\n",
            "2. It is an orthogonal (also called orthonormal) matrix and therefore its inverse is the same as its transpose, that is, $\\mathbf{R}^{-1} = \\mathbf{R}^\\top$\n",
            "3. The matrix-vector product $\\mathbf{R}\\mathbf{v}$ preserves the length and relative orientation of vectors $\\mathbf{v}$ and therefore its determinant is $1$\n",
            "\n",
            "2D Rotations are a member of the Special Orthogonal (SO) group of dimension 2 which we write as $\\mathbf{R} \\in \\mathbf{SO}(2) \\subset \\mathbb{R}^{2 \\times 2}$\n",
            "\n",
            "3D Rotations are a member of the Special Orthogonal (SO) group of dimension 3 which we write as $\\mathbf{R} \\in \\mathbf{SO}(3) \\subset \\mathbb{R}^{3 \\times 3}$\n",
            "\n",
            "* _The $\\subset$ symbol means that the set on the left is a subset of the set on the right._\n",
            "\n",
            "Being a group under the operation of __matrix multiplication__ means that the product of any two matrices belongs to the group, as does its inverse.\n",
            "\n",
            "Let's consider what these properties mean.\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Preserve Lengths\n",
            "\n",
            "__In this example we are using a 2D rotation matrix__: $\\mathbf{R} \\in \\mathbf{SO}(2)$,  although the same applies to 3D rotations.\n",
            "\n",
            "What does it mean to preserve lengths? \n",
            "\n",
            "This property means that the following must be true\n",
            "\n",
            "$$\n",
            "    \\| \\mathbf{v} \\| = \\| \\mathbf{R}\\mathbf{v} \\|\n",
            "$$\n",
            "\n",
            "where $\\mathbf{v}$ is some vector, $\\| \\mathbf{v} \\|$ is the length of vector $\\mathbf{v}$, $\\mathbf{R}$ is some valid rotation matrix and $\\| \\mathbf{R}\\mathbf{v} \\|$ is the length of the vector $\\mathbf{R}\\mathbf{v}$.\n",
            "\n",
            "* _The notation $\\| \\mathbf{v} \\|$ means the Euclidean norm or \"length\" of the vector $\\mathbf{v}$.  It is $(x^2 + y^2 + z^2)^{1/2}$ for a 3D vector.  It is sometimes referred to as the 2-norm and denoted by $\\| \\mathbf{v} \\|_2$._\n",
            "\n",
            "Intuitively, this means that if we rotate a vector, the length of the vector does not change. In the below example, we have a coordinate vector\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{p}} = \\begin{pmatrix} 1 \\\\ 2 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "and we also have two coordinate frames, the world frame $\\{0\\}$ and another coordinate frame $\\{a\\}$ that has been rotated with respect to $\\{0\\}$.\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<img src=\"Figures/1/fig2-2.png\" alt=\"drawing\" width=\"1000\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "In this figure, we have attached a grid to the origin of world frame $\\{0\\}$ and a slightly transparent grid to the rotated frame $\\{a\\}$. Using these grids and associated axes, we can see the coordinates of $\\mathbf{p}$ relative to the origin of both coordinate frames. Note that despite $\\mathbf{p}$ having different coordinates in each frame, the length of $\\mathbf{p}$, as shown by the dotted line, is the same in both frames\n",
            "\n",
            "$$\n",
            "    \\| {^0\\mathbf{p}} \\| = \\| {^a\\mathbf{p}} \\|\n",
            "$$"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Preserves Angles\n",
            "\n",
            "Dot product revision: [Dot products and duality | 3Blue1Brown](https://youtu.be/LyGKycYT2v0)\n",
            "\n",
            "__In this example we are using a 2D rotation matrix__: $\\mathbf{R} \\in \\mathbf{SO}(2)$,  but the same principles apply to 3D rotation matrices.\n",
            "\n",
            "We say that the nature of rotation matrices preserve angles, this means that\n",
            "\n",
            "$$\n",
            "    \\mathbf{v}_1 \\cdot \\mathbf{v}_2 = \\mathbf{R} \\mathbf{v}_1 \\cdot \\mathbf{R} \\mathbf{v}_2\n",
            "$$\n",
            "\n",
            "angle between the two vectors (remember the dot product is the cosine of the angle between the vectors) is the same before and after they are both rotated where $\\mathbf{v}_1$ and $\\mathbf{v}_2$ are some vectors of appropriate dimension and $\\mathbf{R}$ is a valid rotation matrix.\n",
            "\n",
            "$$\n",
            "    \\begin{pmatrix}\n",
            "        r_{11} & r_{12} \\\\\n",
            "        r_{21} & r_{22} \\\\\n",
            "    \\end{pmatrix}\n",
            "    \\begin{pmatrix}\n",
            "        1 \\\\ 0\n",
            "    \\end{pmatrix}\n",
            "    =\n",
            "    \\begin{pmatrix}\n",
            "        r_{11} \\\\ r_{21}\n",
            "    \\end{pmatrix}\n",
            "    \\quad\n",
            "    \\text{and}\n",
            "    \\quad\n",
            "        \\begin{pmatrix}\n",
            "        r_{11} & r_{12} \\\\\n",
            "        r_{21} & r_{22} \\\\\n",
            "    \\end{pmatrix}\n",
            "    \\begin{pmatrix}\n",
            "        0 \\\\ 1\n",
            "    \\end{pmatrix}\n",
            "    =\n",
            "    \\begin{pmatrix}\n",
            "        r_{12} \\\\ r_{22}\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "and we have\n",
            "\n",
            "$$\n",
            "    \\begin{pmatrix}\n",
            "        r_{11} \\\\ r_{12}\n",
            "    \\end{pmatrix}\n",
            "    \\cdot\n",
            "    \\begin{pmatrix}\n",
            "        r_{12} \\\\ r_{22}\n",
            "    \\end{pmatrix}\n",
            "    = 0\n",
            "$$\n",
            "\n",
            "because the columns of $\\mathbf{R}$ are orthogonal."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "We can demonstrate this using two position vectors expressed in the world coordinate frame $\\{0\\}$\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{p}_1} = \\begin{pmatrix} 1 \\\\ 2 \\end{pmatrix}\n",
            "    \\quad\n",
            "    \\text{and}\n",
            "    \\quad\n",
            "    {^0\\mathbf{p}_2} = \\begin{pmatrix} 2 \\\\ 1 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "The angle between the two vectors is $30$ degrees.\n",
            "\n",
            "We have a second coordinate frame $\\{a\\}$ that has been rotated with respect to $\\{0\\}$ by $-120$ degrees.\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{R}_0} =\n",
            "    \\begin{pmatrix}\n",
            "        -\\frac{1}{2} & \\frac{\\sqrt{3}}{2} \\\\\n",
            "        -\\frac{\\sqrt{3}}{2} & -\\frac{1}{2}\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "The coordinates of the $\\mathbf{p}_1$ and $\\mathbf{p}_2$ in the rotated frame $\\{a\\}$ is\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{p}_1} = {^a\\mathbf{R}_0} {^0\\mathbf{p}_1} = \\begin{pmatrix} 1.23 \\\\ -1.89 \\end{pmatrix}\n",
            "    \\\\\n",
            "    {^a\\mathbf{p}_2} = {^a\\mathbf{R}_0} {^0\\mathbf{p}_2} = \\begin{pmatrix} -0.13 \\\\ -2.23 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "the angle between the two transformed vectors is still $30$ degrees.\n",
            "\n",
            "The figure below shows two dots $\\mathbf{p}_1$ and $\\mathbf{p}_2$. The angle between the two vectors in each frame remains the same when represented relative to each coordinate frame."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<img src=\"Figures/1/fig2-3.png\" alt=\"drawing\"/>"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 3,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "The coordinates of p1 relative to {0} are [1. 2.]\n",
                  "The coordinates of p2 relative to {0} are [2. 1.]\n",
                  "The angle between p1 and p2 relative to {0} is 36.87 degrees\n",
                  "The coordinates of p1 relative to {a} are [ 1.23 -1.87]\n",
                  "The coordinates of p2 relative to {a} are [-0.13 -2.23]\n",
                  "The angle between p1 and p2 relative to {a} is 36.87 degrees\n"
               ]
            }
         ],
         "source": [
            "# Create p1 and p2 using numpy arrays\n",
            "# These are the coordinates of the points in the world frame\n",
            "p1 = np.array([1.0, 2.0])\n",
            "p2 = np.array([2.0, 1.0])\n",
            "\n",
            "# Print the coordinates of the points\n",
            "print(f\"The coordinates of p1 relative to {{0}} are {p1}\")\n",
            "print(f\"The coordinates of p2 relative to {{0}} are {p2}\")\n",
            "\n",
            "# Calculate the angle between the two vectors\n",
            "angle_w = np.rad2deg(np.arccos(np.clip(np.dot(p1 / np.linalg.norm(p1), p2 / np.linalg.norm(p2)), -1.0, 1.0)))\n",
            "\n",
            "print(f\"The angle between p1 and p2 relative to {{0}} is {np.round(angle_w, 2)} degrees\")\n",
            "\n",
            "# We create a 2D rotation matrix that rotates by 120 degrees\n",
            "# This will rotate from {0} to {a}\n",
            "# We are using the spatialmath package to do this\n",
            "# We use the SO2 class to create a rotation matrix\n",
            "# We use the A property to get the rotation matrix as a numpy array instead of an SO2 object\n",
            "aRw = sm.SO2(np.deg2rad(-120)).A\n",
            "\n",
            "# Get the point coordinates in the {a} frame\n",
            "# Using the group operation of matrix multiplication\n",
            "# This is completed using the @ symbol when using numpy arrays\n",
            "ap1 = aRw @ p1\n",
            "ap2 = aRw @ p2\n",
            "\n",
            "# Print the results\n",
            "print(f\"The coordinates of p1 relative to {{a}} are {np.round(ap1, 2)}\")\n",
            "print(f\"The coordinates of p2 relative to {{a}} are {np.round(ap2, 2)}\")\n",
            "\n",
            "# Calculate the angle between the two transformed vectors\n",
            "angle_a = np.rad2deg(np.arccos(np.clip(np.dot(ap1 / np.linalg.norm(ap1), ap2 / np.linalg.norm(ap2)), -1.0, 1.0)))\n",
            "\n",
            "print(f\"The angle between p1 and p2 relative to {{a}} is {np.round(angle_a, 2)} degrees\")"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## What makes the Special Orthogonal Group Special?\n",
            "\n",
            "Determinant revision: [The determinant | 3Blue1Brown](https://youtu.be/Ip3X9LOh2dk)\n",
            "\n",
            "Intuitively, the determinant of a matrix is a scalar measure of how a matrix transforms the volume of a space. For example, a matrix with a determinant of $0.5$ will transform a space by a factor of $0.5$ in volume.\n",
            "\n",
            "If our rotation matrices preserves the length of vectors and also preserves the angles between vectors, what do you think the determinant should be?\n",
            "\n",
            "The determinant of a rotation matrix is $1$ because it preserves the volume of the space. However, this does not make the group special as the determinant of any orthogonal matrix is $\\pm 1$. The difference between the Orthogonal Group and the Special Orthogonal Group is that the determinant of the rotation matrices in the Special Orthogonal Group is $+1$.\n",
            "\n",
            "What would happen if you transformed a coordinate frame by a matrix with a determinant of $-1$?\n",
            "\n",
            "Having a determinant of $-1$ would mean that the volume of the space would be effectively inverted. This would flip the axes of the transformed frame and you would end up with a left-handed coordinate frame."
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 4,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Determinant of R: 1.0\n"
               ]
            }
         ],
         "source": [
            "# We create a 2D rotation matrix that rotates by 15 degrees\n",
            "R = sm.SO2(15 * np.pi / 180).A\n",
            "\n",
            "# print the determinant of the rotation matrix\n",
            "print(f\"Determinant of R: {np.round(np.linalg.det(R), 2)}\")"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Matrix Groups\n",
            "\n",
            "We have mentioned the term \"group\" a few times and it is important to understand what this means. A group is a set of elements that are closed _under_ some operation. For example, the set of integers $\\mathbb{Z}$ is a group under addition -- adding two integers gives another integer. The set of real numbers $\\mathbb{R}$ is a group under multiplication. The set of complex numbers $\\mathbb{C}$ is a group under multiplication.  \n",
            "\n",
            "Rotation matrices $\\mathbf{SO}(2)$ and $\\mathbf{SO}(3)$ are a group under matrix multiplication.\n",
            "This means that the product of any two matrices that belong to same group also belongs to that group\n",
            "\n",
            "Elements of a group also have an inverse operation that produces an element belonging to the group. For example, the inverse of addition is subtraction and the inverse of multiplication is division. The inverse of a matrix is the matrix that when multiplied by the original matrix gives the identity matrix.  Integers do not form a group under multiplication because there is no inverse operation for multiplication -- the inverse of any integer besides $\\pm 1$ is not an integer and therefore outside the group.\n",
            "\n",
            "PROBS SHOULD INTRODUCE THE IDENTITY ELEMENT.\n",
            "\n",
            "INCLUDE THE AXIOMS OF A GROUP, COPY FROM RVC3?\n",
            "\n",
            "NEEDS TO BE SMOOTHED INTO THE NEXT SECTION"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 5,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Point coordinates relative to {0} = [1. 2.]\n",
                  "Point coordinates relative to {a} = [0.45 2.19]\n",
                  "The rotated coordinates match what we can see in the plot above\n",
                  "Length of original vector = 2.24\n",
                  "Length of rotated vector = 2.24\n"
               ]
            }
         ],
         "source": [
            "# Here is a 2D example that corresponds to the plot above\n",
            "\n",
            "# Our original point is located at (1, 2) realtive to {0}\n",
            "p = np.array([1.0, 2.0])\n",
            "\n",
            "# Print the original point\n",
            "print(f\"Point coordinates relative to {{0}} = {p}\")\n",
            "\n",
            "# We create a 2D rotation matrix that rotates by 15 degrees\n",
            "# This will rotate from {0} to {a}\n",
            "wRa = sm.SO2(15 * np.pi / 180).A\n",
            "\n",
            "# Get the point coordinates in the {a} frame\n",
            "ap = wRa @ p\n",
            "\n",
            "# Print the rotated point\n",
            "print(f\"Point coordinates relative to {{a}} = {np.round(ap, 2)}\")\n",
            "\n",
            "print(\"The rotated coordinates match what we can see in the plot above\")\n",
            "\n",
            "# Let's check the length of the original and rotated vector\n",
            "print(f\"Length of original vector = {np.round(np.linalg.norm(p), 2)}\")\n",
            "print(f\"Length of rotated vector = {np.round(np.linalg.norm(ap), 2)}\") "
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 6,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Point coordinates relative to {0} = [1. 2. 3.]\n",
                  "Point coordinates relative to {a} = [-0.13  2.23  3.  ]\n",
                  "Length of original vector = 3.74\n",
                  "Length of rotated vector = 3.74\n"
               ]
            }
         ],
         "source": [
            "# Here is a 3D example\n",
            "\n",
            "# Make a point located at (1, 2, 3) in {0}\n",
            "p = np.array([1.0, 2.0, 3.0])\n",
            "\n",
            "# Print the original frame\n",
            "print(f\"Point coordinates relative to {{0}} = {p}\")\n",
            "\n",
            "# We create a 3D rotation matrix that rotates by 30 degrees\n",
            "# This will rotate from {0} to {a}\n",
            "# We use the SO3 class with the Rz class method to create a rotation matrix about the z-axis\n",
            "wRa = sm.SO3.Rz(30 * np.pi / 180).A\n",
            "\n",
            "# Get the point coordinates in the {a} frame\n",
            "ap = wRa @ p\n",
            "\n",
            "# Print the rotated frame\n",
            "print(f\"Point coordinates relative to {{a}} = {np.round(ap, 2)}\")\n",
            "\n",
            "# Let's check the length of the original and rotated vector\n",
            "print(f\"Length of original vector = {np.round(np.linalg.norm(p), 2)}\")\n",
            "print(f\"Length of rotated vector = {np.round(np.linalg.norm(ap), 2)}\") "
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Group Inverse Operation\n",
            "\n",
            "__In this example we are using a 3D rotation matrix__: $\\mathbf{R} \\in \\mathbf{SO}(3)$, although the same applies to 2D rotations.\n",
            "\n",
            "\n",
            "What is $\\mathbf{R}^\\top \\mathbf{R}$?\n",
            "\n",
            "Looking at the first operation within the matrix multiplication\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}^\\top \\mathbf{R} = \\begin{pmatrix}\n",
            "        \\color{red} r_{11} & \\color{red} r_{21} & \\color{red} r_{31} \\\\\n",
            "        r_{12} & r_{22} & r_{32} \\\\\n",
            "        r_{13} & r_{23} & r_{33} \\\\\n",
            "    \\end{pmatrix}\n",
            "    \\begin{pmatrix}\n",
            "        \\color{red} r_{11} & r_{12} & r_{13} \\\\\n",
            "        \\color{red} r_{21} & r_{22} & r_{23} \\\\\n",
            "        \\color{red} r_{31} & r_{32} & r_{33} \\\\\n",
            "    \\end{pmatrix}\n",
            "    =\n",
            "    \\begin{pmatrix}\n",
            "        \\color{red} 1 & ? & ? \\\\\n",
            "        ? & ? & ? \\\\\n",
            "        ? & ? & ? \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "where we get $1$ because the columns of $\\mathbf{R}$ have unit length. Looking at the second operation, we have\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}^\\top \\mathbf{R} = \\begin{pmatrix}\n",
            "        \\color{red} r_{11} & \\color{red} r_{21} & \\color{red} r_{31} \\\\\n",
            "        r_{12} & r_{22} & r_{32} \\\\\n",
            "        r_{13} & r_{23} & r_{33} \\\\\n",
            "    \\end{pmatrix}\n",
            "    \\begin{pmatrix}\n",
            "        r_{11} & \\color{red}  r_{12} & r_{13} \\\\\n",
            "        r_{21} & \\color{red}  r_{22} & r_{23} \\\\\n",
            "        r_{31} & \\color{red}  r_{32} & r_{33} \\\\\n",
            "    \\end{pmatrix}\n",
            "    =\n",
            "    \\begin{pmatrix}\n",
            "        1 & \\color{red} 0 & ? \\\\\n",
            "        ? & ? & ? \\\\\n",
            "        ? & ? & ? \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "due to each column of $\\mathbf{R}$ being orthogonal to the others.\n",
            "\n",
            "Finishing the multiplication following this pattern we obtain the identity matrix\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}^\\top \\mathbf{R} = \\begin{pmatrix}\n",
            "        r_{11} & r_{21} & r_{31} \\\\\n",
            "        r_{12} & r_{22} & r_{32} \\\\\n",
            "        r_{13} & r_{23} & r_{33} \\\\\n",
            "    \\end{pmatrix}\n",
            "    \\begin{pmatrix}\n",
            "        r_{11} & r_{12} & r_{13} \\\\\n",
            "        r_{21} & r_{22} & r_{23} \\\\\n",
            "        r_{31} & r_{32} & r_{33} \\\\\n",
            "    \\end{pmatrix}\n",
            "    =\n",
            "    \\begin{pmatrix}\n",
            "        1 & 0 & 0 \\\\\n",
            "        0 & 1 & 0 \\\\\n",
            "        0 & 0 & 1 \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "The definition of the matrix inverse is\n",
            "\n",
            "$$\n",
            "    \\mathbf{A} \\mathbf{A}^{-1} = \\mathbf{A}^{-1} \\mathbf{A} = \\mathbf{I}\n",
            "$$\n",
            "\n",
            "This gives us the following relationship\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}^{-1} = \\mathbf{R}^\\top\n",
            "$$\n",
            "\n",
            "$\\mathbf{R}^\\top$ is also a valid rotation matrix. This means that the rows of $\\mathbf{R}$ are also unit length and orthogonal."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## What is an inverse Rotation?\n",
            "\n",
            "The inverse rotation is the \"opposite\" rotation. If we apply a rotation and then its inverse then we are back where we started.  This means that if you rotate a vector by $\\mathbf{R}$, then rotate it again by $\\mathbf{R}^{-1}$, you will end up with the original vector. Lets try it out\n"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 7,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Point coordinates in the world frame = [1. 2. 3.]\n",
                  "Point coordinates in the {a} frame = [-0.1339746   2.23205081  3.        ]\n"
               ]
            }
         ],
         "source": [
            "# Make a 3D point with coordinates x = 1, y = 2, z = 3\n",
            "p = np.array([1.0, 2.0, 3.0])\n",
            "\n",
            "# Print the point\n",
            "print(f\"Point coordinates in the world frame = {p}\")\n",
            "\n",
            "# Make a new coordinate frame {a} that is rotated 30 degrees on the z-axis relative to {0}\n",
            "# This is an SO3 rotation matrix that rotates about the z-axis by 30 degrees\n",
            "aR0 = sm.SO3.Rz(np.deg2rad(30)).A\n",
            "\n",
            "# Rotate the vector using the rotation matrix\n",
            "ap = aR0 @ p\n",
            "\n",
            "# Print the rotated frame\n",
            "print(f\"Point coordinates in the {{a}} frame = {ap}\")"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 8,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Point coordinates back in the world frame = [1. 2. 3.]\n",
                  "This is the same as the original frame coordinates\n"
               ]
            }
         ],
         "source": [
            "# Now let's rotate the frame back to its original position using the inverse of the rotation matrix\n",
            "# We get the inverse of the rotation matrix by taking the transpose\n",
            "# We get the transpose of a numpy array by using the T property\n",
            "p_original = aR0.T @ ap\n",
            "\n",
            "# Print the rotated point\n",
            "print(f\"Point coordinates back in the world frame = {p_original}\")\n",
            "print(\"This is the same as the original frame coordinates\")"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 9,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Rz = \n",
                  "[[ 0.87 -0.5   0.  ]\n",
                  " [ 0.5   0.87  0.  ]\n",
                  " [ 0.    0.    1.  ]]\n",
                  "\n",
                  "Rz Transposed = \n",
                  "[[ 0.87  0.5   0.  ]\n",
                  " [-0.5   0.87  0.  ]\n",
                  " [ 0.    0.    1.  ]]\n",
                  "\n",
                  "Rz Inverse = \n",
                  "[[ 0.87  0.5   0.  ]\n",
                  " [-0.5   0.87  0.  ]\n",
                  " [ 0.    0.    1.  ]]\n"
               ]
            }
         ],
         "source": [
            "# Lets look at Rz and Rz.T\n",
            "\n",
            "# Print Rz\n",
            "print(f\"Rz = \\n{np.round(aR0, 2)}\\n\")\n",
            "\n",
            "# Print Rz.T\n",
            "print(f\"Rz Transposed = \\n{np.round(aR0.T, 2)}\\n\")\n",
            "\n",
            "# Print inv(Rz)\n",
            "print(f\"Rz Inverse = \\n{np.round(np.linalg.inv(aR0), 2)}\")\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "We can see through the code output above that the inverse rotation is equal to the transpose of the rotation matrix."
         ]
      },
      {
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Making a 2D Rotation Matrix\n",
            "\n",
            "We can make a 2D rotation matrix by using the following formula\n",
            "\n",
            "$$\n",
            "    \\mathbf{R} = \\begin{pmatrix}\n",
            "        \\cos (\\theta) & -\\sin (\\theta) \\\\\n",
            "        \\sin (\\theta) & \\cos (\\theta) \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "where $\\theta$ is the angle of rotation."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Making a 3D Rotation Matrix\n",
            "\n",
            "We can make a 3D rotation matrix for rotation about the $x$-, $y$-, and $z$-axis through the following formula\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}_x =\n",
            "    \\begin{pmatrix}\n",
            "        1 & 0 & 0 \\\\\n",
            "        0 & \\cos (\\theta_x) & -\\sin (\\theta_x) \\\\\n",
            "        0 & \\sin (\\theta_x) & \\cos (\\theta_x) \\\\\n",
            "    \\end{pmatrix}\n",
            "    $$\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}_y =\n",
            "    \\begin{pmatrix}\n",
            "        \\cos (\\theta_y) & 0 & \\sin (\\theta_y) \\\\\n",
            "        0 & 1 & 0 \\\\\n",
            "        -\\sin (\\theta_y) & 0 & \\cos (\\theta_y) \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}_z =\n",
            "    \\begin{pmatrix}\n",
            "        \\cos (\\theta_z) & -\\sin (\\theta_z) & 0 \\\\\n",
            "        \\sin (\\theta_z) & \\cos (\\theta_z) & 0 \\\\\n",
            "        0 & 0 & 1 \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "    \n",
            "where $\\theta$ is the angle of rotation.\n",
            "\n",
            "We can make any arbitrary rotation matrix by multiplying a combination of these three matrices together.\n",
            "In Module 2 we will look at how these matrices are generated and what they mean.\n",
            "\n",
            "\n",
            "\n"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 10,
         "metadata": {},
         "outputs": [],
         "source": [
            "# Let's make some python methods that create SO3 rotation matrices\n",
            "\n",
            "# This method creates a rotation matrix that rotates about the x-axis\n",
            "def Rx(theta: float):\n",
            "    return np.array([\n",
            "        [1, 0, 0],\n",
            "        [0, np.cos(theta), -np.sin(theta)],\n",
            "        [0, np.sin(theta), np.cos(theta)]\n",
            "    ])\n",
            "\n",
            "# This method creates a rotation matrix that rotates about the y-axis\n",
            "def Ry(theta: float):\n",
            "    return np.array([\n",
            "        [np.cos(theta), 0, np.sin(theta)],\n",
            "        [0, 1, 0],\n",
            "        [-np.sin(theta), 0, np.cos(theta)]\n",
            "    ])\n",
            "\n",
            "# This method creates a rotation matrix that rotates about the z-axis\n",
            "def Rz(theta: float):\n",
            "    return np.array([\n",
            "        [np.cos(theta), -np.sin(theta), 0],\n",
            "        [np.sin(theta), np.cos(theta), 0],\n",
            "        [0, 0, 1]\n",
            "    ])"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 11,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Rotation about x-axis using our method: \n",
                  "[[ 1.         0.         0.       ]\n",
                  " [ 0.         0.8660254 -0.5      ]\n",
                  " [ 0.         0.5        0.8660254]]\n",
                  "\n",
                  "Rotation about x-axis using spatialmath: \n",
                  "   1         0         0         \n",
                  "   0         0.866    -0.5       \n",
                  "   0         0.5       0.866     \n",
                  "\n"
               ]
            }
         ],
         "source": [
            "# Let's try it out and compare to the spatialmath python package\n",
            "\n",
            "# Make a rotation about the x-axis by 30 degrees using our method\n",
            "rot_x_ours = Rx(30 * np.pi / 180)\n",
            "\n",
            "# Make a rotation about the x-axis by 30 degrees using spatialmath\n",
            "rot_x_sm= sm.SO3.Rx(30 * np.pi / 180)\n",
            "\n",
            "# Print the rotation matrices\n",
            "print(f\"Rotation about x-axis using our method: \\n{rot_x_ours}\")\n",
            "print(f\"\\nRotation about x-axis using spatialmath: \\n{rot_x_sm}\")"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "The matrix elements are the same, but the formatting is different.  What's going on here?  What is the Python type of the results in each case?  Why are the numbers colored red in the second case?"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Test your Understanding\n",
            "\n",
            "#### Question 2.1\n",
            "Write a Python method that creates an $\\mathbf{SO}(2)$ rotation matrix. The method should take in an angle $\\theta$ and return the rotation matrix.\n",
            "\n",
            "```python\n",
            "def R(theta):\n",
            "    pass\n",
            "```\n",
            "\n",
            "#### Question 2.2\n",
            "If a coordinate frame's origin at ${^0\\mathbf{t}} = (1,\\ 0)^\\top$ is rotated by $45^\\circ$, what is the new position of the coordinate frame?\n",
            "\n",
            "#### Question 2.3\n",
            "If a coordinate frame's origin at  ${^0\\mathbf{t}} = (4,\\ 2,\\ 1)^\\top$ is rotated by $45^\\circ$ about the $y$-axis, what is the new position of the coordinate frame?\n",
            "\n",
            "#### Question 2.3\n",
            "If a coordinate frame's origin at  ${^0\\mathbf{t}} = (1,\\ 5,\\ 2)^\\top$ is rotated by $30^\\circ$ about the $y$-axis followed by a $60^\\circ$ rotation about the $z$-axis, what is the new position of the coordinate frame?\n",
            "\n",
            "#### Question 2.4\n",
            "If we have a rotation matrix about the $x$-axis of $45^\\circ$:\n",
            "\n",
            "$$\n",
            "    \\mathbf{R}_a =\n",
            "    \\begin{pmatrix}\n",
            "        1 & 0 & 0 \\\\\n",
            "        0 & 0.707 & -0.707 \\\\\n",
            "        0 & 0.707 & 0.707 \\\\\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "1. Which group is this matrix a member of?\n",
            "2. What is the inverse of this matrix (without using Python)?\n",
            "3. Show that $\\mathbf{R}_a$ is an orthogonal matrix\n",
            "4. Is $2 \\cdot \\mathbf{R}_a$ a member of the group you answered in 1?\n",
            "5. Is $\\mathbf{R}_a^2$ a member of the group you answered in 1?\n",
            "6. Is $\\mathbf{R}_a \\mathbf{R}_a^\\top $ a member of the group you answered in 1?\n",
            "\n",
            "#### Question 2.5\n",
            "\n",
            "If we have a rotation matrix $\\mathbf{R}_b \\in \\mathbf{SO}(3)$, solve the following\n",
            "\n",
            "$$\n",
            "    \\left(\n",
            "        \\mathbf{R}_b \\mathbf{R}_b^\\top\n",
            "    \\right)^\\top\n",
            "    = \\ ?\n",
            "$$\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<br>\n",
            "\n",
            "<a id='3'></a>\n",
            "# 3.0 Homogeneous Coordinates\n",
            "---"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "So far, if we wanted to represent a point in a new reference frame that was translated and rotated with respect to the first, we would have to do it in two steps.\n",
            "\n",
            "For a 2D point ${^a\\mathbf{p}} \\in \\mathbb{R}^2$, and we wish to represent this point in coordinate frame $\\{b\\}$, we would first have to apply a rotation followed by adding the translation\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{p}} = {^b\\mathbf{R}_a} {^a\\mathbf{p}} + {^b\\mathbf{t}_a}\n",
            "$$\n",
            "\n",
            "where ${^b\\mathbf{R}_a} \\in \\mathbf{SO}(2)$ is the rotation matrix and ${^b\\mathbf{t}_a} \\in \\mathbb{R}^2$ is the translation vector."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<img src=\"Figures/1/fig2_11.png\" alt=\"drawing\" width=\"700\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Rotating and Translating\n",
            "\n",
            "We can combine the rotation and translation into a single matrix which performs a _rigid-body transformation_, a _homogeneous transformation_, or simply a _transform_.  This tool is the workhorse of robotics and also computer graphics.\n",
            "\n",
            "* _The term rigid-body transformation comes from the fact that all points in the object have a constant coordinate vector with respect to the object's coordinate frame.  If we move the frame then all the points travel with it._\n",
            "\n",
            "To express a point in homogeneous coordinates, we add a 1 to the end of the vector\n",
            "\n",
            "For a 2D point, this becomes\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = \\begin{pmatrix}\n",
            "        x \\\\\n",
            "        y \\\\\n",
            "        1\n",
            "    \\end{pmatrix} \\in \\mathbb{P}^2\n",
            "$$\n",
            "\n",
            "For a 3D point, this becomes\n",
            "\n",
            "$$\n",
            "    \\mathbf{p} = \\begin{pmatrix}\n",
            "        x \\\\\n",
            "        y \\\\\n",
            "        z \\\\\n",
            "        1\n",
            "    \\end{pmatrix}  \\in \\mathbb{P}^3\n",
            "$$\n",
            "\n",
            "* _The set $\\mathbb{P}^n$ is the set of projective coordinates in $n$ dimensions, and has $n+1$ elements._\n",
            "\n",
            "We can then express the rotation and translation as a single matrix. We call this a homogeneous transformation matrix, a rigid body transformation, or commonly referred to as a transform.\n",
            "\n",
            "A 2D transform is given by\n",
            "\n",
            "\\begin{align*}\n",
            "    \\mathbf{T}\n",
            "    &= \\begin{pmatrix}\n",
            "        r_{11} & r_{12} & t_x \\\\\n",
            "        r_{21} & r_{22} & t_y \\\\\n",
            "        0 & 0 & 1\n",
            "    \\end{pmatrix} \\\\\n",
            "    &= \\begin{pmatrix}\n",
            "        \\mathbf{R} & \\mathbf{t} \\\\\n",
            "        \\mathbf{0} & 1\n",
            "    \\end{pmatrix}\n",
            "\\end{align*}\n",
            "\n",
            "where $\\mathbf{R} \\in \\mathbf{SO}(2)$ is the rotation matrix and $\\mathbf{t} \\in \\mathbb{R}^2$ is the translation vector. \n",
            "This matrix belongs to the Special Euclidean group $\\mathbf{T} \\in \\mathbf{SE}(2) \\subset \\mathbb{R}^{3 \\times 3}$.\n",
            "\n",
            "Expanding out the matrix-vector product\n",
            "\n",
            "$$\n",
            "\\mathbf{T} \\begin{pmatrix} x \\\\ y \\\\ 1 \\end{pmatrix} =\n",
            "\\begin{pmatrix}\n",
            "        r_{11} & r_{12} \\\\\n",
            "        r_{21} & r_{22} \\end{pmatrix} \\begin{pmatrix} x \\\\ y \\end{pmatrix}\n",
            "        + \\begin{pmatrix} t_x \\\\ t_y \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "where we see that the rotation is applied to the coordinate vector first, then it is translated.\n",
            "\n",
            "\n",
            "A 3D transform is given by\n",
            "\n",
            "\\begin{align*}\n",
            "    \\mathbf{T}\n",
            "    &= \\begin{pmatrix}\n",
            "        r_{11} & r_{12} & r_{13} & x \\\\\n",
            "        r_{21} & r_{22} & r_{23} & y \\\\\n",
            "        r_{31} & r_{32} & r_{33} & z \\\\\n",
            "        0 & 0 & 0 & 1\n",
            "    \\end{pmatrix} \\\\\n",
            "    &= \\begin{pmatrix}\n",
            "        \\mathbf{R} & \\mathbf{t} \\\\\n",
            "        \\mathbf{0} & 1\n",
            "    \\end{pmatrix}\n",
            "\\end{align*}\n",
            "\n",
            "where $\\mathbf{R} \\in \\mathbf{SO}(3)$ is the rotation matrix and $\\mathbf{t} \\in \\mathbb{R}^3$ is the translation vector. \n",
            "This matrix belongs to the Special Euclidean group $\\mathbf{T} \\in \\mathbf{SE}(3) \\subset \\mathbb{R}^{4 \\times 4}$."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "### Example 1\n",
            "\n",
            "Let's look at an example of a 2D transform. We have a point in the world frame expressed as a homogeneous coordinate vector\n",
            "\n",
            "$$\n",
            "    {^0\\mathbf{p}} = \\begin{pmatrix}\n",
            "        1 \\\\\n",
            "        2 \\\\\n",
            "        1\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "and we want to transform it into a new coordinate frame with an origin translated $(2,\\ 3)^\\top$ relative to the origin of the world frame with no rotation. We can represent this using an $\\mathbf{SE}(2)$ matrix\n",
            "\n",
            "$$\n",
            "    {\\mathbf{T}} = \\begin{pmatrix}\n",
            "        1 & 0 & 2 \\\\\n",
            "        0 & 1 & 3 \\\\\n",
            "        0 & 0 & 1\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "We can then apply the transformation using matrix multiplication\n",
            "\n",
            "$$\n",
            "    {\\mathbf{p}_*} = {\\mathbf{T}} {\\mathbf{p}} = \\begin{pmatrix}\n",
            "        1 & 0 & 2 \\\\\n",
            "        0 & 1 & 3 \\\\\n",
            "        0 & 0 & 1\n",
            "    \\end{pmatrix}\n",
            "    \\begin{pmatrix}\n",
            "        1 \\\\\n",
            "        2 \\\\\n",
            "        1\n",
            "    \\end{pmatrix}\n",
            "    = \\begin{pmatrix}\n",
            "        3 \\\\\n",
            "        5 \\\\\n",
            "        1\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "Where the point $\\mathbf{p}_*$ in the new coordinate frame is located at $(3,\\ 5)^\\top$."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Group Operations\n",
            "\n",
            "The group operation of the Special Euclidean group is matrix multiplication.\n",
            "\n",
            "$$\n",
            "    \\mathbf{T}_1 \\mathbf{T}_2 = \\mathbf{T}_3\n",
            "$$\n",
            "\n",
            "where $\\mathbf{T}_1, \\mathbf{T}_2, \\mathbf{T}_3 \\in \\mathbf{SE}(2)$ or $\\mathbf{SE}(3)$.\n",
            "\n",
            "Because they form a group, the inverse of a transform is also a member of the group. A transform translates and rotates an object.  The inverse transform represents the translation and rotation that returns the object to its original pose.\n",
            "\n",
            "* _Note that the matrix is not orthogonal so the transpose is not equal to the inverse -- that trick is only for rotation matrices._\n",
            "\n",
            "$$\n",
            "    \\mathbf{T}^{-1} \\neq \\mathbf{T}^\\top\n",
            "$$\n",
            "\n",
            "where $\\mathbf{T}, \\ \\mathbf{T}^{-1} \\in \\mathbf{SE}(2)$ or $\\mathbf{SE}(3)$ and $\\mathbf{T}^\\top \\notin \\mathbf{SE}(2)$ or $\\mathbf{SE}(3)$.\n",
            "\n",
            "The identity matrix is also a member of the group and represents a null transform (no rotation or translation).\n",
            "\n",
            "$$\n",
            "    \\mathbf{T} \\mathbf{I} = \\mathbf{T}\n",
            "$$\n",
            "\n",
            "where $\\mathbf{T}, \\ \\mathbf{I} \\in \\mathbf{SE}(2)$ or $\\mathbf{SE}(3)$"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Composing Transformations\n",
            "\n",
            "_This section will introduce some crucial semantics that provides the key to consistent and accurate use of coordinate frames and transforms._\n",
            "\n",
            "As shown repeatedly in this notebook, we use the following convention to make clear the reference frame and the destination frame\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{p}}\n",
            "$$\n",
            "\n",
            "are point coordinates relative to $\\{a\\}$.\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{t}_b}\n",
            "$$\n",
            "\n",
            "is a translation vector from the origin of $\\{a\\}$ to the origin of $\\{b\\}$.\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{R}_b}\n",
            "$$\n",
            "\n",
            "is a rotation matrix from the origin of $\\{a\\}$ to the origin of $\\{b\\}$.\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{T}_b}\n",
            "$$\n",
            "\n",
            "is a homogeneous transformation matrix from the origin of $\\{a\\}$ to the origin of $\\{b\\}$.\n",
            "\n",
            "The reference frame, the frame that the transformation is relative to, is always expressed as a superscript on the left of the appropriate symbol ($\\mathbf{p}$, $\\mathbf{t}$, $\\mathbf{R}$, $\\mathbf{T}$). The destination frame, the frame that the transformation is applied to, is always expressed as a subscript on the right of the appropriate symbol ($\\mathbf{t}$, $\\mathbf{R}$, $\\mathbf{T}$).\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "### Making a Transformation\n",
            "\n",
            "This is also known as composing a transformation.\n",
            "\n",
            "The rule for transforming frames is that the inner super-subscript must match and the outer super-subscript define the resulting transformation.\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{T}_c} = {^{\\color{red} a}\\mathbf{T}_{\\color{green} b}} {^{\\color{green} b}\\mathbf{T}_{\\color{red} c}}\n",
            "$$\n",
            "\n",
            "where we have obtained the transformation from frame $\\{a\\}$ to frame $\\{c\\}$ by first transforming from $\\{a\\}$ to $\\{b\\}$ and then from $\\{b\\}$ to $\\{c\\}$. The inner super-subscripts are marked in green and must be the same. The outer super-subscripts are marked in red and define the resulting transformation."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "### The Inverse Transformation\n",
            "\n",
            "When transforming from $\\{a\\}$ to $\\{b\\}$ we can obtain the transformation from $\\{b\\}$ to $\\{a\\}$ by inverting the transformation matrix or the rotation matrix.\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{T}_a} = {^a\\mathbf{T}_b}^{-1}\n",
            "$$\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{R}_a} = {^a\\mathbf{R}_b}^{-1}\n",
            "$$\n",
            "\n",
            "For a translation, the inverse translation is simply the negative of the translation vector.\n",
            "\n",
            "$$\n",
            "    {^b\\mathbf{t}_a} = -{^a\\mathbf{t}_b}\n",
            "$$\n",
            "\n",
            "Now is a good time to consider the utility of using an $\\mathbf{SE}(2)$ or $\\mathbf{SE}(3)$ matrix instead of a rotation matrix and translation vector combination.\n",
            "\n",
            "#### Example, putting the Inverse Transformation to use\n",
            "\n",
            "Given the following transforms\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{T}_b}, \\quad {^c\\mathbf{T}_b}\n",
            "$$\n",
            "\n",
            "and we want to compute the transform\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{T}_c}\n",
            "$$\n",
            "\n",
            "we can use the inverse transformation to obtain the desired result\n",
            "\n",
            "\\begin{align*}\n",
            "    {^a\\mathbf{T}_c}\n",
            "    &=\n",
            "    {^a\\mathbf{T}_b} {^c\\mathbf{T}_b}^{-1} \\\\\n",
            "    &=\n",
            "    {^a\\mathbf{T}_b} {^b\\mathbf{T}_c}\n",
            "\\end{align*}\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Test your Understanding\n",
            "\n",
            "#### Question 3.1\n",
            "\n",
            "What is the result of the following matrix multiplication?\n",
            "\n",
            "$$\n",
            "    {^g\\mathbf{T}_0} {^0\\mathbf{T}_q}\n",
            "    = {^?\\mathbf{T}_?}\n",
            "$$\n",
            "\n",
            "#### Question 3.2\n",
            "\n",
            "Create an expression that creates a transform from $\\{h\\}$ to $\\{d\\}$ given the following transforms\n",
            "\n",
            "$$\n",
            "    {^h\\mathbf{T}_0}, \\quad {^0\\mathbf{T}_e}, \\quad {^d\\mathbf{T}_e}\n",
            "$$\n",
            "\n",
            "#### Question 3.3\n",
            "\n",
            "Are the following valid operations? If so, what is the result? If not, why not?\n",
            "\n",
            "1. $\\quad$ $^c\\mathbf{R}_d \\ ^d\\mathbf{R}_g$\n",
            "2. $\\quad$ $^c\\mathbf{t}_d \\ ^d\\mathbf{t}_g$\n",
            "3. $\\quad$ $^d\\mathbf{T}_c^{-1} \\ ^d\\mathbf{T}_g$\n",
            "4. $\\quad$ $^d\\mathbf{R}_c^\\top \\ ^d\\mathbf{R}_g$\n",
            "4. $\\quad$ $^c\\mathbf{R}_d \\ ^g\\mathbf{R}_d^{-1}$\n",
            "5. $\\quad$ $^d\\mathbf{T}_c^{\\top} \\ ^d\\mathbf{T}_g$\n",
            "6. $\\quad$ $^d\\mathbf{T}_c \\ ^d\\mathbf{T}_g^{-1}$\n",
            "7. $\\quad$ $^d\\mathbf{T}_c \\ ^h\\mathbf{T}_c^{-1} \\ ^h\\mathbf{T}_f \\ ^b\\mathbf{T}_f^{-1}$\n",
            "\n",
            "#### Question 3.4\n",
            "\n",
            "Turn the following translation vector into a homogeneous transformation matrix, assuming a null rotation\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{t}_b} = \\begin{pmatrix} 1 \\\\ 2 \\\\ 3 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "#### Question 3.5\n",
            "\n",
            "Turn the following rotation matrix into a homogeneous transformation matrix, assuming a null translation\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{R}_b} = \\begin{pmatrix} 1 & 0 & 0 \\\\ 0 & 0.707 & -0.707 \\\\ 0 & 0.707 & 0.707 \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "#### Question 3.6\n",
            "\n",
            "What is the rotation component of the following homogeneous transformation matrix?\n",
            "\n",
            "What is the translation component of the following homogeneous transformation matrix?\n",
            "\n",
            "$$\n",
            "    {^a\\mathbf{T}_b} =\n",
            "    \\begin{pmatrix}\n",
            "        0.707 & -0.707 & 0 & 2.6 \\\\\n",
            "        0.707 & 0.707 & 0 & 1.8 \\\\\n",
            "        0 & 0 & 1 & 3.2 \\\\\n",
            "        0 & 0 & 0 & 1\n",
            "    \\end{pmatrix}\n",
            "$$\n",
            "\n",
            "\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<br>\n",
            "\n",
            "<a id='4'></a>\n",
            "# 4.0 Pose Graphs\n",
            "---"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "A pose graph is a directed graph what comprises _vertices_, representing coordinate frames, and _edges_ representing transformations between coordinate frames.\n",
            "\n",
            "\n",
            "In the following Figure, the blue circles represent the vertices in a pose graph. Each of these vertices is labelled and represents the pose of a coordinate frame. The grey arrows represent the edges of the pose graph. Each of these edges is labelled and assigned a transformation from the tail coordinate frame to the head coordinate frame. The red arrow represents a transformation which we do not know but would like to infer.\n",
            "\n",
            "In the scenario depicted in the below pose graph, we have a robot arm attached to a mobile robot, a free-standing camera, and a workpiece. Using a mapping system, we know where the mobile robot exists in the world. The robot arm is rigidly attached to the mobile robot with a known transform and we know the pose of the end-effector relative to the base of the robot arm thanks to manipulator kinematics. We have placed the camera in a known location, so we know its pose in the world. Using the camera, we have a system that can find the pose of the workpiece relative to camera.\n",
            "\n",
            "Using the pose graph, we can find the pose of the workpiece relative to the robot arm end-effector. As long as the two vertices in a pose graph are connected by a path of edges, we can find the transform between them -- no matter the direction of the edges or the length of the path.\n",
            "\n",
            "We get the pose of the workpiece relative to the end-effector through the following composition of transforms\n",
            "\n",
            "\\begin{align*}\n",
            "    {^e\\mathbf{T}_p}\n",
            "    &=\n",
            "    {^b\\mathbf{T}_e^{-1}} \\ {^m\\mathbf{T}_b^{-1}} \\ {^0\\mathbf{T}_m^{-1}} \\ {^0\\mathbf{T}_c} \\ {^c\\mathbf{T}_p} \\\\\n",
            "    &=\n",
            "    {^e\\mathbf{T}_b} \\ {^b\\mathbf{T}_m} \\ {^m\\mathbf{T}_0} \\ {^0\\mathbf{T}_c} \\ {^c\\mathbf{T}_p}\n",
            "\n",
            "\\end{align*}\n",
            "\n",
            "\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<img src=\"Figures/1/fig4-1.png\" alt=\"drawing\" width=\"1000\"/>"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Test your Understanding\n",
            "\n",
            "#### Question 4.1\n",
            "\n",
            "Think of some real-world robotics applications for pose graphs."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "#### Question 4.2\n",
            "\n",
            "Can you devise an algorithm, you don't have to code it, that would compute the transformation between any two nodes in the pose graph."
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "<br>\n",
            "\n",
            "<a id='5'></a>\n",
            "# 5.0 Summary of Representations\n",
            "---\n"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "In general, we can use the symbol $\\xi$ as an abstract representation of a motion or its many synonyms\n",
            "\n",
            "* A transformation from one coordinate frame to another\n",
            "* A motion between two coordinate frames\n",
            "* The pose of a coordinate frame relative to another\n",
            "* The position and orientation of a coordinate frame relative to another\n",
            "\n",
            "__These all mean the same thing.__\n",
            "\n",
            "We have gone through a number of different representations for $\\xi$ and have seen that each representation has its own advantages and disadvantages.\n",
            "\n",
            "For __translation only__ motions the following may represent $\\xi$\n",
            "\n",
            "* A translation vector $\\mathbf{t}$\n",
            "\n",
            "For __rotation only__ motions the following could represent $\\xi$\n",
            "\n",
            "* A rotation matrix $\\mathbf{R} \\in \\mathbf{SO}(n)$ for rotational motion in $n$ dimensional space\n",
            "* An angle axis representation $\\theta, \\ \\hat{\\mathbf{\\eta}}$\n",
            "* An Euler vector or exponential coordinates $\\mathbf{\\eta}$\n",
            "* A unit quaternion or versor $\\mathring{q} \\in \\mathbf{S}^3$ for three dimensional motions\n",
            "* An Euler angle sequence $\\Gamma$\n",
            "\n",
            "For motions with __both translation and rotation__ the following may represent $\\xi$\n",
            "\n",
            "* A homogeneous transformation $\\mathbf{T} \\in \\mathbf{SE}(n)$ for rotational and translational motion in $n$ dimensional space\n",
            "* A twist $\\mathbf{s} \\in \\mathbb{R}^{6}$ for three dimensional motions\n",
            "* We can pair a translation vector $\\mathbf{t}$ with any or the rotational representations"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "_Angle-axis, Euler vectors, unit quaternions, Euler angles, and twists will be introduced in the next two modules._"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Test your Understanding\n",
            "\n",
            "#### Question 5.1\n",
            "\n",
            "Write a Python method that can compose two 2-dimensional motions in the form $(x, \\ y, \\ \\theta)$ using basic trigonometry.\n",
            "\n",
            "```python\n",
            "def compose_motion(x1: float, y1: float, theta1:float, x2: float, y2: float, theta2:float) -> tuple:\n",
            "    \"\"\"\n",
            "    Compose two 2D motions\n",
            "\n",
            "    Parameters\n",
            "    ----------\n",
            "        x1\n",
            "            x component of first motion\n",
            "        y1\n",
            "            y component of first motion\n",
            "        theta1\n",
            "            theta component of first motion\n",
            "        x2\n",
            "            x component of second motion\n",
            "        y2\n",
            "            y component of second motion\n",
            "        theta2\n",
            "            theta component of second motion\n",
            "\n",
            "    Returns\n",
            "    -------\n",
            "    motion\n",
            "        tuple: x, y, theta components of composed motion\n",
            "    \"\"\"\n",
            "    pass\n",
            "```\n",
            "\n",
            "#### Question 5.2\n",
            "\n",
            "Write a Python method that can compose two 2-dimensional motions in the form $(x, \\ y, \\ \\theta)$ by first converting them to homogeneous transformations\n",
            "\n",
            "```python\n",
            "def compose_motion2(x1: float, y1: float, theta1:float, x2: float, y2: float, theta2:float) -> tuple:\n",
            "    \"\"\"\n",
            "    Compose two 2D motions\n",
            "\n",
            "    Parameters\n",
            "    ----------\n",
            "        x1\n",
            "            x component of first motion\n",
            "        y1\n",
            "            y component of first motion\n",
            "        theta1\n",
            "            theta component of first motion\n",
            "        x2\n",
            "            x component of second motion\n",
            "        y2\n",
            "            y component of second motion\n",
            "        theta2\n",
            "            theta component of second motion\n",
            "\n",
            "    Returns\n",
            "    -------\n",
            "    motion\n",
            "        (3, 3) numpy array: homogeneous transformation of composed motion SE(2)\n",
            "    \"\"\"\n",
            "    pass\n",
            "```\n",
            "\n",
            "#### Question 5.3\n",
            "\n",
            "Reflect on why we use $\\mathbf{SE}(n)$ to represent and compose motions."
         ]
      }
   ],
   "metadata": {
      "kernelspec": {
         "display_name": "Python 3 (ipykernel)",
         "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.10.9"
      }
   },
   "nbformat": 4,
   "nbformat_minor": 2
}