Created
March 26, 2020 18:26
-
-
Save stephentu/f62d530d0ccdbfd91c10b8e85dcd589d to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
{ | |
"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 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment