mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-09 11:31:38 +08:00
216 lines
7.1 KiB
Plaintext
216 lines
7.1 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Mass-spring-damper\n",
|
|
"\n",
|
|
"In this tutorial, we will describe the mechanics and control of the one degree of freedom translational mass-spring-damper system subject to a control input force. We will first derive the dynamic equations by hand. Then, we will derive them using the `sympy.mechanics` python package.\n",
|
|
"\n",
|
|
"The system on which we will work is depicted below:\n",
|
|
"\n",
|
|
"\n",
|
|
"\n",
|
|
"Note that in what follows, we use the notation $u(t) = F$."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## 1. Mechanics"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### Deriving the dynamical equations by hand\n",
|
|
"\n",
|
|
"#### 1.1 By using Newton equations\n",
|
|
"\n",
|
|
"Using Newton's law, we have:\n",
|
|
"\n",
|
|
"\\begin{align}\n",
|
|
" m \\ddot{x}(t) &= \\sum F_{ext} \\\\\n",
|
|
" &= - b \\dot{x}(t) - k x(t) + u(t)\n",
|
|
"\\end{align}\n",
|
|
"\n",
|
|
"#### 1.2 By using the Lagrange Method\n",
|
|
"\n",
|
|
"Let's first derive the kinematic and potential energies.\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" T = \\frac{1}{2} m \\dot{x} \\\\\n",
|
|
" V = - \\int \\vec{F} . \\vec{dl} = - \\int (-kx \\vec{1_x}) . dx \\vec{1_x} = \\frac{k x^2}{2}\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"The Lagrangian is then given by:\n",
|
|
"\\begin{equation}\n",
|
|
" \\mathcal{L} = T - V = \\frac{1}{2} m \\dot{x} - \\frac{k x^2}{2}\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"Using the Lagrange's equations we can derive the dynamics of the system:\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" \\frac{d}{dt} \\frac{\\partial \\mathcal{L}}{\\partial \\dot{q}} - \\frac{\\partial \\mathcal{L}}{\\partial q} = Q\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"where $q$ are the generalized coordinates (in this case $x$), and $Q$ represents the non-conservative forces (input force, dragging or friction forces, etc).\n",
|
|
"\n",
|
|
"* $\\frac{d}{dt} \\frac{\\partial \\mathcal{L}}{\\partial \\dot{x}} = \\frac{d}{dt} m \\dot{x}(t) = m \\ddot{x}(t) $\n",
|
|
"* $\\frac{\\partial \\mathcal{L}}{\\partial x} = - k x(t) $\n",
|
|
"* $Q = - b \\dot{x}(t) + u(t) $\n",
|
|
"\n",
|
|
"which when putting everything back together gives us:\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" m \\ddot{x}(t) + b \\dot{x}(t) + k x(t) = u(t)\n",
|
|
"\\end{equation}"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### Deriving the dynamical equations using sympy"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import sympy\n",
|
|
"import sympy.physics.mechanics as mechanics\n",
|
|
"from sympy import init_printing\n",
|
|
"init_printing(use_latex='mathjax')\n",
|
|
"from sympy import pprint"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 14,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"⎡ 2 ⎤\n",
|
|
"⎢ d d ⎥\n",
|
|
"⎢b⋅──(q(t)) + 1.0⋅k⋅q(t) + m⋅───(q(t)) - u(t)⎥\n",
|
|
"⎢ dt 2 ⎥\n",
|
|
"⎣ dt ⎦\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"# define variables \n",
|
|
"q = mechanics.dynamicsymbols('q')\n",
|
|
"dq = mechanics.dynamicsymbols('q', 1)\n",
|
|
"u = mechanics.dynamicsymbols('u')\n",
|
|
"\n",
|
|
"# define constants\n",
|
|
"m, k, b = sympy.symbols('m k b')\n",
|
|
"\n",
|
|
"# define the inertial frame\n",
|
|
"N = mechanics.ReferenceFrame('N')\n",
|
|
"\n",
|
|
"# define a particle for the mass\n",
|
|
"P = mechanics.Point('P')\n",
|
|
"P.set_vel(N, dq * N.x) # go in the x direction\n",
|
|
"Pa = mechanics.Particle('Pa', P, m)\n",
|
|
"\n",
|
|
"# define the potential energy for the particle (the kinematic one is derived automatically)\n",
|
|
"Pa.potential_energy = k * q**2 / 2.0\n",
|
|
"\n",
|
|
"# define the Lagrangian and the non-conservative force applied on the point P\n",
|
|
"L = mechanics.Lagrangian(N, Pa)\n",
|
|
"force = [(P, -b * dq * N.x + u * N.x)]\n",
|
|
"\n",
|
|
"# Lagrange equations \n",
|
|
"lagrange = mechanics.LagrangesMethod(L, [q], forcelist = force, frame = N)\n",
|
|
"pprint(lagrange.form_lagranges_equations())"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"## 2. Laplace transform and transfer function"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"Applying the Laplace transform on the dynamic equation:\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" m \\ddot{x}(t) + b \\dot{x}(t) + k x(t) = u(t) \\stackrel{L}{\\rightarrow} m s^2 X(s) + b s X(s) + k X(s) = U(s)\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"The transfer equation is given by:\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" H(s) = \\frac{X(s)}{U(s)} = \\frac{1}{m s^2 + b s + k}\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"By calculating the pole:\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" m s^2 + b s + k = 0 \\Leftrightarrow s = \\frac{-b}{2m} \\pm \\sqrt{\\left(\\frac{b}{2m}\\right)^2 - \\frac{k}{m}}\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"Note that $b, k, m > 0$ because they represent real physical quantities."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"### LTI system\n",
|
|
"\n",
|
|
"We can rewrite the above equation as a first-order system of equations. Let's first define the state vector $\\pmb{x} = \\left[ \\begin{array}{c} x(t) \\\\ \\dot{x}(t) \\end{array} \\right]$ and the control vector $\\pmb{u} = \\left[ \\begin{array}{c} u(t) \\end{array} \\right]$, then we can rewrite the above equation in the form $\\pmb{\\dot{x}} = \\pmb{Ax} + \\pmb{Bu}$, as below:\n",
|
|
"\n",
|
|
"\\begin{equation}\n",
|
|
" \\left[ \\begin{array}{c} \\dot{x}(t) \\\\ \\ddot{x}(t) \\end{array} \\right] = \\left[ \\begin{array}{cc} 0 & 1 \\\\ -\\frac{k}{m} & -\\frac{b}{m} \\end{array} \\right] \\left[ \\begin{array}{c} x(t) \\\\ \\dot{x}(t) \\end{array} \\right] + \\left[ \\begin{array}{c} 0 \\\\ \\frac{1}{m} \\end{array} \\right] \\left[ \\begin{array}{c} u(t) \\end{array} \\right]\n",
|
|
"\\end{equation}\n",
|
|
"\n",
|
|
"If there is no $u(t)$, i.e. $u(t) = 0 \\; \\forall t$, then we have $\\pmb{\\dot{x}} = \\pmb{Ax}$. The solution to this system of equation is $\\pmb{x}(t) = e^{\\pmb{A}t} \\pmb{x}_0$."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"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.12"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 2
|
|
}
|