{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Config-2: Computing differentiable features & collision evaluation\n",
    "\n",
    "The [1st tutorial](https://marctoussaint.github.io/robotics-course/tutorials/1a-configurations.html) introduced the basic concepts of features.\n",
    "The [lecture script section 'Kinematics'](https://marctoussaint.github.io/robotics-course/script/script.html#kinematics) introduces this a bit more formally.\n",
    "\n",
    "This tutorial first gives an overview over available features, and then provides more details specifically on collision features."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Recap of evaluating features directly\n",
    "Let's first recap how features are directly computed on a configuration - as introduced in the [1st tutorial](config_1_intro.ipynb):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.addFile(ry.raiPath('panda/panda.g'))\n",
    "q = C.getJointState()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "[y,J] = C.eval(ry.FS.position, ['gripper'])\n",
    "print('feature value:', y, '\\nJacobian:', J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# the x-axis of the given frame in world coordinates\n",
    "C.eval(ry.FS.vectorX, ['gripper'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The signature of the `eval` method is\n",
    "\n",
    "* The feature symbol (`FS.<name>` in python; `FS_<name>` in cpp)\n",
    "* The set of frames it refers to, given as list of frame names\n",
    "* Optionally: A scale, that can also be a matrix to down-project a feature (see below)\n",
    "* Optionally: A target, which changes the zero-point of the features (see below)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## List of features\n",
    "Here is a full list of feature symbols:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ry.FS.__members__.keys()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But some of these (esp later ones) are exotic or experimental. The core pre-defined features are the following:\n",
    "\n",
    "| FS | frames | D | description |\n",
    "|:---|:---:|:---:|:---|\n",
    "| position | [A] | 3 | 3D position of A in world coordinates |\n",
    "| positionDiff | [A,B] | 3 | difference of 3D positions of A and B in world coordinates |\n",
    "| positionRel | [A,B] | 3 | 3D position of A in B-coordinates |\n",
    "| quaternion | [A] | 4 | 4D quaternion of A in world coordinates\\footnote[There is ways to handle the invariance w.r.t.\\ quaternion sign properly.] |\n",
    "| quaternionDiff | [A,B] | 4 | ... |\n",
    "| quaternionRel | [A,B] | 4 | ... |\n",
    "| pose | [A] | 7 | 7D pose of A in world coordinates |\n",
    "| poseDiff | [A,B] | 7 | ... |\n",
    "| poseRel | [A,B] | 7 | ... |\n",
    "| vectorX | [A] | 3 | The x-basis-vector of frame A in world coordinates |\n",
    "| vectorXDiff | [A,B] | 3 | The difference of the above for two frames A and B |\n",
    "| vectorXRel | [A,B] | 3 | The x-basis-vector of frame A in B-coordinates |\n",
    "| vectorY... | | | same as above |\n",
    "| scalarProductXX | [A,B] | 1 | The scalar product of the x-basis-vector of frame A with the x-basis-vector of frame B |\n",
    "| scalarProduct... | [A,B] | | as above |\n",
    "| angularVel | [A] | 3 | The angular velocity of frame A across two configurations (must be order=1!) |\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",
    "| negDistance | [A,B] | 1 | The NEGATIVE distance between convex meshes A and B, positive for penetration |\n",
    "| qItself | [] | n | The configuration joint vector |\n",
    "| aboveBox | [A,B] | 4 | when all negative, A is above (inside support of) the box B |\n",
    "| insideBox | [A,B] | 6 | when all negative, A is inside the box B |"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Position features\n",
    "Let's briefly clarify the difference between `position`, `positionDiff`, and `positionRel`:\n",
    "\n",
    "* `position` is the position of a frame `A` in world coordinates\n",
    "* `positionDiff` is the difference (in world coordinates) of position `B` MINUS position `A`\n",
    "* `positionRel` is the position of frame `A` in the RELATIVE coordinates of frame `B`\n",
    "\n",
    "The best example is if `A` is a camera: Assume you would like to frame `B` to be positioned exactly at coordinate (0,0,-.3) in the coordinate frame of `A` -- meaning exactly 30cm centrally in front of the camera -- then the following feature would evaluate the error:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.positionRel, ['gripper', 'panda_joint1'], scale=[1], target=[0, 0, .3])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Concretely, if you minimize the above feature, the `gripper` will look exactly(=centrally) at `panda_joint1` with 30cm distance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Scalar product features\n",
    "The `scalarProduct` features are also very useful to define relative between frames (e.g. for grasping). For instance, `(FS.scalarProductXX, ['handL', 'handR'], target=[1])` 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. And\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": [
    "## Scale and target transformation\n",
    "Let's explain the `scale` and `target` in detail: Formally, specifying a target and scale redefines 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 and defines the zero-point of the feature.\n",
    "\n",
    "The scale can be\n",
    "\n",
    "* a scalar (just a factor),\n",
    "* a $D$-vector (multiplying element-wise to the feature)\n",
    "* or a **matrix**.\n",
    "\n",
    "We can do interesting things when `scale` is a matrix. For instance, if we only want the $xy$-position of a frame returned, we can choose a matrix $S=\\begin{pmatrix}1 & 0 & 0 \\\\ 0 & 1 & 0\\end{pmatrix}$, which multiplies to the 3D feature to return a 2D feature. In code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.position, ['gripper'], [[1,0,0],[0,1,0]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A very useful application is again if we care about a camera looking at a point: If we want frame `B` to appear centrally in the $xy$-plane of frame `A`, we can mimimize the feature:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.positionRel, ['gripper', 'panda_joint1'], [[1,0,0],[0,1,0]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Collision features\n",
    "\n",
    "Let's evaluate the accumulative collision scalar and its Jacobian"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.computeCollisions() #collisions/proxies are not automatically computed on set...State\n",
    "C.eval(ry.FS.accumulatedCollisions, [])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's move into collision and redo this"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry\n",
    "C = ry.Config()\n",
    "C.clear()\n",
    "C.addFile(ry.raiPath('panda/panda.g'))\n",
    "C.addFile(ry.raiPath('panda/panda.g'), 'r_')\n",
    "base_r = C.getFrame('r_panda_base')\n",
    "base_r.setPosition([.0, .5, .0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.selectJoints(['panda_joint1', 'panda_joint2', 'r_panda_joint1', 'r_panda_joint2'])\n",
    "C.setJointState([1.,-.8,-1.,-.8])\n",
    "C.view()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The configuration is now in collision. We can evaluate between some specific shape-pairs that show this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.negDistance, ['palm', 'r_palm'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.negDistance, ['panda_coll7', 'r_panda_coll7'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.negDistance, ['panda_coll6', 'r_panda_coll6'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "However, the above features only return penetration (=negDistance) between specific pairs of shapes. For holistic collision checking we query if any pair of shapes collides. This is called **broad phase collision checking**. The following does this (calling `fcl` internally):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.computeCollisions()\n",
    "C.getCollisions()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.getCollisionsTotalPenetration()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The last command is useful for what is classically called binary collision check: if `totalPenetration` is zero, we have no collisions.\n",
    "\n",
    "Finally, we can get the same information in a differentiable manner, as a feature:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.eval(ry.FS.accumulatedCollisions, [])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Enabling/disabling collisions\n",
    "\n",
    "In a typical robotic configuration we might have both, visual and (more coarse) collision shapes. Broadphase collision checking should only include the collision shapes. Further, after broadphase collision checking one typically wants to filter out some collisions: shapes of consecutive robot links should usually not be included in collision checking.\n",
    "\n",
    "In the rai code, shapes have an *integer* `contact` parameter to control this, with the following semantics:\n",
    "\n",
    "* contact = 0 (default) --> the shape never collides (e.g., is a visual)\n",
    "* contact = 1 --> the shape collides with all other shapes\n",
    "* contact < -k (some negative number) --> the shape collides with all shapes except for those that are k-th order parents\n",
    "\n",
    "So, the latter setting is very natural in robot chains: contact=-1 means \"not colliding with the parent link\". To be precise, the \"k-th order parenthood\" means frames between which there are k joints or less.\n",
    "\n",
    "Note `contact` needs to be set *before* the first broadphase collision checking, as it is used in the construction of the underlying collision engine."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "del C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.12.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}