Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
{
"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