4 Differential Spatial Mathematics.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$\\large{\\text{Foundations of Kinematics and Algorithms in Robotics}} \\\\ \\large{\\text{Module 4: Differential Spatial Mathematics}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "### Contents\n",
    "\n",
    "[1.0 Rate of Change of Orientation](#1)  \n",
    "[2.0 Rate of Change of Pose](#2)  \n",
    "[3.0 Transforming Spatial Velocities](#3)  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "import spatialmath.base as smb\n",
    "\n",
    "import spatialgeometry as sg\n",
    "\n",
    "from swift import Swift\n",
    "\n",
    "# The matrix exponential method\n",
    "from scipy.linalg import expm, logm\n",
    "\n",
    "# sympy provides symbolic mathematic operations \n",
    "import sympy as sym\n",
    "\n",
    "# setting up sympy to print nicely\n",
    "from IPython import display as ds\n",
    "from sympy.physics.mechanics import init_vprinting\n",
    "from sympy.physics.vector import vlatex\n",
    "init_vprinting(use_latex='mathjax')"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='1'></a>\n",
    "# 1.0 Rate of Change of Orientation\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.1 Derivative of a Rotation and Angular Velocity"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "We can define a rotation matrix as a function of time\n",
    "\n",
    "$$\n",
    "    {^a\\mathbf{R}_b}(t) = e^{[{^a\\hat{\\eta}_b}(t)]_\\times \\ \\theta(t)}\n",
    "$$\n",
    "\n",
    "where ${^a\\hat{\\eta}_b}$ is the rotation axis of frame ${b}$ with respect to frame ${a}$ and $\\theta(t)$ is the rotation angle. While both of these quantities are functions of time, at an instant in time, we will assume that the rotation axis is fixed. The derivative with respect to time is\n",
    "\n",
    "\\begin{align*}\n",
    "    {^a\\dot{\\mathbf{R}}_b}(t) \n",
    "    &= [{^a\\hat{\\eta}_b}]_\\times \\ \\dot{\\theta}(t) \\ e^{[{^a\\hat{\\eta}_b}]_\\times \\ \\theta(t)} \\\\\n",
    "    &= [{^a\\hat{\\eta}_b}]_\\times \\ \\dot{\\theta}(t) \\ {^a\\mathbf{R}_b}(t)\n",
    "\\end{align*}\n",
    "\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where we can introduce the angular velocity of frame ${b}$ with respect to frame ${a}$ as\n",
    "\n",
    "$$\n",
    "    {^a\\omega_b}(t) = {^a\\hat{\\eta}_b} \\ \\dot{\\theta}(t)\n",
    "$$\n",
    "\n",
    "The angular velocity vector represents a similar concept as the Euler vector, it is a vector\n",
    "\n",
    "$$\n",
    "    \\omega = \\begin{pmatrix} \\omega_0 \\\\ \\omega_1 \\\\ \\omega_2 \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "that defines the _instantaneous_ axis and rate of rotation, with $\\hat{\\eta}$ representing the axis and $\\dot{\\theta}$ representing the rate.\n",
    "\n",
    "The derivative of a rotation matrix can then be written as\n",
    "\n",
    "$$\n",
    "    \\dot{\\mathbf{R}}(t) = [{\\omega}(t)]_\\times \\ \\mathbf{R}(t)\n",
    "$$\n",
    "\n",
    "and then rearranged\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\mathbf{R}}(t) \\ \\mathbf{R}(t)^\\top &=  \\ [{\\omega}(t)]_\\times \\\\\n",
    "    \\mathsf{V}_\\times \\Big(\n",
    "        \\dot{\\mathbf{R}}(t) \\ \\mathbf{R}(t)^\\top\n",
    "    \\Big)\n",
    "    &= \\ {\\omega}(t)\n",
    "\\end{align*}\n",
    "\n",
    "_Note that you may see $\\hat{\\eta}(t)$ also written as $\\hat{\\omega}(t)$ in other texts, these are equivalent._\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which shows how we can obtain the angular velocity from the derivative of a rotation matrix."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle [\\mathbf{\\eta}]_\\times = \\left[\\begin{matrix}0 & - \\theta{\\left(t \\right)} & 0\\\\\\theta{\\left(t \\right)} & 0 & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\mathbf{R} = \\left[\\begin{matrix}\\cos{\\left(\\theta{\\left(t \\right)} \\right)} & - \\sin{\\left(\\theta{\\left(t \\right)} \\right)} & 0\\\\\\sin{\\left(\\theta{\\left(t \\right)} \\right)} & \\cos{\\left(\\theta{\\left(t \\right)} \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# A variable for time\n",
    "t = sym.symbols('t')\n",
    "\n",
    "theta = sym.symbols('theta')\n",
    "\n",
    "# Make the angle a functions of time\n",
    "thetat = sym.Function(theta)(t)\n",
    "\n",
    "# Make our rotation axis\n",
    "etahat = np.array([0, 0, 1])\n",
    "\n",
    "# Create skew of eta\n",
    "etahat_sk = smb.skew(etahat)\n",
    "\n",
    "# Create a rotation matrix\n",
    "R = sym.simplify(sym.Matrix(etahat_sk * thetat).exp())\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"[\\\\mathbf{{\\\\eta}}]_\\\\times = {sym.latex(sym.Matrix(eta_sk))}\"))\n",
    "ds.display(ds.Math(f\"\\\\mathbf{{R}} = {sym.latex(sym.Matrix(R))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the next cell, we will take the derivative of $\\mathbf{R}$ using symbolic differentiation from `sympy`. This is not a very tricky operation, it only involves taking the derivative of a few `sin` and `cos` functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{\\mathbf{R}} = \\left[\\begin{matrix}- \\sin{\\left(\\theta \\right)} \\dot{\\theta} & - \\cos{\\left(\\theta \\right)} \\dot{\\theta} & 0\\\\\\cos{\\left(\\theta \\right)} \\dot{\\theta} & - \\sin{\\left(\\theta \\right)} \\dot{\\theta} & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Compute the derivative of the rotation matrix using sympy\n",
    "Rdot = sym.simplify(R.diff(t))\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"\\\\dot{{\\\\mathbf{{R}}}} = {vlatex(sym.Matrix(Rdot))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the next cell, we will attempt the same differentiation using the theory outlined above and show we get the same result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{\\mathbf{R}} = \\left[\\begin{matrix}- \\sin{\\left(\\theta \\right)} \\dot{\\theta} & - \\cos{\\left(\\theta \\right)} \\dot{\\theta} & 0\\\\\\cos{\\left(\\theta \\right)} \\dot{\\theta} & - \\sin{\\left(\\theta \\right)} \\dot{\\theta} & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Compute the derivative using our theory above\n",
    "Rdot = sym.simplify(etahat_sk * sym.diff(thetat, t) * R)\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"\\\\dot{{\\\\mathbf{{R}}}} = {vlatex(sym.Matrix(Rdot))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we will calculate the angular velocity $\\omega$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\omega = \\left[\\begin{matrix}0 & 0 & \\dot{\\theta}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "omega = sym.simplify(smb.vex(Rdot @ R.T))\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"\\\\omega = {vlatex(omega)}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Which gives us a rather intuitive result. In our example, we had a rotation about the $z$-axis and when we took the derivative of $\\mathbf{R}$, we assumed that at that instant in time the axis of rotation is fixed. Therefore, the derivative of the rotation will also be about the $z$-axis. Given that the angular velocity $\\omega$ describes the instantaneous axis and rate of rotation, we can see that the angular velocity is about the $z$-axis and the rate of rotation is $\\dot{\\theta}$. For rotations not about a single axis, the result is not so simple to interpret -- you can try modifying the axis of rotation in the above example to see what happens to the result."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.2 Derivative of a Moving Reference Frame"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Consider now that angular velocity is measured in the moving frame $\\{b\\}$, for example, it is measured by gyroscope sensors onboard a moving vehicle. We know that\n",
    "\n",
    "$$\n",
    "    {^a\\dot{\\mathbf{R}}_b} = {^a\\mathbf{R}_b} \\ [{^b\\omega_\\mathbf{b}}]_\\times \\in \\mathbb{R}^{3 \\times 3}\n",
    "$$\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and we see that the order of the rotation matrix and the skew-symmetric matrix have been swapped."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1.3 Derivative of a Quaternion"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The derivatives of a unit quaternion is given by\n",
    "\n",
    "$$\n",
    "    {^a\\dot{\\mathring{\\mathbf{q}}}_b} = \\frac{1}{2} \\ {^a\\check{\\omega}_b} \\circ {^a\\mathring{\\mathbf{q}}_b}\n",
    "$$\n",
    "\n",
    "or for a moving frame\n",
    "\n",
    "$$\n",
    "    {^a\\dot{\\mathring{\\mathbf{q}}}_b} = \\frac{1}{2} \\ {^a\\mathring{\\mathbf{q}}_b} \\circ {^b\\check{\\omega}_\\mathbf{b}}\n",
    "$$\n",
    "\n",
    "where ${^a\\dot{\\mathring{\\mathbf{q}}}_b} \\in \\mathbb{H}$ is a regular quaternion (not a unit quaternion), and $\\check{\\omega}$ is a pure quaternion formed from the angular velocity vector\n",
    "\n",
    "$$\n",
    "    \\check{\\omega} = \\begin{pmatrix} 0 \\\\ \\omega_x \\\\ \\omega_y \\\\ \\omega_z \\end{pmatrix}\n",
    "$$."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test Your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "Given a rotation matrix $\\mathbf{R} = \\mathbf{R}_x(\\theta(t))$\n",
    "\n",
    "* What is $\\dot{\\mathbf{R}}$?\n",
    "* What is the angular velocity $\\omega$?\n",
    "* What is $\\dot{\\mathring{\\mathbf{q}}}$\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "Given a rotation matrix $\\mathbf{R} = \\mathbf{R}_y(30^\\circ) \\ \\mathbf{R}_y(45^\\circ) \\ \\mathbf{R}_x(\\theta(t))$\n",
    "\n",
    "* What is $\\dot{\\mathbf{R}}$?\n",
    "* What is the angular velocity $\\omega$?\n",
    "* What is $\\dot{\\mathring{\\mathbf{q}}}$\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "What robotic applications can you think where the derivative of a rotation may be used?"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='2'></a>\n",
    "# 2.0 Rate of Change of Pose\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The derivative of pose can be determined by expressing pose as an $\\mathbf{SE}(3)$ matrix\n",
    "\n",
    "$$\n",
    "    {^a\\mathbf{T}_b}(t) = \\begin{pmatrix} {^a\\mathbf{R}_b} & {^a\\mathbf{t}_b} \\\\ \\mathbf{0}_{1 \\times 3} & 1 \\end{pmatrix} \\in \\mathbf{SE}(3)\n",
    "$$\n",
    "\n",
    "and taking the derivative with respect to time, then substituting our previous rotational derivative\n",
    "\n",
    "\\begin{align*}\n",
    "    {^a\\dot{\\mathbf{T}}_b} &= \\begin{pmatrix} {^a\\dot{\\mathbf{R}}_b} & {^a\\dot{\\mathbf{t}}_b} \\\\ \\mathbf{0}_{1 \\times 3} & 0 \\end{pmatrix} \\\\\n",
    "    &= \\begin{pmatrix} [{^a\\omega_b}]_\\times  \\ {^a\\mathbf{R}_b} & {^a\\dot{\\mathbf{t}}_b} \\\\ \\mathbf{0}_{1 \\times 3} & 0 \\end{pmatrix} \\in \\mathbb{R}^{4 \\times 4}\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The rate of change can be described in terms of the current orientation ${^a\\mathbf{R}_b}$ and _two_ velocities. The linear, or translational, velocity ${^a\\mathbf{v}_b} = {^a\\dot{\\mathbf{t}}_b} \\in \\mathbf{R}^3$ is the velocity of the origin of {$b$} with respect to {$a$}. The angular velocity ${^a\\omega_b} \\in \\mathbf{R}^3$ has already been introduced. We can combine these two velocity vectors to create the spatial velocity vector\n",
    "\n",
    "$$\n",
    "    {^a\\mathbf{\\nu}_b} = \\begin{pmatrix} {^a\\mathbf{v}_b} \\\\ {^a\\omega_b} \\end{pmatrix} \\in \\mathbf{R}^6\n",
    "$$"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is the instantaneous velocity of the origin of {$b$} with respect to {$a$}.\n",
    "\n",
    "Every point in the body has the same angular velocity, but the translational velocity of a point depends on its position within the body. It is common to place {$b$} at the body’s center of mass.\n",
    "\n",
    "_Note that in other texts, $\\mathbf{v}$ and $\\omega$ may be swapped._\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test Your Understanding\n",
    "\n",
    "#### Question 1\n",
    "\n",
    "Given a transformation matrix $\\mathbf{T} = \\mathbf{T}_{t_x}(d(t))$\n",
    "\n",
    "* What is $\\dot{\\mathbf{T}}$?\n",
    "* What is the spatial velocty $\\nu$?\n",
    "* What is the translational velocity $\\mathbf{v}$?\n",
    "* What is the angular velocity $\\omega$?\n",
    "\n",
    "#### Question 2\n",
    "\n",
    "Given a transformation matrix $\\mathbf{T} = \\mathbf{T}_{R_y}(30^\\circ) \\ \\mathbf{T}_{t_x}(d(t))$\n",
    "\n",
    "* What is $\\dot{\\mathbf{T}}$?\n",
    "* What is the spatial velocty $\\nu$?\n",
    "* What is the translational velocity $\\mathbf{v}$?\n",
    "* What is the angular velocity $\\omega$?\n",
    "\n",
    "#### Question 3\n",
    "\n",
    "Given a transformation matrix $\\mathbf{T} = \\mathbf{T}_{t_z}(0.1) \\ \\mathbf{T}_{R_y}(30^\\circ) \\ \\mathbf{T}_{t_x}(d(t))$\n",
    "\n",
    "* What is $\\dot{\\mathbf{T}}$?\n",
    "* What is the spatial velocty $\\nu$?\n",
    "* What is the translational velocity $\\mathbf{v}$?\n",
    "* What is the angular velocity $\\omega$?"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Question 4\n",
    "\n",
    "Write a Python function that can calculate the derivatives of the transformations in Questions 1-3\n",
    "\n",
    "#### Question 5\n",
    "\n",
    "For the transformations in Questions 1-3, what is\n",
    "\n",
    "$$\n",
    "    \\dfrac{\\partial \\ \\mathbf{T}(d)}{\\partial \\ d}\n",
    "$$\n",
    "\n",
    "that is to say, what is the derivative of the transformation matrix with respect to the translation parameter $d$?\n",
    "\n",
    "#### Question 6\n",
    "\n",
    "Write a Python function that returns the partial derivative of the transformation in Question 3 by taking the translation parameter $d$ as an argument.\n",
    "\n",
    "#### Question 7\n",
    "\n",
    "Make a new Python function that returns the spatial velocity $\\nu$ of the transformation in Question 3 given the translation parameter $d$ and velocity $\\dot{d}$ as arguments.\n",
    "\n",
    "#### Question 8\n",
    "\n",
    "Can you modify your function from Question 6 such that it returns a vector $\\mathbf{J}$ that satisfies the following equation?\n",
    "\n",
    "\\begin{align*}\n",
    "    \\Phi \\left( \\dfrac{\\partial \\ \\mathbf{T}(d)}{\\partial \\ d} \\right) \\ \\dot{d} &= \\mathbf{\\nu} \\\\\n",
    "    \\mathbf{J}(d) \\ \\dot{d} &= \\mathbf{\\nu}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\Phi$ is a function that converts the partial derivative in matrix form $\\in \\mathbb{R}^{4 \\times 4}$ to a vector $\\mathbf{J} \\in \\mathbb{R}^6$.\n",
    "\n",
    "#### Question 9\n",
    "\n",
    "Research what a Jacobian is and how we may utilise it in robotics. How is a Jacobian related to what we did in Questions 5-8? What system in robotics has several variable transformations and we wish to know the resulting spatial velocity given the transformation coordinate velocities?"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='3'></a>\n",
    "# 3.0 Transforming Spatial Velocities\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"./Figures/4/fig3_1a_point.png\" width=\"400\">"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Figure above shows two fixed frames and a moving coordinate frame. An observer on the blue fixed frame {$b$} observes an object moving with a spatial velocity $^b\\nu$ with respect to frame {$b$}. For an observer on the red frame {$a$}, that frame’s spatial velocity is\n",
    "\n",
    "\\begin{align*}\n",
    "    {^a\\nu} &= \n",
    "    \\begin{pmatrix}\n",
    "        {^a\\mathbf{R}_b} & \\mathbf{0}_{3 \\times 3} \\\\\n",
    "        \\mathbf{0}_{3 \\times 3} & {^a\\mathbf{R}_b}\n",
    "    \\end{pmatrix}\n",
    "    {^b\\nu} \\\\\n",
    "    &= {^a\\mathbf{J}_b}({^a\\mathbf{T}_b}) \\  {^b\\nu}\n",
    "\\end{align*}\n",
    "\n",
    "where ${^a\\mathbf{J}_b}(\\cdot)$ is a Jacobian or interaction matrix which is a function of the relative orientation of the two frames, and independent of the translation between them. We can investigate this numerically, mirroring the setup of the Figure above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "aTb = sm.SE3.Tx(-2) * sm.SE3.Rz(-np.pi/2) * sm.SE3.Rx(np.pi/2)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If the spatial velocity in frame {$b$} is"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [],
   "source": [
    "bv = np.array([1, 2, 3, 4, 5, 6])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "then the spatial velocity in frame {$a$} is"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {^a\\nu} = \\mathtt{\\text{[-3. -1.  2. -6. -4.  5.]}}$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "aJb = aTb.jacob()\n",
    "aJb.shape\n",
    "\n",
    "av = aJb @ bv\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"{{^a\\\\nu}} = {sym.latex(av)}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and the jacob method has returned a $6 \\times 6$ `NumPy` array. We see that the velocities have been transposed, and sometimes negated due to the different orientation of the frames. The $x$-axis translational and rotational velocities in frame {$b$} have been mapped to the $-y$-axis translational and rotational velocities in frame {$a$}, the $y$-axis velocities have been mapped to the $z$-axis, and the $z$-axis velocities have been mapped to the $-x$-axis."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we consider the case where the two frames are rigidly attached, and moving together, as shown in the Figure below"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There is an observer in each frame who is able to estimate their velocity with respect to their own frame. The velocities they observe are related by\n",
    "\n",
    "\\begin{align*}\n",
    "    {^a\\nu} &= \n",
    "    \\begin{pmatrix}\n",
    "        {^a\\mathbf{R}_b} & [{^a\\mathbf{t}_b}] \\ {^a\\mathbf{R}_b} \\\\\n",
    "        \\mathbf{0}_{3 \\times 3} & {^a\\mathbf{R}_b}\n",
    "    \\end{pmatrix}\n",
    "    {^b\\nu} \\\\\n",
    "    &= \\text{Ad}({^a\\mathbf{T}_b}) \\ {^b\\nu}\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which uses the adjoint matrix of the relative pose and this does depend on the translation between the frames. For example, using the relative pose from the previous example, and with frame {$b$} experiencing pure translation, the velocity experienced by the observer in frame {$a$}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {^a\\nu} = \\mathtt{\\text{[-3. -1.  2.  0.  0.  0.]}}$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "av = aTb.Ad() @ [1, 2, 3, 0, 0, 0]\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"{{^a\\\\nu}} = {sym.latex(av)}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "is also pure translation, but the axes have been transposed due to the relative orientation of the two frames. If the velocity in frame {$b$} is pure angular velocity about the $x$-axis"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {^a\\nu} = \\mathtt{\\text{[ 0.  0.  2.  0. -1.  0.]}}$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "av = aTb.Ad() @ [0, 0, 0, 1, 0, 0]\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"{{^a\\\\nu}} = {sym.latex(np.round(av, 2))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "then the observer in frame {$a$} measures that same angular velocity magnitude, but now transposed to be about the $-y$-axis. There is also a translational velocity in the $z$-direction which is due to the origin of fAg following a circular path around the $x$-axis of frame {$b$}. If we combine these translational and rotational velocities in frame {$b$}, then the spatial velocity in frame {$a$} will be"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {^a\\nu} = \\mathtt{\\text{[-3. -1.  4.  0. -1.  0.]}}$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "av = aTb.Ad() @ [1, 2, 3, 1, 0, 0]\n",
    "\n",
    "# Display the result\n",
    "ds.display(ds.Math(f\"{{^a\\\\nu}} = {sym.latex(np.round(av, 2))}\"))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "egh432",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}