3_invkine.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Inverse kinematics\n",
    "This notebook considers the problem of inverse kinematics, ie solving at each control cycle of the robot a quadratic program from the derivatives (jacobian) of the current state. It introduces the basic function to compute the Jacobians of the robot, and how to use them to compute an inverse-kinematics control law. One of the key difficulties is to understand in which frames each quantities is computed (might be in the world frame, in the local frame attached to the end effector, in some arbitrary goal frame, etc), as we should never mixed quantities expressed in different frames without transporting them in the right frame. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will use a Gepetto-viewer, and the linear algebra of NumPy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp3/generated/inverse_kinematics_import\n",
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "import time\n",
    "from numpy.linalg import pinv,inv,norm,svd,eig\n",
    "from tiago_loader import loadTiago\n",
    "import matplotlib.pylab as plt; plt.ion()\n",
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will use the Tiago robot (https://youtu.be/6BwRqwD066g). This mobile manipulator from PAL Robotics as a mobile basis which can move in the plane (3 dof), a manipulator arm (7 dof) and a head (2 dof) both mounted on a prismatic axis moving vertically (1 dof). This makes 3 dof for the basis, and 9 dof for the body. It also has 2 extra joints tot figure the wheels, which are not very useful for this notebook. The wheels and the basis rotations are represented by the cos and sin of the angle. The size of the configuration vector is then 18, while the velocity vector has dimension 15.\n",
    "\n",
    "A load function is available to make it easy to load the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp3/generated/inverse_kinematics_robot\n",
    "robot = loadTiago()\n",
    "viz = MeshcatVisualizer(robot,url='classical')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7010/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The configuration is represented by a vector of larger dimension, subject to constraints (cos^2+sin^2=1). It is not possible to randomly sample a configuration vector q, as these constraints should be respected. Similarly, we should take care when integrating a velocity as summing a configuration q with a velocity v will not work (dimensions do not match). Two functions in Pinocchio implements these functionnalities."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = pin.randomConfiguration(robot.model)\n",
    "vq = np.random.rand(robot.model.nv)*2-1\n",
    "DT = 1e-2\n",
    "qnext = pin.integrate(robot.model,q,vq*DT)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A simple example moving the robot in the viewer following a constant (random) velocity is as follows."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "for t in range(1000):\n",
    "    q = pin.integrate(robot.model,q,vq*DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(DT/10)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The robot is mobile, hence the camera view in the viewer is not always centered. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Forward kinematics and Jacobian\n",
    "We recall first the basic method to compute the robot forward kinematics.\n",
    "We will consider two frames of interest on the robot: the first one, named <tool> is at the tip of the end-effector ; the second one, named <basis>, is on the front of the robot basis, 10 cm above the ground. Both are represented in Gepetto-Viewer by a frame composed of three RGB arrows. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Frame name: frametool paired to (parent joint/ previous frame)(8/52)\n",
       "with relative placement wrt parent joint:\n",
       "  R =\n",
       "1 0 0\n",
       "0 1 0\n",
       "0 0 1\n",
       "  p =    0    0 0.08\n",
       "containing inertia:\n",
       "  m = 0\n",
       "  c = 0 0 0\n",
       "  I = \n",
       "0 0 0\n",
       "0 0 0\n",
       "0 0 0"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%load tp3/generated/inverse_kinematics_frames"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Frame name: frametool paired to (parent joint/ previous frame)(8/52)\n",
       "with relative placement wrt parent joint:\n",
       "  R =\n",
       "1 0 0\n",
       "0 1 0\n",
       "0 0 1\n",
       "  p =    0    0 0.08\n",
       "containing inertia:\n",
       "  m = 0\n",
       "  c = 0 0 0\n",
       "  I = \n",
       "0 0 0\n",
       "0 0 0\n",
       "0 0 0"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.model.frames[IDX_TOOL]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Computing frame placement\n",
    "These frames are computed by the Pinocchio function framesForwardKinematics, whose results are stored in robot.data.oMf.     "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Tool placement:   R =\n",
      " 0.809241 -0.514412 -0.283741\n",
      " 0.330757  0.798108  -0.50361\n",
      " 0.485519  0.313692  0.816007\n",
      "  p = 0.0646125 0.0949587  -7.59329\n",
      "\n",
      "Basis placement:   R =\n",
      "1 0 0\n",
      "0 1 0\n",
      "0 0 1\n",
      "  p =  0.3    0 0.15\n",
      "\n"
     ]
    }
   ],
   "source": [
    "pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "print(\"Tool placement:\",robot.data.oMf[IDX_TOOL])\n",
    "print(\"Basis placement:\",robot.data.oMf[IDX_BASIS])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is very important to notice the instruction pattern (which is standard in all Pinocchion functions): first call a whole-body algorithm (here *pin.framesForwardKinematics*), then access the results into *robot.data*. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The tool-placement matrix oMtool represents the displacement from the world frame F_o to the tool frame F_tool. It is composed on a rotation matrix oRtool and a 3D vector o_OT: oMf = [ oRf o_OT ], when o_OT is the vector from the origin of frame F_o to the origin of from F_tool ; this vector is expressed in the world frame F_o. We can rather express OT in the tool frame F_tool by multiplying it by oRtool:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [],
   "source": [
    "oMtool = robot.data.oMf[IDX_TOOL]\n",
    "oRtool = oMtool.rotation; o_OT = oMtool.translation\n",
    "tool_OT = oRtool.T @ o_OT"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Take care to the multiplication operator in numpy. The operator* is (unintuitively) mapped to the coefficient-wise multiplication ... i.e. not at all the matrix multiplication. You should use the operator @ to get the real matrix-matrix product. \n",
    "Never mind, you will likely be tricked at least once by this design pattern. If you don't like it ... well there is nothing to do, this is the basic matrix library in Python, the world most used language. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Computing Jacobians\n",
    "The jacobian of a frame of the robot is computed using pio.computeFrameJacobian."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(6, 12)"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL)\n",
    "Jtool.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This matrix has 6 rows and NV=15 columns. It corresponds to the \"spatial\" 6D velocity of the end effector. Let's first focus on the 3 first rows, corresponding to the linear velocity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [],
   "source": [
    "Jtool3 = Jtool[:3,:]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Jacobian as a velocity operator\n",
    "A first way to understand what is this matrix is to see that as an operator that converts the velocity in the configuration space into the linear velocity of the end effector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [],
   "source": [
    "vtool = Jtool3 @ vq"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But in which frame is vtool expressed? The choice in Pinocchio (following algorithmic principles described in [Featherstone 2009]) is to expressed quantities in the local frame by default. So vtool is expressed in the tool frame F_tool (or more precisely, the Galilean frame coinciding with F_tool at the current time instant). We should better denote it with its frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [],
   "source": [
    "tool_Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL)\n",
    "tool_Jtool3 = tool_Jtool[:3,:]\n",
    "tool_vtool = tool_Jtool3 @ vq"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We are using here long variable names. In the code, these explicit notations are maybe to much. It is your choice to use them or not. For sure, you should use similar notations on the paper board when formulating your algorithm, but maybe you prefer lighter variable naming for implementing it.\n",
    "The tool velocity is easier to interpret in the world frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [],
   "source": [
    "o_vtool = oRtool @ tool_vtool"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We generalize this notation to the jacobian expressed in the world frame F_o:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [],
   "source": [
    "o_Jtool3 = oRtool @ tool_Jtool3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Jacobian as a derivative\n",
    "A second interpretation to explain what is the jacobian is to observe that it is the derivative of the vector o_OT (the tool position in world frame). Indeed o_OT is a function of q: o_OT(q). We can take its derivative with respect to q, denoted d o_OT / dq. This derivatives is equal to the Jacobian expressed in the world frame F_o: d o_OT / dq = o_Jtool.\n",
    "To be convinced of that, let's check the finite differences. We take a small movement dq, and see that the change in position o_OT(q+dq) corresponds to the linear prediction o_Jtool3*dq."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.06400179 -0.07806081 -0.46450085]\n",
      "[-0.06406556 -0.078047   -0.46448161]\n"
     ]
    }
   ],
   "source": [
    "# Sample between -0.001 and 0.001\n",
    "EPS = 1e-3\n",
    "dq = (np.random.rand(robot.model.nv)*2-1)*EPS\n",
    "\n",
    "# q2 = q+dq\n",
    "q2 = pin.integrate(robot.model,q,dq)                     \n",
    "pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "\n",
    "# tool position for q\n",
    "o_OT = robot.data.oMf[IDX_TOOL].translation.copy()\n",
    "\n",
    "# tool position for q+dq\n",
    "pin.framesForwardKinematics(robot.model,robot.data,q2)\n",
    "o_OT2 = robot.data.oMf[IDX_TOOL].translation.copy()\n",
    "\n",
    "print((o_OT2 - o_OT).T/EPS)\n",
    "print((o_Jtool3@dq/EPS).T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Frame options in Pinocchio\n",
    "Most algorithms accept an option to specify in wich frame the spatial quantity should be expressed. The two basic options are *pin.LOCAL* and *pin.WORLD*. When related to velocity, *LOCAL* is the linear velocity of the center of the local frame (the TOOL_IDX frame, here) and the angular velocity, both expressed in the local frame. With *WORLD* frame, the two velocities are expressed in the world frame ... but remember that the linear velocity is then difficult to interpret.\n",
    "\n",
    "A last option is given by convenience, which does not respect the mathematics of spatial velocity, but matches the French \"torseur cinématique\": pin.LOCAL_WORLD_ALIGNED gives the linear velocity of the center of the local frame and the angular velocity, both expressed in the world frame. It is convenient especially when we are interested to consider the linear velocity as the derivative of the position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [],
   "source": [
    "o_Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,\n",
    "                                   pin.LOCAL_WORLD_ALIGNED)[:3,:]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You may see this video which explain with more illustriations the 3 frame options:  https://youtu.be/MLFtHLTprE4 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Jacobian in 6D\n",
    "The jacobian of the frame indeed as 6 rows, and corresponds to the spatial velocity of the frame. It is expressed locally, in the tool frame (or more precisely the Galilean frame coincinding with the tool frame to the current time instant)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [],
   "source": [
    "tool_J = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL)\n",
    "tool_nu = tool_J @ vq"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We denote by nu the spatial velocity. It is composed by the linear velocity of the center of the frame vtool and the angular velocity of the tool frame, both expressed in the tool frame: tool_nu = [ tool_vtool, tool_w ].\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "tool_vtool = tool_nu[:3] ; tool_w = tool_nu[3:]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can rather expressed the spatial velocity in the world frame F_o by multiplying it by the so-called action matrix oXtool."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-10.27701894   1.50315172  -1.07705109  -0.16997645  -1.35064114\n",
      "  -0.94447856]\n"
     ]
    }
   ],
   "source": [
    "o_nu = oMtool.action @ tool_nu\n",
    "print(o_nu)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Log in SE(3)\n",
    "Finally, the log operator transforms a displacement M in SE(3) into the spatial (6D) velocity that should be applied during 1 second to achieve this displacement. The spatial velocity is expressed in the frame at the origin of the displacement (i.e. nu=log(oMtool) is expressed in the world frame F_o). In Pinocchio, the log returns an object of class Motion, that can be converted to a numpy vector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-1.6717141  -1.51713667 -7.3815157   0.45309705 -0.42646367  0.46854601]\n"
     ]
    }
   ],
   "source": [
    "o_nu = pin.log(oMtool).vector\n",
    "print(o_nu.T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse kinematics for the moving the robot effector\n",
    "We will first move only the robot end effector, to reach a target defined by a frame F_goal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load -r 22-28 tp3/generated/inverse_kinematics_goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Position the effector (3d)\n",
    "It is time to write your first control law. Write a for-loop to iterate along the control cycles of the robot. At each control cycle, you should:\n",
    "* compute the Jacobian 3D in the world frame o_Jtool3\n",
    "* compute the vector from the tool to the goal, expressed in world frame: o_TG = o_goal - o_tool\n",
    "* compute the control law as vq=-pinv(J)*o_TG\n",
    "* integrated the velocity vq during DT to get a new configuration q.\n",
    "You might want to start from the following initial configuration, or from any random one."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load tp3/generated/inverse_kinematics_init\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is a good idea to store the values of the error between tool and goal o_TG, to plot them later. For that, simply append each o_TG computed at every control cycle in a list."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp3/generated_inverse_kinematics_3d_loop\n",
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between tool and goal.\n",
    "# Loop on an inverse kinematics for 200 iterations.\n",
    "for i in range(200):  # Integrate over 2 second of robot life\n",
    "\n",
    "    # Run the algorithms that outputs values in robot.data\n",
    "    pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "    pin.computeJointJacobians(robot.model,robot.data,q)\n",
    "\n",
    "    # Placement from world frame o to frame f oMtool\n",
    "    oMtool = robot.data.oMf[IDX_TOOL]\n",
    "\n",
    "    # 3D jacobian in world frame\n",
    "    o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:]\n",
    "\n",
    "    # vector from tool to goal, in world frame\n",
    "    o_TG = oMtool.translation-oMgoal.translation\n",
    "    \n",
    "    # Control law by least square\n",
    "    vq = -pinv(o_Jtool3)@o_TG\n",
    "\n",
    "    q = pin.integrate(robot.model,q, vq * DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-3)\n",
    "\n",
    "    herr.append(o_TG) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is interesting to plot the behavior of the robot. If the error at each iteration has been stored as a list of 3x1 matrices, the following code plots it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(herr)\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (m)');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can observe that each component of the error converges toward 0 following an exponential trajetory. The convergence is assymptotic. To fasten the convergence, increase the gain of the control law ($v_q = - \\lambda J^+ e$), where the gain $\\lambda$ has yet been set to 1."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Place the end effector (6D)\n",
    "The previous control law brings the center of the effector toward the center of the goal frame. However, it does not control the orientation of the end effector: the axes of the two frames F_tool and F_goal do not converge.\n",
    "We should now modify the control law to take into account the tool orientation. For that, we compute the error to be the SE(3) log of the displacement from the tool frame F_tool to the goal frame F_goal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "toolMgoal = oMtool.inverse()*oMgoal\n",
    "tool_nu = pin.log(toolMgoal).vector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This error tool_nu is a 6d vector, that can be interpreted as the spatial (6d) velocity that should be applied during 1 second to displace the tool frame F_tool (placed at oMtool) to the goal frame F_goal (placed at oMgoal). This spatial velocity is expressed in the tool frame F_tool. It then corresponds to the 6D jacobian, that is also computed in the same frame F_tool.\n",
    "\n",
    "Implement a second control law, following the same pattern than the previous control law. At each control cycle, you should:\n",
    "* compute the displacement between F_tool and F_goal, denoted toolMgoal\n",
    "* compute the 6D error using the SE(3) log tool_nu\n",
    "* compute the 6D jacobian tool_Jtool\n",
    "* compute the control law vq = pinv(J)*nu\n",
    "* integrate the robot velocity vq during DT to get a new configuration q\n",
    "* log the error by storing it in a list herr."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp3/generated/inverse_kinematics_6d_loop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The tool frame F_tool converges toward the gooal frame F_goal: the center and the axes are finally aligned. The trajectory of the tool center is not a straight line, as the frame F_tool follows a \"straight\" line, not in R^3 but in SE(3).\n",
    "We can also plot the error (assuming that herr is a list of the 6D errors herr)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp3/generated/inverse_kinematics_plot\n",
    "plt.subplot(211)\n",
    "plt.plot([ e[:3] for e in herr])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (m)')\n",
    "plt.subplot(212)\n",
    "plt.plot([ e[3:] for e in herr])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (rad)');\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse kinematics for two tasks\n",
    "We yet controlled the robot with a single task (either the 3d position or the 6d placement errors). Let's see how to take into account a second task. Let's first introduce a second task. \n",
    "\n",
    "#### Introducing a second task: control the gaze\n",
    "The robot has an additional frame named F_gaze, attached to the head and located 40 cm in front of the cameras. The task will be to position (3d) the center of this frame on an object of interest (a red ball)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/opt/openrobots/lib/python3.8/site-packages/pinocchio/shortcuts.py:45: UserWarning: You passed package dir(s) via argument geometry_model and provided package_dirs.\n",
      "  geom_model = pin.buildGeomFromUrdf(model, filename, geometry_type, package_dirs)\n"
     ]
    }
   ],
   "source": [
    "# %load tp3/generated/control_head_robot\n",
    "robot = loadTiago(addGazeFrame=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp3/generated/control_head_gaze\n",
    "IDX_GAZE = robot.model.getFrameId('framegaze')\n",
    "\n",
    "# Add a small ball as a visual target to be reached by the robot\n",
    "ball = np.array([ 1.2,0.5,1.1 ])\n",
    "viz.addSphere('ball', .05, [ .8,.1,.5, .8] )\n",
    "viz.applyConfiguration('ball', list(ball)+[0,0,0,1])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'q0' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Input \u001b[0;32mIn [54]\u001b[0m, in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0m viz\u001b[38;5;241m.\u001b[39mdisplay(\u001b[43mq0\u001b[49m)\n\u001b[1;32m      2\u001b[0m viz\u001b[38;5;241m.\u001b[39mviewer\u001b[38;5;241m.\u001b[39mjupyter_cell()\n",
      "\u001b[0;31mNameError\u001b[0m: name 'q0' is not defined"
     ]
    }
   ],
   "source": [
    "viz.display(q0)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Controlling this point can be done by achieving a simple variation of the control law for positioning (3d) the robot tool."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load tp3/generated/control_head_gaze_loop\n",
    "\n",
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between gaze and ball.\n",
    "# Loop on an inverse kinematics for 200 iterations.\n",
    "for i in range(200):  # Integrate over 2 second of robot life\n",
    "\n",
    "    # Run the algorithms that outputs values in robot.data\n",
    "    pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "    pin.computeJointJacobians(robot.model,robot.data,q)\n",
    "\n",
    "    # Placement from world frame o to frame f oMgaze\n",
    "    oMgaze = robot.data.oMf[IDX_GAZE]\n",
    "\n",
    "    # 6D jacobian in local frame\n",
    "    o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:]\n",
    "\n",
    "    # vector from gaze to ball, in world frame\n",
    "    o_GazeBall = oMgaze.translation-ball\n",
    "    \n",
    "    vq = -pinv(o_Jgaze3) @ o_GazeBall\n",
    "\n",
    "    q = pin.integrate(robot.model,q, vq * DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-3)\n",
    "\n",
    "    herr.append(o_GazeBall) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Performing 2 tasks\n",
    "\n",
    "We now have two tasks (e1,J1) controlling the tool and (e2,J2) controlling the gaze. \n",
    "\n",
    "The null space projector of +J1+ can be computed using the pseudoinverse.\n",
    "Following the control law performing task 1 and task 2 in the null space of task 1 is:\n",
    "$$vq_1 = J_1^+ v_1^*$$\n",
    "$$P_1 = I_9 - J_1^+ J_1$$\n",
    "$$vq_2 = vq_1 + (J_2 P_1)^+ ( v_2^* - J_2 vq_1)$$\n",
    "\n",
    "You can now implement a control law solving the two tasks, i.e positioning the tool while controlling the gaze."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp3/generated/control_head_multi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A third task can be implemented as well by computing the null space of the two first tasks"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Pgaze = Ptool - pinv(o_Jgaze3 @ Ptool) @ o_Jgaze3 @ Ptool"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Homework\n",
    "\n",
    "Load an extra cube in the viewer to figure a table. First control the robot hand to reach an arbitrary point on the table (don't mind for the collision). Then implement a control law to control three tasks:\n",
    "* the tool frame should be kept on the table (i.e. only the z component of the error matter, select only the 3rd row of the matrix).\n",
    "* the gaze should be control to reach the position of a ball object positionned on the table.\n",
    "* the center of the basis frame should reach a given target on the floor. For this task, only the x- and y- component of the task matter, select only the 2 first rows.\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}