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": null,
   "metadata": {},
   "outputs": [],
   "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": null,
   "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",
    "A simple example using QuadProg is as follow. First randomly define a QP problem is\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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "[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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# help(quadprog.solve_qp)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "More modern QP solver are able to handle simultaneously linear equality constraint and posses various strategy to fasten convergence (proximal merit function, Ruiz equilibration...  There is a very wide variety of optimization algorithm for QP problem, the reference book on those subject is:\n",
    "*nocedal j and s j wright. 2006. numerical optimization. springer new york*\n",
    "\n",
    "A recent solver made all the choices to bring the solver as close as possible to robotic type of problem solving : [proxQP](https://hal.inria.fr/hal-03683733/file/Yet_another_QP_solver_for_robotics_and_beyond.pdf)"
   ]
  },
  {
   "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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = RobotHand()\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "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",
    "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": null,
   "metadata": {},
   "outputs": [],
   "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": null,
   "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$). Which contains normal gravity and coriolis effect."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "vq = np.zeros(robot.model.nv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M = pin.crba(robot.model, robot.data, q)\n",
    "b = pin.nle(robot.model, robot.data, q, vq)"
   ]
  },
  {
   "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 the accelration $a_q$ `aq` induce by the torques ?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tauq = np.random.rand(robot.model.nv)\n",
    "aq = np.random.rand(robot.model.nv)  # Replace me"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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* (for forward dynamics) function to double-check our result:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(norm(pin.rnea(robot.model,robot.data,q,vq,aq)-tauq))"
   ]
  },
  {
   "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 = 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 and use general integration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dt = 2e-3\n",
    "N_steps = 5000"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vq += np.random.rand(robot.model.nv)  # Replace me\n",
    "q = np.random.rand(robot.model.nq)  # Replace me"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And a solution.."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_integrate"
   ]
  },
  {
   "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": "markdown",
   "metadata": {},
   "source": [
    "Init simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "viz.display(q)\n",
    "vq = 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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for it in range(N_steps):\n",
    "    t = it*dt\n",
    "\n",
    "    # Retrieve the dynamics quantity at time t\n",
    "    M = np.eye(robot.model.nv)        # REPLACE ME\n",
    "    b = np.zeros(robot.model.nv)      # REPLACE ME\n",
    "\n",
    "    # Compute the force that apply\n",
    "    tauq = np.zeros(robot.model.nv)\n",
    "\n",
    "    # Use generalized PFD to calculate aq\n",
    "    aq = np.zeros(robot.model.nv)     # REPLACE ME\n",
    "\n",
    "    # Double integration to update vs and q\n",
    "    vq = np.zeros(robot.model.nv)    # REPLACE ME\n",
    "    q  = np.zeros(robot.model.nq)   # REPLACE ME\n",
    "\n",
    "    # Visualization\n",
    "    if it%20==0: \n",
    "        viz.display(q)\n",
    "        time.sleep(20*dt)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_freefall"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now modify the simulator code above 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": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)\n",
    "Kf = 0.1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Use the same loop templace as above"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And here the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_frictionfall"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Proportional-derivative and computed torque"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now choose a reference joint position.\n",
    "The joint torques can then be computed to track the desired position, with $$\\tau_q = -K_p (q-q^{des}) - K_v (\\dot{q} - \\dot{q}^{des})$$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, we can also add a force proportional to the desired acceleration. Indeed, when tracking is done correctly the PD controll gives a zero torque. To drive the acceleration toward the good sign we can add an acceleration contribution:\n",
    "\n",
    "$$\\tau_q = -K_p (q-q^{des}) - K_v (\\dot{q} - \\dot{q}^{des}) + \\alpha \\ddot{q}^{des}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First choose the PD gains and simulation parameter:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.zeros(robot.model.nq)\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)\n",
    "# Hyperparameters for the control and the simu\n",
    "Kf = 0.1\n",
    "Kp = 50.               # proportional gain (P of PD)\n",
    "Kv = 2 * np.sqrt(Kp)   # derivative gain (D of PD)\n",
    "alpha = 1.\n",
    "dt = 2e-3             # simulation timestep\n",
    "N_steps = 5000"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then choose the reference trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "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)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, implement the control loop using the following template."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for it in range(N_steps):\n",
    "    t = it*dt\n",
    "\n",
    "    # Retrieve the dynamics quantity at time t\n",
    "    M = np.eye(robot.model.nv)        # REPLACE ME\n",
    "    b = np.zeros(robot.model.nv)      # REPLACE ME\n",
    "\n",
    "    # Compute the force that apply\n",
    "    tauq = np.zeros(robot.model.nv)\n",
    "\n",
    "    # Use generalized PFD to calculate aq\n",
    "    aq = np.zeros(robot.model.nv)     # REPLACE ME\n",
    "\n",
    "    # Double integration to update vs and q\n",
    "    vq = np.zeros(robot.model.nv)    # REPLACE ME\n",
    "    q  = np.zeros(robot.model.nq)   # REPLACE ME\n",
    "\n",
    "    # Visualization\n",
    "    if it%20==0: \n",
    "        viz.display(q)\n",
    "        time.sleep(20*dt)\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": [
    "def plot_joint_profiles(i, hq, hqdes):\n",
    "    plt.subplot(111)\n",
    "    plt.plot([q[i] for q in hq])\n",
    "    plt.plot([q[i] for q in hqdes])\n",
    "    plt.ylabel(f'Joint {i}')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_joint_profiles(11, hq, hqdes)"
   ]
  },
  {
   "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": [
    "Collision detection is done using the state of the art GJK collision detection algorithm.\n",
    "Without saying its name, it is a Frank Wolf optimization scheme on the minkowski difference using the so called \"support function\".\n",
    "\n",
    "A nice blog post on GJK: https://cse442-17f.github.io/Gilbert-Johnson-Keerthi-Distance-Algorithm/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "GJK can easilly be extended with EPA to find the distance and normal to collision. Then because shapes are configuration parametrized, we can deduce a kinematic jacobian that translate configuration speed into normal speed of the collision."
   ]
  },
  {
   "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), which convert configuration velocities into veolocity of the contact interfaces. The contact distances noted $e$, and the self-acceleration of the contact point (similar to Coriolis acceleration, i.e. due to the velocity of the joints, also denoted by $\\dot{J}\\dot{q}$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dist=colwrap.getCollisionDistances(collisions)\n",
    "J = colwrap.getCollisionJacobian(collisions)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The contact accelereation is indeed $$v_c = \\frac{d e}{dt} = J \\dot{q}$$ $$a_c = \\frac{d v_c}{dt} = \\frac{d J \\dot{q}}{dt} = J \\ddot{q} + \\dot{J}\\dot{q}$$where $\\dot{J}\\dot{q}$ 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."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "colwrap.computeCollisions(q, vq)\n",
    "collisions = colwrap.getCollisionList()\n",
    "dist = colwrap.getCollisionDistances(collisions)\n",
    "J = colwrap.getCollisionJacobian(collisions)\n",
    "JdotQdot = colwrap.getCollisionJdotQdot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Better try this with a configuration of the hand where there are some collisions. Here is everything needed to solve a contact problem."
   ]
  },
  {
   "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 only go appart. It can be written as a minimization problem using Gauss principle:\n",
    "$$min \\quad \\frac{1}{2}  (\\ddot{q} - \\ddot{q}_0 )^\\top M (\\ddot{q} - \\ddot{q}_0 )$$\n",
    "$$s.t. \\quad a_c \\geq 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",
    "This model is physical, but because the simulation is time stepped, we need to adapt the physic to get a simulation closer to the reality.\n",
    "\n",
    "### 2. Impact\n",
    "When creating the contact with a non-zero velocity, enforcing a null acceleration is not sufficient as the two bodies will penetrate during the time step. When a new contact is created, the velocity of the contact normals should be nullified to render physic. You can do that by *projecting the current velocity in the null space of the normal jacobian of the new contacts*. 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",
    "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 to nullify the numerical drift effect. We enforce $a_c >= PD(e, v_v)$. The constraint become:\n",
    "$$ J \\ddot{q} + \\dot{J}\\dot{q} \\ge -K_p^c e -K_v^c J \\dot{q}$$\n",
    "\n",
    "\n",
    "### 4. Final numerical problem\n",
    "The problem can be rewritten as a QP:\n",
    "$$min \\quad \\frac{1}{2} \\ddot{q}^T M \\ddot{q} - (M \\ddot{q}_0)^\\top \\ddot{q} + (\\frac{1}{2} \\ddot{q}_0^T M \\ddot{q}_0)    $$\n",
    "$$s.t. \\quad J \\ddot{q} \\ge -K_p^c e - K_v^c J \\dot{q} - \\dot{J}\\dot{q} $$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are now all set up for implementing the contact simulator.\n",
    "The complete loop should be as follows:\n",
    "- torque is computed from a PD tracking a time-varying joint position (question 3).\n",
    "- After computing torque, all collision pairs must be checked to find those with distances collision penetrating more than $10 ^{-4}$\n",
    "- Corresponding Jacobians must be computed and stacked. If a new collision has appeared, the joint velocity must be projected to nullify it.\n",
    "- If not collision is active, the joint acceleration is computed from inverting the mass matrix (question 2).\n",
    "- Otherwise, it is computed using QP (question 4). With the PD corrected contact constraint.\n",
    "- The resulting acceleration is integrated twice (question 1) before displaying the robot starting a new simulation iteration.\n",
    "\n",
    "You can start from the same template as the freecontact solver."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.zeros(robot.model.nq)\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)\n",
    "\n",
    "# Hyperparameters for the control and the simu\n",
    "Kf = 0.1\n",
    "Kp = 50.               # proportional gain (P of PD)\n",
    "Kv = 2 * np.sqrt(Kp)   # derivative gain (D of PD)\n",
    "alpha = 1.\n",
    "\n",
    "dt = 1e-3             # simulation timestep\n",
    "N_steps = 10000\n",
    "\n",
    "Kp_c = 100.\n",
    "Kv_c = 2 * np.sqrt(Kp_c)\n",
    "\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)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start from the previous templates and add the contact simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_contact_simulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_joint_profiles(11, hq_c, hqdes_c)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What are all the problematic that can be explored ?\n",
    "- integrators\n",
    "- QP solvers\n",
    "- Other physic principles (e.g. Coulomb)\n",
    "- Other controller than PD\n",
    "- Numerical conditioning, footprint, convergence of algorithms"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Extension"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Instead of computing the torque $\\tau_q$ as the PD of a trajectory following in the configuration space, we can have different trajectory task in different spaces, then we can compute a PD for each task while being constraint to the kernel space of the previous tasks. We can use the result of the hierarchical procedure as a torq control instead."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can have a hierarchy that change during time e.g. for manipulation planning."
   ]
  },
  {
   "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.10.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}