Part 2/6 Advanced Numerical Inverse Kinematics.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "  %pip install qpsolvers[quadprog]\n",
    "  COLAB = True\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  COLAB = False\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": [
    "# 6.0 Advanced Numerical Inverse Kinematics\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[6.1 Numerical Inverse Kinematics Utility Class](#utility)\n",
    "* Resolved-Rate Motion Control\n",
    "  * Angle-axis error component\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[6.2 Newton-Raphson (NR) Method](#nr)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[6.2 Newton-Raphson (NR) Method](#nr)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[6.3 Gauss-Newton (GN) Method](#gn)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[6.4 Levenberg-Marquardt (LM) Method](#lm)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[6.5 Visualise IK Solutions](#vis)\n",
    "\n",
    "[6.6 Numerical IK Comparison](#exp)\n",
    "\n",
    "[6.7 Fast IK Solvers with the Robotics Toolbox](#rtb)\n",
    "\n",
    "[6.8 Robotics Toolbox IK Speed Comparison](#speed)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# ansitable is a great package for printing tables in a terminal\n",
    "from ansitable import ANSITable\n",
    "\n",
    "# python mechanisms to create abstract classes\n",
    "from abc import ABC, abstractmethod\n",
    "\n",
    "# a package for creating dynamic progress bars\n",
    "from progress.bar import Bar\n",
    "\n",
    "# swift is a lightweight browser-based simulator which comes with the toolbox\n",
    "from swift import Swift\n",
    "\n",
    "# spatialgeometry is a utility package for dealing with geometric objects\n",
    "import spatialgeometry as sg\n",
    "\n",
    "# provides sleep functionaly\n",
    "import time\n",
    "\n",
    "# Typing utilities\n",
    "from typing import Optional\n",
    "\n",
    "# A set of quadratic programming sovlers\n",
    "import qpsolvers as qp\n",
    "\n",
    "# Allows us make method wrappers\n",
    "from functools import wraps\n",
    "\n",
    "# suppress warnings\n",
    "import warnings\n",
    "warnings.filterwarnings('ignore')"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='utility'></a>\n",
    "\n",
    "### 4.1 Numerical Inverse Kinematics Utility Class\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inverse kinematics deals is the problem of determining the corresponding joint coordinates, given some end-effector pose. Numerical inverse kinematics as opposed to analytical inverse kinematics uses an iterative technique.\n",
    "\n",
    "We will start this Notebook by making an abstract base class called `IK`. This class implements everything needed to support a numerical inverse kinematics solver. It stores some general parameters typically used by any IK solver and also has some data structures useful for large-scale experiments. The idea behind the `IK` class is that a specific method class will inherit `IK` and implement the `step` method."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "An important point to note is that numerical IK methods are subject to local minima and in such some will fail to converge on the solution. The choice of the initial joint configuration $\\bf{q}_0$ is important.\n",
    "\n",
    "An alternative approach is to re-start an IK problem with a new random $\\bf{q}_0$ after a few $20 \\sim 50$ iterations rather than persist with a single search attempt with $500 \\sim 5000$ iterations. This is a simple but effective method of performing a global search for the IK solution. We can adjust this using the iteration limit `ilimit` and search limit `slimit` kwargs on the `IK` class."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the Tutorial, we use a quadratic error term to describe the error between the current end-effector pose and desired end-effector pose. This error is consistent across each method we discuss.\n",
    "\n",
    "Each method seeks to minimise the error function\n",
    "\n",
    "\\begin{align*}\n",
    "    E = \\frac{1}{2} \\bf{e}^{\\top} \\bf{W}_e \\bf{e}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{e} \\in \\mathbb{R}^6$ is the position and angle-axis error vector (see Part 1 Notebook 3), and $\\bf{W}_e = \\text{diag}(\\bf{w_e})(\\bf{w_e} \\in \\mathbb{R}^n_{\\geq0})$ is a diagonal weighting matrix which prioritises the corresponding error term. The `IK` class below takes a `we` vector in the `init` method and calculates the error vector `e` and quadratic error term `E` through the `error` method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class IK(ABC):\n",
    "    \"\"\"\n",
    "    An abstract super class which provides basic functionality to perform numerical inverse\n",
    "    kinematics (IK). Superclasses can inherit this class and implement the solve method.\n",
    "\n",
    "    This class also provides a mechanism to collect data on performance for large scale\n",
    "    experiments.\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(\n",
    "        self,\n",
    "        name: str = \"IK Solver\",\n",
    "        ilimit: int = 30,\n",
    "        slimit: int = 100,\n",
    "        tol: float = 1e-6,\n",
    "        we: np.ndarray = np.ones(6),\n",
    "        problems: int = 1000,\n",
    "        reject_jl: bool = True,\n",
    "        λΣ: float=0.0,\n",
    "        λm: float=0.0, \n",
    "        ps: float=0.1,\n",
    "        pi: Optional[np.ndarray]=None,\n",
    "    ):\n",
    "        \"\"\"\n",
    "        name: The name of the IK algorithm\n",
    "        ilimit: How many iterations are allowed within a search before a new search is started\n",
    "        slimit: How many searches are allowed before being deemed unsuccessful\n",
    "        tol: Maximum allowed residual error E\n",
    "        we: A 6 vector which assigns weights to Cartesian degrees-of-freedom\n",
    "        problems: Total number of IK problems within the experiment\n",
    "        reject_jl: Reject solutions with joint limit violations\n",
    "        λΣ: The gain for joint limit avoidance. Setting to 0.0 will remove this completely from the solution\n",
    "        λm: The gain for maximisation. Setting to 0.0 will remove this completely from the solution\n",
    "        ps: The minimum angle/distance (in radians or metres) in which the joint is allowed to approach to its limit\n",
    "        pi: The influence angle/distance (in radians or metres) in null space motion becomes active\n",
    "        \"\"\"\n",
    "\n",
    "        # Solver parameters\n",
    "        self.name = name\n",
    "        self.slimit = slimit\n",
    "        self.ilimit = ilimit\n",
    "        self.tol = tol\n",
    "        self.We = np.diag(we)\n",
    "        self.reject_jl = reject_jl\n",
    "        self.λΣ = λΣ\n",
    "        self.λm = λm\n",
    "        self.ps = ps\n",
    "        self.pi = pi\n",
    "\n",
    "        # Solver results\n",
    "        self.success = np.zeros(problems)\n",
    "        self.searches = np.zeros(problems)\n",
    "        self.iterations = np.zeros(problems)\n",
    "        self.valid_jl = np.zeros(problems)\n",
    "        self.times = np.zeros(problems)\n",
    "\n",
    "        # initialise with NaN\n",
    "        self.searches[:] = np.nan\n",
    "        self.iterations[:] = np.nan\n",
    "        self.success[:] = np.nan\n",
    "        self.valid_jl[:] = np.nan\n",
    "        self.times[:] = np.nan\n",
    "\n",
    "    def solve(self, ets: rtb.ETS, Tep: np.ndarray, q0: np.ndarray):\n",
    "        \"\"\"\n",
    "        This method will attempt to solve the IK problem and obtain joint coordinates\n",
    "        which result the the end-effector pose Tep.\n",
    "\n",
    "        The method returns a tuple:\n",
    "        q: The joint coordinates of the solution (ndarray). Note that these will not\n",
    "            be valid if failed to find a solution\n",
    "        success: True if a solution was found (boolean)\n",
    "        iterations: The number of iterations it took to find the solution (int)\n",
    "        searches: The number of searches it took to find the solution (int)\n",
    "        residual: The residual error of the solution (float)\n",
    "        jl_valid: True if joint coordinates q are within the joint limits\n",
    "        total_t: The total time spent within the step method\n",
    "        \"\"\"\n",
    "\n",
    "        # Iteration count\n",
    "        i = 0\n",
    "        total_i = 0\n",
    "        total_t = 0.0\n",
    "\n",
    "        for search in range(self.slimit):\n",
    "            q = q0[search].copy()\n",
    "            \n",
    "            while i <= self.ilimit:\n",
    "                i += 1\n",
    "\n",
    "                # Attempt a step\n",
    "                try:\n",
    "                    t, E, q = self.step(ets, Tep, q)\n",
    "\n",
    "                    # Acclumulate total time\n",
    "                    total_t += t\n",
    "                except np.linalg.LinAlgError:\n",
    "                    # Abandon search and try again\n",
    "                    break\n",
    "\n",
    "                # Check if we have arrived\n",
    "                if E < self.tol:\n",
    "\n",
    "                    # Wrap q to be within +- 180 deg\n",
    "                    # If your robot has larger than 180 deg range on a joint\n",
    "                    # this line should be modified in incorporate the extra range\n",
    "                    q = (q + np.pi) % (2 * np.pi) - np.pi\n",
    "\n",
    "                    # Check if we have violated joint limits\n",
    "                    jl_valid = self.check_jl(ets, q)\n",
    "\n",
    "                    if not jl_valid and self.reject_jl:\n",
    "                        # Abandon search and try again\n",
    "                        break\n",
    "                    else:\n",
    "                        return q, True, total_i + i, search + 1, E, jl_valid, total_t\n",
    "\n",
    "            total_i += i\n",
    "            i = 0\n",
    "\n",
    "        # If we make it here, then we have failed\n",
    "        return q, False, np.nan, np.nan, E, np.nan, np.nan\n",
    "\n",
    "    def error(self, Te: np.ndarray, Tep: np.ndarray):\n",
    "        \"\"\"\n",
    "        Calculates the engle axis error between current end-effector pose Te and\n",
    "        the desired end-effector pose Tep. Also calulates the quadratic error E\n",
    "        which is weighted by the diagonal matrix We.\n",
    "\n",
    "        Returns a tuple:\n",
    "        e: angle-axis error (ndarray in R^6)\n",
    "        E: The quadratic error weighted by We\n",
    "        \"\"\"\n",
    "        e = rtb.angle_axis(Te, Tep)\n",
    "        E = 0.5 * e @ self.We @ e\n",
    "\n",
    "        return e, E\n",
    "\n",
    "    def check_jl(self, ets: rtb.ETS, q: np.ndarray):\n",
    "        \"\"\"\n",
    "        Checks if the joints are within their respective limits\n",
    "\n",
    "        Returns a True if joints within feasible limits otherwise False\n",
    "        \"\"\"\n",
    "\n",
    "        # Loop through the joints in the ETS\n",
    "        for i in range(ets.n):\n",
    "\n",
    "            # Get the corresponding joint limits\n",
    "            ql0 = ets.qlim[0, i]\n",
    "            ql1 = ets.qlim[1, i]\n",
    "\n",
    "            # Check if q exceeds the limits\n",
    "            if q[i] < ql0 or q[i] > ql1:\n",
    "                return False\n",
    "\n",
    "        # If we make it here, all the joints are fine\n",
    "        return True\n",
    "\n",
    "    @abstractmethod\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        \"\"\"\n",
    "        Superclasses will implement this method to perform a step of the implemented\n",
    "        IK algorithm\n",
    "        \"\"\"\n",
    "        pass\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will also make a method which will wrap each our `step` methods to record how long each call to `step` takes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def timing(func):\n",
    "    @wraps(func)\n",
    "    def wrap(*args, **kw):\n",
    "        t_start = time.time()\n",
    "        E, q = func(*args, **kw)\n",
    "        t_end = time.time()\n",
    "        t = t_end - t_start\n",
    "        return t, E, q\n",
    "    return wrap"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='2'></a>\n",
    "\n",
    "### 4.2 Null-Space Motion in IK\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can greatly improve the solvability of our solvers when they are trying to avoid the joint limits by using the null-space projection devised by Baur [6]\n",
    "\n",
    "\\begin{align*} \n",
    "    \\bf{q}_{k+1} &=\n",
    "    \\bf{q}_k +\n",
    "    {\\bf{J}(\\bf{q}_k)}^{+} \\bf{e}_k +\n",
    "    \\bf{q}_{null} \\\\\n",
    "    \\bf{q}_{null} &= \n",
    "    \\Big(\n",
    "        \\left(\n",
    "            \\bf{1}_n - \\bf{J}(\\bf{q})^{+} \\bf{J}(\\bf{q})\n",
    "        \\right)\n",
    "        \\Big(\n",
    "            \\frac{1}{\\lambda_\\Sigma}\\bf{\\Sigma}\n",
    "        \\Big)\n",
    "    \\Big) \\\\\n",
    "    \\bf{\\Sigma} &=\n",
    "    \\left\\{\n",
    "        \\begin{matrix}\n",
    "            \\sum_{i=1}^n\n",
    "            \\dfrac{(q_i - \\bar{q}_{M_i})^2}\n",
    "                  {(q_{M_i} - \\bar{q}_{M_i})^2}\n",
    "            & q_i \\geq \\bar{q}_{M_i} \\\\\n",
    "            \\sum_{i=1}^n\n",
    "            \\dfrac{(q_i - \\bar{q}_{m_i})^2}\n",
    "                  {(q_{m_i} - \\bar{q}_{m_i})^2}\n",
    "            & q_i \\leq \\bar{q}_{m_i} \\\\\n",
    "            0 & \\text{else}\n",
    "        \\end{matrix}\n",
    "    \\right.\n",
    "\\end{align*}\n",
    "\n",
    "where the maximum and minimum joint angles for a joint are specified with $q_{M_i}$ and $q_{m_i}$, while the maximum and minimum joint angle threshold is specified by $\\bar{q}_{M_i}$ and $\\bar{q}_{m_i}$. Once this threshold is passed, further progress toward the joint limit is penalised by $\\bf{\\Sigma}$. While this addition can help avoid joint limits, it does not guarantee joint limit avoidance. The term $\\lambda_\\Sigma \\in \\mathbb{R}^+$ is a gain which adjusts how aggressively the joint limit is avoided by $\\bf{\\Sigma}$.\n",
    "\n",
    "Lets make a method to calculate $\\bf{\\Sigma}$ in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def null_Σ(ets: rtb.ETS, q: np.ndarray, ps: float, pi: Optional[np.ndarray]):\n",
    "    \"\"\"\n",
    "    Formulates a relationship between joint limits and the joint velocity.\n",
    "    When this is projected into the null-space of the differential kinematics\n",
    "    to attempt to avoid exceeding joint limits\n",
    "\n",
    "    q: The joint coordinates of the robot\n",
    "    ps: The minimum angle/distance (in radians or metres) in which the joint is\n",
    "        allowed to approach to its limit\n",
    "    pi: The influence angle/distance (in radians or metres) in which the velocity\n",
    "        damper becomes active\n",
    "\n",
    "    returns: Σ \n",
    "    \"\"\"\n",
    "\n",
    "    # If pi wasn't supplied, set it to some default value\n",
    "    if pi is None:\n",
    "        pi = 0.3 * np.ones(ets.n)\n",
    "\n",
    "    # Add cost to going in the direction of joint limits, if they are within\n",
    "    # the influence distance\n",
    "    Σ = np.zeros((ets.n, 1))\n",
    "\n",
    "    for i in range(ets.n):\n",
    "        qi = q[i]\n",
    "        ql0 = ets.qlim[0, i]\n",
    "        ql1 = ets.qlim[1, i]\n",
    "\n",
    "        if qi - ql0 <= pi[i]:\n",
    "            Σ[i, 0] = (\n",
    "                -np.power(((qi - ql0) - pi[i]), 2) / np.power((ps - pi[i]), 2)\n",
    "            )\n",
    "        if ql1 - qi <= pi[i]:\n",
    "            Σ[i, 0] = (\n",
    "                np.power(((ql1 - qi) - pi[i]), 2) / np.power((ps - pi[i]), 2)\n",
    "            )\n",
    "\n",
    "    return -Σ"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Furthermore, we can add the manipulability Jacobian to the null-space term as we did in Part 2 Notebook 4 to get\n",
    "\n",
    "\\begin{align*}\n",
    "\\bf{q}_{null} &= \n",
    "\\Big(\n",
    "    \\left(\n",
    "        \\bf{1}_n - \\bf{J}(\\bf{q})^{+} \\bf{J}(\\bf{q})\n",
    "    \\right)\n",
    "    \\Big(\n",
    "        \\frac{1}{\\lambda_\\Sigma}\\bf{\\Sigma} + \\frac{1}{\\lambda_m} \\bf{J}_m(\\bf{q})\n",
    "    \\Big)\n",
    "\\Big)\n",
    "\\end{align*}\n",
    "\n",
    "where $\\lambda_m \\in \\mathbb{R}^+$ is a gain which adjusts how aggressively the manipulability is to be maximised.\n",
    "\n",
    "We will incorporate this as an optional addition to each of our IK solvers we implemented in Part 1 Notebook 4.\n",
    "\n",
    "If we set the `IK` class kwarg `λΣ` to a non-zero value, then $\\bf{\\Sigma}$ will be projected into the null-space, set to 0.0 if you do not wish for it to be a part of the solver. Similarly, set the `IK` class kwarg `λm` to a non-zero value and $\\bf{J}_m$ will be projected into the null-space, set to 0.0 if you do not wish for it to be a part of the solver. Both `λΣ` and `λm` can be non-zero at the same time if desired.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_qnull(\n",
    "        ets: rtb.ETS,\n",
    "        q: np.ndarray,\n",
    "        J: np.ndarray,\n",
    "        λΣ: float,\n",
    "        λm: float,\n",
    "        ps: float,\n",
    "        pi: Optional[np.ndarray]\n",
    "    ):\n",
    "    \"\"\"\n",
    "    Calculates the desired null-space motion according to the gains λΣ and λm.\n",
    "    This is a helper method that is used within the `step` method of an IK solver\n",
    "\n",
    "    Returns qnull: the desired null-space motion\n",
    "    \"\"\"\n",
    "\n",
    "    qnull_grad = np.zeros(ets.n)\n",
    "    qnull = np.zeros(ets.n)\n",
    "\n",
    "    # Add the joint limit avoidance if the gain is above 0\n",
    "    if λΣ > 0:\n",
    "        Σ = null_Σ(ets, q, ps, pi)\n",
    "        qnull_grad += (1.0 / λΣ * Σ).flatten()\n",
    "\n",
    "    # Add the manipulability maximisation if the gain is above 0\n",
    "    if λm > 0:\n",
    "        Jm = ets.jacobm(q)\n",
    "        qnull_grad += (1.0 / λm * Jm).flatten()\n",
    "\n",
    "    # Calculate the null-space motion\n",
    "    if λΣ > 0 or λΣ > 0:\n",
    "        null_space = (np.eye(ets.n) - np.linalg.pinv(J) @ J)\n",
    "        qnull = null_space @ qnull_grad\n",
    "\n",
    "    return qnull.flatten()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='nr'></a>\n",
    "\n",
    "### 4.2 Newton-Raphson (NR) Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Newton-Raphson (NR) method seeks to minimise the error $E$ by iterating upon the following\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} = \\bf{q}_k + {^0\\bf{J}(\\bf{q}_k)}^{-1} \\bf{e}_k.\n",
    "\\end{align*}\n",
    "\n",
    "When using the NR method, the initial joint coordinates $q_0$, should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.\n",
    "When the the problem is solvable, it converges very quickly.\n",
    "However, this method frequently fails to converge on the goal.\n",
    "\n",
    "Most linear algebra libraries (including the Python `numpy` library) implement the pseudoinverse using singular value decomposition, which is robust to singular matrices. Therefore, we can this solver more robust by using the pseudoinverse instead of the normal inverse. \n",
    "\n",
    "The above equation requires the Jacobian to be square and non-singular. For a redundant manipulator (>6 DoF) or for more robust results to singularities we can use the pseudoinverse. We can use the pseudoinverse by supplying `pinv=True` when initialising the `NR` class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "class NR(IK):\n",
    "    def __init__(self, pinv=False, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        self.pinv = pinv\n",
    "\n",
    "        self.name = f\"NR (pinv={pinv})\"\n",
    "\n",
    "        if self.λΣ > 0.0:\n",
    "            self.name += ' Σ'\n",
    "\n",
    "        if self.λm > 0.0:\n",
    "            self.name += ' Jm'\n",
    "\n",
    "    @timing\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        J = ets.jacob0(q)\n",
    "\n",
    "        # Null-space motion\n",
    "        qnull = calc_qnull(ets, q, J, self.λΣ, self.λm, self.ps, self.pi)\n",
    "\n",
    "        if self.pinv:\n",
    "            q += np.linalg.pinv(J) @ e + qnull\n",
    "        else:\n",
    "            q += np.linalg.inv(J) @ e + qnull\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='gn'></a>\n",
    "\n",
    "### 4.3 Gauss-Newton (GN) Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can improve the solvability of the NR method by using the Gauss-Newton (GN) method\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} &= \\bf{q}_k +\n",
    "    \\left(\n",
    "    {\\bf{J}(\\bf{q}_k)}^\\top\n",
    "    \\bf{W}_e \\\n",
    "    {\\bf{J}(\\bf{q}_k)}\n",
    "    \\right)^{-1}\n",
    "    \\bf{g}_k \\\\\n",
    "    \\bf{g}_k &=\n",
    "    {\\bf{J}(\\bf{q}_k)}^\\top\n",
    "    \\bf{W}_e\n",
    "    \\bf{e}_k\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{J} = {^0\\bf{J}}$ is the base-frame manipulator Jacobian. If $\\bf{J}(\\bf{q}_k)$ is non-singular, and $\\bf{W}_e = \\bf{1}_n$, then the above provides the pseudoinverse solution. However, if $\\bf{J}(\\bf{q}_k)$ is singular, the above can not be computed and the GN solution is infeasible.\n",
    "\n",
    "Most linear algebra libraries (including the Python `numpy` library) implement the pseudoinverse using singular value decomposition, which is robust to singular matrices. Therefore, we can this solver more robust by using the pseudoinverse instead of the normal inverse. We can use the pseudoinverse by supplying `pinv=True` when initialising the `GN` class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "class GN(IK):\n",
    "    def __init__(self, pinv=False, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        self.pinv = pinv\n",
    "\n",
    "        self.name = f\"GN (pinv={pinv})\"\n",
    "\n",
    "        if self.λΣ > 0.0:\n",
    "            self.name += ' Σ'\n",
    "\n",
    "        if self.λm > 0.0:\n",
    "            self.name += ' Jm'\n",
    "\n",
    "    @timing\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        # Null-space motion\n",
    "        qnull = calc_qnull(ets, q, J, self.λΣ, self.λm, self.ps, self.pi)\n",
    "\n",
    "        if self.pinv:\n",
    "            q += np.linalg.pinv(J.T @ self.We @ J) @ g + qnull\n",
    "        else:\n",
    "            q += np.linalg.inv(J.T @ self.We @ J) @ g + qnull\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='lm'></a>\n",
    "\n",
    "### 4.4 Levenberg-Marquardt (LM) Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can further improve the solvability though the Levenberg-Marquardt (LM) Method\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} \n",
    "    &= \n",
    "    \\bf{q}_k +\n",
    "    \\left(\n",
    "        \\bf{A}_k\n",
    "    \\right)^{-1}\n",
    "    \\bf{g}_k \\\\\n",
    "    %\n",
    "    \\bf{A}_k\n",
    "    &=\n",
    "    {\\bf{J}(\\bf{q}_k)}^\\top\n",
    "    \\bf{W}_e \\\n",
    "    {\\bf{J}(\\bf{q}_k)}\n",
    "    +\n",
    "    \\bf{W}_n\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{W}_n = \\text{diag}(\\bf{w_n})(\\bf{w_n} \\in \\mathbb{R}^n_{>0})$ is a diagonal damping matrix. The damping matrix ensures that $\\bf{A}_k$ is non-singular and positive definite. The performance of the LM method largely depends on the choice of $\\bf{W}_n$."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Wampler's Method\n",
    "\n",
    "Wampler proposed $\\bf{w_n}$ to be a constant. We provide the variable `λ` as a kwarg to the `LM_Wampler` class to adjust the constant."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LM_Wampler(IK):\n",
    "    def __init__(self, λ=1.0, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        \n",
    "        self.name = f\"LM (Wampler λ={λ})\"\n",
    "        self.λ = λ\n",
    "\n",
    "        if self.λΣ > 0.0:\n",
    "            self.name += ' Σ'\n",
    "\n",
    "        if self.λm > 0.0:\n",
    "            self.name += ' Jm'\n",
    "\n",
    "    @timing\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        Wn = self.λ * np.eye(ets.n)\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        # Null-space motion\n",
    "        qnull = calc_qnull(ets, q, J, self.λΣ, self.λm, self.ps, self.pi)\n",
    "\n",
    "        q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Chan's Method\n",
    "\n",
    "Chan proposed\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{W}_n\n",
    "    &=\n",
    "    \\lambda E_k \\bf{1}_n\n",
    "\\end{align*}\n",
    "\n",
    "where $\\lambda$ is a constant which reportedly does not have much influence on performance. We provide the variable `λ` as a kwarg to the `LM_Chan` class to adjust the weighting term."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LM_Chan(IK):\n",
    "    def __init__(self, λ=1.0, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        \n",
    "        self.name = f\"LM (Chan λ={λ})\"\n",
    "        self.λ = λ\n",
    "\n",
    "        if self.λΣ > 0.0:\n",
    "            self.name += ' Σ'\n",
    "\n",
    "        if self.λm > 0.0:\n",
    "            self.name += ' Jm'\n",
    "\n",
    "    @timing\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        Wn = self.λ * E * np.eye(ets.n)\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        # Null-space motion\n",
    "        qnull = calc_qnull(ets, q, J, self.λΣ, self.λm, self.ps, self.pi)\n",
    "\n",
    "        q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull\n",
    "\n",
    "        return E, q\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Sugihara's Method\n",
    "\n",
    "Sugihara proposed\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{W}_n\n",
    "    &=\n",
    "    E_k \\bf{1}_n + \\text{diag}(\\hat{\\bf{w}}_n)\n",
    "\\end{align*}\n",
    "\n",
    "where $\\hat{\\bf{w}}_n \\in \\mathbb{R}^n$, $\\hat{w}_{n_i} = l^2 \\sim 0.01 l^2$, and $l$ is the length of a typical link within the manipulator.\n",
    "We provide the variable `λ` as a kwarg to the `LM_Sugihara` class to adjust the value of $w_n$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LM_Sugihara(IK):\n",
    "    def __init__(self, name=\"LM (Sugihara)\", λ=1.0, **kwargs):\n",
    "        super().__init__(name, **kwargs)\n",
    "\n",
    "        self.name = f\"LM (Sugihara λ={λ})\"\n",
    "        self.λ = λ\n",
    "\n",
    "        if self.λΣ > 0.0:\n",
    "            self.name += ' Σ'\n",
    "\n",
    "        if self.λm > 0.0:\n",
    "            self.name += ' Jm'\n",
    "\n",
    "    @timing\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        Wn = E * np.eye(ets.n) + self.λ * np.eye(ets.n)\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        # Null-space motion\n",
    "        qnull = calc_qnull(ets, q, J, self.λΣ, self.λm, self.ps, self.pi)\n",
    "\n",
    "        q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='qp'></a>\n",
    "\n",
    "### 4.5 Quadratic Programming Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The addition of null-space motion provides much better results for inverse kinematics when trying to avoid joint limits, but it is only available on redundant robots. For improved constrained inverse kinematics, we can use the augmented QP with slack from Part 2 Notebook 5 with\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} = \\bf{q}_{k} + \\dot{\\bf{q}}.\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where the QP is defined as\n",
    "\n",
    "\\begin{align*} \n",
    "    \\min_x \\quad f_o(\\bf{x}) &= \\frac{1}{2} \\bf{x}^\\top \\mathcal{Q} \\bf{x}+ \\mathcal{C}^\\top \\bf{x}, \\\\ \n",
    "    \\text{subject to} \\quad \\mathcal{J} \\bf{x} &= \\bf{\\nu},  \\\\\n",
    "    \\mathcal{A} \\bf{x} &\\leq \\mathcal{B},  \\\\\n",
    "    \\bf{x}^- &\\leq \\bf{x} \\leq \\bf{x}^+ \n",
    "\\end{align*}\n",
    "\n",
    "with\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{x} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\dot{\\bf{q}} \\\\ \\bf{\\delta}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n+6)}  \\\\\n",
    "    \\mathcal{Q} &=\n",
    "    \\begin{pmatrix}\n",
    "        \\lambda_q \\bf{1}_{n} & \\mathbf{0}_{6 \\times 6} \\\\ \\mathbf{0}_{n \\times n} & \\lambda_\\delta \\bf{1}_{6}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n+6) \\times (n+6)} \\\\\n",
    "    \\mathcal{J} &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{J}(\\bf{q}) & \\bf{1}_{6}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{6 \\times (n+6)} \\\\\n",
    "    \\mathcal{C} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\bf{J}_m \\\\ \\bf{0}_{6 \\times 1}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n + 6)} \\\\\n",
    "    \\mathcal{A} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\bf{1}_{n \\times n + 6} \\\\\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(l + n) \\times (n + 6)} \\\\\n",
    "    \\mathcal{B} &= \n",
    "    \\eta\n",
    "    \\begin{pmatrix}\n",
    "        \\frac{\\rho_0 - \\rho_s}\n",
    "                {\\rho_i - \\rho_s} \\\\\n",
    "        \\vdots \\\\\n",
    "        \\frac{\\rho_n - \\rho_s}\n",
    "                {\\rho_i - \\rho_s} \n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{n} \\\\\n",
    "    \\bf{x}^{-, +} &= \n",
    "    \\begin{pmatrix}\n",
    "        \\dot{\\bf{q}}^{-, +} \\\\\n",
    "        \\bf{\\delta}^{-, +}\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^{(n+6)},\n",
    "\\end{align*}\n",
    "\n",
    "where\n",
    "$\\bf{\\delta} \\in \\mathbb{R}^6$ is the slack vector,\n",
    "$\\lambda_\\delta \\in \\mathbb{R}^+$ is a gain term which adjusts the cost of the norm of the slack vector in the optimiser,\n",
    "$\\dot{\\bf{q}}^{-,+}$ are the minimum and maximum joint velocities, and \n",
    "$\\dot{\\bf{\\delta}}^{-,+}$ are the minimum and maximum slack velocities."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "class QP(IK):\n",
    "    def __init__(self, name=\"QP\", λj=1.0, λs=1.0, **kwargs):\n",
    "        super().__init__(name, **kwargs)\n",
    "\n",
    "        self.name = f\"QP (λj={λj}, λs={λs})\"\n",
    "        self.λj = λj\n",
    "        self.λs = λs\n",
    "\n",
    "        if self.λΣ > 0.0:\n",
    "            self.name += ' Σ'\n",
    "\n",
    "        if self.λm > 0.0:\n",
    "            self.name += ' Jm'\n",
    "\n",
    "    @timing\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "        J = ets.jacob0(q)\n",
    "\n",
    "        # Quadratic component of objective function\n",
    "        Q = np.eye(ets.n + 6)\n",
    "\n",
    "        # Joint velocity component of Q\n",
    "        Q[: ets.n, : ets.n] *= self.λj\n",
    "\n",
    "        # Slack component of Q\n",
    "        Q[ets.n :, ets.n :] = self.λs * (1 / np.sum(np.abs(e))) * np.eye(6)\n",
    "\n",
    "        # The equality contraints\n",
    "        Aeq = np.concatenate((J, np.eye(6)), axis=1)\n",
    "        beq = e.reshape((6,))\n",
    "\n",
    "        # The inequality constraints for joint limit avoidance\n",
    "        if self.λΣ > 0.0:\n",
    "            Ain = np.zeros((ets.n + 6, ets.n + 6))\n",
    "            bin = np.zeros(ets.n + 6)\n",
    "\n",
    "            # Form the joint limit velocity damper\n",
    "            Ain_l = np.zeros((ets.n, ets.n))\n",
    "            Bin_l = np.zeros(ets.n)\n",
    "\n",
    "            for i in range(ets.n):\n",
    "                ql0 = ets.qlim[0, i]\n",
    "                ql1 = ets.qlim[1, i]\n",
    "\n",
    "                if ql1 - q[i] <= self.pi[i]:\n",
    "                    Bin_l[i] = ((ql1 - q[i]) - self.ps) / (self.pi[i] - self.ps)\n",
    "                    Ain_l[i, i] = 1\n",
    "\n",
    "                if q[i] - ql0 <= self.pi[i]:\n",
    "                    Bin_l[i] = -(((ql0 - q[i]) + self.ps) / (self.pi[i] - self.ps))\n",
    "                    Ain_l[i, i] = -1\n",
    "\n",
    "            Ain[: ets.n, : ets.n] = Ain_l\n",
    "            bin[: ets.n] =  (1.0 / self.λΣ) * Bin_l\n",
    "        else:\n",
    "            Ain = None\n",
    "            bin = None\n",
    "        \n",
    "        # Manipulability maximisation\n",
    "        if self.λm > 0.0:\n",
    "            Jm = ets.jacobm(q).reshape((ets.n,))\n",
    "            c = np.concatenate(((1.0 / self.λm) * -Jm, np.zeros(6)))\n",
    "        else:\n",
    "            c = np.zeros(ets.n + 6)\n",
    "\n",
    "        xd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=None, ub=None, solver='quadprog')\n",
    "\n",
    "        q += xd[: ets.n]\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='vis'></a>\n",
    "\n",
    "### 4.5 Visualise IK Solutions\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_Note: This **section** of the notebook must be run locally. Due to how Swift operates, this section will not run on Google Colab_\n",
    "\n",
    "We are going to try out some of our methods and visualise the results in Swift"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Make our robot\n",
    "robot = rtb.models.Valkyrie()\n",
    "\n",
    "# Extract the waist to right index finger links as an ETS\n",
    "start = None\n",
    "end = \"rightIndexFingerPitch3Link\"\n",
    "ets = robot.ets(start, end)\n",
    "\n",
    "# Keep the original joint indices for later\n",
    "og_ets_jindices = [et.jindex for et in ets.joints()]\n",
    "\n",
    "# Reset the joint indices to count from 0 to ets.n\n",
    "for i, et in enumerate(ets.joints()):\n",
    "    et.jindex = i\n",
    "\n",
    "# Calculate parameters for joint limit avoidance\n",
    "q_range = ets.qlim[1, :] - ets.qlim[0, :]\n",
    "pi = 0.5 * q_range\n",
    "ps = 0.001\n",
    "\n",
    "# Make an axes to visualise our current end-effector pose\n",
    "ee_axes = sg.Axes(0.1)\n",
    "\n",
    "# Make an axes to visualise our desired end-effector pose\n",
    "goal_axes = sg.Axes(0.1)\n",
    "\n",
    "def make_ik_env():\n",
    "    # Make the environment\n",
    "    env = Swift()\n",
    "\n",
    "    # Launch the simulator\n",
    "    env.launch(realtime=True, browser=\"notebook\")\n",
    "\n",
    "    # Add our robot to the simulator envionment\n",
    "    env.add(robot)\n",
    "\n",
    "    # Add them to the environment\n",
    "    env.add(ee_axes)\n",
    "    env.add(goal_axes)\n",
    "\n",
    "    return env\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will establish some goal poses and a general visualise method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of problems\n",
    "problems = 10\n",
    "\n",
    "# Iteration and seach limits\n",
    "ilimit = 30\n",
    "slimit = 100\n",
    "\n",
    "# random valid q values for q0\n",
    "q0 = ets.random_q(slimit)\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_Tep = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_Tep[i])\n",
    "\n",
    "def visualise_ik(solver, env):\n",
    "    \"\"\"\n",
    "    A method which will test the solver over each pose in Tep\n",
    "    and visualise the results in Swift\n",
    "    \"\"\"\n",
    "    for pose in Tep:\n",
    "\n",
    "        # Solver for Tep\n",
    "        q, success, iterations, searches, residual, q_valid, _ = solver.solve(ets, pose, q0)\n",
    "\n",
    "        # print the results\n",
    "        print(f\"Successful: {success}, iterations: {iterations}, searches: {searches}, residual: {residual}, q_valid: {q_valid}\")\n",
    "\n",
    "        ### visualise the results\n",
    "        # set the robots joint coordinates to the solution q\n",
    "        # make sure q has no NaNs first\n",
    "        if not np.any(np.isnan(q)):\n",
    "            robot.q[og_ets_jindices] = q\n",
    "\n",
    "            # set the end-effector axes to the end-effector pose at the solution q\n",
    "            # ee_axes.T = robot.fkine(robot.q, end=end).A\n",
    "            ee_axes.T = ets.fkine(q)\n",
    "\n",
    "        # set the goal axes to Tep pose\n",
    "        goal_axes.T = pose\n",
    "\n",
    "        # step the environment to view the changes\n",
    "        env.step(0)\n",
    "\n",
    "        # sleep for 2 seconds to view the results\n",
    "        time.sleep(2)\n",
    "\n",
    "       "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test out the NR method first. Since we are using the Panda robot which has 7 DoF and is redundant we must set `pinv=True`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52013/?53013\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x293dd7550>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Successful: True, iterations: 46, searches: 4, residual: 5.789347025754664e-07, q_valid: True\n",
      "Successful: True, iterations: 48, searches: 4, residual: 5.216091433856326e-07, q_valid: True\n",
      "Successful: True, iterations: 26, searches: 2, residual: 3.7753175620126285e-07, q_valid: True\n",
      "Successful: True, iterations: 14, searches: 1, residual: 7.82472567891041e-07, q_valid: True\n",
      "Successful: True, iterations: 11, searches: 1, residual: 3.5631907638920104e-07, q_valid: True\n",
      "Successful: True, iterations: 131, searches: 10, residual: 9.834891580968926e-07, q_valid: True\n",
      "Successful: True, iterations: 32, searches: 3, residual: 5.127516516736984e-08, q_valid: True\n",
      "Successful: True, iterations: 48, searches: 4, residual: 6.140630585887269e-07, q_valid: True\n",
      "Successful: True, iterations: 51, searches: 4, residual: 2.819849983727459e-07, q_valid: True\n",
      "Successful: True, iterations: 66, searches: 6, residual: 4.5285358496157236e-07, q_valid: True\n"
     ]
    }
   ],
   "source": [
    "if not COLAB:\n",
    "    # Make our solver\n",
    "    solver = NR(pinv=True, ilimit=30, slimit=100, problems=problems, reject_jl=True, λΣ=2.0, λm=10.0, ps=ps, pi=pi)\n",
    "\n",
    "    # Visualise the solver\n",
    "    env = make_ik_env()\n",
    "    visualise_ik(solver, env)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Visualise the LM Chan method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52014/?53014\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x293dc8a50>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Successful: True, iterations: 56, searches: 4, residual: 6.102753054033355e-07, q_valid: True\n",
      "Successful: True, iterations: 8, searches: 1, residual: 3.05853210234488e-07, q_valid: True\n",
      "Successful: True, iterations: 12, searches: 1, residual: 9.113892039556061e-07, q_valid: True\n",
      "Successful: True, iterations: 16, searches: 1, residual: 3.6643612104369494e-07, q_valid: True\n",
      "Successful: True, iterations: 7, searches: 1, residual: 4.425576649499674e-07, q_valid: True\n",
      "Successful: True, iterations: 24, searches: 2, residual: 8.067953484184202e-07, q_valid: True\n",
      "Successful: True, iterations: 51, searches: 3, residual: 3.0835952695748077e-07, q_valid: True\n",
      "Successful: True, iterations: 11, searches: 1, residual: 8.605401694620144e-07, q_valid: True\n",
      "Successful: True, iterations: 11, searches: 1, residual: 7.077552621998414e-07, q_valid: True\n",
      "Successful: True, iterations: 27, searches: 2, residual: 6.344576472196917e-07, q_valid: True\n"
     ]
    }
   ],
   "source": [
    "if not COLAB:\n",
    "    # Make our solver\n",
    "    solver = LM_Chan(λ=0.1, ilimit=30, slimit=100, problems=problems, reject_jl=True, λΣ=2.0, λm=10.0, ps=ps, pi=pi)\n",
    "\n",
    "    # Visualise the solver\n",
    "    env = make_ik_env()\n",
    "    visualise_ik(solver, env)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Feel free to modify the above cells to try out any of the other methods. You may have noticed that many of the solutions violated the joint limits of the panda. In Part II of the Tutorial we will address this issue."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='exp'></a>\n",
    "\n",
    "### 4.6 Numerical IK Comparison\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this Section we will perform a large scale experiment to compare the performance of each IK method.\n",
    "\n",
    "This Notebook is intended to be set up as an IK playground. Feel free to modify, extend, or create your own methods as shown above. Then, simply add your solver to the `solvers` list below and text how it compares to the existing methods. Also feel free to try out different robots!"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As shown above, the `ETS` class has its own method to generate random valid joint coordinate vectors. However, to make the table in the paper reporoducible, we replicate the method here."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "def random_q(ets: rtb.ETS, i: int = 1) -> np.ndarray:\n",
    "    \"\"\"\n",
    "    Generate a random valid joint configuration\n",
    "\n",
    "    :param i: number of configurations to generate\n",
    "\n",
    "    Generates a random q vector within the joint limits defined by\n",
    "    `ets.qlim`.\n",
    "    \"\"\"\n",
    "\n",
    "    if i == 1:\n",
    "        q = np.zeros(ets.n)\n",
    "\n",
    "        for i in range(ets.n):\n",
    "            q[i] = np.random.uniform(ets.qlim[0, i], ets.qlim[1, i])\n",
    "\n",
    "    else:\n",
    "        q = np.zeros((i, ets.n))\n",
    "\n",
    "        for j in range(i):\n",
    "            for i in range(ets.n):\n",
    "                q[j, i] = np.random.uniform(ets.qlim[0, i], ets.qlim[1, i])\n",
    "\n",
    "    return q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the following cell, we make a general IK experiment method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ik_experiment(solvers, Tep, ets):\n",
    "    \"\"\"\n",
    "    This method takes a list of solvers and a list of desired end-effector poses\n",
    "    and will run the complete IK experiment while collecting data and will print\n",
    "    a table once finished\n",
    "    \"\"\"\n",
    "        \n",
    "    with Bar(message=\"Prog\", max=problems, check_tty=False, hide_cursor=False) as bar:\n",
    "        for i, pose in enumerate(Tep):\n",
    "\n",
    "            for solver in solvers:\n",
    "                (\n",
    "                    _,\n",
    "                    solver.success[i],\n",
    "                    solver.iterations[i],\n",
    "                    solver.searches[i],\n",
    "                    _,\n",
    "                    solver.valid_jl[i],\n",
    "                    solver.times[i],\n",
    "                ) = solver.solve(ets, pose, q0)\n",
    "\n",
    "            bar.next()\n",
    "        bar.finish()\n",
    "\n",
    "    # Calculate the time per iteration of each of the solvers\n",
    "    t_iter = [np.nansum(solver.times) / np.nansum(solver.iterations) for solver in solvers]\n",
    "\n",
    "    # Normalise the time to a fraction of the quickest time per iteration within the solvers\n",
    "    t_iter_norm = t_iter / np.min(t_iter)\n",
    "\n",
    "    # Make a table to visualise the results\n",
    "    print(f\"\\nNumerical Inverse Kinematics Methods Compared over {problems} problems\\n\")\n",
    "\n",
    "    table = ANSITable(\n",
    "        \"Method\",\n",
    "        \"Mean Iter.\",\n",
    "        \"Median Iter.\",\n",
    "        \"Infeasible\",\n",
    "        \"Infeasible %\",\n",
    "        \"Mean Searches\",\n",
    "        \"Max Searches\",\n",
    "        \"jl Violations\",\n",
    "        \"Time per Iter.\",\n",
    "        \"Median Time\",\n",
    "        border=\"thin\",\n",
    "    )\n",
    "\n",
    "    for solver, time in zip(solvers, t_iter_norm):\n",
    "        table.row(\n",
    "            solver.name,\n",
    "            np.round(np.nanmean(solver.iterations), 2),\n",
    "            np.nanmedian(solver.iterations),\n",
    "            np.sum(np.isnan(solver.iterations)),\n",
    "            np.round(np.sum(np.isnan(solver.iterations)) / problems * 100.0, 2),\n",
    "            np.round(np.nanmean(solver.searches), 2),\n",
    "            np.nanmax(solver.searches),\n",
    "            np.sum(solver.valid_jl == False),\n",
    "            np.round(time, 2),\n",
    "            np.round(time * np.nanmedian(solver.iterations), 2),\n",
    "        )\n",
    "\n",
    "    table.print()\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then we define some default parameters which each of our experiments will use. The experiments will take a while to run. If you want results faster, reduce the number `problems`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "### Experiment parameters\n",
    "# Number of problems to solve\n",
    "problems = 10000\n",
    "\n",
    "# Maximum slimit, we will preallocate the random q0 values so each\n",
    "# solver uses the same set of random q0 values\n",
    "max_slimit = 100\n",
    "\n",
    "# Calculate parameters for joint limit avoidance\n",
    "ps = 0.001\n",
    "\n",
    "# Joint limit avoidance and manipulability maximisation gains\n",
    "λΣ=2.0\n",
    "λΣ_panda=100.0\n",
    "λm=10.0\n",
    "λm_qp=100.0\n",
    "λm_qp_valk=1000.0"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### UR5 Experiment\n",
    "\n",
    "Lets test our IK solvers on a 6 DoF UR5 robot.\n",
    "\n",
    "What is shown below will recreate the data used for Table 1 in the Paper."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Prog |################################| 10000/10000"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Numerical Inverse Kinematics Methods Compared over 10000 problems\n",
      "\n",
      "┌────────────────────────┬────────────┬──────────────┬────────────┬──────────────┬───────────────┬──────────────┬───────────────┬────────────────┬─────────────┐\n",
      "│                 Method │ Mean Iter. │ Median Iter. │ Infeasible │ Infeasible % │ Mean Searches │ Max Searches │ jl Violations │ Time per Iter. │ Median Time │\n",
      "├────────────────────────┼────────────┼──────────────┼────────────┼──────────────┼───────────────┼──────────────┼───────────────┼────────────────┼─────────────┤\n",
      "│         NR (pinv=True)\u001b[0m │      28.63\u001b[0m │         16.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.46\u001b[0m │         29.0\u001b[0m │             0\u001b[0m │           2.37\u001b[0m │       37.95\u001b[0m │\n",
      "│        LM (Chan λ=0.1)\u001b[0m │      15.52\u001b[0m │          8.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.21\u001b[0m │         14.0\u001b[0m │             0\u001b[0m │           1.41\u001b[0m │       11.27\u001b[0m │\n",
      "│  LM (Wampler λ=0.0001)\u001b[0m │      23.75\u001b[0m │         13.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.35\u001b[0m │         20.0\u001b[0m │             0\u001b[0m │           1.06\u001b[0m │        13.8\u001b[0m │\n",
      "│        LM (Chan λ=0.1)\u001b[0m │      15.52\u001b[0m │          8.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.21\u001b[0m │         14.0\u001b[0m │             0\u001b[0m │            1.0\u001b[0m │         8.0\u001b[0m │\n",
      "│ LM (Sugihara λ=0.0001)\u001b[0m │      21.89\u001b[0m │         13.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.27\u001b[0m │         19.0\u001b[0m │             0\u001b[0m │           1.13\u001b[0m │       14.75\u001b[0m │\n",
      "│QP (λj=0.01, λs=1.0) Jm\u001b[0m │      15.93\u001b[0m │          8.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.22\u001b[0m │         13.0\u001b[0m │             0\u001b[0m │           4.52\u001b[0m │       36.13\u001b[0m │\n",
      "└────────────────────────┴────────────┴──────────────┴────────────┴──────────────┴───────────────┴──────────────┴───────────────┴────────────────┴─────────────┘\n",
      "\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Our robot and ETS\n",
    "robot = rtb.models.UR5()\n",
    "ets = robot.ets()\n",
    "\n",
    "# Setting the seed will make it possible to regenrate Table 1\n",
    "# in the paper (provided no ther values were changed).\n",
    "np.random.seed(10)\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_Tep = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_Tep[i])\n",
    "\n",
    "# random valid q values for q0\n",
    "q0 = ets.random_q(max_slimit)\n",
    "\n",
    "# Calculate parameters for joint limit avoidance\n",
    "q_range = ets.qlim[1, :] - ets.qlim[0, :]\n",
    "pi = 0.5 * q_range\n",
    "\n",
    "solvers_ur5 = [\n",
    "    ### Not avoiding joint limits\n",
    "    NR(pinv=True, problems=problems, reject_jl=False),\n",
    "    LM_Chan(λ=0.1, problems=problems, reject_jl=False),\n",
    "    ### No null\n",
    "    LM_Wampler(λ=1e-4, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    ### QP\n",
    "    QP(λj=0.01, λs=1.0, problems=problems, λΣ=0.0, λm=λm_qp, ps=ps, pi=pi)\n",
    "]\n",
    "\n",
    "ik_experiment(solvers_ur5, Tep, ets)\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Because the UR5 has $\\pm 360°$ rotation in each of its joints, every angle for every joint is reachable. Therefore, our advanced null-space and QP methods which push the joints away from their limits provide no benefit. This is reflected in the table above."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Panda Experiment\n",
    "\n",
    "Lets test our IK solvers on a 7 DoF Panda robot.\n",
    "\n",
    "What is shown below will recreate the data used for Table 2 in the Paper."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Prog |################################| 10000/10000"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Numerical Inverse Kinematics Methods Compared over 10000 problems\n",
      "\n",
      "┌────────────────────────────┬────────────┬──────────────┬────────────┬──────────────┬───────────────┬──────────────┬───────────────┬────────────────┬─────────────┐\n",
      "│                     Method │ Mean Iter. │ Median Iter. │ Infeasible │ Infeasible % │ Mean Searches │ Max Searches │ jl Violations │ Time per Iter. │ Median Time │\n",
      "├────────────────────────────┼────────────┼──────────────┼────────────┼──────────────┼───────────────┼──────────────┼───────────────┼────────────────┼─────────────┤\n",
      "│             NR (pinv=True)\u001b[0m │       27.6\u001b[0m │         16.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.41\u001b[0m │         13.0\u001b[0m │          6697\u001b[0m │           2.99\u001b[0m │       47.87\u001b[0m │\n",
      "│            LM (Chan λ=0.1)\u001b[0m │      11.91\u001b[0m │          8.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.12\u001b[0m │          7.0\u001b[0m │          5394\u001b[0m │           1.28\u001b[0m │       10.23\u001b[0m │\n",
      "│             NR (pinv=True)\u001b[0m │     137.67\u001b[0m │         80.0\u001b[0m │        103\u001b[0m │         1.03\u001b[0m │          7.39\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │           1.53\u001b[0m │      122.44\u001b[0m │\n",
      "│      LM (Wampler λ=0.0001)\u001b[0m │     127.83\u001b[0m │         76.0\u001b[0m │        101\u001b[0m │         1.01\u001b[0m │          7.13\u001b[0m │         99.0\u001b[0m │             0\u001b[0m │            1.0\u001b[0m │        76.0\u001b[0m │\n",
      "│            LM (Chan λ=0.1)\u001b[0m │      37.55\u001b[0m │         18.0\u001b[0m │         91\u001b[0m │         0.91\u001b[0m │          3.81\u001b[0m │         86.0\u001b[0m │             0\u001b[0m │           1.01\u001b[0m │       18.25\u001b[0m │\n",
      "│     LM (Sugihara λ=0.0001)\u001b[0m │      50.13\u001b[0m │         26.0\u001b[0m │         89\u001b[0m │         0.89\u001b[0m │          3.64\u001b[0m │         76.0\u001b[0m │             0\u001b[0m │            1.1\u001b[0m │       28.71\u001b[0m │\n",
      "│           NR (pinv=True) Σ\u001b[0m │     347.46\u001b[0m │        219.0\u001b[0m │        255\u001b[0m │         2.55\u001b[0m │         15.87\u001b[0m │         99.0\u001b[0m │             0\u001b[0m │           8.82\u001b[0m │     1931.41\u001b[0m │\n",
      "│    LM (Wampler λ=0.0001) Σ\u001b[0m │     353.25\u001b[0m │        199.0\u001b[0m │        187\u001b[0m │         1.87\u001b[0m │          14.0\u001b[0m │         99.0\u001b[0m │             0\u001b[0m │           8.59\u001b[0m │     1708.91\u001b[0m │\n",
      "│          LM (Chan λ=0.1) Σ\u001b[0m │       37.4\u001b[0m │         18.0\u001b[0m │         91\u001b[0m │         0.91\u001b[0m │          3.79\u001b[0m │         86.0\u001b[0m │             0\u001b[0m │           8.65\u001b[0m │       155.7\u001b[0m │\n",
      "│   LM (Sugihara λ=0.0001) Σ\u001b[0m │      44.63\u001b[0m │         24.0\u001b[0m │         99\u001b[0m │         0.99\u001b[0m │          2.85\u001b[0m │         97.0\u001b[0m │             0\u001b[0m │           8.78\u001b[0m │      210.71\u001b[0m │\n",
      "│        NR (pinv=True) Σ Jm\u001b[0m │     232.56\u001b[0m │        133.0\u001b[0m │        132\u001b[0m │         1.32\u001b[0m │         10.21\u001b[0m │         99.0\u001b[0m │             0\u001b[0m │          11.59\u001b[0m │      1541.9\u001b[0m │\n",
      "│ LM (Wampler λ=0.0001) Σ Jm\u001b[0m │     177.79\u001b[0m │        104.0\u001b[0m │        107\u001b[0m │         1.07\u001b[0m │          8.56\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │          11.16\u001b[0m │     1160.71\u001b[0m │\n",
      "│       LM (Chan λ=0.1) Σ Jm\u001b[0m │      37.33\u001b[0m │         18.0\u001b[0m │         90\u001b[0m │          0.9\u001b[0m │          3.77\u001b[0m │         86.0\u001b[0m │             0\u001b[0m │          11.19\u001b[0m │      201.39\u001b[0m │\n",
      "│LM (Sugihara λ=0.0001) Σ Jm\u001b[0m │      49.55\u001b[0m │         26.0\u001b[0m │         89\u001b[0m │         0.89\u001b[0m │           3.6\u001b[0m │         97.0\u001b[0m │             0\u001b[0m │          11.27\u001b[0m │      293.06\u001b[0m │\n",
      "│  QP (λj=0.01, λs=1.0) Σ Jm\u001b[0m │      42.42\u001b[0m │         14.0\u001b[0m │         76\u001b[0m │         0.76\u001b[0m │          2.12\u001b[0m │         86.0\u001b[0m │             0\u001b[0m │          10.54\u001b[0m │       147.6\u001b[0m │\n",
      "└────────────────────────────┴────────────┴──────────────┴────────────┴──────────────┴───────────────┴──────────────┴───────────────┴────────────────┴─────────────┘\n",
      "\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Our robot and ETS\n",
    "robot = rtb.models.Panda()\n",
    "ets = robot.ets()\n",
    "\n",
    "# Setting the seed will make it possible to regenrate Table 2\n",
    "# in the paper (provided no ther values were changed).\n",
    "np.random.seed(10)\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_Tep = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_Tep[i])\n",
    "\n",
    "# random valid q values for q0\n",
    "q0 = ets.random_q(max_slimit)\n",
    "\n",
    "# Calculate parameters for joint limit avoidance\n",
    "q_range = ets.qlim[1, :] - ets.qlim[0, :]\n",
    "pi = 0.5 * q_range\n",
    "\n",
    "solvers_panda = [\n",
    "    ### Not avoiding joint limits\n",
    "    NR(pinv=True, problems=problems, reject_jl=False),\n",
    "    LM_Chan(λ=0.1, problems=problems, reject_jl=False),\n",
    "    ### No null\n",
    "    NR(pinv=True, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    LM_Wampler(λ=1e-4, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    ### λΣ\n",
    "    NR(pinv=True, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    LM_Wampler(λ=1e-4, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=λΣ_panda, λm=0.0, ps=ps, pi=pi),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    ### λΣ and λm\n",
    "    NR(pinv=True, problems=problems, λΣ=λΣ_panda, λm=λm, ps=ps, pi=pi),\n",
    "    LM_Wampler(λ=1e-4, problems=problems, λΣ=λΣ_panda, λm=λm, ps=ps, pi=pi),\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=λΣ_panda, λm=λm, ps=ps, pi=pi),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems, λΣ=λΣ_panda, λm=λm, ps=ps, pi=pi),\n",
    "    ### QP\n",
    "    QP(λj=0.01, λs=1.0, problems=problems, λΣ=λΣ, λm=λm_qp, ps=ps, pi=pi)\n",
    "]\n",
    "\n",
    "ik_experiment(solvers_panda, Tep, ets)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Panda has more narrow joint limit range than the UR5. Therefore, there are joint configurations which are invalid as shown by the first two methods in the Table above. In this context, our null-space and QP methods start to provide benefits."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Valkyrie Experiment\n",
    "\n",
    "Lets test our IK solvers on a 13 DoF section of the Valkyrie robot.\n",
    "\n",
    "What is shown below will recreate the data used for Table 3 in the Paper."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Prog |################################| 10000/10000\n",
      "\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Numerical Inverse Kinematics Methods Compared over 10000 problems\n",
      "\n",
      "┌────────────────────────────┬────────────┬──────────────┬────────────┬──────────────┬───────────────┬──────────────┬───────────────┬────────────────┬─────────────┐\n",
      "│                     Method │ Mean Iter. │ Median Iter. │ Infeasible │ Infeasible % │ Mean Searches │ Max Searches │ jl Violations │ Time per Iter. │ Median Time │\n",
      "├────────────────────────────┼────────────┼──────────────┼────────────┼──────────────┼───────────────┼──────────────┼───────────────┼────────────────┼─────────────┤\n",
      "│            LM (Chan λ=0.1)\u001b[0m │       6.31\u001b[0m │          6.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │          9542\u001b[0m │           3.75\u001b[0m │       22.51\u001b[0m │\n",
      "│             NR (pinv=True)\u001b[0m │     285.88\u001b[0m │        235.0\u001b[0m │       2791\u001b[0m │        27.91\u001b[0m │         34.57\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │           1.08\u001b[0m │      253.16\u001b[0m │\n",
      "│            LM (Chan λ=0.1)\u001b[0m │     156.13\u001b[0m │         98.0\u001b[0m │       1765\u001b[0m │        17.65\u001b[0m │         25.22\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │            1.0\u001b[0m │        98.0\u001b[0m │\n",
      "│           NR (pinv=True) Σ\u001b[0m │       82.6\u001b[0m │         37.0\u001b[0m │        109\u001b[0m │         1.09\u001b[0m │          6.82\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │           8.26\u001b[0m │      305.76\u001b[0m │\n",
      "│    LM (Wampler λ=0.0001) Σ\u001b[0m │      82.42\u001b[0m │         37.0\u001b[0m │        109\u001b[0m │         1.09\u001b[0m │           6.8\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │           9.21\u001b[0m │      340.75\u001b[0m │\n",
      "│          LM (Chan λ=0.1) Σ\u001b[0m │      28.18\u001b[0m │         15.0\u001b[0m │         56\u001b[0m │         0.56\u001b[0m │          2.11\u001b[0m │         95.0\u001b[0m │             0\u001b[0m │            9.4\u001b[0m │      140.96\u001b[0m │\n",
      "│   LM (Sugihara λ=0.0001) Σ\u001b[0m │      25.59\u001b[0m │         13.0\u001b[0m │         50\u001b[0m │          0.5\u001b[0m │          1.79\u001b[0m │        100.0\u001b[0m │             0\u001b[0m │           9.36\u001b[0m │       121.7\u001b[0m │\n",
      "│       LM (Chan λ=0.1) Σ Jm\u001b[0m │      28.69\u001b[0m │         15.0\u001b[0m │         60\u001b[0m │          0.6\u001b[0m │          2.14\u001b[0m │         99.0\u001b[0m │             0\u001b[0m │          11.08\u001b[0m │      166.27\u001b[0m │\n",
      "│LM (Sugihara λ=0.0001) Σ Jm\u001b[0m │      24.86\u001b[0m │         13.0\u001b[0m │         56\u001b[0m │         0.56\u001b[0m │          1.74\u001b[0m │         91.0\u001b[0m │             0\u001b[0m │          11.02\u001b[0m │       143.2\u001b[0m │\n",
      "│  QP (λj=0.01, λs=1.0) Σ Jm\u001b[0m │      15.29\u001b[0m │          7.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.27\u001b[0m │         18.0\u001b[0m │             0\u001b[0m │           9.86\u001b[0m │       69.02\u001b[0m │\n",
      "└────────────────────────────┴────────────┴──────────────┴────────────┴──────────────┴───────────────┴──────────────┴───────────────┴────────────────┴─────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Our robot and ETS\n",
    "robot = rtb.models.Valkyrie()\n",
    "\n",
    "# Extract the waist to right index finger links as an ETS\n",
    "start = None\n",
    "end = \"rightIndexFingerPitch3Link\"\n",
    "ets = robot.ets(start, end)\n",
    "\n",
    "# Reset the joint indices to count from 0 to ets.n\n",
    "for i, et in enumerate(ets.joints()):\n",
    "    et.jindex = i\n",
    "\n",
    "# Setting the seed will make it possible to regenrate Table 3\n",
    "# in the paper (provided no ther values were changed).\n",
    "np.random.seed(10)\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_Tep = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_Tep[i])\n",
    "\n",
    "# random valid q values for q0\n",
    "q0 = ets.random_q(max_slimit)\n",
    "\n",
    "# Calculate parameters for joint limit avoidance\n",
    "q_range = ets.qlim[1, :] - ets.qlim[0, :]\n",
    "pi = 0.5 * q_range\n",
    "\n",
    "\n",
    "\n",
    "solvers_panda = [\n",
    "    ### Not avoiding joint limits\n",
    "    LM_Chan(λ=0.1, problems=problems, reject_jl=False),\n",
    "    ### No null\n",
    "    NR(pinv=True, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=0.0, λm=0.0),\n",
    "    ### λΣ\n",
    "    NR(pinv=True, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    LM_Wampler(λ=1e-4, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems, λΣ=λΣ, λm=0.0, ps=ps, pi=pi),\n",
    "    ### λΣ and λm\n",
    "    LM_Chan(λ=0.1, problems=problems, λΣ=λΣ, λm=λm, ps=ps, pi=pi),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems, λΣ=λΣ, λm=λm, ps=ps, pi=pi),\n",
    "    ### QP\n",
    "    QP(λj=0.01, λs=1.0, problems=problems, λΣ=λΣ, λm=λm_qp_valk, ps=ps, pi=pi)\n",
    "]\n",
    "\n",
    "ik_experiment(solvers_panda, Tep, ets)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This section of the Valkyrie has joint limits which are even more narrow than the Panda. The solutions of IK methods (the first row of the Table above) which aren't trying to find solutions with feasible joint coordinates find a large \n",
    "\n",
    "here are joint configurations which are invalid as shown by the first two methods in the Table above. In this context, our null-space and QP methods start to provide benefits."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rtb'></a>\n",
    "\n",
    "### 4.7 Fast IK Solvers with the Robotics Toolbox\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section we will briefly demonstrate the IK solvers implemented in the Robotics Toolbox. \n",
    "\n",
    "The Toolbox has implemented some of the above IK techniques using C extensions and wrapped by python methods. The results generated from these methods will not perfectly replicate the above results as\n",
    "\n",
    "* We can not use the same random number generator or seed\n",
    "* The C implementations use different matrix inverse and pseudo-inverse calculators\n",
    "\n",
    "So far, we have not implemented the null-space projections or the QP solver in C extensions but these will be included in future versions of the Robotics Toolbox.\n",
    "\n",
    "Note that with these solvers, we can add the `reject_jl=True` kwarg for the solver to reject solutions which violate joint limits in a similar manner to how it has been implemented above. Using this kwarg, we will compare the implemented IK solvers within the Toolbox at finding IK solutions with achievable joint coordinates."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We make a simplified wrapper class for this experiment as we only wish to collect the data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "class RTB_IK:\n",
    "    def __init__(self, name, solve, problems=problems):\n",
    "\n",
    "        # Solver attributes\n",
    "        self.name = name\n",
    "        self.solve = solve\n",
    "\n",
    "        # Solver results\n",
    "        self.success = np.zeros(problems)\n",
    "        self.searches = np.zeros(problems)\n",
    "        self.iterations = np.zeros(problems)\n",
    "\n",
    "        # initialise with NaN\n",
    "        self.success[:] = np.nan\n",
    "        self.searches[:] = np.nan\n",
    "        self.iterations[:] = np.nan\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create the experiment parameters here"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Our robot and ETS\n",
    "robot = rtb.models.Panda()\n",
    "ets = robot.ets()\n",
    "\n",
    "# Number of problems to solve\n",
    "rtb_problems = 10000\n",
    "\n",
    "# Cartesion DoF priority matrix\n",
    "we = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_rand = ets.random_q(rtb_problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((rtb_problems, 4, 4))\n",
    "\n",
    "for i in range(rtb_problems):\n",
    "    Tep[i] = ets.eval(q_rand[i])\n",
    "\n",
    "# Maximum iterations allowed in a search\n",
    "ilimit = 30\n",
    "\n",
    "# Maximum searches allowed per problem\n",
    "slimit = 100\n",
    "\n",
    "# Solution tolerance\n",
    "tol = 1e-6"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create a list of solvers similar to Section 4.6 but use `lambda` funtions to point to the solver we wish to use. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "rtb_solvers = [\n",
    "    RTB_IK(\n",
    "        \"Newton Raphson (pinv=True)\",\n",
    "        lambda Tep: ets.ik_NR(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            pinv=True,\n",
    "            joint_limits=True\n",
    "        ),\n",
    "        problems=rtb_problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"Gauss Newton (pinv=True)\",\n",
    "        lambda Tep: ets.ik_GN(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            pinv=True,\n",
    "            joint_limits=True\n",
    "        ),\n",
    "        problems=rtb_problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Wampler 1e-4\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=1e-4,\n",
    "            joint_limits=True,\n",
    "            method=\"wampler\"\n",
    "        ),\n",
    "        problems=rtb_problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Chan 0.1\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            q0=None,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=0.1,\n",
    "            joint_limits=True,\n",
    "            method=\"chan\"\n",
    "        ),\n",
    "        problems=rtb_problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Sugihara 0.0001\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=0.0001,\n",
    "            joint_limits=True,\n",
    "            method=\"sugihara\"\n",
    "        ),\n",
    "        problems=rtb_problems,\n",
    "    ),\n",
    "]\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can run the experiment. This will be much faster than the Python implementations shown above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Prog |################################| 10000/10000"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Numerical Inverse Kinematics Methods Compared over 10000 problems\n",
      "\n",
      "┌───────────────────────────┬────────┬────────┬─────────────────┬───────────────────┬────────────┬──────────────┬───────────────┬──────────────┐\n",
      "│                    Method │ sLimit │ iLimit │ Mean Iterations │ Median Iterations │ Infeasible │ Infeasible % │ Mean Searches │ Max Searches │\n",
      "├───────────────────────────┼────────┼────────┼─────────────────┼───────────────────┼────────────┼──────────────┼───────────────┼──────────────┤\n",
      "│Newton Raphson (pinv=True)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           572.3\u001b[0m │             356.5\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │         30.25\u001b[0m │        101.0\u001b[0m │\n",
      "│  Gauss Newton (pinv=True)\u001b[0m │    100\u001b[0m │     30\u001b[0m │          563.94\u001b[0m │             362.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │         29.92\u001b[0m │        101.0\u001b[0m │\n",
      "│           LM Wampler 1e-4\u001b[0m │    100\u001b[0m │     30\u001b[0m │          501.13\u001b[0m │             312.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │         27.56\u001b[0m │        101.0\u001b[0m │\n",
      "│               LM Chan 0.1\u001b[0m │    100\u001b[0m │     30\u001b[0m │           57.33\u001b[0m │              29.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          5.92\u001b[0m │        101.0\u001b[0m │\n",
      "│        LM Sugihara 0.0001\u001b[0m │    100\u001b[0m │     30\u001b[0m │           78.04\u001b[0m │              40.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          5.68\u001b[0m │        101.0\u001b[0m │\n",
      "└───────────────────────────┴────────┴────────┴─────────────────┴───────────────────┴────────────┴──────────────┴───────────────┴──────────────┘\n",
      "\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "with Bar(message=\"Prog\", max=rtb_problems, check_tty=False, hide_cursor=False) as bar:\n",
    "    for i, pose in enumerate(Tep):\n",
    "\n",
    "        for solver in rtb_solvers:\n",
    "            (\n",
    "                _,\n",
    "                solver.success[i],\n",
    "                solver.iterations[i],\n",
    "                solver.searches[i],\n",
    "                _,\n",
    "            ) = solver.solve(pose)\n",
    "\n",
    "        bar.next()\n",
    "    bar.finish()\n",
    "\n",
    "\n",
    "# Make a table to visualise the results\n",
    "print(f\"\\nNumerical Inverse Kinematics Methods Compared over {rtb_problems} problems\\n\")\n",
    "\n",
    "table = ANSITable(\n",
    "    \"Method\",\n",
    "    \"sLimit\",\n",
    "    \"iLimit\",\n",
    "    \"Mean Iterations\",\n",
    "    \"Median Iterations\",\n",
    "    \"Infeasible\",\n",
    "    \"Infeasible %\",\n",
    "    \"Mean Searches\",\n",
    "    \"Max Searches\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "for solver in rtb_solvers:\n",
    "    # print(solver.iterations)\n",
    "    table.row(\n",
    "        solver.name,\n",
    "        slimit,\n",
    "        ilimit,\n",
    "        np.round(np.nanmean(solver.iterations), 2),\n",
    "        np.nanmedian(solver.iterations),\n",
    "        np.sum(np.isnan(solver.iterations)),\n",
    "        np.round(np.sum(np.isnan(solver.iterations)) / rtb_problems * 100.0, 2),\n",
    "        np.round(np.nanmean(solver.searches), 2),\n",
    "        np.nanmax(solver.searches),\n",
    "    )\n",
    "\n",
    "table.print()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='speed'></a>\n",
    "\n",
    "### 4.8 Robotics Toolbox IK Speed Comparison\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this last section we will compare the speed of some of the Robotics Toolbox IK implementations while rejecting solutions which have unreachable joint coordinates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of problems to solve\n",
    "speed_problems = 10000\n",
    "\n",
    "# Cartesion DoF priority matrix\n",
    "we = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_rand = ets.random_q(speed_problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((speed_problems, 4, 4))\n",
    "\n",
    "for i in range(speed_problems):\n",
    "    Tep[i] = ets.eval(q_rand[i])\n",
    "\n",
    "# Maximum iterations allowed in a search\n",
    "ilimit = 30\n",
    "\n",
    "# Maximum searches allowed per problem\n",
    "slimit = 100\n",
    "\n",
    "# Solution tolerance\n",
    "tol = 1e-6"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the solver list, we aren't bothering with a wrapper class since we don't care about the stats, only the speed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed_solvers = [\n",
    "    lambda Tep: ets.ik_NR(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        pinv=True,\n",
    "        joint_limits=True,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_GN(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        pinv=True,\n",
    "        joint_limits=True,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=1e-4,\n",
    "        joint_limits=True,\n",
    "        method=\"wampler\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        q0=None,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=0.1,\n",
    "        joint_limits=True,\n",
    "        method=\"chan\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=0.0001,\n",
    "        joint_limits=True,\n",
    "        method=\"sugihara\",\n",
    "    ),\n",
    "]\n",
    "\n",
    "speed_names = [\n",
    "    \"Newton Raphson (pinv=True)\",\n",
    "    \"Gauss Newton (pinv=True)\",\n",
    "    \"LM Wampler 1e-4\",\n",
    "    \"LM Chan 0.1\",\n",
    "    \"LM Sugihara 0.0001\",\n",
    "]\n",
    "\n",
    "times = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Solving with Newton Raphson (pinv=True)\n",
      "Solving with Gauss Newton (pinv=True)\n",
      "Solving with LM Wampler 1e-4\n",
      "Solving with LM Chan 0.1\n",
      "Solving with LM Sugihara 0.0001\n",
      "\n",
      "Numerical Inverse Kinematics Methods Times Compared over 10000 problems\n",
      "\n",
      "┌───────────────────────────┬────────────────┬────────────────────────────────┐\n",
      "│                    Method │ Total Time (s) │ Average Time per Solution (μs) │\n",
      "├───────────────────────────┼────────────────┼────────────────────────────────┤\n",
      "│Newton Raphson (pinv=True)\u001b[0m │         32.299\u001b[0m │                       3229.904\u001b[0m │\n",
      "│  Gauss Newton (pinv=True)\u001b[0m │        44.9978\u001b[0m │                      4499.7833\u001b[0m │\n",
      "│           LM Wampler 1e-4\u001b[0m │        10.4467\u001b[0m │                      1044.6699\u001b[0m │\n",
      "│               LM Chan 0.1\u001b[0m │         1.1382\u001b[0m │                        113.816\u001b[0m │\n",
      "│        LM Sugihara 0.0001\u001b[0m │         1.5778\u001b[0m │                       157.7755\u001b[0m │\n",
      "└───────────────────────────┴────────────────┴────────────────────────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "for name, solver in zip(speed_names, speed_solvers):\n",
    "    print(f\"Solving with {name}\")\n",
    "\n",
    "    start = time.time()\n",
    "\n",
    "    for i in range(speed_problems):\n",
    "        solver(Tep[i])\n",
    "\n",
    "    total_time = time.time() - start\n",
    "    times.append(total_time)\n",
    "\n",
    "\n",
    "print(f\"\\nNumerical Inverse Kinematics Methods Times Compared over {speed_problems} problems\\n\")\n",
    "\n",
    "table = ANSITable(\n",
    "    \"Method\",\n",
    "    \"Total Time (s)\",\n",
    "    \"Average Time per Solution (μs)\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "for name, t in zip(speed_names, times):\n",
    "    table.row(\n",
    "        name,\n",
    "        np.round(t, 4),\n",
    "        np.round((t / speed_problems) * 1e6, 4),\n",
    "    )\n",
    "\n",
    "table.print()"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3.9.5 ('rtb')",
   "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.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}