4-simu.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4b08843e-22d6-470b-8a00-5816543f5745",
   "metadata": {},
   "source": [
    "# Dynamics: simulation and control\n",
    "This notebook focuses on the simulation of polyarticulated systems, with final exercices where simple control laws have to be designed. The first part deals with collision detection using the hppfcl module of pinocchio. We then build a complete simulation engine for rigid unilateral contacts. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "73d0f647-c9b2-46fd-8d3c-30ab56730076",
   "metadata": {},
   "outputs": [],
   "source": [
    "import gepetuto.magic"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4b518ada-8e3a-4ca3-a723-573b3a08dab7",
   "metadata": {},
   "source": [
    "## Set up"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "17cd2485-7c55-4b12-bde3-54c9618de471",
   "metadata": {},
   "source": [
    "We will use several models in this tutorial:\n",
    "- a simple scene with 3 convex objects (buildSceneThreeBodies)\n",
    "- a variation of this first scene with more objects and walls (buildScenePillsBox)\n",
    "- a stack of cubes of various size (buildSceneCubes)\n",
    "- a robot hand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0ad858f-7312-43c8-9a3e-082b7b5ce34b",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.scenes import buildSceneThreeBodies, buildScenePillsBox, buildSceneCubes, buildSceneRobotHand"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3530cb9f-3683-4e3f-8797-2974abf7a7ce",
   "metadata": {},
   "source": [
    "We rely on the HPP-FCL module of Pinocchio, which compute collision between geometries."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6aa4d346-f6bd-4abf-b726-9244f34a0b8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "import hppfcl\n",
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "import time\n",
    "from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer\n",
    "from tp4 import compatibility\n",
    "import matplotlib.pylab as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "41615ac1-066c-4053-9dc6-3b32bec220a6",
   "metadata": {},
   "source": [
    "## A basic example of collisions"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "13f95e22-d485-455a-bcb1-b1839c75cdba",
   "metadata": {},
   "source": [
    "The goad of this section is to introduce a simple example of collision distances between bodies, and the underlying notions of witness points and segment and normal direction.\n",
    "\n",
    "Let's build a simple scene with 3 objects and display their proximity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea1e9b79-8d99-40a1-b32a-9a5ede9c9ed5",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_display_witness_build\n",
    "# Build a scene\n",
    "model,geom_model = buildSceneThreeBodies()\n",
    "data = model.createData()\n",
    "geom_data = geom_model.createData()\n",
    "\n",
    "# Start meshcat\n",
    "viz = MeshcatVisualizer(model=model, collision_model=geom_model,\n",
    "                        visual_model=geom_model)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c46cca4-e0e1-4237-8491-ef294aa49b8a",
   "metadata": {},
   "outputs": [],
   "source": [
    "q=pin.randomConfiguration(model)\n",
    "viz.display(q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb423930-ee1a-4e6a-9c7f-558778e7a8ed",
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0786e388-2692-42be-aa29-87d0fc8f1f16",
   "metadata": {},
   "source": [
    "We can compute the distances between the 3 objects. Let's do it and show the closest points."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "07d43c00-17fd-4a10-88ee-b169031b8181",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.display_witness import DisplayCollisionWitnessesInMeshcat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cce02cbf-b035-4b79-aaae-5b895dd87aae",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_display_witness_witness\n",
    "# Build the viewer add-on to display the witnesses.\n",
    "mcWitnesses = DisplayCollisionWitnessesInMeshcat(viz)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ec306059-8588-44e5-bc6f-5a87e194df4b",
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.computeDistances(model,data,geom_model,geom_data,q)\n",
    "mcWitnesses.displayDistances(geom_data)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fb8350bd-2ad6-4096-99c2-5e9cad867f7f",
   "metadata": {},
   "source": [
    "Each collision pair corresponds to a pair of closets points, respectively located at the surface of the collision geometries if they are not colliding. These points are sometime called the *witness* points.\n",
    "\n",
    "The witness points are connected by the *witness* segment. This segment is normal to the two collision surfaces, in the case each surface is smooth around the witness point. The normalized direction is called the collision *normal*. Its orientation is a convention (the most logical convention is to go from body 1 to body 2 of the collision pair)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8c10fa58-6669-478d-80da-e845c3e9478b",
   "metadata": {},
   "source": [
    "Let's move the objects to better see these witness elements."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e3286b67-e2ad-4bc4-a984-0eaf25b19cac",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_display_witness_trajectory\n",
    "v = (np.random.rand(model.nv)*2-1)*1e-3\n",
    "r0 = [ np.linalg.norm(q[7*i:7*i+3]) for i in range(model.nq//7) ]\n",
    "for t in range(100):\n",
    "\n",
    "    # Update the robot position along an arbitrary trajectory\n",
    "    q = pin.integrate(model,q,v*10)\n",
    "    for i in range(model.nq//7):\n",
    "        q[7*i:7*i+3] *= r0[i]/np.linalg.norm(q[7*i:7*i+3])\n",
    "    viz.display(q)\n",
    "\n",
    "    # Display the witness points\n",
    "    pin.computeDistances(model,data,geom_model,geom_data,q)\n",
    "    mcWitnesses.displayDistances(geom_data)\n",
    "\n",
    "    time.sleep(.01)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ee975065-d25a-4eb5-9bc1-2613d3ca82ff",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "## Details about Pinocchio HPP-FCL "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "192d4e0c-754f-44ce-b0d4-d4d0f7bf329a",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "Let's see now how to extract the contact and distance information from Pinocchio and the module HPP-FCL, and how to store this in a proper data structure."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d40fe852-9e16-4e06-a213-fb844b170098",
   "metadata": {},
   "source": [
    "### Geometry model\n",
    "The geometry model contains a set of object, each described by a name, a geometry primitive and a placement with respect to a parent joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "39058748-2c4c-4ac4-91fc-8874235af3d7",
   "metadata": {},
   "outputs": [],
   "source": [
    "geom1 = geom_model.geometryObjects[0]\n",
    "geom1.name, geom1.parentJoint, geom1.geometry, geom1.placement"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e77c0968-6e81-4993-9e95-08107cf2b70c",
   "metadata": {},
   "source": [
    "In addition, we also store the pairs of geometry objects that should be considered when evaluating collisions and distances."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5b800e76-7029-464a-a54f-01ce712410bd",
   "metadata": {},
   "outputs": [],
   "source": [
    "list(geom_model.collisionPairs)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6f528f42-4838-4f2a-868a-d28b39c30d27",
   "metadata": {},
   "source": [
    "### HPP-FCL computeDistances and computeCollisions"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d734bc72-7237-4214-a66a-15c79788071e",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "The geometry algorithms are implemented in HPP-FCL under two different sets of functions, which respectively compute the distance between bodies, and the collision between bodies.\n",
    "When computing the distance, a unique pair of witness points is produced, and the signed distance is also evaluated.\n",
    "When computing the collision, an effort is made to compute all the contact points, and early stop can be activated to reduce the algorithm cost as soon as a collision is found. \n",
    "All in all, for this initiation, both can be considered quite similarly.\n",
    "\n",
    "Both functions are parametrized by a *request* object, and write their output in a *result* object. *Request* and *result* objects are preallocated in the geometry data, one of each for each pair of collisions. If you activate or deactivate a collision pair, you have to regenerate these objects (and so if you add a new geometry object in the list). "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f3e5096c-357f-43f1-a902-bca73cb960f9",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(geom_model.collisionPairs), len(geom_data.collisionRequests), len(geom_data.distanceResults)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5357039c-51ea-400d-b289-06ceaa3235ff",
   "metadata": {},
   "source": [
    "The placement of the geometry objects with respect to world frame are stored in geom_data.oMg, and computed as\n",
    "$$^oM_g(q) = ^oM_i(q) ^iM_g$$\n",
    "with $^oM_g$ the placement of the object wrt world, $^oM_i$ the placement of the parent joint and $^iM_g$ the (fixed) placement of the object wrt the parent joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "978eeb5f-c5a6-4e08-b2a4-fb9eb5a85ceb",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(geom_model.geometryObjects),len(geom_data.oMg)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "71102a56-755d-4612-ab62-6f17ca10a786",
   "metadata": {},
   "source": [
    "This computation is triggered by *pin.updateGeometryPlacements*, after forward kinematics has been run or by forcing the refresh of the forward kinematics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45b06ca0-1010-49d3-b4d5-2256a58a98e4",
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0fc670d9-cf14-4c1e-b531-4940201083ae",
   "metadata": {},
   "source": [
    "The computation of the distances and collisions is triggered by their respective function and by default forces the kinematic update."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6a81a5c8-3125-449b-8d6f-f92466028c23",
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.computeCollisions(model,data,geom_model,geom_data,q)\n",
    "pin.computeDistances(model,data,geom_model,geom_data,q)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "44f6dc9a-01b4-4352-a032-b198952e3510",
   "metadata": {},
   "source": [
    "These two functions actually iterate over each pair of collisions and run *pin.computeDistance* or *pin.computeCollision*, which in turn runs *hppfcl.distance* and *hppfcl.collide*. \n",
    "HPP-FCL directly works with geometry placements, ignoring the kinematics and the configuration space which is provided by Pinocchio.\n",
    "All these functions have very similar signatures, and Pinocchio is mostly doing a basic wrapping around the HPP-FCL library and gently connects the forward kinematics with the collision algorithms. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a86846c1-dbc5-49f3-a340-9fdbe8331ce6",
   "metadata": {},
   "source": [
    "### The *result* objects\n",
    "The distance result contains the pair of witness points $p_1$ and $p_2$, the normal direction pointing from $p_1$ to $p_2$ and the signed distance."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "561bb912-6d6f-498d-9bed-41688a13f800",
   "metadata": {},
   "outputs": [],
   "source": [
    "d = geom_data.distanceResults[0]\n",
    "print(\"p1:\",d.getNearestPoint1())\n",
    "print(\"p2:\",d.getNearestPoint2())\n",
    "print(\"n:\",d.normal)\n",
    "print(\"dist:\",d.min_distance)\n",
    "print(\"check:\",np.cross(d.normal,d.getNearestPoint2()-d.getNearestPoint1())) # The two vectors are parallel."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "25a31655-e464-4a1d-8380-1dcad5659d29",
   "metadata": {},
   "source": [
    "The collision object contains similar information, but can stores several witness pairs instead of a single one, or none if there is no collisions.\n",
    "The collision is decided based on a security margin, tuned in the collision request object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "366c4117-da54-4730-97b6-da470171dd56",
   "metadata": {},
   "outputs": [],
   "source": [
    "print('Margin:',geom_data.collisionRequests[0].security_margin)\n",
    "c = geom_data.collisionResults[0]\n",
    "print('Number of collisions:',len(c.getContacts()))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "618e1de8-bb66-4623-aa9d-70e6cf558d48",
   "metadata": {},
   "source": [
    "Now choose a configuration in collision and look at the content of this contact list."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2229503f-e182-4318-bd27-c80de57a60d9",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load_snippet tp4/generated/example_find_collisions_find"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "834c88c9-6242-4a8c-8281-af03e75e0528",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_find_collisions_print\n",
    "for pairId,c in enumerate(geom_data.collisionResults):\n",
    "    if len(c.getContacts())>0:\n",
    "        contact = c.getContact(0)\n",
    "        print([ n for n in dir(contact) if '__' not in n])\n",
    "        break\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5e66fa93-c2ed-4d49-8f9d-1eba0cc31756",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "## From collision detection to contact model"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0e0133ae-0468-4e77-b481-6686574eaa51",
   "metadata": {},
   "source": [
    "### The maths\n",
    "Once the collision are detected, we have to decide the model of the contact interaction that will happen at this location. It can range from a fixed 3D contact (bilateral), a sliding contact enforcing only constraints along the normal direction, a frictional contact or even a full 6D contact with no degree of freedom between the two corresponding bodies (and many other models you can imagine).\n",
    "To describe the contact model, we best formulate it in a reference frame at the contact location. Here we arbitrarily decide to align the *z* direction of the contact frame with the normal direction (oriented from body 1 to body 2) and set the two other *x* and *y* directions to any arbitrary orthogonal basis of the tangential plane. If the collision distance is exactly 0 (which will never be), the center of the frame should be at the contact point. In practice, we can define either two contact frames at each witness point or a single one at the middle point between both. Since the contact distance will never be large, it should be approximately the same and both solutions are acceptable in practice."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4d20a527-ac57-4d2d-9a1a-16a9f95ccaba",
   "metadata": {},
   "outputs": [],
   "source": [
    "OC1 = contact.getNearestPoint1()\n",
    "OC2 = contact.getNearestPoint2()\n",
    "normal = contact.normal"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "134b4184-bae0-415c-b7c7-6193f0a7d279",
   "metadata": {},
   "source": [
    "From these vectors, build the two contact frames $^oM_{c1}$ and $^oM_{c2}$ centered in $c_1$  and $c_2$ and with *z* axis aligned with the *normal* direction. You can for example use the pin.Quaternion.FromTwoVectors(v1,v2) function, which returns a rotation $r(.)$ such that $r(v_1)=v_2$ ie that transforms $v_1$ into $v_2$ (here we want to transform the *z* axis $z=[0,0,1]$ into the *normal* vector)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21241041-5ef2-49c1-ba5c-6629c934759a",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/create_rigid_contact_models_for_hppfcl_frames"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1be01ed8-bde7-4190-a4a8-818c71bec31f",
   "metadata": {},
   "source": [
    "The pin.RigidConstaintModel class in Pinocchio conveniently stores these contact quantities: the two parent joints that carries the corresponding colliding bodies, the placements of the contact frames with respect to the joint frames $^{j1}M_{c1}$, $^{jc}M_{c2}$, and a flag specifying if the contact model is 3D (point contact) or 6D (surface contact), which will later be convenient to evaluate the corresponding Jacobian."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9e03990-39fd-4d3a-8621-d0db52b6432f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/create_rigid_contact_models_for_hppfcl_model\n",
    "contact_model = pin.RigidConstraintModel(\n",
    "        pin.ContactType.CONTACT_3D,\n",
    "        model,\n",
    "        jid1,oMj1.inverse()*oMc1,\n",
    "        jid2,oMj2.inverse()*oMc2,\n",
    "        pin.LOCAL)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e1620531-4dda-40d0-9469-db87263cce0b",
   "metadata": {},
   "outputs": [],
   "source": [
    "contact_data = contact_model.createData()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3ab5875-8d73-442f-9ff0-9e964b035584",
   "metadata": {},
   "source": [
    "### Creation wrapper\n",
    "This code has been conveniently wrapped in a dedicated function, that takes all the collision or distance results and returns a list of contact models."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af53a163-e1e6-4f18-8e7a-8ea5880694ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.create_rigid_contact_models_for_hppfcl import createContactModelsFromCollisions,createContactModelsFromDistances"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "380d3dd5-740a-4142-bf27-3ca9fd9eeccc",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/create_rigid_contact_models_for_hppfcl_example\n",
    "pin.computeCollisions(model,data,geom_model,geom_data,q,False)\n",
    "contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data)\n",
    "contact_datas = [ cm.createData() for cm in contact_models ]\n",
    "\n",
    "pin.computeDistances(model,data,geom_model,geom_data,q)\n",
    "contact_models = createContactModelsFromDistances(model,data,geom_model,geom_data,\n",
    "                                                       threshold=10) # threshold in meter\n",
    "contact_datas = [ cm.createData() for cm in contact_models ]\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b9593582-6095-4c11-a69b-228402cb6635",
   "metadata": {},
   "source": [
    "### Display the contact models\n",
    "A robot description (like the robot URDF) typically describes two sets of geometries: a first set, accurate, detailled and texture fore visualization and non real-time evaluations (called the *visual* model); a second one, less detailed, sometimes approximated or convexified, for efficient computations (often called the *collision* model).\n",
    "\n",
    "Let's display in the 3D viewer the contact models with red contact patches (disks) located at the center of the contact frames and aligned with the *x-y* contact plane. \n",
    "To make it easy to render, we will put these patches in the *visual* model of the system. \n",
    "A fix set of patches is preallocated at initialization (and hidden or displaced far from the scene center when not useful). When contacts are created, the patches are revealed and properly place for rendering.\n",
    "The functions in the file display_collision_patches have be written for that."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "12e9c358-cf30-4fd9-a683-cad9b0c630d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.display_collision_patches import preallocateVisualObjects,updateVisualObjects"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "471e4424-bdbc-4aa9-a37a-6687d1c1ca4d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_display_collision_patches_create\n",
    "# Obtained by simply copying the collision model\n",
    "visual_model = geom_model.copy()\n",
    "preallocateVisualObjects(visual_model)\n",
    "\n",
    "# Start meshcat\n",
    "viz = MeshcatVisualizer(model=model, collision_model=geom_model,\n",
    "                        visual_model=visual_model)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ef3e4c3-5ef5-47df-97dd-970e710dfe06",
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69e03a49-a388-4949-a4a6-d5bd408b1ea3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_display_collision_patches_display\n",
    "updateVisualObjects(model,data,contact_models,contact_datas,visual_model,viz)\n",
    "viz.display(q)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "07ae1cd6-50f1-4d00-a566-4939510387f7",
   "metadata": {},
   "source": [
    "You have to run updateVisualObjects each time you change q, and before calling viz.display(q)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "80b536e4-2388-40bc-a990-c54b04d3b596",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "##  Building a (very) simple kinematic simulator"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0824cecd-cb7f-42d8-baf3-6f925610e67c",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "Let's build a very simple collision simulator. Starting from an initial configuration where some bodies are colliding, you have to choose a velocity of each body to disentangle the colliding parts.\n",
    "The scene created above is composed of 3 bodies, each attached to a \"free-flyer\" joint. The configuration of the scene is the concatenation of the configuration of each joint, corresponding to translation and quaternion describing the placement of each body. The configuration velocity also is the concatenation of the velocity of each body, each corresponding to the spatial velocity $\\nu = (v,\\omega)$ of each body expressed in the local frame. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ddf6c029-3b70-43be-9d58-bf28fd152aaa",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_separate_objects_hyperparams\n",
    "# HYPER PARAMETERS OF THE PUSH STRATEGY\n",
    "PUSH_FACTOR = .1\n",
    "EPSILON = 1e-1\n",
    "NB_ITER = 100\n",
    "# Compute the contact information based on distances or collisions?\n",
    "USE_DISTANCE = True\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f376e938-3fe0-4c1f-8d86-f1a366f02479",
   "metadata": {},
   "source": [
    "At each iteration, you have to choose a configuration velocity, then integrate it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98ccc6e4-d045-4715-bf1b-6bdf98bfe20b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set minimal distance to be EPSILON\n",
    "# (can be zero, but the rendering is less clear).\n",
    "for r in geom_data.collisionRequests:\n",
    "    r.security_margin = EPSILON\n",
    "\n",
    "# Keep distance history for active pairs (indexed by contact name)\n",
    "h_dist = {}\n",
    "\n",
    "# Iteratively push the colliding pairs ...\n",
    "for i in range(NB_ITER):\n",
    "\n",
    "    # We will compute a change of configuration dq.\n",
    "    # 0 if no active pair.\n",
    "    dq = np.zeros(model.nv)\n",
    "\n",
    "    # Compute the collision at current configuration.\n",
    "    if USE_DISTANCE:\n",
    "        pin.computeDistances(model,data,geom_model,geom_data,q)\n",
    "    else:\n",
    "        pin.computeCollisions(model,data,geom_model,geom_data,q)\n",
    "\n",
    "    # From hppfcl contact information, build a pin.RigidContactModel\n",
    "    if USE_DISTANCE:\n",
    "        contact_models = createContactModelsFromDistances(model,data,geom_model,geom_data,EPSILON)\n",
    "    else:\n",
    "        contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data)\n",
    "    contact_datas = [ cm.createData() for cm in contact_models ]\n",
    "\n",
    "    # For each detected contact ...\n",
    "    for cmodel,cdata in zip(contact_models,contact_datas):\n",
    "\n",
    "        # Recover contact information\n",
    "        jid1 = cmodel.joint1_id\n",
    "        j1Mc1 = cmodel.joint1_placement\n",
    "        jid2 = cmodel.joint2_id\n",
    "        j2Mc2 = cmodel.joint2_placement\n",
    "\n",
    "        # Compute signed distance\n",
    "        oMc1 = cdata.oMc1 = data.oMi[jid1]*j1Mc1\n",
    "        oMc2 = cdata.oMc2 = data.oMi[jid2]*j2Mc2\n",
    "        dist = oMc1.actInv(oMc2.translation)[2]-EPSILON  # signed distance\n",
    "        \n",
    "        # ### TODO\n",
    "        # Here, do something to the velocity of the two joints carrying the \n",
    "        # two colliding bodies.\n",
    "        # Displacement for body 1\n",
    "        dq[model.idx_vs[jid1]:model.idx_vs[jid1]+6] += 0 ### TODO FIX ME\n",
    "        # Displacement for body 2\n",
    "        dq[model.idx_vs[jid2]:model.idx_vs[jid2]+6] -= 0 ### TODO FIX ME\n",
    "        \n",
    "        # Log the distance in h_dist for future plot\n",
    "        if cmodel.name not in h_dist:\n",
    "            h_dist[cmodel.name] = np.zeros(NB_ITER)\n",
    "        h_dist[cmodel.name][i] = dist\n",
    "\n",
    "    # Finally, modify the current config q with the push dq\n",
    "    q = pin.integrate(model,q,dq)\n",
    "\n",
    "    # Display the current configuration\n",
    "    if i % 10 == 0:\n",
    "        # Meshcat is slow to display the patches, display once in a while\n",
    "        updateVisualObjects(model,data,contact_models,contact_datas,visual_model,viz)\n",
    "        viz.display(q)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "00a8894c-9de6-42b6-af52-6e324a5b49ad",
   "metadata": {},
   "source": [
    "If you don't have any strong ideas, you can start to compute a \"push\" repulsive 3D velocity that the first witness point should adopt. The second witness point should likely adopt the opposite velocity. This is the value of the velocity vector field at the contact point. Now choose the spatial velocity at the center of the body so that the value of the corresponding vector field matches. In a first trial, a spatial velocity with 0 angular velocity $\\omega$ should be evident to guess. Can you imagine a more efficient linear+angular velocity that would work as well?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5887d564-28be-44d3-91d6-fc6a0cdb2633",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/example_separate_objects_loop"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "59b76549-262f-46d9-b75c-4a5e5d70bc22",
   "metadata": {},
   "source": [
    "### With a more complex scene\n",
    "Now let's load a square jar full of pills. Run the same algorithm on it. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3585bdd1-c846-426e-9a54-d003d6814eb1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/scenes_pills\n",
    "model,geom_model = buildScenePillsBox(seed=2,nobj=30,wall_size=2.0,one_of_each=True)\n",
    "visual_model = geom_model.copy()\n",
    "viz = MeshcatVisualizer(model=model, collision_model=geom_model,\n",
    "                        visual_model=geom_model)\n",
    "\n",
    "# Generate colliding configuration\n",
    "data = model.createData()\n",
    "geom_data = geom_model.createData()\n",
    "for i in range(10):\n",
    "    q0 = pin.randomConfiguration(model)\n",
    "    pin.computeCollisions(model,data,geom_model,geom_data,q0)\n",
    "    if sum([ len(c.getContacts()) for c in geom_data.collisionResults ])>10:\n",
    "        break\n",
    "    print(sum([ len(c.getContacts()) for c in geom_data.collisionResults ]))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cd5d3db6-c91e-4ab7-a96e-962a8d6b9337",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q0.copy()\n",
    "viz.display(q)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "13f92a3c-0466-4d93-8b83-b944a165afc2",
   "metadata": {},
   "source": [
    "You can plot the convergence of the colliding bodies from initial negative (penetration) distance to 0."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9cb3fa62-81f3-437c-99ac-91e89ff0ee7a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_separate_objects_plot\n",
    "# Plot the distances\n",
    "for k,v in h_dist.items():\n",
    "    h = plt.plot(v,label=k)\n",
    "plt.legend()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cb91c3a0-29fe-4698-bf96-4a6866b3c2af",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "## Contact-less simulator"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f42d6b2f-287e-4597-8317-a88ca332d0b6",
   "metadata": {},
   "source": [
    "We now have all the contact information, let's look at the system dynamics. \n",
    "We will write first a contact-less simulator, just integrating free fall.\n",
    "Better use an actuated model for that."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b386961a-07cd-4c7a-8a50-67cead09de8a",
   "metadata": {},
   "source": [
    "### Model of a robot hand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bef3f77c-f77c-48b3-ae33-cb2d8d3ab503",
   "metadata": {},
   "outputs": [],
   "source": [
    "model,geom_model = buildSceneRobotHand()\n",
    "q0 = model.referenceConfigurations['default']\n",
    "data = model.createData()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c4be91b2-09da-4f16-ba4a-0f2afb836164",
   "metadata": {},
   "outputs": [],
   "source": [
    "visual_model = geom_model.copy()\n",
    "viz = MeshcatVisualizer(model=model, collision_model=geom_model,\n",
    "                        visual_model=geom_model)\n",
    "viz.display(q0)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0eeeac2-e7d8-4f8a-a859-e052c48cbbdf",
   "metadata": {},
   "source": [
    "### Evaluating the core elements of the dynamics"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e699d99-7762-4d7a-991f-5c216cd2bf6f",
   "metadata": {},
   "source": [
    "In a first time, let's play with the dynamics without constraints.\n",
    "Choosing an arbitrary joint torque $\\tau_q$, inocchio can compute the robot acceleration and integrate it.\n",
    "The dynamic equation of the robot is $M a_q + b = \\tau_q$, with $M$ the mass, $a_q$ the joint acceleration and $b$ the drift.\n",
    "The mass matrix can be computed using *CRB* algorithm (function of q). The drift is computed using *NLE* (nonlinear effects) algorithm (function of $q$, $v_q$). \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ea3d313-e91c-4245-bc8a-4cf65250a7a8",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_init\n",
    "q = q0.copy()\n",
    "vq = np.zeros(model.nv)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eabd9644-416f-405e-ae67-2649f321a5e1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_mass\n",
    "M = pin.crba(model, data, q)\n",
    "b = pin.nle(model, data, q, vq)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "605402bf-8821-48fd-934b-6402f58c9ca5",
   "metadata": {},
   "source": [
    "These terms correspond to the inverse dynamics. They can be numerically inverted to compute the direct dynamics."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9ed96e4c-4fa5-4e89-a8a8-c4a1ff0d60ed",
   "metadata": {},
   "source": [
    "Using $M$ and $b$ computed by the above algorithms, and knowing a given set of joint torques $\\tau_q$, how would you compute $a_q$ so that $M a_q + b = \\tau_q$?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3df4112-e5d9-41e1-a6b0-e5962e27cf91",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/example_control_dyninv"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bba72024",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_dyninv\n",
    "tauq = np.random.rand(model.nv)*2-1\n",
    "aq = np.linalg.inv(M) @ (tauq - b)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bb7fc322-9386-44c8-aa28-bff401ac306e",
   "metadata": {},
   "source": [
    "The inverse-dynamics algorithm indeed compute the needed torques to achieve a given acceleration. We can use the *RNEA* function to double-check our result:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2fe4be72-214c-4339-b190-b44d08c58529",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(pin.rnea(model,data,q,vq,aq)-tauq)\n",
    "print(pin.rnea(model,data,q,vq,vq*0)-pin.nle(model,data,q,vq))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ad26d663-0265-465f-a066-07b4de29b3f9",
   "metadata": {},
   "source": [
    "### Integrating the acceleration"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6d09e3bf-5b3f-4270-b5a6-a1c9a94862bd",
   "metadata": {},
   "source": [
    "Once aq as been computed, it is straight forward to integrate it to velocity using $v_q += a_q \\Delta t$. \n",
    "Integration to joint position is more complex in general, as we saw for the mobile robot. Here, $n_q = n_v$ and everything is Euclinea, so a simple += integration would work, but since it is implemented in pinocchio, let's keep the good habits:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "407ba7b9-68ad-497b-9424-0d93600f02fa",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_hyper\n",
    "# Hyperparameters for the simu\n",
    "DT = 1e-3  # simulation timestep\n",
    "DT_VISU = 1/50\n",
    "DURATION = 3. # duration of simulation\n",
    "T = int(DURATION/DT) # number of time steps\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4759743f-b58b-497e-b744-2310d9961aae",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_integrate\n",
    "vq += aq * DT\n",
    "q = pin.integrate(model, q, vq * DT)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a6fbd473-521d-49d7-9aaf-17dd505fd369",
   "metadata": {},
   "source": [
    "Now, you should be able to implement a first simulation (not a rendering any more) of the robot behavior when no torque is applied (tauq = 0).\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8fb0d18-32ba-4db5-aa54-433441c2ded2",
   "metadata": {},
   "outputs": [],
   "source": [
    "tauq = np.zeros(model.nv)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "097a6bc5-bae7-47e7-99ac-938ff453c166",
   "metadata": {},
   "source": [
    "Fill the template below to get a free-falling dynamics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af3945eb-cb66-49b9-b9d7-3013b9a91eed",
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(T):\n",
    "    t = i * DT\n",
    "\n",
    "    # Compute the model.\n",
    "    # TODO FIX ME\n",
    "\n",
    "    # Simulated the resulting acceleration (forward dynamics\n",
    "    # TODO FIX ME\n",
    "    \n",
    "    # Integrate the acceleration.\n",
    "    # TODO FIX ME\n",
    "    \n",
    "    # Display once in a while...\n",
    "    if DT_VISU is not None and abs((t) % DT_VISU)<=0.9*DT:\n",
    "        viz.display(q)\n",
    "        time.sleep(DT_VISU)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2450db49-f72d-4b00-8ea4-944597427b20",
   "metadata": {},
   "source": [
    "Now modify the simulator to encompass joint friction, i.e. the torque is opposite to the velocity with a friction coefficient $K_f$ (take $K_f=0.1$ as a start)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "510da04b-0334-4158-b835-b0ff6175c2f7",
   "metadata": {},
   "source": [
    "### (optional) Reference trajectory\n",
    "If you like it, a class is implemented to compute a reference trajectory as a sinus with various frequency, phase and amplitude on each joint. It is implemented in traj_ref with the class TrajRef."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d71cb37-b381-420f-9c2e-87ea343acfcb",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.traj_ref import TrajRef"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4be658b2-4eb7-457f-b294-6c57f80c1f4f",
   "metadata": {},
   "source": [
    "In this simple example, we define a trajectory in dimension 3, starting at t=0 from q=[0,0,0], with 3 sinusoid of frequency 1,2 and 3, all with the same amplitude 1.5."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "08f591cf-cfe7-4983-96e1-1256d599fe15",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load  tp4/generated/traj_ref_main\n",
    "qdes = TrajRef(\n",
    "    np.array([0, 0, 0.0]), omega=np.array([1, 2, 3.0]), amplitude=1.5\n",
    ")\n",
    "t = 0.2\n",
    "print(qdes(t), qdes.velocity(t), qdes.acceleration(t))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4d5f98d5-17c1-4759-8b3c-b071b6a12726",
   "metadata": {},
   "source": [
    "### Proportional-derivative and computed torque"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e73df67e-9a36-44b0-a630-871923d88262",
   "metadata": {},
   "source": [
    "Now choose a reference joint position (possibly time varying, like in the hand example).\n",
    "The joint torques can then be computed to track the desired position, with $\\tau_q = -K_p (q-q^{des}) - K_v v_q$. Both gains $K_p$ and $K_v$ should be properly chosen. Optimal tracking is obtained with $K_v = 2 \\sqrt{K_p}$. \n",
    "In general, a desired velocity is also tracked to avoid tracking errors.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "600b351e-3907-4b2c-9166-379cf916e4f0",
   "metadata": {},
   "source": [
    "First choose the PD gains:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "60a40cc6-4fb9-4ad9-b90f-d24ee51a23b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_hyper_control\n",
    "# Hyperparameters for the control\n",
    "Kp = 50.0  # proportional gain (P of PD)\n",
    "Kv = 2 * np.sqrt(Kp)  # derivative gain (D of PD)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db361dde-d485-452f-bc09-8c8f56b98482",
   "metadata": {},
   "source": [
    "Then choose the reference trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "97a72e68-c72d-4d7c-a418-cd9857830fdb",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_control_trajref\n",
    "from tp4.traj_ref import TrajRef  # noqa E402\n",
    "\n",
    "qdes = TrajRef(\n",
    "    q0,\n",
    "    omega=np.array([0, 0.1, 1, 1.5, 2.5, -1, -1.5, -2.5, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6]),\n",
    "    amplitude=1.5,\n",
    ")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "44de3e5c-cee5-4961-9b5f-962f3fbc9c04",
   "metadata": {},
   "source": [
    "Finally, implement the control loop using the following template."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "151088bb-225f-48c8-b912-b7d4310f7ae0",
   "metadata": {},
   "outputs": [],
   "source": [
    "hq    = []   ### For storing the logs of measured trajectory q\n",
    "hqdes = []   ### For storing the logs of desired trajectory qdes\n",
    "for i in range(10000):\n",
    "    t = i*DT\n",
    "\n",
    "    # Compute the PD control.\n",
    "    tauq = np.zeros(model.nv)   # REPLACE ME\n",
    "    \n",
    "    # Compute the model M and b.\n",
    "    M = np.eye(model.nv)        # REPLACE ME\n",
    "    b = np.zeros(model.nv)      # REPLACE ME\n",
    "    \n",
    "    # Simulated the resulting acceleration (forward dynamics\n",
    "    aq = np.zeros(model.nv)     # REPLACE ME\n",
    "\n",
    "    # Integrate the acceleration.\n",
    "    vq += np.zeros(model.nv)    # REPLACE ME\n",
    "    q  = q+np.zeros(model.nv)   # REPLACE ME\n",
    "\n",
    "    # Display once in a while...\n",
    "    if DT_VISU is not None and abs((t) % DT_VISU)<=0.9*DT:\n",
    "        viz.display(q)\n",
    "        time.sleep(DT_VISU)\n",
    "\n",
    "    # Log the history.\n",
    "    hq.append(q.copy())\n",
    "    hqdes.append(qdes.copy())\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5d6093fa-1272-4ed4-8b2c-05923775f5c0",
   "metadata": {},
   "source": [
    "Here is the solution, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d5a0a78-2d37-4aef-bc69-c1b78be422b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/example_control_loop"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bbb42d3e-a9d0-4477-8c69-49c6f1b844a8",
   "metadata": {},
   "source": [
    "Here, there is a strong coupling between joints, due to the mass matrix that is not compensated in the simple PD law. In theory, the computed torques is to compute the joint torque by inverse dynamics from a reference joint acceleration. This boils down to canceling the simulation equation by choosing the proper terms in the control law. It is now very interesting to implement in case of perfect dynamics knowledge. It might be more interesting to study in case the simulation is done with the perfect M, while the control is computed with approximate M (for example, using only the diagonal terms of the mass matrix). Let's rather simulate contact.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c7464b1c-33e2-49a0-9039-bdbfc4a23113",
   "metadata": {},
   "source": [
    "It is intersting to plot the results. In the solution of the previous example, the effective and desired position q and qdes have been stored in a log list. We can display them with matplotlib."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "290064d4-6974-483e-ac79-bb17a6134be8",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.subplot(211)\n",
    "plt.plot([q[2] for q in hq])\n",
    "plt.plot([q[2] for q in hqdes])\n",
    "plt.ylabel('Joint #2')\n",
    "plt.subplot(212)\n",
    "plt.plot([q[3] for q in hq])\n",
    "plt.plot([q[3] for q in hqdes]);\n",
    "plt.ylabel('Joint #1')\n",
    "plt.xlabel('Iterations')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a0f90e0e-d352-4cb1-bcdd-2b43604d314b",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "## Friction-less simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a435ff98-8358-40b1-a2bc-1d8a35c96827",
   "metadata": {},
   "source": [
    "We will now combine the two aspects of the model: the collisions between geometries, and the dynamics of inertias. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8dc1c831-7ad4-4582-b008-6e854d7bc197",
   "metadata": {},
   "source": [
    "### Choose a model \n",
    "Let's come back to a simple model first: a cube on a floor."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c1b0f01c-06fe-4b3c-a186-4a932adeed33",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_simu_frictionless_init\n",
    "# ### SCENE\n",
    "# Create scene with multiple objects\n",
    "model,geom_model = buildSceneCubes(1,with_floor=True,with_corner_collisions=True,with_cube_collisions=False)\n",
    "\n",
    "# Create the corresponding data to the models\n",
    "data = model.createData()\n",
    "geom_data = geom_model.createData()\n",
    "\n",
    "for req in geom_data.collisionRequests:\n",
    "    req.security_margin = 1e-3\n",
    "    req.num_max_contacts = 20\n",
    "\n",
    "# ### VIZUALIZATION\n",
    "visual_model = geom_model.copy()\n",
    "preallocateVisualObjects(visual_model,10)\n",
    "viz = MeshcatVisualizer(model=model, collision_model=geom_model,\n",
    "                        visual_model=visual_model)\n",
    "updateVisualObjects(model,data,[],[],visual_model,viz)\n",
    "\n",
    "# ### INIT MODEL STATE\n",
    "q0 = model.referenceConfigurations['default']\n",
    "\n",
    "viz.display(q0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "046c337c-265d-4622-bfc8-be51c60b1325",
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "458beb7c-4b70-4671-a2ee-f0f792355c6f",
   "metadata": {},
   "source": [
    "### Contact Jacobian"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b7f03751-1954-4547-ae60-6e2c2ba656a7",
   "metadata": {},
   "source": [
    "The pin.RigidConstraintModel makes it possible to directly compute the corresponding jacobian. When considering 3D contact models, the jacobian maps the configuration velocity to the relative velocity of the two contact points in the local frame attached to the first witness.\n",
    "Let's build a basic contact model to show the API:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "824ab2bc-2978-4b64-a0d5-8ea0c26dc9bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "contact_model = pin.RigidConstraintModel(pin.ContactType.CONTACT_3D,model,1,pin.SE3.Identity(), 0, pin.SE3.Identity(), pin.LOCAL)\n",
    "contact_data = contact_model.createData()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "10bc3f89-9833-42db-a228-bf71b0a910bd",
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.computeJointJacobians(model,data,q0)\n",
    "pin.getConstraintsJacobian(model,data,[contact_model],[contact_data])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "09e8e3af-8270-44b7-a4ff-16a9cb99ed3a",
   "metadata": {},
   "source": [
    "The constraint Jacobian express the constraint in term of velocity of body 1 wrt to body 2. It is more intuitive to think to the opposite: so you have to take the negative Jacobian (ie velocity of body 2 wrt body 1, whose normal component should be positive). Here, we said we will only consider the normal component of the contact, so select the last row (and if there is more than one contact point, every next rows over 3)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "35d637f2-9d97-4a24-a734-98b6d79cd81a",
   "metadata": {},
   "outputs": [],
   "source": [
    "Jc = -pin.getConstraintsJacobian(model,data,[contact_model],[contact_data])[2::3]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6e7853f7-fdc4-4f91-ba39-affc6ff8046e",
   "metadata": {},
   "source": [
    "### Simulation without friction: primal formulation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f15ede1f-0fc4-4201-99af-e3f09704cbf2",
   "metadata": {},
   "source": [
    "You are now in position to write your first complete (friction-less) collision simulator.\n",
    "The state of the system is the configuration $q$ and velocity $v_q$. The joint torques have to be decided (by a controller) as a function of the state. Here we take them constant null as the cube is unactuated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0e429215-fa87-4b69-86d1-3fdd2bd54c4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q0.copy()\n",
    "v = np.zeros(model.nv)\n",
    "tau = np.zeros(model.nv)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dd8363df-dc9e-4cc2-8102-f2038884cc28",
   "metadata": {},
   "source": [
    "At each new simulation cycle, you need to choose the next velocity. \n",
    "If there is no contact, then the velocity is simply obtained after one step of free fall:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b4cc12d-9c2a-44d4-9d87-d8baacf9a268",
   "metadata": {},
   "outputs": [],
   "source": [
    "vf = v + DT * pin.aba(model, data, q, v, tau)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9d3e3fa1-1ed0-46e5-a63c-5a4dd7e81517",
   "metadata": {},
   "source": [
    "If there is some collisions, then the next velocity will be as close as possible to the free fall (\"as close as possible\" in the sense of the metrics imposed by the inertia matrix), while respecting the constraints of positive normal velocities. Mathematically, this writes:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b7c7c0bd-a184-42b2-804f-3bc72d1f4f5c",
   "metadata": {},
   "source": [
    "$$\\min_{v_+} f(v_+) = \\frac{1}{2} \\| v_+ - v_f \\|_M^2 $$\n",
    "such that\n",
    "$$J_c v_+ \\le 0$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "280840bb-9885-4894-b552-219c2ac71a3a",
   "metadata": {},
   "source": [
    "where $f(v_+) = \\frac{1}{2} \\| v_+ - v_f \\|_M^2 = \\frac{1}{2} v_+^T M v_+ - (M v_f)^T v_+$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b62e9718-2814-4660-ab3f-efd3496354b6",
   "metadata": {},
   "source": [
    "This defines a well-shaped LCPQ that can be solved by any off-the-shelf QP solver, [for example ProxQP, as explained in this companion notebook](appendix-proxqp.ipynb) ."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fac34682-82fd-4d63-a551-580798385191",
   "metadata": {},
   "source": [
    "Now write the simulation loop. At each simulation cycle:\n",
    "1. Check the collisions, create the contact model and the corresponding contact data.\n",
    "2. Evaluate the free-fall next velocity $v_f$ with ABA\n",
    "3. If no collision, then the next velocity is v_f, continue.\n",
    "4. If collisions, then evaluate the inertia matrix $M$ with pin.crba, and the Jacobian $J_c$ with pin.getConstraintJacobian; form the primal QP, solve it. The result is the next velocity, the Lagrange multipliers are the contact forces.\n",
    "5. Optionally, print, display, log."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4a066b42-c37c-4335-87df-526bfecaef0e",
   "metadata": {},
   "source": [
    "Here is a canvas:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a502dba3-3be7-457f-b7bb-881d10c8c2ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_simu_frictionless_loop\n",
    "# ### MAIN LOOP\n",
    "# ### MAIN LOOP\n",
    "# ### MAIN LOOP\n",
    "for t in range(T):\n",
    "\n",
    "    # Compute free dynamics\n",
    "    tau = np.zeros(model.nv)\n",
    "    pin.computeCollisions(model, data, geom_model, geom_data, q)\n",
    "    vf = v + DT * pin.aba(model, data, q, v, tau)\n",
    "\n",
    "    # Create contact models from collision\n",
    "    contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data)\n",
    "    contact_datas = [ cm.createData() for cm in contact_models ]\n",
    "    \n",
    "    nc = len(contact_models)\n",
    "    if nc==0:\n",
    "        # No collision, just integrate the free dynamics\n",
    "        v = vf\n",
    "    else:\n",
    "        # With at least one collision ...\n",
    "        # Compute mass matrix.\n",
    "        # (pin.crba should be enough in P3X, but CAT to be sure in P2X);\n",
    "        pin.computeAllTerms(model, data, q, v)\n",
    "        # The contact solver express the constraint in term of velocity of\n",
    "        # body 1 wrt to body 2. It is more intuitive to think to the opposite\n",
    "        # so take the negative jacobian (ie velocity of body 2 wrt body 1, whose\n",
    "        # normal component should be positive).\n",
    "        # Select only normal components of contact\n",
    "        # (frictionless slide on the tangent components, uncontrained)\n",
    "        J = -pin.getConstraintsJacobian(model, data, contact_models, contact_datas)[2::3,:]\n",
    "        assert(J.shape == (nc,model.nv))\n",
    "\n",
    "        M = data.M\n",
    "\n",
    "        # ## TODO: formulate the LCQP to compute vnext\n",
    "        v = np.zeros(model.nv)\n",
    "    \n",
    "    # Finally, integrate the valocity\n",
    "    q = pin.integrate(model , q, v*DT)\n",
    "\n",
    "    # Visualize once in a while\n",
    "    if DT_VISU is not None and abs((t*DT) % DT_VISU)<=0.9*DT:\n",
    "        updateVisualObjects(model,data,contact_models,contact_datas,visual_model,viz)\n",
    "        viz.display(q)\n",
    "        time.sleep(DT_VISU)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3fa9925e-d695-45e1-a16e-a001ae9a2622",
   "metadata": {},
   "source": [
    "Should you need it, here is the primal formulation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "92ccf302-e5a1-4c68-9e24-3f65a9f58e97",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/example_simu_frictionless_primal"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "68217f12-7401-46c8-97ab-f1d5291362f4",
   "metadata": {},
   "source": [
    "### Delassus matrix"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "76c46738-2802-41e3-9b09-194f156e3560",
   "metadata": {},
   "source": [
    "The Dual formulation requires another matrix: the Delassus operator, defined as:\n",
    "$$\\Lambda_c = J_c M^{-1} J_c^T$$\n",
    "Physically, this is the inverse of the apparent (or operational, or contact) inertia, i.e. the mass that an operator would feel if it touches the system in this configuration at the contact points.\n",
    "The pin.RigidConstraintModel makes it possible to evaluate $\\Lambda_c$ more efficiently, but for now that's enough for us."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "558615c2-aa42-4ba4-aafd-c2f57c820c20",
   "metadata": {},
   "outputs": [],
   "source": [
    "Minv = pin.computeMinverse(model,data,q)\n",
    "delasus = J@Minv@J.T"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dd29fbc4-1b60-45c6-840a-b66d11ea607f",
   "metadata": {},
   "source": [
    "### (optional) Simulation without friction: primal formulation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1bea4464-146f-4a9f-8fdc-b3a8b56c0b9f",
   "metadata": {},
   "source": [
    "The dual of the contact LCQP that we wrote is:\n",
    "$$\\min_\\lambda g(\\lambda) = \\frac{1}{2} \\lambda^T \\Lambda_c \\lambda$$\n",
    "subject to\n",
    "$$\\lambda \\ge 0$$\n",
    "$$ \\Lambda_c \\lambda + J_c v_f \\ge 0 $$\n",
    "\n",
    "Here, the decision variable is $\\lambda$ which can be understood as the contact impulses (i.e. $\\lambda = f.DT$, the effect of the forces during the timestep). The joint velocity is not appearing in this problem, as all quantities are in the operation (contact) space. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "aab31e05-9df5-41f3-a7d2-4d97825cfbd1",
   "metadata": {},
   "source": [
    "Once the contact forces are found, the configuration velocity can be evaluated by calling the pin.aba algorithm, while specifying the contact forces."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb893597-5517-4167-89d1-8f9d854c7644",
   "metadata": {},
   "outputs": [],
   "source": [
    "forces = np.zeros(Jc.shape[0])\n",
    "vnext = v + DT * pin.aba(model, data, q, v, tau + J.T @ forces/DT)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f2952ae4-a12d-4381-81ec-d730ab1a0937",
   "metadata": {},
   "source": [
    "Now rewrite the simulation problem with the dual formulation. Here is the canvas:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cd596254-63a8-4076-8293-7fd5f93470e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_simu_frictionless_loop\n",
    "# ### MAIN LOOP\n",
    "# ### MAIN LOOP\n",
    "# ### MAIN LOOP\n",
    "for t in range(T):\n",
    "\n",
    "    # Compute free dynamics\n",
    "    tau = np.zeros(model.nv)\n",
    "    pin.computeCollisions(model, data, geom_model, geom_data, q)\n",
    "    vf = v + DT * pin.aba(model, data, q, v, tau)\n",
    "\n",
    "    # Create contact models from collision\n",
    "    contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data)\n",
    "    contact_datas = [ cm.createData() for cm in contact_models ]\n",
    "    \n",
    "    nc = len(contact_models)\n",
    "    if nc==0:\n",
    "        # No collision, just integrate the free dynamics\n",
    "        v = vf\n",
    "    else:\n",
    "        # With at least one collision ...\n",
    "        # Compute mass matrix.\n",
    "        # (pin.crba should be enough in P3X, but CAT to be sure in P2X);\n",
    "        pin.computeAllTerms(model, data, q, v)\n",
    "        # The contact solver express the constraint in term of velocity of\n",
    "        # body 1 wrt to body 2. It is more intuitive to think to the opposite\n",
    "        # so take the negative jacobian (ie velocity of body 2 wrt body 1, whose\n",
    "        # normal component should be positive).\n",
    "        # Select only normal components of contact\n",
    "        # (frictionless slide on the tangent components, uncontrained)\n",
    "        J = -pin.getConstraintsJacobian(model, data, contact_models, contact_datas)[2::3,:]\n",
    "        assert(J.shape == (nc,model.nv))\n",
    "\n",
    "        Minv = pin.computeMinverse(model,data,q)\n",
    "        delasus = J@Minv@J.T\n",
    "\n",
    "        # ## TODO: formulate the dual LCQP to compute lambda\n",
    "        forces = np.zeros(delasus.shape[0])\n",
    "\n",
    "        # The next velocity results from the contact forces\n",
    "        vnext = v + DT * pin.aba(model, data, q, v, tau + J.T @ forces/DT)\n",
    "\n",
    "    # Finally, integrate the valocity\n",
    "    q = pin.integrate(model , q, v*DT)\n",
    "\n",
    "    # Visualize once in a while\n",
    "    if DT_VISU is not None and abs((t*DT) % DT_VISU)<=0.9*DT:\n",
    "        updateVisualObjects(model,data,contact_models,contact_datas,visual_model,viz)\n",
    "        viz.display(q)\n",
    "        time.sleep(DT_VISU)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c242f3ae-20b5-4527-aef4-74ff0e67de56",
   "metadata": {},
   "source": [
    "Here is the dual formulation, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bd454cd9-63c1-4d18-89f9-b91ee9be023a",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/example_simu_frictionless_dual"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "13f4ed87-15cc-43ce-9bf1-ce602cd33128",
   "metadata": {},
   "source": [
    "### Comparison: primal / dual"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c38d93d5-ecb3-44ad-863a-a1c277ee2eee",
   "metadata": {},
   "source": [
    "Once you have implemented both primal and dual formulations, you can double check that the results are the same."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21bd7654-9203-46fd-a376-0141053ce464",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load_snippet tp4/generated/example_simu_frictionless_check"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "40869f92-f96f-471d-8f64-6ef96a851d19",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "## Salade de fruit"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "77bf2e0b-f917-4fe6-9f42-2c4b66580c7c",
   "metadata": {},
   "source": [
    "Let's now put all that together and make something nice out of it. You will implement a simple grasping strategy for the robot hand to hold an cube, then implement a PD controller and validate it in the simulator to check if your grasp is stable.\n",
    "\n",
    "1. Load the hand model with an object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "44b3d8d8-fd04-4242-a520-0667cc9eeb15",
   "metadata": {},
   "outputs": [],
   "source": [
    "model,geom_model = buildSceneRobotHand(with_item=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0a48f793-3eef-48e7-ad52-19e1f9ad427f",
   "metadata": {},
   "source": [
    "2. For an arbitrary placement of the object that you will select, and a random initial configuration of the hand, progressively close the hand while monitoring the collision. Once a collision is created, either block the corresponding finger or (better) project the velocity in the null space of the constraint Jacobian to further maintain the contact.\n",
    "3. Stop when you have enough contact points.\n",
    "4. Search the minimal joint torque and corresponding contact forces such that the dynamics is enforced and the forces are sufficiently positive:\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b68225c6-042e-4013-89c6-f5f827369f08",
   "metadata": {},
   "source": [
    "$$\\min_{\\tau,f} \\| \\tau \\|^2$$\n",
    "such that\n",
    "$$g = \\begin{bmatrix} 0 \\\\ \\tau \\end{bmatrix} + J_c^T f$$\n",
    "$$f \\ge \\epsilon$$\n",
    "with $g=g(q)$ the vector of joint gravity and $\\epsilon$ a robustness threshold for the forces (take care that the object is unactuacted)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b8f397d8-3815-41d5-8a96-b2df6e2c530b",
   "metadata": {},
   "source": [
    "5. Put hand and object in your simulator, set a PD controller with the reference configuration you like, and check that your object stay static. It is acceptable that it falls (we cannot win all the time)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9d373260-7235-49c0-a75d-55a3f7c78fb5",
   "metadata": {},
   "source": [
    "**Implement this solution in a standalone python script or notebook which 1. choose a grasp configuration, 2. compute a pair of grasp torques and forces and 3. simulate the grasp with a frictionless model.**"
   ]
  }
 ],
 "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}