Part 1/3 Resolved-Rate Motion Control.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",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_Note: This notebook must be run locally. Due to how the Swift Simulator operates, this notebook will not run on Google Colab_"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3.0 Resolved-Rate Motion Control\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and 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",
    "[3.1 Robotics Toolbox for Python - Robot Class Introduction ](#utility)\n",
    "\n",
    "[3.2 Swift Introduction](#swift)\n",
    "\n",
    "[3.3 Resolved-Rate Motion Control](#rrmc)\n",
    "* Resolved-Rate Motion Control\n",
    "\n",
    "[3.4 Position based Servoing](#pbs)\n",
    "* Resolved-Rate Motion Control\n",
    "\n",
    "[3.5 Velocity Profile](#err)\n",
    "* Resolved-Rate Motion Control\n",
    "\n",
    "[3.6 Using the Robotics Toolbox](#rtb)\n",
    "\n",
    "[3.7 Dynamic Target Example](#rea)\n",
    "* Resolved-Rate Motion Control"
   ]
  },
  {
   "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",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "\n",
    "# swift is a lightweight browser-based simulator which comes eith the toolbox\n",
    "from swift import Swift\n",
    "\n",
    "# the Python math library\n",
    "import math\n",
    "\n",
    "# spatialgeometry is a utility package for dealing with geometric objects\n",
    "import spatialgeometry as sg\n",
    "\n",
    "# typing utilities\n",
    "from typing import Tuple"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='utility'></a>\n",
    "\n",
    "### 3.1 Robotics Toolbox for Python - Robot Class Introduction \n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `roboticstoolbox` package provides many ways of interfacing with a robot and performing dynamic or kinematic operations. In Notebooks 1 and 2 we used the `ET` and `ETS` classes. These provide a low level kinematic-only representation of a robot. \n",
    "\n",
    "Robots contain more information than just kinematic data (which is all that the ETS stores). For example, they may contain dynamic data (mass, inertia, centre of gravity etc.) and geometric data. The `Link` class is used to hold this data for a single link within a manipulator. A `Link` contains an `ETS` which is used to represent the kinematics of that link.\n",
    "\n",
    "We use the `Robot` class as a high level way of representing a whole robot along with all data. A `Robot` class instance contains a tree structure to represent how links are connected. Links can only have up to one parent but unlimited children. With this structure, we can represent serial-link manipulators (such as the Panda), branched mechanisms (such as the YuMi two armed manipulator), but not mechanisms which form closed loops in their structure.  The `Robot` instance may also hold additional data such as known joint configurations and the robot's pose relative to the world-frame.\n",
    "\n",
    "A robot will typically contain an end-effector or gripper. We represent these using the `Gripper` class. A `Robot` contains a list of grippers under the `Robot.grippers` attribute. Each `Gripper` contains its own tree structure of `Link` objects (Think of a `Gripper` as a mini `Robot`).\n",
    "\n",
    "We can convert a section or a sequence of a `Robot` to an `ETS` using the `Robot.ets(start, end)` method. The `start` argument defaults to the base link of the robot. The `end` link will default to the first gripper if present, otherwise the first leaf of the link tree. Otherwise `start` and `end` may be a `str` representing the name of the desired link or a reference to the desired link. Note that the referenced link must be within the robot or its grippers. There are no other limits on `start` and `end`, for example `start` could be after `end` in the chain or even from one branch to another (think of going from the link from the left gripper to the right gripper on the YuMi). \n",
    "\n",
    "Within the `roboticstoolbox`, we have many defined robots. These can be accessed from the `.models` namespace.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ERobot: panda (by Franka Emika), 7 joints (RRRRRRR), 1 gripper, geometry, collision\n",
      "┌─────┬──────────────┬───────┬─────────────┬────────────────────────────────────────────────┐\n",
      "│link │     link     │ joint │   parent    │              ETS: parent to link               │\n",
      "├─────┼──────────────┼───────┼─────────────┼────────────────────────────────────────────────┤\n",
      "│   0\u001b[0m │ \u001b[38;5;4mpanda_link0\u001b[0m  │      \u001b[0m │ BASE\u001b[0m        │ SE3()\u001b[0m                                          │\n",
      "│   1\u001b[0m │ panda_link1\u001b[0m  │     0\u001b[0m │ panda_link0\u001b[0m │ SE3(0, 0, 0.333) ⊕ Rz(q0)\u001b[0m                      │\n",
      "│   2\u001b[0m │ panda_link2\u001b[0m  │     1\u001b[0m │ panda_link1\u001b[0m │ SE3(-90°, -0°, 0°) ⊕ Rz(q1)\u001b[0m                    │\n",
      "│   3\u001b[0m │ panda_link3\u001b[0m  │     2\u001b[0m │ panda_link2\u001b[0m │ SE3(0, -0.316, 0; 90°, -0°, 0°) ⊕ Rz(q2)\u001b[0m       │\n",
      "│   4\u001b[0m │ panda_link4\u001b[0m  │     3\u001b[0m │ panda_link3\u001b[0m │ SE3(0.0825, 0, 0; 90°, -0°, 0°) ⊕ Rz(q3)\u001b[0m       │\n",
      "│   5\u001b[0m │ panda_link5\u001b[0m  │     4\u001b[0m │ panda_link4\u001b[0m │ SE3(-0.0825, 0.384, 0; -90°, -0°, 0°) ⊕ Rz(q4)\u001b[0m │\n",
      "│   6\u001b[0m │ panda_link6\u001b[0m  │     5\u001b[0m │ panda_link5\u001b[0m │ SE3(90°, -0°, 0°) ⊕ Rz(q5)\u001b[0m                     │\n",
      "│   7\u001b[0m │ panda_link7\u001b[0m  │     6\u001b[0m │ panda_link6\u001b[0m │ SE3(0.088, 0, 0; 90°, -0°, 0°) ⊕ Rz(q6)\u001b[0m        │\n",
      "│   8\u001b[0m │ \u001b[38;5;4m@panda_link8\u001b[0m │      \u001b[0m │ panda_link7\u001b[0m │ SE3(0, 0, 0.107)\u001b[0m                               │\n",
      "└─────┴──────────────┴───────┴─────────────┴────────────────────────────────────────────────┘\n",
      "\n",
      "┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐\n",
      "│name │ q0  │ q1     │ q2  │ q3    │ q4  │ q5    │ q6   │\n",
      "├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤\n",
      "│  qr\u001b[0m │  0°\u001b[0m │ -17.2°\u001b[0m │  0°\u001b[0m │ -126°\u001b[0m │  0°\u001b[0m │  115°\u001b[0m │  45°\u001b[0m │\n",
      "│  qz\u001b[0m │  0°\u001b[0m │  0°\u001b[0m    │  0°\u001b[0m │  0°\u001b[0m   │  0°\u001b[0m │  0°\u001b[0m   │  0°\u001b[0m  │\n",
      "└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Make a panda robot\n",
    "panda = rtb.models.Panda()\n",
    "\n",
    "print(panda)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From the output of the above cell, we can see that the Panda has several links, and the ETS of each link. We can also see the named configurations `qr` and `qz` which we can access through `panda.qr` and `panda.qz` respectively.\n",
    "\n",
    "We can perform kinematic operations with a `Robot` object in a similar manner to an `ETS` object. The only difference is that we can pass arguments defining the `start` and `end` links. The `start` and `end` links define which component of the `Robot` the kinematic operation is performed on and works in the same way described above for the `Robot.ets(start, end)` method.\n",
    "\n",
    "In the following cell, will perform forward kinematics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "  \u001b[38;5;1m 0.995   \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0.09983 \u001b[0m \u001b[38;5;4m 0.484   \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m-1       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;1m 0.09983 \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m-0.995   \u001b[0m \u001b[38;5;4m 0.4126  \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
      "\n",
      "  \u001b[38;5;1m-0.3233  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0.9463  \u001b[0m \u001b[38;5;4m 0.3755  \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;1m-0.9463  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m-0.3233  \u001b[0m \u001b[38;5;4m 0.6132  \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
      "\n",
      "  \u001b[38;5;1m 0.693   \u001b[0m \u001b[38;5;1m-0.693   \u001b[0m \u001b[38;5;1m-0.1987  \u001b[0m \u001b[38;5;4m 0.486   \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;1m 0.1405  \u001b[0m \u001b[38;5;1m-0.1405  \u001b[0m \u001b[38;5;1m 0.9801  \u001b[0m \u001b[38;5;4m 0.06697 \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;1m-0.7071  \u001b[0m \u001b[38;5;1m-0.7071  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
      "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Use the default start and end links\n",
    "print(panda.fkine(panda.qr))\n",
    "\n",
    "# Use string start and end links\n",
    "print(panda.fkine(panda.qr, start=\"panda_link0\", end=\"panda_link5\"))\n",
    "\n",
    "# Use reference start and end links\n",
    "print(panda.fkine(panda.qr, start=panda.links[3], end=panda.grippers[0]))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='swift'></a>\n",
    "\n",
    "### 3.2 Swift Introduction \n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Swift is a browser based robot simulator. We must first make the environment and then use the `launch` method to open the simulator.\n",
    "\n",
    "Each time `launch()` is called, a new browser tab is opened with a clean Swift environment.\n",
    "\n",
    "By passing the `realtime=True` keyword argument to the `launch` method, the simulator will try to run as close as possible to realtime, instead of as fast as possible.\n",
    "\n",
    "We can also add geometric shapes (and custom meshes but we won't show that here). Lets add two axes, to show the current end-effector pose and another to show some other pose such as the desired end-effector pose. There are other primitive shapes such as cuboids, arrows, cylinders and spheres."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52000/?53000\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x29bcffa10>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "2"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Make the environment\n",
    "env = Swift()\n",
    "\n",
    "# Launch the simulator, will open a browser tab in your default\n",
    "# browser (chrome is recommended)\n",
    "# The realtime flag will ask the simulator to simulate as close as\n",
    "# possible to realtime as apposed to as fast as possible\n",
    "# We can also choose to see Swift within the notebook using the\n",
    "# browser=\"notebook\" flag\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "\n",
    "# We can then add our robot to the simulator envionment\n",
    "env.add(panda)\n",
    "\n",
    "# end-effector axes\n",
    "ee_axes = sg.Axes(0.1)\n",
    "\n",
    "# goal axes\n",
    "goal_axes = sg.Axes(0.1)\n",
    "\n",
    "# Add the axes to the environment\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes) "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can change the pose of the shapes using the `.T` attribute"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can set the pose of the shape using the `.T` attribute.\n",
    "# This pose can be either a spatialmat SE3 or a 4x4 ndarray\n",
    "ee_axes.T = panda.fkine(panda.q)\n",
    "\n",
    "# Set the goal axes to something along the x axis\n",
    "goal_axes.T = sm.SE3.Trans(0.5, 0.0, 0.5)\n",
    "\n",
    "# step the environment to view the changes\n",
    "env.step(0)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The joint configuration which `Swift` displays the robot is stored in `Robot.q`. Therefore, by modifying this attribute, we can modify the robot in `Swift`. Note that we have to call `Swift.step(dt)` to step the simulator and visualise any changes and `dt` corresponds to the length of time to simulate for."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set the joint coordinates to qr\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the environment with a time step of 0 seconds\n",
    "env.step(0)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`Swift` uses the vector stored in `Robot.qd` to simulate joint velocities. Therefore, by modifying this attribute, we can make the robot in `Swift` move."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set all joint velocities to 0.1 rad/s\n",
    "panda.qd = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1])\n",
    "\n",
    "# Step the environment 100 times with a time step of 50 milliseconds\n",
    "for _ in range(100):\n",
    "    env.step(0.05)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rrmc'></a>\n",
    "\n",
    "### 3.3 Resolved-Rate Motion Control\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "Resolved-rate motion control (RRMC) is a simple and elegant method to generate straight-line motion of the end effector. RRMC is a direct application of the first-order differential equation\n",
    "\n",
    "\\begin{equation*}\n",
    "   {^0\\bf{\\nu}} = {^0\\bf{J}}(\\bf{q}) \\ \\bf{\\dot{q}}\n",
    "\\end{equation*}\n",
    "\n",
    "where ${^0(\\cdot)}$ denotes the base-frame. RRMC is simply the inverse of the above\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{\\dot{q}} = {^0\\bf{J}}(\\bf{q})^{-1} \\ {^0\\bf{\\nu}}\n",
    "\\end{equation*}\n",
    "\n",
    "which can only be solved when $\\bf{J}(\\bf{q})$ is square (and non-singular), which is when the robot has 6 degrees-of-freedom.\n",
    "\n",
    "For redundant robots there is no unique solution for the above equation. Consequently, the most common approach is to use the Moore-Penrose pseudoinverse\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{\\dot{q}} = {^0\\bf{J}}(\\bf{q})^{+} \\ {^0\\bf{\\nu}}.\n",
    "\\end{equation*}\n",
    "\n",
    "The pseudoinverse will return joint velocities with the minimum velocity norm of the possible solutions.\n",
    "\n",
    "Since the Panda robot has 7 degrees-of-freedom, we will use the psuedoinverse method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.     0.3081 0.     0.2966 0.     0.0115 0.    ]\n"
     ]
    }
   ],
   "source": [
    "# Our desired end-effector velocity\n",
    "# A constant 0.1 m/s along the x-axis\n",
    "ev = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0]\n",
    "\n",
    "# The base-frame manipulator Jacobian in the qr configuration\n",
    "J = panda.jacob0(panda.qr)\n",
    "\n",
    "# Calculate the required joint velocities to achieve the desired end-effector velocity ev\n",
    "dq = np.linalg.pinv(J) @ ev\n",
    "\n",
    "# Visualise the results\n",
    "print(np.round(dq, 4))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets check this out working in the `Swift` simulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52001/?53001\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x29bcd8e90>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# Specify our desired end-effector velocity\n",
    "ev = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0]\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "# Run the simulation for 5 seconds\n",
    "for _ in range(50):\n",
    "\n",
    "    # Work out the manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the next example we use the end-effector frame Jacobian and rotate around the z-axis"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52002/?53002\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x29bcd81d0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# Specify our desired end-effector velocity\n",
    "ev = [0.0, 0.0, 0.0, 0.0, 0.0, 0.5]\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "# Run the simulation for 5 seconds\n",
    "for _ in range(50):\n",
    "\n",
    "    # Work out the manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacobe(panda.q)\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='pbs'></a>\n",
    "\n",
    "### 3.4 Position based Servoing\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A more useful application of RRMC is to employ it in a closed-loop pose controller which we denote position-based servoing (PBS). Using this method we can get the end-effector to travel in a straight line, in the robot's task space, towards some desired end-effector pose. The PBS scheme relies on an error vector which represents the translation and rotation from the end-effector's current pose to the desired pose\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{e} =\n",
    "    \\begin{pmatrix}\n",
    "        \\tau\\left({^0\\bf{T}}_{e^*}\\right) - \\tau\\left({^0\\bf{T}}_{e}\\right) \\\\\n",
    "        \\alpha\n",
    "        \\left(\n",
    "            \\rho\\left({^0\\bf{T}}_{e^*}\\right)\n",
    "            \\rho\\left({^0\\bf{T}}_{e}\\right)^\\top\n",
    "        \\right)\n",
    "    \\end{pmatrix} \\in \\mathbb{R}^6\n",
    "\\end{equation*}\n",
    "\n",
    "where the top three rows correspond to the translational error in the world frame, the bottom three rows correspond to the rotational error in the world frame, ${^0\\bf{T}}_{e}$ is the forward kinematics of the robot which represents the end-effector pose in the base frame of the robot, ${^0\\bf{T}}_{e^*}$ is the desired end-effector pose in the base frame of the robot ($\\cdot^*$ denotes desired not actual), and $\\alpha(\\cdot) : \\bf{SO}(3) \\mapsto \\mathbb{R}^3$ transforms a rotation matrix to its Euler vector equivalent. "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To calculate the Euler vector (also known as angle-axis representation) we first calculate\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{l} =\n",
    "    \\begin{pmatrix}\n",
    "        r_{32} - r_{23} \\\\\n",
    "        r_{13} - r_{31} \\\\\n",
    "        r_{21} - r_{12} \\\\\n",
    "    \\end{pmatrix}.\n",
    "\\end{align*}\n",
    "\n",
    "If $\\bf{R}$ is not a diagonal matrix then the angle-axis equivalent of $\\bf{R}$ is calculated as\n",
    "\n",
    "\\begin{align*}\n",
    "    \\alpha\n",
    "    \\left(\n",
    "        \\bf{R}\n",
    "    \\right) &=\n",
    "    \\dfrac\n",
    "    {\\text{atan2}\n",
    "    \\left(\n",
    "        \\lVert\\bf{l}\\rVert, r_{11} + r_{22} + r_{33} - 1\n",
    "    \\right)}\n",
    "    {\\lVert\\bf{l}\\rVert}\\bf{l}.\n",
    "\\end{align*}\n",
    "\n",
    "If $\\bf{R}$ is a diagonal matrix then we use different formulas. For the case where $\\begin{pmatrix}r_{11} & r_{22} & r_{33}\\end{pmatrix} = \\begin{pmatrix}1&1&1\\end{pmatrix}$ then $\\alpha(\\bf{R}) = \\begin{pmatrix}0 & 0 & 0\\end{pmatrix}^\\top$ otherwise\n",
    "\n",
    "\\begin{align*}\n",
    "    \\alpha\n",
    "    \\left(\n",
    "        \\bf{R}\n",
    "    \\right) &=\n",
    "    \\frac{\\pi}{2}\n",
    "    \\begin{pmatrix}\n",
    "        r_{11} + 1 \\\\\n",
    "        r_{22} + 1 \\\\\n",
    "        r_{33} + 1 \\\\\n",
    "    \\end{pmatrix}.\n",
    "\\end{align*}\n",
    "\n",
    "In the following cell, we create a method `angle_axis` which calculates the vector $\\bf{e}$ from two poses `T` and `Td`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def angle_axis(T: np.ndarray, Td: np.ndarray) -> np.ndarray:\n",
    "    \"\"\"\n",
    "    Returns the error vector between T and Td in angle-axis form.\n",
    "\n",
    "    :param T: The current pose\n",
    "    :param Tep: The desired pose\n",
    "\n",
    "    :returns e: the error vector between T and Td\n",
    "    \"\"\"\n",
    "\n",
    "    e = np.empty(6)\n",
    "\n",
    "    # The position error\n",
    "    e[:3] = Td[:3, -1] - T[:3, -1]\n",
    "\n",
    "    R = Td[:3, :3] @ T[:3, :3].T\n",
    "\n",
    "    li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])\n",
    "\n",
    "    if np.linalg.norm(li) < 1e-6:\n",
    "        # If li is a zero vector (or very close to it)\n",
    "\n",
    "        # diagonal matrix case\n",
    "        if np.trace(R) > 0:\n",
    "            # (1,1,1) case\n",
    "            a = np.zeros((3,))\n",
    "        else:\n",
    "            a = np.pi / 2 * (np.diag(R) + 1)\n",
    "    else:\n",
    "        # non-diagonal matrix case\n",
    "        ln = np.linalg.norm(li)\n",
    "        a = math.atan2(ln, np.trace(R) - 1) * li / ln\n",
    "\n",
    "    e[3:] = a\n",
    "\n",
    "    return e"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To construct the PBS scheme we take the error term $\\bf{e}$ to set $\\bf{\\nu}$ in RRMC equation at each time step\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{\\nu} = k \\bf{e}\n",
    "\\end{align*}\n",
    "\n",
    "where $k$ is a proportional gain term which controls the rate of convergence to the goal. \n",
    "\n",
    "It is important to remember that the vector $\\bf{e}$ is non-homogeneous since its elements have different units (metres and radians).\n",
    "\n",
    "Therefore, $k$ can also be a diagonal matrix to set gains for each task-space DoF\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{k} =\n",
    "    \\text{diag}\n",
    "    (\n",
    "        k_t, \\ k_t, \\ k_t, \\ k_r, \\ k_r, \\ k_r\n",
    "    )\n",
    "\\end{align*}\n",
    "\n",
    "where $k_t$ is the translational motion gain and $k_r$ is the rotational motion gain. Note that since the error $\\bf{e}$ is in the base-frame of the robot we must use the base-frame manipulator Jacobian $^0\\bf{J}(\\bf{q})$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def p_servo(\n",
    "    Te: np.ndarray, Tep: np.ndarray, gain: np.ndarray, threshold: float = 0.1\n",
    ") -> Tuple[np.ndarray, bool]:\n",
    "    \"\"\"\n",
    "    Position-based servoing.\n",
    "\n",
    "    Returns the end-effector velocity which will cause the robot to approach\n",
    "    the desired pose.\n",
    "\n",
    "    :param Te: The current pose of the end-effecor in the base frame.\n",
    "    :type wTe: ndarray\n",
    "    :param Tep: The desired pose of the end-effecor in the base frame.\n",
    "    :type wTep: ndarray\n",
    "    :param gain: The gain for the controller. A vector corresponding to each\n",
    "        Cartesian axis.\n",
    "    :type gain: array-like\n",
    "    :param threshold: The threshold or tolerance of the final error between\n",
    "        the robot's pose and desired pose\n",
    "    :type threshold: float\n",
    "\n",
    "    :returns v: The velocity of the end-effector which will casue the robot\n",
    "        to approach Tep\n",
    "    :rtype v: ndarray(6)\n",
    "    :returns arrived: True if the robot is within the threshold of the final\n",
    "        pose\n",
    "    :rtype arrived: bool\n",
    "    \"\"\"\n",
    "\n",
    "    # Calculate the pose error vector\n",
    "    e = angle_axis(Te, Tep)\n",
    "\n",
    "    # Construct our gain diagonal matrix\n",
    "    k = np.diag(gain)\n",
    "\n",
    "    # Calculate our desired end0effector velocity\n",
    "    v = k @ e\n",
    "\n",
    "    # Check if we have arrived\n",
    "    arrived = True if np.sum(np.abs(e)) < threshold else False\n",
    "\n",
    "    return v, arrived\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets try it out in `Swift`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52003/?53003\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x29bcd81d0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "gain = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = panda.fkine(panda.q) * sm.SE3.Trans(0.3, 0.3, 0.25) * sm.SE3.RPY(0.0, 0.5, 0.5)\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = p_servo(Te, Tep, gain=gain, threshold=0.001)\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='err'></a>\n",
    "\n",
    "### 3.5 Velocity Profile\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The control scheme we have just described will cause the error to asymptotically decrease to zero. \n",
    "For real applications this is slow and impractical.\n",
    "\n",
    "We can improve this by increasing $k_t$ and $k_r$ and capping the $\\bf{\\nu}$ norm at some value $\\nu_{m}$, before stopping when the error norm drops below some value ${\\lVert \\bf{e} \\rVert}_m$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{\\nu}\n",
    "    &=\n",
    "    \\left\\{ \n",
    "    \\begin{matrix*}[l]\n",
    "        \\bf{k}\\bf{e} \\dfrac{\\nu_{m}}{\\lVert \\bf{k}\\bf{e} \\rVert} & \\text{if} \\ \\ \\lVert \\bf{k}\\bf{e} \\rVert > \\nu_{m} \\\\\n",
    "        \\ \\bf{k}\\bf{e} & \\text{if} \\ \\ \\nu_{m} \\geq \\lVert \\bf{k}\\bf{e} \\rVert > {\\lVert \\bf{e} \\rVert}_m \\\\\n",
    "        \\bf{0} & \\text{otherwise.}\n",
    "    \\end{matrix*}\n",
    "    \\right.\n",
    "\\end{align*}\n",
    "\n",
    "This will cause the error to reduce at a consistent rate until the end of the motion where the velocity asymptotically decreases to safely stop the robot. The effect of this is displayed in Figure \\ref{fig:rrmc2}. Alternatively, the above could be modified to create different velocity profiles, such as a linearly decreasing velocity norm, if desired. Additionally, the above can be modified to adjust the translational and angular velocity pofiles separately."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52004/?53004\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x2a8192310>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "# Note this is higher than the previous cell\n",
    "gain = 3 * np.array([1.0, 1.0, 1.0, 1.3, 1.3, 1.3])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = panda.fkine(panda.q) * sm.SE3.Trans(0.3, 0.3, 0.25) * sm.SE3.RPY(0.0, 0.5, 0.5)\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# Set the maximum ee velocity\n",
    "ev_max = 1.0\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = p_servo(Te, Tep, gain=gain, threshold=0.001)\n",
    "\n",
    "    # Calculate the magnitude of the ee velocity\n",
    "    ev_norm = np.linalg.norm(ev)\n",
    "\n",
    "    # if ee vel is greater than the max\n",
    "    if ev_norm > ev_max:\n",
    "        ev = (ev_max / ev_norm) * ev\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rtb'></a>\n",
    "\n",
    "### 3.6 Using the Robotics Toolbox\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We defined our own `angle_axis` and `p_servo` methods in the Notebook for demonstration. However, the Toolbox has it own implementations of these methods which we demonstrate below."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The toolbox `p_servo` can use two different error vectors to calculate the required end-effector velocity. The default method is `rpy` in the end-effector frame. Check the method docstring for more details about this method."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Example using `rpy` option in the `p_servo` method. Note that we must use `jacobe` with this method since the error is in the end-effector frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52005/?53005\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x2a819ae90>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "gain = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = panda.fkine(panda.q) * sm.SE3.Trans(0.3, 0.3, 0.25) * sm.SE3.RPY(0.0, 0.5, 0.5)\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacobe(panda.q)\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=gain, threshold=0.001, method='rpy')\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Example using `angle-axis` option in the `p_servo` method. Note that we must use `jacob0` with this method since the error is in the base frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52006/?53006\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x2a81a6510>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "gain = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "Tep = panda.fkine(panda.q) * sm.SE3.Trans(0.3, 0.3, 0.25) * sm.SE3.RPY(0.0, 0.5, 0.5)\n",
    "Tep = Tep.A\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = Tep\n",
    "\n",
    "# Run the simulation until the robot arrives at the goal\n",
    "while not arrived:\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required end-effector velocity and whether the robot has arrived\n",
    "    ev, arrived = rtb.p_servo(Te, Tep, gain=gain, threshold=0.001, method='angle-axis')\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rea'></a>\n",
    "\n",
    "### 3.7 Dynamic Target Example\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For a _reactive_ task, where a dynamic environment is causing the goal pose to change, we simply need to update the desired end-effector pose ${^0\\mathbf{T}}_{e^*}$ at each time step. In this example, we have full environemnt knowledge so we know where the target is at all times. In practice, this may not be possible. In such a scenario, a technique such as position-based visual servoing could be used to achieve this."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52007/?53007\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x2a81c1e50>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a new environment and add our robot\n",
    "env = Swift()\n",
    "env.launch(realtime=True, browser=\"notebook\")\n",
    "env.add(panda)\n",
    "env.add(ee_axes)\n",
    "env.add(goal_axes)\n",
    "\n",
    "# Change the robot configuration to the ready position\n",
    "panda.q = panda.qr\n",
    "\n",
    "# Step the sim to view the robot in this configuration\n",
    "env.step(0)\n",
    "\n",
    "# A variable to specify when to break the loop\n",
    "arrived = False\n",
    "\n",
    "# Specify the gain for the p_servo method\n",
    "gain = 2.0 * np.ones(6)\n",
    "\n",
    "# Specify our timestep\n",
    "dt = 0.05\n",
    "\n",
    "# Set the goal axes to Tep\n",
    "goal_axes.T = sm.SE3.Trans(0.4, -0.2, 0.05) * sm.SE3.Rx(np.pi)\n",
    "\n",
    "# Make the target move, set its velocity\n",
    "goal_axes.v = np.array([0.0, 0.02, 0.02, 0.0, 0.0, 0.0])\n",
    "\n",
    "# Run the simulation for 300 steps\n",
    "# As the target never stops moving, the robot will never arrive at the goal,\n",
    "# but it will continuously track towards it\n",
    "for _ in range(300):\n",
    "\n",
    "    # Work out the base frame manipulator Jacobian using the current robot configuration\n",
    "    J = panda.jacob0(panda.q)\n",
    "\n",
    "    # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n",
    "    Te = panda.fkine(panda.q).A\n",
    "\n",
    "    # Since the Panda has 7 joints, the Jacobian is not square, therefore we must\n",
    "    # use the pseudoinverse (the pinv method)\n",
    "    J_pinv = np.linalg.pinv(J)\n",
    "\n",
    "    # Calculate the required end-effector velocity\n",
    "    # Here we feed in the goal_axes.T, which is an SE3, as our goal pose\n",
    "    ev, _ = rtb.p_servo(Te, goal_axes.T, gain=gain, threshold=0.001, method='angle-axis')\n",
    "\n",
    "    # Calculate the required joint velocities and apply to the robot\n",
    "    panda.qd = J_pinv @ ev\n",
    "\n",
    "    # Update the ee axes\n",
    "    ee_axes.T = Te\n",
    "\n",
    "    # Step the simulator by dt seconds\n",
    "    env.step(dt)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "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,
  "vscode": {
   "interpreter": {
    "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}