4_dynamics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dynamics: simulation and control\n",
    "This notebook focuses on the robot dynamics, for both simulating the behavior of the robot when forces are applied to it, and computing dynamic-aware control laws like computed torque.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.\n"
     ]
    }
   ],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up \n",
    "We will use Pinocchio, the viewer and the QP solver quadprog. We use an ad-hoc model of a robot hand with simple geometry to make the contact simulation problem simple. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import time\n",
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "from numpy.random import rand\n",
    "from numpy.linalg import inv,pinv,norm,eig,svd\n",
    "import matplotlib.pylab as plt; plt.ion()\n",
    "import quadprog\n",
    "from tp4.robot_hand import RobotHand\n",
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## QuadProg solver\n",
    "QuadProg is a simple QP solver that is very convenient to import in Python and very simple to use. \n",
    "\n",
    "It is package in robotpkg apt repository (alternatively in pip), install it with \n",
    "\"sudo apt install robotpkg-py35-quadprog\" or alternatively \"pip install --user quadprog\".\n",
    "\n",
    "A simple example using QuadProg is as follow. First randomly define a QP problem min 1/2 x'Ax - b'x s.t. Cx>=0\n",
    "\n",
    "$$\\min_x \\frac{1}{2} x^T A x - b^T x$$\n",
    "Under the constraints:\n",
    "$$C x \\ge d$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_qp_matrices\n",
    "A = np.random.rand(5,5)*2-1\n",
    "A = A @ A.T ### Make it positive symmetric\n",
    "b = np.random.rand(5)\n",
    "\n",
    "C = np.random.rand(10,5)\n",
    "d = np.random.rand(10)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The quadratic program can be solved by calling:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/example_qp_solve\n",
    "[x,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d)  # Notice that C.T is passed instead of C"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where x is the solution, cost is the value of the cost function at x, niter is the number of iterations needed to obtain the optimum, lag is the dual optimum (lagrange multipliers) and iact are the active constraints (numbered from 1 to N, i.e. lag[iact-1] is not null). See the doc for more details."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# help(quadprog.solve_qp)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A robot hand for our test\n",
    "\n",
    "We are going to use a 4-finger hand, whose model is defined in Python (no urdf model) using capsule volumes.\n",
    "The hand is defined in the python code robot_hand.py. You can load and \n",
    "display it like this (don't forget to turn gepetto-viewer on with starting gepetto-gui)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7001/static/\n"
     ]
    }
   ],
   "source": [
    "# %load tp4/generated/solution_pd_robothand\n",
    "robot = RobotHand()\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7001/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Interactive sliders (for geeks)\n",
    "\n",
    "For this one, you need to additionally install [ipywidgets](https://github.com/jupyter-widgets/ipywidgets/blob/8.0.0b1/docs/source/examples/Widget%20Basics.ipynb). Execute the following then restart your jupyter process:\n",
    "\n",
    "```\n",
    "pip install ipywidgets\n",
    "\n",
    "jupyter nbextension enable --py widgetsnbextension\n",
    "```\n",
    "\n",
    "\n",
    "Never mind, we don't really need it. You can also play with the hand by changing its configuration manually. \n",
    "\n",
    "Take care that the hand is small: zoom in to see it in the window."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5cb644284a6b4a85965432abd9bb9127",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.7, description='thumb', max=1.5, min=-0.5, step=0.01), Output()), _d…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "cbbc0d9d2937419fb6f3fb9c09ffab9e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.3, description='others', max=2.0), Output()), _dom_classes=('widget-…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "try:\n",
    "    q0 = robot.q0.copy()\n",
    "    from ipywidgets import interact\n",
    "    @interact(thumb=(-.5, 1.5, 0.01))\n",
    "    def move_thumb(thumb=0.7):\n",
    "        q0[11:] = thumb\n",
    "        viz.display(q0)\n",
    "    @interact(others=(0., 2., 0.1))\n",
    "    def move_fingers(others=0.3):\n",
    "        q0[1:11] = others\n",
    "        viz.display(q0)\n",
    "except:\n",
    "    print('iPyWidget should be installed, see above. ')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember that the model rendered in the viewer is just a display, not a simulation (yet).  You can pretty much achieve any movements you want in this display, because we are not enforcing (yet) any physical law. For example, if you want to move the fingers following any trajectories you like, you can do:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "\n",
    "for i in range(500): # Put 1000 or 5000 if you want a longer move.\n",
    "    for iq in range(3,robot.model.nq):\n",
    "        q[iq] = -1+np.cos(i*1e-2*(1+iq/5))\n",
    "    viz.display(q)\n",
    "    time.sleep(2e-3)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Forward unconstrained dynamics\n",
    "### Computing the acceleration"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In a first time, let's play with the dynamics without constraints (hence without quadprog).\n",
    "\n",
    "Choosing an arbitrary joint torque $\\tau_q$, pinocchio can compute the robot acceleration and integrate it.\n",
    "\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": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/solution_pd_init\n",
    "q = robot.q0.copy()\n",
    "vq = np.zeros(robot.model.nv)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/solution_pd_mass\n",
    "M = pin.crba(robot.model, robot.data, q)\n",
    "b = pin.nle(robot.model, robot.data, q, vq)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These terms correspond to the inverse dynamics. They can be numerically inverted to compute the direct dynamics."
   ]
  },
  {
   "cell_type": "markdown",
   "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": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_dyninv"
   ]
  },
  {
   "cell_type": "markdown",
   "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": 13,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'aq' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Input \u001b[0;32mIn [13]\u001b[0m, in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28mprint\u001b[39m(norm(pin\u001b[38;5;241m.\u001b[39mrnea(robot\u001b[38;5;241m.\u001b[39mmodel,robot\u001b[38;5;241m.\u001b[39mdata,q,vq,\u001b[43maq\u001b[49m)\u001b[38;5;241m-\u001b[39mtauq))\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28mprint\u001b[39m(norm(pin\u001b[38;5;241m.\u001b[39mrnea(robot\u001b[38;5;241m.\u001b[39mmodel,robot\u001b[38;5;241m.\u001b[39mdata,q,vq,vq\u001b[38;5;241m*\u001b[39m\u001b[38;5;241m0\u001b[39m)\u001b[38;5;241m-\u001b[39mpin\u001b[38;5;241m.\u001b[39mnle(robot\u001b[38;5;241m.\u001b[39mmodel,robot\u001b[38;5;241m.\u001b[39mdata,q,vq)))\n",
      "\u001b[0;31mNameError\u001b[0m: name 'aq' is not defined"
     ]
    }
   ],
   "source": [
    "print(norm(pin.rnea(robot.model,robot.data,q,vq,aq)-tauq))\n",
    "print(norm(pin.rnea(robot.model,robot.data,q,vq,vq*0)-pin.nle(robot.model,robot.data,q,vq)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Integrating the acceleration\n",
    "\n",
    "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": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "dt = 5e-3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'aq' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Input \u001b[0;32mIn [15]\u001b[0m, in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;66;03m# %load tp4/generated/solution_pd_integrate\u001b[39;00m\n\u001b[0;32m----> 2\u001b[0m vq \u001b[38;5;241m+\u001b[39m\u001b[38;5;241m=\u001b[39m \u001b[43maq\u001b[49m \u001b[38;5;241m*\u001b[39m dt\n\u001b[1;32m      3\u001b[0m q \u001b[38;5;241m=\u001b[39m pin\u001b[38;5;241m.\u001b[39mintegrate(robot\u001b[38;5;241m.\u001b[39mmodel, q, vq \u001b[38;5;241m*\u001b[39m dt)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'aq' is not defined"
     ]
    }
   ],
   "source": [
    "# %load tp4/generated/solution_pd_integrate\n",
    "vq += aq * dt\n",
    "q = pin.integrate(robot.model, q, vq * dt)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "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": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "tauq = np.zeros(robot.model.nv)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Fill the template below to get a free-falling dynamics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "for it in range(1000):\n",
    "    t = it*dt\n",
    "    ### Compute acceleration instead of this comment\n",
    "    ### Integrate the acceleration twice instead of this comment\n",
    "    \n",
    "    if i%20==0: \n",
    "        robot.display(q)\n",
    "        time.sleep(20*dt)"
   ]
  },
  {
   "cell_type": "markdown",
   "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",
   "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,
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.traj_ref import TrajRef"
   ]
  },
  {
   "cell_type": "markdown",
   "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,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load  tp4/generated/traj_ref_main\n",
    "qdes = TrajRef(np.array([0,0,0.]),omega = np.array([1,2,3.]),amplitude=1.5)\n",
    "t = 0.2\n",
    "print(qdes(t),qdes.velocity(t),qdes.acceleration(t))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Proportional-derivative and computed torque"
   ]
  },
  {
   "cell_type": "markdown",
   "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",
   "metadata": {},
   "source": [
    "First choose the PD gains:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/solution_pd_hyper\n",
    "# Hyperparameters for the control and the simu\n",
    "Kp = 50.               # proportional gain (P of PD)\n",
    "Kv = 2 * np.sqrt(Kp)   # derivative gain (D of PD)\n",
    "dt = 1e-3              # simulation timestep\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then choose the reference trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/solution_pd_trajref\n",
    "from tp4.traj_ref import TrajRef\n",
    "qdes = TrajRef(robot.q0,omega = np.array([0,.1,1,1.5,2.5,-1,-1.5,-2.5,.1,.2,.3,.4,.5,.6]),amplitude=1.5)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, implement the control loop using the following template."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/solution_pd_loop\n",
    "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(robot.model.nv)   # REPLACE ME\n",
    "    \n",
    "    # Compute the model M and b.\n",
    "    M = np.eye(robot.model.nv)        # REPLACE ME\n",
    "    b = np.zeros(robot.model.nv)      # REPLACE ME\n",
    "    \n",
    "    # Simulated the resulting acceleration (forward dynamics\n",
    "    aq = np.zeros(robot.model.nv)     # REPLACE ME\n",
    "\n",
    "    # Integrate the acceleration.\n",
    "    vq += np.zeros(robot.model.nv)    # REPLACE ME\n",
    "    q  = q+np.zeros(robot.model.nv)   # REPLACE ME\n",
    "\n",
    "    # Display every TDISP iterations.\n",
    "    TDISP = 50e-3    # Display every 50ms\n",
    "    if not i % int(TDISP/dt):  # Only display once in a while ...\n",
    "        viz.display(q)\n",
    "        time.sleep(TDISP)\n",
    "\n",
    "    # Log the history.\n",
    "    hq.append(q.copy())\n",
    "    hqdes.append(qdes.copy())\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_loop"
   ]
  },
  {
   "cell_type": "markdown",
   "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",
   "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,
   "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",
   "metadata": {},
   "source": [
    "## Collision checking"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pinocchio is built on top of HPP FCL (a fork of the Free-Collision-Lib) to compute collision and distances. For this notebook, a wrapper as been set up to make the computation easier.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.collision_wrapper import CollisionWrapper\n",
    "colwrap = CollisionWrapper(robot)\n",
    "colwrap.computeCollisions(q)\n",
    "collisions = colwrap.getCollisionList()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "getCollisionList returns a list of all the active collisions. The list containts triplets i,c,r, with i the index of the collision pair, c the collition pair and r a structure containg the collision details, in particular the collision position in world frame and the collision normal. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From this list, it is possible to compute various quantities, in particular the contact Jacobian (corresponding to the normal direct of each contact), the contact distances (positive or negative when using capsules, negative distances might not be properly defined when using more complex geometries), and the self-acceleration of the contact point (similar to Coriolis acceleration, i.e. due to the velocity of the joints, also denoted by Jdot*qdot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dist=colwrap.getCollisionDistances(collisions)\n",
    "J = colwrap.getCollisionJacobian(collisions)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Better try this with a configuration of the hand where there are some collisions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Contact simulator"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Enforce positive acceleration \n",
    "\n",
    "The joint acceleration is constrained by the contact constraint. It can be written as a minimization problem using Gauss principle\n",
    "$$min \\quad \\frac{1}{2}  (\\ddot{q} - \\ddot{q}_0 )^T M (\\ddot{q} - \\ddot{q}_0 )$$\n",
    "$$s.t. \\quad J \\ddot{q} >= 0 $$\n",
    "where $ \\ddot{q}_0 = M^{-1} (\\tau-b)$ is the free acceleration, i.e. the acceleration obtained in Question 2 where no constraint is active.\n",
    "\n",
    "\n",
    "### 2. Impact\n",
    "\n",
    "When creating the contact with a non-zero velocity, enforcing a null acceleration is not sufficient as the two bodies will penetrate. When a new contact is created, the velocity of the contact normals should be nullified. You can do that by projecting the current velocity in the null space of the normal jacobian. Ideally, this projection should be done while respecting the Gauss principle, but that is a little bit too advanced for the purpose of this notebook.\n",
    "\n",
    "### 3. Penetration correction\n",
    "\n",
    "Enforcing 0 velocity at impact and 0 acceleration later should be enough in theory. Yet some numerical drift might push the bodies inside each other. We can then enforce the contact acceleration to be at least a PD pushing the two volums out of each other, by enforcing J qddot >= PD(dist,J*vq) where dist is the distance of penetration that we want to nullify, and J*vq is the velocity along the normal.\n",
    "\n",
    "### 4. Self-acceleration\n",
    "\n",
    "The contact accelereation is indeed Jqddot + Jdot qdot, where Jdot*qdot is a Coriolis term. This can be computed if calling colwrap.computeCollision(q,vq) (add the vq argument). Now, colwrap.getCollisionJdotQdot will return the self-acceleration. The constraint should finally be\n",
    "$$ J \\ddot{q} + \\dot{J}\\dot{q} \\ge -K_p dist -K_v J \\dot{q}$$\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are now all set up for implementing the contact simulator.\n",
    "he complete loop should be as follows: tauq is computed from a PD tracking a time-varying joint position (question 3). After computing tauq, all collision pairs must be checked to find those with distances below 1e-3. Corresponding Jacobians must be computed and stacked. If a new collision as appeared, the joint velocity must be projected to nullify it. If not collision is active, the joint acceleration is computed from inverting the mass matrix (question 2). Otherwise, it is computed using QuadProg (question 4). The resulting acceleration is integrated twice (question 1) before displaying the robot starting a new simulation iteration.\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}