Jacobian.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Jacobian\n",
    "\n",
    "In this tutorial we will see how to compute some jacobian and related data:\n",
    "\n",
    " * [Kinematics Jacobian](#Kinematics-Jacobian)\n",
    " * [Center of Mass Jacobian](#Center-of-Mass-Jacobian)\n",
    " * [Centroidal Momentum Matrix](#Centroidal-Momentum-Matrix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "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": "markdown",
   "metadata": {},
   "source": [
    "## What's a Jacobian ?\n",
    "\n",
    "A Jacobian is the partial derivative of a multi-variate function with respect of one of his variable.\n",
    "In [RBDyn](https://github.com/jorisv/RBDyn) the Jacobian is generaly the derivative with respect of the $ \\mathbf{q} $ articular position vector.\n",
    "\n",
    " * Kinematics: $ \\frac{\\partial {}^{b} X_O(\\mathbf{q})}{\\partial \\mathbf{q}} = J_b(\\mathbf{q}) $\n",
    " * Center of Mass: $ \\frac{\\partial \\mathbf{c}(\\mathbf{q})}{\\partial \\mathbf{q}} = J_{CoM}(\\mathbf{q}) $\n",
    "\n",
    "Here $ {}^{b} X_O $ is the configuration of a body $ b $ and $ \\mathbf{c} $ the center of mass of the MultiBody system.\n",
    "\n",
    "Those Jacobian will be really useful when using gradient based optimisation (inverse kinematic). But we can also use it for control like show the following equations:\n",
    "\n",
    " * $ \\hat{v}_b = \\frac{\\partial {}^{b} X_O(\\mathbf{q})}{\\partial \\mathbf{t}} = \\frac{\\partial {}^{b} X_O(\\mathbf{q})}{\\partial \\mathbf{q}} \\frac{\\partial \\mathbf{q}}{\\partial \\mathbf{t}} = J_b(\\mathbf{q}) \\mathbf{\\alpha} $\n",
    " * $ \\dot{c} = \\frac{\\partial \\mathbf{c}(\\mathbf{q})}{\\partial \\mathbf{t}} = \\frac{\\partial \\mathbf{c}(\\mathbf{q})}{\\partial \\mathbf{q}} \\frac{\\partial \\mathbf{q}}{\\partial \\mathbf{t}} = J_{CoM}(\\mathbf{q}) \\mathbf{\\alpha} $\n",
    "\n",
    "A last kind-of jacobian is the Centroidal Momentum matrix ($ CM $):\n",
    "\n",
    " * Centroidal Momentum: $ \\hat{h}_c(\\mathbf{q}, \\mathbf{\\alpha}) = CM(\\mathbf{q}) \\mathbf{\\alpha} $\n",
    " \n",
    "This matrix will also be useful to control the Centroidal Momentum of a MultiBody system.\n",
    "\n",
    "## Kinematics Jacobian\n",
    "\n",
    "The kinematics Jacobian allow to compute some values related to a body $ b $:\n",
    " * Jacobian matrix $ J_b $\n",
    " * Jacobian matrix time derivative $ \\dot{J}_b $\n",
    " * Body velocity $ \\hat{v}_b $\n",
    " * Body normal acceleration $ \\dot{J}_b \\alpha $\n",
    " \n",
    "In this tutorial we will only focus on the Jacobian matrix and Body velocity functions.\n",
    "\n",
    "### Classic use\n",
    "\n",
    "The `rbd::Jacobian` class can be a little tricky to use. This class have some states and you should be careful about how you use it.\n",
    "\n",
    "Let's see the class constructor:\n",
    "```c++\n",
    "Jacobian(const MultiBody& mb, int bodyId,\n",
    "\t\t const Eigen::Vector3d& point=Eigen::Vector3d::Zero())\n",
    "```\n",
    "You need to provide the Multibody system, the body **id** and an optional `point` ($ {}^p r_b $) translation on this body. It's recommended to not use the `point` parameter since it can be provided has an argument of some methods.\n",
    "\n",
    "By calling this constructor you will be able to compute the following Jacobian:\n",
    "$$\n",
    "\\frac{\\partial xlt({}^{p} r_b) {}^{b} X_O(\\mathbf{q})}{\\partial \\mathbf{q}} = J_{bp}(\\mathbf{q})\n",
    "$$\n",
    "Where $ {}^{b} X_O $ is the transformation from the origin to the body b and $ xlt({}^{p} r_b) $ the `point` translation provided has argument of the `rbd::Jacobian` constructor.\n",
    "\n",
    "There is two methods to easily compute the dense Jacobian matrix:\n",
    "```c++\n",
    "const Eigen::MatrixXd& jacobian(const MultiBody& mb, const MultiBodyConfig& mbc)\n",
    "const Eigen::MatrixXd& bodyJacobian(const MultiBody& mb, const MultiBodyConfig& mbc)\n",
    "```\n",
    "The first one will compute the Jacobian in Origin frame orientation: $ plx({}^O E_b) J_{bp}(\\mathbf{q}) $.\n",
    "The second one will compute the Jacobian in body frame orientation: $ J_{bp}(\\mathbf{q}) $.\n",
    "\n",
    "Both method **Input**:\n",
    " * bodyPosW\n",
    " * motionSubspace\n",
    "\n",
    "Let's illustrate it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Dense Jacobian in Origin frame orientation\n",
      "1 1 0 0\n",
      "0 0 1 0\n",
      "0 0 0 1\n",
      "0 0 0 0\n",
      "0 0 0 0\n",
      "0 0 0 0\n",
      "\n",
      "Dense Jacobian in body frame orientation\n",
      "1 1 0 0\n",
      "0 0 1 0\n",
      "0 0 0 1\n",
      "0 0 0 0\n",
      "0 0 0 0\n",
      "0 0 0 0\n",
      "\n"
     ]
    }
   ],
   "source": [
    "mbc.zero(mb)\n",
    "rbd.forwardKinematics(mb, mbc)\n",
    "rbd.forwardVelocity(mb, mbc) # mandatory because jacobian need mbc.motionSubspace !\n",
    "\n",
    "b4Id = mbg.bodyIdByName('b4')\n",
    "jac_b4 = rbd.Jacobian(mb, b4Id)\n",
    "jacO = jac_b4.jacobian(mb, mbc)\n",
    "jacB = jac_b4.bodyJacobian(mb, mbc)\n",
    "\n",
    "print 'Dense Jacobian in Origin frame orientation'\n",
    "print jacO\n",
    "print\n",
    "print 'Dense Jacobian in body frame orientation'\n",
    "print jacB\n",
    "print"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Jacobian are similar because b4 have the same frame than the origin.\n",
    "\n",
    "Now if we try with a non zero configuration of the MultiBody."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Dense Jacobian in Origin frame orientation\n",
      "        1  0.514286 -0.367726  0.774782\n",
      "        0  0.689068 -0.360671 -0.628571\n",
      "        0  0.510584  0.857143 0.0679007\n",
      "        0         0         0         0\n",
      "        0         0         0         0\n",
      "        0         0         0         0\n",
      "\n",
      "Dense Jacobian in body frame orientation\n",
      "   0.514286           1 5.55112e-17           0\n",
      "  -0.367726 5.55112e-17           1           0\n",
      "   0.774782           0           0           1\n",
      "          0           0           0           0\n",
      "          0           0           0           0\n",
      "          0           0           0           0\n",
      "\n"
     ]
    }
   ],
   "source": [
    "quat = e.Quaterniond(np.pi/3., e.Vector3d(0.1, 0.5, 0.3).normalized())\n",
    "\n",
    "mbc.q = [[],\n",
    "         [np.pi/2.],\n",
    "         [np.pi/3.],\n",
    "         [-np.pi/2.],\n",
    "         [0.5],\n",
    "         [quat.w(), quat.x(), quat.y(), quat.z()]]\n",
    "\n",
    "rbd.forwardKinematics(mb, mbc)\n",
    "jacO = jac_b4.jacobian(mb, mbc)\n",
    "jacB = jac_b4.bodyJacobian(mb, mbc)\n",
    "\n",
    "print 'Dense Jacobian in Origin frame orientation'\n",
    "print jacO\n",
    "print\n",
    "print 'Dense Jacobian in body frame orientation'\n",
    "print jacB\n",
    "print"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can use the following property to check that our's computations are correct:\n",
    "\n",
    "$$\n",
    "plx({}^O E_b) \\hat{v}_{bp} =\n",
    "plx({}^O E_b) \\frac{\\partial xlt({}^{p} r_b) {}^{b} X_O(\\mathbf{q})}{\\partial \\mathbf{t}} =\n",
    "plx({}^O E_b) J_{bp}(\\mathbf{q}) \\mathbf{\\alpha}\n",
    "$$\n",
    "and\n",
    "$$\n",
    "\\hat{v}_{bp} =\n",
    "\\frac{\\partial xlt({}^{p} r_b) {}^{b} X_O(\\mathbf{q})}{\\partial \\mathbf{t}} =\n",
    "J_{bp}(\\mathbf{q}) \\mathbf{\\alpha}\n",
    "$$\n",
    "\n",
    "But before doing that we have to be careful. Our's robot is constitued of 6 DoF but if we look, the jacobian of b4 only have 4 columns.\n",
    "\n",
    "The jacobian method is computing the dense jacobian. Before applying the $ \\alpha $ articular velocity vector on it we need to transform this matrix in his sparse form. Hopefully the `rbd::Jacobian` class have a method to do that:\n",
    "```c++\n",
    "void fullJacobian(const MultiBody& mb,\n",
    "\t\t          const Eigen::Ref<const Eigen::MatrixXd>& jac,\n",
    "\t\t          Eigen::MatrixXd& res) const\n",
    "```\n",
    "This method take the MultiBody system, the dense Jacobian `jac` and the output sparse Jacobian `res`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Sparse Jacobian in Origin frame orientation\n",
      "        1         0         0         0  0.514286 -0.367726  0.774782\n",
      "        0         0         0         0  0.689068 -0.360671 -0.628571\n",
      "        0         0         0         0  0.510584  0.857143 0.0679007\n",
      "        0         0         0         0         0         0         0\n",
      "        0         0         0         0         0         0         0\n",
      "        0         0         0         0         0         0         0\n",
      "\n",
      "Sparse Jacobian in body frame orientation\n",
      "   0.514286           0           0           0           1 5.55112e-17           0\n",
      "  -0.367726           0           0           0 5.55112e-17           1           0\n",
      "   0.774782           0           0           0           0           0           1\n",
      "          0           0           0           0           0           0           0\n",
      "          0           0           0           0           0           0           0\n",
      "          0           0           0           0           0           0           0\n",
      "\n",
      "alpha: [[], [0.0], [0.0], [0.0], [0.0], [0.0, 0.0, 0.0]]\n",
      "Residual in Origin orientation frame: 0.0\n",
      "Residual in body orientation frame: 0.0\n",
      "\n",
      "alpha: [[], [0.47677333721005144], [0.7741694328769283], [0.8247737497589269], [0.25609730486578974], [0.28190958251947873, 0.8252984923746803, 0.29314447966087087]]\n",
      "Residual in Origin orientation frame: 1.11022302463e-16\n",
      "Residual in body orientation frame: 1.57009245868e-16\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# allocate sparse matrix\n",
    "sparseJacO = e.MatrixXd(6, mb.nrDof())\n",
    "sparseJacB = e.MatrixXd(6, mb.nrDof())\n",
    "\n",
    "jac_b4.fullJacobian(mb, jacO, sparseJacO)\n",
    "jac_b4.fullJacobian(mb, jacB, sparseJacB)\n",
    "\n",
    "print 'Sparse Jacobian in Origin frame orientation'\n",
    "print sparseJacO\n",
    "print\n",
    "print 'Sparse Jacobian in body frame orientation'\n",
    "print sparseJacB\n",
    "print\n",
    "\n",
    "# 0 alpha vector\n",
    "mbc.alpha = map(lambda j: j.zeroDof(), mb.joints())\n",
    "rbd.forwardVelocity(mb, mbc) # run the forward velocity to compute bodyPosW and bodyPosB\n",
    "\n",
    "# take back body velocity in Origin orientation frame and in body orientation frame\n",
    "b4Index = mb.bodyIndexById(b4Id)\n",
    "bodyVelW = list(mbc.bodyVelW)\n",
    "bodyVelB = list(mbc.bodyVelB)\n",
    "V_b4_O = bodyVelW[b4Index]\n",
    "V_b4 = bodyVelB[b4Index]\n",
    "\n",
    "# convert the alpha articular parameter vector into a numpy vector\n",
    "alphaVec = e.toNumpy(rbd.dofToVector(mb, mbc.alpha))\n",
    "\n",
    "# compute velocity from jacobian\n",
    "jacVelO = e.toNumpy(sparseJacO)*alphaVec\n",
    "jacVelB = e.toNumpy(sparseJacB)*alphaVec\n",
    "\n",
    "print 'alpha:', map(list, mbc.alpha)\n",
    "print 'Residual in Origin orientation frame:', np.linalg.norm(jacVelO - e.toNumpy(V_b4_O.vector()))\n",
    "print 'Residual in body orientation frame:', np.linalg.norm(jacVelB - e.toNumpy(V_b4.vector()))\n",
    "print\n",
    "\n",
    "# now we apply a new alpha vector\n",
    "alphaVec = np.mat(np.random.rand(mb.nrDof(),1))\n",
    "\n",
    "mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alphaVec))\n",
    "rbd.forwardVelocity(mb, mbc) # run the forward velocity to compute bodyPosW and bodyPosB\n",
    "\n",
    "bodyVelW = list(mbc.bodyVelW)\n",
    "bodyVelB = list(mbc.bodyVelB)\n",
    "V_b4_O = bodyVelW[b4Index]\n",
    "V_b4 = bodyVelB[b4Index]\n",
    "\n",
    "# compute velocity from jacobian\n",
    "jacVelO = e.toNumpy(sparseJacO)*alphaVec\n",
    "jacVelB = e.toNumpy(sparseJacB)*alphaVec\n",
    "\n",
    "print 'alpha:', map(list, mbc.alpha)\n",
    "print 'Residual in Origin orientation frame:', np.linalg.norm(jacVelO - e.toNumpy(V_b4_O.vector()))\n",
    "print 'Residual in body orientation frame:', np.linalg.norm(jacVelB - e.toNumpy(V_b4.vector()))\n",
    "print"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It's also possible to directly compute $ plx({}^O E_b) \\hat{v}_{bp} $ and $ \\hat{v}_{bp} $ with the following methods:\n",
    "```c++\n",
    "sva::MotionVecd velocity(const MultiBody& mb, const MultiBodyConfig& mbc) const\n",
    "sva::MotionVecd bodyVelocity(const MultiBody& mb, const MultiBodyConfig& mbc) const\n",
    "```\n",
    "First one **Input**:\n",
    " * bodyPosW\n",
    " * bodyVelB\n",
    " \n",
    "Second one **Input**:\n",
    " * bodyVelB"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Modern use\n",
    "\n",
    "We see how to use the `rbd::Jacobian` class. It's really easy to compute the Jacobian of a point attached to a body in two different orientation frame. But how to compute a Jacobian in a different frame ?\n",
    "\n",
    "`rbd::Jacobian` provide a method to do like this. Instead of using the `point` static translation you can provide à full transformation has argument:\n",
    "```c++\n",
    "const Eigen::MatrixXd& jacobian(const MultiBody& mb, const MultiBodyConfig& mbc,\n",
    "\t\t\t                    const sva::PTransformd& X_0_p);\n",
    "```\n",
    "**Input**:\n",
    " * bodyPosW\n",
    " * motionSubspace\n",
    "\n",
    "The Jacobian computed is the following:\n",
    "$$\n",
    "\\hat{v}_{bp} =\n",
    "\\frac{\\partial {}^{p} X_O}{\\partial \\mathbf{t}} =\n",
    "J_{bp}(\\mathbf{q}) \\mathbf{\\alpha}\n",
    "$$\n",
    "\n",
    "It's then really easy to compute the Origin orientation and the body frame Jacobian with\n",
    "$$ {}^{p} X_O = plx({}^O E_b) xlt({}^{p} r_O) {}^{b} X_O $$\n",
    "and\n",
    "$$ {}^{p} X_O = xlt({}^{p} r_O) {}^{b} X_O $$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Residual of Origin orientation frame Jacobian: 2.09205062047e-16\n",
      "Residual of body frame Jacobian: 0.0\n"
     ]
    }
   ],
   "source": [
    "bodyPosW = list(mbc.bodyPosW)\n",
    "X_O_b = bodyPosW[b4Index]\n",
    "X_b_p = sva.PTransformd(jac_b4.point())\n",
    "X_O_p = X_b_p*X_O_b\n",
    "X_O_p_O = sva.PTransformd(X_O_b.rotation()).inv()*X_O_p\n",
    "\n",
    "jacO_modern = jac_b4.jacobian(mb, mbc, X_O_p_O)\n",
    "jacB_modern = jac_b4.jacobian(mb, mbc, X_O_p)\n",
    "\n",
    "print 'Residual of Origin orientation frame Jacobian:', np.linalg.norm(e.toNumpy(jacO) - e.toNumpy(jacO_modern))\n",
    "print 'Residual of body frame Jacobian:', np.linalg.norm(e.toNumpy(jacB) - e.toNumpy(jacB_modern))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Like for the classic methods it's possible to compute the velocicy $ \\hat{v}_{bp} $ from the `rbd::Jacobian` class:\n",
    "```c++\n",
    "sva::MotionVecd velocity(const MultiBody& mb,\n",
    "                         const MultiBodyConfig& mbc, const sva::PTransformd& X_b_p) const\n",
    "```\n",
    "**Input**:\n",
    " * bodyVelB\n",
    " * $ {}^{p} X_b $ transformation from the Jacobian body $ b $ to the attached frame.\n",
    "\n",
    "**BEWARE** the modern `jacobian` method take the $ {}^{p} X_O = {}^{p} X_b {}^{b} X_O $ transform while the modern `velocity` method take the $ {}^{p} X_b $ transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Veloctiy in Origin orientation frame:  0.545394 -0.287668  0.871242         0         0         0\n",
      "Velocity in body frame (classic): 0.527107 0.649976  0.66254        0        0        0\n",
      "Velocity in body frame (modern): 0.527107 0.649976  0.66254        0        0        0\n"
     ]
    }
   ],
   "source": [
    "V_O_p_O_classic = jac_b4.velocity(mb, mbc)\n",
    "V_O_p_classic = jac_b4.bodyVelocity(mb, mbc)\n",
    "V_O_p = jac_b4.velocity(mb, mbc, X_b_p)\n",
    "\n",
    "print 'Veloctiy in Origin orientation frame:', V_O_p_O_classic\n",
    "print 'Velocity in body frame (classic):', V_O_p_classic\n",
    "print 'Velocity in body frame (modern):', V_O_p"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Center of Mass Jacobian\n",
    "\n",
    "The center of mass of a $ N $ rigid body system can be computed with the following equation:\n",
    "$$\n",
    "\\mathbf{c} = \\sum\\limits_{i=1}^{N} \\frac{w_i m_i {}^{c_i} r_O}{\\mathbf{m}}\n",
    "$$\n",
    "Where\n",
    " * $ m_i $ is the mass of the body $ i $\n",
    " * $ {}^{c_i} r_O $ is the position of the center of mass of the body $ i $\n",
    " * $ \\mathbf{m} = \\sum\\limits_{i=1}^{N} m_i $ the mass of the system\n",
    " * $ w_i $ a custom weight set for each body by the user (1 by default)\n",
    "\n",
    "The jacobian is then:\n",
    "$$\n",
    "\\frac{\\partial \\mathbf{c}(\\mathbf{q})}{\\partial \\mathbf{q}} = J_{CoM}(\\mathbf{q})\n",
    "$$\n",
    "\n",
    "There is two implementation of the CoM Jacobian:\n",
    " * `rbd::DummyCoMJacobian`: Use a simple but slow algorithm\n",
    " * `rbd::CoMJacobian`: Faster implementation of the algorithm\n",
    " \n",
    "Both have a quit similar API, but it's recommanded to only use `rbd::CoMJacobian` that is faster and don't have any drawback compared to `rbd::DummyCoMJacobian`.\n",
    "\n",
    "The `rbd::CoMJacobian` class allow to compute the following values:\n",
    " * CoM Jacobian matrix $ J_{CoM} $\n",
    " * CoM Jacobian matrix time derivative $ \\dot{J}_{CoM} $\n",
    " * CoM velocity in Origin orientation frame $ \\dot{\\mathbf{c}} $\n",
    " * CoM normal acceleration $ \\dot{J}_{CoM} \\alpha $\n",
    "\n",
    "Like the Kinematic Jacobian we will only see how to compute the CoM Jacobian matrix and te CoM velocity.\n",
    "\n",
    "The `rbd::CoMJacobian` have two constructor:\n",
    "```c++\n",
    "CoMJacobian(const MultiBody& mb)\n",
    "CoMJacobian(const MultiBody& mb, std::vector<double> weight)\n",
    "```\n",
    "The first one initialize the class with a $ w_i $ vector to one while the second one use the vector provided by the user.\n",
    "\n",
    "The `rbd::CoMJacobian` store some inertial value of the MultiBody so **Remember** to call the following method each time you modifying an inertial parameter of the MultiBody:\n",
    "```c++\n",
    "void updateInertialParameters(const MultiBody& mb)\n",
    "```\n",
    "\n",
    "The following method allow to compute the CoM Jacobian matrix in the Origin orientation frame:\n",
    "```c++\n",
    "\tconst Eigen::MatrixXd& jacobian(const MultiBody& mb, const MultiBodyConfig& mbc)\n",
    "```\n",
    "**Input**:\n",
    " * bodyPosW\n",
    " * motionSubspace\n",
    " \n",
    "Finally the next method compute the linear velocity of the CoM in the Origin orientation frame\n",
    "```c++\n",
    "\tEigen::Vector3d velocity(const MultiBody& mb, const MultiBodyConfig& mbc) const\n",
    "```\n",
    "**Input**:\n",
    " * bodyPosW\n",
    " * bodyVelB"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CoM velocity from velocity: [[-0.09346977 -0.13628341  0.19342472]]\n",
      "CoM velocity from Jacobian: [[-0.09346977 -0.13628341  0.19342472]]\n",
      "Residual: 6.79869977755e-17\n"
     ]
    }
   ],
   "source": [
    "# create a random alpha vector\n",
    "alphaVec = np.mat(np.random.rand(mb.nrDof(),1))\n",
    "\n",
    "mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alphaVec))\n",
    "rbd.forwardVelocity(mb, mbc) # run the forward velocity to compute bodyPosW and bodyPosB\n",
    "\n",
    "# compute the jacobian\n",
    "jac_com = rbd.CoMJacobian(mb)\n",
    "jac_com_mat = jac_com.jacobian(mb, mbc)\n",
    "\n",
    "# compute the velocity and the velocity from the CoM Jacobian matrix\n",
    "vel_com = jac_com.velocity(mb, mbc)\n",
    "vel_com_jac = e.toNumpy(jac_com_mat)*alphaVec\n",
    "\n",
    "print 'CoM velocity from velocity:', e.toNumpy(vel_com).T\n",
    "print 'CoM velocity from Jacobian:', vel_com_jac.T\n",
    "print 'Residual:', np.linalg.norm(e.toNumpy(vel_com).T - vel_com_jac.T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Centroidal Momentum Matrix\n",
    "\n",
    "The Centroidal Momentum at the CoM frame $ \\hat{h}_c $ is defined by the following equation:\n",
    "$$\n",
    "\\hat{h}_c = \\sum\\limits_{i=1}^{N} {}^{\\mathbf{c}} w_i X^{*}_i (\\hat{I}_i \\hat{v}_i)\n",
    "$$\n",
    "Where\n",
    " * $ {}^{\\mathbf{c}} X^{*}_i $ is the dual transform from the body $ i $ to the center of mass\n",
    " * $ \\hat{I}_i $ is the Rigid Body Inertia of the body $ i $\n",
    " * $ \\hat{v}_i $ is the velocity vector of the body $ i $\n",
    " * $ w_i $ a custom weight set for each body by the user (1 by default)\n",
    " \n",
    "The Centroidal Momentum Matrix ($ CM $) will allow to rewrite the Centroidal Momentum equation in the following form:\n",
    "$$\n",
    "\\hat{h}_c = CM \\mathbf{\\alpha}\n",
    "$$\n",
    "This form will be really useful when using optimization solver because it can make a relation between $ \\hat{h}_c $ and the $ \\mathbf{\\alpha} $ control vector.\n",
    "\n",
    "The `rbd::CentroidalMomentumMatrix` class can compute the following values:\n",
    " * The Centroidal Momentum Matrix in CoM frame $ CM $\n",
    " * The Centroidal Momentum Matrix in CoM frame time derivative $ \\dot{CM} $\n",
    " * The Centroidal Momentum in CoM frame $ \\hat{h}_c\n",
    " * The Centroidal Momentum normal acceleration in CoM frame $ \\dot{CM} \\alpha $\n",
    " \n",
    "Like the Kinematic and the CoM Jacobian we will not focus on the time derivative computation in this tutorial.\n",
    "\n",
    "First let's look at the constructors:\n",
    "```c++\n",
    "CentroidalMomentumMatrix(const MultiBody& mb)\n",
    "CentroidalMomentumMatrix(const MultiBody &mb, std::vector<double> weight)\n",
    "```\n",
    "The first one initialize the class with a $ w_i $ vector to one while the second one use the vector provided by the user.\n",
    "\n",
    "Then to compute the CM Matrix you need to call:\n",
    "```c++\n",
    "void computeMatrix(const MultiBody& mb, const MultiBodyConfig& mbc,\n",
    "            \t   const Eigen::Vector3d& com)\n",
    "```\n",
    "**Input**:\n",
    " * bodyPosW\n",
    " * motionSubspace\n",
    " * CoM translation from Origin (not in MultiBodyConfig)\n",
    " \n",
    "You can then obtain the computed matrix with the `const Eigen::MatrixXd& matrix() const` getter.\n",
    "\n",
    "Finally you can compute the $ \\hat{h}_c $ value with the following method:\n",
    "```c++\n",
    "sva::ForceVecd momentum(const MultiBody& mb,\n",
    "                        const MultiBodyConfig& mbc, const Eigen::Vector3d& com) const\n",
    "```\n",
    "**Input**:\n",
    " * bodyPosW\n",
    " * bodyVelB\n",
    " * CoM translation from Origin (not in MultiBodyConfig)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Centroidal Momentum from momentum: [[ 4.98169283 -0.30382868  3.73266045 -0.35257911 -0.9196868   0.71035043]]\n",
      "Centroidal Momentum from CM Matrix: [[ 4.98169283 -0.30382868  3.73266045 -0.35257911 -0.9196868   0.71035043]]\n",
      "Residual: 1.1185187043e-15\n"
     ]
    }
   ],
   "source": [
    "# create a random alpha vector\n",
    "alphaVec = np.mat(np.random.rand(mb.nrDof(),1))\n",
    "\n",
    "mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alphaVec))\n",
    "rbd.forwardVelocity(mb, mbc) # run the forward velocity to compute bodyPosW and bodyPosB\n",
    "com = rbd.computeCoM(mb, mbc)\n",
    "\n",
    "# compute the CM Matrix\n",
    "CMM = rbd.CentroidalMomentumMatrix(mb)\n",
    "CMM.computeMatrix(mb, mbc, com)\n",
    "CMM_mat = CMM.matrix()\n",
    "\n",
    "# compute the momentum and the momentum from the CM Matrix\n",
    "h_c = CMM.momentum(mb, mbc, com)\n",
    "h_c_jac = e.toNumpy(CMM_mat)*alphaVec\n",
    "\n",
    "print 'Centroidal Momentum from momentum:', e.toNumpy(h_c.vector()).T\n",
    "print 'Centroidal Momentum from CM Matrix:', h_c_jac.T\n",
    "print 'Residual:', np.linalg.norm(e.toNumpy(h_c.vector()).T - h_c_jac.T)"
   ]
  },
  {
   "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
}