4_with_obstacles.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "233d2ae1",
   "metadata": {},
   "source": [
    "# Working with obstacles\n",
    "This notebook aims at giving the basis to work with obstacles, first by examplifying the basic API of HPP-FCL, then by writing an optimization problem under noncollision avoidance constraint. One of the main message is that the collision constraint is hard, and that a lot of work remains necessary, to make it efficient in practice.\n",
    "\n",
    "We first show how to compute the minimal ellipsoid that encapsulate a body of the robot. \n",
    "Then a inverse-geometry problem is written under the constraint that a set of points remains outside of the encapsulating ellipsoid.\n",
    "\n",
    "The notebook is written for a simple UR10 robot, and you should be able to mix it with notebook #3 to make a complete trajectory optimization for a humanoid.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8d70aae",
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e8a9f203",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will use the following tools:\n",
    "- the ur10 model and the Talos model (loaded by example-robot-data)\n",
    "- HPP-FCL, through the Pinocchio API\n",
    "- pinocchio.casadi for writing the problem and computing its derivatives\n",
    "- the IpOpt solver wrapped in casadi\n",
    "- the meshcat viewer"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "496d86af",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/encapsulating_ellipses_import\n",
    "import pinocchio as pin\n",
    "from pinocchio import casadi as cpin\n",
    "import casadi\n",
    "import numpy as np\n",
    "import example_robot_data as robex\n",
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "71f8fa8e",
   "metadata": {},
   "source": [
    "We will load the UR10 model, but feel free to change it for any model you like."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d07b35d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/encapsulating_ellipses_load\n",
    "# --- Load robot model\n",
    "robot = robex.load(\"ur10\")\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45c0b04e",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f676ef39",
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8de21a7c",
   "metadata": {},
   "source": [
    "## Accessing the vertices of the collisions objects\n",
    "Let's quickly summarize the basics of collision avoidance in Pinocchio. Our models are most often loaded from URDF storage, which typically contains two sets of 3D objects: the visuals, very detailed and often colored meshed used for display; and the collisions, less detailed objects used for a fair computation of the collisions. Both are stored in the robot_wrapper, under collision_model and visual_model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cdf37f3c",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(robot.collision_model, robot.visual_model)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f9959caa",
   "metadata": {},
   "source": [
    "Both are corresponding to the same structure of type pin.GeometryModel. It mostly contains a list of geometry objects (that we are going to use) and a list of collision pair (very useful, but not in this notebook).\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f7681a13",
   "metadata": {},
   "outputs": [],
   "source": [
    "geom = robot.collision_model.geometryObjects[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "192eb897",
   "metadata": {},
   "source": [
    "A geometry object is a 3d shape attached to a parent joint at a specific placement. It mostly contains 3 fields: geom.parentJoint the index of the parent joint ; geom.placement, the placement of the 3d shape in the frame of the parent joint; and geom.geometry, a specific structure representing the shape."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32dd7009",
   "metadata": {},
   "outputs": [],
   "source": [
    "shape = geom.geometry\n",
    "print(shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3ed6058",
   "metadata": {},
   "source": [
    "Shapes can be simple features (sphere, capsule, ellipsoid, box, etc), or soup of polygon. This is the most frequent case when loading a model from URDF. Then we are mostly interested by the list of vertices:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "04de2eaf",
   "metadata": {},
   "outputs": [],
   "source": [
    "vertices = shape.vertices()\n",
    "print(vertices)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0d08a721",
   "metadata": {},
   "source": [
    "Most of the time, when you are working with HPP-FCL through Pinocchio, you will not go that deep in the collision description, but rather access it through high level functions of HPP-FCL, like computing distances, detecting collisions, etc. Here we are going to use this list to compute the minimal encapsulating ellispoid."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9052373d",
   "metadata": {},
   "source": [
    "## Encapsulating\n",
    "\n",
    "### Problem formulation\n",
    "An ellispoid $\\mathcal{E}$ can be defined by a symmetric positive matrix $A$ and a center 3d vector $c$, as the set of points respecting:\n",
    "$$ \\forall p \\in \\mathcal{E}, (p-c)^T A (p-c) \\le 1$$\n",
    "\n",
    "The matrix $A$ can be deduced from the orientation of the main axes $R$ and the 3 radii $r=(r_1,r_2,r_3)$ by:\n",
    "$$A=R \\ \\textrm{diag}(\\frac{1}{r^2}) R^T$$\n",
    "where $\\textrm{diag}(\\frac{1}{r^2})$ is the diagonal matrix formed with the inverted squared radii.\n",
    "\n",
    "Inside an optimization program, $R$ is more conveniently represented by ... (if you thought Euler angles, please leave the room) ... angle vectors or quaternion. We will you the first with $R\\triangleq exp(w)$ and $w\\in R^3$ and unconstrained 3d angle vector.\n",
    "\n",
    "The problem can then be written:\n",
    "\n",
    "Decide: \n",
    "- $w\\in R^3$ the ellipsoid orientation\n",
    "- $c \\in R^3$ the ellipsoid center\n",
    "- $r \\in R^3$ the radii\n",
    "\n",
    "Minimizing: the ellipsoid volum $\\prod_{i=1}^{3} r_i$\n",
    "\n",
    "Subject to:\n",
    "- $r>=0$\n",
    "- $\\for v \\in V:  (v-c)^T A (v-c) \\le 1$\n",
    "with $V$ the list of vertices, and $A$ defined as above mentionned from the decision variables.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4c88a7ad",
   "metadata": {},
   "source": [
    "### Helpers \n",
    "When writing this problem, $A$ and $c$ will be represented in the same frame as $V$. Remember that the vertices of the shape are given in the shape frame. Converting them in the joint frame can be done with geom.placement:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b25f1e3a",
   "metadata": {},
   "outputs": [],
   "source": [
    "geom.placement.act(vertices[0, :])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d9ce2285",
   "metadata": {},
   "source": [
    "We will need the following simple Casadi helper to get rid of the SX/MX syntax."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7a5b61a4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/encapsulating_ellipses_helper\n",
    "cw = casadi.SX.sym(\"w\", 3)\n",
    "exp = casadi.Function(\"exp3\", [cw], [cpin.exp3(cw)])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c6abb094",
   "metadata": {},
   "source": [
    "Now write an optimization problem with Casadi to compute the representation of the minimal encapsulating ellipsoid in the frame of the parent joint. For that, follow the steps:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a6e80a8b",
   "metadata": {},
   "source": [
    "1. Define the decision variables $w,c,r$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4f9ade31",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/encapsulating_ellipses_vars"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "910de1b8",
   "metadata": {},
   "source": [
    "2. Shape the matrix $A$ from the decision variables."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6287a68b",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/encapsulating_ellipses_RA"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f502451a",
   "metadata": {},
   "source": [
    "3. Define the cost."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "638dd4c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/encapsulating_ellipses_cost"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7877f866",
   "metadata": {},
   "source": [
    "4. Define the radius positivity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24d80766",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/encapsulating_ellipses_rplus"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "476e3cc7",
   "metadata": {},
   "source": [
    "5. Define the encapsulating constraint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f8be3a33",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/encapsulating_ellipses_points"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9db104b5",
   "metadata": {},
   "source": [
    "6. Solve and recover the optimal values."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "581430ff",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/encapsulating_ellipses_solve"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d9a302d5",
   "metadata": {},
   "source": [
    "### Display and future use\n",
    "\n",
    "The ellipoid can be displayed in Meshcat. You need to give the radii to create the shape, and then place it at the center with proper orientaion. So Meshcat needs $(c,r,R)$ and not $(A,c)$. You can get $r,R$ from $A$ by Eigen decomposion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4092daf1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/encapsulating_ellipses_meshcat\n",
    "# Build the ellipsoid 3d shape\n",
    "# Ellipsoid in meshcat\n",
    "viz.addEllipsoid(\"el\", sol_r, [0.3, 0.9, 0.3, 0.3])\n",
    "# jMel is the placement of the ellipsoid in the joint frame\n",
    "jMel = pin.SE3(sol_R, sol_c)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f0b2608f",
   "metadata": {},
   "source": [
    "We can now place the initial shape geom, the vertices represented by small spheres, and the ellispoid, at a consistent location."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "03f40757",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/encapsulating_ellipses_vizplace\n",
    "# Place the body, the vertices and the ellispod at a random configuration oMj_rand\n",
    "oMj_rand = pin.SE3.Random()\n",
    "viz.applyConfiguration(viz.getViewerNodeName(geom, pin.VISUAL), oMj_rand)\n",
    "for i in np.arange(0, vertices.shape[0]):\n",
    "    viz.applyConfiguration(\n",
    "        f\"world/point_{i}\", oMj_rand.act(vertices[i]).tolist() + [1, 0, 0, 0]\n",
    "    )\n",
    "viz.applyConfiguration(\"el\", oMj_rand * jMel)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd77e60e",
   "metadata": {},
   "source": [
    "## Inverse geometry with collisions"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "45ce7be5",
   "metadata": {},
   "source": [
    "We will now write an inverse geometry problem under noncollision constraint. Ideally, the noncollision constraint should be that the distance between the bodies is positive (we might even argue whether this is sufficient or not). HPP-FCL is computing this function, yet differenciating it is more difficult as distances between nonstrictly-convex object is not a smooth function. Recent works, for example the work of Louis Montaut (2022), give some direction to solve this issue, but they are not yet fully available, and in particular not with the automatic differentiation system of Casadi.\n",
    "\n",
    "We are then going to use a simple collision constraint: keep a point outside of an ellispoid by making the quadratic ellispoid equation larger than 1:\n",
    "$$ \\forall \\mathcal{E}, \\forall p, (^\\mathcal{E}p-c_\\mathcal{E})^T A_\\mathcal{E} (^\\mathcal{E}p-c_\\mathcal{E}) \\ge 1$$\n",
    "\n",
    "with $\\mathcal{E} = (A_\\mathcal{E},c_\\mathcal{E})$ the ellispoid en $^\\mathcal{E}p$ the representation of the obstacle $p$ (a simple point) in the ellispoid frame.\n",
    "\n",
    "Louis Montaut (2022). Collision Detection Accelerated: An Optimization Perspective, by L Montaut, Q Le Lidec, V Petrik, J Sivic, J Carpentier. RSS\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9ebe0b17",
   "metadata": {},
   "source": [
    "We are going to do that for a UR-10 robot positioning its end effector, for you to adapt it to any humanoid robot you want within a full optimal control setting."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62feb08d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_hyper\n",
    "### HYPER PARAMETERS\n",
    "Mtarget = pin.SE3(pin.utils.rotate(\"y\", 3), np.array([-0.8, -0.1, 0.2]))  # x,y,z\n",
    "q0 = np.array([0, 5, 3, 0, 2, 0])\n",
    "endEffectorFrameName = \"tool0\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "049d6ce8",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_model\n",
    "# The pinocchio model is what we are really interested by.\n",
    "model = robot.model\n",
    "data = model.createData()\n",
    "endEffector_ID = model.getFrameId(endEffectorFrameName)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a5ad9e1b",
   "metadata": {},
   "source": [
    "### UR-10 ellipsoids and obstacles\n",
    "You can compute the encapsulating ellispoids with the first part of the notebook, for the robot you prefer. Do that for all meaningfull bodies of your robot, and store them in a list which each items e contains the following fields:\n",
    "- e.A: the ellipsoid matrix\n",
    "- e.center: its center\n",
    "- e.rotation: the orientation of its main axes\n",
    "- e.radius: the 3 radii\n",
    "- e.placement: SE3(e.rotation,e.center)\n",
    "- e.id: the id of the joint it is attached to\n",
    "- e.name: for display\n",
    "\n",
    "In case you need it, here is the UR10 set up, using the SimpleNamespace class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "01d0b9e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "from types import SimpleNamespace"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c4dbd4a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_ellipses\n",
    "# These values are computed using encapsulating_ellipse.py\n",
    "ellipses = [\n",
    "    SimpleNamespace(\n",
    "        name=\"shoulder_lift_joint\",\n",
    "        A=np.array(\n",
    "            [\n",
    "                [75.09157846, 0.34008563, -0.08817025],\n",
    "                [0.34008563, 60.94969446, -0.55672959],\n",
    "                [-0.08817025, -0.55672959, 3.54456814],\n",
    "            ]\n",
    "        ),\n",
    "        center=np.array([-1.05980885e-04, -5.23471160e-02, 2.26280651e-01]),\n",
    "    ),\n",
    "    SimpleNamespace(\n",
    "        name=\"elbow_joint\",\n",
    "        A=np.array(\n",
    "            [\n",
    "                [1.30344372e02, -5.60880392e-02, -1.87555288e-02],\n",
    "                [-5.60880392e-02, 9.06119040e01, 1.65531606e-01],\n",
    "                [-1.87555288e-02, 1.65531606e-01, 4.08568387e00],\n",
    "            ]\n",
    "        ),\n",
    "        center=np.array([-2.01944435e-05, 7.22262249e-03, 2.38805264e-01]),\n",
    "    ),\n",
    "    SimpleNamespace(\n",
    "        name=\"wrist_1_joint\",\n",
    "        A=np.array(\n",
    "            [\n",
    "                [2.31625634e02, 5.29558437e-01, -1.62729657e-01],\n",
    "                [5.29558437e-01, 2.18145143e02, -1.42425434e01],\n",
    "                [-1.62729657e-01, -1.42425434e01, 1.73855962e02],\n",
    "            ]\n",
    "        ),\n",
    "        center=np.array([-9.78431524e-05, 1.10181763e-01, 6.67932259e-03]),\n",
    "    ),\n",
    "    SimpleNamespace(\n",
    "        name=\"wrist_2_joint\",\n",
    "        A=np.array(\n",
    "            [\n",
    "                [2.32274519e02, 1.10812959e-01, -1.12998357e-02],\n",
    "                [1.10812959e-01, 1.72324444e02, -1.40077876e01],\n",
    "                [-1.12998357e-02, -1.40077876e01, 2.19132854e02],\n",
    "            ]\n",
    "        ),\n",
    "        center=np.array([-2.64650554e-06, 6.27960760e-03, 1.11112087e-01]),\n",
    "    ),\n",
    "]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59b2da3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_ellipses_2\n",
    "for e in ellipses:\n",
    "    e.id = robot.model.getJointId(e.name)\n",
    "    l, P = np.linalg.eig(e.A)\n",
    "    e.radius = 1 / l**0.5\n",
    "    e.rotation = P\n",
    "    e.placement = pin.SE3(P, e.center)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "565d10ca",
   "metadata": {},
   "source": [
    "Obstacles are stored in a similar list, with each item o containing:\n",
    "- o.name: for display\n",
    "- o.radius: for visualizing it as a sphere in meshcat\n",
    "- o.pos: the 3d position in world frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9093424d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_obstacles\n",
    "# Obstacle positions are arbitrary. Their radius is meaningless, just for visualization.\n",
    "obstacles = [\n",
    "    SimpleNamespace(radius=0.01, pos=np.array([-0.4, 0.2 + s, 0.5]), name=f\"obs_{i_s}\")\n",
    "    for i_s, s in enumerate(np.arange(-0.5, 0.5, 0.1))\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "78da257e",
   "metadata": {},
   "source": [
    "Here is the visualisation of the set up."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4280b4aa",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_vizellipses\n",
    "for e in ellipses:\n",
    "    viz.addEllipsoid(f\"el_{e.name}\", e.radius, [0.3, 0.9, 0.3, 0.3])\n",
    "for io, o in enumerate(obstacles):\n",
    "    viz.addSphere(f\"obs_{io}\", o.radius, [0.8, 0.3, 0.3, 0.9])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2203515a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_vizsimples\n",
    "# --- Add box to represent target\n",
    "# Add a vizualization for the target\n",
    "boxID = \"world/box\"\n",
    "viz.addBox(boxID, [0.05, 0.1, 0.2], [1.0, 0.2, 0.2, 0.5])\n",
    "# Add a vizualisation for the tip of the arm.\n",
    "tipID = \"world/blue\"\n",
    "viz.addBox(tipID, [0.08] * 3, [0.2, 0.2, 1.0, 0.5])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a1e4ff74",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6dfba0b2",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_viz\n",
    "def displayScene(q, dt=1e-1):\n",
    "    \"\"\"\n",
    "    Given the robot configuration, display:\n",
    "    - the robot\n",
    "    - a box representing endEffector_ID\n",
    "    - a box representing Mtarget\n",
    "    \"\"\"\n",
    "    pin.framesForwardKinematics(model, data, q)\n",
    "    M = data.oMf[endEffector_ID]\n",
    "    viz.applyConfiguration(boxID, Mtarget)\n",
    "    viz.applyConfiguration(tipID, M)\n",
    "    for e in ellipses:\n",
    "        M = data.oMi[e.id]\n",
    "        viz.applyConfiguration(f\"el_{e.name}\", M * e.placement)\n",
    "    for io, o in enumerate(obstacles):\n",
    "        viz.applyConfiguration(f\"obs_{io}\", pin.SE3(np.eye(3), o.pos))\n",
    "    viz.display(q)\n",
    "    time.sleep(dt)\n",
    "\n",
    "\n",
    "displayScene(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fef5a27b",
   "metadata": {},
   "source": [
    "### Writing the problem\n",
    "The inverse geometry with collision will be written:\n",
    "\n",
    "Decide: $q \\in \\mathcal{Q}$ the robot configuration\n",
    "\n",
    "Minimizing: $|| tool(q) - target ||^2$\n",
    "\n",
    "Subject to: $$\\forall \\mathcal{E}, \\forall p, (^\\mathcal{E}p-c_\\mathcal{E})^T A_\\mathcal{E} (^\\mathcal{E}p-c_\\mathcal{E}) \\ge 1$$\n",
    "\n",
    "with $tool(q)$ the end effector position with respect to a given $target$, $\\mathcal{E}$ the ellipsoids described in associated joint frames, and $^\\mathcal{E}p = \\ ^oM_\\mathcal{E}^{-1} \\ ^op$ the position of each obstacle expressed in the joint frames associated to each ellispoid.\n",
    "\n",
    "Classically, we need the basic Casadi helpers to get read of the SX/MX formulation.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25e6c797",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_casadi\n",
    "# --- Casadi helpers\n",
    "cmodel = cpin.Model(model)\n",
    "cdata = cmodel.createData()\n",
    "\n",
    "cq = casadi.SX.sym(\"q\", model.nq, 1)\n",
    "cpin.framesForwardKinematics(cmodel, cdata, cq)\n",
    "error6_tool = casadi.Function(\n",
    "    \"etool\",\n",
    "    [cq],\n",
    "    [cpin.log6(cdata.oMf[endEffector_ID].inverse() * cpin.SE3(Mtarget)).vector],\n",
    ")\n",
    "error3_tool = casadi.Function(\n",
    "    \"etool\", [cq], [cdata.oMf[endEffector_ID].translation - Mtarget.translation]\n",
    ")\n",
    "error_tool = error3_tool"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4260a19b",
   "metadata": {},
   "source": [
    "For each ellispoid, we need to compute the obstacle position in local frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b34e13c7",
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp4/generated/invgeom_with_obstacles_e_pos\n",
    "cpos = casadi.SX.sym(\"p\", 3)\n",
    "for e in ellipses:\n",
    "    # Position of the obstacle cpos in the ellipse frame.\n",
    "    e.e_pos = casadi.Function(\n",
    "        f\"e{e.id}\", [cq, cpos], [cdata.oMi[e.id].inverse().act(casadi.SX(cpos))]\n",
    "    )"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c3ce4007",
   "metadata": {},
   "source": [
    "Using these helpers, write the optimization problem following these steps.\n",
    "\n",
    "1. Declare the variables, and compute the cost."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "864809a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/invgeom_with_obstacles_opti"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c99ed96",
   "metadata": {},
   "source": [
    "2. For each pair ellispoid-obstacle, add a constraint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24812b6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/invgeom_with_obstacles_constraint"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9740909a",
   "metadata": {},
   "source": [
    "3. Solve"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "db04cfda",
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/invgeom_with_obstacles_solve"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}