spherical_cartpole.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y, theta, phi = sympy.symbols('x, y, theta, phi', cls=sympy.Function)\n",
    "m_p, m_c, r, g, t = sympy.symbols('m_p, m_c, r, g, t')\n",
    "k_x, k_y, k_theta, k_phi = sympy.symbols('k_x, k_y, k_theta, k_phi')\n",
    "\n",
    "x_d = sympy.Derivative(x(t), t)\n",
    "y_d = sympy.Derivative(y(t), t)\n",
    "theta_d = sympy.Derivative(theta(t), t)\n",
    "phi_d = sympy.Derivative(phi(t), t)\n",
    "\n",
    "x_dd = sympy.Derivative(x(t), (t, 2))\n",
    "y_dd = sympy.Derivative(y(t), (t, 2))\n",
    "theta_dd = sympy.Derivative(theta(t), (t, 2))\n",
    "phi_dd = sympy.Derivative(phi(t), (t, 2))\n",
    "\n",
    "f_x, f_y = sympy.symbols('f_x, f_y')\n",
    "\n",
    "sin = sympy.sin\n",
    "cos = sympy.cos\n",
    "\n",
    "q_c = sympy.Matrix([x(t), y(t), 0])\n",
    "q_p = sympy.Matrix([\n",
    "    r*sin(theta(t))*sin(phi(t)) + x(t),\n",
    "    -r*sin(theta(t))*cos(phi(t)) + y(t),\n",
    "    -r*cos(theta(t))\n",
    "])\n",
    "\n",
    "q_c_d = sympy.Derivative(q_c, t).doit()\n",
    "q_p_d = sympy.Derivative(q_p, t).doit()\n",
    "\n",
    "K = (1/2)*m_p*(q_p_d[0]**2 + q_p_d[1]**2 + q_p_d[2]**2) + (1/2)*m_c*(q_c_d[0]**2 + q_c_d[1]**2)\n",
    "U = -m_p*g*r*cos(theta(t))\n",
    "\n",
    "L = sympy.simplify(K - U)\n",
    "\n",
    "x_eq = sympy.Derivative(sympy.Derivative(L, x_d).doit(), t).doit() - sympy.Derivative(L, x(t)).doit() - f_x + k_x * x_d\n",
    "y_eq = sympy.Derivative(sympy.Derivative(L, y_d).doit(), t).doit() - sympy.Derivative(L, y(t)).doit() - f_y + k_y * y_d\n",
    "theta_eq = sympy.Derivative(sympy.Derivative(L, theta_d).doit(), t).doit() - sympy.Derivative(L, theta(t)).doit() + k_theta * theta_d\n",
    "phi_eq = sympy.Derivative(sympy.Derivative(L, phi_d).doit(), t).doit() - sympy.Derivative(L, phi(t)).doit() + k_phi * phi_d\n",
    "\n",
    "q_dd = sympy.solvers.solve([x_eq, y_eq, theta_eq, phi_eq], [x_dd, y_dd, theta_dd, phi_dd])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{f_{x} m_{c} r - f_{x} m_{p} r \\sin^{2}{\\left(\\phi{\\left(t \\right)} \\right)} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} + f_{x} m_{p} r \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} + f_{y} m_{p} r \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} + \\frac{g m_{c} m_{p} r \\sin{\\left(\\phi{\\left(t \\right)} \\right)}}{\\tan{\\left(\\theta{\\left(t \\right)} \\right)}} - \\frac{g m_{c} m_{p} r \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\cos^{3}{\\left(\\theta{\\left(t \\right)} \\right)}}{\\sin{\\left(\\theta{\\left(t \\right)} \\right)}} + \\frac{k_{\\phi} m_{c} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)}}{\\sin{\\left(\\theta{\\left(t \\right)} \\right)}} + k_{\\phi} m_{p} \\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)} + k_{\\theta} m_{c} \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\cos{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)} + m_{c} m_{p} r^{2} \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin^{3}{\\left(\\theta{\\left(t \\right)} \\right)} \\left(\\frac{d}{d t} \\phi{\\left(t \\right)}\\right)^{2} + m_{c} m_{p} r^{2} \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\left(\\frac{d}{d t} \\theta{\\left(t \\right)}\\right)^{2}}{m_{c} r \\left(m_{c} + m_{p} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)}\\right)}$"
      ],
      "text/plain": [
       "(f_x*m_c*r - f_x*m_p*r*sin(phi(t))**2*sin(theta(t))**2 + f_x*m_p*r*sin(theta(t))**2 + f_y*m_p*r*sin(phi(t))*sin(theta(t))**2*cos(phi(t)) + g*m_c*m_p*r*sin(phi(t))/tan(theta(t)) - g*m_c*m_p*r*sin(phi(t))*cos(theta(t))**3/sin(theta(t)) + k_phi*m_c*cos(phi(t))*Derivative(phi(t), t)/sin(theta(t)) + k_phi*m_p*sin(theta(t))*cos(phi(t))*Derivative(phi(t), t) + k_theta*m_c*sin(phi(t))*cos(theta(t))*Derivative(theta(t), t) + m_c*m_p*r**2*sin(phi(t))*sin(theta(t))**3*Derivative(phi(t), t)**2 + m_c*m_p*r**2*sin(phi(t))*sin(theta(t))*Derivative(theta(t), t)**2)/(m_c*r*(m_c + m_p*sin(theta(t))**2))"
      ]
     },
     "execution_count": 44,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q_dd[x_dd].subs({\n",
    "    x(t): 0,    \n",
    "    y(t): 0,\n",
    "    x_d: 0,\n",
    "    y_d: 0,\n",
    "    theta(t): \n",
    "})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[nan]\n",
      " [nan]\n",
      " [ 0.]\n",
      " [nan]]\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/stephentu/anaconda3/lib/python3.7/site-packages/numpy/__init__.py:2: RuntimeWarning: invalid value encountered in double_scalars\n",
      "  NumPy\n",
      "/Users/stephentu/anaconda3/lib/python3.7/site-packages/numpy/__init__.py:2: RuntimeWarning: divide by zero encountered in double_scalars\n",
      "  NumPy\n"
     ]
    }
   ],
   "source": [
    "q_dd_vec = sympy.Matrix([q_dd[x_dd], q_dd[y_dd], q_dd[theta_dd], q_dd[phi_dd]])\n",
    "q_dd_fn = sympy.lambdify((x(t), y(t), theta(t), phi(t), x_d, y_d, theta_d, phi_d, f_x, f_y, m_p, m_c, r, g, k_x, k_y, k_theta, k_phi), q_dd_vec, modules=[np])\n",
    "\n",
    "def dynamics(q, qd, u, params):\n",
    "    params = np.array(params)\n",
    "    args = np.hstack((q, qd, u, params))\n",
    "    return q_dd_fn(*args)\n",
    "\n",
    "q = np.array([0, 0, 0, np.pi])\n",
    "qd = np.array([0, 0, 0, 0])\n",
    "u = np.array([0, 0])\n",
    "params = (1, 1, 1, 9.81, 0.1, 0.1, 0.1, 0.1)\n",
    "\n",
    "print(dynamics(q, qd, u, params))\n",
    "\n",
    "#x_sym, y_sym, phi_sym, theta_sym = sympy.symbols('x_sym, y_sym, phi_sym, theta_sym')\n",
    "#q_dd_vec = q_dd_vec.subs([(x(t), x_sym), (y(t), y_sym), (theta(t), theta_sym), (phi(t), phi_sym)])\n",
    "#q_dd_fn = sympy.lambdify((x_sym, y_sym, theta_sym, phi_sym, x_d, y_d, theta_d, phi_d, f_x, f_y, m_p, m_c, r, g, k_x, k_y, k_theta, k_phi), q_dd_vec, modules=[np])\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{f_{x} m_{c} r - f_{x} m_{p} r \\sin^{2}{\\left(\\phi{\\left(t \\right)} \\right)} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} + f_{x} m_{p} r \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} + f_{y} m_{p} r \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} + \\frac{g m_{c} m_{p} r \\sin{\\left(\\phi{\\left(t \\right)} \\right)}}{\\tan{\\left(\\theta{\\left(t \\right)} \\right)}} - \\frac{g m_{c} m_{p} r \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\cos^{3}{\\left(\\theta{\\left(t \\right)} \\right)}}{\\sin{\\left(\\theta{\\left(t \\right)} \\right)}} + \\frac{k_{\\phi} m_{c} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)}}{\\sin{\\left(\\theta{\\left(t \\right)} \\right)}} + k_{\\phi} m_{p} \\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)} + k_{\\theta} m_{c} \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\cos{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)} + m_{c} m_{p} r^{2} \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin^{3}{\\left(\\theta{\\left(t \\right)} \\right)} \\left(\\frac{d}{d t} \\phi{\\left(t \\right)}\\right)^{2} + m_{c} m_{p} r^{2} \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\left(\\frac{d}{d t} \\theta{\\left(t \\right)}\\right)^{2}}{m_{c} r \\left(m_{c} + m_{p} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)}\\right)}$"
      ],
      "text/plain": [
       "(f_x*m_c*r - f_x*m_p*r*sin(phi(t))**2*sin(theta(t))**2 + f_x*m_p*r*sin(theta(t))**2 + f_y*m_p*r*sin(phi(t))*sin(theta(t))**2*cos(phi(t)) + g*m_c*m_p*r*sin(phi(t))/tan(theta(t)) - g*m_c*m_p*r*sin(phi(t))*cos(theta(t))**3/sin(theta(t)) + k_phi*m_c*cos(phi(t))*Derivative(phi(t), t)/sin(theta(t)) + k_phi*m_p*sin(theta(t))*cos(phi(t))*Derivative(phi(t), t) + k_theta*m_c*sin(phi(t))*cos(theta(t))*Derivative(theta(t), t) + m_c*m_p*r**2*sin(phi(t))*sin(theta(t))**3*Derivative(phi(t), t)**2 + m_c*m_p*r**2*sin(phi(t))*sin(theta(t))*Derivative(theta(t), t)**2)/(m_c*r*(m_c + m_p*sin(theta(t))**2))"
      ]
     },
     "execution_count": 40,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q_dd_vec[0].subs({\n",
    "    x(t): 0,    \n",
    "    y(t): 0,\n",
    "    x_d: 0,\n",
    "    y_d: 0\n",
    "})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\pi$"
      ],
      "text/plain": [
       "pi"
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sympy.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}