{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# My First Inverse Kinematic (IK)\n",
    "\n",
    "In this tutorial we will see how to use some [RBDyn](https://github.com/jorisv/RBDyn) algorithms and some Jacobians to compute the inverse kinematic.\n",
    "\n",
    "We will not speak about analytic IK that is kind of obsolet. Instead we will focus on gradient based numerical optimization IK.\n",
    "\n",
    "This tutorial will not be a course on gradient based numerical optimization.\n",
    "Reader than want to see more on this topic can read [Methodt for non-linear least squares problems](http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf) for an introduction.\n",
    "The [Numerical Optimization](http://www.springer.com/fr/book/9780387303031) book is also a great ressource for this topic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%gui wx\n",
    "from IPython.display import Image\n",
    "from tvtk.tools import ivtk\n",
    "from graph import Axis\n",
    "\n",
    "# create the viewer\n",
    "viewer = ivtk.viewer()\n",
    "viewer.size = (640, 480)\n",
    "\n",
    "viewer.scene.camera.focal_point = (0.138, 0.958, -0.205)\n",
    "viewer.scene.camera.position = (3.763, 1.435, 4.453)\n",
    "\n",
    "display_number = 1\n",
    "def display():\n",
    "    global display_number\n",
    "    path = 'img/MyFirstIK/%s.png' % display_number\n",
    "    display_number += 1\n",
    "    viewer.scene.save(path)\n",
    "    return Image(path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "TutorialTree structure:\n",
      "\n",
      "  Return the MultiBodyGraph, MultiBody and the zeroed MultiBodyConfig with the\n",
      "  following tree structure:\n",
      "\n",
      "                b4\n",
      "             j3 | Spherical\n",
      "  Root     j0   |   j1     j2     j4\n",
      "  ---- b0 ---- b1 ---- b2 ----b3 ----b5\n",
      "  Fixed    RevX   RevY    RevZ   PrismZ\n",
      "  \n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import eigen3 as e\n",
    "import spacevecalg as sva\n",
    "import rbdyn as rbd\n",
    "\n",
    "from robots import TutorialTree\n",
    "\n",
    "print 'TutorialTree structure:'\n",
    "print TutorialTree.__doc__\n",
    "\n",
    "# create a robot with the same structure than the one in the MultiBody tutorial\n",
    "mbg, mb, mbc = TutorialTree()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# draw the robot\n",
    "from graph import MultiBodyViz\n",
    "\n",
    "# add visualization of the of the b4 and b5 body that don't have any successors\n",
    "X_b4_ef = sva.PTransformd(sva.RotY(-np.pi/2.), e.Vector3d(0.2, 0., 0.))\n",
    "X_b5_ef = sva.PTransformd(sva.RotX(-np.pi/2.), e.Vector3d(0., 0.2, 0.))\n",
    "mbv = MultiBodyViz(mbg, mb, endEffectorDict={'b4':(X_b4_ef, 0.1, (1., 1., 0.)),\n",
    "                                             'b5':(X_b5_ef, 0.1, (1., 1., 0.))})\n",
    "\n",
    "mbv.addActors(viewer.scene)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## One Task IK\n",
    "\n",
    "We define a Task by the two following values:\n",
    " * $ \\mathbf{g}(\\mathbf{q}) $: An error\n",
    " * $ J_{\\mathbf{g}} $: The $ \\mathbf{g} $ error Jacobian $ \\frac{\\partial \\mathbf{g}(\\mathbf{q})}{\\partial \\mathbf{q}} $\n",
    " \n",
    "Our objective is to solve the following problem:\n",
    "$$\n",
    "\\underset{\\mathbf{q}}{\\text{minimize }} \\frac{1}{2} \\| \\mathbf{g}(\\mathbf{q}) \\|^2\n",
    "$$\n",
    "\n",
    "This is a non-linear least squares problem so we can use a Gauss-Newton like method.\n",
    "\n",
    "In this method we will compute a descent direction $ \\mathbf{\\alpha} $ by solving the following problem:\n",
    "$$\n",
    "(J_{\\mathbf{g}}^T J_{\\mathbf{g}}) \\mathbf{\\alpha} = -J_{\\mathbf{g}}^T \\mathbf{g}\n",
    "$$\n",
    "This problem can be solve by the following least square where $ ^{\\#} $ is the pseudoinverse operator:\n",
    "$$\n",
    "\\mathbf{\\alpha} = -J_{\\mathbf{g}}^{\\#} \\mathbf{g}\n",
    "$$\n",
    "Then we can integrate $ \\mathbf{\\alpha} $ to $ \\mathbf{q} $ with the following equation $ \\mathbf{q}_{k+1} = \\mathbf{q}_{k} + \\Delta_{st} \\mathbf{\\alpha}_{k} $ where $ \\Delta_{st} $ is the integration step. In this tutorial we will not make any line search and set $ \\Delta_{st} = 1 $.\n",
    "\n",
    "Since the size of $ \\mathbf{q} $ and $ \\mathbf{\\alpha} $ can be different because of some joint we will use the [eulerIntegration alogrithm](http://nbviewer.ipython.org/github/jorisv/sva_rbdyn_tutorials/blob/master/SomeAlgorithm.ipynb#Euler-Integration) to make this step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def oneTaskMin(mb, mbc, task, delta=1., maxIter=100, prec=1e-8):\n",
    "    q = e.toNumpy(rbd.paramToVector(mb, mbc.q))\n",
    "    iterate = 0\n",
    "    minimizer = False\n",
    "    while iterate < maxIter and not minimizer:\n",
    "        # compute task data\n",
    "        g = task.g(mb, mbc)\n",
    "        J = task.J(mb, mbc)\n",
    "        \n",
    "        # compute alpha\n",
    "        alpha = -np.mat(np.linalg.lstsq(J, g)[0])\n",
    "        \n",
    "        # integrate and run the forward kinematic\n",
    "        mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alpha))\n",
    "        rbd.eulerIntegration(mb, mbc, delta)\n",
    "        rbd.forwardKinematics(mb, mbc)\n",
    "        \n",
    "        # take the new q vector\n",
    "        q = e.toNumpy(rbd.paramToVector(mb, mbc.q))\n",
    "        \n",
    "        alphaInf = np.linalg.norm(alpha, np.inf)\n",
    "        yield iterate, q, alpha, alphaInf # yield the current state\n",
    "        \n",
    "        # check if the current alpha is a minimizer\n",
    "        if alphaInf < prec:\n",
    "            minimizer = True\n",
    "        iterate += 1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Body Task\n",
    "\n",
    "Our first task will allow to target a static frame $ {}^{T} X_{O} $ for a frame $ p $ on a body $ b $ noted $ {}^{p} X_{O} = {}^{p} X_{b} {}^{b} X_{O} $.\n",
    "The error function will be:\n",
    "$$\n",
    "\\mathbf{g}^{body} = sva.transformError({}^{T} X_{O}, {}^{p} X_{O})\n",
    "$$\n",
    "The `sva::transformError` function will return the error in the Origin orientation frame so the the jacobian also have te be in this frame:\n",
    "$$\n",
    "J_{\\mathbf{g}^{bdoy}} = \\frac{\\partial plx({}^{O} E_{p}) {}^{p} X_{O}(\\mathbf{q})}{\\partial \\mathbf{q}}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "class BodyTask(object):\n",
    "    def __init__(self, mb, bodyId, X_O_T, X_b_p=sva.PTransformd.Identity()):\n",
    "        \"\"\"\n",
    "        Compute the error and the jacobian to target a static frame for a body.\n",
    "        Parameters: \n",
    "            - mb: MultiBody\n",
    "            - bodyId: ID of the body that should move\n",
    "            - X_0_T: targeted frame (PTransformd)\n",
    "            - X_b_p: static frame on the body bodyId\n",
    "        \"\"\"\n",
    "        self.bodyIndex = mb.bodyIndexById(bodyId)\n",
    "        self.X_O_T = X_O_T\n",
    "        self.X_b_p = X_b_p\n",
    "        self.jac = rbd.Jacobian(mb, bodyId)\n",
    "        self.jac_mat_sparse = e.MatrixXd(6, mb.nrDof())\n",
    "            \n",
    "    def X_O_p(self, mbc):\n",
    "        X_O_b = list(mbc.bodyPosW)[self.bodyIndex]\n",
    "        return self.X_b_p*X_O_b\n",
    "        \n",
    "    def g(self, mb, mbc):\n",
    "        X_O_p = self.X_O_p(mbc)\n",
    "        g_body = sva.transformError(self.X_O_T, X_O_p)\n",
    "        return e.toNumpy(g_body.vector())\n",
    "        \n",
    "    def J(self, mb, mbc):\n",
    "        X_O_p = self.X_O_p(mbc)\n",
    "        # set transformation in Origin orientation frame\n",
    "        X_O_p_O = sva.PTransformd(X_O_p.rotation()).inv()*X_O_p\n",
    "        jac_mat_dense = self.jac.jacobian(mb, mbc, X_O_p_O)\n",
    "        self.jac.fullJacobian(mb, jac_mat_dense, self.jac_mat_sparse)\n",
    "        return e.toNumpy(self.jac_mat_sparse)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Configuration\n",
    "\n",
    "We will target a frame $ {}^{T} X_O $ that the TutorialTree robot can reach with the body b5 end effector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYkAAAGnCAIAAADNGTysAAAcPElEQVR4nO3dbWxc133n8R8lUZL1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\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Set an initial configuration\n",
    "mbcIK = rbd.MultiBodyConfig(mbc)\n",
    "quat = e.Quaterniond(np.pi/3., e.Vector3d(0.1, 0.5, 0.3).normalized())\n",
    "mbcIK.q = [[],\n",
    "           [3.*np.pi/4.],\n",
    "           [np.pi/3.],\n",
    "           [-3.*np.pi/4.],\n",
    "           [0.],\n",
    "           [quat.w(), quat.x(), quat.y(), quat.z()]]\n",
    "\n",
    "rbd.forwardKinematics(mb, mbcIK)\n",
    "rbd.forwardVelocity(mb, mbcIK) # for motionSubspace\n",
    "\n",
    "# target frame\n",
    "X_O_T = sva.PTransformd(sva.RotY(np.pi/2.), e.Vector3d(1.5, 0.5, 1.))\n",
    "\n",
    "# create the task\n",
    "bodyTask = BodyTask(mb, mbg.bodyIdByName(\"b5\"), X_O_T, X_b5_ef)\n",
    "\n",
    "# display initial configuration\n",
    "from graph import Axis\n",
    "axis_b5 = Axis(bodyTask.X_O_p(mbcIK), length=0.3, text='b5')\n",
    "axis_T = Axis(X_O_T, length=0.5, text='Target')\n",
    "axis_b5.addActors(viewer.scene)\n",
    "axis_T.addActors(viewer.scene)\n",
    "\n",
    "mbv.display(mb, mbcIK)\n",
    "display()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Result\n",
    "\n",
    "The result is reached in 7 iterations !"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "g_body translation error: [[  2.22044605e-16   1.11022302e-16   1.35963107e-32]]\n",
      "g_body rotation error: [[ -6.12323400e-17   0.00000000e+00  -9.72006753e-18]]\n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYkAAAGnCAIAAADNGTysAAAYd0lEQVR4nO3dXWxc533n8R9JUe/i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\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# copy the initial configuration to avoid the algorithm to change it\n",
    "mbcIKSolve = rbd.MultiBodyConfig(mbcIK)\n",
    "q_res = None\n",
    "X_O_p_res = None\n",
    "alphaInfList = []\n",
    "for iterate, q, alpha, alphaInf in oneTaskMin(mb, mbcIKSolve, bodyTask, delta=1., maxIter=200, prec=1e-8):\n",
    "    X_O_p = bodyTask.X_O_p(mbcIKSolve)\n",
    "    q_res = q\n",
    "    alphaInfList.append(alphaInf)\n",
    "    \n",
    "mbcIKResult = rbd.MultiBodyConfig(mbcIK)\n",
    "mbcIKResult.q = rbd.vectorToParam(mb, e.toEigenX(q_res))\n",
    "rbd.forwardKinematics(mb, mbcIKResult)\n",
    "\n",
    "g_body = bodyTask.g(mb, mbcIKResult)\n",
    "print 'g_body translation error:', g_body[3:].T\n",
    "print 'g_body rotation error:', g_body[:3].T\n",
    "\n",
    "axis_b5.X = X_O_p\n",
    "mbv.display(mb, mbcIKResult)\n",
    "display()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.text.Text at 0x7fedb2deee10>"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYEAAAEPCAYAAACk43iMAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7fedbac32f50>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "plt.plot(alphaInfList)\n",
    "plt.ylabel('alphaInf')\n",
    "plt.xlabel('iterate')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Multi Task IK"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "This formulation has some flaw.\n",
    "The BodyTask only give a target for the body b5, so the optimization process is free to move b4.\n",
    "\n",
    "Another issue with the mono task IK is the Jacobian singularity. If at one point the BodyTask Jacobian become singular (rank deficient) the BodyTask minimization could fail. By providing more task to solve and redundancy we can solve this issue.\n",
    "\n",
    "To solve this issue we can use a multi task IK (multi objective):\n",
    "$$\n",
    "\\underset{\\mathbf{q}}{\\text{minimize }} \\frac{1}{2} \\sum\\limits_{i=1}^{N} w_i\\| \\mathbf{g}_i(\\mathbf{q}) \\|^2\n",
    "$$\n",
    "Instead of minimizing one task error function we will minimize $ N $ task error function.\n",
    "\n",
    "Some task can enter in conflict. By exemple a task that will move the MultiBody CoM can conflict with a BodyTask.\n",
    "\n",
    "To solve this issue we give a weight $ w $ to each task. This weight will allow us to specify a *soft* priority to each task.\n",
    "\n",
    "The mono task IK method will be used to solve the new problem but instead of using one task jacobian and error function we will stack each jacobian an error function together:\n",
    "$$\n",
    "J_{\\mathbf{g}} = \\begin{bmatrix} w_1 J_1 \\\\ \\vdots \\\\ w_N J_N \\end{bmatrix}\n",
    "\\mathbf{g} = \\begin{bmatrix} w_1 \\mathbf{g}_1 \\\\ \\vdots \\\\ w_N \\mathbf{g}_N \\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def manyTaskMin(mb, mbc, tasks, delta=1., maxIter=100, prec=1e-8):\n",
    "    q = e.toNumpy(rbd.paramToVector(mb, mbc.q))\n",
    "    iterate = 0\n",
    "    minimizer = False\n",
    "    while iterate < maxIter and not minimizer:\n",
    "        # compute task data\n",
    "        gList = map(lambda (w, t): w*t.g(mb, mbc), tasks)\n",
    "        JList = map(lambda (w, t): w*t.J(mb, mbc), tasks)\n",
    "        \n",
    "        g = np.concatenate(gList)\n",
    "        J = np.concatenate(JList)\n",
    "        \n",
    "        # compute alpha\n",
    "        alpha = -np.mat(np.linalg.lstsq(J, g)[0])\n",
    "        \n",
    "        # integrate and run the forward kinematic\n",
    "        mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alpha))\n",
    "        rbd.eulerIntegration(mb, mbc, delta)\n",
    "        rbd.forwardKinematics(mb, mbc)\n",
    "        \n",
    "        # take the new q vector\n",
    "        q = e.toNumpy(rbd.paramToVector(mb, mbc.q))\n",
    "        \n",
    "        alphaInf = np.linalg.norm(alpha, np.inf)\n",
    "        yield iterate, q, alpha, alphaInf # yield the current state\n",
    "        \n",
    "        # check if the current alpha is a minimizer\n",
    "        if alphaInf < prec:\n",
    "            minimizer = True\n",
    "        iterate += 1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Posture Task\n",
    "\n",
    "The posture task will target one reference articular position vector $ \\mathbf{q}_T $. This task is usually a low priority task that will ensure that body with no target will not move and also to always have a full rank jacobian during the optimisation process.\n",
    "\n",
    "For simplicity we will only define the Posture Task for the revolute, prismatic or spherical joint.\n",
    "\n",
    "The task error vector is:\n",
    "$$\n",
    "\\mathbf{g}^{posture} = \\begin{bmatrix} qError(\\mathbf{q}_1, \\mathbf{q}_{T_1}) \\\\ \\vdots \\\\ qError(\\mathbf{q}_N, \\mathbf{q}_{T_N}) \\end{bmatrix}\n",
    "$$\n",
    "Where $ qError \\in \\mathbb{R}^{A_i} $ will compute the articular target error depending of the joint.\n",
    "\n",
    "The $ qError $ will have a different behavior depending of the joint type:\n",
    "$$\n",
    " qError(\\mathbf{q}_i, \\mathbf{q}_{T_i}) =\n",
    " \\begin{cases}\n",
    " \\mathbf{q}_i - \\mathbf{q}_{T_i} & \\quad \\text{if } joint_i \\text{ is revolute/prismatic}\\\\\n",
    " sva.rotationError(\\mathbf{q}_{T_i}, \\mathbf{q}_i) & \\quad \\text{if } joint_i \\text{ is spherical}\n",
    " \\end{cases}\n",
    "$$\n",
    "\n",
    "The jacobian is:\n",
    "$$\n",
    "J_{\\mathbf{g}^{posture}} =\n",
    "\\begin{bmatrix}\n",
    "I^{A_1 \\times A_1} & \\cdots & \\cdots \\\\\n",
    "\\vdots & \\vdots & \\vdots \\\\\n",
    "\\cdots & \\cdots & I^{A_N \\times A_N}\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class PostureTask(object):\n",
    "    def __init__(self, mb, q_T):\n",
    "        \"\"\"\n",
    "        Target a default configuration for the robot\n",
    "        \"\"\"\n",
    "        self.q_T = q_T\n",
    "        \n",
    "        def isDefine(j):\n",
    "            return j.type() in (rbd.Joint.Prism, rbd.Joint.Rev, rbd.Joint.Spherical)\n",
    "        # take back joint and joint index that are define\n",
    "        self.jointIndex = [i for i, j in enumerate(mb.joints()) if isDefine(j)]\n",
    "        self.joints = [mb.joint(index) for index in self.jointIndex]\n",
    "        nrDof = reduce(lambda dof, j: dof + j.dof(), self.joints, 0)\n",
    "        \n",
    "        # initialize g\n",
    "        self.g_mat = np.mat(np.zeros((nrDof, 1)))\n",
    "        \n",
    "        # initialize the jacobian\n",
    "        self.J_mat = np.mat(np.zeros((nrDof, mb.nrDof())))\n",
    "        posInG = 0\n",
    "        for jIndex, j in zip(self.jointIndex, self.joints):\n",
    "            posInDof = mb.jointPosInDof(jIndex)\n",
    "            self.J_mat[posInG:posInG+j.dof(),\n",
    "                       posInDof:posInDof+j.dof()] = np.eye(j.dof())\n",
    "            posInG += j.dof()\n",
    "        \n",
    "    def g(self, mb, mbc):\n",
    "        q = map(list, mbc.q)\n",
    "        jointConfig = list(mbc.jointConfig)\n",
    "        posInG = 0\n",
    "        for jIndex, j in zip(self.jointIndex, self.joints):\n",
    "            if j.type() in (rbd.Joint.Prism, rbd.Joint.Rev):\n",
    "                self.g_mat[posInG:posInG+j.dof(),0] = q[jIndex][0] - self.q_T[jIndex][0]\n",
    "            elif j.type() in (rbd.Joint.Spherical,):\n",
    "                orid = e.Quaterniond(*self.q_T[jIndex]).inverse().matrix()\n",
    "                self.g_mat[posInG:posInG+j.dof(),0] =\\\n",
    "                    e.toNumpy(sva.rotationError(orid, jointConfig[jIndex].rotation()))\n",
    "            posInG += j.dof()\n",
    "        return self.g_mat\n",
    "        \n",
    "    def J(self, mb, mbc):\n",
    "        return self.J_mat"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Configuration\n",
    "\n",
    "Ok, let's try the same IK, but with the Posture Task as low-priority task."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYkAAAGnCAIAAADNGTysAAAd+0lEQVR4nO3dX2xc14Hf8R8pSqJi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\nAHxEmwD4iDYB8BFtAuCj/w8iHdC3sVnwDgAAAABJRU5ErkJggg==\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Set an initial configuration\n",
    "mbcMultiIK = rbd.MultiBodyConfig(mbcIK)\n",
    "\n",
    "postureTask = PostureTask(mb, map(list, mbcMultiIK.q))\n",
    "\n",
    "# display initial configuration\n",
    "from graph import Axis\n",
    "axis_b5.X = bodyTask.X_O_p(mbcMultiIK)\n",
    "axis_T.X = X_O_T\n",
    "\n",
    "mbv.display(mb, mbcMultiIK)\n",
    "display()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Result\n",
    "\n",
    "Ok, nothing impressive in the result that is quite the same of the last one (see the q residual).\n",
    "\n",
    "But we see that the Posture Task with a low priority is not interfering with the Body Task.\n",
    "\n",
    "We will see in a next tutorial some application of the IK for a more complex robot like a humanoid."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "g_body translation error: [[ -8.00000000e-09   1.84799576e-09  -3.62491448e-09]]\n",
      "g_body rotation error: [[  9.70197715e-09   2.41660976e-09   7.69998188e-09]]\n",
      "g_posture error: [[-0.78539815 -1.04719754  0.78539816  0.79999999  0.          0.          0.        ]]\n",
      "residual between the two solution: 1.49418234363e-08\n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYkAAAGnCAIAAADNGTysAAAYxUlEQVR4nO3dXWxc533n8R8piZJF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\noE4AAAAASUVORK5CYII=\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# copy the initial configuration to avoid the algorithm to change it\n",
    "mbcMultiIKSolve = rbd.MultiBodyConfig(mbcMultiIK)\n",
    "# set the weight of bodyTask to 10000 and the weight of the postureTask to 1\n",
    "tasks = [(10000., bodyTask), (1., postureTask)]\n",
    "q_res = None\n",
    "X_O_p_res = None\n",
    "alphaInfList = []\n",
    "for iterate, q, alpha, alphaInf in manyTaskMin(mb, mbcMultiIKSolve, tasks, delta=1., maxIter=200, prec=1e-8):\n",
    "    X_O_p = bodyTask.X_O_p(mbcMultiIKSolve)\n",
    "    q_res = q\n",
    "    alphaInfList.append(alphaInf)\n",
    "    \n",
    "mbcMultiIKResult = rbd.MultiBodyConfig(mbcMultiIK)\n",
    "mbcMultiIKResult.q = rbd.vectorToParam(mb, e.toEigenX(q_res))\n",
    "rbd.forwardKinematics(mb, mbcMultiIKResult)\n",
    "\n",
    "g_body = bodyTask.g(mb, mbcMultiIKResult)\n",
    "g_posture = postureTask.g(mb, mbcMultiIKResult)\n",
    "print 'g_body translation error:', g_body[3:].T\n",
    "print 'g_body rotation error:', g_body[:3].T\n",
    "print 'g_posture error:', g_posture.T\n",
    "\n",
    "monoQRes = e.toNumpy(rbd.paramToVector(mb, mbcIKResult.q))\n",
    "multiQRes = e.toNumpy(rbd.paramToVector(mb, mbcMultiIKResult.q))\n",
    "print 'residual between the two solution:', np.linalg.norm(monoQRes - multiQRes)\n",
    "\n",
    "axis_b5.X = X_O_p\n",
    "mbv.display(mb, mbcMultiIKResult)\n",
    "display()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.text.Text at 0x7fedb2d9cfd0>"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYEAAAEPCAYAAACk43iMAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7fedb2d1e490>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "plt.plot(alphaInfList)\n",
    "plt.ylabel('alphaInf')\n",
    "plt.xlabel('iterate')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "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.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}