01_Stewart_Py_Inverse_Kinematics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "[<img align=\"right\" src=\"./doc/jupyter_nb/Hexapod_general_Anim.gif\" width=\"10%\" height=\"10%\">](./doc/jupyter_nb/Hexapod_general_Anim.gif)\n",
    " \n",
    "## Using Stewart Platform as a tool to understand key robotic concepts\n",
    "\n",
    "The most basic configuration of Stewart Platforms uses 6 legs, each connecting the **base** to the **platform**. They are arranged in 3 pairs, connected with universal joints and actuated linearly. The locations where the legs connect to the base and platforms are called **anchors**.\n",
    "\n",
    "In this tutorial you will learn to solve the inverse kinematics of stewart platforms with (1) **linear actuators** (left), then (2) **rotational actuators** (right).\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/plot_linear.png\" width=\"40%\" height=\"40%\">](./doc/jupyter_nb/plot_linear.png)\n",
    "[<img src=\"./doc/jupyter_nb/plot_rotational.png\" width=\"40%\" height=\"40%\">](./doc/jupyter_nb/plot_rotational.png)\n",
    "\n",
    "\n",
    "#### Base and Platform Anchors \n",
    "The basic parameters that decide the mechanical configuration are typically denoted by \n",
    "\n",
    "$r_B$. Radius of Base (Bottom)\n",
    "\n",
    "$r_P$, Radius of Platform (Top) \n",
    "\n",
    "$\\gamma_B$, Half of angle between two anchors on the base\n",
    "\n",
    "$\\gamma_P$, Half of angle between two anchors on the platform\n",
    "\n",
    "\n",
    "From these $\\gamma_B$ and $\\gamma_P$, we can define  $ \\psi_B \\in \\R ^ {6 \\times 1}$ and $ \\psi_P \\in \\R ^ {6 \\times 1}$  the polar coordinates of the anchors on a unit circle radius.\n",
    "\n",
    "Given $r_B$ and $r_P$, we can define  $B \\in \\R ^{6 \\times 3}$ and $P \\in \\R ^{6 \\times 3}$  which are the coordinates of the anchors in their respective local frames in cartesian space. For example, the anchor points on the base, B are illustrated below:\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/base_kinematic_1.png\" width=\"50%\" height=\"50%\">](./doc/jupyter_nb/base_kinematic_1.png)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "6x3 array for Base anchors \n",
      " [[ 6.04108436  6.04108436 -1.8126619  -4.22842247 -4.22842247 -1.8126619 ]\n",
      " [-1.39474002  1.39474002  5.92910253  4.53436252 -4.53436252 -5.92910253]\n",
      " [ 0.          0.          0.          0.          0.          0.        ]]\n",
      "6x1 array for psi_B \n",
      " [-0.2269     0.2269     1.8674951  2.3212951  3.9618902  4.4156902]\n",
      "6x3 array for Platform anchors \n",
      " [[ 4.87150733  4.87150733 -1.46040129 -3.41110604 -3.41110604 -1.46040129]\n",
      " [-1.12623991  1.12623991  4.78196906  3.65572915 -3.65572915 -4.78196906]\n",
      " [ 0.          0.          0.          0.          0.          0.        ]]\n",
      "6x1 array for psi_P \n",
      " [6.05598776 0.22719755 1.86719755 2.32159265 3.96159265 4.41598776]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "r_B, r_P, gamma_B, gamma_P = 6.2, 5, 0.2269, 0.82\n",
    "# 0.2269rad is 13 degrees, which is standard gamma_B for stewart platforms\n",
    "\n",
    "pi = np.pi\n",
    "\n",
    "## Define the Geometry of the platform\n",
    "# Psi_B (Polar coordinates)\n",
    "psi_B = np.array([ \n",
    "    -gamma_B, \n",
    "    gamma_B,\n",
    "    2*pi/3 - gamma_B, \n",
    "    2*pi/3 + gamma_B, \n",
    "    2*pi/3 + 2*pi/3 - gamma_B, \n",
    "    2*pi/3 + 2*pi/3 + gamma_B])\n",
    "\n",
    "# psi_P (Polar coordinates)\n",
    "# Direction of the points where the rod is attached to the platform.\n",
    "psi_P = np.array([ \n",
    "    pi/3 + 2*pi/3 + 2*pi/3 + gamma_P,\n",
    "    pi/3 + -gamma_P, \n",
    "    pi/3 + gamma_P,\n",
    "    pi/3 + 2*pi/3 - gamma_P, \n",
    "    pi/3 + 2*pi/3 + gamma_P, \n",
    "    pi/3 + 2*pi/3 + 2*pi/3 - gamma_P])\n",
    "\n",
    "# Coordinate of the points where servo arms \n",
    "# are attached to the corresponding servo axis.\n",
    "B = r_B * np.array( [ \n",
    "    [ np.cos(psi_B[0]), np.sin(psi_B[0]), 0],\n",
    "    [ np.cos(psi_B[1]), np.sin(psi_B[1]), 0],\n",
    "    [ np.cos(psi_B[2]), np.sin(psi_B[2]), 0],\n",
    "    [ np.cos(psi_B[3]), np.sin(psi_B[3]), 0],\n",
    "    [ np.cos(psi_B[4]), np.sin(psi_B[4]), 0],\n",
    "    [ np.cos(psi_B[5]), np.sin(psi_B[5]), 0] ])\n",
    "B = np.transpose(B)\n",
    "    \n",
    "# Coordinates of the points where the rods \n",
    "# are attached to the platform.\n",
    "P = r_P * np.array([ \n",
    "    [ np.cos(psi_P[0]),  np.sin(psi_P[0]), 0],\n",
    "    [ np.cos(psi_P[1]),  np.sin(psi_P[1]), 0],\n",
    "    [ np.cos(psi_P[2]),  np.sin(psi_P[2]), 0],\n",
    "    [ np.cos(psi_P[3]),  np.sin(psi_P[3]), 0],\n",
    "    [ np.cos(psi_P[4]),  np.sin(psi_P[4]), 0],\n",
    "    [ np.cos(psi_P[5]),  np.sin(psi_P[5]), 0] ])\n",
    "P = np.transpose(P)\n",
    "\n",
    "print('6x3 array for Base anchors \\n', B)\n",
    "print('6x1 array for psi_B \\n', psi_B)\n",
    "\n",
    "print('6x3 array for Platform anchors \\n', P)\n",
    "print('6x1 array for psi_P \\n', psi_P)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can plot this using matplotlib\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import seaborn\n",
    "\n",
    "def plot_2D_annotate(X, Y):\n",
    "    X = list(X)\n",
    "    Y = list(Y)\n",
    "    ax.plot(X,Y, 'or')  # Plot Points\n",
    "    # Annotate Points\n",
    "    for i, (xy) in enumerate(zip(X, Y)):                                 \n",
    "        ax.annotate('Point ' + str(i) + '\\n(%.5s, %.5s)' % xy, xy=xy, textcoords='data')\n",
    "\n",
    "    X.append(X[0])\n",
    "    Y.append(Y[0])\n",
    "    ax.plot(X,Y)    # Plot lines\n",
    "\n",
    "seaborn.set(style='ticks')\n",
    "# plt.style.use('dark_background')\n",
    "\n",
    "fig = plt.figure(figsize=(14, 6), dpi=80)\n",
    "\n",
    "ax = fig.add_subplot(121)\n",
    "ax.set_title('Base Anchors')\n",
    "plot_2D_annotate(B[0,:], B[1,:])\n",
    "circle_B = plt.Circle((0, 0), r_B, color='g', fill=False)\n",
    "ax.add_patch(circle_B)\n",
    "\n",
    "ax = fig.add_subplot(122)\n",
    "ax.set_title('Platform Anchors')\n",
    "plot_2D_annotate(P[0,:], P[1,:])\n",
    "circle_r = plt.Circle((0, 0), r_P, color='g', fill=False)\n",
    "ax.add_patch(circle_r)\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Defining Home Position\n",
    "\n",
    "The next step is to define the displacement between the base and the platform at home position, $h$. This is the length of your linear actuators at rest. We'll assume it's radius of your base plate.\n",
    "\n",
    "We also need to define the rotation matrices following the standard notation\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rotation_matrices.png\">](./doc/jupyter_nb/rotation_matrices.png)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# # Definition of the platform home position.\n",
    "home_pos= np.array([0, 0, 2*r_B])\n",
    "\n",
    "# Allocate for variables\n",
    "l = np.zeros((3,6))\n",
    "lll = np.zeros((6))\n",
    "\n",
    "# Rotation matrices used later\n",
    "def rotX(theta):\n",
    "    rotx = np.array([\n",
    "        [1,     0    ,    0    ],\n",
    "        [0,  np.cos(theta), -np.sin(theta)],\n",
    "        [0,  np.sin(theta), np.cos(theta)] ])\n",
    "    return rotx\n",
    "\n",
    "def rotY(theta):    \n",
    "    roty = np.array([\n",
    "        [np.cos(theta), 0,  np.sin(theta) ],\n",
    "        [0         , 1,     0       ],\n",
    "        [-np.sin(theta), 0,  np.cos(theta) ] ])   \n",
    "    return roty\n",
    "    \n",
    "def rotZ(theta):    \n",
    "    rotz = np.array([\n",
    "        [ np.cos(theta),-np.sin(theta), 0 ],\n",
    "        [ np.sin(theta), np.cos(theta), 0 ],\n",
    "        [   0        ,     0      , 1 ] ])   \n",
    "    return rotz\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Solving for Inverse Kinematics using Linear Actuators\n",
    "\n",
    "Finally we are ready to solve the inverse kinematics.\n",
    "\n",
    "To find length of legs when given desired translation vectors $T = (t _x, t _y, t _z)^T$ and rotation vector $ \\theta = (\\theta _x, \\theta _y, \\theta _z)^T$ ,\n",
    "\n",
    "We just have to find the new coordinates of each of the anchors on the plate after desired rotation rot and translation.\n",
    "\n",
    "Since the role of each leg is to connect the anchor on the base to the anchor on the platform, the desired vector for each leg (direction and length) is simply the leg's position in 3D space with reference to their respective base anchor\n",
    "\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/leg_math.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/leg_math.png)\n",
    "\n",
    "\n",
    "Where $T$ and $H$ are in $\\R ^ {3 \\times 1}$ replicated 6 times to have dimensions $\\R ^ {3 \\times 6}$ to facilitate matrix calculations\n",
    "\n",
    "This can be understood as \n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/leg_maths.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/leg_maths.png)\n",
    "\n",
    "The length of a leg is simply the magnitude of the leg vector \n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/leg_length_mag.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/leg_length_mag.png)\n",
    "\n",
    "\n",
    "\n",
    "To get the position of the leg with reference to global frame (center of base), we just add the displacement of their respective base anchors\n",
    "\n",
    "And that's all to solve for the inverse kinematics of stewart platforms when using linear actuators. Mad simple.\n",
    "\n",
    "\n",
    "!!! P should be small caps\n",
    "\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/legs_only_pseudocode.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/legs_only_pseudocode.png)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Leg lengths to command in order to achieve desired position of plate: \n",
      " [12.78660249 12.78732386 12.78660249 12.78732386 12.78660249 12.78732386]\n"
     ]
    }
   ],
   "source": [
    "# Given input trans, rotation\n",
    "trans = np.transpose(np.array([0,0,0])) # X, Y, Z\n",
    "rotation = np.transpose(np.array([0,0,pi/6])) # X, Y, Z\n",
    "\n",
    "# Get rotation matrix of platform. RotZ* RotY * RotX -> matmul\n",
    "# R = np.matmul( np.matmul(rotZ(rotation[2]), rotY(rotation[1])), rotX(rotation[0]) )\n",
    "R = np.matmul( np.matmul(rotX(rotation[0]), rotY(rotation[1])), rotZ(rotation[2]) )\n",
    "\n",
    "# Get leg length for each leg\n",
    "# leg = np.repeat(trans[:, np.newaxis], 6, axis=1) + np.repeat(home_pos[:, np.newaxis], 6, axis=1) + np.matmul(np.transpose(R), P) - B \n",
    "l = np.repeat(trans[:, np.newaxis], 6, axis=1) + np.repeat(home_pos[:, np.newaxis], 6, axis=1) + np.matmul(R, P) - B \n",
    "lll = np.linalg.norm(l, axis=0)\n",
    "\n",
    "# Position of leg in global frame\n",
    "L = l + B\n",
    "\n",
    "print('Leg lengths to command in order to achieve desired position of plate: \\n', lll)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Plotting for Visualization\n",
    "\n",
    "Having these leg lengths, we can plot the platform!\n",
    "\n",
    "You should see an interactive plot like this pop out on a separate window\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/plot_linear.png\" width=\"30%\" height=\"30%\">](./doc/jupyter_nb/plot_linear.png)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "from mpl_toolkits.mplot3d.art3d import Poly3DCollection\n",
    "\n",
    "def plot3D_line(ax, vec_arr_origin, vec_arr_dest, color_):\n",
    "    for i in range(6):\n",
    "        ax.plot([vec_arr_origin[0, i] , vec_arr_dest[0, i]],\n",
    "        [vec_arr_origin[1, i], vec_arr_dest[1, i]],\n",
    "        [vec_arr_origin[2, i],vec_arr_dest[2, i]],\n",
    "        color=color_)\n",
    "\n",
    "#interactive plotting in separate window\n",
    "# Comment out this and restart kernel if you're having issues with interactive plot (qt)\n",
    "# %matplotlib qt\n",
    "\n",
    "ax = plt.axes(projection='3d') # Data for a three-dimensional line\n",
    "ax.set_xlim3d(-10, 10)\n",
    "ax.set_ylim3d(-10, 10)\n",
    "ax.set_zlim3d(0, 20)\n",
    "\n",
    "# ax.add_collection3d(Poly3DCollection([list(np.transpose(s.B))]), zs='z')\n",
    "ax.add_collection3d(Poly3DCollection([list(np.transpose(B))], facecolors='green', alpha=0.25))\n",
    "\n",
    "# ax.add_collection3d(base_plot, zs='z')\n",
    "ax.add_collection3d(Poly3DCollection([list(np.transpose(L))], facecolors='blue', alpha=0.25))\n",
    "\n",
    "plot3D_line(ax, B, L, 'orange')\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse Kinematics with Rotational Servos\n",
    "\n",
    "What if instead of linear motors, we are to use rotational servos? To do this we replace the leg of a linear actuator with a rotating servo horn and a rod. From this we can work out a (considerably more complex) analytical solution as follows.  \n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rod_kinematics_1.png\" width=\"90%\" height=\"90%\">](./doc/jupyter_nb/rod_kinematics_1.png)\n",
    "\n",
    "The most obvious variable is $H_k$. It is simply $h_k$ translated by $B_k$ to the global reference frame; whereas $h_k$ is simply the servo arm of length |h| lying on the x axis, rotated by $\\beta_k$ offset in the z axis, then rotated by desired angle $\\alpha_k$ by the y axis. Note: standard notation of rotation matrices follows a right-hand-rule convention for rotation, hence in the diagram, when we are rotating the servos 'upwards, we are rotating $-\\alpha_k$. Here's a post that helped me understand [link](https://robotics.stackexchange.com/questions/10702/rotation-matrix-sign-convention-confusion)\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rod_kinematics_2.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/rod_kinematics_2.png)\n",
    "\n",
    "From here we can solve the system of equations of consisting of the magnitude of each of these variables in question \n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rod_kinematics_3.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/rod_kinematics_3.png)\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rod_kinematics_4.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/rod_kinematics_4.png)\n",
    "\n",
    "Converting the vector into the form of an equation, we can simplify the variables into three variables, g, e and f. These three variables are not dependent on $\\alpha_k$. The remaining terms can be simplified using trigonometric identity $e \\cdot \\sin \\varphi+f \\cdot \\cos \\varphi=\\sqrt{e^{2}+f^{2}} \\sin (\\varphi+\\operatorname{atan} 2(f, e))$\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rod_kinematics_5.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/rod_kinematics_5.png)\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/rod_kinematics_6.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/rod_kinematics_6.png)\n",
    "\n",
    "and that is it. The final equation the solved inverse kinematic solution"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Coding\n",
    "\n",
    "Now we can transcribe into code. First we have to declare some variables: \n",
    "|d| - rod length \n",
    "|h| - servo horn length \n",
    "From these we initialize a new reasonable home position, home_pos\n",
    "\n",
    "We also need to define the resting orientation of the servo arms\n",
    "beta - angle between xz plane and the servo arms. \n",
    "Inspired by hbartle's stewart platform implementation, we don't declare the orientations of the servo shafts, but directly define the offset angles of the servo shafts, when looking at the platform from above.  \n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/beta_visualized.png\" width=\"60%\" height=\"60%\">](./doc/jupyter_nb/beta_visualized.png)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "ldl = 12\n",
    "lhl = 3\n",
    "z = np.sqrt( ldl**2 + lhl**2 - (P[0] - B[0])**2 - (P[1] - B[1])**2)\n",
    "home_pos= np.array([0, 0, z[0] ])\n",
    "\n",
    "# Beta (Angle)\n",
    "# Angle between the plane in which the servo arm moves and the xz-plane of the base CS.\n",
    "beta = np.array([ \n",
    "    pi/2 + pi,        \n",
    "    pi/2,\n",
    "    2*pi/3 + pi/2 + pi , \n",
    "    2*pi/3 + pi/2,\n",
    "    4*pi/3 + pi/2 + pi , \n",
    "    4*pi/3 + pi/2] )\n",
    "\n",
    "# Allocate arrays for variables\n",
    "angles = np.zeros((6))\n",
    "H = np.zeros((3,6)) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following block calculates the angles to issue on the servo motor, depending on the leg's final positions (platform anchor positions) from the previous section. The pseudocode is as following:\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/servo_horn_pseudocode.png\" width=\"80%\" height=\"80%\">](./doc/jupyter_nb/servo_horn_pseudocode.png)\n",
    "\n",
    "The cell should plot the results in a interactive window as follows, where\n",
    "\n",
    "Orange lines: Legs (virtual in this case)\n",
    "\n",
    "Red lines: Servo Horns, h\n",
    "\n",
    "Black lines: Rods, d\n",
    "\n",
    "\n",
    "[<img src=\"./doc/jupyter_nb/plot_rotational.png\" width=\"30%\" height=\"30%\">](./doc/jupyter_nb/plot_rotational.png)\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.60882972 0.22676629 0.60882972 0.22676629 0.60882972 0.22676629]\n"
     ]
    }
   ],
   "source": [
    "# Position of legs, wrt to their individual bases, split for clarity.\n",
    "lx = l[0, :]\n",
    "ly = l[1, :]\n",
    "lz = l[2, :]\n",
    "\n",
    "# Calculate auxiliary quatities g, f and e\n",
    "g = lll**2 - ( ldl**2 - lhl**2 )\n",
    "e = 2 * lhl * lz\n",
    "\n",
    "# Calculate servo angles for each leg\n",
    "for k in range(6):\n",
    "    fk = 2 * lhl * (np.cos(beta[k]) * lx[k] + np.sin(beta[k]) * ly[k])\n",
    "    \n",
    "    # The wanted position could be achieved if the solution of this\n",
    "    # equation is real for all i\n",
    "    angles[k] = np.arcsin(g[k] / np.sqrt(e[k]**2 + fk**2)) - np.arctan2(fk,e[k])\n",
    "    \n",
    "    # Get postion of the point where a spherical joint connects servo arm and rod.\n",
    "    H[:, k] = np.transpose([ \n",
    "        lhl * np.cos(angles[k]) * np.cos(beta[k]) + B[0,k],\n",
    "        lhl * np.cos(angles[k]) * np.sin(beta[k]) + B[1,k],\n",
    "        lhl * np.sin(angles[k]) ])\n",
    "\n",
    "print(angles)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plotting:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "#interactive plotting in separate window\n",
    "%matplotlib qt \n",
    "\n",
    "ax = plt.axes(projection='3d') # Data for a three-dimensional line\n",
    "\n",
    "ax.set_xlim3d(-10, 10)\n",
    "ax.set_ylim3d(-10, 10)\n",
    "ax.set_zlim3d(0, 20)\n",
    "\n",
    "# ax.add_collection3d(Poly3DCollection([list(np.transpose(s.B))]), zs='z')\n",
    "ax.add_collection3d(Poly3DCollection([list(np.transpose(B))], facecolors='green', alpha=0.25))\n",
    "\n",
    "# ax.add_collection3d(base_plot, zs='z')\n",
    "ax.add_collection3d(Poly3DCollection([list(np.transpose(L))], facecolors='blue', alpha=0.25))\n",
    "\n",
    "plot3D_line(ax, B, H, 'red')\n",
    "plot3D_line(ax, H, L, 'black')\n",
    "plot3D_line(ax, B, L, 'orange')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Condensed python class\n",
    "\n",
    "All the code you see in this tutorial is condensed in my python class that you can find under src/stewart_controller.py.\n",
    "\n",
    "The only changes made between this tutorial and the python class is the inclusion of a reference rotation, which rotates the entire platform in the z-axis to align with your applications. Note: using 5pi/6 as reference rotation, you'll find that the longer edge of the top platform now aligns with the X axis, which I personally find to me more aesthetically pleasing :>\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.25544982 0.25544982 0.25544982 0.25544982 0.25544982 0.25544982]\n"
     ]
    }
   ],
   "source": [
    "from src.stewart_controller import Stewart_Platform\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "# To calculate angles, initiate object\n",
    "# Stewart_Platform(r_B, r_P, lhl, ldl, Psi_B, Psi_P, ref_rotation)\n",
    "platform = Stewart_Platform(132/2, 100/2, 30, 130, 0.2269, 0.82, 5*np.pi/6)\n",
    "\n",
    "# platform.calculate(np.array[tx, ty, tz],np.array[rx, ry, rz])\n",
    "servo_angles = platform.calculate( np.array([0,0,0]), np.array([0, 0, 0]) )\n",
    "print(servo_angles)\n",
    "\n",
    "# To Plot\n",
    "%matplotlib qt \n",
    "fig, ax = plt.subplots()\n",
    "ax = platform.plot_platform()\n",
    "plt.draw()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Final Words\n",
    "\n",
    "Congratulations you've learnt how to solve the inverse kinematics of a standard stewart platform! You are encouraged to change some numbers, view the code in debugging mode to access the individual variables to gain a better understanding of what is going on.\n",
    "\n",
    "This notebook functions as a tutorial for learners to solve for the inverse kinematics. A compact python function to solve for inverse kinematics is available in this same repo.\n",
    "\n",
    "### Next Steps\n",
    "I made use of this IK solution to build a 6DOF Motion Simulator - all the resources to recreate it is provided - head over there to build your own at [Stewart Platform Motion Simulator](https://github.com/Yeok-c/Stewart_Platform_Motion_Simulator)! \n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "### Credits\n",
    "Special thanks to my subject lecturer Dr. David Navarro-Alarcon for the helpful discussions and advises.\n",
    "\n",
    "This particular IK solution was a combination of:\n",
    "Robert Eisele's solution [link](https://www.xarg.org/paper/inverse-kinematics-of-a-stewart-platform/)\n",
    "\n",
    "Filip Szufnarowski's paper [link](https://cdn.instructables.com/ORIG/FIQ/KEUA/I55MMY1Y/FIQKEUAI55MMY1Y.pdf)\n",
    "\n",
    "Hbartle's MATLAB implementation [link](https://github.com/hbartle/Stewart_Platform/)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "01f7fb343ae16afa75a0bf551af72d5221b0249706b9790850dd85fd070cc3af"
  },
  "kernelspec": {
   "display_name": "Python 3.6.2 64-bit ('stewart_py': conda)",
   "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.6.13"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}