2. Pose in 3D.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Representing Orientation in 3D"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3D Rotation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Right Hand Rule\n",
    "![](images/Selection_097.png)\n",
    "### Rotation About Axis\n",
    "![](images/Selection_100.png)\n",
    "\n",
    "**Rotation Axis: The axis that will not change**\n",
    "\n",
    "### Example \n",
    "![](images/Selection_098.png)\n",
    "\n",
    "### Determine rotation axis and angle for this (a is the reference coordinates) \n",
    "![](images/Selection_099.png)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Uncomment next line if pytransform3d is not installed  \n",
    "# !pip install pytransform3d"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt\n",
    "# %matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "from pytransform3d.rotations import *\n",
    "X_AXIS = 0\n",
    "Y_AXIS = 1\n",
    "Z_AXIS = 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 62,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rotate_coordinates(axis, angle):\n",
    "    p = np.array([1.0, 1.0, 1.0])\n",
    "    euler = [0, 0, 0]\n",
    "    vector = [0 ,0 , 0]\n",
    "    vector[axis] = 1\n",
    "    euler[axis] = -angle\n",
    "    R = matrix_from_euler_xyz(euler)\n",
    "    ax = plot_basis(R=np.eye(3), ax_s=2)\n",
    "    plot_basis(ax, R, p)\n",
    "    plot_axis_angle(ax,  np.hstack((vector, (angle,))), p, s=0.5)\n",
    "    plt.show()\n",
    "    return R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 63,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 1.  0.  0.]\n",
      " [ 0.  0.  1.]\n",
      " [ 0. -1.  0.]]\n"
     ]
    }
   ],
   "source": [
    "d  = rotate_coordinates(X_AXIS, -np.pi/2)\n",
    "print(np.round(d))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 64,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.  0.  1.]\n",
      " [ 0.  1.  0.]\n",
      " [-1.  0.  0.]]\n"
     ]
    }
   ],
   "source": [
    "d  = rotate_coordinates(Y_AXIS, np.pi/2)\n",
    "print(np.round(d))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0. -1.  0.]\n",
      " [ 1.  0.  0.]\n",
      " [ 0.  0.  1.]]\n"
     ]
    }
   ],
   "source": [
    "d  = rotate_coordinates(Z_AXIS, np.pi/2)\n",
    "print(np.round(d))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3D Rotation Representation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Rotation Matrix Representation\n",
    "\n",
    "\n",
    "$$R_x(\\theta) = \\begin{bmatrix}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & cos(\\theta) & -sin(\\theta)  \\\\\n",
    "0 & sin(\\theta) & cos(\\theta) \\\\\n",
    "\\end{bmatrix}$$\n",
    "\n",
    "\n",
    "$$R_y(\\theta) = \\begin{bmatrix}\n",
    "cos(\\theta) & 0 & sin(\\theta)  \\\\\n",
    "0 & 1 & 0 \\\\\n",
    "- sin(\\theta) & 0 & cos(\\theta)  \\\\\n",
    "\\end{bmatrix}$$\n",
    "\n",
    "\n",
    "$$R_z(\\theta) = \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & 0 \\\\\n",
    "sin(\\theta) & cos(\\theta) & 0 \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Three-Angle Representations (Euler angles)\n",
    "* Another representation of rotation angles\n",
    "* For example $eurler_{xyz}$\n",
    "$$\n",
    "R = R_x(\\phi)R_y(\\theta)R_z(\\psi)\n",
    "$$\n",
    "\n",
    "**Disadvantage : Gimbal Lock (Loss of singularity)**\n",
    "\n",
    "[Source](https://www.youtube.com/watch?v=zc8b2Jo7mno)\n",
    "![](images/gimbal-lock.gif)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 146,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Eulere XYZ : \n",
      "   [0, 0, 3.141592653589793]\n",
      " \n",
      " Rotation Matrix : \n",
      " [[-1.  0.  0.]\n",
      " [-0. -1.  0.]\n",
      " [ 0.  0.  1.]]\n"
     ]
    }
   ],
   "source": [
    "# Transformation between rotation matrix and euler angles\n",
    "euler_xyz = [0, 0, np.pi]\n",
    "print(\"Eulere XYZ : \\n  \", euler_xyz)\n",
    "R = matrix_from_euler_xyz(euler_xyz)\n",
    "print(' \\n Rotation Matrix : \\n', np.round(R))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0.         -0.          3.14159265]\n"
     ]
    }
   ],
   "source": [
    "R = np.array([[-1,  0,  0], \n",
    "              [-0, -1,  0], \n",
    "              [ 0,  0,  1]])\n",
    "euler_xyz = euler_xyz_from_matrix(R)\n",
    "print(euler_xyz)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Two vectors representation\n",
    "\n",
    "* Approach $\\hat{a}$ and Orientation vector $\\hat{o}$ \n",
    "* Normal vector $\\hat{n} = \\hat{a} \\times \\hat{o}$ is the cross product \n",
    "* Example (OpenGL camera coordinates) \n",
    "    * eye to center (approach0\n",
    "    * up vector (orientation)\n",
    "  \n",
    "\n",
    "$$R = \\begin{bmatrix}\n",
    "\\hat{n}_x & \\hat{o}_x & \\hat{a}_x \\\\\n",
    "\\hat{n}_y & \\hat{o}_y & \\hat{a}_y \\\\\n",
    "\\hat{n}_z & \\hat{o}_z & \\hat{a}_z\n",
    "\\end{bmatrix}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Rotation about arbitrary vector\n",
    "* In form of $( axis[x, y, z]m, angle)$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 100,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Axis, angle : \n",
      "  [0.         1.         1.         1.57079633]\n",
      "\n",
      "The Matrix : \n",
      "\n",
      " [[ 0.    -0.707  0.707]\n",
      " [ 0.707  0.5    0.5  ]\n",
      " [-0.707  0.5    0.5  ]]\n"
     ]
    }
   ],
   "source": [
    "axis = [0, 1, 1]\n",
    "angle = np.pi/2\n",
    "v = np.hstack((axis, (angle,)))\n",
    "print(\"Axis, angle : \\n \", v)\n",
    "M = matrix_from_axis_angle(v)\n",
    "print(\"\\nThe Matrix : \\n\\n\", np.round(M, 3 ))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 5. Quaternions\n",
    "\n",
    "* Advnatages is that \n",
    "    * Easy combination\n",
    "    * Easy interpolation\n",
    "    \n",
    "$$\n",
    "\\dot{q} = s + v\n",
    "$$\n",
    "$$\n",
    "s \\in R\n",
    "$$\n",
    "$$\n",
    "v \\in R^3\n",
    "$$\n",
    "$$\n",
    "\\dot{q} = s + v_1 i +  v_2 j + v_3 k \n",
    "$$\n",
    "$$\n",
    "i^2 = j^2 = k^2 = ijk = -1\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Where in unit quaternion**\n",
    "$$ s = cos(\\frac{\\theta}{2})$$\n",
    "$$ v = sin(\\frac{\\theta}{2}) \\hat{n}$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The Quaternion Represenation : \n",
      "  [0.70710678 0.         0.         0.70710678]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "from pytransform3d.rotations import *\n",
    "\n",
    "\n",
    "angle = np.pi / 2\n",
    "\n",
    "p = np.array([1.0, 1.0, 1.0])\n",
    "\n",
    "ax = [0, 0, 1]\n",
    "a = np.hstack((ax, (angle,)))\n",
    "q = quaternion_from_axis_angle(a)\n",
    "print(\"The Quaternion Represenation : \\n \",q)\n",
    "R = matrix_from_quaternion(q)\n",
    "ax = plot_basis(R=np.eye(3), ax_s=2)\n",
    "plot_axis_angle(ax, a, p, s=0.5)\n",
    "plot_basis(ax, R, p)\n",
    "\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Quaternions Operations**\n",
    "* Quaternions product (Matrix vector multiplication)\n",
    "\n",
    "Hamilton's quaternion multiplication. (It is not right in the reference [link](https://people.csail.mit.edu/bkph/articles/Quaternions.pdf))\n",
    "\n",
    "$$\n",
    "\\hat{q}_1 \\oplus \\hat{q}_2 = s_1 s_2 - v_1 . v_2 , <s_1v_2 + s_2v_1+v_1\\times v_2>\n",
    "$$\n",
    "\n",
    "$$\n",
    "\\hat{q}_1 \\oplus \\hat{q}_2 = \\begin{bmatrix}\n",
    "s & -v_1 & -v_2 & -v_3 \\\\\n",
    "v_1 & s & -v_3 & v_2 \\\\\n",
    "v_2 & v_3 & s & -v_1 \\\\\n",
    "v_3 & -v_2 & v_1 & s \n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "s' \\\\\n",
    "v_1' \\\\\n",
    "v_2'\\\\\n",
    "v_3'\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "* Inversion \n",
    "$$\n",
    "\\ominus \\dot{q} = s, <-v>\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 136,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0. 0. 0. 1.]\n"
     ]
    }
   ],
   "source": [
    "# Example Multiplication\n",
    "s, v1, v2, v3 = q[0], q[1], q[2], q[3]\n",
    "q_matrix = np.array([[s,  -v1,  -v2, -v3],\n",
    "                     [v1, s, -v3, v2],\n",
    "                     [v2, v3,  s, -v1],\n",
    "                     [v3, -v2, v1, s]])\n",
    "q_square = q_matrix.dot(q)\n",
    "print(np.round(q_square, 3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 137,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0. 0. 0. 1.]\n"
     ]
    }
   ],
   "source": [
    "new_q = concatenate_quaternions(q,q)\n",
    "print(np.round(new_q, 3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 145,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0.707 -0.    -0.    -0.707]\n"
     ]
    }
   ],
   "source": [
    "# Example Inversion\n",
    "q_inv = np.hstack((q[0], (-q[1:])))\n",
    "print(np.round(q_inv, 3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Comparison Between different 3D Rotation Representations "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "| Representation | Combine Transformations | Interpolation | AVoid Loss Singularity (Gimbal lock)|\n",
    "|-------|----------------|--------------| ------|\n",
    "|Rotation Matrix|$\\checkmark$  | $\\times$ |   $\\checkmark$  |              \n",
    "|Euler | $\\times$  | $\\checkmark$ |   $\\times$  |    \n",
    "|Two vectors|  $\\times$  | $\\checkmark$ |   $\\checkmark$  |    \n",
    "|Vector and angle| $\\times$  | $\\checkmark$ |   $\\checkmark$  |    \n",
    "|**Quaternion** | $\\checkmark$  | $\\checkmark$ |   $\\checkmark$  |    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Pose in 3D \n",
    "Pose in 3D is represented in position (translation) and Orientation (Quaternion rotation)\n",
    "\n",
    "$$\n",
    "\\xi = (t, \\dot{q})\n",
    "$$\n",
    "\n",
    "Or Using transformation matrix \n",
    "\n",
    "$$\\xi = T_{4 \\times 4} = \\begin{bmatrix}\n",
    "R_{3 \\times 3}& t_{3 \\times 1}\\\\\n",
    "0_{3 \\times 1}  & 1\n",
    "\\end{bmatrix}$$\n",
    "\n",
    "**Same Algebric Rules**\n",
    "\n",
    "$$\\xi \\oplus  0 = \\xi$$\n",
    "$$\\xi  \\ominus  0 = \\xi$$\n",
    "$$ \\ominus X_{\\xi_Y} = Y_{\\xi_X}$$\n",
    "$$  X_{\\xi_Y} \\oplus Y_{\\xi_Z}  = X_{\\xi_Z}$$\n",
    "$$  \\xi_1 \\oplus \\xi_2  \\neq \\xi_2 \\oplus \\xi_1$$\n",
    "$$ X_p = X_{\\xi_Y} . Y_p$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Helper functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 154,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.core.debugger import set_trace"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 292,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_pose_matrix(euler, p, lim = 2, s = 1, show = True):\n",
    "    # Euler uses left hand but we use right hand\n",
    "    euler = -1*np.array(euler)\n",
    "    R = matrix_from_euler_xyz(euler)\n",
    "#     set_trace()\n",
    "    T = np.hstack((R, np.reshape(p,(3,1))))\n",
    "    T = np.vstack((T, np.array([0, 0, 0, 1])))\n",
    "    if show:\n",
    "        ax = plot_basis(R=np.eye(3), ax_s=2, s = s)\n",
    "        ax.set_xlim((-lim,lim))\n",
    "        ax.set_ylim((-lim, lim))\n",
    "        ax.set_zlim((-lim, lim))\n",
    "        plot_basis(ax, R, p,s = s )\n",
    "        plt.show()\n",
    "   \n",
    "    return T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "## Exercise"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex1\n",
    "For the given figure, find the pose of $A$ relative to $B$, and the pose of $B$ relative to $C$ as\n",
    "homogeneous transformation matrices. Compose the two poses to get the pose of $A$\n",
    "relative to $C$.\n",
    "\n",
    "![](images/exercise/Selection_097.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Solution**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. $B_{\\xi_A}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 293,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-1. -0.  0.  3.]\n",
      " [ 0. -1.  0.  0.]\n",
      " [ 0.  0.  1.  0.]\n",
      " [ 0.  0.  0.  1.]]\n"
     ]
    }
   ],
   "source": [
    "p = np.array([3, 0, 0])\n",
    "euler_xyz = [0, 0, 0]\n",
    "angle = np.pi\n",
    "euler_xyz[Z_AXIS] = angle\n",
    "B2A = get_pose_matrix(euler_xyz, p)\n",
    "print(np.round(B2A, 3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2. $C_{\\xi_B}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 274,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.    -0.    -1.     2.   ]\n",
      " [ 0.5   -0.866  0.     0.   ]\n",
      " [-0.866 -0.5    0.     0.   ]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "p = np.array([2, 0, 0])\n",
    "euler_xyz = [0, 0, 0]\n",
    "x_angle = -60*np.pi/180\n",
    "y_angle = -90*np.pi/180\n",
    "z_angle = 90*np.pi/180\n",
    "euler_xyz[X_AXIS] = x_angle\n",
    "euler_xyz[Y_AXIS] = y_angle\n",
    "euler_xyz[Z_AXIS] = z_angle\n",
    "C2B = get_pose_matrix(euler_xyz, p)\n",
    "print(np.round(C2B, 3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3. $C_{\\xi_A}$\n",
    "\n",
    "$C_{\\xi_A} = C_{\\xi_B} B_{\\xi_A} $\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 275,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-0.     0.    -1.     2.   ]\n",
      " [-0.5    0.866  0.     1.5  ]\n",
      " [ 0.866  0.5    0.    -2.598]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "C2A = C2B.dot(B2A)\n",
    "print(np.round(C2A, 3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 276,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-0.     0.    -1.     2.   ]\n",
      " [-0.5    0.866  0.     1.5  ]\n",
      " [ 0.866  0.5    0.    -2.598]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "tm = TransformManager()\n",
    "tm.add_transform(\"A\", \"B\", B2A)\n",
    "tm.add_transform(\"B\", \"C\", C2B)\n",
    "C2A = tm.get_transform(\"A\", \"C\")\n",
    "# In the library He uses left hand coordinate \n",
    "# But we use right hand coord to get pose So order is reversed\n",
    "# And this gives the same expected results\n",
    "# Very important Note \n",
    "print(np.round(C2A, 3))\n",
    "ax = tm.plot_frames_in(\"B\", s=2)\n",
    "lim = 3\n",
    "ax.set_xlim((-lim,lim))\n",
    "ax.set_ylim((-lim, lim))\n",
    "ax.set_zlim((-lim, lim))\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex2 \n",
    "![](images/exercise/Selection_098.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Solution**\n",
    "$$\n",
    "A_p = A_{\\xi_B} B_p\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 300,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 9.1  12.56  0.    1.  ]\n"
     ]
    }
   ],
   "source": [
    "euler_xyz = [0, 0, 30 * np.pi/180]\n",
    "t = [10, 5, 0]\n",
    "# In Homogenous coord\n",
    "Bp = [3, 7, 0, 1]\n",
    "A2B = get_pose_matrix(euler_xyz, t, show=False )\n",
    "Ap = A2B.dot(Bp)\n",
    "print(np.round(Ap, 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex3\n",
    "![](images/exercise/Selection_099.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Solution**\n",
    "\n",
    "$$T = R_x(\\phi) R_z(\\theta) = \\begin{bmatrix}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & cos(\\phi) & -sin(\\phi)  \\\\\n",
    "0 & sin(\\phi) & cos(\\phi) \\\\\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & 0 \\\\\n",
    "sin(\\theta) & cos(\\theta) & 0 \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 331,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.8660254  0.5        1.41421356]\n"
     ]
    }
   ],
   "source": [
    "euler_zyx = [30*np.pi/-180, 0, 45*np.pi/-180]\n",
    "T = matrix_from_euler_zyx(euler_zyx)\n",
    "p = [1, 1, 1]\n",
    "Ap = T.dot(p)\n",
    "print(Ap)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex4\n",
    "![](images/exercise/Selection_100.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Solution**\n",
    "$$A_{\\xi_B} = R_x(\\phi) R_z(\\theta) = \\begin{bmatrix}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & cos(\\phi) & -sin(\\phi)  \\\\\n",
    "0 & sin(\\phi) & cos(\\phi) \\\\\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & 0 \\\\\n",
    "sin(\\theta) & cos(\\theta) & 0 \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}$$\n",
    "\n",
    "$$\n",
    "A_p = A_{\\xi_B} B_p\n",
    "$$\n",
    "\n",
    "So \n",
    "$$\n",
    "B_p = A_{\\xi_B}^{-1} A_p\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 332,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 7.56217783 -0.63775643  0.63775643]\n"
     ]
    }
   ],
   "source": [
    "A2B = matrix_from_euler_zyx([30*np.pi/-180, 0, 45*np.pi/-180])\n",
    "Ap = np.array([7, 3, 0]).T\n",
    "Bp = np.linalg.inv(A2B).dot(Ap)\n",
    "A2B = np.hstack((A2B,np.array([0,0,0]).reshape(3,1)))\n",
    "A2B = np.vstack((A2B,np.array([0,0,0,1]).reshape(1,4)))\n",
    "tm = TransformManager()\n",
    "tm.add_transform(\"B\", \"A\", A2B)\n",
    "ax = tm.plot_frames_in(\"A\", s=2)\n",
    "print(Bp)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex5\n",
    "![](images/exercise/Selection_101.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 339,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A2B : \n",
      " [[ 0.9330127   0.0669873   0.35355339]\n",
      " [ 0.0669873   0.9330127  -0.35355339]\n",
      " [-0.35355339  0.35355339  0.8660254 ]]\n",
      "\n",
      "Ap: \n",
      " [1.0669873  1.9330127  0.35355339]\n"
     ]
    }
   ],
   "source": [
    "axis = [0.7070, 0.7070, 0]\n",
    "angle = 30*np.pi/180\n",
    "v = np.hstack((axis, (angle,)))\n",
    "A2B = matrix_from_axis_angle(v)\n",
    "print(\"A2B : \\n\", A2B)\n",
    "Bp = np.array([1, 2, 0]).T\n",
    "Ap = A2B.dot(Bp)\n",
    "print(\"\\nAp: \\n\", Ap)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex6 \n",
    "1. Get rotation matrix of rotation angle $\\theta = 30$ around Z then around X with angle $\\theta = 30$\n",
    "1. Get rotation matrix of rotation angle $\\theta = 30$ around X then around Z with angle $\\theta = 30$\n",
    "\n",
    "**Similar ??**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 353,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Rzx : \n",
      " [[ 0.8660254 -0.4330127  0.25     ]\n",
      " [ 0.5        0.75      -0.4330127]\n",
      " [ 0.         0.5        0.8660254]]\n",
      "\n",
      "Rxz : \n",
      " [[ 0.8660254 -0.5        0.       ]\n",
      " [ 0.4330127  0.75      -0.5      ]\n",
      " [ 0.25       0.4330127  0.8660254]]\n"
     ]
    }
   ],
   "source": [
    "angle = 30*np.pi/-180\n",
    "euler_zyx = [angle, 0, angle] \n",
    "euler_xyz = [angle, 0, angle]\n",
    "\n",
    "Rzx = matrix_from_euler_zyx(euler_zyx)\n",
    "Rxz = matrix_from_euler_xyz(euler_xyz)\n",
    "print('Rzx : \\n', Rzx)\n",
    "print('\\nRxz : \\n', Rxz)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 357,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.axes._subplots.Axes3DSubplot at 0x7f7582dc7b50>"
      ]
     },
     "execution_count": 357,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "ax = plot_basis(R=Rxz, ax_s=2, s = 2)\n",
    "plot_basis(ax, Rzx, s = 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ex7\n",
    "For the given figure, find the pose of $A$ relative to $B$, and the pose of $B$ relative to $C$ as\n",
    "homogeneous transformation matrices. Compose the two poses to get the pose of $A$\n",
    "relative to $C$.\n",
    "\n",
    "![](images/exercise/Selection_103.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Solution**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. $B_{\\xi_A}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 358,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-1. -0.  0.  0.]\n",
      " [ 0. -0. -1.  2.]\n",
      " [ 0. -1.  0.  4.]\n",
      " [ 0.  0.  0.  1.]]\n"
     ]
    }
   ],
   "source": [
    "p = np.array([0, 2, 4])\n",
    "angle_x = np.pi/2\n",
    "angle_y = 0\n",
    "angle_z = np.pi\n",
    "euler_xyz = [angle_x, angle_y, angle_z]\n",
    "B2A = get_pose_matrix(euler_xyz, p)\n",
    "print(np.round(B2A, 3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2. $C_{\\xi_B}$\n",
    "\n",
    "It is simpler to get $B_{\\xi_C}$ and $C_{\\xi_B} =  B_{\\xi_C}^{-1}$ "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 376,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-0.8    0.    -0.6    2.399]\n",
      " [-0.6   -0.     0.8    1.801]\n",
      " [ 0.     1.     0.    -0.   ]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "p = np.array([3, 0, 0])\n",
    "x_angle = -90*np.pi/180\n",
    "y_angle = 0\n",
    "z_angle = (180+36.9)*np.pi/-180\n",
    "euler_xyz[X_AXIS] = x_angle\n",
    "euler_xyz[Y_AXIS] = y_angle\n",
    "euler_xyz[Z_AXIS] = z_angle\n",
    "B2C = get_pose_matrix(euler_xyz, p, s = 3)\n",
    "C2B = np.linalg.inv(B2C)\n",
    "print(np.round(C2B, 3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3. $C_{\\xi_A}$\n",
    "\n",
    "$C_{\\xi_A} = C_{\\xi_B} B_{\\xi_A} $\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 374,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.8  0.6 -0.  -0. ]\n",
      " [ 0.6 -0.8  0.   5. ]\n",
      " [ 0.  -0.  -1.   2. ]\n",
      " [ 0.   0.   0.   1. ]]\n"
     ]
    }
   ],
   "source": [
    "C2A = C2B.dot(B2A)\n",
    "print(np.round(C2A, 2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 380,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.8  0.6 -0.  -0. ]\n",
      " [ 0.6 -0.8  0.   5. ]\n",
      " [ 0.  -0.  -1.   2. ]\n",
      " [ 0.   0.   0.   1. ]]\n"
     ]
    }
   ],
   "source": [
    "tm = TransformManager()\n",
    "tm.add_transform(\"A\", \"B\", B2A)\n",
    "tm.add_transform(\"B\", \"C\", C2B)\n",
    "C2A = tm.get_transform(\"A\", \"C\")\n",
    "# In the library He uses left hand coordinate \n",
    "# But we use right hand coord to get pose So order is reversed\n",
    "# And this gives the same expected results\n",
    "# Very important Note \n",
    "print(np.round(C2A, 2))\n",
    "ax = tm.plot_frames_in(\"B\", s=3)\n",
    "lim = 4\n",
    "ax.set_xlim((-lim,lim))\n",
    "ax.set_ylim((-lim, lim))\n",
    "ax.set_zlim((-lim, lim))\n",
    "plt.show()"
   ]
  }
 ],
 "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.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}