From fa7be33b5db4d3443723df053b3a1c1eadcfb66e Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 9 Dec 2024 21:11:18 +0000
Subject: [PATCH] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 .../notebooks/Installation process.ipynb      |  179 +-
 ...x_0_joint_space_inverse_dynamics_arm.ipynb |  753 +++----
 .../notebooks/ex_1_com_sin_track_talos.ipynb  | 1963 +++++++++--------
 .../ex_3_biped_balance_with_gui.ipynb         |  509 ++---
 4 files changed, 1766 insertions(+), 1638 deletions(-)

diff --git a/exercizes/notebooks/Installation process.ipynb b/exercizes/notebooks/Installation process.ipynb
index 0923bcf9..a7dffb8a 100644
--- a/exercizes/notebooks/Installation process.ipynb	
+++ b/exercizes/notebooks/Installation process.ipynb	
@@ -1,92 +1,93 @@
 {
-  "cells": [
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "## Installation process"
-   ]
+  "cells" : [
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["## Installation process"]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "1) Install the robotpkg binaries which are needed for this "
+        "exercice:\n",
+        "\n", "   ```\n",
+        "   sudo apt-get install robotpkg-sot-talos robotpkg-talos-dev \n",
+        "   sudo apt-get install robotpkg-py27-qt4-gepetto-viewer-corba \n",
+        "   ```"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "#### Alternative to install jupyter\n",
+        "\n",
+        "2) Use ```pip install jupyter``` (but be carefull it may broke your "
+        "environment)\n",
+        "\n",
+        "2) Configure a specific environment with "
+        "[virtualenv](https://virtualenv.pypa.io/en/stable/):\n",
+        "\n",
+        "   - In the directory containing the jupyter notebook download the "
+        "_virtualenv_ source, extract and install it:  \n",
+        "    \n",
+        "    ```\n",
+        "    curl --location --output virtualenv-master.tar.gz "
+        "https://github.com/pypa/virtualenv/tarball/master \n",
+        "    tar xvfz virtualenv-master.tar.gz \n",
+        "    cd pypa-virtualenv-master \n",
+        "    python virtualenv.py myVE\n",
+        "    ```\n",
+        "    \n",
+        "- Then activate your environment:\n",
+        "    \n",
+        "    ```source myVE/bin/activate```\n",
+        "    \n",
+        "      \n",
+        "- And install the needed python packages:\n",
+        "    \n",
+        "    ```pip install jupyter numpy matplotlib```\n",
+        "    \n",
+        "- WARNING: for some obscure reasons, virtualenv removes "
+        "/usr/lib/python2.7/dist-packages from PYTHONPATH. You may need to "
+        "re-add it."
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "3) Source your terminal with the use of this "
+        "[script](http://robotpkg.openrobots.org/install.html) to setup your "
+        "environment variables to find _openrobots_ installation\n",
+        "\n",
+        "4) Make sure you have placed the plot_utils.py in the parent "
+        "directory of the jupyter notebooks\n",
+        "\n", "5) Start the notebook with the command:\n", "\n",
+        "  ```jupyter notebook```\n", "\n",
+        "6) At the end if you have chosen to use _virtualenv_ you should "
+        "deactivate your environment with the command:\n",
+        "\n", "  ```deactivate```"
+      ]
+    }
+  ],
+  "metadata" : {
+    "kernelspec" : {
+      "display_name" : "Python 2",
+      "language" : "python",
+      "name" : "python2"
+    },
+    "language_info" : {
+      "codemirror_mode" : {"name" : "ipython", "version" : 2},
+      "file_extension" : ".py",
+      "mimetype" : "text/x-python",
+      "name" : "python",
+      "nbconvert_exporter" : "python",
+      "pygments_lexer" : "ipython2",
+      "version" : "2.7.12"
+    }
   },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "1) Install the robotpkg binaries which are needed for this exercice:\n",
-    "\n",
-    "   ```\n",
-    "   sudo apt-get install robotpkg-sot-talos robotpkg-talos-dev \n",
-    "   sudo apt-get install robotpkg-py27-qt4-gepetto-viewer-corba \n",
-    "   ```"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "#### Alternative to install jupyter\n",
-    "\n",
-    "2) Use ```pip install jupyter``` (but be carefull it may broke your environment)\n",
-    "\n",
-    "2) Configure a specific environment with [virtualenv](https://virtualenv.pypa.io/en/stable/):\n",
-    "\n",
-    "   - In the directory containing the jupyter notebook download the _virtualenv_ source, extract and install it:  \n",
-    "    \n",
-    "    ```\n",
-    "    curl --location --output virtualenv-master.tar.gz https://github.com/pypa/virtualenv/tarball/master \n",
-    "    tar xvfz virtualenv-master.tar.gz \n",
-    "    cd pypa-virtualenv-master \n",
-    "    python virtualenv.py myVE\n",
-    "    ```\n",
-    "    \n",
-    "- Then activate your environment:\n",
-    "    \n",
-    "    ```source myVE/bin/activate```\n",
-    "    \n",
-    "      \n",
-    "- And install the needed python packages:\n",
-    "    \n",
-    "    ```pip install jupyter numpy matplotlib```\n",
-    "    \n",
-    "- WARNING: for some obscure reasons, virtualenv removes /usr/lib/python2.7/dist-packages from PYTHONPATH. You may need to re-add it."
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "3) Source your terminal with the use of this [script](http://robotpkg.openrobots.org/install.html) to setup your environment variables to find _openrobots_ installation\n",
-    "\n",
-    "4) Make sure you have placed the plot_utils.py in the parent directory of the jupyter notebooks\n",
-    "\n",
-    "5) Start the notebook with the command:\n",
-    "\n",
-    "  ```jupyter notebook```\n",
-    "\n",
-    "6) At the end if you have chosen to use _virtualenv_ you should deactivate your environment with the command:\n",
-    "\n",
-    "  ```deactivate```"
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 2",
-   "language": "python",
-   "name": "python2"
-  },
-  "language_info": {
-   "codemirror_mode": {
-    "name": "ipython",
-    "version": 2
-   },
-   "file_extension": ".py",
-   "mimetype": "text/x-python",
-   "name": "python",
-   "nbconvert_exporter": "python",
-   "pygments_lexer": "ipython2",
-   "version": "2.7.12"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 2
+  "nbformat" : 4,
+  "nbformat_minor" : 2
 }
diff --git a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
index 7c17f757..f3157261 100644
--- a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
+++ b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
@@ -1,391 +1,368 @@
 {
-  "cells": [
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "# Joint-Space Inverse Dynamics\n",
-    "\n",
-    "This notebook shows how to use the Python bindings of the C++ library TSID to control a manipulator in joint space."
-   ]
+  "cells" : [
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "# Joint-Space Inverse Dynamics\n", "\n",
+        "This notebook shows how to use the Python bindings of the C++ library "
+        "TSID to control a manipulator in joint space."
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "import sys\n", "from pathlib import Path\n", "\n",
+        "sys.path.append(\"..\")\n", "\n", "import numpy as np\n",
+        "from numpy.linalg import norm\n", "import matplotlib.pyplot as plt\n",
+        "import plot_utils as plut\n", "import time\n",
+        "import pinocchio as pin\n", "import tsid\n", "\n",
+        "# import gepetto.corbaserver\n", "\n",
+        "import talos_arm_conf as conf"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Create a `RobotWrapper` specifying the URDF file describing "
+                  "the robot."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "robot = tsid.RobotWrapper(conf.urdf, [conf.path], False)\n",
+        "model = robot.model()"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Create an `InverseDynamicsFormulationAccForce` object to "
+                  "collect all tasks together and transform them into a QP."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "formulation = tsid.InverseDynamicsFormulationAccForce(\"tsid\", "
+        "robot, False)\n",
+        "q0 = conf.q0\n", "v0 = np.zeros(robot.nv)\n",
+        "formulation.computeProblemData(0.0, q0, v0)"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" :
+          ["Create a `TaskJointPosture` to control the joint configuration of "
+           "the robot. Set the proportional and derivative gains $k_p$ and "
+           "$k_d$. Add the task to the formulation object with a "
+           "user-specified weight to the priority level 1 (1 corresponds to "
+           "the cost function, 0 corresponds to the constraints)."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "postureTask = tsid.TaskJointPosture(\"task-posture\", robot)\n",
+        "postureTask.setKp(conf.kp_posture * np.ones(robot.nv))\n",
+        "postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * "
+        "np.ones(robot.nv))\n",
+        "priorityLevel = 1\n", "transitionTime = 0.0\n",
+        "formulation.addMotionTask(postureTask, conf.w_posture, priorityLevel, "
+        "transitionTime)"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Create a constant trajectory in Euclidian space to use as "
+                  "reference for the joint posture task."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "trajPosture = tsid.TrajectoryEuclidianConstant(\"traj_joint\", q0)\n",
+        "postureTask.setReference(trajPosture.computeNext())"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" :
+          ["Create a TaskJointBounds to specify the joint velocity limits and "
+           "add it to the formulation object with priority level 0 (which "
+           "means it is added as a hard constraint in the QP)."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "v_max = conf.v_max_scaling * model.velocityLimit\n",
+        "v_min = -v_max\n",
+        "jointBoundsTask = tsid.TaskJointBounds(\"task-joint-bounds\", robot, "
+        "conf.dt)\n",
+        "jointBoundsTask.setVelocityBounds(v_min, v_max)\n",
+        "priorityLevel = 0\n", "formulation.addMotionTask(\n",
+        "    jointBoundsTask, conf.w_joint_bounds, priorityLevel, "
+        "transitionTime\n",
+        ")"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Create a QP solver for solving the TSID QP problem."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "solver = tsid.SolverHQuadProgFast(\"qp solver\")\n",
+        "solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Run the viewer if it's not already running."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
+        "    conf.urdf, [str(Path(conf.path) / \"../..\")]\n", ")\n",
+        "# Viewer = pin.visualize.GepettoVisualizer\n",
+        "Viewer = pin.visualize.MeshcatVisualizer\n", "viz = Viewer(\n",
+        "    robot_display.model, robot_display.collision_model, "
+        "robot_display.visual_model\n",
+        ")\n", "viz.initViewer(loadModel=True)\n", "viz.display(q0)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : ["hasattr(viz.viewer, \"jupyter_cell\") and "
+                  "viz.viewer.jupyter_cell()"]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Create empty arrays to store the simulation trajectories."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "N = conf.N_SIMULATION\n", "tau = np.full((robot.na, N), np.nan)\n",
+        "q = np.full((robot.nq, N + 1), np.nan)\n",
+        "v = np.full((robot.nv, N + 1), np.nan)\n",
+        "dv = np.full((robot.nv, N + 1), np.nan)\n",
+        "q_ref = np.full((robot.nq, N), np.nan)\n",
+        "v_ref = np.full((robot.nv, N), np.nan)\n",
+        "dv_ref = np.full((robot.nv, N), np.nan)\n",
+        "dv_des = np.full((robot.nv, N), np.nan)\n",
+        "samplePosture = trajPosture.computeNext()"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["Specify amplitude, phase and frequency of the sinusoidal "
+                  "joint trajectory to track."]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0])  # amplitude\n",
+        "phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0])  # "
+        "phase\n",
+        "two_pi_f = (\n",
+        "    2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0])\n",
+        ")  # frequency (time 2 PI)\n",
+        "two_pi_f_amp = np.multiply(two_pi_f, amp)\n",
+        "two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "Start simulating the system. At each loop:\n",
+        "* compute the reference joint sinusoidal trajectory and set it to the "
+        "posture task\n",
+        "* compute the QP problem data using formulation\n",
+        "* solve the QP and get the joint accelerations\n",
+        "* integrate the accelerations to update the robot state"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "t = 0.0\n",
+        "dt = conf.dt\n",
+        "q[:, 0], v[:, 0] = q0, v0\n",
+        "\n",
+        "for i in range(N):\n",
+        "    time_start = time.time()\n",
+        "\n",
+        "    # set reference trajectory\n",
+        "    q_ref[:, i] = q0 + amp * np.sin(two_pi_f * t + phi)\n",
+        "    v_ref[:, i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)\n",
+        "    dv_ref[:, i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + "
+        "phi)\n",
+        "    samplePosture.pos(q_ref[:, i])\n",
+        "    samplePosture.vel(v_ref[:, i])\n",
+        "    samplePosture.acc(dv_ref[:, i])\n",
+        "    postureTask.setReference(samplePosture)\n",
+        "\n",
+        "    HQPData = formulation.computeProblemData(t, q[:, i], v[:, i])\n",
+        "    sol = solver.solve(HQPData)\n",
+        "    if sol.status != 0:\n",
+        "        print(f\"Time {t:.3f} QP problem could not be solved! Error "
+        "code:\", sol.status)\n",
+        "        break\n",
+        "\n",
+        "    tau[:, i] = formulation.getActuatorForces(sol)\n",
+        "    dv[:, i] = formulation.getAccelerations(sol)\n",
+        "    dv_des[:, i] = postureTask.getDesiredAcceleration\n",
+        "\n",
+        "    if i % conf.PRINT_N == 0:\n",
+        "        print(f\"Time {t:.3f}\")\n",
+        "        print(\n",
+        "            \"\\ttracking err {}: {:.3f}\".format(\n",
+        "                postureTask.name.ljust(20, \".\"), "
+        "norm(postureTask.position_error, 2)\n",
+        "            )\n",
+        "        )\n",
+        "\n",
+        "    # numerical integration\n",
+        "    v_mean = v[:, i] + 0.5 * dt * dv[:, i]\n",
+        "    v[:, i + 1] = v[:, i] + dt * dv[:, i]\n",
+        "    q[:, i + 1] = pin.integrate(model, q[:, i], dt * v_mean)\n",
+        "    t += conf.dt\n",
+        "\n",
+        "    if i % conf.DISPLAY_N == 0:\n",
+        "        viz.display(q[:, i])\n",
+        "\n",
+        "    time_spent = time.time() - time_start\n",
+        "    if time_spent < conf.dt:\n",
+        "        time.sleep(conf.dt - time_spent)\n",
+        "\n",
+        "# PLOT STUFF\n",
+        "time = np.arange(0.0, N * conf.dt, conf.dt)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
+        "for i in range(robot.nv):\n",
+        "    ax[i].plot(time, q[i, :-1], label=\"q %i\" % i)\n",
+        "    ax[i].plot(time, q_ref[i, :], \"--\", label=\"q ref %i\" % i)\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"q [rad]\")\n", "    leg = ax[i].legend()\n",
+        "    leg.get_frame().set_alpha(0.5)\n", "plt.show()"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
+        "for i in range(robot.nv):\n",
+        "    ax[i].plot(time, v[i, :-1], label=\"v %i \" % i)\n",
+        "    ax[i].plot(time, v_ref[i, :], \"--\", label=\"v ref %i\" % i)\n",
+        "    ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], \":\")\n",
+        "    ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], \":\")\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"v [rad/s]\")\n", "    leg = ax[i].legend()\n",
+        "    leg.get_frame().set_alpha(0.5)\n", "plt.show()"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
+        "for i in range(robot.nv):\n",
+        "    ax[i].plot(time, dv[i, :-1], label=\"dv \" + str(i))\n",
+        "    ax[i].plot(time, dv_ref[i, :], \"--\", label=\"dv ref %i\" % i)\n",
+        "    ax[i].plot(time, dv_des[i, :], \":\", label=\"dv des %i\" % i)\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"dv [rad/s^2]\")\n",
+        "    leg = ax[i].legend()\n", "    leg.get_frame().set_alpha(0.5)\n",
+        "plt.show()"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
+        "for i in range(robot.nv):\n",
+        "    ax[i].plot(time, tau[i, :], label=\"tau %i\" % i)\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"tau [Nm]\")\n", "    leg = ax[i].legend()\n",
+        "    leg.get_frame().set_alpha(0.5)\n", "plt.show()"
+      ]
+    }
+  ],
+  "metadata" : {
+    "kernelspec" : {
+      "display_name" : "Python 3",
+      "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.5.2"
+    }
   },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import sys\n",
