0-introduction_to_numerical_robotics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Introduction to numerical robotics\n",
    "\n",
    "The aim of this notebook is to illustrate common robotic problems and motivate the need for the geometrical tools we are going to present in class. We will first try to solve these problems naively to convince ourselves that we need better tools to reliably achieve our objectives.\n",
    "\n",
    "This notebook also provides a brief overview of [Pinocchio](https://stack-of-tasks.github.io/pinocchio/), a robotics library which we are going to use throughout the semester, in particular for the tutorials, as well as the meshcat visualiser that we will use for displaying our robots. We will briefly see the main methods used to manipulate the geometry model of a manipulator robot: set the configuration (posture), compute the position of an effector, check the collision or the distance to the obstacle. \n",
    "\n",
    "As such this notebook is an informal first step towards our objective of discovering how to make a robot move in a constrained the environment using iterative optimization methods.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import gepetuto.magic"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "\n",
    "We are going to work with the manipulator robot [UR5](https://www.universal-robots.com/products/ur5-robot/). The model of the robot is provided for you in the form of an XML-derived description file format called [urdf](http://wiki.ros.org/urdf/XML/model). You don't need to understand the format in details for this course.\n",
    "\n",
    "We are going to import a few libraries / functions to set the scene accordingly.\n",
    "\n",
    "(urdf, in the robotpkg_example_robot_data), the pinocchio python software, the python optimizers from scipy and for the display of the results: the gepetto viewer and the python matplotlib. We have that with this set of imports:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_import\n",
    "import pinocchio as pin #the pinocchio library\n",
    "from supaero2023.meshcat_viewer_wrapper import MeshcatVisualizer # the meshcat visualiser\n",
    "from supaero2023.load_ur5_with_obstacles import load_ur5_with_obstacles,Target # helper function to load scene\n",
    "from scipy.optimize import fmin_bfgs,fmin_slsqp # some optimisation routines from scipy\n",
    "import time # the remaining libraries are python native\n",
    "import numpy as np \n",
    "from numpy.linalg import norm\n",
    "import matplotlib.pylab as plt \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this class, we provide a helper method to load the robot model and create an obstacle field. The \"reduced\" parameter will be explained in a minute."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_robot\n",
    "\n",
    "robot = load_ur5_with_obstacles(reduced=True)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next few lines initialize a 3D viewer."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_viewer\n",
    "\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The scene should now be displayed in the viewer (check it). You can maintain left-click and move your mouse to change the camera orientation, and zoom using the mouse scroll wheel. Maitaining the right-click and moving around translates the camera while maintenain the orientation. The robot and the red obstacles are encoded in the robot object (we will not need to see in depth what is inside this object). \n",
    "\n",
    "You can display a new configuration of the robot with viz.display, which currently takes as input a numpy.array of dimension 2. These 2 values correspond to a desired joint angle value (in radians) for two articulations of the robot. You can play with the code below to change the configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(np.array([1.,-1.5]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also set up a target, visualised as a green dot. The objective here is that the end of the robot reaches the target. You can change the target position by editing target.position, and display the new position with target.display()."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_target\n",
    "target = Target(viz,position = np.array([.5,.5]))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Using the robot model\n",
    "The robot is originally a 6 degrees-of-freedom (DOF) manipulator. Yet to make the example simple, we will only use the joints 1 and 2. The model has simply been loaded with \"frozen\" extra joints, which will thus not appear in this notebook. Just load the model with reduced=False if you want to recover a model with full DOF. We are now given 3 functions that will help us to find a configuration of the robot that reaches the target.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following function computes the position of the end effector (in 2d):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_endef\n",
    "def endef(q):\n",
    "     '''Return the 2d position of the end effector given a configuration q.'''\n",
    "     pin.framesForwardKinematics(robot.model,robot.data,q) # applies q to the robot model\n",
    "     return robot.data.oMf[-1].translation[[0,2]] #extracts the position of the effector\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next function computes the distance between the end effector and the target."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 60,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_dist\n",
    "def dist(q):\n",
    "     '''Given q, return the distance between the end effector and the target (2d).'''\n",
    "     return norm(endef(q)-target.position)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The last function checks if the robot is in collision, and returns True if a collision is detected."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 61,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_coll\n",
    "\n",
    "def coll(q):\n",
    "     '''Return true if in collision, false otherwise.'''\n",
    "     pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)\n",
    "     return pin.computeCollisions(robot.collision_model,robot.collision_data,False)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Random search of a valid configuration\n",
    "The free space of the robot is defined as the set of configurations such that the robot is not in collision. Although a configuration only has 2 parameters in our example, we can't easily compute this space. However, we can determine if a configuration belongs to the free space or not.  We can thus randomly sample the configuration space until a free configuration is found:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_qrand\n",
    "\n",
    "def qrand(check=False):\n",
    "    '''\n",
    "    Return a random configuration. If check is True, this\n",
    "    configuration is not is collision\n",
    "    '''\n",
    "    while True:\n",
    "        q = np.random.rand(2)*6.4-3.2  # sample between -3.2 and +3.2.\n",
    "        if not check or not coll(q): return q\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(qrand(check=True))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's now find a valid configuration that is arbitrarily close to the target: sample until dist is small enough and coll is false (you may want to display the random trials inside the loop)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_qrand\n",
    "def qrand(check=False):\n",
    "    '''\n",
    "    Return a random configuration. If check is True, this\n",
    "    configuration is not is collision\n",
    "    '''\n",
    "    while True:\n",
    "        q = np.random.rand(2)*6.4-3.2  # sample between -3.2 and +3.2.\n",
    "        if not check or not coll(q): return q\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## From a random configuration to the target\n",
    "Let' s now start from a random configuration. How can we find a path that bring the robot toward the target without touching the obstacles. Any idea?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 70,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load_snippet tp0/generated/simple_path_planning_random_descent"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Configuration space\n",
    "Let's try to have a better look at the configuration space. In this case, it is easy, as it is dimension 2: we can sample it exhaustively and plot it in 2d. For that, let's introduce another function to compute the distance to collision:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 71,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_colldist\n",
    "def collisionDistance(q):\n",
    "     '''Return the minimal distance between robot and environment. '''\n",
    "     pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)\n",
    "     if pin.computeCollisions(robot.collision_model,robot.collision_data,False): 0\n",
    "     idx = pin.computeDistances(robot.collision_model,robot.collision_data)\n",
    "     return robot.collision_data.distanceResults[idx].min_distance\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, let's sample the configuration space and plot the distance-to-target and the distance-to-obstacle field as a function of the 2 angle values that determine the configuration of the robot (We use 1000 samples to spare your CPU, but you need at least 5x more for a good picture)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 102,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_sample\n",
    "def sampleSpace(nbSamples=500):\n",
    "     '''\n",
    "     Sample nbSamples configurations and store them in two lists depending\n",
    "     if the configuration is in free space (hfree) or in collision (hcol), along\n",
    "     with the distance to the target and the distance to the obstacles.\n",
    "     '''\n",
    "     hcol = []\n",
    "     hfree = []\n",
    "     for i in range(nbSamples):\n",
    "          q = qrand(False)\n",
    "          if not coll(q):\n",
    "               hfree.append( list(q.flat) + [ dist(q), collisionDistance(q) ])\n",
    "          else:\n",
    "               hcol.append(  list(q.flat) + [ dist(q), 1e-2 ])\n",
    "     return hcol,hfree\n",
    "\n",
    "def plotConfigurationSpace(hcol,hfree,markerSize=20):\n",
    "     '''\n",
    "     Plot 2 \"scatter\" plots: the first one plot the distance to the target for \n",
    "     each configuration, the second plots the distance to the obstacles (axis q1,q2, \n",
    "     distance in the color space).\n",
    "     '''\n",
    "     htotal = hcol + hfree\n",
    "     h=np.array(htotal)\n",
    "     plt.subplot(2,1,1)\n",
    "     plt.scatter(h[:,0],h[:,1],c=h[:,2],s=markerSize,lw=0)\n",
    "     plt.title(\"Distance to the target\")\n",
    "     plt.colorbar()\n",
    "     plt.subplot(2,1,2)\n",
    "     plt.scatter(h[:,0],h[:,1],c=h[:,3],s=markerSize,lw=0)\n",
    "     plt.title(\"Distance to the obstacles\")\n",
    "     plt.colorbar()\n",
    "\n",
    "hcol,hfree = sampleSpace(1000)\n",
    "plotConfigurationSpace(hcol,hfree)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can try to match your representation of the free space of the robot with this plot. \n",
    "As an example, you can display on this plot some trajectories discovered during a random walk."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 90,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp0/generated/simple_path_planning_traj\n",
    "qinit = np.array([-1.1, -3. ])\n",
    "for i in range(100):\n",
    "     traj = randomDescent(qinit)\n",
    "     if dist(traj[-1])<5e-2:\n",
    "          print('We found a good traj!')\n",
    "          break\n",
    "traj = np.array(traj)\n",
    "### Chose trajectory end to be in [-pi,pi]\n",
    "qend = (traj[-1]+np.pi) % (2*np.pi) - np.pi\n",
    "### Take the entire trajectory it modulo 2 pi\n",
    "traj += (qend-traj[-1])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 89,
   "metadata": {},
   "outputs": [],
   "source": [
    "if 'traj' in locals():   ### Skip this cell if the previous block is not solved.\n",
    "    plotConfigurationSpace(hcol,hfree)\n",
    "    plt.plot(traj[:,0],traj[:,1],'r',lw=3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Optimize the distance under non-collision constraint\n",
    "Finally, let's use one of the classic python solvers (from scipy) to search for a robot configuration that optimizes the distance to the target, under the constraint that the distance to collision is positive.\n",
    "For that, we need to define a cost function (taking the robot configuration and return a scalar) and a constraint function (taking again the robot configuration and returning a scalar or a vector of scalar that should be positive). We additionally use the \"callback\" functionality of the solver, to render the robot configuration corresponding to the current value of the decision variable inside the solver algorithm.\n",
    "We are going to use the \"SLSQP\" solver from scipy, which implements a \"sequential quadratic program\" algorithm and accepts constraints.\n",
    "\n",
    "**See [the notebook about SciPy optimizers](appendix_scipy_optimizers.ipynb) for details.**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp0/generated/simple_path_planning_optim"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Look at the output of the solver. It always returns a variable value, but sometimes the algorithm fails being traped in an unfeasible region. Most of the time, the solver converges to a local minimum where the final distance to the target is nonzero"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 99,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp0/generated/simple_path_planning_useit"
   ]
  }
 ],
 "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": 4
}