n-3D-pendulum-linearized-controlled.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy.physics.mechanics as me\n",
    "import sympy as sm\n",
    "from scipy.integrate import solve_ivp\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "#import matplotlib\n",
    "import time\n",
    "\n",
    "from scipy.linalg import solve_continuous_are # solves the Riccati matrix equation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is a **3D n body pendulum**\\\n",
    "I put it into a vertical position, linearize the equations of motion and try to stabilize in by\n",
    "- moving the point $P_{01}$, it is attached to the horizontal plane\n",
    "- adding a torque to every body. (This is needed, because I model the pendulum bodies as RigidBody, and their rotation cannot be stopped by moving around the point of attachment)\n",
    "\n",
    "THe control variables are\n",
    "- $F_x, F_z$ forces applied to the point $P_0$ where the pendulum is attached to the plane.\n",
    "- Torques applied to each body fixed frame\n",
    "\n",
    "The 'suspension point' is modeled as a Particle, rest are RigidBody\\\n",
    "(I basically copied this program from some other program quite some time ago, and only enlarged it to 3D and rigid bodies. All I did now was to 'beautify' it a bit and make it more readable)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "#===============================================\n",
    "n = 3          #number of pendulums (bodies)\n",
    "#===============================================\n",
    "\n",
    "start = time.time()    \n",
    "\n",
    "N = me.ReferenceFrame('N')\n",
    "P0 = me.Point('P0')\n",
    "P0.set_vel(N, 0)                                      \n",
    "\n",
    "# Generalized coordinates and velocities\n",
    "q = []\n",
    "u = []\n",
    "\n",
    "A = [N]\n",
    "\n",
    "Torque = []\n",
    "for i in range(1, n+1):                                             # Data for pendulun i at at position [i-1]\n",
    "    for j in ('x', 'y', 'z'):\n",
    "        q.append(me.dynamicsymbols('q'+str(i) + str(j)))            #Rotation of frame i relative to N\n",
    "        u.append(me.dynamicsymbols('u'+str(i) + str(j)))           \n",
    "    A.append(me.ReferenceFrame('A'+ str(i)))\n",
    "    Torque.append(sm.symbols('T' + str(i-1)))\n",
    "        \n",
    "q0x, q0z = me.dynamicsymbols('q0x q0z')                             # coordinates of attachment poi nt\n",
    "u0x, u0z = me.dynamicsymbols('u0x, u0z')\n",
    "\n",
    "g, l, m, c1, t = sm.symbols('g l m c1, t')                          # all masses, distances, time, friction are the same for all pendulums\n",
    "r1 = sm.symbols('r1')                                               # distance from Pi to Dmc\n",
    "iXX, iYY, iZZ = sm.symbols('iXX, iYY, iZZ')                         # moments of inertia around the center of mass, principal axes\n",
    "\n",
    "Fx, Fz = sm.symbols('Fx, Fz')                                       #Control parameters. forces at attachment point acting on P01\n",
    "\n",
    "P01 = P0.locatenew('P01', q0x*N.x + q0z*N.z)\n",
    "P01.set_vel(N, u0x*N.x + u0z*N.z)\n",
    "\n",
    "#Frame points, particles for each Pendulum, numbered 1, 2, ..., n\n",
    "rot = [0]                                                           # rot, rot1 needed for the kinematical differential equations\n",
    "rot1 = [0]\n",
    "P = [P01]\n",
    "DMC = [P01]                                                         # list of center of gravity of each body\n",
    "BODY = [me.Particle('P01a', P01, m)]                                # listof bodies\n",
    "\n",
    "for i in range(1, n+1):\n",
    "    A[i].orient_body_fixed(A[0], (q[3*i-3], q[3*i-2], q[3*i-1]), '123')  \n",
    "    rot.append(A[i].ang_vel_in(N))\n",
    "    A[i].set_ang_vel(N, u[3*i-3] * A[i].x + u[3*i-2] * A[i].y + u[3*i-1] * A[i].z )\n",
    "    rot1.append(A[i].ang_vel_in(N))\n",
    "\n",
    "    Pi = P[i-1].locatenew('P' + str(i), l * A[i].y)                 # old way to determine points\n",
    "    Pi.v2pt_theory(P[i-1], N, A[i])\n",
    "    P.append(Pi)\n",
    "    Dmci = P[i-1].locatenew('Dmc' + str(i), r1 * A[i].y)            # location of center of gravity\n",
    "    Dmci.v2pt_theory(P[i-1], N, A[i])\n",
    "    DMC.append(Dmci)\n",
    "\n",
    "    I = me.inertia(A[i], iXX, iYY, iZZ)\n",
    "    Bodyi = me.RigidBody('Body' + str(i), Dmci, A[i], m, (I, Dmci))\n",
    "    BODY.append(Bodyi)   "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set up **Kane's equations**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Kanes forrmalism\n",
    "start1 = time.time()\n",
    "#Kinematic equations\n",
    "kd = [(q0x.diff(t) - u0x)] + [(q0z.diff(t) - u0z)] + [me.dot(rot[i] - rot1[i], uv) for uv in A[i] for i in range(1, n+1)]\n",
    "\n",
    "# forces, torques\n",
    "FL = [(P01, -m*g*N.y + Fx*N.x + Fz*N.z)]\n",
    "\n",
    "for i in range(1, n+1):\n",
    "    FL.append((DMC[i], -m*g*N.y))\n",
    "    FL.append((A[i], Torque[i-1]*A[i].y))\n",
    "\n",
    "q_ind = [q0x, q0z] + q\n",
    "u_ind = [u0x, u0z] + u\n",
    "\n",
    "KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, kd_eqs=kd)\n",
    "(fr, frstar) = KM.kanes_equations(BODY, FL)\n",
    "\n",
    "MM = KM.mass_matrix_full\n",
    "print('MM DS', me.find_dynamicsymbols(MM))\n",
    "print('MM FS', MM.free_symbols)\n",
    "print(f'MM contains {sm.count_ops(MM):,} operations, {sm.count_ops(sm.cse(MM)):,} after cse, \\n')\n",
    "\n",
    "force = KM.forcing_full\n",
    "print('force DS', me.find_dynamicsymbols(force))\n",
    "print('force FS', force.free_symbols)\n",
    "print(f'force contains {sm.count_ops(force):,} operations, {sm.count_ops(sm.cse(force)):,} after cse \\n')\n",
    "print(\"it took {:.3f} sec to establish Kane's equations\".format(time.time() - start1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Lambdification**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Carthesian coordinates of the center points, needed only for plotting them\n",
    "orte_Dmc = ([ [me.dot(ort.pos_from(P0), uv) for uv in N] for ort in DMC] )\n",
    "\n",
    "qL = q_ind + u_ind\n",
    "pL = [g, m, l, r1, iXX, iYY, iZZ, c1, Fx, Fz] + Torque                                     \n",
    "\n",
    "MM_lam = sm.lambdify(qL + pL, MM, cse=True)\n",
    "force_lam = sm.lambdify(qL + pL, force, cse=True)\n",
    "\n",
    "orte_Dmc_lam = []\n",
    "for ort in orte_Dmc:\n",
    "    orte_Dmc_lam.append(sm.lambdify(qL + pL, ort))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical Integration**\\\n",
    "Normally in my simulations I explicitly set all parameters and initial conditions.\\\n",
    "As here I am after something else, linearization and control, I simply pre_set them.\\\n",
    "Of course they may be changed, just a bit more complicated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start2 = time.time()\n",
    "\n",
    "intervall = 5\n",
    "schritte = 250\n",
    "times = np.linspace(0, intervall, schritte)\n",
    "\n",
    "#Anfangsbedingungen\n",
    "#qL = [q0x, q0z] + q + [u0x, u0z] + u\n",
    "#pL = [g, m, l, r1, iXX, iYY, iZZ, c1, Fx, Fz] + Torque    \n",
    "                                  \n",
    "pL_vals = [9.8, 1, 10, 5., 1/2, 1/3, 1/3, 0., 0., 0.] + [ 0. for _ in range(n)]\n",
    "\n",
    "y0 = [0., 0.] + [0, 0, 0] * int(len(q)/3) + [0., 0.] + [1., np.random.choice((-1, 1)) * 5., 1.] * int(len(u)/3)\n",
    "                                            \n",
    "def gradient(t, y, args):                        \n",
    "    vals = np.concatenate((y, args))\n",
    "    sol = np.linalg.solve(MM_lam(*vals), force_lam(*vals))\n",
    "    return np.array(sol).T[0]\n",
    "\n",
    "times = np.linspace(0., intervall, int(schritte*intervall))\n",
    "t_span = (0., intervall)\n",
    "\n",
    "resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,), atol=1.e-10, rtol=1.e-10) #, method='Radau') \n",
    "resultat = resultat1.y.T\n",
    "print('Shape of result: ', resultat.shape)\n",
    "event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}\n",
    "print(event_dict[resultat1.status], ' the message is: ', resultat1.message)\n",
    "print(f'to simulate {intervall:.3f} sec, the routine made {resultat1.nfev:,} function calls and it took {(time.time() - start2):.3f} CPU - sec  ')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot some **coordinates**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "bez = ['X', 'Y', 'Z']\n",
    "fig,(ax1, ax2) = plt.subplots(2, 1, figsize=(10, 10), sharex=True)\n",
    "for i in range(2, int(resultat.shape[1]/2)):\n",
    "    wahl  = (i+1) % 3\n",
    "    wahl1 = (i+1) // 3\n",
    "    ax1.plot(times, np.rad2deg(resultat[:, i]), label=f'{bez[wahl]} - gen. angle of body {wahl1}')\n",
    "ax1.set_title('Generalized angles')\n",
    "ax1.set_ylabel('Angle in degree [°]')\n",
    "ax1.legend();\n",
    "\n",
    "x_coords = np.zeros((len(times), n+1))\n",
    "y_coords = np.zeros((len(times), n+1))\n",
    "z_coords = np.zeros((len(times), n+1))\n",
    "\n",
    "for i in range(len(times)):\n",
    "    for k, ort in enumerate(orte_Dmc_lam):\n",
    "        x_coords[i, k] = ort(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)[0]\n",
    "        y_coords[i, k] = ort(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)[1]\n",
    "        z_coords[i, k] = ort(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)[2]\n",
    "    \n",
    "for j in range(0, n+1):\n",
    "    ax2.plot(times, x_coords[:,j], label='X-Koordinate von Punkt {}'.format(j))\n",
    "    ax2.plot(times, y_coords[:,j], label='Y-Koordinate von Punkt {}'.format(j))\n",
    "    ax2.plot(times, z_coords[:,j], label='Z-Koordinate von Punkt {}'.format(j))\n",
    "ax2.set_title('Coordinates of center of gravity')\n",
    "ax2.set_xlabel('Time [s]')\n",
    "ax2.set_ylabel('Coordinates [m]')\n",
    "ax2.legend(loc='upper right');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Linearisation**\n",
    "\n",
    "From above we have the equation: $M \\cdot \\dot y = force$, that is $\\dot y = M^{-1} \\cdot force$ - but to form $M^{-1} \\cdot force$ symbolically is very time consuming and results in a very large vector. Getting the jacobians of this big vector symbolically may 'take forever'\\\n",
    "I use $\\nabla_1 := \\nabla_{\\left(generalised coordinates, speeds \\right)}$, and $\\nabla_2 := \\nabla_{\\left(controls \\right)}$\n",
    "\n",
    "Hence: $\\nabla_i \\dot y = \\nabla_i M1^{-1} \\cdot force + M^{-1} \\cdot \\nabla_i force$\n",
    "\n",
    "Now: let $\\overline {x_{eq}}$ be the position of equilibrium. Then $force \\Bigr\\rvert_{\\overline {x_{eq}}} = 0$, by definitzion of equilibrium.\n",
    "\n",
    "This means, I can calculate $\\nabla_i force$ symbolically only, and do $M^{-1} \\cdot \\nabla_i force$ later numerically.\n",
    "\n",
    "These are, then, the linearized equations of motion: $\\dot{\\bar y} = \\left( M^{-1} \\cdot \\nabla_1 force \\Bigr\\rvert_{\\overline {x_{eq}}} \\right) \\cdot \\left( \\bar y - \\overline {x_{eq}} \\right) +\\left( M^{-1} \\cdot \\nabla_2 force \\Bigr\\rvert_{\\overline {x_{eq}}} \\right) \\cdot \\overline {controls}$\n",
    "\n",
    "This runs a long time, about 8 min on my PC\n",
    "\n",
    "**NOTE**\\\n",
    "sympy.physics.mechanics does have a *linearize method*, but I think, I may contain a bug.\\\n",
    "In any case, going by 'first principles' is simple here.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start3 = time.time()\n",
    "\n",
    "x_gen = sm.Matrix(qL)                             # generalized coordinates       \n",
    "x_eq = sm.zeros(len(qL),1)                        # equilibrium values for the generalized coordinates, all zero here\n",
    "dict_eq = {k: v for k, v in zip(qL, x_eq)}\n",
    "\n",
    "F_A = force.jacobian(x_gen).subs(dict_eq)\n",
    "F_B = force.jacobian(sm.Matrix([Fx, Fz] + Torque)).subs(dict_eq)\n",
    "\n",
    "print('F_A DS', me.find_dynamicsymbols(F_A))\n",
    "print('F_A FS', F_A.free_symbols)\n",
    "print(f'F_A contains {sm.count_ops(F_A):,} operations, {sm.count_ops(sm.cse(F_A)):,} after cse \\n')\n",
    "\n",
    "print('F_B', me.find_dynamicsymbols(F_B))\n",
    "print('F_B FS', F_B.free_symbols)   \n",
    "print(f'F_B contains {sm.count_ops(F_B):,} operations, {sm.count_ops(sm.cse(F_B)):,} after cse \\n')\n",
    "\n",
    "print(f'Running time {(time.time() - start):.3f} sec')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Calculate the **optimal feed back** to stabilize the linearized system.\\\n",
    "Let P be the solution to the Riccati equation $Q + P \\cdot A + A^T \\cdot P - P \\cdot B \\cdot R^{-1} \\cdot B^T \\cdot P = 0$, where $Q, R$ are the weight matrices in the cost functional related to the coordinates and the control, respectively.\\\n",
    "Then $\\hat{\\overline{control}} = \\left[ R^{-1} \\cdot B^T \\cdot P \\right] \\cdot \\bar y$ ist the optimal control.\\\n",
    "For lack of better reason, I pick $R$ and $Q$ to be the unit matrices of appropriate dimensions.\\\n",
    "The scipy function **solve_continuous_are** numerically solves the *Ricatti equation*.\\\n",
    "The controllability test seems only sufficient for a linear system to be controllable.\\\n",
    "All of this is in books about (linear) control theory. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "\n",
    "pL = [g, m, l, r1, iXX, iYY, iZZ, c1, Fx, Fz] + Torque\n",
    "pL_vals = [9.8, 1., 10., 5., 1, 2, 3, 1.,  0, 0] + [0. for i in range(len(Torque))]\n",
    "\n",
    "F_A_lam = sm.lambdify(qL + pL, F_A, cse=True)\n",
    "F_B_lam = sm.lambdify(qL + pL, F_B, cse=True)\n",
    "\n",
    "equilibrium_point = np.asarray([x.evalf() for x in x_eq], dtype=float)          # just converting the sympy matrix of sympy zeros to a numpy array of float zeros\n",
    "\n",
    "# See the comments above the cell above.\n",
    "A = np.linalg.solve(MM_lam(*equilibrium_point, *pL_vals), F_A_lam(*equilibrium_point, *pL_vals))\n",
    "B = np.linalg.solve(MM_lam(*equilibrium_point, *pL_vals), F_B_lam(*equilibrium_point, *pL_vals))\n",
    "\n",
    "#Is the system controllable? This seems to be only a sufficient condition (?)\n",
    "def controllable(a, b):\n",
    "    a = np.matrix(a)\n",
    "    b = np.matrix(b)\n",
    "    n = a.shape[0]\n",
    "    controllable_matrix = []\n",
    "    for i in range(n):\n",
    "        controllable_matrix.append(a ** i * b)\n",
    "    controllable_matrix = np.hstack(controllable_matrix)\n",
    "    return np.linalg.matrix_rank(controllable_matrix) == n\n",
    "print('System controllable?  ', controllable(A, B))\n",
    "\n",
    "#compute optimal gains K\n",
    "Q = np.eye(A.shape[1])\n",
    "R = np.eye(B.shape[1])\n",
    "S = solve_continuous_are(A, B, Q, R) #solves Riccati equation\n",
    "K = np.dot(np.dot(np.linalg.inv(R), B.T), S)\n",
    "\n",
    "print('it took {:.3f} sec to calculate the feed back'.format(time.time() - start))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical integration** of the linearized system\\\n",
    "The optimal feed back is calculated each step of the integration.\\\n",
    "This is sensitive to the initial conditions, the disturbances from the equilibrium. If they are small enough, it runs very fast, otherwise, the integration runs 'forever'. (In these cases I always interrupted)\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start4 = time.time()\n",
    "\n",
    "intervall = 30\n",
    "schritte = 100\n",
    "\n",
    "# The controls, which originally were in pL_vals will now, of course be the optimal feed back, and added during each step of the integration\n",
    "pL1_vals = [9.8, 1., 10., 5.,  1., 2., 3., 1]\n",
    "\n",
    "# now: kraft = K @ (x(equilibrium) - x(t)). I store the forces and the times in lists, so that I can plot them later.\n",
    "kraefte = []\n",
    "zeiten  = []\n",
    "\n",
    "def gradient(t, x, args):\n",
    "# calculate the control force\n",
    "    kraft = np.dot(K, equilibrium_point - x)\n",
    "    kraefte.append(kraft)\n",
    "    zeiten.append(t)\n",
    "    arguments = np.hstack((x, args, kraft))\n",
    "    return  np.array(np.linalg.solve(MM_lam(*arguments), force_lam(*arguments))).T[0]\n",
    "\n",
    "# The initial conditions introduce some disturbances from the equilibrium point\n",
    "aa = []\n",
    "for i in range(int(len(u)/3)):\n",
    "    aa.append(0.0 * np.random.choice((-1., 1.)))\n",
    "    aa.append(0.55 * np.random.choice((-1., 1.)))\n",
    "    aa.append(0.)\n",
    "y0 = [0., 0.] + [0.15, 0., 0.15] * int(len(q)/3) + [0., 0.] + aa \n",
    "\n",
    "times = np.linspace(0., intervall, int(schritte*intervall))\n",
    "t_span = (0., intervall)\n",
    "\n",
    "resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL1_vals,), atol=1.e-10, rtol=1.e-10) #, method='Radau') \n",
    "resultat = resultat1.y.T\n",
    "print('Shape of result: ', resultat.shape)\n",
    "event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}\n",
    "print(event_dict[resultat1.status], ' the message is: ', resultat1.message)\n",
    "print(f'to simulate {intervall:.3f} sec, the routine made {resultat1.nfev:,} function calls and it took {(time.time() - start4):.3f} CPU - sec  ')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the **generalized angles** and the **feed back**\\\n",
    "It can easily be seen, that the system is driven to its equilibrium point, if the initial disturbances are not too great."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 12), sharex=True)\n",
    "for i in range(2, int(resultat.shape[1]/2)):\n",
    "    wahl  = (i+1) % 3\n",
    "    wahl1 = (i+1) // 3\n",
    "    ax1.plot(times, np.rad2deg(resultat[:, i]), label=f'{bez[wahl]} - gen. angle of body {wahl1}')\n",
    "ax1.set_title('Generalized angles')\n",
    "ax1.set_ylabel('Angle in degree [°]')\n",
    "ax1.legend();\n",
    "\n",
    "kraefte = np.array(kraefte)\n",
    "for i, j in enumerate(('Fx', 'Fy', *['Torque on body ' + str(kk) for kk in range(1, n+1)])):\n",
    "    ax2.plot(zeiten, kraefte[:, i], label=j)\n",
    "ax2.set_title('Control forces / torques')\n",
    "ax2.set_xlabel('Time [s]')\n",
    "ax2.set_ylabel('Force [N] / Torque [Nm]')   \n",
    "ax2.legend();"
   ]
  }
 ],
 "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.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}