CoordinateTransformations.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Section II. MODELING"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Chapter 3. Coordinates and Transformations\n",
    "\n",
    "<style>\n",
    "div.figcaption {    \n",
    "    text-align: center;\n",
    "    margin-left:1em;\n",
    "    margin-top:1em;\n",
    "    margin-right:1em;\n",
    "    margin-bottom:1em;\n",
    "    }\n",
    "</style>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In almost all fields of science and engineering, it is essential to\n",
    "identify and manipulate mathematical representations of physical,\n",
    "real-world quantities. Robotics is no exception. Intelligent robots\n",
    "build a \"mental model\" of themselves and the world as they perceive\n",
    "their environments, and they modify those models when interpreting the\n",
    "past and predicting the future. In an abstract sense, these models are\n",
    "simply a collection of numbers and labels, with no *explicit* meaning to\n",
    "the robot. It is the job of a robot engineer to correctly associate\n",
    "numbers with meaning, and correspondingly, meanings with numbers. (If\n",
    "this sounds like mumbo jumbo at the moment, some concrete examples will\n",
    "be offered soon enough!)\n",
    "\n",
    "Two of the most important mathematical representations are vectors and\n",
    "matrices from linear algebra. Vectors are often representations of\n",
    "positions or directions in two or three dimensions of space, but can\n",
    "also represent other quantities like sensor measurements. Matrices are\n",
    "representations of how representations change, either through an action,\n",
    "or even through a change in how those numbers are interpreted. We will\n",
    "be using them liberally throughout the book, and they appear in almost\n",
    "every subject of robotics. Hence, they must be mastered to get anywhere\n",
    "beyond a superficial understanding of the material.\n",
    "\n",
    "This chapter discusses how vectors and matrices are used in robotics to\n",
    "represent 2D and 3D positions, directions, rigid body motion, and\n",
    "coordinate transformations. It is assumed that all students will have\n",
    "taken a course in linear algebra and can refresh themselves on basic\n",
    "definitions. Appendix 1 reviews the standard vector and matrix notation,\n",
    "conventions, and basic identities that will be used throughout this\n",
    "book."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Vectors and coordinates\n",
    "-----------------------\n",
    "\n",
    "Vectors extend concepts that are familiar to us from working with real\n",
    "numbers $\\mathbb{R}$ to other spaces of interest. They also succinctly\n",
    "represent collections of real numbers that have a common meaning like\n",
    "position or direction, or readings from a signal taken at a given time.\n",
    "They make mathematical expressions more compact, which helps\n",
    "us wrap our heads around more difficult concepts.\n",
    "\n",
    "Most often, $n$-dimensional Euclidean spaces $\\mathbb{R}^n$ is used, in\n",
    "which a vector is simply a tuple of $n$ real numbers. The \"list of\n",
    "numbers\" interpretation is the most common way that vectors are\n",
    "conceived of by engineers and computer scientists, and that is certainly\n",
    "how they are stored and operated upon. Let us call this the \"layman's\n",
    "definition\" of vectors. However, it is often important to realize that\n",
    "these numbers are just an interpretation of a more abstract essential\n",
    "concept -- the underlying physical meaning -- and the numbers will\n",
    "change depending on their manner of interpretation, such as a chosen\n",
    "frame of reference. This section will present common operations in 2D\n",
    "and 3D, and follow it with a discussion about the importance of\n",
    "separating meaning from representation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2D coordinate frames\n",
    "\n",
    "In the \"layman's definition\", an $n$-dimensional *vector* $\\mathbf{x}$ is a\n",
    "tuple of real numbers $\\mathbf{x}=(x_1,\\ldots,x_n) \\in \\mathbb{R}^n$. For now, we\n",
    "will work in $\\mathbb{R}^2$. We will use boldface notation only temporarily\n",
    "to help distinguish between vectors and real numbers. In the future, the\n",
    "boldface will typically be dropped.\n",
    "\n",
    "A 2D position $P$ is represented by a 2-element vector $\\mathbf{p}=(p_x,p_y)$\n",
    "that gives its coordinates relative to axis directions $X$ and $Y$,\n",
    "offset from a position $O$ where the axes cross, called the origin ([Fig. 1](#fig:Coordinates2D)). We\n",
    "will also represent vectors in column vector form:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\n",
    "\\mathbf{p} = \\begin{bmatrix}p_x \\\\\n",
    "p_y \\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "for use in matrix-vector products. Both\n",
    "parenthetical and column vector notations are equivalent and\n",
    "interchangeable.\n",
    "\n",
    "\n",
    "*************\n",
    "\n",
    "![fig:Coordinates2D](figures/modeling/coordinate_axes_2d.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:Coordinates2D\">Figure 1.</a> A point $P$ in the plane (a) has no numerical representation until we\n",
    "define a reference coordinate frame (b), which has origin point $O$ and\n",
    "orthogonal coordinate axes $X$ and $Y$. Its coordinates\n",
    "$\\mathbf{p}=(p_x,p_y)$ are respectively the extents of $P$ along $X$ and $Y$\n",
    "from the origin (c).</div>\n",
    "\n",
    "*************\n",
    "\n",
    "The items $O$, $X$, and $Y$ define the *coordinate frame* in which the\n",
    "coordinates are interpreted. Here $O$ is an arbitrary position in space,\n",
    "and $X$ and $Y$ are *orthogonal directions* with $Y$ rotated $90^\\circ$\n",
    "counter-clockwise from $X$. Note that in isolation, a vector of\n",
    "coordinates does not define a position. A physical position is only\n",
    "defined by coordinates *in reference to a certain coordinate frame*. The\n",
    "frame will often be left implicit, or spoken of as the reference frame\n",
    "of the coordinates.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3D coordinate frames\n",
    "\n",
    "The situation in 3D space is similar, except that we represent a 3D\n",
    "position $P$ with a 3-element vector $\\mathbf{p}=(p_x, p_y, p_z)$ that gives\n",
    "its coordinates relative to axes $X$, $Y$, and $Z$ and offset from an\n",
    "origin $O$ in 3D space where the axes cross. The parenthetical notation\n",
    "is equivalent to the column vector form:\n",
    "\n",
    "$$\n",
    "  \\mathbf{p} = \\begin{bmatrix} p_x \\\\ p_y \\\\ p_z \\end{bmatrix}.\n",
    "$$\n",
    "\n",
    "In 3D the coordinate frame consists of the origin $O$ and the mutually\n",
    "orthogonal axes $X$, $Y$, and $Z$. In this book we will use\n",
    "*right-handed coordinate convention* in which the axes can be envisioned\n",
    "in the layout of the first three fingers of the right hand, suitably\n",
    "arranged at $90^\\circ$ right angles. $X$ axis corresponds to the thumb,\n",
    "$Y$ axis corresponds to the index finger, and $Z$ axis corresponds to the middle finger."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Directional quantities\n",
    "\n",
    "Vectors are also used to represent *directional quantities*, such as a\n",
    "displacement, direction, or derivative. A *displacement* is a difference\n",
    "between points, e.g., $\\mathbf{q}-\\mathbf{p}$ gives the amount that would need to\n",
    "be moved in both the $X$ and $Y$ direction to move from $P$ to $Q$,\n",
    "where $\\mathbf{q}$ gives the coordinates of $Q$ relative to the same\n",
    "reference frame. It has both a direction and a magnitude. In contrast, a\n",
    "*direction* does not have magnitude, and is a unit vector. The direction\n",
    "from $P$ to $Q$ is given by\n",
    "\n",
    "$$\n",
    "    \\frac{\\mathbf{q}-\\mathbf{p}}{\\|\\mathbf{q}-\\mathbf{p}\\|}.\n",
    "$$\n",
    "\n",
    "In 2D, a direction can also be given as an angle $\\theta \\in [0,2\\pi)$ rad,\n",
    "with the convention that the angle measures the counter-clockwise\n",
    "direction from the $X$ axis. The corresponding unit vector is\n",
    "$(\\cos(\\theta),\\sin(\\theta))$.\n",
    "\n",
    "***************\n",
    "\n",
    "![fig:DirectionalQuantities](figures/modeling/directional_quantities.svg)\n",
    "\n",
    "<div class=\"figcaption\">\n",
    "<a name=\"fig:DirectionalQuantities\">Figure 2</a>. Directional quantities arise from displacements (a), directions (b),\n",
    "and derivatives of paths (c).\n",
    "</div>\n",
    "\n",
    "***************\n",
    "\n",
    "A *derivative* is an infinitesimal displacement. If the position $P(t)$\n",
    "is a function of $t$, then its derivative $\\mathbf{p}^\\prime(t)$ is a vector\n",
    "$(p_x(t),p_y(t))$.\n",
    "\n",
    "The major difference between directional and position quantities is that\n",
    "*coordinates of directional quantities do not vary with respect to the\n",
    "choice of origin*. However, coordinates of both positions and directions\n",
    "are affected by the choice of coordinate axes."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Geometric operations\n",
    "\n",
    "The coordinates of a point $\\mathbf{p}$ after translation by a displacement\n",
    "$\\mathbf{d}$ can be computed by vector addition $\\mathbf{p} + \\mathbf{d}$.\n",
    "Interpolation and extrapolation between points $\\mathbf{p}$, $\\mathbf{q}$ is\n",
    "specified by the equation\n",
    "\n",
    "$$\n",
    "    \\mathbf{x}(u) = (1-u)\\mathbf{p} + u\\mathbf{q}\n",
    "$$\n",
    "\n",
    "for $u \\in \\mathbb{R}$. This equation starts at $\\mathbf{x}(0) = \\mathbf{p}$ at $u=0$, and\n",
    "ends at $\\mathbf{x}(1)=\\mathbf{q}$ at $u=1$. Extrapolation can be obtained with\n",
    "$u < 0$ or $u > 1$, as shown in <a href=\"#fig:LineEquation\">Figure 4.a</a>.\n",
    "\n",
    "\n",
    "***************\n",
    "\n",
    "![fig:LineEquations](figures/modeling/line_equations.png)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:LineEquation\">Figure 4</a>. The line passing through points $P$ and $Q$ can be modeled as a\n",
    "parametric interpolation (a) or as a plane equation (b).</div>\n",
    "\n",
    "***************\n",
    "\n",
    "The line through $\\mathbf{p}$ and $\\mathbf{q}$ can be obtained by sweeping the\n",
    "above interpolation / extrapolation formula across the entire range of\n",
    "$u \\in \\mathbb{R}$. The line segment between $\\mathbf{p}$ and $\\mathbf{q}$ is obtained\n",
    "by sweeping $u$ across the range $[0,1]$.\n",
    "\n",
    "An other useful definition of a line in 2D uses a point on the line and\n",
    "an orthogonal direction. We define $\\mathbf{p}^\\perp = (-p_y,p_x)$ as an\n",
    "orthogonal direction to $\\mathbf{p}=(p_x,p_y)$, which has the same magnitude\n",
    "but is rotated 90$^\\circ$ clockwise. The line through the origin passing\n",
    "through $\\mathbf{p}$ can be expressed in the form of all solutions $\\mathbf{x}$ to\n",
    "the equation $\\mathbf{x} \\cdot \\mathbf{p}^\\perp = 0$. Similarly, the line through\n",
    "points $P$ and $Q$ can be expressed as the equation\n",
    "\n",
    "$$\n",
    "    \\mathbf{x} \\cdot (\\mathbf{p} - \\mathbf{q})^\\perp = \\mathbf{p} \\cdot (\\mathbf{p} - \\mathbf{q})^\\perp\n",
    "$$\n",
    "\n",
    "Another expression of lines is the following:\n",
    "\n",
    "$$\n",
    "    \\mathbf{x} \\cdot \\mathbf{n} = c\n",
    "$$\n",
    "\n",
    "Where $\\mathbf{n}$ is orthogonal to the direction of the line and\n",
    "$c = \\mathbf{p} \\cdot \\mathbf{n}$ for any point $\\mathbf{p}$ on the line.\n",
    "(<a href=\"#fig:LineEquation\">Fig. 4.b.</a>)\n",
    "\n",
    "This definition is known as the *plane equation*, which generalizes\n",
    "lines in 2D to planes in 3D and hyperplanes in higher dimensions. Each\n",
    "of these is object of $n-1$ dimensions in an $n$-dimensional space,\n",
    "which we call a generalized plane. A unique representation for a\n",
    "generalized plane is $\\mathbf{x}\\cdot \\mathbf{u} = b$ where $\\mathbf{u}$ is a unit\n",
    "vector orthogonal to the plane known as the *normal direction* and $b$\n",
    "is a nonnegative *offset* that determines the distance away from the\n",
    "origin."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Transformations\n",
    "---------------\n",
    "\n",
    "Transformations are functions that map $n$-D vectors to other $n$-D\n",
    "vectors: $T : \\mathbb{R}^n \\rightarrow \\mathbb{R}^n$. They can represent\n",
    "geometric operations, which are caused by movement or action, as well as\n",
    "changes of coordinates, which are caused by changes of interpretation.\n",
    "Many common spatial transformations, including translations, rotations,\n",
    "and scaling are represented by matrix / vector operations. Changes of\n",
    "coordinate frames are also matrix / vector operations. As a result,\n",
    "transformation matrices are stored and operated on ubiquitously in\n",
    "robotics."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Linear transformations\n",
    "\n",
    "A *linear transformation* is one that can be represented as a matrix\n",
    "operation\n",
    "\n",
    "$$\\mathbf{y} = A\\mathbf{x}$$\n",
    "\n",
    "where $\\mathbf{x} \\in \\mathbb{R}^m$,\n",
    "$\\mathbf{y} \\in \\mathbb{R}^n$, and A is an $m \\times n$ matrix. In particular, in\n",
    "2D, this operation takes the form\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}q_x \\\\ q_y \\end{bmatrix} = \\begin{bmatrix}\n",
    "a & b \\\\\n",
    "c & d\n",
    "\\end{bmatrix} \\begin{bmatrix}p_x \\\\ p_y \\end{bmatrix} = \\begin{bmatrix}\n",
    "a p_x + b p_y\\\\\n",
    "c p_x + d p_y\n",
    "\\end{bmatrix}.\n",
    "$$\n",
    "\n",
    "In 3D, a linear transformation takes the form "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\n",
    "\\begin{bmatrix}\n",
    "q_x \\\\\n",
    "q_y \\\\\n",
    "q_z \n",
    "\\end{bmatrix} = \\begin{bmatrix} \n",
    "a_{11} & a_{12} & a_{13} \\\\ \n",
    "a_{21} & a_{22} & a_{23} \\\\ \n",
    "a_{31} & a_{32} & a_{33} \n",
    "\\end{bmatrix} \n",
    "\\cdot \\begin{bmatrix}\n",
    "p_x \\\\\n",
    "p_y \\\\\n",
    "p_z \n",
    "\\end{bmatrix} = \\begin{bmatrix}\n",
    "a_{11} p_x + a_{12} p_y + a_{13} p_z \\\\\n",
    "a_{21} p_x + a_{22} p_y + a_{23} p_z \\\\\n",
    "a_{31} p_x + a_{32} p_y + a_{33} p_z \n",
    "\\end{bmatrix}.\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rotations in 2D\n",
    "\n",
    "Rotations about the origin by angle $\\theta$ can be defined as linear\n",
    "transformations. Consider two reference frames with a common origin $O$,\n",
    "the pre-rotation axes $X$ and $Y$, and the post-rotation axes $X^\\prime$\n",
    "and $Y^\\prime$. Depicting $X^\\prime$ on top of $X$ and $Y$ as a line\n",
    "emanating from the origin, and using a little trigonometry, we shall see\n",
    "that $X^\\prime$ has coordinates\n",
    "$\\mathbf{x}^\\prime = (\\cos \\theta,\\sin \\theta)$. It is a bit more involved,\n",
    "but not much, to determine that $Y^\\prime$ has coordinates\n",
    "$\\mathbf{y}^\\prime = (-\\sin \\theta, \\cos \\theta)$.\n",
    "\n",
    "Now consider that along with the coordinate frames, a point $P$ was\n",
    "rotated to $P^\\prime$. We will derive how to determine its new\n",
    "coordinates relative to the original reference frame. Notice that\n",
    "$P^\\prime$ still has coordinates $(p_x,p_y)$ relative to the\n",
    "post-rotation frame $X^\\prime$, $Y^\\prime$, since distances do not\n",
    "shrink or grow when objects are rotated. Specifically, $P^\\prime$ is\n",
    "obtained by walking $p_x$ units from the origin in the direction of\n",
    "$X^\\prime$, and then $p_y$ units in the direction of $Y^\\prime$\n",
    "([Fig. 5](#fig:Rotation2D)). Hence, to determine its coordinates in the\n",
    "original reference frame, we can use the fact that the coordinates of\n",
    "$X^\\prime$ and $Y^\\prime$ are known:\n",
    "\n",
    "$$\n",
    "\\mathbf{p}^\\prime = p_x \\mathbf{x}^\\prime + p_y \\mathbf{y}^\\prime = \\begin{bmatrix}\n",
    "p_x \\cos \\theta - p_y \\sin \\theta \\\\ \n",
    "p_x \\sin \\theta + p_y \\cos \\theta\n",
    "\\end{bmatrix}.\n",
    "$$\n",
    "\n",
    "***************\n",
    "\n",
    "![fig:Rotation2D](figures/modeling/rotation2d.svg)\n",
    "![fig:Rotation2D2](figures/modeling/rotation2d_2.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:Rotation2D\">Figure 5</a>. Rotating at an angle $\\theta$ about the origin to achieve a new point\n",
    "$P^\\prime$ (a). To calculate the coordinates of $P^\\prime$ (b), we first\n",
    "obtain the coordinates of transformed axes $X^\\prime$ and $Y^\\prime$\n",
    "(c,d) and then use the parallelogram rule (e).</div>\n",
    "\n",
    "****************\n",
    "\n",
    "A more compact and convenient way of writing this is with a matrix\n",
    "equation $$\\mathbf{p}^\\prime = R(\\theta) \\mathbf{p}$$ with the rotation matrix\n",
    "given by\n",
    "\n",
    "$$\n",
    "R(\\theta) = \\begin{bmatrix}\n",
    "\\cos \\theta & -\\sin \\theta \\\\ \n",
    "\\sin \\theta & \\cos \\theta\n",
    "\\end{bmatrix}.\n",
    "$$\n",
    "\n",
    "There are several useful properties of such matrices:\n",
    "\n",
    "1.  The matrix composition\n",
    "    $R(\\theta_1) R(\\theta_2) = R(\\theta_1 + \\theta_2)$ gives the\n",
    "    rotation matrix for the sum of the angles.\n",
    "\n",
    "2.  The determinant\n",
    "    $\\det(R(\\theta)) = \\cos^2 \\theta + \\sin^2 \\theta = 1$ for all\n",
    "    $\\theta$.\n",
    "\n",
    "3.  Due to the identities $\\cos(-\\theta) = \\cos(\\theta)$ and\n",
    "    $\\sin(-\\theta) = - \\sin(\\theta)$, the operation of rotating about\n",
    "    $-\\theta$ is equivalent to a matrix transpose:\n",
    "\n",
    "    $$\n",
    "    R(-\\theta) = \\begin{bmatrix}\n",
    "    \\cos \\theta & \\sin \\theta \\\\ \n",
    "    -\\sin \\theta & \\cos \\theta\n",
    "    \\end{bmatrix} = R(\\theta)^T.\n",
    "    $$\n",
    "\n",
    "4.  Moreover, the transpose is equivalent to the matrix inverse:\n",
    "\n",
    "    $$R(-\\theta) = R(\\theta)^T = R(\\theta)^{-1}.$$\n",
    "    \n",
    "    In other words, rotation matrices are orthogonal.\n",
    "\n",
    "5.  Vector norms are invariant under rotation:\n",
    "\n",
    "    $$\\| R(\\theta) \\mathbf{x} \\| = \\| \\mathbf{x} \\|.$$\n",
    "\n",
    "6.  The rotation matrix is only dependent on the argument's value modulo\n",
    "    $2\\pi$.\n",
    "\n",
    "The space of rotations is known as the *special orthogonal group*\n",
    "$SO(2)$. The reason why it is called the special orthogonal group is\n",
    "that it is the set of all orthogonal $2\\times 2$ matrices with positive\n",
    "determinant, while there do exist other orthogonal matrices with\n",
    "determinant -1.\n",
    "\n",
    "Property 4 implies that it is more proper to consider rotation matrices\n",
    "as only representing instantaneous *orientation* rather than accumulated\n",
    "amounts of revolution. For example, if a motor has spun $720^\\circ$, the\n",
    "matrix representation is indistinguishable from the 0 rotation. In\n",
    "certain applications that demand reasoning about accumulated revolution,\n",
    "the representation $\\theta \\in \\mathbb{R}$ is more appropriate than a\n",
    "matrix. More about this topic will be discussed when we cover\n",
    "topological spaces."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rotations in 3D\n",
    "\n",
    "In 3D, rotations can also be defined as linear transformations, although\n",
    "parameterizing them is not as simple as in 2D. In [the next chapter](3DRotations.ipynb), 3D\n",
    "rotation representations will discussed in further detail, but for now\n",
    "let us describe some of their properties.\n",
    "\n",
    "A rotation in 3D can be represented by a matrix equation\n",
    "$$\\mathbf{p}^\\prime = R \\mathbf{p}$$ with $R$ a $3\\times 3$ rotation matrix.\n",
    "\n",
    "\n",
    "***************\n",
    "\n",
    "![fig:Coordinates3D2](figures/modeling/rotation3d.svg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:Coordinates3D2\">Figure 6</a>. After a rotation, the coordinates of the transformed point\n",
    "(relative to the original axes) are determined via a matrix\n",
    "multiplication with the rotation matrix $R$.</div>\n",
    "\n",
    "***************\n",
    "\n",
    "Like in 2D, rotation matrices satisfy several properties:\n",
    "\n",
    "1.  The composition of two rotation matrices $R_1 R_2$ is also a\n",
    "    rotation matrix.\n",
    "\n",
    "2.  In general, however, $R_1 R_2 \\neq R_2 R_1$ unless the two are\n",
    "    rotations about a shared axis.\n",
    "\n",
    "3.  The determinant $\\det(R) = 1$ for all rotation matrices.\n",
    "\n",
    "4.  The transpose of a rotation matrix is its inverse: $R^T = R^{-1}$,\n",
    "    or $R R^T = R^T R = I$.\n",
    "\n",
    "5.  Vector norms are invariant under rotation.\n",
    "\n",
    "The space of 3D rotations is known as the *special orthogonal group*\n",
    "$SO(3)$.\n",
    "\n",
    "Another interpretation of rotation matrices is to interpret each of the\n",
    "3 columns as the coordinates of each of the coordinate axes after\n",
    "rotation. In other words, when a coordinate frame rotates by matrix $R$\n",
    "about the origin, the entries of the rotation matrix can be interpreted\n",
    "as:\n",
    "\n",
    "$$\n",
    "  R = \\begin{bmatrix} x_x & y_x & z_x \\\\ x_y & y_y & z_y \\\\ x_z & y_z & z_z \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "where $(x_x,x_y,x_z)$ give the coordinates of the\n",
    "new $X$ axis in the old frame, $(y_x,y_y,y_z)$ gives the coordinates of\n",
    "the new $Y$ axis, and $(z_x,z_y,z_z)$ gives the coordinates of the new\n",
    "$Z$ axis.\n",
    "\n",
    "***************\n",
    "\n",
    "![fig:Coordinates3D](figures/modeling/coordinate_axes_3d.svg)\n",
    "\n",
    "<div style=\"text-align:center\"><a name=\"fig:Coordinates3D2\">Figure 7</a>. A 3D rotation is encoded by a $3\\times 3$ matrix whose columns\n",
    "give the coordinates of the rotated axes $\\mathit{relative}$ to the original\n",
    "axes. </div>\n",
    "\n",
    "***************\n",
    "\n",
    "This interpretation is useful when determining the coordinates\n",
    "of a rotated point in the original reference frame: if the point is\n",
    "given by coordinates $\\mathbf{p} = (p_x,p_y,p_z)$ such that\n",
    "$P-O = p_x X + p_y Y + p_z Z$, then the new coordinates of $P$ relative\n",
    "to the old reference frame are given by $R \\mathbf{p}$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Scaling\n",
    "\n",
    "Axis-aligned scaling in 2D about the origin can be represented as a\n",
    "linear transform with matrix\n",
    "\n",
    "$$\n",
    "  S(s_x,s_y) = \\begin{bmatrix}s_x & 0 \\\\ 0 & s_y \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "where $s_x$ is the scaling about the $X$ direction and $s_y$ is the\n",
    "scaling about the $Y$ direction. If $s_x = s_y$ this is known as a\n",
    "uniform scaling.\n",
    "\n",
    "This definition can be generalized to $n$-D space using an $n$-D scaling\n",
    "vector $\\mathbf{s}$ which determines the scaling in each direction by mapping\n",
    "to the diagonal of an $n \\times n$ matrix:\n",
    "\n",
    "$$\n",
    "  S(\\mathbf{s}) = diag(\\mathbf{s}).\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Compositions of linear transformations\n",
    "\n",
    "When performing two linear transformations one after another, the\n",
    "results are determined via matrix multiplication. Suppose that\n",
    "$T_1(\\mathbf{x})$ and $T_2(\\mathbf{x})$ are both linear transformations with\n",
    "matrices $A$ and $B$, respectively. When performing the operation of\n",
    "$T_2$ first to obtain $\\mathbf{y} = T_2(\\mathbf{x})$, then performing $T_1$ to\n",
    "obtain $\\mathbf{z}=T_1(\\mathbf{y})$, the ultimate result is:\n",
    "\n",
    "$$\n",
    "\\mathbf{z} = T_1(T_2(\\mathbf{x}))\n",
    "$$\n",
    "\n",
    "where it should be noted that $T_1$ appears\n",
    "first in the equation even though it is performed after $T_2$. Expanding\n",
    "this into matrix products,\n",
    "\n",
    "$$\n",
    "  \\mathbf{z} = A B \\mathbf{x}\n",
    "$$\n",
    "\n",
    "holding for all values\n",
    "of $\\mathbf{x}$. As a result, the function composition $T_1 \\circ T_2$ is\n",
    "also a linear transformation with matrix $AB$.\n",
    "\n",
    "Using composition we can derive other useful transformations, like\n",
    "scaling not aligned to an axis. Suppose we wish to scale some\n",
    "coordinates by value $s$ in a direction $\\mathbf{v}$, where $\\mathbf{v}$ is a unit\n",
    "vector. We can think of this as first rotating by an angle $\\theta$ so\n",
    "that the $X$ axis is aligned with $\\mathbf{v}$, then performing an\n",
    "axis-aligned scaling, then rotating back to the original coordinate\n",
    "frame:\n",
    "\n",
    "$$\n",
    "    A = R(-\\theta) S(s,1) R(\\theta).\n",
    "$$\n",
    "\n",
    "It so happens that\n",
    "\n",
    "$$\n",
    "    R(\\theta) = \\begin{bmatrix}v_x & v_y \\\\ -v_y & v_x \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "performs the rotation that\n",
    "we desire. As a result we can compute:\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "A & = \\begin{bmatrix}v_x & -v_y \\\\ v_y & v_x \\end{bmatrix} \\begin{bmatrix}s & 0 \\\\ 0 & 1 \\end{bmatrix} \\begin{bmatrix}v_x & v_y \\\\ -v_y & v_x \\end{bmatrix} = \\begin{bmatrix}v_x & -v_y \\\\ v_y & v_x \\end{bmatrix} \\begin{bmatrix}sv_x & sv_y \\\\ -v_y & v_x \\end{bmatrix} \\\\\n",
    "  & = \\begin{bmatrix}s v_x^2 + v_y^2 & s v_x v_y - v_y v_x \\\\ s v_x v_y - v_x v_y & s v_y^2 + v_x^2 \\end{bmatrix} = I + (s-1)\\begin{bmatrix}v_x^2 & v_x v_y \\\\ v_x v_y & v_y^2\\end{bmatrix}\\end{aligned}\n",
    "$$\n",
    "\n",
    "Note that due to the non-symmetricity of matrix multiplication, order of\n",
    "transformation matters: a rotation followed by a scaling is not\n",
    "necessarily the same as a scaling followed by a rotation. However, with\n",
    "a little inspection, we can derive the following symmetric compositions:\n",
    "\n",
    "-   As a consequence of\n",
    "    $R(\\theta_1) R(\\theta_2) = R(\\theta_1 + \\theta_2)$, rotation by\n",
    "    angle $\\theta_1$ followed by angle $\\theta_2$ is symmetric:\n",
    "    $R(\\theta_1) R(\\theta_2) = R(\\theta_2) R(\\theta_1)$. (Note that\n",
    "    symmetry does not generally hold in 3D!)\n",
    "\n",
    "-   A rotation and a uniform scaling.\n",
    "\n",
    "-   Two axis-aligned scalings.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rigid transformations\n",
    "\n",
    "Rigid transformations in two dimensions have two properties:\n",
    "\n",
    "1.  The distance between two points do not change after being\n",
    "    transformed.\n",
    "\n",
    "2.  In 2D, the orientation and area of any triangle does not change, and\n",
    "    in 3D, the orientation and volume of any tetrahedron does not\n",
    "    change.\n",
    "\n",
    "The form of all rigid transforms is a rotation $R$ followed by an arbitrary translation $\\mathbf{t}$: \n",
    "\n",
    "$$\n",
    "    T(\\mathbf{x}) = R \\mathbf{x} + \\mathbf{t}\n",
    "$$\n",
    "\n",
    "Which can be thought of applying a *rotation about the origin first*,\n",
    "and then a *translation second*. Proving that all rigid transforms have\n",
    "this form will be left as an exercise.\n",
    "\n",
    "It is also possible to interpret rigid transforms as rotation about an\n",
    "arbitrary point. Letting the *center of rotation* be denoted $\\mathbf{c}$, a\n",
    "rotation about $\\mathbf{c}$ can be constructed by translating a point so that\n",
    "$\\mathbf{c}$ is the origin, then rotating about the origin by some matrix\n",
    "$R$, and then translating back to the original origin. This form is:\n",
    "$$T(\\mathbf{x}) = R(\\mathbf{x} - \\mathbf{c}) + \\mathbf{c}.$$ The parenthetical term is the\n",
    "translation to $\\mathbf{c}$ as the origin, the multiplication by $R$ is the\n",
    "rotation about the new origin, and the addition of $\\mathbf{c}$ is the\n",
    "translation back to the original origin. These two representations are\n",
    "related by $\\mathbf{t} = \\mathbf{c} - R\\mathbf{c}$, and $\\mathbf{c}  = (I - R)^{-1} \\mathbf{t}$.\n",
    "\n",
    "The set of rigid transformations is called the *special Euclidean group*\n",
    "$SE(2)$ in 2D, and $SE(3)$ in 3D. Repeated application of rigid\n",
    "transformations also produce a rigid transformation. Given two rigid\n",
    "transforms\n",
    "\n",
    "$$\n",
    "    T_1(\\mathbf{x}) = R_1\\mathbf{x} + \\mathbf{t_1}\n",
    "$$\n",
    "\n",
    "and\n",
    "\n",
    "$$\n",
    "    T_2(\\mathbf{x}) = R_2\\mathbf{x} + \\mathbf{t_2}\n",
    "$$\n",
    "\n",
    "then the composite transform\n",
    "$T_1 \\circ T_2$ is the operation of performing $T_2$ first, then $T_1$.\n",
    "If we let $\\mathbf{y} = T_2(\\mathbf{x})$, and $\\mathbf{z} = T_1(\\mathbf{y})$, then we\n",
    "obtain:\n",
    "\n",
    "$$\n",
    "\\mathbf{z} = T_1(T_2(\\mathbf{x})) = R_1 (R_2\\mathbf{x} + \\mathbf{t_2}) + \\mathbf{t_1}.\n",
    "$$\n",
    "\n",
    "By the\n",
    "distributive property of matrix multiplication, we have\n",
    "\n",
    "$$\n",
    "\\mathbf{z} = R_1 R_2\\mathbf{x} + R_1 \\mathbf{t_2} + \\mathbf{t_1}.\n",
    "$$\n",
    "\n",
    "This is simply a rigid\n",
    "transform with rotation matrix $R_1 R_2$ and translation by\n",
    "$(R_1 \\mathbf{t_2} + \\mathbf{t_1})$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse transformations\n",
    "\n",
    "Not all transformations have inverses, but rotations, translations,\n",
    "rigid transformations, and many linear transformations do. As described\n",
    "before, the inverse of a rotation matrix is simply its transpose.\n",
    "Translations are inverted by translating in the negative direction.\n",
    "Linear transforms $A\\mathbf{x}$ are invertible only if the matrix is\n",
    "invertible, with the inverse transformation $A^{-1}\\mathbf{x}$.\n",
    "\n",
    "Rigid transformations are also invertible, and their inverse is also a\n",
    "rigid transformation: $$\\label{eq:RigidTransformInverse}\n",
    "T_{(R,\\mathbf{t})} ^{-1} = T_{(R^{T},-R^{T} \\mathbf{t})}$$ where\n",
    "$T_{(R,\\mathbf{t})}(\\mathbf{x}) = R \\mathbf{x} + \\mathbf{t}$. Proof of this equation will be\n",
    "left as an exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rigid movement\n",
    "\n",
    "Rigid transformations are used to represent movement of rigid bodies in\n",
    "space. If, in 2D the origin of a body moves by translation $\\mathbf{t}$ in\n",
    "its original reference frame and rotates by angle $R = R(\\theta)$,\n",
    "then the transformation that *converts positional coordinates from the\n",
    "new coordinate frame to the original coordinate frame* is given by\n",
    "$T_p(\\mathbf{x}) = R \\mathbf{x} + \\mathbf{t}$. In other words, if $\\mathbf{x}$ gives the\n",
    "coordinates of a position $P$ that is attached to the body, then after\n",
    "moving, $P$ will have coordinates $T_p(\\mathbf{x})$ relative to the original\n",
    "body's frame. However, the transformation of directional coordinates\n",
    "will simply be a rotation and ignore translation:\n",
    "$T_d(\\mathbf{v}) = R \\mathbf{v}$. In other words, if $\\mathbf{v}$ gives the coordinates\n",
    "of a directional quantity $V$ that is attached to the body (such as the\n",
    "direction of a line attached to the body), then $V$ will have\n",
    "coordinates $T_d(\\mathbf{v})$ relative to the original coordinate frame (as\n",
    "in other directional quantities, these are interpreted ignoring the\n",
    "origin)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Representation of coordinate frames and coordinate transforms\n",
    "\n",
    "Coordinate frames, as well as conversions between them, are interpreted\n",
    "as rigid transformations.\n",
    "\n",
    "Any 2D coordinate frame $F$ with origin $O$ and axes $X$ and $Y$ may be\n",
    "represented by the coordinates of $O$, $X$, and $Y$ in some privileged\n",
    "*world frame*. If $O$ has coordinates $\\mathbf{t}$, and $X$ and $Y$ have\n",
    "(directional) coordinates $\\mathbf{x} = (x_{1},x_{2})$ and\n",
    "$\\mathbf{y} = (y_{1},y_{2})$ relative to $W$, then the world coordinates of\n",
    "any point $P$ such that $\\mathbf{p}$ is its coordinates in the frame $F$ can\n",
    "be calculated by the rigid transform\n",
    "\n",
    "$$\n",
    "  \\mathbf{p}_W = \\begin{bmatrix}x_{1} & y_{1} \\\\ x_{2} & y_{2} \\end{bmatrix} \\mathbf{p} + \\mathbf{t}.\n",
    "$$\n",
    "\n",
    "Hence,\n",
    "by storing $R = \\begin{bmatrix}x_{1} & y_{1} \\\\ x_{2} & y_{2} \\end{bmatrix}$ and $\\mathbf{t}$ we can\n",
    "perform this calculation for any point in $F$. (We remark that the matrix\n",
    "is a rotation because $X$ and $Y$ are both orthogonal and $Y$ is\n",
    "$90^\\circ$ ccw from $X$.) The information stored for a 3D coordinate\n",
    "frame is similarly a rotation matrix $R$ and origin coordinates $\\mathbf{t}$.\n",
    "This operation is known as the *coordinate transform* $A \\rightarrow W$\n",
    "with $A$ the *source* frame and $W$ the *target* frame . We can also\n",
    "perform the reverse coordinate transform from $W \\rightarrow A$ by\n",
    "applying the inverse transform.\n",
    "\n",
    "Changes of coordinate frames can also be represented in terms of rigid\n",
    "transforms. Suppose $A$ and $B$ are two coordinate frames, where $A$ is\n",
    "represented with respect to the world frame by a rotation matrix $R_A$\n",
    "and translation $\\mathbf{t}_A$, and $B$ is represented by $R_B$ and\n",
    "$\\mathbf{t}_B$. Then given the coordinates $\\mathbf{p}^A$ of some point $P$\n",
    "relative to $A$, we can determine $P$'s coordinates relative to\n",
    "$\\mathbf{p}^B$ in two steps. First, we calculate its world coordinates:\n",
    "\n",
    "$$\\mathbf{p}^W = T_A(\\mathbf{p}^A) = R_A \\mathbf{p}^A + \\mathbf{t}_A$$\n",
    "\n",
    "And then we perform the inverse of\n",
    "$B$ coordinates to world coordinate to obtain its coordinates with\n",
    "respect to $B$:\n",
    "\n",
    "$$\\mathbf{p}^B = T_B^{-1}(\\mathbf{p}^W) = R_B^T (\\mathbf{p}^W - \\mathbf{t}_B).$$\n",
    "\n",
    "This transform can be calculated for all points by the composition of\n",
    "the transform from $A \\rightarrow W$ and then $W \\rightarrow B$:\n",
    "$$\\mathbf{p}^B = T_B^{-1}(T_A(\\mathbf{p}^A)) = R_B^T R_A \\mathbf{p}^A + R_B^T(\\mathbf{t}_A - \\mathbf{t}_B).$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Homogeneous coordinate representations\n",
    "\n",
    "Homogeneous coordinates gives a convenient representation of rigid\n",
    "transforms as linear transforms on an expanded space. Moreover, it\n",
    "compactly represents the distinction between positional and directional\n",
    "quantities. The idea is to augment every point with an additional *homogeneous\n",
    "coordinate*, which is 1 if it is positional and 0 if it is directional.\n",
    "This operation is denoted with the hat operator $\\hat{\\cdot}$.\n",
    "\n",
    "For 2D points and directions, we have\n",
    "$$\\hat{\\mathbf{p}} = \\begin{bmatrix} p_x \\\\ p_y \\\\ 1 \\end{bmatrix}$$\n",
    "$$\\hat{\\mathbf{v}} = \\begin{bmatrix} v_x \\\\ v_y \\\\ 0 \\end{bmatrix}$$\n",
    "Then, a 2D rigid\n",
    "transformation of both positions and directions is represented uniformly\n",
    "by a $3\\times 3$ matrix multiplication:\n",
    "$$\\hat{T}(\\hat {\\mathbf{x}}) = \\begin{bmatrix}\n",
    "\\cos \\theta & -\\sin \\theta & t_x \\\\\n",
    "\\sin \\theta & \\cos \\theta & t_y \\\\\n",
    "0 & 0 & 1 \n",
    "\\end{bmatrix} \\hat{\\mathbf{x}}$$\n",
    "where the original transformation\n",
    "$T(\\mathbf{x}) = R(\\theta) \\mathbf{x} + \\mathbf{t}$ is a rotation about angle $\\theta$\n",
    "followed by a translation of vector $\\mathbf{t} = (t_x,t_y)$.\n",
    "\n",
    "In 3D, the hat operator adds a 4th coordinate:\n",
    "For 2D points and directions, we have\n",
    "$$\\hat{\\mathbf{p}} = \\begin{bmatrix} p_x \\\\ p_y \\\\ p_z \\\\ 1 \\end{bmatrix}$$\n",
    "$$\\hat{\\mathbf{v}} = \\begin{bmatrix} v_x \\\\ v_y \\\\ v_z \\\\ 0 \\end{bmatrix}$$\n",
    "and a 3D transform is represented \n",
    "by a $4\\times 4$ matrix multiplication:\n",
    "$$\\hat{T}(\\hat {\\mathbf{x}}) = \\begin{bmatrix}\n",
    "  &   &   &  t_x \\\\\n",
    "  & R &   &  t_y \\\\\n",
    "  &   &   &  t_z \\\\\n",
    "0 & 0 & 0 & 1 \n",
    "\\end{bmatrix} \\hat{\\mathbf{x}}$$\n",
    "\n",
    "Note that when applied to homogeneous positions, the rigid transform is\n",
    "applied to the first two coordinates of the vector while the homogeneous\n",
    "coordinate remains 1 (since the dot product of a position representation\n",
    "with the last row of the matrix is 1). Also, when applied to homogenous\n",
    "directions, only the rotation is applied to the first two coordinates of\n",
    "the vector, since the third 0 coordinate nullifies the effect of the\n",
    "third column. The homoeneous coordinate remains 0, since the dot product\n",
    "of a position representation with the last row of the matrix is 0.\n",
    "\n",
    "The nice thing about this representation is that transform application is a matrix-vector multiply, transform composition is a matrix-matrix multiply, and transform inversion is a matrix inversion. This makes it much easier to write out complex transformations. For example, consider the problem of the coordinate transform from frame $A$ to frame $B$ that we described above.  Rather than writing out the operator expression $T_B^{-1}(T_A(\\mathbf{p}^A))$, using homogeneous coordinates this becomes a series of matrix-matrix and matrix-vector multiplies:\n",
    "\n",
    "$$\\hat{p}^B = \\hat{T}_B^{-1} \\cdot \\hat{T}_A \\cdot \\hat{p}^A$$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Working with coordinate representations of positions and directional quantities\n",
    "-------------------------------------------------------------------------------\n",
    "\n",
    "In small 2D examples we can often treat coordinates casually, with our\n",
    "minds quickly filling in the gaps of meaning. However, once we introduce\n",
    "rotations, or move to 3D and beyond, this type of casual thinking runs\n",
    "the risk of making the robot walk through a wall! It is\n",
    "unlikely that your linear algebra software package (e.g., Matlab, or\n",
    "Numpy in Python) will help you guard against performing invalid\n",
    "operations, since you will be working with vector or matrix datatypes.\n",
    "Nor will they tell you how to properly convert between reference\n",
    "coordinate systems.\n",
    "\n",
    "This section will once again discuss the importance of separating the\n",
    "coordinate representation of an object from its physical meaning. It\n",
    "will also describe some common pitfalls when working with coordinates,\n",
    "and tools to help reduce the risk of making calculation errors."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### An illustrative example\n",
    "\n",
    "Let us start with a running example where a point $P$ is drawn on a\n",
    "sheet of graph paper $G$. Some physical position is taken as the\n",
    "origin $O$ by convention, as are the axes $X$ and $Y$ which are attached\n",
    "to the graph paper. If we were to draw the straight line from $O$ to\n",
    "$P$, suppose the movement of the pen moves $p_x$ units in the $X$\n",
    "direction and $p_y$ units in the $Y$ direction. If we were to move the\n",
    "paper (say, a few inches to the left and rotated 25$^\\circ$), those dots\n",
    "have all moved in physical space, but since they were attached to the\n",
    "paper, the distances between them did not change. Nor did the 90$^\\circ$\n",
    "angle between $X$ and $Y$, nor did the distances from $P$ to the closest\n",
    "positions on the $X$ or $Y$ axes. Hence there are many physical\n",
    "quantities that have not changed, even through the positions themselves\n",
    "have.\n",
    "\n",
    "Let us also suppose that just before moving the paper, we have erased\n",
    "the arrow, and now we need to draw it again. If we recorded precisely\n",
    "how our arm moved between $O$ and $P$ when the line was originally\n",
    "drawn, we can simply reproduce that arm movement (you know\\... like a\n",
    "robot) to draw $P$ again. We can see $O$ at the crossing of the axes, so\n",
    "we would simply need to find the new physical location of position $P$\n",
    "to end the arrow. Luckily, we can still use the values $(p_x,p_y)$ to\n",
    "find $P$ given $O$, $X$, and $Y$. The procedure is simple: start at $O$,\n",
    "then walk $p_x$ squares in the new $X$ direction, and finally walk $p_y$\n",
    "squares in the new $Y$ direction. Hence, we say that $(p_x,p_y)$ are the\n",
    "*coordinates* of the position $P$, relative to origin $O$ and axes $X$\n",
    "and $Y$.\n",
    "\n",
    "More formally, the coordinate frame defined by the physical position $O$\n",
    "and directions $X$ and $Y$ defines a one-to-one correspondence between\n",
    "every point $P$ on the paper and the vector of coordinates\n",
    "$(p_x,p_y) \\in \\mathbb{R}^2$. It would also be desirable to be able to say\n",
    "that $P = O + p_x X + p_y Y$, as this corresponds with our casual\n",
    "understanding of how to \"arrive at\" $P$. However, we have only defined\n",
    "addition and scalar multiplication of vectors, not positions or\n",
    "directions (remember, $O$, $P$, $X$, and $Y$ are not themselves vectors,\n",
    "they are elements of thought with a physical meaning).\n",
    "\n",
    "Luckily, we can do this more formally if we define $O$, $X$, and $Y$\n",
    "with reference so some other *privileged coordinate frame*, which we\n",
    "will call the *world frame* $W$. If we let $\\mathbf{p}^W$, $\\mathbf{o}^W$,\n",
    "$\\mathbf{x}^W$, and $\\mathbf{y}^W$ be the coordinates of $P$, $O$, $X$, and $Y$\n",
    "with respect to $W$, then we indeed have the relationship between their\n",
    "coordinates $\\mathbf{p}^W = \\mathbf{o}^W + p_x \\mathbf{x}^W + p_y \\mathbf{y}^W$. Moreover,\n",
    "$\\mathbf{x}^W \\cdot (\\mathbf{p}^W - \\mathbf{o}^W) = p^x$, and\n",
    "$\\mathbf{y}^W \\cdot (\\mathbf{p}^W - \\mathbf{o}^W) = p^y$. An important fact is that\n",
    "*these equations hold regardless of the choice of world frame*. Hence we\n",
    "can conclude that it is reasonable to casually state that\n",
    "$P = O + p_x X + p_y Y$, $X \\cdot (P - O) = p_x$, and\n",
    "$Y \\cdot (P - O) = p_y$ without regards to choosing a particular value\n",
    "of $W$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Directional quantities\n",
    "\n",
    "Next, let us illustrate how the coordinates of directional quantities\n",
    "transform. As an illustration, let us go back in time and add a second\n",
    "point $Q$ on the paper, and consider the line segment $\\overline{PQ}$.\n",
    "Originally, $\\overline{PQ}$ was drawn by moving a pen in a straight line\n",
    "from $P$ to $Q$, with a diplacement $D$. In the frame of the grid, this\n",
    "stroke moved the pen $d_x$ cells in the $X$ direction and $d_y$ cells in\n",
    "the $Y$ direction, with $(d_x,d_y) = (q_x-p_x, q_y-p_y)$.\n",
    "\n",
    "Once the graph paper has moved, and we wish to draw $\\overline{PQ}$\n",
    "again in the paper's new location, but suppose that now instead of\n",
    "remembering $(q_x, q_y)$ we only remembered the values of $(d_x,d_y)$.\n",
    "To reproduce the movement starting at $P$, the stroke in the new\n",
    "displacement $D$ consists of a movement of $d_x$ units in the new $X$\n",
    "direction and $d_y$ units in the new $Y$ direction.\n",
    "\n",
    "Notice that *the origin has dropped* when performing this exercise,\n",
    "because the choice of origin when defining $(p_x,p_y)$ and $(q_x,q_y)$\n",
    "cancels out when performing the difference. Using the same world\n",
    "coordinates trick as performed above, we can also say in casual\n",
    "shorthand that $D = d_x X + d_y Y$, $D = Q - P$, $d_x = D \\cdot X$, and\n",
    "$d_y = D \\cdot Y$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Associating physical meaning to operations on coordinates\n",
    "\n",
    "The main distinction between positions and directions is that the choice\n",
    "of origin does not affect how directions are represented. However, both\n",
    "are affected by the choice of coordinate axes. Hence, when performing\n",
    "changes of coordinates, the interpretation of position quantities\n",
    "changes in a different manner from that of directional quantities.\n",
    "\n",
    "Positions and directional quantities also transform *into each other*\n",
    "when various operations are performed on them. When two positions are\n",
    "differenced, a displacement is produced. When a displacement is added to\n",
    "a position, another position is obtained. Displacements can be added to\n",
    "other displacements, or multiplied a scalar. However, it is meaningless\n",
    "to multiply a position by a scalar, or to add two position together,\n",
    "*even though these are valid vector operations on their\n",
    "representations*. (In cases where these latter two operations would seem\n",
    "to make sense, it is more appropriate to talk about displacements with\n",
    "respect to some other position.) Directions transform in much the same\n",
    "way that displacements do, but they do not scale or add.\n",
    "\n",
    "In summary, the following mathematical operations on coordinates\n",
    "correspond to meaningful physical operations. The output of these\n",
    "operations (the right hand sides of the equations) may transform the\n",
    "type of object being represented. Here, P indicates a position, D\n",
    "indicates a displacement, V indicates a direction, and c is a scalar.\n",
    "\n",
    "\n",
    "| **Valid operations**                 | **Meaning**                                              |\n",
    "| :-: | :-: |\n",
    "| P - P = D                            | Calculating the displacement between points              |\n",
    "| P + D = P                            | Displacing a point by a displacement                     |\n",
    "| D + D = D                            | Calculating the displacement caused by two displacements |\n",
    "| c D = D                              | Scaling a displacement                                   |\n",
    "| D/‖D‖  = V if D ≠ 0                   | Finding the direction of a displacement                  |\n",
    "| c V = D                              | Scaling a direction to obtain a displacement             |\n",
    "\n",
    "    \n",
    "Here, it is assumed that all items share the same coordinate frame.\n",
    "Operations on items not sharing the same coordinate frame are not\n",
    "meaningful, nor are the following types of operations:\n",
    "\n",
    "| **Invalid operations** | **Meaning**                   |\n",
    "| :-: | :-: |\n",
    "| c P                    | Scaling a point               |\n",
    "| P + P                  | Adding two points             |\n",
    "| V + V                  | Adding two directions         |\n",
    "| P + V                  | Adding a direction to a point |\n",
    "\n",
    "Finally, dot products and norms are meaningful only when performed on\n",
    "directional quantities."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "### Working with coordinates\n",
    "\n",
    "When working with different coordinate representations, it is easy to\n",
    "make mistakes in calculations. Below are some common pitfalls.\n",
    "\n",
    "Mistaken coordinate conventions include:\n",
    "\n",
    "1.  Assuming coordinates are given in a different frame.\n",
    "\n",
    "2.  Incorrectly assuming a matrix represents its inverse.\n",
    "\n",
    "3.  Incorrectly assuming a matrix represents its transpose (row-major vs\n",
    "    column-major).\n",
    "\n",
    "4.  Assuming a coordinate frame is represented with respect to an\n",
    "    incorrect base frame.\n",
    "\n",
    "5.  Applying a coordinate transform to a quantity not represented with\n",
    "    respect to the source frame.\n",
    "\n",
    "6. Operating on the wrong units (e.g., a frame's translation is expressed in *m* while the point's coordinates are expressed in *mm*)\n",
    "\n",
    "To guard against these mistakes, it is common practice to indicate\n",
    "frames of reference with letters and annotate variables with their frame\n",
    "of reference.  The superscript/subscript convention used by this book is\n",
    "popular amongst many authors.  In this convention, we indicate the frame\n",
    "of reference of each coordinate variable in its superscript, e.g. $\\mathbf{p}^A$ denotes a\n",
    "point expressed in frame $A$. A coordinate transform from frame\n",
    "$A$ to frame $B$ is expressed in the form $T_A^B$.  Think about it\n",
    "this way:\n",
    "\n",
    "> $F$ in *superscript*: Coordinates should be interpreted as \"relative to $F$\".\n",
    "\n",
    "> $F$ in *subscript*: Coordinates should be interpreted as \"describing the transform of frame $F$\".\n",
    "\n",
    "In this convention, *superscripts and subscripts must cancel\n",
    "out for the equation to make sense*.  Specifically, the following rules apply:\n",
    "\n",
    "1.  When applying a transform to a coordinate, the subscript of the\n",
    "    transform's source frame must match the point's superscript (reference\n",
    "    frame). For example, $\\mathbf{p}^B = T_A^B\\mathbf{p}^A$ is allowed but\n",
    "    $T_B^A \\mathbf{p}_B$ is illegal.\n",
    "\n",
    "2.  When composing two transforms, the source frame of the second frame\n",
    "    must match the destination of the first frame. For example,\n",
    "    $T_A^C = T_B^C T_A^B$ is\n",
    "    allowed but $T_A^B T_B^C$ is illegal.\n",
    "\n",
    "3.  When inverting a transform, the order of frames swaps: e.g.\n",
    "    $(T_A^B)^{-1} = T_B^A$.\n",
    "   \n",
    "4.  When adding or subtracting coordinates, their superscripts must match!\n",
    "    For example, $\\mathbf{a}^W - \\mathbf{b}^W$ is the displacement between\n",
    "    two points expressed in the world frame, but $\\mathbf{a}^W - \\mathbf{b}^C$\n",
    "    is an illegal operation.\n",
    "\n",
    "By convention, a coordinate without a superscript is assumed to be associated\n",
    "with the world frame $W$: $\\mathbf{p} \\equiv \\mathbf{p}^W$.  Moreover,a\n",
    "coordinate transform $T_A$ is equivalent to a transform\n",
    "from $A$ to $W$: $T_A \\equiv T_A^W$.\n",
    "\n",
    "Matrix expressions are similar to standard expressions regarding real\n",
    "numbers in that addition and subtraction are equivalent, multiplication\n",
    "is nearly equivalent, and inverses give an approximation of division.\n",
    "But, this similarity leads to common pitfalls when manipulating matrix\n",
    "equations. Here are some common mistakes that should be safeguarded\n",
    "against:\n",
    "\n",
    "1.  Performing transformations out of order, or swapping the arguments\n",
    "    of a matrix product (products are not commutative).\n",
    "\n",
    "2.  Propagating transposes or inverses into a matrix product without\n",
    "    swapping the order of arguments.\n",
    "\n",
    "3.  Assuming that a matrix is invertible (or worse, assuming a\n",
    "    non-square matrix is invertible).\n",
    "\n",
    "4.  Performing operations on matrices of incompatible size.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Coordinate management\n",
    "\n",
    "\n",
    "It is essential when working with coordinates to understand 1) which\n",
    "coordinate frame is being used, 2) what unit is being used for\n",
    "measurement, and 3) do the coordinates represent a position or\n",
    "directional quantity? Failure to do so is a frequent source of errors.\n",
    "When we begin discussing forward kinematics, this requires substantial\n",
    "bookkeeping and notation. Moreover, in a production robot, dozens or\n",
    "hundreds of coordinate conventions may be used.\n",
    "\n",
    "***************\n",
    "![fig:ROStf](figures/modeling/ros_tf.png)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:ROSPR2\">Figure 8</a>. The Willow Garage PR2 robot has dozens of coordinate frames for its various links and sensors.</div>\n",
    "\n",
    "***************\n",
    "\n",
    "\n",
    "As a result, some middleware systems come with coordinate management\n",
    "systems. These systems attach an annotation to a coordinate, so that\n",
    "each position or directional quantity consists of a vector $\\mathbf{p}$\n",
    "stored alongside its annotation $A_p$. The annotation may include\n",
    "whether the quantity is positional or directional, as well as an\n",
    "identifier of its coordinate frame. The system will also store a\n",
    "list of coordinate frames, represented in coordinates relative to some\n",
    "privileged frame (usually called the \"world frame\"). Such systems will\n",
    "allow users to query the coordinates of points and directions in\n",
    "arbitrary frames, and to calculate coordinate transformation matrices.\n",
    "With these systems, it becomes somewhat more convenient to maintain and\n",
    "manipulate a large number of quantities and reference frames. Examples\n",
    "include the `tf` module in ROS\n",
    "([Fig. 9](#fig:ROSTf)) and the `coordinates` module in Klamp't.\n",
    "\n",
    "***************\n",
    "\n",
    "![fig:ROSCoordinateManagement](figures/modeling/ros_tf2.jpg)\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:ROSTf\">Figure 9</a>. Coordinate management functionality is provided by the Robot Operating System (ROS) Tf module, showing names and visualizations for the PR2.</div>\n",
    "\n",
    "***************\n",
    "\n",
    "The convenience of coordinate management systems comes at the cost of typically more verbose code and the learning curve of becoming familiar with a new API."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Summary\n",
    "-------\n",
    "\n",
    "Key takeaways:\n",
    "\n",
    "-   Coordinates are numerical representations of geometric concepts,\n",
    "    like points, directions, frames of reference, and movement.\n",
    "\n",
    "-   Points, directions, and displacements in $n$-dimensional space are\n",
    "    represented by $n$-dimensional vectors, while rotations and scalings\n",
    "    are represented by $n\\times n$ matrices.\n",
    "\n",
    "-   Rigid transformations consist of a rotation followed by a\n",
    "    translation. They represent both rigid body movement and changes of\n",
    "    coordinate frame.\n",
    "\n",
    "-   Homogeneous coordinates represent rigid transforms using matrix\n",
    "    multiplication in an $n+1$ dimensional space where the last\n",
    "    coordinate is either 0 or 1.\n",
    "\n",
    "-   When working with coordinates it is easy to make mistakes. Having\n",
    "    clear assumptions, clear notation and/or using coordinate management\n",
    "    software can reduce the risk of error.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercises\n",
    "\n",
    "1. Calculate the matrices representing $+90^\\circ$, $+180^\\circ$, and\n",
    "   $-90^\\circ$ 2D rotations.\n",
    "\n",
    "*  Prove that $R(\\theta)^{-1} = R(-\\theta)$ using linear algebra and\n",
    "   geometric identities.\n",
    "\n",
    "*  Prove that $R(\\theta_1) R(\\theta_2) = R(\\theta_1 + \\theta_2)$ using\n",
    "   geometric identities.\n",
    "\n",
    "*  Give examples of 3D rotation matrices for which\n",
    "   $R_1 R_2 \\neq R_2 R_1$.\n",
    "\n",
    "*  Prove that vector length is preserved under rotation\n",
    "   $\\| R(\\theta) \\mathbf{x} \\| = \\| \\mathbf{x} \\|$. Use this fact to prove that\n",
    "   distance is preserved under rigid transforms.\n",
    "\n",
    "*  Prove that all length-preserving transforms in 2D are either\n",
    "   rotations or rotations followed by a mirroring transform:\n",
    "   \n",
    "   $$\n",
    "   T(\\mathbf{x}) = \\begin{bmatrix} -1 & 0 \\\\ 0 & 1 \\end{bmatrix} \\mathbf{x}.\n",
    "   $$\n",
    "   \n",
    "*  Produce the transform (i.e., rotation matrix $R$ and translation $\\mathbf{t}$) that\n",
    "   rotates any point $\\mathbf{x}$ by $45^\\circ$ about the point $(1,0)$. \n",
    "   \n",
    "   What is the general form of a rotation of angle $\\theta$  about a center point $\\mathbf{c}$?\n",
    "\n",
    "*  A mobile robot has a coordinate convention that $X$ is forward and\n",
    "   $Y$ is to the left. It has an sweeping laser sensor attached to it\n",
    "   that returns distance readings at all angles around the robot using\n",
    "   a spinning mirror. The laser sensor gives perfect depth readings,\n",
    "   and is attached so that the mirror's axis of rotation is centered at\n",
    "   the point $(0.7,0.05)$ in the robot's $X,Y$ frame and oriented so\n",
    "   that the $0^\\circ$ reading is pointing in the robot's $X$ direction.\n",
    "   At time $t$, the laser detects an obstacle at 1 m distance at the\n",
    "   $15^\\circ$ reading. At time $t+1$, the robot moves 0.5 m forward,\n",
    "   0.1 m to the left, and 20$^\\circ$ counter-clockwise. Assuming the\n",
    "   obstacle does not move, at which angle will the sensor detect the\n",
    "   obstacle, and at what distance reading?\n",
    "\n",
    "*  Suppose a 3D camera produces points $(x,y,z)$ in a coordinate frame\n",
    "   that has $+x$ pointing to the right of the image, $+y$ pointing\n",
    "   upward, and $+z$ pointing forward. Is the camera given with a\n",
    "   right-handed or a left handed coordinate system?\n",
    "\n",
    "   Suppose we wish to find the transformation for points detected by\n",
    "   the camera so that that the camera origin is placed at the\n",
    "   coordinates $(1,0,2)$ in the world frame, its forward direction\n",
    "   points in the world's $X$ direction, up in the image points in the\n",
    "   $Z$ direction, and left in the image points in the $Y$ direction.\n",
    "   Give a transformation for converting camera points to world points.\n",
    "\n",
    "*  Prove\n",
    "   ($\\ref{eq:RigidTransformInverse}$), i.e., that\n",
    "   $T_{(R,\\mathbf{t})} ^{-1} = T_{(R^T,-R^T \\mathbf{t})}$.\n",
    "\n",
    "*   The midpoint between positions $P$ and $Q$ is given by the equation\n",
    "    $M = 0.5 * (P+Q)$. But this expression appears to violate the\n",
    "    [assumptions above](#Associating-Physical-Meaning-to-Operations-on-Coordinates),\n",
    "    stating that addition and scalar multiplication with\n",
    "    positions are not meaningful operations. How can one make sense of\n",
    "    this equation in terms of purely meaningful operations?\n",
    "\n",
    "*   Implement a 2D coordinate management system, in your programming\n",
    "    language of choice, where each position must be annotated with a\n",
    "    frame. Your system should define an API to create positions and\n",
    "    frames, and to retrieve coordinates of any position in any given\n",
    "    frame. Frames should be identified with string names. The manager\n",
    "    should maintain a list of named coordinate frames, each of which is\n",
    "    represented in its coordinates relative to the world coordinate\n",
    "    system (call this special frame \"world\"). Define a `Point` data\n",
    "    structure that stores two elements\n",
    "\n",
    "    -   `coords`: the point's coordinates, expressed in terms of its\n",
    "        reference frame.\n",
    "\n",
    "    -   `frame`: the name of its reference frame.\n",
    "\n",
    "    Do the same for a `Directional` data structure. Now, implement methods\n",
    "    `Point.to(frame)` and `Directional.to(frame)` that compute a new `Point`\n",
    "    and `Directional`, respectively, whose reference frames are now the\n",
    "    given frame, and whose coordinates are expressed correctly relative\n",
    "    to the new frame.\n",
    "\n",
    "*   Extend the coordinate management system of exercise 12 to implement\n",
    "    geometric operations, enforcing that each of the items needs to be\n",
    "    of the same reference frame.  Ensure that `Point - Point = Directional`,\n",
    "    `Point + Directional = Directional`, `Directional +/- Directional = Directional`,\n",
    "    and `Directional * scalar = Directional`.\n",
    "    \n",
    "    [This tutorial](https://www.tutorialsteacher.com/python/magic-methods-in-python) for\n",
    "    operator overriding in Python will come in handy here.\n",
    "\n",
    "*   Extend the coordinate management system of exercise 12 to also include\n",
    "    **units** that should be specified to the `Point`, `Directional`, and `Frame`\n",
    "    structure upon construction. Implement support for `'m'`, `'mm'`, `'cm'` and `'km'`\n",
    "    arguments indicating meters, millimeters, centimeters, and kilometers."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Skeleton code for exercise 12\n",
    "\n",
    "class Frame:\n",
    "    #TODO: what here?\n",
    "    pass\n",
    "\n",
    "#a dictionary from strings to Frame objects, to be used in Point.to and Direction.to\n",
    "named_frames = dict()\n",
    "\n",
    "class Point:\n",
    "    def __init__(self,coords,frame):\n",
    "        self.coords = coords\n",
    "        self.frame = frame\n",
    "    def to(self,newframe):\n",
    "        \"\"\"Returns a Point expressing the same physical point in space, but represented in the new frame\"\"\"\n",
    "        #TODO: what here?\n",
    "        return Point(self.coords,newframe)\n",
    "\n",
    "class Directional:\n",
    "    def __init__(self,coords,frame):\n",
    "        self.coords = coords\n",
    "        self.frame = frame\n",
    "    def to(self,newframe):\n",
    "        \"\"\"Returns a Directional expressing the same physical direction in space, but represented in the new frame\"\"\"\n",
    "        #what here?\n",
    "        return Point(self.coords,newframe)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": [
     "remove_cell_html"
    ]
   },
   "source": [
    "## Interactive quiz"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "code_folding": [
     0
    ],
    "tags": [
     "remove_cell_html"
    ]
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "<link rel=\"stylesheet\" href=\"//stackpath.bootstrapcdn.com/font-awesome/4.7.0/css/font-awesome.min.css\"/>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "8453e491da6a4af4a0bc5484f1fa80a4",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(HTML(value='<style>\\n            .correct { background:#8f8;padding:10px 25px 10px 25px;width:4…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#This code must be run from the RoboticSystemsBook folder\n",
    "\n",
    "# If you are running on Google Colab, uncomment the following code:\n",
    "#\n",
    "# %cd ~\n",
    "# !git clone --depth 1 https://github.com/krishauser/RoboticSystemsBook\n",
    "# %cd RoboticSystemsBook\n",
    "\n",
    "from rsbook_code.assessment import quiz\n",
    "quiz.show(\"coordinate_transforms\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": false,
   "autoclose": false,
   "autocomplete": false,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": false,
   "report_style_numbering": false,
   "user_envs_cfg": false
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": true,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {
    "height": "calc(100% - 180px)",
    "left": "10px",
    "top": "150px",
    "width": "335.188px"
   },
   "toc_section_display": true,
   "toc_window_display": true
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}