-    "from pathlib import Path\n",
-    "\n",
-    "sys.path.append(\"..\")\n",
-    "\n",
-    "import numpy as np\n",
-    "from numpy.linalg import norm\n",
-    "import matplotlib.pyplot as plt\n",
-    "import plot_utils as plut\n",
-    "import time\n",
-    "import pinocchio as pin\n",
-    "import tsid\n",
-    "\n",
-    "# import gepetto.corbaserver\n",
-    "\n",
-    "import talos_arm_conf as conf"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create a `RobotWrapper` specifying the URDF file describing the robot."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "robot = tsid.RobotWrapper(conf.urdf, [conf.path], False)\n",
-    "model = robot.model()"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create an `InverseDynamicsFormulationAccForce` object to collect all tasks together and transform them into a QP."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "formulation = tsid.InverseDynamicsFormulationAccForce(\"tsid\", robot, False)\n",
-    "q0 = conf.q0\n",
-    "v0 = np.zeros(robot.nv)\n",
-    "formulation.computeProblemData(0.0, q0, v0)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create a `TaskJointPosture` to control the joint configuration of the robot. Set the proportional and derivative gains $k_p$ and $k_d$. Add the task to the formulation object with a user-specified weight to the priority level 1 (1 corresponds to the cost function, 0 corresponds to the constraints)."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "postureTask = tsid.TaskJointPosture(\"task-posture\", robot)\n",
-    "postureTask.setKp(conf.kp_posture * np.ones(robot.nv))\n",
-    "postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv))\n",
-    "priorityLevel = 1\n",
-    "transitionTime = 0.0\n",
-    "formulation.addMotionTask(postureTask, conf.w_posture, priorityLevel, transitionTime)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create a constant trajectory in Euclidian space to use as reference for the joint posture task."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "trajPosture = tsid.TrajectoryEuclidianConstant(\"traj_joint\", q0)\n",
-    "postureTask.setReference(trajPosture.computeNext())"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create a TaskJointBounds to specify the joint velocity limits and add it to the formulation object with priority level 0 (which means it is added as a hard constraint in the QP)."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "v_max = conf.v_max_scaling * model.velocityLimit\n",
-    "v_min = -v_max\n",
-    "jointBoundsTask = tsid.TaskJointBounds(\"task-joint-bounds\", robot, conf.dt)\n",
-    "jointBoundsTask.setVelocityBounds(v_min, v_max)\n",
-    "priorityLevel = 0\n",
-    "formulation.addMotionTask(\n",
-    "    jointBoundsTask, conf.w_joint_bounds, priorityLevel, transitionTime\n",
-    ")"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create a QP solver for solving the TSID QP problem."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "solver = tsid.SolverHQuadProgFast(\"qp solver\")\n",
-    "solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Run the viewer if it's not already running."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
-    "    conf.urdf, [str(Path(conf.path) / \"../..\")]\n",
-    ")\n",
-    "# Viewer = pin.visualize.GepettoVisualizer\n",
-    "Viewer = pin.visualize.MeshcatVisualizer\n",
-    "viz = Viewer(\n",
-    "    robot_display.model, robot_display.collision_model, robot_display.visual_model\n",
-    ")\n",
-    "viz.initViewer(loadModel=True)\n",
-    "viz.display(q0)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "hasattr(viz.viewer, \"jupyter_cell\") and viz.viewer.jupyter_cell()"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Create empty arrays to store the simulation trajectories."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "N = conf.N_SIMULATION\n",
-    "tau = np.full((robot.na, N), np.nan)\n",
-    "q = np.full((robot.nq, N + 1), np.nan)\n",
-    "v = np.full((robot.nv, N + 1), np.nan)\n",
-    "dv = np.full((robot.nv, N + 1), np.nan)\n",
-    "q_ref = np.full((robot.nq, N), np.nan)\n",
-    "v_ref = np.full((robot.nv, N), np.nan)\n",
-    "dv_ref = np.full((robot.nv, N), np.nan)\n",
-    "dv_des = np.full((robot.nv, N), np.nan)\n",
-    "samplePosture = trajPosture.computeNext()"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Specify amplitude, phase and frequency of the sinusoidal joint trajectory to track."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0])  # amplitude\n",
-    "phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0])  # phase\n",
-    "two_pi_f = (\n",
-    "    2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0])\n",
-    ")  # frequency (time 2 PI)\n",
-    "two_pi_f_amp = np.multiply(two_pi_f, amp)\n",
-    "two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Start simulating the system. At each loop:\n",
-    "* compute the reference joint sinusoidal trajectory and set it to the posture task\n",
-    "* compute the QP problem data using formulation\n",
-    "* solve the QP and get the joint accelerations\n",
-    "* integrate the accelerations to update the robot state"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "t = 0.0\n",
-    "dt = conf.dt\n",
-    "q[:, 0], v[:, 0] = q0, v0\n",
-    "\n",
-    "for i in range(N):\n",
-    "    time_start = time.time()\n",
-    "\n",
-    "    # set reference trajectory\n",
-    "    q_ref[:, i] = q0 + amp * np.sin(two_pi_f * t + phi)\n",
-    "    v_ref[:, i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)\n",
-    "    dv_ref[:, i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + phi)\n",
-    "    samplePosture.pos(q_ref[:, i])\n",
-    "    samplePosture.vel(v_ref[:, i])\n",
-    "    samplePosture.acc(dv_ref[:, i])\n",
-    "    postureTask.setReference(samplePosture)\n",
-    "\n",
-    "    HQPData = formulation.computeProblemData(t, q[:, i], v[:, i])\n",
-    "    sol = solver.solve(HQPData)\n",
-    "    if sol.status != 0:\n",
-    "        print(f\"Time {t:.3f} QP problem could not be solved! Error code:\", sol.status)\n",
-    "        break\n",
-    "\n",
-    "    tau[:, i] = formulation.getActuatorForces(sol)\n",
-    "    dv[:, i] = formulation.getAccelerations(sol)\n",
-    "    dv_des[:, i] = postureTask.getDesiredAcceleration\n",
-    "\n",
-    "    if i % conf.PRINT_N == 0:\n",
-    "        print(f\"Time {t:.3f}\")\n",
-    "        print(\n",
-    "            \"\\ttracking err {}: {:.3f}\".format(\n",
-    "                postureTask.name.ljust(20, \".\"), norm(postureTask.position_error, 2)\n",
-    "            )\n",
-    "        )\n",
-    "\n",
-    "    # numerical integration\n",
-    "    v_mean = v[:, i] + 0.5 * dt * dv[:, i]\n",
-    "    v[:, i + 1] = v[:, i] + dt * dv[:, i]\n",
-    "    q[:, i + 1] = pin.integrate(model, q[:, i], dt * v_mean)\n",
-    "    t += conf.dt\n",
-    "\n",
-    "    if i % conf.DISPLAY_N == 0:\n",
-    "        viz.display(q[:, i])\n",
-    "\n",
-    "    time_spent = time.time() - time_start\n",
-    "    if time_spent < conf.dt:\n",
-    "        time.sleep(conf.dt - time_spent)\n",
-    "\n",
-    "# PLOT STUFF\n",
-    "time = np.arange(0.0, N * conf.dt, conf.dt)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
-    "for i in range(robot.nv):\n",
-    "    ax[i].plot(time, q[i, :-1], label=\"q %i\" % i)\n",
-    "    ax[i].plot(time, q_ref[i, :], \"--\", label=\"q ref %i\" % i)\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"q [rad]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "plt.show()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
-    "for i in range(robot.nv):\n",
-    "    ax[i].plot(time, v[i, :-1], label=\"v %i \" % i)\n",
-    "    ax[i].plot(time, v_ref[i, :], \"--\", label=\"v ref %i\" % i)\n",
-    "    ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], \":\")\n",
-    "    ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], \":\")\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"v [rad/s]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "plt.show()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
-    "for i in range(robot.nv):\n",
-    "    ax[i].plot(time, dv[i, :-1], label=\"dv \" + str(i))\n",
-    "    ax[i].plot(time, dv_ref[i, :], \"--\", label=\"dv ref %i\" % i)\n",
-    "    ax[i].plot(time, dv_des[i, :], \":\", label=\"dv des %i\" % i)\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"dv [rad/s^2]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "plt.show()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
-    "for i in range(robot.nv):\n",
-    "    ax[i].plot(time, tau[i, :], label=\"tau %i\" % i)\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"tau [Nm]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "plt.show()"
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 3",
-   "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.5.2"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 4
+  "nbformat" : 4,
+  "nbformat_minor" : 4
 }
diff --git a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
index 97834dcd..6695aa19 100644
--- a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
+++ b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
@@ -1,917 +1,1052 @@
 {
-  "cells": [
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "# Task Space Inverse Dynamics"
-   ]
+  "cells" : [
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : ["# Task Space Inverse Dynamics"]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "In this notebook is presented a simple use of the TSID framework on a "
+        "humanoid robot, Talos.\n",
+        "\n", "### Notations and Definitions\n", "\n",
+        "The robot system has a state $x$ and we denote the control inputs as "
+        "$u$. <br>\n",
+        "\n",
+        "The choice of the robot model depends both on the __robot__ and the "
+        "**task** to perform. For legged robots the typical model of the "
+        "actuators is a torque source because we will need to control the "
+        "contact forces exchanged between robot and environment. Therefore the "
+        "control input $u$ are motor torques $\\tau$. <br>\n",
+        "With the robot configuration $q$ and its velocity $\\dot{q}$ we have "
+        "the robot state $x$:\n",
+        "\n", "$$ x \\triangleq (q, \\dot{q}) $$"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "### Under-Actuated Systems\n",
+        "\n",
+        "A legged robot is __under actuated__, its number of actuators is less "
+        "than its number of degrees of freedom (DoFs). <br>\n",
+        "We denotes $n_{va}$ the number of actuators and $n_v$ the number of "
+        "DoFs:\n",
+        "\n",
+        "$$ n_{va} < n_v $$\n",
+        "\n",
+        "\n",
+        "Assume elements of $q$ are ordered, $q \\triangleq (q_u, q_a)$, "
+        "where:\n",
+        "-  $q_u \\in \\mathbb{R}^{n_{qu}} $ are the passive (unactuated) "
+        "joints\n",
+        "-  $q_a \\in \\mathbb{R}^{n_{qa}} $ are the actuated joints\n",
+        "\n",
+        "Similarly, $v \\triangleq (v_u, v_a)$, where $v_u \\in "
+        "\\mathbb{R}^{n_{vu}}$ and $v_a \\in \\mathbb{R}^{n_{va}}$.\n",
+        "\n",
+        "$ S \\triangleq [\\ 0_{n_{va} \\ \\times \\ n_{vu}} \\ \\ I_{n_{va}}] "
+        "$ is a selection matrix associated to the actuated joints:\n",
+        "$$ v_a = S \\ v $$\n",
+        "\n",
+        "\n",
+        "The dynamic of an under-actuated mechanical system is:\n",
+        "\n",
+        "$$ M(q) \\ \\dot{v} \\ + \\ h(q, v) \\ = \\ S^T \\tau \\ + \\ J(q)^T "
+        "f $$\n",
+        "\n",
+        "where $M(q) \\in \\mathbb{R}^{n_v × n_v}$ is the mass matrix, $h(q,v) "
+        "\\in \\mathbb{R}^{n_v}$ are the bias forces, $\\tau \\in "
+        "\\mathbb{R}^{n_{va}}$ are the joint torques, $f \\in "
+        "\\mathbb{R}^{n_f}$ are the contact forces, and $J(q) \\in "
+        "\\mathbb{R}^{n_f×n_v}$ is the contact Jacobian.\n",
+        "\n",
+        "This dynamic is often decomposed into unactuated and actuated "
+        "parts:\n",
+        "\n",
+        "\\begin{array}{r c r c l}\n",
+        "      M_u(q) \\ \\dot{v}  & + & h_u(q, v) & = & J_u(q)^T f \\\\\n",
+        "      M_a(q) \\ \\dot{v}  & + & h_a(q, v) & = & \\tau \\ + \\ "
+        "J_a(q)^T f\n",
+        "\\end{array}\n",
+        "\n",
+        "Where\n",
+        "\n",
+        "\\begin{array}{r c l}\n",
+        "   M & = & \\begin{bmatrix}\n",
+        "   M_u \\\\\n",
+        "   M_a\n",
+        "\\end{bmatrix}  \\\\\n",
+        "   h & = & \\begin{bmatrix}\n",
+        "   h_u \\\\\n",
+        "   h_a\n",
+        "\\end{bmatrix}  \\\\\n",
+        "  J & = & [J_u \\ \\ J_a]\n",
+        "\\end{array}\n",
+        "   "
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "### Generic Output Function\n",
+        "\n",
+        "An output function $y$, can be an end-effector pose (such as a "
+        "gripper), the robot center-of-mass, a visual feature position inside "
+        "an image.\n",
+        "We assume that an output is a function from the robot configuration "
+        "vector $q$ to a set called $Y$:\n",
+        "\n",
+        "$$y = f_{task}(q)$$\n",
+        "\n",
+        "It is also assumed that $f_{task}$ is $\\mathcal{C}^1$ then:\n",
+        "$$J = \\displaystyle \\frac{\\partial f_{task}}{\\partial q}$$\n",
+        "\n",
+        "When a desired output trajectory $y^*(t)$ is given, a task error is "
+        "defined as:\n",
+        "\n",
+        "$$\n",
+        "  e: \\mathcal{C} \\times \\mathbf{R} \\rightarrow \\mathbf{R}^m "
+        "\\qquad\n",
+        "  e = y \\ominus y^*\n",
+        "$$\n",
+        "\n",
+        "where $\\ominus$ is the difference operator in the output space (it "
+        "is the standard difference if the output space is Euclidian).\n",
+        "\n",
+        "We can now define the desired dynamics of the task.\n",
+        "Usually, when the robot is controlled in velocity, the desired "
+        "dynamics is set to:\n",
+        "\n",
+        "$$  \\dot{e}^* = - K e $$\n",
+        "\n",
+        "\n",
+        "When the robot is controlled at the acceleration level (which is our "
+        "case because we assume a torque controlled robot), the desired task "
+        "dynamics is typically specified with a PID control law:\n",
+        "\n",
+        "$$\n",
+        "  \\ddot{e}^* = K_P e + K_D \\dot{e} + K_I \\displaystyle "
+        "\\int_{j=0}^{j=k} e(j) dj\n",
+        "$$\n",
+        "\n",
+        "with $K_P$ the error gain, $K_D$ the derivative of the error gain, "
+        "and $K_I$ the integral gain.\n",
+        "\n",
+        "We can now derive the real error dynamics as:\n",
+        "\n",
+        "$$  \\dot{e} = \\dot{y} - \\dot{y}^*\n",
+        "     = J \\dot{q} - \\dot{y}^*$$\n",
+        "\n",
+        "therefore:\n",
+        "\n",
+        "$$\\ddot{e} = J \\ddot{q} + \\dot{J} \\dot{q} - \\ddot{y}^* $$\n"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "### QP Optimisation problem with acceleration and torque\n",
+        "\n",
+        "The  transcription  of  the  motion  reference  from  the  task space "
+        " to  the  whole-body  control  is  naturally  written  as  a "
+        "quadratic  program  (QP).  A  QP  is  composed  of  two layers,  "
+        "namely  the  constraints  and  the  cost.  It  can  be  seen  as a  "
+        "hierarchy  of  two  levels,  the  constraint  having  priority  over "
+        "the  cost. Inequalities can also be taken into account  directly  as  "
+        "constraints,  or indirectly in  the  cost  function.\n",
+        "\n",
+        "Let us simplify the equations of motion based on the rigid body "
+        "dynamics assuming there is no contact:\n",
+        "\n",
+        "$$M \\dot{v} + h = S^T \\tau $$\n",
+        "\n",
+        "Let us assume we have a task error $e$ regulating an output function "
+        "$y$.\n",
+        "\n",
+        "We can introduce a slack  variable $s$ (an implicit optimization "
+        "variable) to  represent the difference between desired and real error "
+        "dynamics:\n",
+        "\n",
+        "$$\\ddot{e} - \\ddot{e}^*  \\triangleq  s $$\n",
+        "\n",
+        "A simple formulation of the QP problem can then be expressed as:\n",
+        "\n",
+        "$$   \\underset{\\dot{v},\\tau}{\\min} \\quad  \\| s \\|^2 \\\\\n",
+        "\t\\textrm{s.t.} \\quad  M \\dot{v} + h = S^T \\tau $$\n",
+        "\n",
+        "If the system is in contact with environment, its dynamic must "
+        "account for contact forces $f_{ext}$. If contacts are soft, "
+        "measured/estimated contact forces $\\hat{f}_{ext}$ can be easily "
+        "included:\n",
+        "\n",
+        "$$\t\\underset{\\dot{v},\\tau}{\\min} \\quad \\| s \\|^2 \\\\\n",
+        "\t\\textrm{s.t.} \\quad M \\dot{v} + h = S^T \\tau + J_c^T "
+        "\\hat{f}_{ext} $$\n",
+        "\n",
+        "\n",
+        "But if contacts are rigid, they constrain the motion. They are "
+        "usually modelled as nonlinear functions, which are differentiated "
+        "twice:\n",
+        "\n",
+        "$$\tc(q)  =  0  \\, \\Leftrightarrow  \\text{Contact point do not "
+        "move}$$\n",
+        "\n",
+        "$$\tJ_c \\, v  = 0  \\, \\Leftrightarrow  \\text{Contact point "
+        "velocities are null}$$\n",
+        "\n",
+        "$$\tJ_c \\, \\dot{v} + \\dot{J_c} \\, v  =  0  \\, \\Leftrightarrow  "
+        "\\text{Contact point accelerations are null}$$\n",
+        "\n",
+        "\n",
+        "This leads to the following optimization problem:\n",
+        "\n",
+        "$$\t\\underset{\\dot{v},\\tau}{\\min} \\quad  \\| s \\|^2 \\\\\n",
+        "\t\\textrm{s.t.} \\quad \\Bigg[\n",
+        "\t\\begin{array}{lll}\n",
+        "\t\tJ_c & 0 & 0 \\\\\n",
+        "\t\tM  & -J_c^T & -S^T\n",
+        "\t\\end{array}\n",
+        "\t\\Bigg] \\, \\Bigg[\\begin{array}{l}\n",
+        "                    \\dot{v} \\\\\n",
+        "                    f \\\\\n",
+        "                    \\tau\n",
+        "                    \\end{array}\n",
+        "               \\Bigg] \\, = \\, \\Bigg[\\begin{array}{l}\n",
+        "                                    - \\dot{J_c} \\, v \\\\\n",
+        "                                    -h\n",
+        "                                    \\end{array}\n",
+        "                              \\Bigg]$$\n",
+        "\n",
+        "The main benefit of QP solvers is that they can handle inequality "
+        "constraints. Inequalities are mainly used to defined boundaries of "
+        "the system, such as joint torque, velocity or position limits, or "
+        "friction cones for the contact forces."
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "### Weighted Sum\n",
+        "\n",
+        "Complex robots are typically redundant with respect to the main task "
+        "they must perform, this redundancy can be used to execute secondary "
+        "tasks. This multi-objective optimization can be achieved by setting "
+        "respective weights between the tasks (strategy used in TSID), or by "
+        "imposing a strict hierarchy between them (cascade of QP or HQP).\n",
+        "\n",
+        "Assume robot must perform N tasks, each defined by a task function "
+        "$g_i(\\cdot)$:\n",
+        "\n",
+        "$$\tg_i = \\| A_i \\Bigg[ \\begin{array}{l} \\dot{v} \\\\ f \\\\ "
+        "\\tau \\end{array}\\Bigg] - a_i \\|^2 $$\n",
+        "\n",
+        "The easiest strategy is to sum all functions using user-defined "
+        "weights $w_i$:\n",
+        "\n",
+        "$$\t\\underset{\\dot{v},f,\\tau}{\\min} \\quad \\displaystyle "
+        "\\sum_{i=0}^N{w_i \\, g_i} \\\\\n",
+        "\t\\textrm{s.t.} \\quad \\Bigg[\n",
+        "\t\\begin{array}{lll}\n",
+        "\t\tJ_c & 0 & 0 \\\\\n",
+        "\t\tM  & -J_c^T & -S^T\n",
+        "\t\\end{array}\n",
+        "\t\\Bigg] \\, \\Bigg[\\begin{array}{l}\n",
+        "                    \\dot{v} \\\\\n",
+        "                    f \\\\\n",
+        "                    \\tau\n",
+        "                    \\end{array}\n",
+        "               \\Bigg] \\, = \\, \\Bigg[\\begin{array}{l}\n",
+        "                                    - \\dot{J_c} \\, v \\\\\n",
+        "                                    -h\n",
+        "                                    \\end{array}\n",
+        "                              \\Bigg]$$\n",
+        "\n",
+        "This problem remains standard computationally-efficient (compared to "
+        "Least Square Programming). However, finding proper weights can be "
+        "hard because too extreme weights lead to numerical issues. "
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "# An example: CoM Sinusoidal trajectory on the robot TALOS\n", "\n",
+        "The goal of this exercise is to create a set of tasks allowing the "
+        "tracking of a sinusoidal trajectory with the CoM of the biped robot "
+        "Talos.\n",
+        "\n",
+        "In the following we describe the process to create, initialise and "
+        "solve the QP problem defined by the tasks."
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Python import needed by the exercise\n",
+        "import matplotlib.pyplot as plt\n",
+        "import numpy as np\n",
+        "from numpy.linalg import norm\n",
+        "from pathlib import Path\n",
+        "import time as tmp\n",
+        "\n",
+        "# import the library TSID for the Whole-Body Controller\n",
+        "import tsid\n",
+        "\n",
+        "# import the pinocchio library for the mathematical methods (Lie "
+        "algebra) and multi-body dynamics computations.\n",
+        "import pinocchio as pin\n",
+        "\n",
+        "# import example_example_robot_data to get the current path for the "
+        "robot models\n",
+        "from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
+        "\n",
+        "import sys\n",
+        "\n",
+        "sys.path.append(\"..\")\n",
+        "\n",
+        "# import graphical tools\n",
+        "import plot_utils as plut"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Definition of the tasks gains, weights and the foot geometry (for "
+        "contact task)\n",
+        "\n",
+        "lxp = 0.1  # foot length in positive x direction\n",
+        "lxn = 0.11  # foot length in negative x direction\n",
+        "lyp = 0.069  # foot length in positive y direction\n",
+        "lyn = 0.069  # foot length in negative y direction\n",
+        "lz = 0.107  # foot sole height with respect to ankle joint\n",
+        "mu = 0.3  # friction coefficient\n",
+        "fMin = 1.0  # minimum normal force\n",
+        "fMax = 1000.0  # maximum normal force\n",
+        "\n",
+        "rf_frame_name = \"leg_right_6_joint\"  # right foot joint name\n",
+        "lf_frame_name = \"leg_left_6_joint\"  # left foot joint name\n",
+        "contactNormal = np.array(\n",
+        "    [0.0, 0.0, 1.0]\n",
+        ")  # direction of the normal to the contact surface\n",
+        "\n",
+        "w_com = 1.0  # weight of center of mass task\n",
+        "w_posture = 0.1  # weight of joint posture task\n",
+        "w_forceRef = 1e-3  # weight of force regularization task\n",
+        "w_waist = 1.0  # weight of waist task\n",
+        "\n",
+        "kp_contact = 30.0  # proportional gain of contact constraint\n",
+        "kp_com = 20.0  # proportional gain of center of mass task\n",
+        "kp_waist = 500.0  # proportional gain of waist task\n",
+        "\n",
+        "kp_posture = np.array(  # proportional gain of joint posture task\n",
+        "    [\n",
+        "        10.0,\n",
+        "        5.0,\n",
+        "        5.0,\n",
+        "        1.0,\n",
+        "        1.0,\n",
+        "        10.0,  # lleg, low gain on axis along y and knee\n",
+        "        10.0,\n",
+        "        5.0,\n",
+        "        5.0,\n",
+        "        1.0,\n",
+        "        1.0,\n",
+        "        10.0,  # rleg\n",
+        "        500.0,\n",
+        "        500.0,  # chest\n",
+        "        50.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,  # larm\n",
+        "        50.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,\n",
+        "        10.0,  # rarm\n",
+        "        100.0,\n",
+        "        100.0,\n",
+        "    ]  # head\n",
+        ")\n",
+        "\n",
+        "dt = 0.001  # controller time step\n",
+        "PRINT_N = 500  # print every PRINT_N time steps\n",
+        "DISPLAY_N = 20  # update robot configuration in viwewer every "
+        "DISPLAY_N time steps\n",
+        "N_SIMULATION = 10000  # number of time steps simulated"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Set the path where the urdf file of the robot is registered\n",
+        "path = EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
+        "urdf = path + \"/talos_data/robots/talos_reduced.urdf\"\n",
+        "# Create the robot wrapper from the urdf, it will give the model of "
+        "the robot and its data\n",
+        "robot = tsid.RobotWrapper(urdf, [path], pin.JointModelFreeFlyer(), "
+        "False)\n",
+        "srdf = path + \"/talos_data/srdf/talos.srdf\""
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Creation of the robot wrapper for gepetto viewer (graphical "
+        "interface)\n",
+        "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
+        "    urdf, [str(Path(path) / \"../..\")], pin.JointModelFreeFlyer()\n",
+        ")\n", "# Viewer = pin.visualize.GepettoVisualizer\n",
+        "Viewer = pin.visualize.MeshcatVisualizer\n", "viz = Viewer(\n",
+        "    robot_display.model, robot_display.collision_model, "
+        "robot_display.visual_model\n",
+        ")\n", "viz.initViewer(loadModel=True)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Take the model of the robot and load its reference configurations\n",
+        "model = robot.model()\n",
+        "pin.loadReferenceConfigurations(model, srdf, False)\n",
+        "# Set the current configuration q to the robot configuration "
+        "half_sitting\n",
+        "q = model.referenceConfigurations[\"half_sitting\"]\n",
+        "# Set the current velocity to zero\n", "v = np.zeros(robot.nv)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Display the robot in Gepetto Viewer in the configuration q = "
+        "halfSitting\n",
+        "viz.display(q)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Check that the frames of the feet exist.\n",
+        "assert model.existFrame(rf_frame_name)\n",
+        "assert model.existFrame(lf_frame_name)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "t = 0.0  # time\n", "\n",
+        "# InverseDynamicsFormulationAccForce is the class in charge of\n",
+        "# creating the inverse dynamics HQP problem using\n",
+        "# the robot accelerations (base + joints) and the contact forces as "
+        "decision variables\n",
+        "invdyn = tsid.InverseDynamicsFormulationAccForce(\"tsid\", robot, "
+        "False)\n",
+        "invdyn.computeProblemData(t, q, v)\n",
+        "# Get the data -> initial data\n", "data = invdyn.data()"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "## Tasks Definitions\n",
+        "\n",
+        "A **Task** is a control objective for the robot, which is used at "
+        "each control cycle to generate a **Constraint**.\n",
+        "Note that constraints are purely mathematical objects that are "
+        "independent of the concept of robot, while Tasks are instead "
+        "robot-related objects.\n",
+        "\n",
+        "A **Constraint** is a linear equality or inequality.\n",
+        "In TSID the HQP is defined as a collection of constraints with "
+        "different priority levels and weights.\n",
+        "There are three kinds of constraints defined in TSID:\n",
+        "\n",
+        "- Equalities, represented by matrix $A$ and vector $a$:\n",
+        "  $$ Ax = a $$\n",
+        "\n",
+        "- Inequalities, represented by matrix $A$ and vectors $lb$ and "
+        "$ub$:\n",
+        "  $$ lb ≤ Ax ≤ ub $$\n",
+        "\n",
+        "- Bounds, represented by vectors $lb$ and $ub$:\n",
+        "  $$ lb ≤ x ≤ ub $$\n",
+        "\n",
+        "There are three kinds of Task in TSID:\n",
+        "\n",
+        "- **TaskMotion**: computes a constraint that is a linear function of "
+        "the robot accelerations\n",
+        "- **TaskContactForce**: computes a constraint that is a linear "
+        "function of the contact forces\n",
+        "- **TaskActuation**: computes a constraint that is a linear function "
+        "of the joint torques\n",
+        "\n",
+        "Tasks can compute either equality constraints, or bounds, or "
+        "inequality constraints.\n",
+        "\n",
+        "Examples of **TaskMotion**:\n",
+        "\n",
+        "- **TaskComEquality**: computes an equality constraint to specify a "
+        "desired acceleration of the center of mass (CoM) of the robot.\n",
+        "- **TaskJointPosture**: computes an equality constraint to specify "
+        "the desired joint accelerations.\n",
+        "- **TaskSE3Equality**: computes an equality constraint to specify the "
+        "desired acceleration for a frame attached to one of the links of the "
+        "robot.\n",
+        "- **TaskJointBounds**: computes a bounds constraint to specify the "
+        "joint acceleration bounds in order to satisfy the joint "
+        "position/velocity/acceleration limits."
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "In this exercise, for the sinusoidal movement of the CoM, we need 3 "
+        "tasks of class TaskMotion:\n",
+        "\n",
+        " - **TaskComEquality** as **constraint of the control** (priority "
+        "level = 0) to make the CoM follow a sinusoidal trajectory. It is the "
+        "most important task so it is introduced as a constraint.\n",
+        " - **TaskSE3Equality** in the **cost function** (priority level = 1) "
+        "for the waist of the robot, to maintain its orientation (with a "
+        "reference trajectory). It is an important task so it has a weight of "
+        "1.\n",
+        " - **TaskJointPosture** in the **cost function** (priority level = 1) "
+        "for the posture of the robot, to maintain it to half-sitting as much "
+        "as possible (with a reference trajectory). It is the least important "
+        "task so it has a weight of 0.1."
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# COM Task\n",
+        "comTask = tsid.TaskComEquality(\"task-com\", robot)\n",
+        "comTask.setKp(kp_com * np.ones(3))  # Proportional gain defined "
+        "before = 20\n",
+        "comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3))  # Derivative gain "
+        "= 2 * sqrt(20)\n",
+        "# Add the task to the HQP with weight = 1.0, priority level = 0 (as "
+        "constraint) and a transition duration = 0.0\n",
+        "invdyn.addMotionTask(comTask, w_com, 0, 0.0)\n",
+        "\n",
+        "# WAIST Task\n",
+        "waistTask = tsid.TaskSE3Equality(\n",
+        "    \"task-waist\", robot, \"root_joint\"\n",
+        ")  # waist -> root_joint\n",
+        "waistTask.setKp(kp_waist * np.ones(6))  # Proportional gain defined "
+        "before = 500\n",
+        "waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6))  # Derivative "
+        "gain = 2 * sqrt(500)\n",
+        "\n",
+        "# Add a Mask to the task which will select the vector dimensions on "
+        "which the task will act.\n",
+        "# In this case the waist configuration is a vector 6d (position and "
+        "orientation -> SE3)\n",
+        "# Here we set a mask = [0 0 0 1 1 1] so the task on the waist will "
+        "act on the orientation of the robot\n",
+        "mask = np.ones(6)\n",
+        "mask[:3] = 0.0\n",
+        "waistTask.setMask(mask)\n",
+        "# Add the task to the HQP with weight = 1.0, priority level = 1 (in "
+        "the cost function) and a transition duration = 0.0\n",
+        "invdyn.addMotionTask(waistTask, w_waist, 1, 0.0)\n",
+        "\n",
+        "\n",
+        "# POSTURE Task\n",
+        "postureTask = tsid.TaskJointPosture(\"task-posture\", robot)\n",
+        "postureTask.setKp(\n",
+        "    kp_posture\n",
+        ")  # Proportional gain defined before (different for each joints)\n",
+        "postureTask.setKd(2.0 * kp_posture)  # Derivative gain = 2 * kp\n",
+        "# Add the task with weight = 0.1, priority level = 1 (in cost "
+        "function) and a transition duration = 0.0\n",
+        "invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "## Rigid Contacts\n",
+        "\n",
+        "A **Rigid Contact** is a description of a rigid contact between a "
+        "body of the robot and the environment.\n",
+        "The main difference between a task and a rigid contact is that a "
+        "rigid contact is associated to reaction forces, while a task is "
+        "not.\n",
+        "\n",
+        "This class allows to use different representations for the motion "
+        "space and the force space (more details on this below), and it is "
+        "characterize by the following four elements.\n",
+        "\n",
+        "- Motion Task\n",
+        "    - Represents the motion constraint (equality) associated to the "
+        "rigid contact\n",
+        "    - $ J_c \\, \\dot{v} \\, = \\, - \\dot{J_c} \\, v \\, - \\, K_p "
+        "\\ e - \\, K_d \\, \\dot{e}$\n",
+        "\n",
+        "\n",
+        "- Force Task\n",
+        "    - Computes the inequality constraints acting on the contact "
+        "forces\n",
+        "    - e.g., friction cone constraints : $A \\, f \\,  ≤ \\, a$\n",
+        "\n",
+        "\n",
+        "- Force Regularization Task\n",
+        "    - Computes a regularization cost on the contact forces\n",
+        "    - e.g., keep them close to friction cone center\n",
+        "\n",
+        "\n",
+        "- Force-Generator matrix $T$\n",
+        "    - Maps force variables to motion constraint representation\n",
+        "    - Dynamic: $ M \\ \\dot{v} \\ + \\ h \\ = \\ S^T \\tau \\ + \\ "
+        "J_c^T T f $\n",
+        "    - Motion constraint : $ J_c \\, \\dot{v} \\, = \\, - \\dot{J_c} "
+        "\\, v$\n",
+        "    - Friction cones: $A \\, f \\,  ≤ \\, a$"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "### Contact 6d\n", "\n",
+        "In case of a unilateral plane contact (with polygonal shape), the "
+        "motion constraint is 6d, because the body in contact cannot move in "
+        "any direction.\n",
+        "##### PROBLEM\n", "\n",
+        "Minimal force representation would be 6d (3d force + 3d moment):\n",
+        "- It is hard to write friction constraints with 6d force "
+        "representation (especially for non-rectangular contact shapes)\n",
+        "- A solution would be to represent the reaction force as collection "
+        "of 3d forces applied at vertices of contact surface (writting "
+        "friction constraints is then easy)\n",
+        "    - But it leads to redundant representation, e.g., 4-vertex "
+        "surface → 12 variables\n",
+        "    - The redundancy is an issue for motion constraints if the HQP "
+        "solver does not handle redundant constraints (as eiQuadProg).\n",
+        "\n", "##### SOLUTION\n", "\n",
+        "- Use 6d representation for motion constraint $ J_c \\, \\dot{v} \\, "
+        "= \\, - \\dot{J_c} \\, v \\, \\in \\mathbf{R}^6$\n",
+        "- But use 12d representation for force variable $f \\in "
+        "\\mathbf{R}^{12}$\n",
+        "- A force-generator matrix $T \\in \\mathbf{R}^{6 \\times 12}$ "
+        "defines the mapping between the two representations:\n",
+        "    $\\tau_{c} \\, = J_c^T \\, T \\, f$"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "In this exercise, we need two Rigid Contacts, one for each foot as "
+        "**Contact6d**.\n",
+        "\n",
+        "The Rigid Contacts are always defined as **constraints** (priority "
+        "level = 0) to maintain the robot in contact."
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# CONTACTS 6D\n",
+        "# Definition of the foot geometry with respect to the ankle joints "
+        "(which are the ones controlled)\n",
+        "contact_Point = np.ones((3, 4)) * lz\n",
+        "contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]\n",
+        "contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]\n",
+        "\n",
+        "# The feet are the only bodies in contact in this experiment and "
+        "their geometry defines the plane of contact\n",
+        "# between the robot and the environement -> it is a Contact6D\n",
+        "\n",
+        "# To define a contact6D :\n",
+        "# We need the surface of contact (contact_point), the normal vector "
+        "of contact (contactNormal along the z-axis)\n",
+        "# the friction parameter with the ground (mu = 0.3), the normal force "
+        "bounds (fMin =1.0, fMax=1000.0)\n",
+        "\n",
+        "# Right Foot\n",
+        "contactRF = tsid.Contact6d(\n",
+        "    \"contact_rfoot\", robot, rf_frame_name, contact_Point, "
+        "contactNormal, mu, fMin, fMax\n",
+        ")\n",
+        "contactRF.setKp(kp_contact * np.ones(6))  # Proportional gain defined "
+        "before = 30\n",
+        "contactRF.setKd(\n",
+        "    2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
+        ")  # Derivative gain = 2 * sqrt(30)\n",
+        "# Reference position of the right ankle -> initial position\n",
+        "H_rf_ref = robot.position(data, model.getJointId(rf_frame_name))\n",
+        "contactRF.setReference(H_rf_ref)\n",
+        "# Add the contact to the HQP with weight = 0.1 for the force "
+        "regularization task,\n",
+        "# and priority level = 0 (as real constraint) for the motion "
+        "constraint\n",
+        "invdyn.addRigidContact(contactRF, w_forceRef)\n",
+        "\n",
+        "# Left Foot\n",
+        "contactLF = tsid.Contact6d(\n",
+        "    \"contact_lfoot\", robot, lf_frame_name, contact_Point, "
+        "contactNormal, mu, fMin, fMax\n",
+        ")\n",
+        "contactLF.setKp(kp_contact * np.ones(6))  # Proportional gain defined "
+        "before = 30\n",
+        "contactLF.setKd(\n",
+        "    2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
+        ")  # Derivative gain = 2 * sqrt(30)\n",
+        "# Reference position of the left ankle -> initial position\n",
+        "H_lf_ref = robot.position(data, model.getJointId(lf_frame_name))\n",
+        "contactLF.setReference(H_lf_ref)\n",
+        "# Add the contact to the HQP with weight = 0.1 for the force "
+        "regularization task,\n",
+        "# and priority level = 0 (as real constraint) for the motion "
+        "constraint\n",
+        "invdyn.addRigidContact(contactLF, w_forceRef)"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" : [
+        "## TSID Trajectory\n", "\n",
+        "A **Trajectory** is a multi-dimensional function of time describing "
+        "the motion of an object and its time derivatives.\n",
+        "For standard use in control, the method *compute_next* is provided, "
+        "which computes the value of the trajectory at the next time step.\n",
+        "\n",
+        "In the example, we need to set 3 trajectories, one for each task.\n",
+        "These trajectories will give at each time step the desired position, "
+        "velocity and acceleration of the different tasks (CoM, posture and "
+        "waist).\n",
+        "In our case, the posture and the waist will be constant, equal to "
+        "their initial values.\n",
+        "For the CoM however, we will update the trajectory with a sinusoidal "
+        "signal at each time step."
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Set the reference trajectory of the tasks\n", "\n",
+        "com_ref = data.com[0]  # Initial value of the CoM\n",
+        "trajCom = tsid.TrajectoryEuclidianConstant(\"traj_com\", com_ref)\n",
+        "sampleCom = (\n", "    trajCom.computeNext()\n",
+        ")  # Compute the first step of the trajectory from the initial "
+        "value\n",
+        "\n", "q_ref = q[\n", "    7:\n",
+        "]  # Initial value of the joints of the robot (in halfSitting "
+        "position without the freeFlyer (6 first values))\n",
+        "trajPosture = tsid.TrajectoryEuclidianConstant(\"traj_joint\", "
+        "q_ref)\n",
+        "\n", "waist_ref = robot.position(\n",
+        "    data, model.getJointId(\"root_joint\")\n",
+        ")  # Initial value of the waist (root_joint)\n",
+        "# Here the waist is defined as a 6d vector (position + orientation) "
+        "so it is in the SE3 group (Lie group)\n",
+        "# Thus, the trajectory is not Euclidian but remains in the SE3 domain "
+        "-> TrajectorySE3Constant\n",
+        "trajWaist = tsid.TrajectorySE3Constant(\"traj_waist\", waist_ref)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Initialisation of the Solver\n",
+        "# Use EiquadprogFast: dynamic matrix sizes (memory allocation "
+        "performed only when resizing)\n",
+        "solver = tsid.SolverHQuadProgFast(\"qp solver\")\n",
+        "# Resize the solver to fit the number of variables, equality and "
+        "inequality constraints\n",
+        "solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Initialisation of the plot variables which will be updated during "
+        "the simulation loop\n",
+        "# These variables describe the behavior of the CoM of the robot "
+        "(reference and real position, velocity and acceleration)\n",
+        "com_pos = np.full((3, N_SIMULATION), np.nan)\n",
+        "com_vel = np.full((3, N_SIMULATION), np.nan)\n",
+        "com_acc = np.full((3, N_SIMULATION), np.nan)\n", "\n",
+        "com_pos_ref = np.full((3, N_SIMULATION), np.nan)\n",
+        "com_vel_ref = np.full((3, N_SIMULATION), np.nan)\n",
+        "com_acc_ref = np.full((3, N_SIMULATION), np.nan)\n",
+        "com_acc_des = np.full((3, N_SIMULATION), np.nan)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Parametes of the CoM sinusoid\n", "\n",
+        "offset = robot.com(data)  # offset of the measured CoM\n",
+        "amp = np.array([0.0, 0.05, 0.0])  # amplitude function of 0.05 along "
+        "the y axis\n",
+        "two_pi_f = (\n", "    2 * np.pi * np.array([0.0, 0.5, 0.0])\n",
+        ")  # 2π function along the y axis with 0.5 amplitude\n",
+        "two_pi_f_amp = two_pi_f * amp  # 2π function times amplitude "
+        "function\n",
+        "two_pi_f_squared_amp = (\n", "    two_pi_f * two_pi_f_amp\n",
+        ")  # 2π function times squared amplitude function"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Simulation loop\n",
+        "# At each time step compute the next desired positions of the tasks\n",
+        "# Set them as new references for each tasks\n",
+        "\n",
+        "# The CoM trajectory is set with the sinusoid parameters:\n",
+        "# a sine for the position, a cosine (derivative of sine) for the "
+        "velocity\n",
+        "# and a -sine (derivative of cosine) for the acceleration\n",
+        "\n",
+        "# Compute the new problem data (HQP problem update)\n",
+        "# Solve the problem with the solver\n",
+        "\n",
+        "# Get the forces and the accelerations computed by the solver\n",
+        "# Update the plot variables of the CoM\n",
+        "# Print the forces applied at each feet\n",
+        "# Print the tracking error of the CoM task and the norm of the "
+        "velocity and acceleration needed to follow the\n",
+        "# reference trajectory\n",
+        "\n",
+        "# Integrate the acceleration computed by the QP\n",
+        "# One simple Euler integration from acceleration to velocity\n",
+        "# One integration (velocity to position) with pinocchio to have the "
+        "freeFlyer updated\n",
+        "# Display the result with gepetto viewer\n",
+        "\n",
+        "for i in range(N_SIMULATION):\n",
+        "    time_start = tmp.time()\n",
+        "\n",
+        "    sampleCom.pos(offset + amp * np.sin(two_pi_f * t))\n",
+        "    sampleCom.vel(two_pi_f_amp * np.cos(two_pi_f * t))\n",
+        "    sampleCom.acc(-two_pi_f_squared_amp * np.sin(two_pi_f * t))\n",
+        "    comTask.setReference(sampleCom)\n",
+        "\n",
+        "    sampleWaist = trajWaist.computeNext()\n",
+        "    waistTask.setReference(sampleWaist)\n",
+        "\n",
+        "    samplePosture = trajPosture.computeNext()\n",
+        "    postureTask.setReference(samplePosture)\n",
+        "\n",
+        "    HQPData = invdyn.computeProblemData(t, q, v)\n",
+        "    # if i == 0:\n",
+        "    #     HQPData.print_all()\n",
+        "\n",
+        "    sol = solver.solve(HQPData)\n",
+        "    if sol.status != 0:\n",
+        "        print(\"QP problem could not be solved! Error code:\", "
+        "sol.status)\n",
+        "        break\n",
+        "\n",
+        "    tau = invdyn.getActuatorForces(sol)\n",
+        "    dv = invdyn.getAccelerations(sol)\n",
+        "\n",
+        "    com_pos[:, i] = robot.com(invdyn.data())\n",
+        "    com_vel[:, i] = robot.com_vel(invdyn.data())\n",
+        "    com_acc[:, i] = comTask.getAcceleration(dv)\n",
+        "    com_pos_ref[:, i] = sampleCom.pos()\n",
+        "    com_vel_ref[:, i] = sampleCom.vel()\n",
+        "    com_acc_ref[:, i] = sampleCom.acc()\n",
+        "    com_acc_des[:, i] = comTask.getDesiredAcceleration\n",
+        "\n",
+        "    if i % PRINT_N == 0:\n",
+        "        print(f\"Time {t:.3f}\")\n",
+        "        if invdyn.checkContact(contactRF.name, sol):\n",
+        "            f = invdyn.getContactForce(contactRF.name, sol)\n",
+        "            print(\n",
+        "                \"\\tnormal force {}: {:.1f}\".format(\n",
+        "                    contactRF.name.ljust(20, \".\"), "
+        "contactRF.getNormalForce(f)\n",
+        "                )\n",
+        "            )\n",
+        "\n",
+        "        if invdyn.checkContact(contactLF.name, sol):\n",
+        "            f = invdyn.getContactForce(contactLF.name, sol)\n",
+        "            print(\n",
+        "                \"\\tnormal force {}: {:.1f}\".format(\n",
+        "                    contactLF.name.ljust(20, \".\"), "
+        "contactLF.getNormalForce(f)\n",
+        "                )\n",
+        "            )\n",
+        "\n",
+        "        print(\n",
+        "            \"\\ttracking err {}: {:.3f}\".format(\n",
+        "                comTask.name.ljust(20, \".\"), "
+        "norm(comTask.position_error, 2)\n",
+        "            )\n",
+        "        )\n",
+        "        print(f\"\\t||v||: {norm(v, 2):.3f}\\t ||dv||: "
+        "{norm(dv):.3f}\")\n",
+        "\n",
+        "    v_mean = v + 0.5 * dt * dv\n",
+        "    v += dt * dv\n",
+        "    q = pin.integrate(model, q, dt * v_mean)\n",
+        "    t += dt\n",
+        "\n",
+        "    if i % DISPLAY_N == 0:\n",
+        "        viz.display(q)\n",
+        "\n",
+        "    time_spent = tmp.time() - time_start\n",
+        "    if time_spent < dt:\n",
+        "        tmp.sleep(dt - time_spent)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# PLOT the result\n", "time = np.arange(0.0, N_SIMULATION * dt, dt)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Position tracking of the CoM along the x,y,z axis\n", "\n",
+        "(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
+        "for i in range(3):\n",
+        "    ax[i].plot(time, com_pos[i, :], label=\"CoM %i\" % i)\n",
+        "    ax[i].plot(time, com_pos_ref[i, :], \"r:\", label=\"CoM Ref %i\" "
+        "% i)\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"CoM [m]\")\n", "    leg = ax[i].legend()\n",
+        "    leg.get_frame().set_alpha(0.5)\n", "\n", "plt.show()"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "# Velocity tracking of the CoM along the x,y,z axis\n", "\n",
+        "(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
+        "for i in range(3):\n",
+        "    ax[i].plot(time, com_vel[i, :], label=\"CoM Vel %i\" % i)\n",
+        "    ax[i].plot(time, com_vel_ref[i, :], \"r:\", label=\"CoM Vel Ref "
+        "%i\" % i)\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"CoM Vel [m/s]\")\n",
+        "    leg = ax[i].legend()\n", "    leg.get_frame().set_alpha(0.5)\n",
+        "\n", "plt.show()"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {"scrolled" : false},
+      "outputs" : [],
+      "source" : [
+        "# Acceleration tracking of the CoM along the x,y,z axis\n", "\n",
+        "(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
+        "for i in range(3):\n",
+        "    ax[i].plot(time, com_acc[i, :], label=\"CoM Acc %i\" % i)\n",
+        "    ax[i].plot(time, com_acc_ref[i, :], \"r:\", label=\"CoM Acc Ref "
+        "%i\" % i)\n",
+        "    ax[i].plot(time, com_acc_des[i, :], \"g--\", label=\"CoM Acc Des "
+        "%i\" % i)\n",
+        "    ax[i].set_xlabel(\"Time [s]\")\n",
+        "    ax[i].set_ylabel(\"CoM Acc [m/s^2]\")\n",
+        "    leg = ax[i].legend()\n", "    leg.get_frame().set_alpha(0.5)\n",
+        "\n", "plt.show()"
+      ]
+    },
+    {
+      "cell_type" : "markdown",
+      "metadata" : {},
+      "source" :
+          ["As expected the robot's CoM follows the sinusoidal trajectory "
+           "along the y axis (the CoM task defines a sinusoidal movement only "
+           "on this axis). On the other axes there are small oscillations but "
+           "the robot remains stable.  "]
+    }
+  ],
+  "metadata" : {
+    "kernelspec" : {
+      "display_name" : "Python 3",
+      "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.5.2"
+    }
   },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "In this notebook is presented a simple use of the TSID framework on a humanoid robot, Talos.\n",
-    "\n",
-    "### Notations and Definitions\n",
-    "\n",
-    "The robot system has a state $x$ and we denote the control inputs as $u$. <br>\n",
-    "\n",
-    "The choice of the robot model depends both on the __robot__ and the **task** to perform. For legged robots the typical model of the actuators is a torque source because we will need to control the contact forces exchanged between robot and environment. Therefore the control input $u$ are motor torques $\\tau$. <br>\n",
-    "With the robot configuration $q$ and its velocity $\\dot{q}$ we have the robot state $x$:\n",
-    "\n",
-    "$$ x \\triangleq (q, \\dot{q}) $$"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "### Under-Actuated Systems\n",
-    "\n",
-    "A legged robot is __under actuated__, its number of actuators is less than its number of degrees of freedom (DoFs). <br>\n",
-    "We denotes $n_{va}$ the number of actuators and $n_v$ the number of DoFs:\n",
-    "\n",
-    "$$ n_{va} < n_v $$\n",
-    "\n",
-    "\n",
-    "Assume elements of $q$ are ordered, $q \\triangleq (q_u, q_a)$, where:\n",
-    "-  $q_u \\in \\mathbb{R}^{n_{qu}} $ are the passive (unactuated) joints\n",
-    "-  $q_a \\in \\mathbb{R}^{n_{qa}} $ are the actuated joints\n",
-    "\n",
-    "Similarly, $v \\triangleq (v_u, v_a)$, where $v_u \\in \\mathbb{R}^{n_{vu}}$ and $v_a \\in \\mathbb{R}^{n_{va}}$.\n",
-    "\n",
-    "$ S \\triangleq [\\ 0_{n_{va} \\ \\times \\ n_{vu}} \\ \\ I_{n_{va}}] $ is a selection matrix associated to the actuated joints:\n",
-    "$$ v_a = S \\ v $$\n",
-    "\n",
-    "\n",
-    "The dynamic of an under-actuated mechanical system is:\n",
-    "\n",
-    "$$ M(q) \\ \\dot{v} \\ + \\ h(q, v) \\ = \\ S^T \\tau \\ + \\ J(q)^T f $$\n",
-    "\n",
-    "where $M(q) \\in \\mathbb{R}^{n_v × n_v}$ is the mass matrix, $h(q,v) \\in \\mathbb{R}^{n_v}$ are the bias forces, $\\tau \\in \\mathbb{R}^{n_{va}}$ are the joint torques, $f \\in \\mathbb{R}^{n_f}$ are the contact forces, and $J(q) \\in \\mathbb{R}^{n_f×n_v}$ is the contact Jacobian.\n",
-    "\n",
-    "This dynamic is often decomposed into unactuated and actuated parts:\n",
-    "\n",
-    "\\begin{array}{r c r c l}\n",
-    "      M_u(q) \\ \\dot{v}  & + & h_u(q, v) & = & J_u(q)^T f \\\\\n",
-    "      M_a(q) \\ \\dot{v}  & + & h_a(q, v) & = & \\tau \\ + \\ J_a(q)^T f\n",
-    "\\end{array}\n",
-    "\n",
-    "Where\n",
-    "\n",
-    "\\begin{array}{r c l}\n",
-    "   M & = & \\begin{bmatrix}\n",
-    "   M_u \\\\\n",
-    "   M_a\n",
-    "\\end{bmatrix}  \\\\\n",
-    "   h & = & \\begin{bmatrix}\n",
-    "   h_u \\\\\n",
-    "   h_a\n",
-    "\\end{bmatrix}  \\\\\n",
-    "  J & = & [J_u \\ \\ J_a]\n",
-    "\\end{array}\n",
-    "   "
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "### Generic Output Function\n",
-    "\n",
-    "An output function $y$, can be an end-effector pose (such as a gripper), the robot center-of-mass, a visual feature position inside an image.\n",
-    "We assume that an output is a function from the robot configuration vector $q$ to a set called $Y$:\n",
-    "\n",
-    "$$y = f_{task}(q)$$\n",
-    "\n",
-    "It is also assumed that $f_{task}$ is $\\mathcal{C}^1$ then:\n",
-    "$$J = \\displaystyle \\frac{\\partial f_{task}}{\\partial q}$$\n",
-    "\n",
-    "When a desired output trajectory $y^*(t)$ is given, a task error is defined as:\n",
-    "\n",
-    "$$\n",
-    "  e: \\mathcal{C} \\times \\mathbf{R} \\rightarrow \\mathbf{R}^m \\qquad\n",
-    "  e = y \\ominus y^*\n",
-    "$$\n",
-    "\n",
-    "where $\\ominus$ is the difference operator in the output space (it is the standard difference if the output space is Euclidian).\n",
-    "\n",
-    "We can now define the desired dynamics of the task.\n",
-    "Usually, when the robot is controlled in velocity, the desired dynamics is set to:\n",
-    "\n",
-    "$$  \\dot{e}^* = - K e $$\n",
-    "\n",
-    "\n",
-    "When the robot is controlled at the acceleration level (which is our case because we assume a torque controlled robot), the desired task dynamics is typically specified with a PID control law:\n",
-    "\n",
-    "$$\n",
-    "  \\ddot{e}^* = K_P e + K_D \\dot{e} + K_I \\displaystyle \\int_{j=0}^{j=k} e(j) dj\n",
-    "$$\n",
-    "\n",
-    "with $K_P$ the error gain, $K_D$ the derivative of the error gain, and $K_I$ the integral gain.\n",
-    "\n",
-    "We can now derive the real error dynamics as:\n",
-    "\n",
-    "$$  \\dot{e} = \\dot{y} - \\dot{y}^*\n",
-    "     = J \\dot{q} - \\dot{y}^*$$\n",
-    "\n",
-    "therefore:\n",
-    "\n",
-    "$$\\ddot{e} = J \\ddot{q} + \\dot{J} \\dot{q} - \\ddot{y}^* $$\n"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "### QP Optimisation problem with acceleration and torque\n",
-    "\n",
-    "The  transcription  of  the  motion  reference  from  the  task space  to  the  whole-body  control  is  naturally  written  as  a quadratic  program  (QP).  A  QP  is  composed  of  two layers,  namely  the  constraints  and  the  cost.  It  can  be  seen  as a  hierarchy  of  two  levels,  the  constraint  having  priority  over the  cost. Inequalities can also be taken into account  directly  as  constraints,  or indirectly in  the  cost  function.\n",
-    "\n",
-    "Let us simplify the equations of motion based on the rigid body dynamics assuming there is no contact:\n",
-    "\n",
-    "$$M \\dot{v} + h = S^T \\tau $$\n",
-    "\n",
-    "Let us assume we have a task error $e$ regulating an output function $y$.\n",
-    "\n",
-    "We can introduce a slack  variable $s$ (an implicit optimization variable) to  represent the difference between desired and real error dynamics:\n",
-    "\n",
-    "$$\\ddot{e} - \\ddot{e}^*  \\triangleq  s $$\n",
-    "\n",
-    "A simple formulation of the QP problem can then be expressed as:\n",
-    "\n",
-    "$$   \\underset{\\dot{v},\\tau}{\\min} \\quad  \\| s \\|^2 \\\\\n",
-    "\t\\textrm{s.t.} \\quad  M \\dot{v} + h = S^T \\tau $$\n",
-    "\n",
-    "If the system is in contact with environment, its dynamic must account for contact forces $f_{ext}$. If contacts are soft, measured/estimated contact forces $\\hat{f}_{ext}$ can be easily included:\n",
-    "\n",
-    "$$\t\\underset{\\dot{v},\\tau}{\\min} \\quad \\| s \\|^2 \\\\\n",
-    "\t\\textrm{s.t.} \\quad M \\dot{v} + h = S^T \\tau + J_c^T \\hat{f}_{ext} $$\n",
-    "\n",
-    "\n",
-    "But if contacts are rigid, they constrain the motion. They are usually modelled as nonlinear functions, which are differentiated twice:\n",
-    "\n",
-    "$$\tc(q)  =  0  \\, \\Leftrightarrow  \\text{Contact point do not move}$$\n",
-    "\n",
-    "$$\tJ_c \\, v  = 0  \\, \\Leftrightarrow  \\text{Contact point velocities are null}$$\n",
-    "\n",
-    "$$\tJ_c \\, \\dot{v} + \\dot{J_c} \\, v  =  0  \\, \\Leftrightarrow  \\text{Contact point accelerations are null}$$\n",
-    "\n",
-    "\n",
-    "This leads to the following optimization problem:\n",
-    "\n",
-    "$$\t\\underset{\\dot{v},\\tau}{\\min} \\quad  \\| s \\|^2 \\\\\n",
-    "\t\\textrm{s.t.} \\quad \\Bigg[\n",
-    "\t\\begin{array}{lll}\n",
-    "\t\tJ_c & 0 & 0 \\\\\n",
-    "\t\tM  & -J_c^T & -S^T\n",
-    "\t\\end{array}\n",
-    "\t\\Bigg] \\, \\Bigg[\\begin{array}{l}\n",
-    "                    \\dot{v} \\\\\n",
-    "                    f \\\\\n",
-    "                    \\tau\n",
-    "                    \\end{array}\n",
-    "               \\Bigg] \\, = \\, \\Bigg[\\begin{array}{l}\n",
-    "                                    - \\dot{J_c} \\, v \\\\\n",
-    "                                    -h\n",
-    "                                    \\end{array}\n",
-    "                              \\Bigg]$$\n",
-    "\n",
-    "The main benefit of QP solvers is that they can handle inequality constraints. Inequalities are mainly used to defined boundaries of the system, such as joint torque, velocity or position limits, or friction cones for the contact forces."
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "### Weighted Sum\n",
-    "\n",
-    "Complex robots are typically redundant with respect to the main task they must perform, this redundancy can be used to execute secondary tasks. This multi-objective optimization can be achieved by setting respective weights between the tasks (strategy used in TSID), or by imposing a strict hierarchy between them (cascade of QP or HQP).\n",
-    "\n",
-    "Assume robot must perform N tasks, each defined by a task function $g_i(\\cdot)$:\n",
-    "\n",
-    "$$\tg_i = \\| A_i \\Bigg[ \\begin{array}{l} \\dot{v} \\\\ f \\\\ \\tau \\end{array}\\Bigg] - a_i \\|^2 $$\n",
-    "\n",
-    "The easiest strategy is to sum all functions using user-defined weights $w_i$:\n",
-    "\n",
-    "$$\t\\underset{\\dot{v},f,\\tau}{\\min} \\quad \\displaystyle \\sum_{i=0}^N{w_i \\, g_i} \\\\\n",
-    "\t\\textrm{s.t.} \\quad \\Bigg[\n",
-    "\t\\begin{array}{lll}\n",
-    "\t\tJ_c & 0 & 0 \\\\\n",
-    "\t\tM  & -J_c^T & -S^T\n",
-    "\t\\end{array}\n",
-    "\t\\Bigg] \\, \\Bigg[\\begin{array}{l}\n",
-    "                    \\dot{v} \\\\\n",
-    "                    f \\\\\n",
-    "                    \\tau\n",
-    "                    \\end{array}\n",
-    "               \\Bigg] \\, = \\, \\Bigg[\\begin{array}{l}\n",
-    "                                    - \\dot{J_c} \\, v \\\\\n",
-    "                                    -h\n",
-    "                                    \\end{array}\n",
-    "                              \\Bigg]$$\n",
-    "\n",
-    "This problem remains standard computationally-efficient (compared to Least Square Programming). However, finding proper weights can be hard because too extreme weights lead to numerical issues. "
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "# An example: CoM Sinusoidal trajectory on the robot TALOS\n",
-    "\n",
-    "The goal of this exercise is to create a set of tasks allowing the tracking of a sinusoidal trajectory with the CoM of the biped robot Talos.\n",
-    "\n",
-    "In the following we describe the process to create, initialise and solve the QP problem defined by the tasks."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Python import needed by the exercise\n",
-    "import matplotlib.pyplot as plt\n",
-    "import numpy as np\n",
-    "from numpy.linalg import norm\n",
-    "from pathlib import Path\n",
-    "import time as tmp\n",
-    "\n",
-    "# import the library TSID for the Whole-Body Controller\n",
-    "import tsid\n",
-    "\n",
-    "# import the pinocchio library for the mathematical methods (Lie algebra) and multi-body dynamics computations.\n",
-    "import pinocchio as pin\n",
-    "\n",
-    "# import example_example_robot_data to get the current path for the robot models\n",
-    "from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
-    "\n",
-    "import sys\n",
-    "\n",
-    "sys.path.append(\"..\")\n",
-    "\n",
-    "# import graphical tools\n",
-    "import plot_utils as plut"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Definition of the tasks gains, weights and the foot geometry (for contact task)\n",
-    "\n",
-    "lxp = 0.1  # foot length in positive x direction\n",
-    "lxn = 0.11  # foot length in negative x direction\n",
-    "lyp = 0.069  # foot length in positive y direction\n",
-    "lyn = 0.069  # foot length in negative y direction\n",
-    "lz = 0.107  # foot sole height with respect to ankle joint\n",
-    "mu = 0.3  # friction coefficient\n",
-    "fMin = 1.0  # minimum normal force\n",
-    "fMax = 1000.0  # maximum normal force\n",
-    "\n",
-    "rf_frame_name = \"leg_right_6_joint\"  # right foot joint name\n",
-    "lf_frame_name = \"leg_left_6_joint\"  # left foot joint name\n",
-    "contactNormal = np.array(\n",
-    "    [0.0, 0.0, 1.0]\n",
-    ")  # direction of the normal to the contact surface\n",
-    "\n",
-    "w_com = 1.0  # weight of center of mass task\n",
-    "w_posture = 0.1  # weight of joint posture task\n",
-    "w_forceRef = 1e-3  # weight of force regularization task\n",
-    "w_waist = 1.0  # weight of waist task\n",
-    "\n",
-    "kp_contact = 30.0  # proportional gain of contact constraint\n",
-    "kp_com = 20.0  # proportional gain of center of mass task\n",
-    "kp_waist = 500.0  # proportional gain of waist task\n",
-    "\n",
-    "kp_posture = np.array(  # proportional gain of joint posture task\n",
-    "    [\n",
-    "        10.0,\n",
-    "        5.0,\n",
-    "        5.0,\n",
-    "        1.0,\n",
-    "        1.0,\n",
-    "        10.0,  # lleg, low gain on axis along y and knee\n",
-    "        10.0,\n",
-    "        5.0,\n",
-    "        5.0,\n",
-    "        1.0,\n",
-    "        1.0,\n",
-    "        10.0,  # rleg\n",
-    "        500.0,\n",
-    "        500.0,  # chest\n",
-    "        50.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,  # larm\n",
-    "        50.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,\n",
-    "        10.0,  # rarm\n",
-    "        100.0,\n",
-    "        100.0,\n",
-    "    ]  # head\n",
-    ")\n",
-    "\n",
-    "dt = 0.001  # controller time step\n",
-    "PRINT_N = 500  # print every PRINT_N time steps\n",
-    "DISPLAY_N = 20  # update robot configuration in viwewer every DISPLAY_N time steps\n",
-    "N_SIMULATION = 10000  # number of time steps simulated"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Set the path where the urdf file of the robot is registered\n",
-    "path = EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
-    "urdf = path + \"/talos_data/robots/talos_reduced.urdf\"\n",
-    "# Create the robot wrapper from the urdf, it will give the model of the robot and its data\n",
-    "robot = tsid.RobotWrapper(urdf, [path], pin.JointModelFreeFlyer(), False)\n",
-    "srdf = path + \"/talos_data/srdf/talos.srdf\""
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Creation of the robot wrapper for gepetto viewer (graphical interface)\n",
-    "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
-    "    urdf, [str(Path(path) / \"../..\")], pin.JointModelFreeFlyer()\n",
-    ")\n",
-    "# Viewer = pin.visualize.GepettoVisualizer\n",
-    "Viewer = pin.visualize.MeshcatVisualizer\n",
-    "viz = Viewer(\n",
-    "    robot_display.model, robot_display.collision_model, robot_display.visual_model\n",
-    ")\n",
-    "viz.initViewer(loadModel=True)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Take the model of the robot and load its reference configurations\n",
-    "model = robot.model()\n",
-    "pin.loadReferenceConfigurations(model, srdf, False)\n",
-    "# Set the current configuration q to the robot configuration half_sitting\n",
-    "q = model.referenceConfigurations[\"half_sitting\"]\n",
-    "# Set the current velocity to zero\n",
-    "v = np.zeros(robot.nv)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Display the robot in Gepetto Viewer in the configuration q = halfSitting\n",
-    "viz.display(q)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Check that the frames of the feet exist.\n",
-    "assert model.existFrame(rf_frame_name)\n",
-    "assert model.existFrame(lf_frame_name)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "t = 0.0  # time\n",
-    "\n",
-    "# InverseDynamicsFormulationAccForce is the class in charge of\n",
-    "# creating the inverse dynamics HQP problem using\n",
-    "# the robot accelerations (base + joints) and the contact forces as decision variables\n",
-    "invdyn = tsid.InverseDynamicsFormulationAccForce(\"tsid\", robot, False)\n",
-    "invdyn.computeProblemData(t, q, v)\n",
-    "# Get the data -> initial data\n",
-    "data = invdyn.data()"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "## Tasks Definitions\n",
-    "\n",
-    "A **Task** is a control objective for the robot, which is used at each control cycle to generate a **Constraint**.\n",
-    "Note that constraints are purely mathematical objects that are independent of the concept of robot, while Tasks are instead robot-related objects.\n",
-    "\n",
-    "A **Constraint** is a linear equality or inequality.\n",
-    "In TSID the HQP is defined as a collection of constraints with different priority levels and weights.\n",
-    "There are three kinds of constraints defined in TSID:\n",
-    "\n",
-    "- Equalities, represented by matrix $A$ and vector $a$:\n",
-    "  $$ Ax = a $$\n",
-    "\n",
-    "- Inequalities, represented by matrix $A$ and vectors $lb$ and $ub$:\n",
-    "  $$ lb ≤ Ax ≤ ub $$\n",
-    "\n",
-    "- Bounds, represented by vectors $lb$ and $ub$:\n",
-    "  $$ lb ≤ x ≤ ub $$\n",
-    "\n",
-    "There are three kinds of Task in TSID:\n",
-    "\n",
-    "- **TaskMotion**: computes a constraint that is a linear function of the robot accelerations\n",
-    "- **TaskContactForce**: computes a constraint that is a linear function of the contact forces\n",
-    "- **TaskActuation**: computes a constraint that is a linear function of the joint torques\n",
-    "\n",
-    "Tasks can compute either equality constraints, or bounds, or inequality constraints.\n",
-    "\n",
-    "Examples of **TaskMotion**:\n",
-    "\n",
-    "- **TaskComEquality**: computes an equality constraint to specify a desired acceleration of the center of mass (CoM) of the robot.\n",
-    "- **TaskJointPosture**: computes an equality constraint to specify the desired joint accelerations.\n",
-    "- **TaskSE3Equality**: computes an equality constraint to specify the desired acceleration for a frame attached to one of the links of the robot.\n",
-    "- **TaskJointBounds**: computes a bounds constraint to specify the joint acceleration bounds in order to satisfy the joint position/velocity/acceleration limits."
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "In this exercise, for the sinusoidal movement of the CoM, we need 3 tasks of class TaskMotion:\n",
-    "\n",
-    " - **TaskComEquality** as **constraint of the control** (priority level = 0) to make the CoM follow a sinusoidal trajectory. It is the most important task so it is introduced as a constraint.\n",
-    " - **TaskSE3Equality** in the **cost function** (priority level = 1) for the waist of the robot, to maintain its orientation (with a reference trajectory). It is an important task so it has a weight of 1.\n",
-    " - **TaskJointPosture** in the **cost function** (priority level = 1) for the posture of the robot, to maintain it to half-sitting as much as possible (with a reference trajectory). It is the least important task so it has a weight of 0.1."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# COM Task\n",
-    "comTask = tsid.TaskComEquality(\"task-com\", robot)\n",
-    "comTask.setKp(kp_com * np.ones(3))  # Proportional gain defined before = 20\n",
-    "comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3))  # Derivative gain = 2 * sqrt(20)\n",
-    "# Add the task to the HQP with weight = 1.0, priority level = 0 (as constraint) and a transition duration = 0.0\n",
-    "invdyn.addMotionTask(comTask, w_com, 0, 0.0)\n",
-    "\n",
-    "# WAIST Task\n",
-    "waistTask = tsid.TaskSE3Equality(\n",
-    "    \"task-waist\", robot, \"root_joint\"\n",
-    ")  # waist -> root_joint\n",
-    "waistTask.setKp(kp_waist * np.ones(6))  # Proportional gain defined before = 500\n",
-    "waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6))  # Derivative gain = 2 * sqrt(500)\n",
-    "\n",
-    "# Add a Mask to the task which will select the vector dimensions on which the task will act.\n",
-    "# In this case the waist configuration is a vector 6d (position and orientation -> SE3)\n",
-    "# Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot\n",
-    "mask = np.ones(6)\n",
-    "mask[:3] = 0.0\n",
-    "waistTask.setMask(mask)\n",
-    "# Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0\n",
-    "invdyn.addMotionTask(waistTask, w_waist, 1, 0.0)\n",
-    "\n",
-    "\n",
-    "# POSTURE Task\n",
-    "postureTask = tsid.TaskJointPosture(\"task-posture\", robot)\n",
-    "postureTask.setKp(\n",
-    "    kp_posture\n",
-    ")  # Proportional gain defined before (different for each joints)\n",
-    "postureTask.setKd(2.0 * kp_posture)  # Derivative gain = 2 * kp\n",
-    "# Add the task with weight = 0.1, priority level = 1 (in cost function) and a transition duration = 0.0\n",
-    "invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "## Rigid Contacts\n",
-    "\n",
-    "A **Rigid Contact** is a description of a rigid contact between a body of the robot and the environment.\n",
-    "The main difference between a task and a rigid contact is that a rigid contact is associated to reaction forces, while a task is not.\n",
-    "\n",
-    "This class allows to use different representations for the motion space and the force space (more details on this below), and it is characterize by the following four elements.\n",
-    "\n",
-    "- Motion Task\n",
-    "    - Represents the motion constraint (equality) associated to the rigid contact\n",
-    "    - $ J_c \\, \\dot{v} \\, = \\, - \\dot{J_c} \\, v \\, - \\, K_p \\ e - \\, K_d \\, \\dot{e}$\n",
-    "\n",
-    "\n",
-    "- Force Task\n",
-    "    - Computes the inequality constraints acting on the contact forces\n",
-    "    - e.g., friction cone constraints : $A \\, f \\,  ≤ \\, a$\n",
-    "\n",
-    "\n",
-    "- Force Regularization Task\n",
-    "    - Computes a regularization cost on the contact forces\n",
-    "    - e.g., keep them close to friction cone center\n",
-    "\n",
-    "\n",
-    "- Force-Generator matrix $T$\n",
-    "    - Maps force variables to motion constraint representation\n",
-    "    - Dynamic: $ M \\ \\dot{v} \\ + \\ h \\ = \\ S^T \\tau \\ + \\ J_c^T T f $\n",
-    "    - Motion constraint : $ J_c \\, \\dot{v} \\, = \\, - \\dot{J_c} \\, v$\n",
-    "    - Friction cones: $A \\, f \\,  ≤ \\, a$"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "### Contact 6d\n",
-    "\n",
-    "In case of a unilateral plane contact (with polygonal shape), the motion constraint is 6d, because the body in contact cannot move in any direction.\n",
-    "##### PROBLEM\n",
-    "\n",
-    "Minimal force representation would be 6d (3d force + 3d moment):\n",
-    "- It is hard to write friction constraints with 6d force representation (especially for non-rectangular contact shapes)\n",
-    "- A solution would be to represent the reaction force as collection of 3d forces applied at vertices of contact surface (writting friction constraints is then easy)\n",
-    "    - But it leads to redundant representation, e.g., 4-vertex surface → 12 variables\n",
-    "    - The redundancy is an issue for motion constraints if the HQP solver does not handle redundant constraints (as eiQuadProg).\n",
-    "\n",
-    "##### SOLUTION\n",
-    "\n",
-    "- Use 6d representation for motion constraint $ J_c \\, \\dot{v} \\, = \\, - \\dot{J_c} \\, v \\, \\in \\mathbf{R}^6$\n",
-    "- But use 12d representation for force variable $f \\in \\mathbf{R}^{12}$\n",
-    "- A force-generator matrix $T \\in \\mathbf{R}^{6 \\times 12}$ defines the mapping between the two representations:\n",
-    "    $\\tau_{c} \\, = J_c^T \\, T \\, f$"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "In this exercise, we need two Rigid Contacts, one for each foot as **Contact6d**.\n",
-    "\n",
-    "The Rigid Contacts are always defined as **constraints** (priority level = 0) to maintain the robot in contact."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# CONTACTS 6D\n",
-    "# Definition of the foot geometry with respect to the ankle joints (which are the ones controlled)\n",
-    "contact_Point = np.ones((3, 4)) * lz\n",
-    "contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]\n",
-    "contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]\n",
-    "\n",
-    "# The feet are the only bodies in contact in this experiment and their geometry defines the plane of contact\n",
-    "# between the robot and the environement -> it is a Contact6D\n",
-    "\n",
-    "# To define a contact6D :\n",
-    "# We need the surface of contact (contact_point), the normal vector of contact (contactNormal along the z-axis)\n",
-    "# the friction parameter with the ground (mu = 0.3), the normal force bounds (fMin =1.0, fMax=1000.0)\n",
-    "\n",
-    "# Right Foot\n",
-    "contactRF = tsid.Contact6d(\n",
-    "    \"contact_rfoot\", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax\n",
-    ")\n",
-    "contactRF.setKp(kp_contact * np.ones(6))  # Proportional gain defined before = 30\n",
-    "contactRF.setKd(\n",
-    "    2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
-    ")  # Derivative gain = 2 * sqrt(30)\n",
-    "# Reference position of the right ankle -> initial position\n",
-    "H_rf_ref = robot.position(data, model.getJointId(rf_frame_name))\n",
-    "contactRF.setReference(H_rf_ref)\n",
-    "# Add the contact to the HQP with weight = 0.1 for the force regularization task,\n",
-    "# and priority level = 0 (as real constraint) for the motion constraint\n",
-    "invdyn.addRigidContact(contactRF, w_forceRef)\n",
-    "\n",
-    "# Left Foot\n",
-    "contactLF = tsid.Contact6d(\n",
-    "    \"contact_lfoot\", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax\n",
-    ")\n",
-    "contactLF.setKp(kp_contact * np.ones(6))  # Proportional gain defined before = 30\n",
-    "contactLF.setKd(\n",
-    "    2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
-    ")  # Derivative gain = 2 * sqrt(30)\n",
-    "# Reference position of the left ankle -> initial position\n",
-    "H_lf_ref = robot.position(data, model.getJointId(lf_frame_name))\n",
-    "contactLF.setReference(H_lf_ref)\n",
-    "# Add the contact to the HQP with weight = 0.1 for the force regularization task,\n",
-    "# and priority level = 0 (as real constraint) for the motion constraint\n",
-    "invdyn.addRigidContact(contactLF, w_forceRef)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "## TSID Trajectory\n",
-    "\n",
-    "A **Trajectory** is a multi-dimensional function of time describing the motion of an object and its time derivatives.\n",
-    "For standard use in control, the method *compute_next* is provided, which computes the value of the trajectory at the next time step.\n",
-    "\n",
-    "In the example, we need to set 3 trajectories, one for each task.\n",
-    "These trajectories will give at each time step the desired position, velocity and acceleration of the different tasks (CoM, posture and waist).\n",
-    "In our case, the posture and the waist will be constant, equal to their initial values.\n",
-    "For the CoM however, we will update the trajectory with a sinusoidal signal at each time step."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Set the reference trajectory of the tasks\n",
-    "\n",
-    "com_ref = data.com[0]  # Initial value of the CoM\n",
-    "trajCom = tsid.TrajectoryEuclidianConstant(\"traj_com\", com_ref)\n",
-    "sampleCom = (\n",
-    "    trajCom.computeNext()\n",
-    ")  # Compute the first step of the trajectory from the initial value\n",
-    "\n",
-    "q_ref = q[\n",
-    "    7:\n",
-    "]  # Initial value of the joints of the robot (in halfSitting position without the freeFlyer (6 first values))\n",
-    "trajPosture = tsid.TrajectoryEuclidianConstant(\"traj_joint\", q_ref)\n",
-    "\n",
-    "waist_ref = robot.position(\n",
-    "    data, model.getJointId(\"root_joint\")\n",
-    ")  # Initial value of the waist (root_joint)\n",
-    "# Here the waist is defined as a 6d vector (position + orientation) so it is in the SE3 group (Lie group)\n",
-    "# Thus, the trajectory is not Euclidian but remains in the SE3 domain -> TrajectorySE3Constant\n",
-    "trajWaist = tsid.TrajectorySE3Constant(\"traj_waist\", waist_ref)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Initialisation of the Solver\n",
-    "# Use EiquadprogFast: dynamic matrix sizes (memory allocation performed only when resizing)\n",
-    "solver = tsid.SolverHQuadProgFast(\"qp solver\")\n",
-    "# Resize the solver to fit the number of variables, equality and inequality constraints\n",
-    "solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Initialisation of the plot variables which will be updated during the simulation loop\n",
-    "# These variables describe the behavior of the CoM of the robot (reference and real position, velocity and acceleration)\n",
-    "com_pos = np.full((3, N_SIMULATION), np.nan)\n",
-    "com_vel = np.full((3, N_SIMULATION), np.nan)\n",
-    "com_acc = np.full((3, N_SIMULATION), np.nan)\n",
-    "\n",
-    "com_pos_ref = np.full((3, N_SIMULATION), np.nan)\n",
-    "com_vel_ref = np.full((3, N_SIMULATION), np.nan)\n",
-    "com_acc_ref = np.full((3, N_SIMULATION), np.nan)\n",
-    "com_acc_des = np.full((3, N_SIMULATION), np.nan)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Parametes of the CoM sinusoid\n",
-    "\n",
-    "offset = robot.com(data)  # offset of the measured CoM\n",
-    "amp = np.array([0.0, 0.05, 0.0])  # amplitude function of 0.05 along the y axis\n",
-    "two_pi_f = (\n",
-    "    2 * np.pi * np.array([0.0, 0.5, 0.0])\n",
-    ")  # 2π function along the y axis with 0.5 amplitude\n",
-    "two_pi_f_amp = two_pi_f * amp  # 2π function times amplitude function\n",
-    "two_pi_f_squared_amp = (\n",
-    "    two_pi_f * two_pi_f_amp\n",
-    ")  # 2π function times squared amplitude function"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Simulation loop\n",
-    "# At each time step compute the next desired positions of the tasks\n",
-    "# Set them as new references for each tasks\n",
-    "\n",
-    "# The CoM trajectory is set with the sinusoid parameters:\n",
-    "# a sine for the position, a cosine (derivative of sine) for the velocity\n",
-    "# and a -sine (derivative of cosine) for the acceleration\n",
-    "\n",
-    "# Compute the new problem data (HQP problem update)\n",
-    "# Solve the problem with the solver\n",
-    "\n",
-    "# Get the forces and the accelerations computed by the solver\n",
-    "# Update the plot variables of the CoM\n",
-    "# Print the forces applied at each feet\n",
-    "# Print the tracking error of the CoM task and the norm of the velocity and acceleration needed to follow the\n",
-    "# reference trajectory\n",
-    "\n",
-    "# Integrate the acceleration computed by the QP\n",
-    "# One simple Euler integration from acceleration to velocity\n",
-    "# One integration (velocity to position) with pinocchio to have the freeFlyer updated\n",
-    "# Display the result with gepetto viewer\n",
-    "\n",
-    "for i in range(N_SIMULATION):\n",
-    "    time_start = tmp.time()\n",
-    "\n",
-    "    sampleCom.pos(offset + amp * np.sin(two_pi_f * t))\n",
-    "    sampleCom.vel(two_pi_f_amp * np.cos(two_pi_f * t))\n",
-    "    sampleCom.acc(-two_pi_f_squared_amp * np.sin(two_pi_f * t))\n",
-    "    comTask.setReference(sampleCom)\n",
-    "\n",
-    "    sampleWaist = trajWaist.computeNext()\n",
-    "    waistTask.setReference(sampleWaist)\n",
-    "\n",
-    "    samplePosture = trajPosture.computeNext()\n",
-    "    postureTask.setReference(samplePosture)\n",
-    "\n",
-    "    HQPData = invdyn.computeProblemData(t, q, v)\n",
-    "    # if i == 0:\n",
-    "    #     HQPData.print_all()\n",
-    "\n",
-    "    sol = solver.solve(HQPData)\n",
-    "    if sol.status != 0:\n",
-    "        print(\"QP problem could not be solved! Error code:\", sol.status)\n",
-    "        break\n",
-    "\n",
-    "    tau = invdyn.getActuatorForces(sol)\n",
-    "    dv = invdyn.getAccelerations(sol)\n",
-    "\n",
-    "    com_pos[:, i] = robot.com(invdyn.data())\n",
-    "    com_vel[:, i] = robot.com_vel(invdyn.data())\n",
-    "    com_acc[:, i] = comTask.getAcceleration(dv)\n",
-    "    com_pos_ref[:, i] = sampleCom.pos()\n",
-    "    com_vel_ref[:, i] = sampleCom.vel()\n",
-    "    com_acc_ref[:, i] = sampleCom.acc()\n",
-    "    com_acc_des[:, i] = comTask.getDesiredAcceleration\n",
-    "\n",
-    "    if i % PRINT_N == 0:\n",
-    "        print(f\"Time {t:.3f}\")\n",
-    "        if invdyn.checkContact(contactRF.name, sol):\n",
-    "            f = invdyn.getContactForce(contactRF.name, sol)\n",
-    "            print(\n",
-    "                \"\\tnormal force {}: {:.1f}\".format(\n",
-    "                    contactRF.name.ljust(20, \".\"), contactRF.getNormalForce(f)\n",
-    "                )\n",
-    "            )\n",
-    "\n",
-    "        if invdyn.checkContact(contactLF.name, sol):\n",
-    "            f = invdyn.getContactForce(contactLF.name, sol)\n",
-    "            print(\n",
-    "                \"\\tnormal force {}: {:.1f}\".format(\n",
-    "                    contactLF.name.ljust(20, \".\"), contactLF.getNormalForce(f)\n",
-    "                )\n",
-    "            )\n",
-    "\n",
-    "        print(\n",
-    "            \"\\ttracking err {}: {:.3f}\".format(\n",
-    "                comTask.name.ljust(20, \".\"), norm(comTask.position_error, 2)\n",
-    "            )\n",
-    "        )\n",
-    "        print(f\"\\t||v||: {norm(v, 2):.3f}\\t ||dv||: {norm(dv):.3f}\")\n",
-    "\n",
-    "    v_mean = v + 0.5 * dt * dv\n",
-    "    v += dt * dv\n",
-    "    q = pin.integrate(model, q, dt * v_mean)\n",
-    "    t += dt\n",
-    "\n",
-    "    if i % DISPLAY_N == 0:\n",
-    "        viz.display(q)\n",
-    "\n",
-    "    time_spent = tmp.time() - time_start\n",
-    "    if time_spent < dt:\n",
-    "        tmp.sleep(dt - time_spent)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# PLOT the result\n",
-    "time = np.arange(0.0, N_SIMULATION * dt, dt)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Position tracking of the CoM along the x,y,z axis\n",
-    "\n",
-    "(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
-    "for i in range(3):\n",
-    "    ax[i].plot(time, com_pos[i, :], label=\"CoM %i\" % i)\n",
-    "    ax[i].plot(time, com_pos_ref[i, :], \"r:\", label=\"CoM Ref %i\" % i)\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"CoM [m]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "\n",
-    "plt.show()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Velocity tracking of the CoM along the x,y,z axis\n",
-    "\n",
-    "(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
-    "for i in range(3):\n",
-    "    ax[i].plot(time, com_vel[i, :], label=\"CoM Vel %i\" % i)\n",
-    "    ax[i].plot(time, com_vel_ref[i, :], \"r:\", label=\"CoM Vel Ref %i\" % i)\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"CoM Vel [m/s]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "\n",
-    "plt.show()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {
-    "scrolled": false
-   },
-   "outputs": [],
-   "source": [
-    "# Acceleration tracking of the CoM along the x,y,z axis\n",
-    "\n",
-    "(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
-    "for i in range(3):\n",
-    "    ax[i].plot(time, com_acc[i, :], label=\"CoM Acc %i\" % i)\n",
-    "    ax[i].plot(time, com_acc_ref[i, :], \"r:\", label=\"CoM Acc Ref %i\" % i)\n",
-    "    ax[i].plot(time, com_acc_des[i, :], \"g--\", label=\"CoM Acc Des %i\" % i)\n",
-    "    ax[i].set_xlabel(\"Time [s]\")\n",
-    "    ax[i].set_ylabel(\"CoM Acc [m/s^2]\")\n",
-    "    leg = ax[i].legend()\n",
-    "    leg.get_frame().set_alpha(0.5)\n",
-    "\n",
-    "plt.show()"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "As expected the robot's CoM follows the sinusoidal trajectory along the y axis (the CoM task defines a sinusoidal movement only on this axis). On the other axes there are small oscillations but the robot remains stable.  "
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 3",
-   "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.5.2"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 2
+  "nbformat" : 4,
+  "nbformat_minor" : 2
 }
diff --git a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
index 882a2415..3ad3180a 100644
--- a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
+++ b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
@@ -1,250 +1,265 @@
 {
-  "cells": [
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import threading\n",
-    "import time\n",
-    "import sys\n",
-    "\n",
-    "from ipywidgets import interact\n",
-    "import ipywidgets as widgets\n",
-    "import numpy as np\n",
-    "import pinocchio as pin\n",
-    "\n",
-    "sys.path.append(\"..\")\n",
-    "import talos_conf as conf\n",
-    "import vizutils\n",
-    "from tsid_biped import TsidBiped"
-   ]
+  "cells" : [
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "import threading\n", "import time\n", "import sys\n", "\n",
+        "from ipywidgets import interact\n", "import ipywidgets as widgets\n",
+        "import numpy as np\n", "import pinocchio as pin\n", "\n",
+        "sys.path.append(\"..\")\n", "import talos_conf as conf\n",
+        "import vizutils\n", "from tsid_biped import TsidBiped"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "tsid = TsidBiped(conf)\n",
+        "tsid.q0[2] = 1.02127\n",
+        "\n",
+        "com_0 = tsid.robot.com(tsid.formulation.data())\n",
+        "H_rf_0 = tsid.robot.framePosition(\n",
+        "    tsid.formulation.data(), "
+        "tsid.model.getFrameId(conf.rf_frame_name)\n",
+        ")\n",
+        "H_lf_0 = tsid.robot.framePosition(\n",
+        "    tsid.formulation.data(), "
+        "tsid.model.getFrameId(conf.lf_frame_name)\n",
+        ")\n",
+        "\n",
+        "vizutils.addViewerSphere(\n",
+        "    tsid.viz, \"world/com\", conf.SPHERE_RADIUS, "
+        "conf.COM_SPHERE_COLOR\n",
+        ")\n",
+        "vizutils.addViewerSphere(\n",
+        "    tsid.viz, \"world/com_ref\", conf.REF_SPHERE_RADIUS, "
+        "conf.COM_REF_SPHERE_COLOR\n",
+        ")\n",
+        "vizutils.addViewerSphere(tsid.viz, \"world/rf\", conf.SPHERE_RADIUS, "
+        "conf.RF_SPHERE_COLOR)\n",
+        "vizutils.addViewerSphere(\n",
+        "    tsid.viz, \"world/rf_ref\", conf.REF_SPHERE_RADIUS, "
+        "conf.RF_REF_SPHERE_COLOR\n",
+        ")\n",
+        "vizutils.addViewerSphere(tsid.viz, \"world/lf\", conf.SPHERE_RADIUS, "
+        "conf.LF_SPHERE_COLOR)\n",
+        "vizutils.addViewerSphere(\n",
+        "    tsid.viz, \"world/lf_ref\", conf.REF_SPHERE_RADIUS, "
+        "conf.LF_REF_SPHERE_COLOR\n",
+        ")"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "push_robot_active, push_robot_com_vel, com_vel_entry = False, 3 * "
+        "[0.0], None\n",
+        "run, push_robot_active = threading.Event(), threading.Event()\n",
+        "run.set()"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "def run_simu():\n",
+        "    i, t = 0, 0.0\n",
+        "    q, v = tsid.q, tsid.v\n",
+        "    time_avg = 0.0\n",
+        "    while run.is_set():\n",
+        "        time_start = time.time()\n",
+        "\n",
+        "        tsid.comTask.setReference(tsid.trajCom.computeNext())\n",
+        "        "
+        "tsid.postureTask.setReference(tsid.trajPosture.computeNext())\n",
+        "        tsid.rightFootTask.setReference(tsid.trajRF.computeNext())\n",
+        "        tsid.leftFootTask.setReference(tsid.trajLF.computeNext())\n",
+        "\n",
+        "        HQPData = tsid.formulation.computeProblemData(t, q, v)\n",
+        "\n",
+        "        sol = tsid.solver.solve(HQPData)\n",
+        "        if sol.status != 0:\n",
+        "            print(\"QP problem could not be solved! Error code:\", "
+        "sol.status)\n",
+        "            break\n",
+        "\n",
+        "        # tau = tsid.formulation.getActuatorForces(sol)\n",
+        "        dv = tsid.formulation.getAccelerations(sol)\n",
+        "        q, v = tsid.integrate_dv(q, v, dv, conf.dt)\n",
+        "        i, t = i + 1, t + conf.dt\n",
+        "\n",
+        "        if push_robot_active.is_set():\n",
+        "            push_robot_active.clear()\n",
+        "            data = tsid.formulation.data()\n",
+        "            if tsid.contact_LF_active:\n",
+        "                J_LF = tsid.contactLF.computeMotionTask(0.0, q, v, "
+        "data).matrix\n",
+        "            else:\n",
+        "                J_LF = np.zeros((0, tsid.model.nv))\n",
+        "            if tsid.contact_RF_active:\n",
+        "                J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, "
+        "data).matrix\n",
+        "            else:\n",
+        "                J_RF = np.zeros((0, tsid.model.nv))\n",
+        "            J = np.vstack((J_LF, J_RF))\n",
+        "            J_com = tsid.comTask.compute(t, q, v, data).matrix\n",
+        "            A = np.vstack((J_com, J))\n",
+        "            b = np.concatenate((np.array(push_robot_com_vel), "
+        "np.zeros(J.shape[0])))\n",
+        "            v = np.linalg.lstsq(A, b, rcond=-1)[0]\n",
+        "\n",
+        "        if i % conf.DISPLAY_N == 0:\n",
+        "            tsid.viz.display(q)\n",
+        "            x_com = tsid.robot.com(tsid.formulation.data())\n",
+        "            x_com_ref = tsid.trajCom.getSample(t).pos()\n",
+        "            H_lf = tsid.robot.framePosition(tsid.formulation.data(), "
+        "tsid.LF)\n",
+        "            H_rf = tsid.robot.framePosition(tsid.formulation.data(), "
+        "tsid.RF)\n",
+        "            x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]\n",
+        "            x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]\n",
+        "            vizutils.applyViewerConfiguration(\n",
+        "                tsid.viz, \"world/com\", [*x_com.tolist(), 0, 0, 0, "
+        "1.0]\n",
+        "            )\n",
+        "            vizutils.applyViewerConfiguration(\n",
+        "                tsid.viz, \"world/com_ref\", [*x_com_ref.tolist(), 0, "
+        "0, 0, 1.0]\n",
+        "            )\n",
+        "            vizutils.applyViewerConfiguration(\n",
+        "                tsid.viz, \"world/rf\", pin.SE3ToXYZQUATtuple(H_rf)\n",
+        "            )\n",
+        "            vizutils.applyViewerConfiguration(\n",
+        "                tsid.viz, \"world/lf\", pin.SE3ToXYZQUATtuple(H_lf)\n",
+        "            )\n",
+        "            vizutils.applyViewerConfiguration(\n",
+        "                tsid.viz, \"world/rf_ref\", [*x_rf_ref.tolist(), 0, "
+        "0, 0, 1.0]\n",
+        "            )\n",
+        "            vizutils.applyViewerConfiguration(\n",
+        "                tsid.viz, \"world/lf_ref\", [*x_lf_ref.tolist(), 0, "
+        "0, 0, 1.0]\n",
+        "            )\n",
+        "\n",
+        "        time_spent = time.time() - time_start\n",
+        "        time_avg = (i * time_avg + time_spent) / (i + 1)\n",
+        "\n",
+        "        if time_avg < 0.9 * conf.dt:\n",
+        "            time.sleep(conf.dt - time_avg)\n",
+        "    print(\"end of simulation\")"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : [
+        "output = widgets.Output()\n",
+        "\n",
+        "th_simu = threading.Thread(target=run_simu)\n",
+        "th_simu.start()\n",
+        "\n",
+        "\n",
+        "@interact(com_x=(-10.0, 10.0), com_y=(-15.0, 15.0), com_z=(-40.0, "
+        "40.0))\n",
+        "def sliders_com(com_x, com_y, com_z):\n",
+        "    tsid.trajCom.setReference(\n",
+        "        com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * "
+        "com_z]).T\n",
+        "    )\n",
+        "\n",
+        "\n",
+        "@interact(rf_x=(-30.0, 30.0), rf_y=(-30.0, 30.0), rf_z=(-30.0, "
+        "30.0))\n",
+        "def sliders_rf(rf_x, rf_y, rf_z):\n",
+        "    H_rf_ref = H_rf_0.copy()\n",
+        "    H_rf_ref.translation += np.array([1e-2 * rf_x, 1e-2 * rf_y, 1e-2 "
+        "* rf_z]).T\n",
+        "    tsid.trajRF.setReference(H_rf_ref)\n",
+        "\n",
+        "\n",
+        "@interact(lf_x=(-30.0, 30.0), lf_y=(-30.0, 30.0), lf_z=(-30.0, "
+        "30.0))\n",
+        "def sliders_lf(lf_x, lf_y, lf_z):\n",
+        "    H_lf_ref = H_lf_0.copy()\n",
+        "    H_lf_ref.translation += +np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 "
+        "* lf_z]).T\n",
+        "    tsid.trajLF.setReference(H_lf_ref)\n",
+        "\n",
+        "\n",
+        "def buttons(b):\n",
+        "    if b.description == \"Stop\":\n",
+        "        run.clear()\n",
+        "        th_simu.join()\n",
+        "    elif b.description == \"Switch Right Foot\":\n",
+        "        if tsid.contact_RF_active:\n",
+        "            tsid.remove_contact_RF()\n",
+        "        else:\n",
+        "            tsid.add_contact_RF()\n",
+        "    elif b.description == \"Switch Left Foot\":\n",
+        "        if tsid.contact_LF_active:\n",
+        "            tsid.remove_contact_LF()\n",
+        "        else:\n",
+        "            tsid.add_contact_LF()\n",
+        "\n",
+        "\n",
+        "for action in [\"Stop\", \"Switch Right Foot\", \"Switch Left "
+        "Foot\"]:\n",
+        "    button = widgets.Button(description=action)\n",
+        "    button.on_click(buttons)\n",
+        "    output.append_display_data(button)"
+      ]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : ["hasattr(tsid.viz.viewer, \"jupyter_cell\") and "
+                  "tsid.viz.viewer.jupyter_cell()"]
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : []
+    },
+    {
+      "cell_type" : "code",
+      "execution_count" : null,
+      "metadata" : {},
+      "outputs" : [],
+      "source" : []
+    }
+  ],
+  "metadata" : {
+    "kernelspec" : {
+      "display_name" : "Python 3",
+      "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.5.2"
+    }
   },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "tsid = TsidBiped(conf)\n",
-    "tsid.q0[2] = 1.02127\n",
-    "\n",
-    "com_0 = tsid.robot.com(tsid.formulation.data())\n",
-    "H_rf_0 = tsid.robot.framePosition(\n",
-    "    tsid.formulation.data(), tsid.model.getFrameId(conf.rf_frame_name)\n",
-    ")\n",
-    "H_lf_0 = tsid.robot.framePosition(\n",
-    "    tsid.formulation.data(), tsid.model.getFrameId(conf.lf_frame_name)\n",
-    ")\n",
-    "\n",
-    "vizutils.addViewerSphere(\n",
-    "    tsid.viz, \"world/com\", conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR\n",
-    ")\n",
-    "vizutils.addViewerSphere(\n",
-    "    tsid.viz, \"world/com_ref\", conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR\n",
-    ")\n",
-    "vizutils.addViewerSphere(tsid.viz, \"world/rf\", conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)\n",
-    "vizutils.addViewerSphere(\n",
-    "    tsid.viz, \"world/rf_ref\", conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR\n",
-    ")\n",
-    "vizutils.addViewerSphere(tsid.viz, \"world/lf\", conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)\n",
-    "vizutils.addViewerSphere(\n",
-    "    tsid.viz, \"world/lf_ref\", conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR\n",
-    ")"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "push_robot_active, push_robot_com_vel, com_vel_entry = False, 3 * [0.0], None\n",
-    "run, push_robot_active = threading.Event(), threading.Event()\n",
-    "run.set()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "def run_simu():\n",
-    "    i, t = 0, 0.0\n",
-    "    q, v = tsid.q, tsid.v\n",
-    "    time_avg = 0.0\n",
-    "    while run.is_set():\n",
-    "        time_start = time.time()\n",
-    "\n",
-    "        tsid.comTask.setReference(tsid.trajCom.computeNext())\n",
-    "        tsid.postureTask.setReference(tsid.trajPosture.computeNext())\n",
-    "        tsid.rightFootTask.setReference(tsid.trajRF.computeNext())\n",
-    "        tsid.leftFootTask.setReference(tsid.trajLF.computeNext())\n",
-    "\n",
-    "        HQPData = tsid.formulation.computeProblemData(t, q, v)\n",
-    "\n",
-    "        sol = tsid.solver.solve(HQPData)\n",
-    "        if sol.status != 0:\n",
-    "            print(\"QP problem could not be solved! Error code:\", sol.status)\n",
-    "            break\n",
-    "\n",
-    "        # tau = tsid.formulation.getActuatorForces(sol)\n",
-    "        dv = tsid.formulation.getAccelerations(sol)\n",
-    "        q, v = tsid.integrate_dv(q, v, dv, conf.dt)\n",
-    "        i, t = i + 1, t + conf.dt\n",
-    "\n",
-    "        if push_robot_active.is_set():\n",
-    "            push_robot_active.clear()\n",
-    "            data = tsid.formulation.data()\n",
-    "            if tsid.contact_LF_active:\n",
-    "                J_LF = tsid.contactLF.computeMotionTask(0.0, q, v, data).matrix\n",
-    "            else:\n",
-    "                J_LF = np.zeros((0, tsid.model.nv))\n",
-    "            if tsid.contact_RF_active:\n",
-    "                J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, data).matrix\n",
-    "            else:\n",
-    "                J_RF = np.zeros((0, tsid.model.nv))\n",
-    "            J = np.vstack((J_LF, J_RF))\n",
-    "            J_com = tsid.comTask.compute(t, q, v, data).matrix\n",
-    "            A = np.vstack((J_com, J))\n",
-    "            b = np.concatenate((np.array(push_robot_com_vel), np.zeros(J.shape[0])))\n",
-    "            v = np.linalg.lstsq(A, b, rcond=-1)[0]\n",
-    "\n",
-    "        if i % conf.DISPLAY_N == 0:\n",
-    "            tsid.viz.display(q)\n",
-    "            x_com = tsid.robot.com(tsid.formulation.data())\n",
-    "            x_com_ref = tsid.trajCom.getSample(t).pos()\n",
-    "            H_lf = tsid.robot.framePosition(tsid.formulation.data(), tsid.LF)\n",
-    "            H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)\n",
-    "            x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]\n",
-    "            x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]\n",
-    "            vizutils.applyViewerConfiguration(\n",
-    "                tsid.viz, \"world/com\", [*x_com.tolist(), 0, 0, 0, 1.0]\n",
-    "            )\n",
-    "            vizutils.applyViewerConfiguration(\n",
-    "                tsid.viz, \"world/com_ref\", [*x_com_ref.tolist(), 0, 0, 0, 1.0]\n",
-    "            )\n",
-    "            vizutils.applyViewerConfiguration(\n",
-    "                tsid.viz, \"world/rf\", pin.SE3ToXYZQUATtuple(H_rf)\n",
-    "            )\n",
-    "            vizutils.applyViewerConfiguration(\n",
-    "                tsid.viz, \"world/lf\", pin.SE3ToXYZQUATtuple(H_lf)\n",
-    "            )\n",
-    "            vizutils.applyViewerConfiguration(\n",
-    "                tsid.viz, \"world/rf_ref\", [*x_rf_ref.tolist(), 0, 0, 0, 1.0]\n",
-    "            )\n",
-    "            vizutils.applyViewerConfiguration(\n",
-    "                tsid.viz, \"world/lf_ref\", [*x_lf_ref.tolist(), 0, 0, 0, 1.0]\n",
-    "            )\n",
-    "\n",
-    "        time_spent = time.time() - time_start\n",
-    "        time_avg = (i * time_avg + time_spent) / (i + 1)\n",
-    "\n",
-    "        if time_avg < 0.9 * conf.dt:\n",
-    "            time.sleep(conf.dt - time_avg)\n",
-    "    print(\"end of simulation\")"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "output = widgets.Output()\n",
-    "\n",
-    "th_simu = threading.Thread(target=run_simu)\n",
-    "th_simu.start()\n",
-    "\n",
-    "\n",
-    "@interact(com_x=(-10.0, 10.0), com_y=(-15.0, 15.0), com_z=(-40.0, 40.0))\n",
-    "def sliders_com(com_x, com_y, com_z):\n",
-    "    tsid.trajCom.setReference(\n",
-    "        com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * com_z]).T\n",
-    "    )\n",
-    "\n",
-    "\n",
-    "@interact(rf_x=(-30.0, 30.0), rf_y=(-30.0, 30.0), rf_z=(-30.0, 30.0))\n",
-    "def sliders_rf(rf_x, rf_y, rf_z):\n",
-    "    H_rf_ref = H_rf_0.copy()\n",
-    "    H_rf_ref.translation += np.array([1e-2 * rf_x, 1e-2 * rf_y, 1e-2 * rf_z]).T\n",
-    "    tsid.trajRF.setReference(H_rf_ref)\n",
-    "\n",
-    "\n",
-    "@interact(lf_x=(-30.0, 30.0), lf_y=(-30.0, 30.0), lf_z=(-30.0, 30.0))\n",
-    "def sliders_lf(lf_x, lf_y, lf_z):\n",
-    "    H_lf_ref = H_lf_0.copy()\n",
-    "    H_lf_ref.translation += +np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 * lf_z]).T\n",
-    "    tsid.trajLF.setReference(H_lf_ref)\n",
-    "\n",
-    "\n",
-    "def buttons(b):\n",
-    "    if b.description == \"Stop\":\n",
-    "        run.clear()\n",
-    "        th_simu.join()\n",
-    "    elif b.description == \"Switch Right Foot\":\n",
-    "        if tsid.contact_RF_active:\n",
-    "            tsid.remove_contact_RF()\n",
-    "        else:\n",
-    "            tsid.add_contact_RF()\n",
-    "    elif b.description == \"Switch Left Foot\":\n",
-    "        if tsid.contact_LF_active:\n",
-    "            tsid.remove_contact_LF()\n",
-    "        else:\n",
-    "            tsid.add_contact_LF()\n",
-    "\n",
-    "\n",
-    "for action in [\"Stop\", \"Switch Right Foot\", \"Switch Left Foot\"]:\n",
-    "    button = widgets.Button(description=action)\n",
-    "    button.on_click(buttons)\n",
-    "    output.append_display_data(button)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "hasattr(tsid.viz.viewer, \"jupyter_cell\") and tsid.viz.viewer.jupyter_cell()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": []
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": []
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 3",
-   "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.5.2"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 4
+  "nbformat" : 4,
+  "nbformat_minor" : 4
 }