1-invgeom.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Check your background of numerical programming\n",
    "\n",
    "This notebook is a quick recap of the main practical concepts of numerical programming, and introduces the Casadi module of the Pinocchio library:\n",
    "- Pinocchio is obviously a library gathering the main rigid-body dynamics library.\n",
    "- Casadi is an auto-differentiation tools, i.e. you can write symbolic expression and differentiate them. It is also wrapping the IpOpt solver, which is a quite robust and generic constraint-based optimization solver. You can then quickly write a problem with some symbolic expression for the cost and constraints, and Casadi will transfer it to IpOpt for solving it.\n",
    "- The Pinocchio/Casadi module is a part of the Pinocchio solver written by Justin Carpentier which uses Casadi to unroll the algorithms of Pinocchio into symbolic expression, which then can be differentiated. It is a very powerful tool for initiation and prototyping, although it cannot scales to very large problems (in particular whole-body optimization problems) due to various technical problems that we can discuss.\n",
    "\n",
    "## Definition of the toy problem studied here\n",
    "\n",
    "We propose to explain how to define and solve inverse-geometry problems, i.e. problems where the main decision variable is a single robot configuration.\n",
    "\n",
    "Inverse geometry is the problem of computing a configuration $q$ of a robot model that satisfies a set of 'tasks', which we formalize as error functions $e(q)$ that are zero when the task is achieved.\n",
    "\n",
    "- $q \\in \\mathcal{C}$: configuration of the robot.\n",
    "- $e_i(q) \\in \\mathbb{R}$: error function for task $i$. In this tutorial our error function is real valued.\n",
    "\n",
    "Typical tasks for limbed robots include putting a foot/end-effector on a surface, moving the center of mass (CoM) to a target location, etc. For example, an error function for a foot position task could be:\n",
    "\n",
    "$$\n",
    "e(q) = \\|p_\\mathrm{foot}^* - p_\\mathrm{foot}(q)\\|\n",
    "$$\n",
    "\n",
    "In this tutorial, we will define a task function $e(q)$ for the end-effector pose of a manipulator and solve the inverse geometry problem\n",
    "\n",
    "$$\n",
    "\\underset{q}{\\mathrm{minimize}}\\ e(q)\n",
    "$$\n",
    "\n",
    "using an off-the-shelf nonlinear optimizer that evaluates gradients by finite differences (slower), then a more efficient formulation using CasADi (faster)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Before we start...\n",
    "\n",
    "%load is a \"magic\" command of Jupyter. When we will invite you to think to a solution by yourselves, we will indicate it by the %do_not_load command. Feel free to transform it to a plain %load if you want the answer. This is define in the following import."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "NB: as for all the tutorials, a magic command %do_not_load is introduced to hide\n",
      "    the solutions to some questions. Change it for %load if you want to see (and\n",
      "    execute) the solution.\n"
     ]
    }
   ],
   "source": [
    "import gepetuto.magic"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's import everything we will need in this tutorial:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_imports\n",
    "import time\n",
    "import unittest\n",
    "import example_robot_data as robex\n",
    "import numpy as np\n",
    "import casadi\n",
    "import pinocchio as pin\n",
    "import pinocchio.casadi as cpin\n",
    "from wan2024.meshcat_viewer_wrapper import MeshcatVisualizer, colors\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true
   },
   "source": [
    "Let us start by loading the UR5 robot model from `example-robot-data` and define its neutral configuration as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_robot\n",
    "robot = robex.load(\"ur10\")\n",
    "model = robot.model\n",
    "data = robot.data\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will also use the MeshCat visualizer to watch the optimization unfold. Let's set up a visualizer for our robot model:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "*** You asked to start meshcat \"classically\" in tcp://127.0.0.1:6000\n",
      "*** Did you start meshcat manually (meshcat-server)\n",
      "Wrapper tries to connect to server <tcp://127.0.0.1:6000>\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    }
   ],
   "source": [
    "# %load tp1/generated/invgeom_visualizer\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0*3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Definition of the problem"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Task target\n",
    "\n",
    "Our task will be to move the end effector *frame* of the UR5 to a desired pose (pose := position + orientation). For this purpose, we define:\n",
    "\n",
    "- The name of the corresponding frame, which is ``\"tool0\"`` for the UR5.\n",
    "- The target pose of this frame, which we define by an affine transform ${}_{world} M_{target} \\in SE(3)$ (also known as a \"placement\").\n",
    "\n",
    "The robot will start from an arbitrary initial configuration (hint: to make the solver's life easier, avoid the singular $[0\\ 0\\ \\ldots\\ 0]$ configuration) $q_0$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_task_params\n",
    "robot.q0 = np.array([0, -np.pi / 2, 0, 0, 0, 0])\n",
    "\n",
    "tool_id = model.getFrameId(\"tool0\")\n",
    "\n",
    "in_world_M_target = pin.SE3(\n",
    "    pin.utils.rotate(\"x\", np.pi / 4),\n",
    "    np.array([-0.5, 0.1, 0.2]),\n",
    ")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Sidetrack: visualization\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_visualizer_callback\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])\n",
    "\n",
    "def displayScene(q):\n",
    "    \"\"\"\n",
    "    Given the robot configuration, display:\n",
    "    - the robot\n",
    "    - a box representing tool_id\n",
    "    - a box representing in_world_M_target\n",
    "    \"\"\"\n",
    "    pin.framesForwardKinematics(model, data, q)\n",
    "    in_world_M_tool = data.oMf[tool_id]\n",
    "    viz.applyConfiguration(boxID, in_world_M_target)\n",
    "    viz.applyConfiguration(tipID, in_world_M_tool)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-1)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We are now ready to optimize for real!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introducting Casadi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We wil now define our problem in [CasADi](https://web.casadi.org), a framework that allows us to formulate problems as computation graphs from which it can compute derivatives (notably first-order derivatives, *a.k.a.* gradients, and second-order derivatives, *a.k.a.* Hessians) automatically. This feature is called *automatic differentiation*."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Modelling with CasADi\n",
    "\n",
    "Pinocchio, in the version 3 we are using in today's tutorial, provides a `pinocchio.casadi` interface to represent configurations (such as $q \\in \\mathcal{C}$ or $M \\in SE(3)$) in CasADi computation graphs, and differentiate them appropriately."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "CasADi being designed for Euclidean variables, differentiating properly over manifolds is not as straightforward as representing everything as matrices and vectors in $\\mathbb{R}^n$ and taking their usual derivatives. Pinocchio allows us to take derivatives over manifolds properly, without worrying about what happens below the surface. To be able to do that, we first need to redefine our model as a CasADi one:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_casadi_model\n",
    "# --- Casadi helpers\n",
    "cmodel = cpin.Model(model)\n",
    "cdata = cmodel.createData()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Our configuration variable $q$ is now a CasADi ``SX`` symbol of size $n_q$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_cq\n",
    "cq = casadi.SX.sym(\"q\", model.nq, 1)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "SX(@1=3, @2=1, [(@1*(q_0+@2)), (@1*(q_1+@2)), (@1*(q_2+@2)), (@1*(q_3+@2)), (@1*(q_4+@2)), (@1*(q_5+@2))])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "3*(cq+1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We complete the computation graph from ``cmodel`` to ``cq`` by calling the corresponding forward-kinematics function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_casadi_fk\n",
    "cpin.framesForwardKinematics(cmodel, cdata, cq)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "SE3(SX(@1=cos(q_0), @2=4.89664e-12, @3=cos(q_1), @4=sin(q_1), @5=((@2*@3)-@4), @6=(@1*@5), @7=cos(q_2), @8=((@2*@4)+@3), @9=(@1*@8), @10=sin(q_2), @11=((@6*@7)-(@9*@10)), @12=cos(q_3), @13=sin(q_3), @14=((@2*@12)-@13), @15=((@6*@10)+(@9*@7)), @16=(@12+(@2*@13)), @17=((@11*@14)-(@15*@16)), @18=cos(q_4), @19=sin(q_0), @20=sin(q_4), @21=((@17*@18)-(@19*@20)), @22=cos(q_5), @23=((@2*@13)+@12), @24=((@2*@12)-@13), @25=((@11*@23)+(@15*@24)), @26=sin(q_5), @27=(@19*@5), @28=(@19*@8), @29=((@27*@7)-(@28*@10)), @30=((@27*@10)+(@28*@7)), @31=((@29*@14)-(@30*@16)), @32=((@31*@18)+(@1*@20)), @33=((@29*@23)+(@30*@24)), @34=(@3+(@2*@4)), @35=((@2*@3)-@4), @36=((@34*@7)+(@35*@10)), @37=((@35*@7)-(@34*@10)), @38=((@36*@14)+(@37*@16)), @39=(@38*@18), @40=((@37*@24)-(@36*@23)), @41=0, @42=((@17*@20)+(@19*@18)), @43=((@21*@26)+(@25*@22)), @44=((@1*@18)-(@31*@20)), @45=((@32*@26)+(@33*@22)), @46=(@38*@20), @47=((@40*@22)-(@39*@26)), @48=0.612, @49=-0.1719, @50=0.220941, @51=0.5723, @52=0.1149, @53=0.1157, @54=0.0922, [[((@21*@22)-(@25*@26)), (-((@2*@42)+@43)), ((@2*@43)-@42), (((((((@48*@9)-(@49*@19))-(@50*@19))+(@51*@15))-(@52*@19))+(@53*@25))-(@54*@42))],  [((@32*@22)-(@33*@26)), ((@2*@44)-@45), (@44+(@2*@45)), ((((((@50*@1)+((@49*@1)+(@48*@28)))+(@51*@30))+(@52*@1))+(@53*@33))+(@54*@44))],  [(-((@39*@22)+(@40*@26))), ((@2*@46)-@47), (@46+(@2*@47)), ((((0.1273+(@48*@35))+(@51*@37))+(@53*@40))+(@54*@46))],  [@41, @41, @41, 1]]))"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cdata.oMf[tool_id]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Task error\n",
    "\n",
    "For a given robot configuration $q$, the end effector will be located at ${}_{world} M_{tool}(q)$, which is a function of $q$ we can compute with Pinocchio's forward kinematics function. The goal of our task will be to make ${}_{world} M_{tool}(q)$ coincide with ${}_{world} M_{target}$, which we can define mathematically as: minimize the (Lie-group) difference ${}_{world} M_{target} \\ominus {}_{world} M_{tool}(q)$, or equivalently:\n",
    "\n",
    "$$\n",
    "\\underset{q}{\\mathrm{minimize}} \\ e(q) := \\| {}_{world} M_{target} \\ominus {}_{world} M_{tool}(q) \\|\n",
    "$$\n",
    "\n",
    "Remember the definition of the $\\ominus$ (left) Lie-group difference? No? Let's write it in Python anyway! \n",
    "\n",
    "As a CasADi function, it writes:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_casadi_error\n",
    "error_tool = casadi.Function(\n",
    "    \"etool\",\n",
    "    [cq],\n",
    "    [\n",
    "        cpin.log6(\n",
    "            cdata.oMf[tool_id].inverse() * cpin.SE3(in_world_M_target)\n",
    "        ).vector\n",
    "    ],\n",
    ")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "SX(@1=0, [-0.5, 0.173346, 0.150342, 0.785398, @1, @1])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cpin.log6(cpin.SE3(in_world_M_target)).vector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we define a computation graph from our optimization variable to our error function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_casadi_computation_graph\n",
    "opti = casadi.Opti()\n",
    "var_q = opti.variable(model.nq)\n",
    "totalcost = casadi.sumsqr(error_tool(var_q))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "MX(opti0_x_1)"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "var_q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "MX(etool(opti0_x_1){0})"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "error_tool(var_q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "MX(@1=etool(opti0_x_1){0}, dot(@1, @1))"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "totalcost"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### IPOPT optimization\n",
    "We now specify a cost to minimize, and an algorithm to search for the solution."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp1/generated/invgeom_ipopt\n",
    "opti.minimize(totalcost)\n",
    "opti.solver(\"ipopt\")  # select the backend solver\n",
    "opti.callback(lambda i: displayScene(opti.debug.value(var_q)))\n",
    "# %load tp1/generated/invgeom_solve\n",
    "# Caution: in case the solver does not converge, we are picking the candidate values\n",
    "# at the last iteration in opti.debug, and they are NO guarantee of what they mean.\n",
    "try:\n",
    "    sol = opti.solve_limited()\n",
    "    sol_q = opti.value(var_q)\n",
    "except:\n",
    "    print(\"ERROR in convergence, plotting debug info.\")\n",
    "    sol_q = opti.debug.value(var_q)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "#opti.subject_to(var_q >= 0)\n",
    "#opti.subject_to(var_q[1] == 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we can call the solver:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "******************************************************************************\n",
      "This program contains Ipopt, a library for large-scale nonlinear optimization.\n",
      " Ipopt is released as open source code under the Eclipse Public License (EPL).\n",
      "         For more information visit https://github.com/coin-or/Ipopt\n",
      "******************************************************************************\n",
      "\n",
      "This is Ipopt version 3.14.13, running with linear solver MUMPS 5.2.1.\n",
      "\n",
      "Number of nonzeros in equality constraint Jacobian...:        0\n",
      "Number of nonzeros in inequality constraint Jacobian.:        0\n",
      "Number of nonzeros in Lagrangian Hessian.............:       21\n",
      "\n",
      "Total number of variables............................:        6\n",
      "                     variables with only lower bounds:        0\n",
      "                variables with lower and upper bounds:        0\n",
      "                     variables with only upper bounds:        0\n",
      "Total number of equality constraints.................:        0\n",
      "Total number of inequality constraints...............:        0\n",
      "        inequality constraints with only lower bounds:        0\n",
      "   inequality constraints with lower and upper bounds:        0\n",
      "        inequality constraints with only upper bounds:        0\n",
      "\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "   0  1.6997854e+01 0.00e+00 9.12e+00  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0\n",
      "   1  1.5092964e+01 0.00e+00 8.09e+00  -1.0 8.00e-02   2.0 1.00e+00 1.00e+00f  1\n",
      "   2  1.1394817e+01 0.00e+00 6.48e+00  -1.0 1.90e-01   1.5 1.00e+00 1.00e+00f  1\n",
      "   3  6.7968648e+00 0.00e+00 4.31e+00  -1.0 3.68e-01   1.0 1.00e+00 1.00e+00f  1\n",
      "   4  3.2648123e+00 0.00e+00 2.32e+00  -1.0 6.48e-01   0.6 1.00e+00 1.00e+00f  1\n",
      "   5  1.4326652e+00 0.00e+00 7.29e-01  -1.0 1.13e+00   0.1 1.00e+00 1.00e+00f  1\n",
      "   6  1.1893846e+00 0.00e+00 1.20e+00  -1.7 3.97e+00  -0.4 1.00e+00 1.00e+00f  1\n",
      "   7  6.8502827e-01 0.00e+00 5.43e-01  -1.7 5.82e-01   0.0 1.00e+00 1.00e+00f  1\n",
      "   8  2.2516725e-01 0.00e+00 1.69e-01  -1.7 7.48e-01  -0.4 1.00e+00 1.00e+00f  1\n",
      "   9  9.0593871e-02 0.00e+00 1.09e-01  -2.5 7.24e-01  -0.9 1.00e+00 1.00e+00f  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  10  6.7217982e-03 0.00e+00 1.17e-01  -2.5 2.24e+00    -  1.00e+00 5.00e-01f  2\n",
      "  11  6.2230545e-05 0.00e+00 9.47e-03  -2.5 9.63e-02    -  1.00e+00 1.00e+00f  1\n",
      "  12  1.9832645e-08 0.00e+00 9.28e-05  -3.8 1.19e-02    -  1.00e+00 1.00e+00f  1\n",
      "  13  2.3562783e-15 0.00e+00 4.59e-08  -5.7 2.77e-04    -  1.00e+00 1.00e+00f  1\n",
      "  14  2.7188160e-29 0.00e+00 2.94e-15  -8.6 6.84e-08    -  1.00e+00 1.00e+00f  1\n",
      "\n",
      "Number of Iterations....: 14\n",
      "\n",
      "                                   (scaled)                 (unscaled)\n",
      "Objective...............:   2.7188160431922703e-29    2.7188160431922703e-29\n",
      "Dual infeasibility......:   2.9406336429710158e-15    2.9406336429710158e-15\n",
      "Constraint violation....:   0.0000000000000000e+00    0.0000000000000000e+00\n",
      "Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00\n",
      "Complementarity.........:   0.0000000000000000e+00    0.0000000000000000e+00\n",
      "Overall NLP error.......:   2.9406336429710158e-15    2.9406336429710158e-15\n",
      "\n",
      "\n",
      "Number of objective function evaluations             = 20\n",
      "Number of objective gradient evaluations             = 15\n",
      "Number of equality constraint evaluations            = 0\n",
      "Number of inequality constraint evaluations          = 0\n",
      "Number of equality constraint Jacobian evaluations   = 0\n",
      "Number of inequality constraint Jacobian evaluations = 0\n",
      "Number of Lagrangian Hessian evaluations             = 14\n",
      "Total seconds in IPOPT                               = 1.585\n",
      "\n",
      "EXIT: Optimal Solution Found.\n",
      "      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval\n",
      "callback_fun  | 547.43ms ( 36.50ms)   1.57 s (104.44ms)        15\n",
      "       nlp_f  | 276.00us ( 13.80us) 245.81us ( 12.29us)        20\n",
      "  nlp_grad_f  | 336.00us ( 21.00us) 333.77us ( 20.86us)        16\n",
      "  nlp_hess_l  |   2.71ms (193.36us)   2.72ms (194.29us)        14\n",
      "       total  | 578.98ms (578.98ms)   1.59 s (  1.59 s)         1\n"
     ]
    }
   ],
   "source": [
    "# %load tp1/generated/invgeom_solve\n",
    "# Caution: in case the solver does not converge, we are picking the candidate values\n",
    "# at the last iteration in opti.debug, and they are NO guarantee of what they mean.\n",
    "try:\n",
    "    sol = opti.solve_limited()\n",
    "    sol_q = opti.value(var_q)\n",
    "except:\n",
    "    print(\"ERROR in convergence, plotting debug info.\")\n",
    "    sol_q = opti.debug.value(var_q)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can check that the solver converged to the same solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The robot finally reached effector placement at\n",
      "   R =\n",
      "           1  7.77156e-16 -3.33067e-16\n",
      "-6.66134e-16     0.707107    -0.707107\n",
      "-3.88578e-16     0.707107     0.707107\n",
      "  p = -0.5  0.1  0.2\n",
      "\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(  R =\n",
       "            1 -3.33067e-16 -7.77156e-16\n",
       " -6.66134e-16    -0.707107    -0.707107\n",
       " -3.88578e-16     0.707107    -0.707107\n",
       "   p =     -0.5 0.165195 0.134805,\n",
       "   R =\n",
       "            1 -3.33067e-16 -7.77156e-16\n",
       " -6.66134e-16    -0.707107    -0.707107\n",
       " -3.88578e-16     0.707107    -0.707107\n",
       "   p =     -0.5 0.165195 0.134805)"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# %load tp1/generated/invgeom_check_final_placement\n",
    "pin.framesForwardKinematics(model,data,sol_q)\n",
    "print(\n",
    "    \"The robot finally reached effector placement at\\n\",\n",
    "    data.oMf[tool_id]\n",
    ")\n",
    "robot.placement(sol_q,6),data.oMi[6]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "  R =\n",
       "        1         0         0\n",
       "        0  0.707107 -0.707107\n",
       "        0  0.707107  0.707107\n",
       "  p = -0.5  0.1  0.2"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "in_world_M_target"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([ 3.44169138e-15,  3.05311332e-15, -2.28983499e-15, -3.33066907e-16,\n",
       "         3.33066907e-16,  7.45793918e-16]),\n",
       " array([ 3.44169138e-15,  3.05311332e-15, -2.28983499e-15, -3.33066907e-16,\n",
       "         3.33066907e-16,  7.45793918e-16]))"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "opti.value(error_tool(var_q)),pin.log(data.oMf[tool_id].inverse()*in_world_M_target).vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-3.77728452,  1.25943505, -2.44804804, -0.91798303,  0.96552974,\n",
       "        2.37748027])"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol_q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.SE3.__repr__=pin.SE3.__str__"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Probleme plus complexe"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'opti' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[57], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mdel\u001b[39;00m var_q,opti,var_tau,var_f,cq,ctau,cf\n",
      "\u001b[0;31mNameError\u001b[0m: name 'opti' is not defined"
     ]
    }
   ],
   "source": [
    "del var_q,opti,var_tau,var_f,cq,ctau,cf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 108,
   "metadata": {},
   "outputs": [],
   "source": [
    "opti2 = casadi.Opti()\n",
    "var_q = opti2.variable(model.nq)\n",
    "var_tau = opti2.variable(model.nq)\n",
    "var_f = opti2.variable(6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 109,
   "metadata": {},
   "outputs": [],
   "source": [
    "cq = casadi.SX.sym(\"cq2\",cmodel.nq,1)\n",
    "ctau = casadi.SX.sym(\"ctau\",cmodel.nq,1)\n",
    "cf = casadi.SX.sym(\"cf\",6,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [],
   "source": [
    "cpin.framesForwardKinematics(cmodel,cdata,cq)\n",
    "cpin.computeGeneralizedGravity(cmodel,cdata,cq)\n",
    "cpin.computeJointJacobians(cmodel,cdata);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 111,
   "metadata": {},
   "outputs": [],
   "source": [
    "Jtool = cpin.getFrameJacobian(cmodel,cdata,tool_id,pin.LOCAL_WORLD_ALIGNED)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 112,
   "metadata": {},
   "outputs": [],
   "source": [
    "dyncst = casadi.Function(\n",
    "    \"dyn\",\n",
    "    [cq,ctau,cf],\n",
    "    [cdata.g - ctau - Jtool.T@cf]\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 113,
   "metadata": {},
   "outputs": [],
   "source": [
    "error3 = casadi.Function(\"e3\",[cq],\n",
    "                         [ cdata.oMf[tool_id].translation-\n",
    "                          in_world_M_target.translation ])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 114,
   "metadata": {},
   "outputs": [],
   "source": [
    "fdes = np.array([ 10,0,0, 0,0,0])\n",
    "totalcost = casadi.sumsqr(var_f-fdes) + 1e-3*casadi.sumsqr(var_tau)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 115,
   "metadata": {},
   "outputs": [],
   "source": [
    "opti2.subject_to( error3(var_q) == 0)\n",
    "opti2.subject_to( dyncst(var_q,var_tau,var_f) == 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 116,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "This is Ipopt version 3.14.13, running with linear solver MUMPS 5.2.1.\n",
      "\n",
      "Number of nonzeros in equality constraint Jacobian...:       84\n",
      "Number of nonzeros in inequality constraint Jacobian.:        0\n",
      "Number of nonzeros in Lagrangian Hessian.............:       61\n",
      "\n",
      "Total number of variables............................:       18\n",
      "                     variables with only lower bounds:        0\n",
      "                variables with lower and upper bounds:        0\n",
      "                     variables with only upper bounds:        0\n",
      "Total number of equality constraints.................:        9\n",
      "Total number of inequality constraints...............:        0\n",
      "        inequality constraints with only lower bounds:        0\n",
      "   inequality constraints with lower and upper bounds:        0\n",
      "        inequality constraints with only upper bounds:        0\n",
      "\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "   0  1.0000000e+02 1.21e+02 1.94e+01  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0\n",
      "   1  1.0147618e+03 1.72e+01 1.00e+03  -1.0 6.95e+01   0.0 1.00e+00 1.00e+00h  1\n",
      "   2  9.1640315e+02 3.92e+01 3.55e+04  -1.0 3.39e+01   2.2 1.00e+00 2.50e-01f  3\n",
      "   3  3.9039486e+02 3.89e+01 2.47e+06  -1.0 4.66e+01   2.7 1.00e+00 1.00e+00f  1\n",
      "   4  5.9863888e+02 1.00e+01 8.42e+06  -1.0 7.01e+00   4.9 1.00e+00 1.00e+00h  1\n",
      "   5  5.8052918e+02 1.98e+01 3.00e+07  -1.0 1.22e+01   5.3 1.00e+00 5.00e-01f  2\n",
      "   6  1.6129378e+02 6.28e+00 7.65e+08  -1.0 1.38e+01   5.7 1.00e+00 1.00e+00f  1\n",
      "   7  2.8871527e+02 6.38e+00 2.07e+10  -1.0 2.79e+01   8.0 1.00e+00 2.50e-01h  3\n",
      "   8  2.9806576e+02 8.82e+00 5.43e+10  -1.0 4.04e+02   7.5 1.00e+00 3.12e-02h  6\n",
      "   9  2.8622819e+02 8.54e+00 3.04e+10  -1.0 4.73e+01   7.9 1.00e+00 6.25e-02f  5\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  10  4.2445425e+02 1.02e+01 3.47e+11  -1.0 2.23e+02   7.4 1.00e+00 3.12e-02h  6\n",
      "  11  4.0626780e+02 9.56e+00 4.37e+11  -1.0 3.91e+01   7.9 1.00e+00 1.25e-01f  4\n",
      "  12  2.4102393e+02 9.47e+00 4.79e+11  -1.0 7.96e+02   7.4 1.00e+00 1.56e-02f  7\n",
      "  13  2.3772645e+02 9.31e+00 4.42e+11  -1.0 9.35e+01   7.8 1.00e+00 3.12e-02f  6\n",
      "  14  2.4189184e+02 8.85e+00 3.44e+11  -1.0 4.10e+01   8.2 1.00e+00 6.25e-02h  5\n",
      "  15  2.4252393e+02 8.41e+00 2.86e+11  -1.0 4.03e+01   7.8 1.00e+00 6.25e-02h  5\n",
      "  16  2.9283518e+02 7.77e+00 2.77e+11  -1.0 2.73e+01   8.2 1.00e+00 5.00e-01h  2\n",
      "  17  4.4547638e+03 6.61e+01 6.43e+14  -1.0 4.33e+01  10.4 1.00e+00 1.00e+00w  1\n",
      "  18  5.7555568e+03 1.31e+02 8.63e+14  -1.0 6.42e+01  10.9 1.00e+00 1.00e+00w  1\n",
      "  19  3.3571769e+03 1.10e+02 5.41e+15  -1.0 3.84e+01  12.2 1.00e+00 1.00e+00w  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  20  3.0185708e+02 7.81e+00 2.74e+11  -1.0 1.13e+02  12.6 1.00e+00 7.81e-03h  7\n",
      "  21  1.3426764e+02 2.81e+01 8.46e+15  -1.0 5.99e+01  12.1 1.00e+00 1.00e+00H  1\n",
      "  22  8.9793470e+01 1.79e+01 2.21e+16  -1.0 1.39e+01  13.5 1.00e+00 1.00e+00f  1\n",
      "  23  7.6024019e+02 8.62e+00 9.89e+16  -1.0 1.58e+01  13.9 1.00e+00 1.00e+00H  1\n",
      "  24  7.6934125e+02 8.84e+00 9.49e+16  -1.0 1.42e+02  14.3 1.00e+00 3.12e-02h  6\n",
      "  25  7.5826711e+02 7.91e+00 1.17e+17  -1.0 1.32e+02  14.7 1.00e+00 6.25e-02f  5\n",
      "  26  9.4311717e+02 7.33e+00 1.33e+17  -1.0 8.48e+02  14.3 1.00e+00 3.91e-03h  9\n",
      "  27  8.0336186e+02 7.09e+00 1.18e+17  -1.0 8.59e+02  14.7 1.00e+00 3.91e-03f  9\n",
      "  28  8.0693507e+02 7.08e+00 1.18e+17  -1.0 6.11e+02  15.1 1.00e+00 1.22e-04h 14\n",
      "  29r 8.0693507e+02 7.08e+00 9.99e+02   0.9 0.00e+00  14.6 0.00e+00 4.77e-07R 22\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  30r 6.2808392e+02 1.72e+00 9.96e+02   0.9 8.65e+03    -  4.61e-02 1.08e-03f  1\n",
      "  31  6.2808412e+02 1.72e+00 1.51e+10  -1.0 1.05e-04  14.2 1.00e+00 1.00e+00h  1\n",
      "  32  6.2993741e+02 1.72e+00 5.63e+12  -1.0 2.22e+02  13.7 1.00e+00 2.44e-04h 13\n",
      "  33  6.3184330e+02 1.72e+00 8.05e+12  -1.0 2.33e+02  13.2 1.00e+00 2.44e-04h 13\n",
      "  34  6.3375263e+02 1.72e+00 9.30e+12  -1.0 2.46e+02  12.7 1.00e+00 2.44e-04h 13\n",
      "  35  6.3549465e+02 1.72e+00 1.02e+13  -1.0 2.62e+02  12.3 1.00e+00 2.44e-04h 13\n",
      "  36  6.3754818e+02 1.71e+00 1.32e+13  -1.0 2.84e+02  11.8 1.00e+00 4.88e-04h 12\n",
      "  37  6.1674808e+02 1.57e+00 9.70e+14  -1.0 6.55e+02  11.3 1.00e+00 7.81e-03f  8\n",
      "  38  7.5750158e+02 1.67e+00 7.39e+14  -1.0 2.20e+03  10.8 1.00e+00 1.95e-03h 10\n",
      "  39r 7.5750158e+02 1.67e+00 9.99e+02   0.2 0.00e+00  10.3 0.00e+00 4.77e-07R 22\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  40r 7.4522188e+02 5.62e-01 9.84e+02   0.2 1.67e+03    -  1.00e+00 9.91e-04f  1\n",
      "  41  7.4509831e+02 5.66e-01 1.91e+08  -1.0 2.59e-02   9.9 1.00e+00 1.00e+00h  1\n",
      "  42  5.7832464e+02 4.05e+00 1.19e+13  -1.0 9.09e+02   9.4 1.00e+00 7.81e-03f  8\n",
      "  43  5.8090176e+02 4.05e+00 1.19e+13  -1.0 1.53e+02   8.9 1.00e+00 4.88e-04h 12\n",
      "  44  5.8390262e+02 4.05e+00 1.19e+13  -1.0 4.52e+02   8.4 1.00e+00 2.44e-04h 13\n",
      "  45  5.8608473e+02 4.05e+00 1.19e+13  -1.0 1.59e+03   8.0 1.00e+00 6.10e-05h 15\n",
      "  46  5.8956660e+02 4.05e+00 1.19e+13  -1.0 5.16e+02   8.4 1.00e+00 2.44e-04h 13\n",
      "  47  5.9086932e+02 4.05e+00 1.19e+13  -1.0 1.97e+03   7.9 1.00e+00 3.05e-05h 16\n",
      "  48  5.9489415e+02 4.05e+00 1.19e+13  -1.0 5.88e+02   8.3 1.00e+00 2.44e-04h 13\n",
      "  49r 5.9489415e+02 4.05e+00 9.99e+02   0.6 0.00e+00   7.9 0.00e+00 4.77e-07R 22\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  50r 6.0722340e+02 7.04e-01 9.99e+02   0.6 4.01e+03    -  1.26e-01 9.99e-04f  1\n",
      "  51  6.0748869e+02 5.83e-01 4.06e+06  -1.0 8.43e-02   7.4 1.00e+00 1.00e+00h  1\n",
      "  52  6.0777558e+02 5.83e-01 4.06e+06  -1.0 2.74e+01   6.9 1.00e+00 2.44e-04h 13\n",
      "  53  6.0805916e+02 5.83e-01 4.05e+06  -1.0 2.84e+01   6.4 1.00e+00 2.44e-04h 13\n",
      "  54  6.0832385e+02 5.83e-01 4.05e+06  -1.0 3.07e+01   5.9 1.00e+00 2.44e-04h 13\n",
      "  55  6.0872357e+02 5.83e-01 4.04e+06  -1.0 3.78e+01   5.5 1.00e+00 4.88e-04h 12\n",
      "  56  6.0978099e+02 5.80e-01 3.62e+06  -1.0 2.52e+02   5.0 1.00e+00 3.91e-03h  9\n",
      "  57  6.0999795e+02 5.80e-01 3.62e+06  -1.0 4.59e+01   5.4 1.00e+00 2.44e-04h 13\n",
      "  58  6.1033153e+02 5.79e-01 3.51e+06  -1.0 4.17e+02   4.9 1.00e+00 1.95e-03h 10\n",
      "  59  6.1118348e+02 5.79e-01 3.46e+06  -1.0 5.49e+01   5.4 1.00e+00 9.77e-04h 11\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  60  6.1115352e+02 5.79e-01 3.46e+06  -1.0 9.19e+02   4.9 1.00e+00 6.10e-05f 15\n",
      "  61  6.1156352e+02 5.78e-01 3.45e+06  -1.0 6.72e+01   5.3 1.00e+00 4.88e-04h 12\n",
      "  62  2.7437406e+08 1.18e+04 3.73e+12  -1.0 1.62e+04   4.8 1.00e+00 1.00e+00w  1\n",
      "  63  1.4077774e+08 7.33e+03 1.65e+12  -1.0 9.92e+03   4.4 1.00e+00 1.00e+00w  1\n",
      "  64  4.1099167e+07 7.98e+03 1.48e+12  -1.0 4.74e+03   4.8 1.00e+00 1.00e+00w  1\n",
      "  65  6.1155844e+02 5.78e-01 3.45e+06  -1.0 2.68e+03   7.0 1.00e+00 9.54e-07f 20\n",
      "  66  6.1233207e+02 5.78e-01 3.41e+06  -1.0 4.24e+01   6.5 1.00e+00 4.88e-04h 12\n",
      "  67  6.1308063e+02 5.78e-01 3.39e+06  -1.0 4.51e+01   6.1 1.00e+00 4.88e-04h 12\n",
      "  68  6.1369913e+02 5.78e-01 3.37e+06  -1.0 4.88e+01   5.6 1.00e+00 4.88e-04h 12\n",
      "  69  6.1378475e+02 5.77e-01 3.37e+06  -1.0 1.70e+02   5.1 1.00e+00 2.44e-04h 13\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  70  6.1439993e+02 5.77e-01 3.35e+06  -1.0 5.15e+01   5.5 1.00e+00 4.88e-04h 12\n",
      "  71  6.1441952e+02 5.77e-01 3.35e+06  -1.0 2.24e+02   5.1 1.00e+00 1.22e-04h 14\n",
      "  72  6.1472047e+02 5.77e-01 3.34e+06  -1.0 5.39e+01   5.5 1.00e+00 2.44e-04h 13\n",
      "  73  6.1471173e+02 5.77e-01 3.34e+06  -1.0 3.02e+02   5.0 1.00e+00 1.22e-04f 14\n",
      "  74  6.1499863e+02 5.77e-01 3.33e+06  -1.0 5.85e+01   5.4 1.00e+00 2.44e-04h 13\n",
      "  75  2.0443003e+05 3.87e+02 7.94e+09  -1.0 4.39e+02   5.0 1.00e+00 1.00e+00w  1\n",
      "  76  5.5350308e+04 1.02e+02 4.66e+08  -1.0 2.65e+02   4.5 1.00e+00 1.00e+00w  1\n",
      "  77  6.3965302e+04 2.47e+02 1.62e+10  -1.0 6.36e+01   5.8 1.00e+00 1.00e+00w  1\n",
      "  78  6.1495432e+02 5.77e-01 3.33e+06  -1.0 8.77e+01   6.2 1.00e+00 1.22e-04f 13\n",
      "  79  6.1534346e+02 5.76e-01 3.33e+06  -1.0 5.52e+01   5.8 1.00e+00 2.44e-04h 13\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  80  6.1556075e+02 5.76e-01 3.32e+06  -1.0 9.57e+01   5.3 1.00e+00 2.44e-04h 13\n",
      "  81  6.1595031e+02 5.76e-01 3.31e+06  -1.0 5.75e+01   5.7 1.00e+00 2.44e-04h 13\n",
      "  82  6.1613872e+02 5.76e-01 3.31e+06  -1.0 1.17e+02   5.2 1.00e+00 2.44e-04h 13\n",
      "  83  6.1652671e+02 5.76e-01 3.30e+06  -1.0 5.98e+01   5.7 1.00e+00 2.44e-04h 13\n",
      "  84  6.1667918e+02 5.76e-01 3.29e+06  -1.0 1.43e+02   5.2 1.00e+00 2.44e-04h 13\n",
      "  85  6.1706318e+02 5.76e-01 3.29e+06  -1.0 6.22e+01   5.6 1.00e+00 2.44e-04h 13\n",
      "  86  6.1717122e+02 5.75e-01 3.28e+06  -1.0 1.78e+02   5.1 1.00e+00 2.44e-04h 13\n",
      "  87  6.1754837e+02 5.75e-01 3.27e+06  -1.0 6.47e+01   5.6 1.00e+00 2.44e-04h 13\n",
      "  88  5.8625558e+04 2.03e+02 7.81e+09  -1.0 2.26e+02   5.1 1.00e+00 1.00e+00w  1\n",
      "  89  3.6823835e+04 9.27e+01 4.43e+09  -1.0 1.72e+02   5.5 1.00e+00 1.00e+00w  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      "  90  5.9071707e+04 2.09e+02 5.83e+11  -1.0 5.18e+01   6.8 1.00e+00 1.00e+00w  1\n",
      "  91  6.1757412e+02 5.75e-01 3.27e+06  -1.0 9.91e+01   7.3 1.00e+00 1.22e-04h 13\n",
      "  92  6.1811901e+02 5.75e-01 3.24e+06  -1.0 6.26e+01   6.8 1.00e+00 2.44e-04h 13\n",
      "  93  6.1865126e+02 5.75e-01 3.22e+06  -1.0 6.47e+01   6.3 1.00e+00 2.44e-04h 13\n",
      "  94  6.1912452e+02 5.75e-01 3.21e+06  -1.0 6.78e+01   5.8 1.00e+00 2.44e-04h 13\n",
      "  95  6.1940137e+02 5.75e-01 3.20e+06  -1.0 9.72e+01   5.3 1.00e+00 2.44e-04h 13\n",
      "  96  6.1933956e+02 5.75e-01 3.20e+06  -1.0 1.19e+03   4.9 1.00e+00 3.05e-05f 16\n",
      "  97  6.1958135e+02 5.75e-01 3.19e+06  -1.0 1.15e+02   5.3 1.00e+00 2.44e-04h 13\n",
      "  98  6.1954310e+02 5.75e-01 3.19e+06  -1.0 4.14e+03   4.8 1.00e+00 7.63e-06f 18\n",
      "  99  6.1974333e+02 5.74e-01 3.18e+06  -1.0 1.36e+02   5.2 1.00e+00 2.44e-04h 13\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 100  6.2019995e+02 5.74e-01 3.17e+06  -1.0 7.41e+01   5.7 1.00e+00 2.44e-04h 13\n",
      " 101  3.4534065e+04 1.28e+02 1.55e+10  -1.0 1.66e+02   5.2 1.00e+00 1.00e+00w  1\n",
      " 102  1.9860429e+05 3.87e+02 3.85e+10  -1.0 2.44e+02   5.6 1.00e+00 1.00e+00w  1\n",
      " 103  7.3457688e+04 8.63e+01 8.56e+11  -1.0 2.31e+02   7.0 1.00e+00 1.00e+00w  1\n",
      " 104  6.2035202e+02 5.74e-01 3.16e+06  -1.0 3.57e+02   7.4 1.00e+00 2.44e-04h 12\n",
      " 105  6.2097706e+02 5.74e-01 3.10e+06  -1.0 7.32e+01   6.9 1.00e+00 2.44e-04h 13\n",
      " 106  6.2159193e+02 5.74e-01 3.07e+06  -1.0 7.55e+01   6.4 1.00e+00 2.44e-04h 13\n",
      " 107  6.2215029e+02 5.74e-01 3.05e+06  -1.0 7.91e+01   5.9 1.00e+00 2.44e-04h 13\n",
      " 108  6.2251406e+02 5.74e-01 3.04e+06  -1.0 8.67e+01   5.5 1.00e+00 2.44e-04h 13\n",
      " 109  6.2244254e+02 5.74e-01 3.04e+06  -1.0 4.83e+02   5.0 1.00e+00 6.10e-05f 15\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 110  6.2277374e+02 5.73e-01 3.02e+06  -1.0 1.01e+02   5.4 1.00e+00 2.44e-04h 13\n",
      " 111  6.2266180e+02 5.73e-01 3.02e+06  -1.0 6.73e+02   4.9 1.00e+00 6.10e-05f 15\n",
      " 112  6.2295348e+02 5.73e-01 3.01e+06  -1.0 1.18e+02   5.4 1.00e+00 2.44e-04h 13\n",
      " 113  6.2286853e+02 5.73e-01 3.00e+06  -1.0 1.03e+03   4.9 1.00e+00 3.05e-05f 16\n",
      " 114  2.6842135e+04 1.31e+02 3.04e+10  -1.0 1.38e+02   5.3 1.00e+00 1.00e+00w  1\n",
      " 115  3.4147894e+05 4.81e+02 6.46e+10  -1.0 4.91e+02   4.8 1.00e+00 1.00e+00w  1\n",
      " 116  1.7859786e+07 4.92e+03 7.10e+12  -1.0 4.18e+03   5.3 1.00e+00 1.00e+00w  1\n",
      " 117  6.2311273e+02 5.73e-01 2.99e+06  -1.0 8.67e+03   5.7 1.00e+00 2.44e-04h 12\n",
      " 118  6.2317269e+02 5.73e-01 2.98e+06  -1.0 1.92e+02   5.2 1.00e+00 1.22e-04h 14\n",
      " 119  6.2367213e+02 5.73e-01 2.96e+06  -1.0 9.17e+01   5.6 1.00e+00 2.44e-04h 13\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 120  6.2369216e+02 5.73e-01 2.96e+06  -1.0 2.34e+02   5.2 1.00e+00 1.22e-04h 14\n",
      " 121  6.2417480e+02 5.73e-01 2.94e+06  -1.0 9.52e+01   5.6 1.00e+00 2.44e-04h 13\n",
      " 122  6.2414623e+02 5.73e-01 2.93e+06  -1.0 2.87e+02   5.1 1.00e+00 1.22e-04f 14\n",
      " 123  6.2460656e+02 5.72e-01 2.91e+06  -1.0 9.88e+01   5.5 1.00e+00 2.44e-04h 13\n",
      " 124  6.2451868e+02 5.72e-01 2.90e+06  -1.0 3.57e+02   5.1 1.00e+00 1.22e-04f 14\n",
      " 125  6.2495019e+02 5.72e-01 2.88e+06  -1.0 1.03e+02   5.5 1.00e+00 2.44e-04h 13\n",
      " 126  6.2486902e+02 5.72e-01 2.88e+06  -1.0 4.53e+02   5.0 1.00e+00 6.10e-05f 15\n",
      " 127  2.1140285e+04 7.72e+01 2.51e+10  -1.0 1.11e+02   5.4 1.00e+00 1.00e+00w  1\n",
      " 128  3.0311871e+05 4.47e+02 1.42e+11  -1.0 6.16e+02   5.9 1.00e+00 1.00e+00w  1\n",
      " 129  4.4867174e+04 3.11e+02 2.57e+11  -1.0 3.85e+02   6.3 1.00e+00 1.00e+00w  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 130  6.2526307e+02 5.72e-01 2.85e+06  -1.0 1.53e+02   8.5 1.00e+00 2.44e-04h 12\n",
      " 131  6.2609657e+02 5.72e-01 4.49e+06  -1.0 1.00e+02   8.0 1.00e+00 2.44e-04h 13\n",
      " 132  6.2694369e+02 5.72e-01 5.22e+06  -1.0 1.03e+02   7.6 1.00e+00 2.44e-04h 13\n",
      " 133  6.2779015e+02 5.72e-01 5.47e+06  -1.0 1.07e+02   7.1 1.00e+00 2.44e-04h 13\n",
      " 134  6.2859070e+02 5.71e-01 5.55e+06  -1.0 1.12e+02   6.6 1.00e+00 2.44e-04h 13\n",
      " 135  6.2920781e+02 5.71e-01 5.58e+06  -1.0 1.20e+02   6.1 1.00e+00 2.44e-04h 13\n",
      " 136  6.2923136e+02 5.71e-01 5.58e+06  -1.0 2.04e+02   5.7 1.00e+00 2.44e-04h 13\n",
      " 137  6.2898858e+02 5.71e-01 5.58e+06  -1.0 1.10e+03   5.2 1.00e+00 3.05e-05f 16\n",
      " 138  6.2889254e+02 5.71e-01 5.59e+06  -1.0 2.37e+02   5.6 1.00e+00 2.44e-04f 13\n",
      " 139  6.2858778e+02 5.71e-01 5.59e+06  -1.0 1.49e+03   5.1 1.00e+00 3.05e-05f 16\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 140  9.5902211e+04 3.22e+02 1.02e+11  -1.0 2.77e+02   5.6 1.00e+00 1.00e+00w  1\n",
      " 141  6.1318550e+04 1.90e+02 3.08e+10  -1.0 2.26e+02   5.1 1.00e+00 1.00e+00w  1\n",
      " 142  3.9567327e+04 4.66e+02 7.31e+10  -1.0 3.01e+02   4.6 1.00e+00 1.00e+00w  1\n",
      " 143  6.2846813e+02 5.71e-01 5.59e+06  -1.0 2.30e+02   5.0 1.00e+00 1.22e-04f 13\n",
      " 144  6.2818392e+02 5.71e-01 5.59e+06  -1.0 3.77e+02   5.5 1.00e+00 1.22e-04f 14\n",
      " 145  6.2703770e+02 5.71e-01 5.58e+06  -1.0 1.87e+04   5.0 1.00e+00 6.10e-05f 15\n",
      " 146  6.2648670e+02 1.06e+00 4.67e+08  -1.0 3.98e+02   5.4 1.00e+00 1.56e-02f  7\n",
      " 147r 6.2648670e+02 1.06e+00 9.99e+02   0.0 0.00e+00   4.9 0.00e+00 4.77e-07R 22\n",
      " 148r 6.1380005e+02 5.64e-01 9.84e+02   0.0 1.06e+03    -  1.00e+00 9.91e-04f  1\n",
      " 149  6.6504356e+02 5.37e-01 1.09e+06  -1.0 7.06e+00   4.4 1.00e+00 2.50e-01h  3\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 150  6.6693397e+02 5.36e-01 1.09e+06  -1.0 9.68e+01   4.0 1.00e+00 9.77e-04h 11\n",
      " 151  6.6742778e+02 5.36e-01 1.09e+06  -1.0 1.39e+02   3.5 1.00e+00 4.88e-04h 12\n",
      " 152  6.6740033e+02 5.36e-01 1.09e+06  -1.0 2.55e+02   3.0 1.00e+00 2.44e-04f 13\n",
      " 153  6.6782310e+02 5.36e-01 1.09e+06  -1.0 1.47e+02   3.4 1.00e+00 4.88e-04h 12\n",
      " 154  6.6791037e+02 5.36e-01 1.09e+06  -1.0 2.74e+02   3.0 1.00e+00 1.22e-04h 14\n",
      " 155  6.6825686e+02 5.36e-01 1.09e+06  -1.0 1.56e+02   3.4 1.00e+00 4.88e-04h 12\n",
      " 156  6.6854330e+02 5.35e-01 1.09e+06  -1.0 3.69e+02   2.9 1.00e+00 6.10e-05h 15\n",
      " 157  6.6867375e+02 5.35e-01 1.09e+06  -1.0 1.65e+02   3.3 1.00e+00 2.44e-04h 13\n",
      " 158  6.7025817e+02 5.35e-01 1.08e+06  -1.0 1.11e+02   3.8 1.00e+00 9.77e-04h 11\n",
      " 159  5.0985589e+04 1.30e+02 5.04e+07  -1.0 1.77e+02   3.3 1.00e+00 1.00e+00w  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 160  1.4715497e+05 2.58e+02 1.82e+09  -1.0 2.94e+02   3.7 1.00e+00 1.00e+00w  1\n",
      " 161  7.4652115e+04 6.04e+02 1.25e+09  -1.0 4.33e+02   4.1 1.00e+00 1.00e+00w  1\n",
      " 162  6.7033176e+02 5.35e-01 1.08e+06  -1.0 2.04e+02   6.4 1.00e+00 2.44e-04h 12\n",
      " 163  6.8038191e+02 5.33e-01 3.57e+06  -1.0 8.01e+01   5.9 1.00e+00 3.91e-03h  9\n",
      " 164  6.8907444e+02 5.31e-01 4.73e+06  -1.0 8.78e+01   5.4 1.00e+00 3.91e-03h  9\n",
      " 165  6.9175837e+02 5.30e-01 4.86e+06  -1.0 1.01e+02   4.9 1.00e+00 1.95e-03h 10\n",
      " 166  6.9118217e+02 5.29e-01 4.87e+06  -1.0 1.35e+02   4.5 1.00e+00 9.77e-04f 11\n",
      " 167  6.6362303e+02 5.27e-01 4.82e+06  -1.0 3.49e+02   4.0 1.00e+00 3.91e-03f  9\n",
      " 168  6.6015237e+02 5.25e-01 4.99e+06  -1.0 1.34e+02   4.4 1.00e+00 3.91e-03f  9\n",
      " 169  5.9372154e+02 1.61e+00 2.90e+06  -1.0 4.36e+02   3.9 1.00e+00 1.56e-02f  7\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 170  5.8553287e+02 2.47e+00 8.13e+06  -1.0 1.36e+02   4.4 1.00e+00 3.12e-02f  6\n",
      " 171  5.6700864e+02 2.96e+00 4.63e+06  -1.0 3.16e+02   3.9 1.00e+00 1.56e-02f  7\n",
      " 172  8.5359209e+03 1.02e+02 5.38e+08  -1.0 8.84e+01   4.3 1.00e+00 1.00e+00w  1\n",
      " 173  9.9403809e+02 2.60e+01 2.23e+09  -1.0 9.11e+01   5.6 1.00e+00 1.00e+00w  1\n",
      " 174  5.2508743e+03 1.02e+02 7.75e+09  -1.0 4.79e+01   6.1 1.00e+00 1.00e+00w  1\n",
      " 175  5.6224375e+02 3.15e+00 1.72e+06  -1.0 8.80e+01   7.4 1.00e+00 3.12e-02f  5\n",
      " 176  5.9019972e+02 2.63e+00 1.15e+09  -1.0 1.36e+01   6.9 1.00e+00 1.25e-01h  4\n",
      " 177  6.0620326e+02 2.57e+00 1.13e+09  -1.0 1.56e+01   6.4 1.00e+00 3.12e-02h  6\n",
      " 178  6.1102295e+02 2.55e+00 1.12e+09  -1.0 2.88e+01   6.0 1.00e+00 7.81e-03h  8\n",
      " 179  6.1269365e+02 2.54e+00 1.11e+09  -1.0 1.37e+01   6.4 1.00e+00 3.91e-03h  9\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 180  6.1330184e+02 2.53e+00 1.11e+09  -1.0 3.25e+01   5.9 1.00e+00 9.77e-04h 11\n",
      " 181  7.4280889e+02 2.71e+00 1.16e+09  -1.0 1.44e+01   6.3 1.00e+00 2.50e-01h  3\n",
      " 182  7.6083945e+02 2.28e+00 9.24e+08  -1.0 7.22e+00   6.8 1.00e+00 2.50e-01h  3\n",
      " 183  7.2447279e+02 2.11e+00 8.40e+08  -1.0 1.81e+01   6.3 1.00e+00 1.25e-01f  4\n",
      " 184  7.0401283e+02 1.83e+00 6.36e+08  -1.0 4.75e+00   6.7 1.00e+00 2.50e-01f  3\n",
      " 185  7.3497704e+02 1.42e+01 1.29e+09  -1.0 6.45e+00   7.1 1.00e+00 1.00e+00w  1\n",
      " 186  6.0290326e+03 8.29e+01 4.97e+10  -1.0 5.81e+01   6.7 1.00e+00 1.00e+00w  1\n",
      " 187  5.9690238e+03 7.26e+01 1.31e+12  -1.0 1.14e+02   8.0 1.00e+00 1.00e+00w  1\n",
      " 188  7.0235171e+02 2.34e+00 3.89e+08  -1.0 3.16e+02   9.3 1.00e+00 2.50e-01f  2\n",
      " 189  1.2549517e+03 1.85e+00 4.80e+11  -1.0 1.10e+01   8.8 1.00e+00 1.00e+00H  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 190  1.2467247e+03 1.66e+00 9.42e+10  -1.0 1.17e+00   9.3 1.00e+00 1.00e+00f  1\n",
      " 191  1.2584852e+03 1.55e-02 4.19e+09  -1.0 9.85e-01   8.8 1.00e+00 1.00e+00h  1\n",
      " 192  1.2584814e+03 6.92e-06 2.29e+07  -1.0 1.56e-02   8.3 1.00e+00 1.00e+00h  1\n",
      " 193  1.2584814e+03 8.95e-12 7.29e+02  -1.0 1.07e-05   7.8 1.00e+00 1.00e+00h  1\n",
      " 194  1.2584813e+03 1.24e-14 2.44e+01  -1.0 1.07e-06   7.4 1.00e+00 1.00e+00f  1\n",
      " 195  1.2584810e+03 1.63e-13 2.44e+01  -1.0 3.21e-06   6.9 1.00e+00 1.00e+00f  1\n",
      " 196  1.2584802e+03 1.12e-12 2.44e+01  -1.0 9.63e-06   6.4 1.00e+00 1.00e+00f  1\n",
      " 197  1.2584777e+03 1.03e-11 2.44e+01  -1.0 2.89e-05   5.9 1.00e+00 1.00e+00f  1\n",
      " 198  1.2584701e+03 9.24e-11 2.44e+01  -1.0 8.67e-05   5.4 1.00e+00 1.00e+00f  1\n",
      " 199  1.2584474e+03 8.31e-10 2.44e+01  -1.0 2.60e-04   5.0 1.00e+00 1.00e+00f  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 200  1.2583792e+03 7.48e-09 2.44e+01  -1.0 7.80e-04   4.5 1.00e+00 1.00e+00f  1\n",
      " 201  1.2581748e+03 6.73e-08 2.44e+01  -1.0 2.34e-03   4.0 1.00e+00 1.00e+00f  1\n",
      " 202  1.2575617e+03 6.06e-07 2.44e+01  -1.0 7.02e-03   3.5 1.00e+00 1.00e+00f  1\n",
      " 203  1.2557255e+03 5.44e-06 2.44e+01  -1.0 2.10e-02   3.1 1.00e+00 1.00e+00f  1\n",
      " 204  1.2502420e+03 4.88e-05 2.43e+01  -1.0 6.30e-02   2.6 1.00e+00 1.00e+00f  1\n",
      " 205  1.2340153e+03 4.33e-04 2.41e+01  -1.0 1.88e-01   2.1 1.00e+00 1.00e+00f  1\n",
      " 206  1.1872601e+03 3.76e-03 2.36e+01  -1.0 5.51e-01   1.6 1.00e+00 1.00e+00f  1\n",
      " 207  1.0622114e+03 3.04e-02 2.23e+01  -1.0 1.56e+00   1.2 1.00e+00 1.00e+00f  1\n",
      " 208  7.8448163e+02 2.07e-01 1.89e+01  -1.0 3.96e+00   0.7 1.00e+00 1.00e+00f  1\n",
      " 209  3.6114133e+02 1.42e+00 2.53e+01  -1.0 8.12e+00   0.2 1.00e+00 1.00e+00f  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 210  2.6503897e+02 4.86e-01 2.30e+02  -1.0 4.08e+00   0.6 1.00e+00 1.00e+00f  1\n",
      " 211  1.4969403e+02 1.77e-01 4.32e+01  -1.0 6.21e+00   0.1 1.00e+00 1.00e+00f  1\n",
      " 212  3.5655597e+01 6.76e-01 7.16e+00  -1.0 6.12e+00  -0.3 1.00e+00 1.00e+00f  1\n",
      " 213  2.4070970e+00 1.79e+00 6.24e+00  -1.0 4.37e+00  -0.8 1.00e+00 1.00e+00f  1\n",
      " 214  1.0332474e+00 7.71e-02 5.59e-01  -1.0 2.02e+00  -1.3 1.00e+00 1.00e+00f  1\n",
      " 215  1.0146770e+00 3.63e-04 1.26e-02  -1.7 1.99e-01  -1.8 1.00e+00 1.00e+00h  1\n",
      " 216  1.0142189e+00 5.96e-04 9.84e-04  -3.8 1.23e-01  -2.2 1.00e+00 1.00e+00h  1\n",
      " 217  1.0142026e+00 1.59e-04 2.00e-04  -5.7 1.03e-01  -2.7 1.00e+00 1.00e+00h  1\n",
      " 218  1.0142287e+00 1.51e-05 2.18e-05  -5.7 3.38e-02  -3.2 1.00e+00 1.00e+00h  1\n",
      " 219  1.0142321e+00 2.03e-07 8.45e-07  -5.7 3.93e-03  -3.7 1.00e+00 1.00e+00h  1\n",
      "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n",
      " 220  1.0142321e+00 3.28e-10 1.14e-08  -8.6 1.58e-04  -4.1 1.00e+00 1.00e+00h  1\n",
      " 221  1.0142321e+00 6.59e-14 3.85e-14  -9.0 2.18e-06    -  1.00e+00 1.00e+00h  1\n",
      "\n",
      "Number of Iterations....: 221\n",
      "\n",
      "                                   (scaled)                 (unscaled)\n",
      "Objective...............:   1.0142321412926760e+00    1.0142321412926760e+00\n",
      "Dual infeasibility......:   3.8531179025400245e-14    3.8531179025400245e-14\n",
      "Constraint violation....:   6.5947247662734299e-14    6.5947247662734299e-14\n",
      "Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00\n",
      "Complementarity.........:   0.0000000000000000e+00    0.0000000000000000e+00\n",
      "Overall NLP error.......:   6.5947247662734299e-14    6.5947247662734299e-14\n",
      "\n",
      "\n",
      "Number of objective function evaluations             = 1852\n",
      "Number of objective gradient evaluations             = 222\n",
      "Number of equality constraint evaluations            = 1852\n",
      "Number of inequality constraint evaluations          = 0\n",
      "Number of equality constraint Jacobian evaluations   = 226\n",
      "Number of inequality constraint Jacobian evaluations = 0\n",
      "Number of Lagrangian Hessian evaluations             = 221\n",
      "Total seconds in IPOPT                               = 23.785\n",
      "\n",
      "EXIT: Optimal Solution Found.\n",
      "      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval\n",
      "callback_fun  | 829.41ms (  3.80ms)  23.32 s (106.97ms)       218\n",
      "       nlp_f  |   4.16ms (  2.25us)   4.09ms (  2.21us)      1852\n",
      "       nlp_g  |  17.12ms (  9.24us)  16.57ms (  8.95us)      1852\n",
      "  nlp_grad_f  |   1.60ms (  7.17us)   1.41ms (  6.30us)       223\n",
      "  nlp_hess_l  |  46.64ms (214.94us)  47.01ms (216.62us)       217\n",
      "   nlp_jac_g  |  27.24ms (120.01us)  27.58ms (121.48us)       227\n",
      "       total  |   1.30 s (  1.30 s)  23.79 s ( 23.79 s)         1\n"
     ]
    }
   ],
   "source": [
    "opti2.minimize(totalcost)\n",
    "opti2.solver(\"ipopt\")  # select the backend solver\n",
    "opti2.callback(lambda i: displayScene(opti2.debug.value(var_q)))\n",
    "# %load tp1/generated/invgeom_solve\n",
    "# Caution: in case the solver does not converge, we are picking the candidate values\n",
    "# at the last iteration in opti.debug, and they are NO guarantee of what they mean.\n",
    "try:\n",
    "    sol = opti2.solve_limited()\n",
    "    sol_q = opti2.value(var_q)\n",
    "    sol_tau = opti2.value(var_tau)\n",
    "    sol_f = opti2.value(var_f)\n",
    "except:\n",
    "    print(\"ERROR in convergence, plotting debug info.\")\n",
    "    sol_q = opti2.debug.value(var_q)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 88,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-5.01657669e-14, -3.37612891e-15, -4.65346857e-15,  1.74134256e-13,\n",
       "        3.86103435e-14, -1.10845609e-14])"
      ]
     },
     "execution_count": 88,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol_tau"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 89,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 1.00000000e+01,  4.66634504e-19, -1.85092845e-13, -1.96103774e-16,\n",
       "        4.85834388e-13, -6.43526863e-17])"
      ]
     },
     "execution_count": 89,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol_f"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 103,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-2.33863269e-18, -1.44026113e-12, -7.52749008e-13,  8.19785795e-14,\n",
       "       -0.00000000e+00, -0.00000000e+00])"
      ]
     },
     "execution_count": 103,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "opti2.value(dyncst(var_q,0*var_tau,0*var_f))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 102,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 1.41371669e+01, -4.71238898e+00, -1.66256038e-14, -7.85398163e+00,\n",
       "       -2.82743339e+01,  2.56252579e+01])"
      ]
     },
     "execution_count": 102,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "opti2.value(var_q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 104,
   "metadata": {},
   "outputs": [],
   "source": [
    "grav = casadi.Function(\"grav\", [cq],[ cdata.g ])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 105,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-2.33863269e-18, -1.44026113e-12, -7.52749008e-13,  8.19785795e-14,\n",
       "        0.00000000e+00,  0.00000000e+00])"
      ]
     },
     "execution_count": 105,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "opti2.value(grav(var_q))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {},
   "outputs": [],
   "source": [
    "displayScene(sol_q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-2.33863269e-18, -1.44026113e-12, -7.52749008e-13,  8.19785795e-14,\n",
       "        0.00000000e+00,  0.00000000e+00])"
      ]
     },
     "execution_count": 107,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pin.computeGeneralizedGravity(model,data,sol_q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}