tutorials/2-features.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Features and Objectives\n",
    "\n",
    "This doc is mostly text, explaining the general concept of features, listing the ones defined in rai, and explaining how they define objectives for optimization.\n",
    "\n",
    "At the bottom there are also examples on the collision features."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Features\n",
    "\n",
    "We assume a single configuration $x$, or a whole set of configurations\n",
    "$\\{x_1,..,x_T\\}$. Each $x_i \\in\\mathbb{R}$ are the DOFs of that\n",
    "configuration.\n",
    "\n",
    "A feature $\\phi$ is a differentiable mapping\n",
    "$$\\phi: x \\mapsto \\mathbb{R}^D$$\n",
    "of a single configuration into some $D$-dimensional space, or a mapping\n",
    "$$\\phi: (x_0,x_2,..,x_k) \\mapsto \\mathbb{R}^D$$\n",
    "of a $(k+1)$-tuple of configurations to a $D$-dimensional space.\n",
    "\n",
    "The rai code implements many features, most of them are accessible via\n",
    "a feature symbol (FS). They are declared in\n",
    "https://github.com/MarcToussaint/rai/blob/master/rai/Kin/featureSymbols.h\n",
    "\n",
    "Here is a table of feature symbols, with the\n",
    "respective dimensionality $D$, the default order $k$, and a\n",
    "description\n",
    "\n",
    "| FS | frames | $D$ | $k$ | description |\n",
    "|:---:|:---:|:---:|:---:|:---:|\n",
    "| position | {o1} | 3 || 3D position of o1 in world coordinates |\n",
    "| positionDiff | {o1,o2} | 3 || difference of 3D positions of o1 and o2 in world coordinates |\n",
    "| positionRel | {o1,o2} | 3 || 3D position of o1 in o2 coordinates |\n",
    "| quaternion | {o1} | 4 || 4D quaternion of o1 in world coordinates\\footnote{There is ways to handle the invariance w.r.t.\\ quaternion sign properly.} |\n",
    "| quaternionDiff | {o1,o2} | 4 || ... |\n",
    "| quaternionRel | {o1,o2} | 4 || ... |\n",
    "| pose | {o1} | 7 || 7D pose of o1 in world coordinates |\n",
    "| poseDiff | {o1,o2} | 7 || ... |\n",
    "| poseRel | {o1,o2} | 7 || ... |\n",
    "| vectorX | {o1} | 3 || The x-axis of frame o1 rotated back to world coordinates |\n",
    "| vectorXDiff | {o1,o2} | 3 || The difference of the above for two frames o1 and o2 |\n",
    "| vectorXRel | {o1,o2} | 3 || The x-axis of frame o1 rotated as to be seend from the frame o2 |\n",
    "| vectorY... | | | | same as above |\n",
    "| scalarProductXX | {o1,o2} | 1 || The scalar product of the x-axis fo frame o1 with the x-axis of frame o2 |\n",
    "| scalarProduct... | {o1,o2} | | | as above |\n",
    "| gazeAt | {o1,o2} | 2 | | The 2D projection of the origin of frame o2 onto the xy-plane of frame o1 |\n",
    "| angularVel | {o1} | 3 | 1 | The angular velocity of frame o1 across two configurations |\n",
    "| accumulatedCollisions | {} | 1 | | The sum of collision penetrations; when negative/zero, nothing is colliding |\n",
    "| jointLimits | {} | 1 | | The sum of joint limit penetrations; when negative/zero, all joint limits are ok |\n",
    "| distance | {o1,o1} | 1 | | The NEGATIVE distance between convex meshes o1 and o2, positive for penetration |\n",
    "| qItself | {} | $n$ | | The configuration joint vector |\n",
    "| aboveBox | {o1,o2} | 4 | | when all negative, o1 is above (inside support of) the box o2 |\n",
    "| insideBox | {o1,o2} | 6 | | when all negative, o1 is inside the box o2 |\n",
    "| standingAbove | | | | ? |\n",
    "\n",
    "A features is typically defined by\n",
    "* The feature symbol (`FS_...` in cpp; `FS....` in python)\n",
    "* The set of frames it refers to\n",
    "* Optionally: A target, which changes the zero-point of the features (optimization typically try to drive features to zero, see below)\n",
    "* Optionally: A scaling, that can also be a matrix to down-project a feature\n",
    "* Optionally: The order $k$, which can make the feature a velocity or acceleration feature\n",
    "\n",
    "Target and scale redefine a feature to become\n",
    "$$\n",
    "  \\phi(x) \\gets \\texttt{scale} \\cdot (\\phi(x) - \\texttt{target})\n",
    "$$\n",
    "The target needs to be a $D$-dim vector. The scale can be a matrix, which projects features; e.g., and 3D position to just $x$-position.\n",
    "\n",
    "The order of a feature is usually $k=0$, meaning that it is defined over a single configuration only. $k=1$ means that it is defined over two configurations (1st oder Markov), and redefines the feature to become the difference or velocity\n",
    "$$\n",
    "  \\phi(x_1,x_2) \\gets \\frac{1}{\\tau}(\\phi(x_2) - \\phi(x_1))\n",
    "$$\n",
    "$k=2$ means that it is defined over three configurations (2nd order Markov), and redefines the feature to become the acceleration\n",
    "$$\n",
    "  \\phi(x_1,x_2,x_3) \\equiv \\frac{1}{\\tau^2}(\\phi(x_1) + \\phi(x_3) - 2 \\phi(x_2))\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Examples\n",
    "\n",
    "```\n",
    "(FS.position, {'hand'})\n",
    "```\n",
    "is the 3D position of the hand in world coordinates\n",
    "\n",
    "```\n",
    "(FS.positionRel, {'handL', 'handR'}, scale=[[0,0,1]], target=[0.1])\n",
    "```\n",
    "is the z-position position of the left hand measured in the frame of the right hand, with target 10centimeters.\n",
    "\n",
    "```\n",
    "(FS.position, {'handL'}, order=1)\n",
    "```\n",
    "is the 3D velocity of the left hand in world coordinates\n",
    "\n",
    "```\n",
    "(FS.scalarProductXX, {'handL', 'handR'}, target=[1])\n",
    "```\n",
    "says that the scalar product of the x-axes (e.g. directions of the index finger) of both hands should equal 1, which means they are aligned.\n",
    "\n",
    "```\n",
    "(FS.scalarProductXY, {'handL', 'handR'})\n",
    "(FS.scalarProductXZ, {'handL', 'handR'})\n",
    "```\n",
    "says that the the x-axis of handL should be orthogonal (zero scalar product) to the y- and z-axis of handR. So this also describes aligning both x-axes. However, this formulation is much more robust, as it has good error gradients around the optimum."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Objectives\n",
    "\n",
    "Features are meant to define objectives in an optimization problem. An objective is\n",
    "* a feature\n",
    "* an indicator $\\rho_k\n",
    "\\in\\{\\texttt{ineq, eq, sos}\\}$ that states whether the features\n",
    "implies an inequality, an equality, or a sum-of-square objective\n",
    "* and an index tuple $\\pi_k \\subseteq \\{1,..,n\\}$ that states which\n",
    "configurations this feature is defined over.\n",
    "\n",
    "Then, given a set\n",
    "$\\{\\phi_1,..,\\phi_K\\}$ of $K$ features, and a set $\\{x_1,..,x_n\\}$ of\n",
    "$n$ configurations, this defines the mathematical program\n",
    "\n",
    "\\begin{align}\n",
    "  \\min_{x_1,..,x_n} \\sum_{k : \\rho_k=\\texttt{sos}} \\phi_k(x_{\\pi_k})^T \\phi_k(x_{\\pi_k})\n",
    "  ~\\text{s.t.}~ \\mathop\\forall_{k : \\rho_k=\\texttt{ineq}} \\phi_k(x_{\\pi_k}) \\le 0 ~,\\quad\n",
    "  \\mathop\\forall_{k : \\rho_k=\\texttt{eq}} \\phi_k(x_{\\pi_k}) = 0 ~,\\quad\n",
    "\\end{align}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Code example for collision features\n",
    "\n",
    "* Get list of collisions and proximities for the whole configuration\n",
    "* Get a accumulative, differentiable collision measure\n",
    "* Get proximity/penetration specifically for a pair of shapes\n",
    "* Other geometric collision features for a pair of shapes (witness points, normal, etc) -- all differentiable"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append('../build')\n",
    "import numpy as np\n",
    "import libry as ry\n",
    "\n",
    "C = ry.Config()\n",
    "C.addFile('../rai-robotModels/pr2/pr2.g');\n",
    "C.addFile('../rai-robotModels/objects/kitchen.g');\n",
    "\n",
    "C.view()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's evaluate the accumulative collision scalar and its Jacobian"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "coll = C.feature(ry.FS.accumulatedCollisions, [])\n",
    "\n",
    "C.computeCollisions() #collisions/proxies are not automatically computed on set...State\n",
    "coll.eval(C)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's move into collision and redo this"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.selectJointsByTag([\"base\"])\n",
    "C.setJointState([1.5,1,0])\n",
    "\n",
    "C.computeCollisions()\n",
    "coll.eval(C)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can get more verbose information like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.getCollisions()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.getCollisions(0) #only report proxies with distance<0 (penetrations)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The computeCollisions() method calls a collision detection engine (SWIFT++) for the whole configuration, checking all shapes that are collision-activated. The activation/deactivation of collision computations is a nuissance! the 'contact' flag in g-files specifies which shapes are activated by default, and if the value is negative, that collisions with parent shapes are not included. (In the KOMO class, you can use activateCollisionPairs and deactivateCollisionPairs to modify these defaults in optimization problems... TODO: also in Config)\n",
    "\n",
    "When you're interested in the distance or penetration of one specific pair of objects, you don't need to call computeCollisions() and instead query a feature that calls the GJK (and others) algorithm directly only for this pair:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dist = C.feature(ry.FS.distance, ['coll_wrist_r', '_10'])\n",
    "dist.eval(C)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that this returns the NEGATIVE distance (because one typically wants to put an inequality (<=0) on this). The C++ code implements many more features of the collision geometry, including the normal, witness points, etc. Can be added to python easily on request."
   ]
  }
 ],
 "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}