equations-of-motion-floating-inverted-pendulum.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Equations of motion of a floating double pendulum\n",
    "Consider a two-link inverted pendulum, where each link is a homogenous rod.\n",
    "<!--- ![Two-link inverted pendulum](./figures/two-link.png \"Two-link inverted pendulum\") -->\n",
    "<img src=\"./figures/two-link.png\" alt=\"Two-link inverted pendulum\" style=\"width: 300px;\"/>\n",
    "## The center of mass of each link and the combined pendulum\n",
    "Let $p_1$ and $p_2$ denote the position of CoM of link 1 and link 2, respectively. We get\n",
    "\\begin{equation}\n",
    "p_1 = \\begin{bmatrix} q_3 + \\frac{l_1}{2}s_1\\\\ q_4 + \\frac{l_1}{2}c_1 \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "p_2 = \\begin{bmatrix} q_3 + l_1s_1 + \\frac{l_2}{2}s_{1+2}\\\\ q_4 + l_1c_1 + \\frac{l_2}{2}c_{1+2} \\end{bmatrix}\n",
    "\\end{equation}\n",
    "The center of mass of the double pendulum becomes\n",
    "\\begin{equation}\n",
    "p_c = \\frac{m_1}{m_2+m_2}p_1 + \\frac{m_2}{m_1 + m_2} p_2.\n",
    "\\end{equation}\n",
    "## The velocities of the links\n",
    "The velocity of the centers of mass:\n",
    "\\begin{equation}\n",
    "\\dot{p}_1 = \\begin{bmatrix} \\dot{q}_3 + \\frac{l_1}{2}c_1\\dot{q}_1\\\\ \\dot{q}_4 + \\frac{l_1}{2}s_1\\dot{q}_1 \\end{bmatrix} = \\begin{bmatrix} \\frac{l_1}{2}c_1 & 0 & 1  & 0\\\\ \\frac{l_1}{2}s_1 & 0 & 0 & 1 \\end{bmatrix} \\begin{bmatrix} \\dot{q}_1\\\\ \\dot{q}_2\\\\ \\dot{q}_3\\\\ \\dot{q}_4 \\end{bmatrix} = V_1(q)\\dot{q}\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "\\dot{p}_2 = \\begin{bmatrix} \\dot{q}_3 - l_1c_1\\dot{q}_1 - \\frac{l_2}{2}c_{1+2}(\\dot{q}_1 + \\dot{q}_2)\\\\ \\dot{q}_4 - l_1s_1\\dot{q_1} - \\frac{l_2}{2}s_{1+2}(\\dot{q}_1 + \\dot{q}_2) \\end{bmatrix} = \\begin{bmatrix} -l_1c_1 - \\frac{l_2}{2}c_{1+2} & - \\frac{l_2}{2}c_{1+2} & 1 & 0\\\\ -l1s_1- \\frac{l_2}{2}s_{1+2} & - \\frac{l_2}{2}s_{1+2} & 0 & 1\\end{bmatrix} \\begin{bmatrix} \\dot{q}_1\\\\ \\dot{q}_2\\\\ \\dot{q}_3\\\\ \\dot{q}_4 \\end{bmatrix} = V_2(q)\\dot{q}\n",
    "\\end{equation}\n",
    "\n",
    "The rotational velocities:\n",
    "\\begin{equation}\n",
    "\\omega_1 = \\dot{q}_1 = \\begin{bmatrix} 1 & 0 & 0 & 0 \\end{bmatrix} \\begin{bmatrix} \\dot{q}_1\\\\ \\dot{q}_2\\\\ \\dot{q}_3\\\\ \\dot{q}_4 \\end{bmatrix} = \\Omega_1\\dot{q}.\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "\\omega_2 = \\dot{q}_1  + \\dot{q}_2 = \\begin{bmatrix} 1 & 1 & 0 & 0 \\end{bmatrix} \\begin{bmatrix} \\dot{q}_1\\\\ \\dot{q}_2\\\\ \\dot{q}_3\\\\ \\dot{q}_4 \\end{bmatrix} = \\Omega_2\\dot{q}.\n",
    "\\end{equation}\n",
    "\n",
    "## The kinetic energy of the pendulum\n",
    "Both links contribute to the kinetic energy. For each link, the kinetic energy consists of two parts: the translational velocity of the center of mass, and the rotational  velocity of the link. Recall that the moment of inertia of a rod for rotations about its center of mass is $m\\frac{l^2}{12}$. We get\n",
    "\\begin{equation}\n",
    "T = \\frac{m_1}{2} \\dot{q}^T V_1(q)^T V_1(q) \\dot{q} + \\frac{m_2}{2} \\dot{q}^T V_2(q)^T V_2(q) \\dot{q} + \\frac{m_1l_1^2}{24}\\dot{q}^T\\Omega_1^T\\Omega_1\\dot{q} + \\frac{m_2l_2^2}{24}\\dot{q}^T\\Omega_2^T\\Omega_2\\dot{q} = \\frac{1}{2} \\dot{q}^T H(q) \\dot{q}, \n",
    "\\end{equation}\n",
    "where the mass matrix $H$ is \n",
    "\\begin{equation}\n",
    "H(q) = m_1\\left(V_1(q)^T V_1(q) + \\frac{l_1^2}{12}\\Omega_1^T\\Omega_1\\right) + m_2\\left(V_2(q)^T V_2(q) + \\frac{l_2^2}{12}\\Omega_2^T\\Omega_2\\right) \n",
    "\\end{equation}\n",
    "\n",
    "## The potential energy\n",
    "The potential energy of the double-pendulum is\n",
    "\\begin{equation}\n",
    "U(q) = m_1gy_1 + m_2gy_2 = m_1g(q_4 +\\frac{l_1}{2}c_1) + m_2g(q_4 + l_1c_1 + \\frac{l_2}{2}c_{1+2})\n",
    "\\end{equation}\n",
    "\n",
    "## The Lagrangian and the equations of motion\n",
    "The lagrangian is defined as the difference between the kinetic and potential energy\n",
    "\\begin{equation}\n",
    "L = T-U = \\frac{1}{2} \\dot{q}^T H(q) \\dot{q} - U(q)\n",
    "\\end{equation}\n",
    "From this the four equations of motion  are obtained by the differentiation\n",
    "\\begin{equation}\n",
    "\\frac{d}{dt} \\frac {\\partial L}{\\partial \\dot{q}_i} - \\frac{\\partial L}{\\partial q_i} = \\tau_i,\n",
    "\\end{equation}\n",
    "\n",
    "The first term in the equations of motion is obtained as\n",
    "\\begin{equation}\n",
    "\\frac {\\partial L}{\\partial \\dot{q}_i} = e_i^TH(q)\\dot{q} = H_i(q)^T\\dot{q},\n",
    "\\end{equation}\n",
    "where $e_i$ is the vector that picks element $i$, i.e. a unit vetor with a one in element $i$. Hence, since the mass matrix is symmetric, $H_i(q)$ is the $i$th column of $H(q)$. \n",
    "\n",
    "Differentiating w.r.t time, we get \n",
    "\\begin{equation}\n",
    "\\frac{d}{dt} \\frac{\\partial L}{\\partial \\dot{q}_i} = H_i(q)^T\\ddot{q} + \\left(\\frac{\\partial H_i}{\\partial q}\\dot{q}\\right)^T\\dot{q} = H_i(q)^T\\ddot{q} + \\Gamma_i(q,\\dot{q})^T \\dot{q},\n",
    "\\end{equation}\n",
    "where, again, $H_i(q)$ is the $i$th column of the mass matrix $H(q)$, and $\\Gamma_i(q,\\dot{q})$ is the column vector\n",
    "\\begin{equation}\n",
    "\\Gamma_i(q, \\dot{q}) = \\frac{\\partial H_i(q)}{\\partial q} \\dot{q}.\n",
    "\\end{equation}\n",
    "The second term becomes\n",
    "\\begin{equation}\n",
    "\\frac{\\partial L}{\\partial q_i} = \\frac{1}{2}\\dot{q}^T \\frac{\\partial H(q)}{\\partial q_i} \\dot{q} - \\frac{\\partial U(q)}{\\partial q_i}.\n",
    "\\end{equation}\n",
    "\n",
    "Combining the terms for the equation of motion for coordinate $q_i$ we get\n",
    "\\begin{equation}\n",
    "H_i(q)^T\\ddot{q} + \\Gamma_i(q,\\dot{q})^T \\dot{q} - \\frac{1}{2}\\dot{q}^T \\frac{\\partial H(q)}{\\partial q_i} \\dot{q} + \\frac{\\partial u(q)}{\\partial q_i} = \\tau_i\n",
    "\\end{equation}\n",
    "or \n",
    "\\begin{equation}\n",
    "H_i(q)^T\\ddot{q} + C_i(q,\\dot{q})^T \\dot{q} + G_i(q) = \\tau_i,\n",
    "\\end{equation}\n",
    "where $C_i(q,\\dot{q})$ is the column vector\n",
    "\\begin{equation}\n",
    "C_i(q,\\dot{q}) = \\Gamma_i(q,\\dot{q}) -  \\frac{1}{2}\\frac{\\partial H(q)}{\\partial q_i}\\dot{q} = \\frac{\\partial H_i(q)}{\\partial q} \\dot{q} -  \\frac{1}{2}\\frac{\\partial H(q)}{\\partial q_i}\\dot{q} = \\left(\\frac{\\partial H_i(q)}{\\partial q} -\\frac{1}{2}\\frac{\\partial H(q)}{\\partial q_i}\\right)\\dot{q},\n",
    "\\end{equation}\n",
    "and \n",
    "\\begin{equation}\n",
    "G_i(q) = \\frac{\\partial U(q)}{\\partial q_i}.\n",
    "\\end{equation}\n",
    "Defining the matrix\n",
    "\\begin{equation}\n",
    "C(q,\\dot{q}) = \\begin{bmatrix} C_1(q,\\dot{q})^T \\\\ C_2(q,\\dot{q})^T\\\\ C_3(q,\\dot{q})^T\\\\ C_4(q,\\dot{q})^T \\end{bmatrix}\n",
    "\\end{equation}\n",
    "and the vector\n",
    "\\begin{equation}\n",
    "G(q) = \\begin{bmatrix} G_1(q) \\\\ G_2(q)\\\\ G_3(q)\\\\ G_4(q) \\end{bmatrix}\n",
    "\\end{equation}\n",
    "The equations of motion can be written on the general manipulator form \n",
    "\\begin{equation}\n",
    "H(q)\\ddot{q} + C(q, \\dot{q})\\dot{q} + G(q) = \\tau\n",
    "\\end{equation}\n",
    "\n",
    "\n",
    "## The generalized forces\n",
    "We separate the joint torques ($\\tau_1$ and $\\tau_2$, which are actuated) from the external forces ($F_x$, $F_y$), so\n",
    "\\begin{equation}\n",
    "\\tau = \\begin{bmatrix} \\tau_1 \\\\ \\tau_2 \\\\ F_x\\\\F_y \\end{bmatrix}\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import sympy as sy\n",
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# In the implementation, we will make the model more general, by allowing the moment of inertia about the \n",
    "# respective center of mass to be specified. Also, the position of the CoM from the pivot of each link\n",
    "# is given as a variable 0 < alpha_i <= 1. So, alpha_i=1 means that the CoM of the link is at the end of the link.\n",
    "\n",
    "q1, q2, q3, q4, qd1, qd2, qd3, qd4 = sy.symbols('q1, q2, q3, q4, qd1, qd2, qd3, qd4', real=True)\n",
    "qdd1, qdd2, qdd3, qdd4 = sy.symbols('qdd1, qdd2, qdd3, qdd4', real=True)\n",
    "g, m1, m2, l1, l2, I1, I2, a1, a2 = sy.symbols('g, m1, m2, l1, l2, I1, I2, alpha1, alpha2', real=True, positive=True)\n",
    "tau1, tau2, Fx, Fy = sy.symbols('tau1, tau2, Fx, Fy', real=True)\n",
    "\n",
    "\n",
    "q = sy.Matrix([[q1],[q2],[q3], [q4]])\n",
    "qd = sy.Matrix([[qd1],[qd2],[qd3], [qd4]])\n",
    "qdd = sy.Matrix([[qdd1],[qdd2],[qdd3], [qdd4]])\n",
    "tau = sy.Matrix([])\n",
    "c1 = sy.cos(q1)\n",
    "s1 = sy.sin(q1)\n",
    "c12 = sy.cos(q1+q2)\n",
    "s12 = sy.sin(q1+q2)\n",
    "\n",
    "p1 = sy.Matrix([[q3 +a1*l1*s1],[q4+a1*l1*c1]])\n",
    "p2 = sy.Matrix([[q3 +l1*s1 + a2*l2*s12],[q4+l1*c1 + a2*l2*c12]])\n",
    "V1 = sy.Matrix([[a2*l1*c1, 0, 1, 0],[-a2*l1*s1, 0, 0, 1]])\n",
    "pd1 = V1*qd\n",
    "V2 = sy.Matrix([[l1*c1+a2*l2*c12, a2*l2*c12, 1, 0 ],[-l1*s1-a2*l2*s12, -a2*l2*s12, 0, 1]])\n",
    "pd2 = V2*qd\n",
    "Omega1 = sy.Matrix([[1, 0, 0 , 0]])\n",
    "w1 = Omega1*qd\n",
    "Omega2 = sy.Matrix([[1, 1, 0 , 0]])\n",
    "w2 = Omega2*qd\n",
    "\n",
    "H = m1*V1.T*V1 + m2*V2.T*V2 + I1*Omega1.T*Omega1 + I2*Omega2.T*Omega2\n",
    "\n",
    "T = 0.5*qd.T*H*qd\n",
    "U = sy.Matrix([m1*g*p1[1] + m2*g*p2[1]])\n",
    "\n",
    "# The equations of motion on manipulator form\n",
    "C = sy.zeros(4,4)\n",
    "G = sy.zeros(4,1)\n",
    "\n",
    "for i in range(4):\n",
    "    qi = q[i]\n",
    "    Hi = H[:,i]\n",
    "    Gammai = Hi.jacobian(q)*qd\n",
    "    ddtdLdqidot = Hi.T*qdd + Gammai.T*qd\n",
    "    dHdqi = H.diff(qi)\n",
    "    Di = 0.5*dHdqi*qd\n",
    "    Gi = U.diff(qi)\n",
    "    dLdqi = Di.T*qd - Gi\n",
    "    #lhs1 = ddtdLdq1dot - dLdq1\n",
    "    # Form the terms for the manipulator form\n",
    "    Ci = Gammai - Di\n",
    "    C[i,:] = Ci.T\n",
    "    G[i] = Gi\n",
    "\n",
    "#dd\n",
    "\n",
    "lhs = H*qdd + C*qd + G\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "H\n",
      "  2        2                             2   \n",
      "l₁ ⋅m₁ + l₁ ⋅m₂ + 2⋅l₁⋅l₂⋅m₂⋅cos(q₂) + l₂ ⋅m₂\n",
      "l₂⋅m₂⋅(l₁⋅cos(q₂) + l₂)\n",
      "  2   \n",
      "l₂ ⋅m₂\n",
      "C\n",
      "                                          2        \n",
      "-2⋅l₁⋅l₂⋅m₂⋅qd₁⋅qd₂⋅sin(q₂) - l₁⋅l₂⋅m₂⋅qd₂ ⋅sin(q₂)\n",
      "G\n",
      "⎡-g⋅(l₁⋅m₁⋅sin(q₁) + l₁⋅m₂⋅sin(q₁) + l₂⋅m₂⋅sin(q₁ + q₂))⎤\n",
      "⎢                                                       ⎥\n",
      "⎢                 -g⋅l₂⋅m₂⋅sin(q₁ + q₂)                 ⎥\n",
      "⎢                                                       ⎥\n",
      "⎢                           0                           ⎥\n",
      "⎢                                                       ⎥\n",
      "⎣                      g⋅m₁ + g⋅m₂                      ⎦\n"
     ]
    }
   ],
   "source": [
    "## Verify calculations by comparing to equations of motion given explicitly in Tedrake 2009\n",
    "Htest = H.subs(I1,0).subs(I2,0).subs(a1,1).subs(a2,1)\n",
    "Ctest = C.subs(I1,0).subs(I2,0).subs(a1,1).subs(a2,1).subs(qd3,0).subs(qd4,0)\n",
    "Gtest = G.subs(I1,0).subs(I2,0).subs(a1,1).subs(a2,1).subs(qd3,0).subs(qd4,0)\n",
    "\n",
    "print \"H\"\n",
    "sy.pretty_print(sy.trigsimp(Htest[0,0]))\n",
    "sy.pretty_print(sy.trigsimp(Htest[1,0]))\n",
    "sy.pretty_print(sy.trigsimp(Htest[1,1]))\n",
    "# OK!\n",
    "\n",
    "print \"C\"\n",
    "sy.pretty_print(sy.trigsimp(Ctest[0,0])*qd[0] + sy.trigsimp(Ctest[0,1])*qd[1])\n",
    "# OK!\n",
    "\n",
    "print \"G\"\n",
    "sy.pretty_print(sy.trigsimp(Gtest))\n",
    "# OK!\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Center of mass dynamics\n",
    "The location of center of mass is\n",
    "\\begin{equation}\n",
    "c = \\begin{bmatrix} c_x\\\\c_z \\end{bmatrix} = \\frac{m_1}{m_1+m_2} p_1(q) + \\frac{m_2}{m_1+m_2} p_2(q).\n",
    "\\end{equation}\n",
    "\n",
    "Newton's equations of motion tell us that the acceleration of the CoM equals the sum of external forces acting on the system, and the rate of change of angular momentum of the body w.r.t CoM equals the sum of torques of the external forces w.r.t the CoM.\n",
    "\n",
    "Let $L$ denote the angular momentum of the body, let \n",
    "\\begin{equation}\n",
    "F = \\begin{bmatrix} F_a\\\\M_a \\end{bmatrix}\n",
    "\\end{equation}\n",
    "be the external force and torque applied at the base joint (feet), and \n",
    "\\begin{equation}\n",
    "a(q) = \\begin{bmatrix} a_x\\\\ a_z \\end{bmatrix} = p_b(q) - c(q)\n",
    "\\end{equation}\n",
    "be the vector from the CoM to the base joint where the force is applied. The torque due to the force $F_a$ w.r.t CoM is then\n",
    "\\begin{equation}\n",
    "    M_y = a_zF_x - a_xF_z\n",
    "\\end{equation}\n",
    "\n",
    "The equations of motion can then be written\n",
    "\\begin{equation}\n",
    "\\begin{bmatrix}  D_1\\\\ D_2 \\end{bmatrix} F = \\begin{bmatrix} (m_1+m_2)\\ddot{c} - (m_1+m_2)g\\\\ \\dot{L} \\end{bmatrix},\n",
    "\\end{equation}\n",
    "where\n",
    "\\begin{equation}\n",
    "D_1 = \\begin{bmatrix} I & 0 \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "D_2 = \\begin{bmatrix} a_z & -a_x & 1 \\end{bmatrix}.\n",
    "\\end{equation}\n",
    "The vector $g$ is the gravity vector which points in the negative $z$-direction, i.e.\n",
    "\\begin{equation}\n",
    "g = \\begin{bmatrix} 0\\\\-9.8 \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "Introducing\n",
    "\\begin{equation}\n",
    "K(q) = \\begin{bmatrix} D_1\\\\ D_2(q) \\end{bmatrix},\n",
    "\\end{equation}\n",
    "and\n",
    "\\begin{equation}\n",
    "u = \\begin{bmatrix} (m_1+m_2)\\ddot{c} - (m_1+m_2)g\\\\ \\dot{L} \\end{bmatrix},\n",
    "\\end{equation}\n",
    "the equation of motion of the CoM can be written compactly\n",
    "\\begin{equation}\n",
    "K(q)F = u.\n",
    "\\end{equation}\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Fa,Ma,L,Ld,cddx,cddz = sy.symbols('Fa, Ma, L, Ld, cddx, cddz', real=True)\n",
    "m = m1+m2\n",
    "pc = m1/m * p1 + m2/m * p2\n",
    "pb = sy.Matrix([q3,q4]) # Column vector\n",
    "pa = pb-pc\n",
    "D1 = sy.Matrix([[1, 0, 0], [0, 1, 0]])\n",
    "D2 = sy.Matrix([[pa[1], -pa[0], 1]])\n",
    "gvec = g*sy.Matrix([0,-1])\n",
    "\n",
    "# Abbreviation as in Stephens and Atkeson 2010: KF = u\n",
    "K = D1.col_join(D2)\n",
    "u = (m*sy.Matrix([cddx, cddz])-m*gvec).col_join(sy.Matrix([[Ld]]))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Determining allowable external forces that give desired CoM acceleration\n",
    "The right hand side of the equations of motion\n",
    "\\begin{equation}\n",
    "u = \\begin{bmatrix} (m_1+m_2)\\ddot{c} - (m_1+m_2)g\\\\ \\dot{L} \\end{bmatrix},\n",
    "\\end{equation}\n",
    "consists of known quantities ($m_1$, $m_2$, $g$) and specified, desired quantities ($\\ddot{c}$ and $\\dot{L}$).\n",
    "On the left hand side, the matrix $K(q)$ depends on the current configuration $q$, since\n",
    "\\begin{equation}\n",
    "K(q) = \\begin{bmatrix} D_1\\\\ D_2(q) \\end{bmatrix} = \\begin{bmatrix} 1 & 0 & 0\\\\0 & 1 & 0\\\\ a_z(q) & -a_x(q) & 1 \\end{bmatrix}.\n",
    "\\end{equation}\n",
    "\n",
    "\n",
    "## Constraints\n",
    "Not all forces found as solutions to \n",
    "\\begin{equation}\n",
    "K(q)F = u\n",
    "\\end{equation}\n",
    "are allowed, since they must satisfy constraints on the support polygon (base of support) and friction.\n",
    "<!--- ![Two-link inverted pendulum](./figures/two-link.png \"Two-link inverted pendulum\") -->\n",
    "<img src=\"./figures/BoS-constraints.png\" alt=\"Constraints due to the Base of Support\" style=\"width: 600px;\"/>\n",
    "For the base of support, the constraint takes the form\n",
    "\\begin{equation}\n",
    "-d_x^- \\le -\\frac{M_y}{F_z} \\le d_x^+,\n",
    "\\end{equation}\n",
    "OBS: In Stephens and Atkeson eq (10) there is no minus sign in front of $d_x^-$, meaning it must be a negative number. Here, we take it to be a distance, hence positive.\n",
    "\n",
    "$d_x^-$ is the lower limit of the base of support, and $d_x^+$ is the upper limit. This constraint is equivalent to\n",
    "\\begin{equation}\n",
    "\\frac{M_y}{F_z} \\le d_x^-\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "-\\frac{M_y}{F_z} \\le d_x^+,\n",
    "\\end{equation}\n",
    "which can be written \n",
    "\\begin{equation}\n",
    "M_y - F_z d_x^- \\le 0\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "-M_y - F_z d_x^+ \\le 0\n",
    "\\end{equation}\n",
    "\n",
    "\n",
    "The constraint of friction in 3D is nonlinear. It states that the relative magnitude of the horizontal to the vertical force must be less than a fricion coefficient $\\mu$ specific for the interface between feet and ground.\n",
    "\\begin{equation}\n",
    "\\left| \\frac{\\sqrt{F_x^2 + F_y^2}}{F_z} \\right | \\le \\mu\n",
    "\\end{equation}\n",
    "In the 2D case the constraint is linear, and can be written\n",
    "\\begin{equation}\n",
    "\\left| \\frac{F_x}{F_z} \\right | \\le \\mu,\n",
    "\\end{equation}\n",
    "or\n",
    "\\begin{equation}\n",
    "\\frac{F_x}{F_z} \\le \\mu\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "-\\frac{F_x}{F_z} \\le \\mu\n",
    "\\end{equation}\n",
    "which can be written\n",
    "\\begin{equation}\n",
    "F_x - \\mu F_z \\le 0\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "-F_x - \\mu F_z \\le 0.\n",
    "\\end{equation}\n",
    "where it is assumed that $F_z$ is positive.\n",
    "\n",
    "The constraints can be written as the linear inequality\n",
    "\\begin{equation}\n",
    "BF \\le 0,\n",
    "\\end{equation}\n",
    "where\n",
    "\\begin{equation}\n",
    "B = \\begin{bmatrix} 0 & -d_x^- & 1\\\\ 0 & -dx^+ &-1\\\\ 1 & -\\mu & 0\\\\ -1 & -\\mu & 0 \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "# Optimization problem\n",
    "Determining the required contact forces can be formulated as the constrainted optimization problem (Stephens and Atkeson eq (15))\n",
    "\\begin{equation}\n",
    "\\hat{F} = \\arg \\min_F (KF - u)^T(KF-u) + F^TWF,\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "\\text{s.t.} \\quad BF \\le 0,\n",
    "\\end{equation}\n",
    "The matrix $W$ can be used to weight the force differently, for instance to penalize horizontal forces or torques at the ankle joint. \n",
    "\n",
    "The criterion can be rewritten as \n",
    "\\begin{equation}\n",
    "f(F) =  (KF - u)^T(KF-u) + F^TWF = F^T(W + K^TK)F - 2u^T K F + u^Tu,\n",
    "\\end{equation}\n",
    "the last term does not depend on $F$ and can be ignored in the optimization. This gives the quadratic programming problem (QP)\n",
    "\\begin{equation}\n",
    " \\text{minimize} \\quad F^T(W + K^TK)F - 2u^T K F,\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "\\text{s.t.} \\quad BF \\le 0.\n",
    "\\end{equation}\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡6.70422222222222     0    -3.06666666666667⎤\n",
      "⎢                                           ⎥\n",
      "⎢        0          2.002          0        ⎥\n",
      "⎢                                           ⎥\n",
      "⎣-3.06666666666667    0          2.002      ⎦\n",
      "⎡763.333333333333⎤\n",
      "⎢                ⎥\n",
      "⎢    -1469.85    ⎥\n",
      "⎢                ⎥\n",
      "⎣     -400.0     ⎦\n",
      "⎡0   -0.08  1 ⎤\n",
      "⎢             ⎥\n",
      "⎢0   -0.16  -1⎥\n",
      "⎢             ⎥\n",
      "⎢1   -0.6   0 ⎥\n",
      "⎢             ⎥\n",
      "⎣-1  -0.6   0 ⎦\n",
      "object\n",
      "     pcost       dcost       gap    pres   dres\n",
      " 0: -5.4194e+05 -6.1733e+05  8e+04  2e-14  3e-01\n",
      " 1: -5.8469e+05 -5.8861e+05  4e+03  1e-13  4e-03\n",
      " 2: -5.8490e+05 -5.8504e+05  1e+02  2e-13  1e-04\n",
      " 3: -5.8498e+05 -5.8498e+05  4e+00  8e-14  1e-06\n",
      " 4: -5.8498e+05 -5.8498e+05  4e-02  4e-14  1e-08\n",
      "Optimal solution found.\n",
      "Constrained solution\n",
      "[-8.70e+01]\n",
      "[ 7.35e+02]\n",
      "[ 5.88e+01]\n",
      "\n",
      "Unconstrained solution\n",
      "[-7.51e+01]\n",
      "[ 7.34e+02]\n",
      "[ 8.48e+01]\n",
      "\n"
     ]
    }
   ],
   "source": [
    "import cvxopt as cvx\n",
    "\n",
    "dxmin,dxplus,mu = sy.symbols('dxmin, dxplus, mu')\n",
    "# The general formula for the QP when using cvxopt is\n",
    "# minimize  0.5 x^T Q x + p x,\n",
    "# subject to G x <= h,     B x z= 0 here\n",
    "#            A x = b       not used here\n",
    "# Determine matrices for the QP\n",
    "lmbda = 0.001  # Used to make sure the weighting matrix W is not too big wrt K^TK\n",
    "WW = sy.Matrix([[1, 0, 0],[0, 1, 0],[0,0,1]])*lmbda\n",
    "QQ = 2*(WW + K.T*K)\n",
    "pp = (-2*u.T*K).T\n",
    "GG = sy.Matrix([[0, -dxmin, 1], [0, -dxplus, -1], [1, -mu, 0], [-1, -mu, 0]])\n",
    "hh = sy.Matrix([0,0,0,0])\n",
    "\n",
    "# Numerical example using cvxopt\n",
    "#q1_ = np.pi/12\n",
    "q1_ = 0\n",
    "q2_ = 0\n",
    "q3_ = 0.5\n",
    "q4_ = 0.5\n",
    "cdd = 1 # Magnitude of desired acceleration\n",
    "cddx_ = -1.\n",
    "cddz_ = 0.\n",
    "Ld_ = 200.\n",
    "\n",
    "g_ = 9.799\n",
    "m1_ = 35.\n",
    "m2_ = 40.\n",
    "l1_ = 1.\n",
    "l2_ = 1.\n",
    "alpha1_ = 1.\n",
    "alpha2_ = 1.\n",
    "mu_ = 0.6\n",
    "dxmin_ = 0.08\n",
    "dxplus_ = 0.16\n",
    "\n",
    "# Substitue numerical values\n",
    "QQ_ = QQ.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)\n",
    "QQ_ = QQ_.subs(a2, alpha2_)\n",
    "pp_ = pp.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)\n",
    "pp_ = pp_.subs(a2, alpha2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_)\n",
    "GG_ = GG.subs(dxmin, dxmin_).subs(dxplus, dxplus_).subs(mu, mu_)\n",
    "sy.pretty_print(QQ_)\n",
    "sy.pretty_print(pp_)\n",
    "sy.pretty_print(GG_)\n",
    "\n",
    "# Convert to numpy\n",
    "QQn = sy.matrix2numpy(QQ_)\n",
    "ppn = sy.matrix2numpy(pp_)\n",
    "GGn = sy.matrix2numpy(GG_)\n",
    "hhn = sy.matrix2numpy(hh)\n",
    "\n",
    "# And then to cvxopt matrices\n",
    "print QQn.dtype\n",
    "QQc = cvx.matrix(QQn.astype(np.float64))\n",
    "ppc = cvx.matrix(ppn.astype(np.float64))\n",
    "GGc = cvx.matrix(GGn.astype(np.float64))\n",
    "hhc = cvx.matrix(hhn.astype(np.float64))\n",
    "\n",
    "sol=cvx.solvers.qp(QQc, ppc, GGc, hhc)\n",
    "# Try also unconstrained problem\n",
    "solU = cvx.solvers.qp(QQc, ppc)\n",
    "print \"Constrained solution\"\n",
    "print sol['x']\n",
    "print \"Unconstrained solution\"\n",
    "print solU['x']\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Verify solution to required contact forces, using standard single pendulum model\n",
    "\n",
    "If the body is static in a certain position, and we require it to be static, then the desired acceleration is zero $\\ddot{c}= 0$ and the desired change in angular momentum is also zero, $\\dot{L} = 0$. \n",
    "\n",
    "The force and momentum at the contact point (foot) must then be such that they balance the force of gravity, while achieving no acceleration of the body. The horizontal force must be zero, and the reaction force must go through the center of mass. This means that the center of pressure is directly below the center of mass. \n",
    "\n",
    "The result from the optimization is a vector $\\hat{F}$ consisting of the horizontal and vertical forces, and the moment about the $y$ - - axis. \n",
    "\\begin{equation}\n",
    "\\hat{F} = \\begin{bmatrix} \\hat{F}_x\\\\ \\hat{F}_z \\\\ \\hat{M}_y \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "Consider a person (or robot) standing facing in the $x$ - direction. If $\\hat{M}_y>0$, we must have a center of pressure which is behind the ankle joint. The person is leaning backwards, and the distance must be\n",
    "\\begin{equation}\n",
    "\\hat{d}_x = \\frac{\\hat{M}_y}{\\hat{F}_z}.\n",
    "\\end{equation}\n",
    "This must be the same as the horizontal distance between the CoM and the base joint.\n",
    "\n",
    "Let's check this below.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/kjartan/anaconda2/lib/python2.7/site-packages/pyface/ui/wx/init.py:25: wxPyDeprecationWarning: Using deprecated class PySimpleApp. \n",
      "  _app = wx.PySimpleApp()\n",
      "/home/kjartan/anaconda2/lib/python2.7/site-packages/pyface/ui/wx/init.py:29: wxPyDeprecationWarning: Call to deprecated item 'InitAllImageHandlers'. \n",
      "  wx.InitAllImageHandlers()\n"
     ]
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import double_pendulum as dpu"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "     pcost       dcost       gap    pres   dres\n",
      " 0: -5.0689e+05 -5.6534e+05  6e+04  4e-14  4e-01\n",
      " 1: -5.5086e+05 -5.5555e+05  5e+03  9e-14  3e-17\n",
      " 2: -5.5149e+05 -5.5155e+05  6e+01  9e-14  3e-17\n",
      " 3: -5.5150e+05 -5.5150e+05  6e-01  6e-14  5e-17\n",
      " 4: -5.5150e+05 -5.5150e+05  6e-03  2e-13  1e-16\n",
      "Optimal solution found.\n",
      "Constrained solution\n",
      "[ 7.37e+01]\n",
      "[ 7.35e+02]\n",
      "[ 5.88e+01]\n",
      "\n",
      "Unconstrained solution\n",
      "[ 1.50e+02]\n",
      "[ 7.34e+02]\n",
      "[ 2.11e+02]\n",
      "\n",
      "⎡  m₁⋅(α₁⋅l₁⋅sin(q₁) + q₃)   m₂⋅(α₂⋅l₂⋅sin(q₁ + q₂) + l₁⋅sin(q₁) + q₃)     ⎤\n",
      "⎢- ─────────────────────── - ───────────────────────────────────────── + q₃⎥\n",
      "⎢          m₁ + m₂                            m₁ + m₂                      ⎥\n",
      "⎢                                                                          ⎥\n",
      "⎢  m₁⋅(α₁⋅l₁⋅cos(q₁) + q₄)   m₂⋅(α₂⋅l₂⋅cos(q₁ + q₂) + l₁⋅cos(q₁) + q₄)     ⎥\n",
      "⎢- ─────────────────────── - ───────────────────────────────────────── + q₄⎥\n",
      "⎣          m₁ + m₂                            m₁ + m₂                      ⎦\n",
      "My/Fz - ax = 0.017416\n"
     ]
    }
   ],
   "source": [
    "reload(dpu)\n",
    "# Set desired accelerations to zero\n",
    "# Numerical example using cvxopt\n",
    "#q1_ = np.pi/12\n",
    "alpha1_ = 0.7\n",
    "alpha2_ = 0.6\n",
    "q1_ = -np.pi/12\n",
    "q2_ = np.pi/4\n",
    "q3_ = 0.5\n",
    "q4_ = 0.5\n",
    "# Desired acceleration\n",
    "cddx_ = 2.\n",
    "cddz_ = 0.\n",
    "Ld_ = 0.\n",
    "\n",
    "# Substitue numerical values\n",
    "QQ_ = QQ.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)\n",
    "QQ_ = QQ_.subs(a2, alpha2_)\n",
    "pp_ = pp.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)\n",
    "pp_ = pp_.subs(a2, alpha2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_)\n",
    "GG_ = GG.subs(dxmin, dxmin_).subs(dxplus, dxplus_).subs(mu, mu_)\n",
    "\n",
    "# Convert to numpy\n",
    "QQn = sy.matrix2numpy(QQ_, dtype=np.float64)\n",
    "ppn = sy.matrix2numpy(pp_, dtype=np.float64)\n",
    "GGn = sy.matrix2numpy(GG_, dtype=np.float64)\n",
    "hhn = sy.matrix2numpy(hh, dtype=np.float64)\n",
    "\n",
    "# And then to cvxopt matrices\n",
    "QQc = cvx.matrix(QQn)\n",
    "ppc = cvx.matrix(ppn)\n",
    "GGc = cvx.matrix(GGn)\n",
    "hhc = cvx.matrix(hhn)\n",
    "\n",
    "sol=cvx.solvers.qp(QQc, ppc, GGc, hhc)\n",
    "# Try also unconstrained problem\n",
    "solU = cvx.solvers.qp(QQc, ppc)\n",
    "print \"Constrained solution\"\n",
    "print sol['x']\n",
    "print \"Unconstrained solution\"\n",
    "print solU['x']\n",
    "\n",
    "# Verify that My/Fz equals distance in x-dir between CoM and base joint\n",
    "sy.pretty_print(pa)\n",
    "pa_ = pa.subs(q1, q1_).subs(q2, q2_).subs(q3, q3_).subs(q4, q4_).subs(a1, alpha1_).subs(a2, alpha2_)\n",
    "pa_ = pa_.subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_)\n",
    "pa_n = sy.matrix2numpy(pa_, dtype=np.float64)\n",
    "\n",
    "print \"My/Fz - ax = %f\" % (sol['x'][2]/sol['x'][1] - pa_n[0])\n",
    "\n",
    "Fx = sol['x'][0]\n",
    "Fz = sol['x'][1]\n",
    "My = sol['x'][2]\n",
    "\n",
    "#Fx = solU['x'][0]\n",
    "#Fz = solU['x'][1]\n",
    "#My = solU['x'][2]\n",
    "\n",
    "dp = dpu.DoublePendulum()\n",
    "dp.L1 = l1_\n",
    "dp.L2 = l2_\n",
    "dp.a1 = alpha1_\n",
    "dp.a2 = alpha2_\n",
    "dp.m1 = m1_\n",
    "dp.m2 = m2_\n",
    "animation = dpu.DoublePendulumAnimation(dp)\n",
    "animation.init_dp(q1_, q2_, q3_, q4_, Fx, Fz, My)\n",
    "\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "det K = 1.000000\n",
      "uhat = \n",
      "⎡-0.116569624340393⎤\n",
      "⎢                  ⎥\n",
      "⎢ 734.17651532825  ⎥\n",
      "⎢                  ⎥\n",
      "⎣-0.102374946774901⎦\n",
      "u_n =\n",
      "[[   0.   ]\n",
      " [ 734.925]\n",
      " [   0.   ]]\n",
      "F_n =\n",
      "[[  5.19626687e-15]\n",
      " [  7.34925000e+02]\n",
      " [  1.02714797e+02]]\n",
      "uhat_n = \n",
      "[[  5.19626687e-15]\n",
      " [  7.34925000e+02]\n",
      " [  0.00000000e+00]]\n",
      "K_ = \n",
      "⎡       1                  0           0⎤\n",
      "⎢                                       ⎥\n",
      "⎢       0                  1           0⎥\n",
      "⎢                                       ⎥\n",
      "⎣-1.1397924750211  -0.139762284355361  1⎦\n",
      "K'*K = \n",
      "⎡2.29912688611473        0.1593         -1.1397924750211 ⎤\n",
      "⎢                                                        ⎥\n",
      "⎢     0.1593        1.01953349612823   -0.139762284355361⎥\n",
      "⎢                                                        ⎥\n",
      "⎣-1.1397924750211  -0.139762284355361          1         ⎦\n"
     ]
    }
   ],
   "source": [
    "# Check solution\n",
    "Fhat = sy.Matrix(solU['x'])\n",
    "K_ = K.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)\n",
    "K_ = K_.subs(a2, alpha2_)\n",
    "# Convert to numpy\n",
    "Kn = sy.matrix2numpy(K_, dtype = np.float64)\n",
    "print \"det K = %f\" % np.linalg.det(Kn)\n",
    "print \"uhat = \"\n",
    "uhat = K_*Fhat\n",
    "\n",
    "sy.pretty_print(uhat)\n",
    "un = sy.matrix2numpy(u.subs(m1, m1_).subs(m2,m2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_),\n",
    "                    dtype = np.float64)\n",
    "print \"u_n =\"\n",
    "print un\n",
    "Fn = np.linalg.solve(Kn, un)\n",
    "print \"F_n =\"\n",
    "print Fn\n",
    "\n",
    "uhatn = np.dot(Kn, Fn)\n",
    "print \"uhat_n = \"\n",
    "print uhatn\n",
    "\n",
    "print \"K_ = \"\n",
    "sy.pretty_print(K_)\n",
    "print \"K'*K = \"\n",
    "sy.pretty_print(K_.T*K_)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'c' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-9-c34e0a83ce98>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0msy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mpretty_print\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mc\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      2\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      3\u001b[0m \u001b[0msy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mpretty_print\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mu\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[0msy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mpretty_print\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mK\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[0msy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mpretty_print\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mu\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msubs\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mm1\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mm1_\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msubs\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mm2\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mm2_\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msubs\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mcddx\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcddx_\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msubs\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mcddz\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcddz_\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msubs\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mLd\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mLd_\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msubs\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mg\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mg_\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mNameError\u001b[0m: name 'c' is not defined"
     ]
    }
   ],
   "source": [
    "sy.pretty_print(c)\n",
    "\n",
    "sy.pretty_print(u)\n",
    "sy.pretty_print(K)\n",
    "sy.pretty_print(u.subs(m1, m1_).subs(m2,m2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Jacobian of base joint\n",
    "The position of the base joint (ankle joint) is simply\n",
    "\\begin{equation}\n",
    "p_b = \\begin{bmatrix} 0 & 0 & 1 & 0\\\\ 0 & 0 & 0 & 1\\end{bmatrix} \\begin{bmatrix} q_1\\\\q_2\\\\q_3\\\\q_4\\end{bmatrix}\n",
    "\\end{equation}\n",
    "and the velocity\n",
    "\\begin{equation}\n",
    "v_b = \\begin{bmatrix} 0 & 0 & 1 & 0\\\\ 0 & 0 & 0 & 1\\end{bmatrix} \\begin{bmatrix} \\dot{q}_1\\\\\\dot{q}_2\\\\\\dot{q}_3\\\\\\dot{q}_4\\end{bmatrix}\n",
    "\\end{equation}\n",
    "The feet are asumed to be parallell to the ground, so the orientation is constant\n",
    "\\begin{equation}\n",
    "\\omega_b = 0\n",
    "\\end{equation}\n",
    "\n",
    "Thus, the Jacobian is the matrix $J$ that transforms generalized velocities into velocity of the base joint \n",
    "\\begin{equation}\n",
    "\\begin{bmatrix} v_b\\\\\\omega_b \\end{bmatrix} = J \\dot{q} = \\begin{bmatrix} 0 & I_2\\\\0 & 0\\end{bmatrix} \\dot{q}. \n",
    "\\end{equation}\n",
    "\n",
    "# Constraint equation for the generalized coordinates\n",
    "The equations of motion for the mechanism must be accompanied by a constraint equation obtained by differentiating the equation\n",
    "\\begin{equation}\n",
    "\\begin{bmatrix} v_b\\\\\\omega_b \\end{bmatrix} = J \\dot{q} = \\begin{bmatrix} 0 & I_2\\\\0 & 0\\end{bmatrix} \\dot{q}. \n",
    "\\end{equation}\n",
    "This leads to \n",
    "\\begin{equation}\n",
    "\\begin{bmatrix} \\dot{v}_b\\\\\\dot{\\omega}_b \\end{bmatrix} = J \\ddot{q} + \\dot{J}\\dot{q}. \n",
    "\\end{equation}\n",
    "Specifically, in our case\n",
    "\\begin{equation}\n",
    "\\begin{bmatrix} \\dot{v}_b\\\\\\dot{\\omega}_b \\end{bmatrix} = \\begin{bmatrix} 0 & I_2\\\\0 & 0\\end{bmatrix} \\ddot{q}. \n",
    "\\end{equation}\n",
    "The constraint equation simply tells us that the acceleration in the last two generalized coordinates (the position of the base) must be zero, since we assume that the feet are firmly on the ground. \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dynamic balance force control\n",
    "We have worked out most of the preliminary detials, and are now ready to derive the dynamics balance force control.\n",
    "\n",
    "## Augmented equations of motion\n",
    "The equations of motion of the mechanism with the constraint equation is\n",
    "\\begin{equation}\n",
    "H(q)\\ddot{q} + C(q, \\dot{q})\\dot{q} + G(q) = S\\tau + J^T(q)F\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "J\\ddot{q} + \\dot{J}\\dot{q} = \\ddot{P}_b,\n",
    "\\end{equation}\n",
    "where $\\ddot{P}_b$ is the translational and angular acceleration of the base (the feet). The matrix $S$ picks the elements of $\\tau$ that are actuated. In our case, since only the joints are actuated, not the position of the feet, gives\n",
    "\\begin{equation}\n",
    "S = \\begin{bmatrix} I_2 & 0\\\\ 0 & 0 \\end{bmatrix}.\n",
    "\\end{equation}\n",
    "The transpose of the Jacobian $J^T(q)$ relates the external force acting at the feet to forces at the joints. In our case this becomes\n",
    "\\begin{equation}\n",
    "J^T(q) = \\begin{bmatrix} 0 & 0\\\\ I_2 & 0\\end{bmatrix},\n",
    "\\end{equation}\n",
    "and is not dependent on the generalized coordinates $q$. The transposed Jacobian tells us that of the forces and moment acting on the feet \n",
    "\\begin{equation}\n",
    "F = \\begin{bmatrix} F_x\\\\F_z\\\\M_y \\end{bmatrix},\n",
    "\\end{equation}\n",
    "none give (or require) any torques at the joints. Only the generalized coordinates corresponding to the position of the feet are affected, and only by the forces, not by $M_y$.  \n",
    "\n",
    "The equations of motion of the mechanism are augmented with the equations of motion of the center of mass. The unknowns in this equation are the generalized accelerations $\\ddot{q}$, the generalized forces $\\tau$ and the external force $F$. The state of the robot, i.e. $q$ and $\\dot{q}$, is assumed to be known. This gives\n",
    "\\begin{equation}\n",
    "\\begin{bmatrix}\n",
    "H(q) &  -S & -J^T\\\\\n",
    "J(q) & 0 & 0\\\\\n",
    "0 & 0 & D_1\\\\\n",
    "0 & 0 & D_2\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix} \\ddot{q} \\\\ \\tau \\\\ F \\end{bmatrix}\n",
    "= \\begin{bmatrix} -N(q, \\dot{q}) \\\\ \\ddot{P}_{\\text{des}} - \\dot{J}\\dot{q} \\\\ m\\ddot{c}_{\\text{des}} + F_g, \\\\ \\dot{L}_{\\text{des}} \\end{bmatrix}\n",
    "\\end{equation}\n",
    "where $N(q, \\dot{q})=C(q,\\dot{q})\\dot{q} + G(q)$. \n",
    "\n",
    "Now, if we solve separately for $F$ using the formulation as an QP in the previous section, we can simplify the equations to \n",
    "\\begin{equation}\n",
    "\\begin{bmatrix}\n",
    "H(q) & -S\\\\\n",
    "J & 0\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix} \\ddot{q} \\\\ \\tau \\end{bmatrix}\n",
    "= \\begin{bmatrix} -N(q, \\dot{q}) + J^T\\hat{F}\\\\ -\\dot{J}\\dot{q} \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "In the case of our floating double pendulum, this equation becomes\n",
    "\\begin{equation}\n",
    "\\begin{bmatrix}\n",
    "H(q) & -\\begin{bmatrix} I_2 & 0\\\\ 0 & 0 \\end{bmatrix}\\\\\n",
    "\\begin{bmatrix} 0 & I_2\\\\0 & 0\\end{bmatrix} & 0\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix} \\ddot{q} \\\\ \\tau \\end{bmatrix}\n",
    "= \\begin{bmatrix} -N(q, \\dot{q}) + \\begin{bmatrix} 0 & 0\\\\ I_2 & 0\\end{bmatrix}\\hat{F}\\\\ 0  \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "The system of equation can be written \n",
    "\\begin{equation}\n",
    "Dz = f,\n",
    "\\end{equation}\n",
    "where $D$ is $8 \\times 8$. It can be solved, with regularization matrix $W$ with a pseudo-inverse\n",
    "\\begin{equation}\n",
    "z = (D^TD + W)^{-1}D^Tf.\n",
    "\\end{equation}\n",
    "\n",
    "The solution is \n",
    "\\begin{equation}\n",
    "z = \\begin{bmatrix} \\ddot{q} \\\\ \\tau_{\\text{dbfc}} \\end{bmatrix},\n",
    "\\end{equation}\n",
    "where $\\tau_{\\text{dbfc}}$ are the generalized torques used for feedback control. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Let's implement the equations and test a few things. \n",
    "N = C*qd + G\n",
    "J = sy.Matrix([[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]])\n",
    "S = sy.Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]])\n",
    "Fxs, Fzs, Mys = sy.symbols('Fx, Fz, My') \n",
    "Fhat = sy.Matrix([Fxs, Fzs, Mys])\n",
    "Dr1 = H.row_join(-S)\n",
    "Dr2 = J.row_join(sy.zeros(3,4))\n",
    "D = Dr1.col_join(Dr2)\n",
    "f1 = -N+J.T*Fhat\n",
    "f = f1.col_join(sy.zeros(3,1))\n",
    "\n",
    "# Substitue numerical values\n",
    "qd1_ = 0.1\n",
    "qd2_ = 0.2\n",
    "qd3_ = 0\n",
    "qd4_ = 0\n",
    "I1_ = 1\n",
    "I2_ = 2\n",
    "numvals = {q1:q1_, q2:q2_, q3:q3_, q4:q4_, qd1:qd1_, qd2:qd2_, qd3:qd3_, qd4:qd4_,\n",
    "           m1:m1_, m2:m2_, l1:l1_, l2:l2_, a1:alpha1_, a2:alpha2_, I1:I1_, I2:I2_,\n",
    "           g:9.8, Fxs:10, Fzs:800, Mys:100}\n",
    "\n",
    "D_ = D.subs(numvals)\n",
    "f_ = f.subs(numvals)\n",
    "sy.pretty_print(D_)\n",
    "sy.pretty_print(f_)\n",
    "print D_.rank()\n",
    "Wn = 0.1*np.eye(8)\n",
    "Dn = sy.matrix2numpy(D_, dtype=np.float64)\n",
    "fn = sy.matrix2numpy(f_, dtype=np.float64)\n",
    "\n",
    "z = np.linalg.solve(np.dot(Dn.T, Dn) + Wn, np.dot(Dn.T, fn))\n",
    "print z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.linalg.solve?"
   ]
  },
  {
   "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.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}