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, Gepetto-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 pio\n",
    "from pinocchio.utils import *\n",
    "import numpy as np\n",
    "from numpy.linalg import inv,pinv,norm,eig\n",
    "pio.switchToNumpyMatrix()\n",
    "import matplotlib.pylab as plt\n",
    "plt.ion()\n",
    "from tp4.robot_hand import RobotHand\n",
    "import quadprog"
   ]
  },
  {
   "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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load -r 10-16 tp4/qp.py\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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load -r 19 tp4/qp.py\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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# help(quadprog.solve_qp)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A robot hand for our test\n",
    "\n",
    "For this notebook, we need the additional package Fast-Collision-Library \"sudo apt install robotpkg-py36-hpp-fcl\".\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",
    "robot.display(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Take care that the hand is small: zoom in to see it in the window (or press the space bar).\n",
    "\n",
    "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",
    "    robot.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 *RNE* algorithm (function of $q$, $v_q$ and $a_q$ with $a_q=0$). \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q   = rand(robot.model.nq)\n",
    "vq  = rand(robot.model.nv)\n",
    "aq0 = zero(robot.model.nv)\n",
    "b = pio.rnea(robot.model,robot.data,q,vq,aq0)  # compute dynamic drift -- Coriolis, centrifugal, gravity\n",
    "M = pio.crba(robot.model,robot.data,q)         # compute mass matrix M\n",
    "\n",
    "print (np.linalg.norm(M-M.T))\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": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tauq = zero(robot.model.nv)"
   ]
  },
  {
   "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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 32 tp4/solution_pd.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The inverse-dynamics algorithm indeed compute the needed torques to achieve a given acceleration. We can use this function to double-check our result:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(norm(pio.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 += 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,
   "metadata": {},
   "outputs": [],
   "source": [
    "dt = 1e-2\n",
    "vq += aq*dt\n",
    "q = pio.integrate(robot.model,q,vq*dt)"
   ]
  },
  {
   "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": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tauq = zero(robot.model.nv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load -r 32-35 tp4/solution_pd.py\n",
    "aq = inv(M) * (tauq - b)\n",
    "vq += aq * dt\n",
    "q = pio.integrate(robot.model, q, vq * dt)\n"
   ]
  },
  {
   "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": "code",
   "execution_count": null,
   "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%10==0: \n",
    "        robot.display(q)\n",
    "        time.sleep(5e-3)\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## (optional) Reference trajectory\n",
    "If you like it, we define an object 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 -r 30- tp4/traj_ref.py\n",
    "qdes = TrajRef(np.matrix([0,0,0.]).T,omega = np.array([1,2,3.]).T,amplitude=1.5)\n",
    "t = 0.2\n",
    "print(qdes(t).T,qdes.velocity(t).T,qdes.acceleration(t).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": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 16- tp4/solution_pd.py"
   ]
  },
  {
   "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.plot([q[2:4] for q in hq])\n",
    "plt.plot([q[2:4] for q in 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": [
    "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 2",
   "language": "python",
   "name": "python2"
  },
  "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}