Bipod_EOM_V3_thbod_locked.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "6496397d",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Once deleted, variables cannot be recovered. Proceed (y/[n])? y\n"
     ]
    }
   ],
   "source": [
    "%reset\n",
    "\n",
    "import sympy as sym\n",
    "import numpy as np\n",
    "import pickle as pickle\n",
    "\n",
    "sym.init_printing()\n",
    "from IPython.display import display\n",
    "\n",
    "#symbols\n",
    "legs = ['A','B']\n",
    "links = ['UL','LL','UR','LR'] #upper left link, lower left link etc etc\n",
    "bases = ['body','boom']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "b516cfb2",
   "metadata": {},
   "outputs": [],
   "source": [
    "#Parameters\n",
    "masses = {}\n",
    "lengths = {}\n",
    "inertias = {}\n",
    "COMR = {} # COM ratio distance\n",
    "\n",
    "BLD = sym.symbols('BLD') # from COM of body leg distance\n",
    "g = sym.symbols('g')\n",
    "foot_angle = sym.symbols('foot_angle') # 136*(np.pi/180)\n",
    "foot_length = sym.symbols('foot_length')\n",
    "cg = sym.symbols('cg')\n",
    "lx_boom = sym.symbols('lx_boom')\n",
    "ly_boom = sym.symbols('ly_boom')\n",
    "y_bodyOffset = sym.symbols('y_bodyOffset')\n",
    "y_boomOffset = sym.symbols('y_boomOffset')\n",
    "x_footOffset = sym.symbols('x_footOffset')\n",
    "\n",
    "for leg in legs:\n",
    "    for link in links:   \n",
    "        masses.update({(link,leg): sym.symbols('m_{0}'.format(link+leg))})\n",
    "        lengths.update({(link,leg): sym.symbols('l_{0}'.format(link+leg))})\n",
    "        inertias.update({(link,leg): sym.symbols('In_{0}'.format(link+leg))})\n",
    "        \n",
    "for base in bases:\n",
    "    masses.update({(base): sym.symbols('m_{0}'.format(base))})\n",
    "    lengths.update({(base): sym.symbols('l_{0}'.format(base))})\n",
    "    if base == 'boom':\n",
    "        inertias.update({(base,'X'): sym.symbols('In_{0}_X'.format(base))})\n",
    "        inertias.update({(base,'Y'): sym.symbols('In_{0}_Y'.format(base))})\n",
    "    else:\n",
    "        inertias.update({(base): sym.symbols('In_{0}'.format(base))})\n",
    "\n",
    "#gen coordinates\n",
    "x_body,y_body = sym.symbols(['x_body','y_body'])\n",
    "dx_body,dy_body = sym.symbols(['\\dot{\\\\x}_{body}','\\dot{\\\\y}_{body}'])\n",
    "ddx_body,ddy_body = sym.symbols(['\\ddot{\\\\x}_{body}','\\ddot{\\\\y}_{body}'])\n",
    "\n",
    "th = {}\n",
    "dth = {}\n",
    "ddth = {}\n",
    "\n",
    "for leg in legs:\n",
    "    for link in links:\n",
    "        th.update({(link,leg): sym.symbols('theta_{0}'.format(link+leg))}) \n",
    "        dth.update({(link,leg): sym.symbols('\\dot{{\\\\theta}}_{{{0}}}'.format(link+leg))})\n",
    "        ddth.update({(link,leg): sym.symbols('\\ddot{{\\\\theta}}_{{{0}}}'.format(link+leg))})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "id": "a7439771",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = sym.Matrix([x_body]+[y_body]+[th[link,leg] for leg in legs for link in links])\n",
    "dq = sym.Matrix([dx_body]+[dy_body]+[dth[link,leg] for leg in legs for link in links])\n",
    "ddq = sym.Matrix([ddx_body]+[ddy_body]+[ddth[link,leg] for leg in legs for link in links])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "id": "abb9c15e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( \\left[\\begin{matrix}x_{body}\\\\y_{body}\\\\\\theta_{ULA}\\\\\\theta_{LLA}\\\\\\theta_{URA}\\\\\\theta_{LRA}\\\\\\theta_{ULB}\\\\\\theta_{LLB}\\\\\\theta_{URB}\\\\\\theta_{LRB}\\end{matrix}\\right], \\  \\left[\\begin{matrix}\\dot{\\x}_{body}\\\\\\dot{\\y}_{body}\\\\\\dot{\\theta}_{ULA}\\\\\\dot{\\theta}_{LLA}\\\\\\dot{\\theta}_{URA}\\\\\\dot{\\theta}_{LRA}\\\\\\dot{\\theta}_{ULB}\\\\\\dot{\\theta}_{LLB}\\\\\\dot{\\theta}_{URB}\\\\\\dot{\\theta}_{LRB}\\end{matrix}\\right], \\  \\left[\\begin{matrix}\\ddot{\\x}_{body}\\\\\\ddot{\\y}_{body}\\\\\\ddot{\\theta}_{ULA}\\\\\\ddot{\\theta}_{LLA}\\\\\\ddot{\\theta}_{URA}\\\\\\ddot{\\theta}_{LRA}\\\\\\ddot{\\theta}_{ULB}\\\\\\ddot{\\theta}_{LLB}\\\\\\ddot{\\theta}_{URB}\\\\\\ddot{\\theta}_{LRB}\\end{matrix}\\right]\\right)$"
      ],
      "text/plain": [
       "⎛⎡x_body⎤  ⎡ \\dot{\\x}_{body}  ⎤  ⎡ \\ddot{\\x}_{body}  ⎤⎞\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢y_body⎥  ⎢ \\dot{\\y}_{body}  ⎥  ⎢ \\ddot{\\y}_{body}  ⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢θ_ULA ⎥  ⎢\\dot{\\theta}_{ULA}⎥  ⎢\\ddot{\\theta}_{ULA}⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢θ_LLA ⎥  ⎢\\dot{\\theta}_{LLA}⎥  ⎢\\ddot{\\theta}_{LLA}⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢θ_URA ⎥  ⎢\\dot{\\theta}_{URA}⎥  ⎢\\ddot{\\theta}_{URA}⎥⎟\n",
       "⎜⎢      ⎥, ⎢                  ⎥, ⎢                   ⎥⎟\n",
       "⎜⎢θ_LRA ⎥  ⎢\\dot{\\theta}_{LRA}⎥  ⎢\\ddot{\\theta}_{LRA}⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢θ_ULB ⎥  ⎢\\dot{\\theta}_{ULB}⎥  ⎢\\ddot{\\theta}_{ULB}⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢θ_LLB ⎥  ⎢\\dot{\\theta}_{LLB}⎥  ⎢\\ddot{\\theta}_{LLB}⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎜⎢θ_URB ⎥  ⎢\\dot{\\theta}_{URB}⎥  ⎢\\ddot{\\theta}_{URB}⎥⎟\n",
       "⎜⎢      ⎥  ⎢                  ⎥  ⎢                   ⎥⎟\n",
       "⎝⎣θ_LRB ⎦  ⎣\\dot{\\theta}_{LRB}⎦  ⎣\\ddot{\\theta}_{LRB}⎦⎠"
      ]
     },
     "execution_count": 33,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q,dq,ddq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "id": "8bf8493e",
   "metadata": {},
   "outputs": [],
   "source": [
    "for leg in legs:\n",
    "    for link in links:\n",
    "        COMR.update({(link,leg): sym.symbols('COMR_{0}'.format(link+leg))})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "id": "e195f06d",
   "metadata": {},
   "outputs": [],
   "source": [
    "''' Setup positions (to COM) and velocities'''\n",
    "\n",
    "r_bases = {}\n",
    "\n",
    "# position to COM of each link - where the mass is approximated to be\n",
    "r_bases.update({'body': sym.Matrix([[x_body,\n",
    "                                     y_body]]).transpose()})\n",
    "\n",
    "r_bases.update({'boom': sym.Matrix([[(cg/lx_boom)*x_body,\n",
    "                                     (cg/ly_boom)*y_body]]).transpose()})\n",
    "\n",
    "# define absolute angles\n",
    "abs_angles = {}\n",
    "r_links = {}\n",
    "\n",
    "# using small angle approximation\n",
    "abs_angles.update({('boom','thx_boom'): x_body/lx_boom})\n",
    "abs_angles.update({('boom','thy_boom'): y_body/ly_boom})\n",
    "\n",
    "for leg in legs:\n",
    "    \n",
    "    abs_angles.update({('LU',leg): th['UL',leg]})\n",
    "    abs_angles.update({('LL',leg): th['LL',leg] - (np.pi - abs_angles['LU',leg])})\n",
    "    abs_angles.update({('RU',leg): th['UR',leg]})\n",
    "    abs_angles.update({('RL',leg): th['LR',leg] - (np.pi - abs_angles['RU',leg]) }) \n",
    "    \n",
    "    r_links.update({('UL',leg): sym.Matrix([[x_body-BLD-lengths['UL',leg]*COMR['UL',leg]*sym.cos(abs_angles['LU',leg]),\n",
    "                                             y_body-y_bodyOffset-lengths['UL',leg]*COMR['UL',leg]*sym.sin(abs_angles['LU',leg])]]).transpose()})\n",
    "    \n",
    "    r_links.update({('UR',leg): sym.Matrix([[x_body+BLD+lengths['UR',leg]*COMR['UR',leg]*sym.cos(np.pi-abs_angles['RU',leg]),\n",
    "                                             y_body-y_bodyOffset-lengths['UR',leg]*COMR['UR',leg]*sym.sin(np.pi-abs_angles['RU',leg])]]).transpose()})\n",
    "    \n",
    "    r_links.update({('LL',leg): sym.Matrix([[r_links['UL',leg][0]-(1-COMR['UL',leg])*lengths['UL',leg]*sym.cos(abs_angles['LU',leg])+lengths['LL',leg]*COMR['LL',leg]*sym.cos(np.pi-abs_angles['LL',leg]),\n",
    "                                             r_links['UL',leg][1]-(1-COMR['UL',leg])*lengths['UL',leg]*sym.sin(abs_angles['LU',leg])-lengths['LL',leg]*COMR['LL',leg]*sym.sin(np.pi-abs_angles['LL',leg])]]).transpose()})\n",
    "\n",
    "    r_links.update({('LR',leg): sym.Matrix([[r_links['UR',leg][0]+(1-COMR['UR',leg])*lengths['UR',leg]*sym.cos(np.pi-abs_angles['RU',leg])-lengths['LR',leg]*COMR['LR',leg]*sym.cos(abs_angles['RL',leg]) + x_footOffset,\n",
    "                                             r_links['UR',leg][1]-(1-COMR['UR',leg])*lengths['UR',leg]*sym.sin(np.pi-abs_angles['RU',leg])-lengths['LR',leg]*COMR['LR',leg]*sym.sin(abs_angles['RL',leg])]]).transpose()})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "id": "c8ea7b18",
   "metadata": {},
   "outputs": [],
   "source": [
    "dr_bases = {}\n",
    "dr_links = {}\n",
    "w_links = {}\n",
    "w_bases = {}\n",
    "\n",
    "\n",
    "\n",
    "thx_boom = sym.Matrix([[sym.asin(x_body/lx_boom)]])\n",
    "thy_boom = sym.Matrix([[sym.asin(y_body/ly_boom)]])\n",
    "dthx_boom = thx_boom.jacobian(q)*dq\n",
    "dthy_boom = thy_boom.jacobian(q)*dq\n",
    "\n",
    "w_bases.update({('boom','dthx_boom'): dthx_boom[0]})\n",
    "w_bases.update({('boom','dthy_boom'): dthy_boom[0]})\n",
    "\n",
    "# w_bases.update({('boom','dthx_boom'): dx_body/lx_boom})\n",
    "# w_bases.update({('boom','dthy_boom'): dy_body/ly_boom})\n",
    "\n",
    "for base in bases:\n",
    "    dr_bases.update({base: r_bases[base].jacobian(q)*dq})\n",
    "\n",
    "for leg in legs:\n",
    "    \n",
    "    w_links.update({('UL',leg): dth['UL',leg]})\n",
    "    w_links.update({('LL',leg): dth['UL',leg] + dth['LL',leg]})\n",
    "    w_links.update({('UR',leg): dth['UR',leg]})\n",
    "    w_links.update({('LR',leg): dth['UR',leg] + dth['LR',leg]})\n",
    "    \n",
    "    for link in links:\n",
    "        dr_links.update({(link,leg): r_links[link,leg].jacobian(q)*dq})\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "id": "396edefa",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_bases = {}\n",
    "T_links = {}\n",
    "V_bases = {}\n",
    "V_links = {}\n",
    "\n",
    "# Kinetic Energy\n",
    "T_bases.update({'body': 0.5*sym.Matrix([[masses['body'],masses['body']]])*sym.matrix_multiply_elementwise(dr_bases['body'],dr_bases['body'])})\n",
    "T_bases.update({'boom': 0.5*sym.Matrix([[masses['boom'],masses['boom']]])*sym.matrix_multiply_elementwise(dr_bases['boom'],dr_bases['boom']) + sym.Matrix([[0.5*w_bases['boom','dthx_boom']*w_bases['boom','dthx_boom']*inertias['boom','X']]]) + sym.Matrix([[0.5*w_bases['boom','dthy_boom']*w_bases['boom','dthy_boom']*inertias['boom','Y']]])})\n",
    "\n",
    "# Potential Energy\n",
    "for base in bases:\n",
    "    if base == 'body':\n",
    "        V_bases.update({base: masses[base]*g*r_bases[base][1]})\n",
    "    else:\n",
    "        V_bases.update({base: masses[base]*g*(y_boomOffset + ((y_body-y_boomOffset)*cg/ly_boom))})\n",
    "\n",
    "for leg in legs:\n",
    "    for link in links:\n",
    "        T_links.update({(link,leg): 0.5*sym.Matrix([[masses[link,leg],masses[link,leg]]])*sym.matrix_multiply_elementwise(dr_links[link,leg],dr_links[link,leg]) + sym.Matrix([[0.5*w_links[link,leg]*w_links[link,leg]*inertias[link,leg]]])})\n",
    "        V_links.update({(link,leg): masses[link,leg]*g*r_links[link,leg][1]})\n",
    "\n",
    "Ttot = T_bases['body'] + T_bases['boom']\n",
    "Vtot = V_bases['body'] + V_bases['boom']\n",
    "    \n",
    "for leg in legs:\n",
    "    for link in links:\n",
    "        Ttot = Ttot + T_links[link,leg] \n",
    "        Vtot = Vtot + V_links[link,leg] \n",
    "        \n",
    "Ttot = Ttot[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "id": "c562cafc",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Manipulator Equation (M*ddq + C + G = Tau + Q)\n",
    "\n",
    "# M Matrix\n",
    "M = sym.zeros(len(q),len(q))\n",
    "\n",
    "for i in range(len(q)):\n",
    "    for j in range(len(q)):\n",
    "        M[i,j] = sym.diff(sym.diff(Ttot,dq[i]),dq[j])\n",
    "\n",
    "M = sym.simplify(M)\n",
    "\n",
    "M_size = int(np.sqrt(len(M))) # assumes square matrix (len(M) = 4x4 = 16)\n",
    "\n",
    "dM = sym.zeros(M_size,M_size)\n",
    "\n",
    "for i in range(M_size):\n",
    "    for j in range(M_size):\n",
    "        dM[i,j] = sym.Matrix([M[i,j]]).jacobian(q)*dq\n",
    "\n",
    "# C Matrix\n",
    "C = dM*dq - (sym.Matrix([Ttot]).jacobian(q)).transpose() # 4x1 - 1x4^Transpose\n",
    "\n",
    "# G Matrix\n",
    "G = (sym.Matrix([Vtot]).jacobian(q)).transpose() # 4x1\n",
    "\n",
    "tau = {}\n",
    "lamda = {}\n",
    "GRF = {}\n",
    "\n",
    "connect_position = {}\n",
    "foot_position = {}\n",
    "\n",
    "Jbn = {}\n",
    "Jfoot = {}\n",
    "lamda_GRF = {}\n",
    "lamda_connect = {}\n",
    "\n",
    "Tau = sym.Matrix([[0],[0]])\n",
    "\n",
    "Q = sym.zeros(len(q),1)\n",
    "\n",
    "for leg in legs:\n",
    "    tau.update({('left',leg): sym.symbols('tau_left{0}'.format(leg))})\n",
    "    tau.update({('right',leg): sym.symbols('tau_right{0}'.format(leg))})\n",
    "    \n",
    "    lamda.update({('x',leg): sym.symbols('lamda_x{0}'.format(leg))})\n",
    "    lamda.update({('y',leg): sym.symbols('lamda_y{0}'.format(leg))})\n",
    "    \n",
    "    GRF.update({('x',leg): sym.symbols('GRF_x{0}'.format(leg))})\n",
    "    GRF.update({('y',leg): sym.symbols('GRF_y{0}'.format(leg))})\n",
    "    \n",
    "    \n",
    "    connect_position.update({('left',leg): sym.Matrix([[r_links['LL',leg][0]+(1-COMR['LL',leg])*lengths['LL',leg]*sym.cos(np.pi-abs_angles['LL',leg])],\n",
    "                                                       [r_links['LL',leg][1]-(1-COMR['LL',leg])*lengths['LL',leg]*sym.sin(np.pi-abs_angles['LL',leg])]]).transpose()})\n",
    "    \n",
    "    connect_position.update({('right',leg): sym.Matrix([[r_links['LR',leg][0]-(1-COMR['LR',leg])*lengths['LR',leg]*sym.cos(abs_angles['RL',leg]) - x_footOffset],\n",
    "                                                        [r_links['LR',leg][1]-(1-COMR['LR',leg])*lengths['LR',leg]*sym.sin(abs_angles['RL',leg])]]).transpose()})\n",
    "\n",
    "    foot_position.update({('right',leg): sym.Matrix([[connect_position['right',leg][0] + foot_length*sym.cos(foot_angle - (np.pi)/2)],\n",
    "                                                     [connect_position['right',leg][1] - foot_length*sym.sin(foot_angle - (np.pi)/2)]]).transpose()})\n",
    "\n",
    "    Jbn.update({('left',leg): connect_position['left',leg].jacobian(q)})                    \n",
    "    Jbn.update({('right',leg): connect_position['right',leg].jacobian(q)}) \n",
    "    \n",
    "    Jfoot.update({(leg): foot_position['right',leg].jacobian(q)})\n",
    "    \n",
    "    lamda_GRF.update({(leg): sym.Matrix([[GRF['x',leg]],[GRF['y',leg]]])})\n",
    "    \n",
    "    lamda_connect.update({('left',leg): sym.Matrix([[-lamda['x',leg]],[-lamda['y',leg]]])})\n",
    "    lamda_connect.update({('right',leg): sym.Matrix([[lamda['x',leg]],[lamda['y',leg]]])})\n",
    "    \n",
    "    Tau = Tau.col_join(sym.Matrix([[tau['left',leg]],[0],[tau['right',leg]],[0]]))\n",
    "    \n",
    "    Q = Q + (Jfoot[leg].transpose()*lamda_GRF[leg]) + (Jbn['left',leg].transpose()*lamda_connect['left',leg]) + (Jbn['right',leg].transpose()*lamda_connect['right',leg])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "id": "1589ea75",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABoAAAAQCAYAAAAI0W+oAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/YYfK9AAAACXBIWXMAABJ0AAASdAHeZh94AAABd0lEQVR4nL3UP2gUYRAF8N9JKkOMEBALC8mRWJpC8F8hWiSWKVJGxE5QFNFSGEawVVC0FsFe2zQpxD8I1mKiwUKxUUQk2HkW9y2sd7ccsujA8vjmzTdv3i47nV6v53/ERP2QmSs4gQUcxBQeRcRqU4PM3IcbOI0ZfMZjZER8q+p2DNy7jotF6NO4KTOzi9c4h1e4jS1cxovMnBnpCFfwEe+Ks/UxWvexB5ci4m5tgFul102cH3IUEesRsRkRYz9cZs5iER9wb4AObONMZk4OCf1lnCq4FhG/Bgb+gWfYiSNthQ4U3GjgNwvOtxWaLvi9ga/yu9sKjYtOwV5boWri6QZ+V72ujdDbgvMN/FzBjbZC1T+2mJl/9MnMKRzHT7xsJRQR77GG/bgwQCcm8TAitqFTX6qZuYzlctyLJf2V8rTkvkTEtVp9F8/1t8MTvMFhnNR/Zcci4usoRws4W56lkput5VZGuDqEB0XgKrq4g6OVyJCjfxm/ATXhbF4zGbnWAAAAAElFTkSuQmCC\n",
      "text/latex": [
       "$\\displaystyle 10$"
      ],
      "text/plain": [
       "10"
      ]
     },
     "execution_count": 39,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "len(Tau)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "id": "4060721a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Manipulator Equation \n",
    "EOM = M*ddq + G + C - Tau - Q \n",
    "\n",
    "# save the EOM in a file (saving the EOM saves lots of time)\n",
    "import dill\n",
    "dill.dump_session('unsimplified_bipod_thbodlocked_v3.db')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "id": "3c2f5646",
   "metadata": {},
   "outputs": [],
   "source": [
    "# with open('mark_unsimplified_bipod_v3','w') as f:\n",
    "#     for j in range(len(q)):\n",
    "#         f.write(str(EOM[j]))\n",
    "#         f.write('\\n')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "id": "8aac272d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# import dill\n",
    "# dill.load_session('unsimplified_bipod_thbodlocked_v3.db')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "id": "37c637e4",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "j = 1 is done\n",
      "j = 2 is done\n",
      "j = 3 is done\n",
      "j = 4 is done\n",
      "j = 5 is done\n",
      "j = 6 is done\n",
      "j = 7 is done\n",
      "j = 8 is done\n",
      "j = 9 is done\n",
      "j = 10 is done\n"
     ]
    }
   ],
   "source": [
    "\n",
    "EOMs = sym.zeros(len(q),1)\n",
    "for j in range(len(q)):\n",
    "    EOMs[j] = EOM[j].simplify()\n",
    "    print('j = {0} is done'.format(j+1))\n",
    "    dill.dump_session('simplified_bipodv3_thbodlocked_EOM_{0}.db'.format(j+1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "id": "88d17311",
   "metadata": {},
   "outputs": [],
   "source": [
    "import dill\n",
    "dill.dump_session('simplified_bipod_thbodlocked_final_mark.db')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc86fcc5",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}