2_geometry_3d.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Direct and inverse geometry of 3d robots\n",
    "This notebook introduces the kinematic tree of Pinocchio for a serial manipulator, explain how to compute the forward and inverse geometry (from configuration to end-effector placements, and inversely). The ideas are examplified with a simplified case-study taken from parallel robotics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload  # noqa: F401"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "import numpy as np\n",
    "from numpy.linalg import norm\n",
    "import pinocchio as pin\n",
    "import example_robot_data as robex\n",
    "from scipy.optimize import fmin_bfgs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kinematic tree in Pinocchio\n",
    "Let's now play with 3D robots. We will load the models from URDF files.\n",
    "\n",
    "*The robot UR5* is a low-cost manipulator robot with good performances. It is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format \".dae\". Both the URDF and the DAE files are available in the repository in the model directory. \n",
    "\n",
    "This robot model, as well as other models used in the notebooks, are installed from the apt paquet robotpkg-example-robot-data and stored in /opt/openrobots/share/example-robot-data.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_1\n",
    "\n",
    "robot = robex.load('ur5')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The kinematic tree is represented by two C++ objects called Model (which contains the model constants: lengths, masses, names, etc) and Data (which contains the working memory used by the model algorithms). Both C\\++ objects are contained in a unique Python class. The first class is called RobotWrapper and is generic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(robot.model)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the next steps, we are going to work with the RobotWrapper.\n",
    "\n",
    "Import the class RobotWrapper and create an instance of this class in the python terminal. At initialization, RobotWrapper will read the model description in the URDF file given as argument. In the following, we will use the model of the UR5 robot, available in the directory \"models\" of pinocchio (available in the homedir of the VBox). The code of the RobotWrapper class is in /opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.\n",
    "\n",
    "Here are some import methods of the class.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.q0 contains a reference initial configuration of the robot (not a pretty good one for the UR-5)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.index('joint name') returns the index of the joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.index(' wrist_3_joint')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.names is a container (~list) that contains all the joint names"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for i, n in enumerate(robot.model.names):\n",
    "    print(i, n)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.frames contains all the import frames attached to the robot. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for f in robot.model.frames:\n",
    "    print(f.name, 'attached to joint #', f.parent)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.placement(idx) and robot.framePlacement(idx) returns the placement (i.e. translation+rotation of the joint / frame in argument."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.placement(robot.q0, 6)  # Placement of the end effector."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The dimension of the configuration space (i.e. the number of joints) is given in:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "NQ = robot.model.nq\n",
    "NV = robot.model.nv  # for this simple robot, NV == NQ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Display simple geometries\n",
    "The robot is displayed in the viewer. We are going to use Meshcat to visualize the 3d robot and scene. First open the viewer and load the robot geometries."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors  # noqa: E402\n",
    "\n",
    "viz = MeshcatVisualizer(robot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A configuration *q* can be displayed in the viewer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.array([-1., -1.5, 2.1, -.5, -.5, 0])\n",
    "\n",
    "viz.display(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Other geometries (cubes, spheres, etc) can be displayed as well."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_2\n",
    "\n",
    "# Add a red box in the viewer\n",
    "ballID = \"world/ball\"\n",
    "viz.addSphere(ballID, 0.1, colors.red)\n",
    "\n",
    "# Place the ball at the position ( 0.5, 0.1, 0.2 )\n",
    "# The viewer expect position and rotation, apppend the identity quaternion\n",
    "q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n",
    "viz.applyConfiguration(ballID, q_ball)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Forward (direct) geometry\n",
    "\n",
    "First, let's do some forward geometry, i.e. use Pinocchio to compute where is the end effector knowning the robot configuration.\n",
    "\n",
    "# Simple pick ...\n",
    "\n",
    "Say we have a target at position [.5,.1,.2] and we would like the robot to grasp it.\n",
    "First decide (by any way you want, e.g. trial and error) the configuration of the robot so that the end effector touches the ball.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_3\n",
    "\n",
    "q0 = np.zeros(NQ)  # set the correct values here\n",
    "q0[0] = 0.5\n",
    "q0[1] = 0.\n",
    "q0[2] = -1.5\n",
    "q0[3] = 0.\n",
    "q0[4] = 0.\n",
    "q0[5] = 0.\n",
    "\n",
    "viz.display(q0)\n",
    "\n",
    "# Take care to explicitely mention copy when you want a copy of array.\n",
    "q = q0.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ... and simple place"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the reference position you built, the end effector placement can be obtained by calling "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.placement(q, 6).translation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Only the translation part of the placement has been selected. The rotation is free.\n",
    "\n",
    "Now, choose any trajectory you want in the configuration space (it can be sinus-cosinus waves, polynomials, splines, straight lines). Make a for loop to display the robot at sampling positions along this trajectory. The function sleep can be used to slow down the loop."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(.1)  # in second"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At each instant of your loop, recompute the position of the ball and display it so that it always \"sticks\" to the robot end effector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TODO ####################################################\n",
    "# Replace here with your initial configuration\n",
    "q = q0 = np.random.rand(NQ) * 6 - 3\n",
    "# TODO ####################################################\n",
    "\n",
    "# Compute initial translation between effector and box.\n",
    "# Translation of end-eff wrt world at initial configuration\n",
    "o_eff = robot.placement(q, 6).translation\n",
    "# Translation of box wrt world\n",
    "o_ball = q_ball[:3]\n",
    "eff_ball = o_ball - o_eff\n",
    "\n",
    "for i in range(100):\n",
    "    # Replace here by your choice of computing q(t)\n",
    "    q += np.random.rand(6) * 2e-1 - 1e-1\n",
    "\n",
    "    # TODO ####################################################\n",
    "    # Replace here by your computation of the new box position\n",
    "    o_ball = np.array([0., 0., 0.])\n",
    "    # /TODO ###################################################\n",
    "\n",
    "    # Display the new robot and box configurations.\n",
    "    # The viewer expect a placement (position-rotation).\n",
    "    viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n",
    "    viz.display(q)\n",
    "    time.sleep(0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution is below, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp2/generated/simple_pick_and_place_4"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pick and place in 3D\n",
    "\n",
    "Say now that the object is a rectangle and not a sphere. Pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_5\n",
    "\n",
    "# Add a red box in the viewer\n",
    "boxID = \"world/box\"\n",
    "viz.delete(ballID)\n",
    "viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)\n",
    "\n",
    "# Place the box at the position (0.5, 0.1, 0.2)\n",
    "q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n",
    "viz.applyConfiguration('world/box', q_box)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A configuration with the arm nicely attached to the box is:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_6\n",
    "\n",
    "q0 = np.zeros(NQ)\n",
    "q0[0] = -0.375\n",
    "q0[1] = -1.2\n",
    "q0[2] = 1.71\n",
    "q0[3] = -q0[1] - q0[2]\n",
    "q0[4] = q0[0]\n",
    "\n",
    "viz.display(q0)\n",
    "q = q0.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Redo the same question as before, but now also choosing the orientation of the box. For that, at each robot configuration in your for-loop, compute the box placement wrt the world (let's denote it by oMbox) and display both the box and the robot configuration in the view."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp2/generated/simple_pick_and_place_7"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse geometry\n",
    "\n",
    "We only yet computed the forward geometry, i.e. from configurations to end-effector placement. Let's to the inverse map not.\n",
    "\n",
    "### Inverse geometry in 3D\n",
    "\n",
    "Let's now first control the position (i.e. translation only) of the end effector of a manipulator robot to a given position. For this first part, we will use the fixed serial-chain robot model.\n",
    "\n",
    "Recall first that the position (3D) of the joint with index \"i=6\" at position \"q\" can be access by the following two lines of code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.placement(q, 6).translation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the scipy solver [used in the previous notebook](1_geometry_2d.ipynb#section_optim), compute a configuration q where the end effector reaches p. For that, implement a cost function that takes a configuration as argument and returns the squared distance between the end effetor and the target."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp2/generated/invgeom3d_1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse geometry in 6D\n",
    "6D means: translation and rotation. Change the previous cost function for a cost measuring the difference between the current placement root.placement(q,6) and a reference placement oMdes. \n",
    "For that, you can use the SE(3) log function to score the distance between two placements. The log returns a 6D velocity, represented by a class Motion, that must be transformed to a vector of R^6 from which you can take the norm.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.log(pin.SE3.Identity()).vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/invgeom6d_1\n",
    "\n",
    "# --- Add box to represent target\n",
    "viz.addBox(\"world/box\", [.05, .1, .2], [1., .2, .2, .5])\n",
    "viz.addBox(\"world/blue\", [.05, .1, .2], [.2, .2, 1., .5])\n",
    "\n",
    "#\n",
    "# OPTIM 6D #########################################################\n",
    "#\n",
    "\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    M = robot.placement(q, 6)\n",
    "    return norm(pin.log(M.inverse() * Mtarget).vector)\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.applyConfiguration('world/box', Mtarget)\n",
    "    viz.applyConfiguration('world/blue', robot.placement(q, 6))\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-1)\n",
    "\n",
    "\n",
    "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n",
    "\n",
    "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Optimizing in the quaternion space\n",
    "\n",
    "Let's now work with a floating robot: the quadruped ANYmal. This robot has 12 joints, but Q-space of size 19 (robot.model.nq) and Q-tangent space of size 18 (robot.model.nv). This is because with need 7D vector to encode the robot placement in space, which indeed to only 6 DOF.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = robex.load('solo12')\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/floating_1\n",
    "\n",
    "robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']]\n",
    "\n",
    "# --- Add box to represent target\n",
    "colors = ['red', 'blue', 'green', 'magenta']\n",
    "for color in colors:\n",
    "    viz.addSphere(\"world/%s\" % color, .05, color)\n",
    "    viz.addSphere(\"world/%s_des\" % color, .05, color)\n",
    "\n",
    "#\n",
    "# OPTIM 6D #########################################################\n",
    "#\n",
    "\n",
    "targets = [\n",
    "    np.array([-0.7, -0.2, 1.2]),\n",
    "    np.array([-0.3, 0.5, 0.8]),\n",
    "    np.array([0.3, 0.1, -0.1]),\n",
    "    np.array([0.9, 0.9, 0.5])\n",
    "]\n",
    "for i in range(4):\n",
    "    targets[i][2] += 1\n",
    "\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    cost = 0.\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation\n",
    "        cost += norm(p_i - targets[i])**2\n",
    "    return cost\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.applyConfiguration('world/box', Mtarget)\n",
    "\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i])\n",
    "        viz.applyConfiguration('world/%s' % colors[i], p_i)\n",
    "        viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])\n",
    "\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-1)\n",
    "\n",
    "\n",
    "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Configuration of parallel robots\n",
    "A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).\n",
    "\n",
    "The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import utils.load_ur5_parallel as robex2  # noqa: E402\n",
    "\n",
    "robot = robex2.load()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "[w, h, d] = [0.5, 0.5, 0.005]\n",
    "color = [red, green, blue, transparency] = [1, 1, 0.78, .8]\n",
    "viz.addBox('world/robot0/toolplate', [w, h, d], color)\n",
    "Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))\n",
    "viz.applyConfiguration('world/robot0/toolplate', Mtool)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "effIdxs = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]\n",
    "robot.framePlacement(robot.q0, effIdxs[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.\n",
    "\n",
    "Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)): \n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "quat = pin.Quaternion(0.7, 0.2, 0.2, 0.6).normalized()\n",
    "print(quat.matrix())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.9.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}