Ellipse-on-uneven-street.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "import numpy as np\n",
    "from scipy.optimize import fsolve, minimize\n",
    "from scipy.integrate import solve_ivp\n",
    "from matplotlib import animation\n",
    "from IPython.display import HTML\n",
    "import matplotlib\n",
    "from matplotlib import patches\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "import time\n",
    "matplotlib.rcParams['animation.embed_limit'] = 2**128"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A homogenious ellipse of mass *m* and parameters *a, b* rolls on an uneven street without slipping of jumping. A particle of mass $m_o$ may be attached anywhere within th ellipse.\\\n",
    "The street is a 'line' in the X/Y plane, gravitation points in the negative Y - direction.\n",
    "\n",
    "\n",
    "Note: the special case of the ellipse running on a horizontal line is solved explicitly here:\n",
    "https://www.mapleprimes.com/DocumentFiles/210428_post/rolling-ellipse.pdf\n",
    "\n",
    "**Parameters**\n",
    "- *N*: inertial frame\n",
    "- *A*: frame fixed to the ellipse\n",
    "- $P_0$: point fixed in *N*\n",
    "- *Dmc*: center of the ellipse\n",
    "- *CP*: contact point\n",
    "- $P_o$: location of the particle fixed to the ellipse\n",
    "\n",
    "\n",
    "- *q, u*: angle of rotation of the ellipse, its speed\n",
    "- $x, u_x$: X coordinate of the contact point CP, its speed\n",
    "- $m_x, m_y, um_x, um_y$: coordinates of the center of the ellipse, its speeds\n",
    "\n",
    "\n",
    "- $m, m_o$: mass of the ellipse, of the particle attached to the ellipse\n",
    "- *a, b*: semi axes of the ellipse\n",
    "- *amplitude, frequenz*: parameters for the street.\n",
    "- $i_{ZZ}$: moment of inertia of the ellipse around the Z axis\n",
    "- $\\alpha, \\beta$: determine the location of the particel w.r.t. Dmc\n",
    "- $CP_x, CP_y$: needed only to set up the equation for the ellipse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "\n",
    "m, mo, g, CPx, CPy, a, b, iZZ, alpha, beta = sm.symbols('m, mo, g, CPx, CPy, a, b, iZZ, alpha, beta')\n",
    "mx, my, umx, umy = me.dynamicsymbols('mx, my, umx, umy')\n",
    "q, x, u, ux = me.dynamicsymbols('q, x, u, ux')\n",
    "\n",
    "t = sm.symbols('t')\n",
    "amplitude, frequenz = sm.symbols('amplitude, frequenz')\n",
    "\n",
    "N, A = sm.symbols('N, A', cls=me.ReferenceFrame)\n",
    "P0, Dmc, CP, Po = sm.symbols('P0, Dmc, CP, Po', cls=me.Point)\n",
    "\n",
    "P0.set_vel(N, 0.)\n",
    "\n",
    "A.orient_axis(N, q, N.z)\n",
    "A.set_ang_vel(N, u*N.z)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Model the street.\\\n",
    "It is a parabola, open to the top, with superimposed sinus waves.\\\n",
    "Then I calculate the formula for its osculating circle, the formula of which I found in the internet."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Modeling the street\n",
    "#============================================\n",
    "rumpel = 3  # the higher the number the more 'uneven the street'\n",
    "#============================================\n",
    "\n",
    "strasse = sum([amplitude/j * sm.sin(j*frequenz * CPx) for j in range(1, rumpel)])\n",
    "strassen_form = (frequenz/2. * CPx)**2\n",
    "gesamt = strassen_form + strasse\n",
    "gesamtdx = gesamt.diff(CPx)\n",
    "\n",
    "r_max = ((sm.S(1.) + (gesamt.diff(CPx))**2 )**sm.S(3/2)/gesamt.diff(CPx, 2)).subs({CPx: x})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Find the center of the ellipse**\\\n",
    "To do this, I proceed as follows:\n",
    "- I set up the general equation for a ellipse which is rotated about an angle $\\phi$ and its center is at an arbitrary point (mx, my). With this equation, given a point on the circumference of the ellipse and its rotation $\\phi$ one can numerically calculate mx, my.\n",
    "- I pick the point of the circumference of the ellipse in such a way, that it is the *contact point* with the street. A *necessary* condition for this is, that the tangent of the ellipse at this point $x_0$ is parallel to the tangent of the street at the same point.\n",
    "- in order to get $\\frac{d}{dx}ellipse(y, a, b, $\\phi$)$ I first solve the equation of the ellipse for *y*, that is *y = function(x, a, b, $\\phi$)*, and then I calculate $\\frac{d}{dx}function(..)$.\n",
    "\n",
    "Soving the equation of the ellipse for *y* has two solutions. I picked the right one by trial and error.\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# This is to get the center of the ellipse, given the contact point and its derivative\n",
    "#=============================================\n",
    "Diag = sm.diag(1./a**2, 1./b**2, 1.)\n",
    "#A_matrix = sm.Matrix([[sm.cos(q1), sm.sin(q1)], [-sm.sin(q1), sm.cos(q1)]]) # rotational matrix\n",
    "A1 = A.dcm(N)\n",
    "print('A1: ', A1, '\\n')\n",
    "vektor =sm.Matrix([(CPx - mx), (CPy - my), 0.])\n",
    "ellipse = ((vektor.T * A1.T) * Diag * (A1 * vektor)) - sm.Matrix([sm.S(1.)])\n",
    "ellipse[0, 0] = ellipse[0, 0].simplify()\n",
    "print('ellipse free symbols', ellipse.free_symbols)\n",
    "print('ellipse DS', me.find_dynamicsymbols(ellipse), '\\n')\n",
    "\n",
    "ellipse_x = sm.solve(ellipse, CPy) # ellipse solved for y = f(x), so d/dt(f(x)) may be calculated\n",
    "ellipse_dx = (ellipse_x[0][0].diff(CPx)).simplify()\n",
    "\n",
    "print('ellipse_dx free symbols', ellipse_dx.free_symbols)\n",
    "print('ellipse_dx DS', me.find_dynamicsymbols(ellipse_dx), '\\n')\n",
    "\n",
    "# Richtung1 gives the center of the ellipsoid, given the contact points and its rotation q1\n",
    "richtung = (sm.Matrix([ellipse[0, 0], ellipse_dx - gesamtdx]).subs({CPy: gesamt})).subs({CPx: x})\n",
    "print('Richtung free symbols', richtung.free_symbols)\n",
    "print('Richtung DS', me.find_dynamicsymbols(richtung), '\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Speed constraints**\\\n",
    "Above I calculated the (non linear) equation for the coordinates *mx, my* of the center of the ellipse. From this *configuration constraint* I calculate the resulting *speed constraints* the usual way:\n",
    "- umx = loesung[0]\n",
    "- umy = loesung[1]\n",
    "\n",
    "*linsolve* does not work here practically, it takes too long.\n",
    "loesung also contains $\\frac{d}{dt}x(t)$, and I substitute as soon as $rhs_x$ is available, see below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "CP.set_pos(P0, x*N.x + gesamt*N.y)\n",
    "Dmc.set_pos(P0, mx*N.x + my*N.y)\n",
    "\n",
    "richtung_dict = {sm.Derivative(mx, t): umx, sm.Derivative(my, t): umy, sm.Derivative(q, t): u}\n",
    "\n",
    "richtungdt = richtung.diff(t).subs(richtung_dict)\n",
    "\n",
    "matrix_A = richtungdt.jacobian((umx, umy))\n",
    "vector_b = richtungdt.subs({umx: 0., umy: 0.})\n",
    "loesung = matrix_A.LUsolve(-vector_b)\n",
    "print('loesung DS', me.find_dynamicsymbols(loesung))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Relationship of x(t) to q(t)**:\n",
    "\n",
    "\n",
    "\n",
    "Obviously, $ x(t) = function(q(t), gesamt(x(t)), a, b) $.\n",
    "When the ellipse is rotated from 0 to $q$, the arc length is $\\int_{0}^{q(t)} \\sqrt{a^2sin(k)^2 + b^2cos(k)^2}\\,dk \\ $.\n",
    "\n",
    "The arc length of a function f(k(t)) from 0 to $x(t)$ is: $ \\int_{0}^{x(t)} \\sqrt{1 +  (\\frac{d}{dx}(f(k(t))^2} \\,dk \\ $ (I found all this in the internet)\n",
    "\n",
    "This gives the sought after relationship between $q(t)$ and $x(t)$:\n",
    "\n",
    "$\\int_{0}^{q(t)} \\sqrt{a^2sin(k)^2 + b^2cos(k)^2}\\,dk \\  \\   =  \\int_{0}^{x(t)} \\sqrt{1 + (\\frac{d}{dk}(gesamt(k(t))^2}\\,dk \\ $, differentiated w.r.t *t*:\n",
    "- $\\sqrt{a^2sin(q(t))^2 + b^2cos(q(t))^2}\\  \\cdot (-\\frac{d}{dt}q(t))  = \\sqrt{1 + (\\frac{d}{dx}(gesamt(x(t))^2} \\cdot d/dt(x(t)) $, that is solved for $\\frac{d}{dt}(x(t))$:\n",
    "\n",
    "\n",
    "- $\\frac{d}{dt}(x(t)) = \\dfrac{-\\sqrt{a^2sin(q(t))^2 + b^2cos(q(t))^2}} {\\sqrt{1 + (\\frac{d}{dx}(gesamt(x(t))^2}} \\cdot u(t)$\n",
    "\n",
    "The - sign is a consequence of the 'right hand rule' for frames. This is the sought after first order differential equation for $x(t)$.\n",
    "\n",
    "I calculate the **speed of Dmc** using *Dmc.pos_from(P0).diff(t, N)* This contains terms of $\\frac{d}{dt}x(t)$.\n",
    "For some reason I do not understand, they **must** be replaced by $rhs_x$ right here! If I do not substitute $\\frac{d}{dt}x(t)$ right here, this term gets into the force vector function, and cannot be removed there anymore - at least I found no way of doing it.\\\n",
    "Actually, if I do not replace $\\frac{d}{dt}x(t)$ right here and I try to get the *dynamic symbols* of the force vector, they do **not** contain $\\frac{d}{dt}x(t)$ - but lambdifying with cse = True will throw an error.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "sigma = sm.sqrt((a*sm.sin(q))**2 + (b*sm.cos(q))**2)\n",
    "subs_dict1 = {sm.Derivative(q, t): u, CPx: x}\n",
    "rhsx = (-u * sigma/sm.sqrt(1. + gesamtdx**2)).subs(subs_dict1)\n",
    "print('rhsx DS', me.find_dynamicsymbols(rhsx))\n",
    "print('rhsx free symbols', rhsx.free_symbols)\n",
    "print(rhsx.count_ops(visual=False))\n",
    "\n",
    "loesung = loesung.subs({sm.Derivative(x, t): rhsx})\n",
    "print('loesung DS', me.find_dynamicsymbols(loesung))\n",
    "Dmc_dict = {umx: loesung[0], umy: loesung[1], sm.Derivative(x, t): rhsx}\n",
    "Dmc.set_vel(N, (Dmc.pos_from(P0).diff(t, N)).subs(richtung_dict).subs(Dmc_dict))\n",
    "\n",
    "Po.set_pos(Dmc, a*alpha*A.x + b*beta*A.y)\n",
    "Po.v2pt_theory(Dmc, N, A)\n",
    "\n",
    "print('v(Dmc) DS:', me.find_dynamicsymbols(Dmc.vel(N), reference_frame=N))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Kane's equations**\\\n",
    "There is nothing special here.\\\n",
    "I need to solve the differential equation for $x(t)$ numerically, so this is added to the force vector. Of course the mass matrix must the enlarged correspondingly."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start1 = time.time()\n",
    "I = me.inertia(A, 0., 0., iZZ)\n",
    "bodye = me.RigidBody('bodye', Dmc, A, m, (I, Dmc))\n",
    "Poa = me.Particle('Poa', Po, mo)\n",
    "BODY = [bodye, Poa]\n",
    "\n",
    "FL = [(Dmc, -m*g*N.y), (Po, -mo*g*N.y)]\n",
    "\n",
    "kd = [u - q.diff(t), umx - mx.diff(t), umy - my.diff(t)]\n",
    "speed_constr = [umx - loesung[0], umy - loesung[1]]\n",
    "\n",
    "q1 = [q, mx, my]\n",
    "u_ind = [u]\n",
    "u_dep = [umx, umy]\n",
    "\n",
    "KM1 = me.KanesMethod(N, q_ind=q1, u_ind=u_ind, u_dependent=u_dep, kd_eqs=kd, \n",
    "        velocity_constraints=speed_constr)\n",
    "(fr, frstar) = KM1.kanes_equations(BODY, FL)\n",
    "MM1 = KM1.mass_matrix_full\n",
    "force1 = KM1.forcing_full #.subs({sm.Derivative(x, t): rhsx})\n",
    "\n",
    "force = sm.Matrix.vstack(force1, sm.Matrix([rhsx]))\n",
    "force = force.subs({sm.Derivative(x, t): rhsx})\n",
    "force = force.subs({sm.Derivative(x, t): rhsx})\n",
    "print('force DS', me.find_dynamicsymbols(force))\n",
    "print('force free symbols', force.free_symbols)\n",
    "print('force has {} operations'.format(sum([force[i].count_ops(visual=False) \n",
    "                                             for i in range(len(force))])), '\\n')\n",
    "\n",
    "MM = sm.Matrix.hstack(MM1, sm.zeros(6, 1))\n",
    "hilfs = sm.Matrix.hstack(sm.zeros(1, 6), sm.eye(1))\n",
    "MM = sm.Matrix.vstack(MM, hilfs)\n",
    "print('MM DS', me.find_dynamicsymbols(MM))\n",
    "print('MM free symbols', MM.free_symbols)\n",
    "print('MM has {} operations'.format(sum([MM[i, j].count_ops(visual=False) for i in range(MM.shape[0]) \n",
    "        for j in range(MM.shape[1])])), '\\n')\n",
    "\n",
    "print(f'it took {time.time() - start1 :.3f} sec to establish Kanes equations')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the *sympy functions* are converted to *numpy functions* so numerical calculations may be done.\\\n",
    "Before, I calculate the functions for the *potential energy* and for the *kinetic energy*."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "pot_energie = m * g * me.dot(Dmc.pos_from(P0), N.y) + mo * g * me.dot(Po.pos_from(P0), N.y)\n",
    "kin_energie = sum([koerper.kinetic_energy(N) for koerper in BODY])\n",
    "\n",
    "qL = q1 + u_ind + u_dep + [x]\n",
    "pL = [m, mo, g, a, b, iZZ, alpha, beta, amplitude, frequenz]\n",
    "\n",
    "MM_lam = sm.lambdify(qL + pL, MM, cse=True)\n",
    "force_lam = sm.lambdify(qL + pL, force, cse=True)\n",
    "\n",
    "richtung_lam = sm.lambdify([mx, my] + [q, x] + pL, richtung, cse=True)\n",
    "gesamt = gesamt.subs({CPx: x})\n",
    "gesamt_lam = sm.lambdify([x] + pL, gesamt, cse=True)\n",
    "loesung_lam = sm.lambdify([q, mx, my, x, u] + pL, loesung, cse=True)\n",
    "loesung1_lam = sm.lambdify(qL + pL, loesung, cse=True)  # just for ease of plotting\n",
    "\n",
    "pot_lam = sm.lambdify(qL + pL, pot_energie, cse=True)\n",
    "kin_lam = sm.lambdify(qL + pL, kin_energie, cse=True)\n",
    "\n",
    "r_max_lam = sm.lambdify([x] + pL, r_max, cse=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical integration**\n",
    "- the parameters and the initial values of independent coordinates are set.\n",
    "- the initial values of the dependent coordinates are calculated numerically.\n",
    "- the ellipse must touch the street on exactly one point. I i check whether this condition is met, and an exception is raised if the minimal osculating circle of the street is smaller than the largest osculating circle of the ellipse.\n",
    "- an exception is raised if $\\alpha$ or $\\beta$ are selected such that the particle will be outside of the ellipse. \n",
    "\n",
    "For stiff problems, method = *'Radau'* seems to be better than *no method*.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#=============================================\n",
    "# Input parameters\n",
    "#=============================================\n",
    "m1 = 1.\n",
    "mo1 = 1.\n",
    "g1 = 9.8\n",
    "a1 = 1.\n",
    "b1 = 2.\n",
    "amplitude1 = 1.\n",
    "frequenz1  = 0.275\n",
    "\n",
    "alpha1 = 0.5\n",
    "beta1 = 0.5\n",
    "\n",
    "q11 = 0.5\n",
    "u11 = 2.\n",
    "x11 = -10.\n",
    "\n",
    "intervall = 15.\n",
    "#==============================================\n",
    "start1 = time.time()\n",
    "\n",
    "if alpha1**2/a1**2 + beta1**2/b1**2 >= 1.:\n",
    "    raise Exception('Particle is outside the ellipse')\n",
    "\n",
    "iZZ1 = 0.25 * m1 * (a1**2 + b1**2)   # from the internet\n",
    "schritte = int(intervall * 30.)\n",
    "\n",
    "pL_vals = [m1, mo1, g1, a1, b1, iZZ1, alpha1, beta1, amplitude1, frequenz1]\n",
    "\n",
    "# numerically find the starting values of mx, my\n",
    "def func1(x0, args):\n",
    "    return richtung_lam(*x0, *args).reshape(2)\n",
    "x0 = (x11, gesamt_lam(x11, *pL_vals) + b1)\n",
    "args = [q11, x11] + pL_vals\n",
    "for _ in range(3):\n",
    "    mxmy, _, _, nachricht = fsolve(func1, x0, args, full_output=True, xtol=1.e-12)\n",
    "    x0 = mxmy\n",
    "mx1, my1 = mxmy\n",
    "print(' {} To {:.4f} / {:.4f}. Accuracy is {} / {}'.format(nachricht, mx1, my1, func1(mxmy, args)[0], \n",
    "        func1(mxmy, args)[1]))\n",
    "\n",
    "# get initial speed of Dmc\n",
    "umx1, umy1 = loesung_lam(q11, mx1, my1, x11, u11, *pL_vals)\n",
    "umx1, umy1 = umx1[0], umy1[0]\n",
    "print('initial speed of Dmc: umx = {:.3f}, umy = {:.3f}'.format(umx1, umy1), '\\n')\n",
    "\n",
    "y0 = [q11, mx1, my1] + [u11] + [umx1, umy1] + [x11]\n",
    "print('initial conditions are:')\n",
    "print(y0, '\\n')\n",
    "\n",
    "#find the largest admissible r_max, given strasse, amplitude, frequenz\n",
    "r_max = max(a1**2/b1, b1**2/a1)  # max osculating circle of an ellipse\n",
    "def func2(x, args):\n",
    "# just needed to get the arguments matching for minimize\n",
    "    return np.abs(r_max_lam(x, *args))\n",
    "\n",
    "x0 = 0.1            # initial guess\n",
    "minimal = minimize(func2, x0, pL_vals)\n",
    "if r_max < (x111 := minimal.get('fun')):\n",
    "    print('selected r_max = {} is less than maximally admissible radius = {:.2f}, hence o.k.'\n",
    "          .format(r_max, x111), '\\n')\n",
    "else:\n",
    "    print('selected r_max {} is larger than admissible radius {:.2f}, hence NOT o.k.'\n",
    "          .format(r_max, x111), '\\n')\n",
    "    raise Exception('the radius of the disc is too large')\n",
    "\n",
    "\n",
    "def gradient(t, y, args):\n",
    "    sol = np.linalg.solve(MM_lam(*y, *args), force_lam(*y, *args))\n",
    "    return np.array(sol).T[0]\n",
    "\n",
    "\n",
    "times = np.linspace(0., intervall, schritte)\n",
    "t_span = (0., intervall)\n",
    "\n",
    "resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,), method='Radau', atol=1.e-6,\n",
    "    rtol=1.e-6)\n",
    "resultat = resultat1.y.T\n",
    "print(resultat.shape)\n",
    "event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}\n",
    "print(event_dict[resultat1.status])\n",
    "print('the integration made {} function calls. It took {:.3f} sec'\n",
    "      .format(resultat1.nfev, time.time() - start1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the generalized coordinates you want to see."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "bezeichnung = ['q', 'mx', 'my', 'u', 'umx', 'umy', 'x']\n",
    "for i in (0, 1, 2):\n",
    "    ax.plot(times, resultat[:, i], label=bezeichnung[i])\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_title('generalized coordinates')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Energies of the system**\\\n",
    "The total energy should be constant. I assume the small deviations are due to numerical errors, as making $a_{tol}, r_{tol}$ smaller reduces the deviation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "kin_np = np.empty(schritte)\n",
    "pot_np = np.empty(schritte)\n",
    "total_np = np.empty(schritte)\n",
    "\n",
    "for i in range(schritte):\n",
    "    kin_np[i] = kin_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)\n",
    "    pot_np[i] = pot_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)\n",
    "    total_np[i] = kin_np[i] + pot_np[i]\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "ax.plot(times, pot_np, label='potential energy')\n",
    "ax.plot(times, kin_np, label='kinetic energy')\n",
    "ax.plot(times, total_np, label='total energy')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel(\"energy (Nm)\")\n",
    "ax.set_title('Energies of the system')\n",
    "ax.legend();\n",
    "total_max = np.max(total_np)\n",
    "total_min = np.min(total_np)\n",
    "print('max deviation of total energy from being constant is {:.4f} % of max total energy'.\n",
    "      format((total_max - total_min)/total_max * 100))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "*Violation of the speed constraints*\\\n",
    "Ideally, they should be zero.\\\n",
    "My formula may be a bit *circular* as I use the speeds calculated in the formula for the speed constraints. I do not know how else I should do it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "deltaX_np = np.empty(schritte)\n",
    "deltaY_np = np.empty(schritte)\n",
    "\n",
    "for i in range(schritte):\n",
    "    deltaX_np[i] = loesung1_lam(*[resultat[i, j] for j in range(resultat.shape[1])], \n",
    "        *pL_vals)[0] - resultat[i, 4]\n",
    "    deltaY_np[i] = loesung1_lam(*[resultat[i, j] for j in range(resultat.shape[1])], \n",
    "        *pL_vals)[1] - resultat[i, 5]\n",
    "    \n",
    "fig, ax = plt.subplots(figsize=(10,5))\n",
    "ax.plot(times, deltaX_np, label='violation in X direction')\n",
    "ax.plot(times, deltaY_np, label='violation in Y direction')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_title('violation of speed constraints')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Animate the motion of the ellipse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#Animation\n",
    "\n",
    "Dmcx = np.array([resultat[i, 1] for i in range(resultat.shape[0])])\n",
    "Dmcy = np.array([resultat[i, 2] for i in range(resultat.shape[0])])\n",
    "\n",
    "Po_lam = sm.lambdify(qL + pL, [me.dot(Po.pos_from(P0), uv) for uv in (N.x, N.y)])\n",
    "Po_np = np.array([Po_lam(*[resultat[i, j] for j in range(7)], *pL_vals) for i in range(schritte)])\n",
    "\n",
    "# needed to give the picture the right size.\n",
    "xmin = np.min(Dmcx)\n",
    "xmax = np.max(Dmcx)\n",
    "ymin = np.min(Dmcy)\n",
    "ymax = np.max(Dmcy)\n",
    "\n",
    "# Data to draw the uneven street\n",
    "cc = max(a1, b1)\n",
    "strassex = np.linspace(xmin - 1.*cc, xmax + 1.*cc, schritte)\n",
    "strassey = [gesamt_lam(strassex[i], *pL_vals) for i in range(schritte)]\n",
    "\n",
    "test_np = np.sort(resultat[:, 6])\n",
    "test1_np = np.array([gesamt_lam(test_np[i], *pL_vals) for i in range(schritte)])\n",
    "\n",
    "if u11 > 0.:\n",
    "    wohin = 'left'\n",
    "else:\n",
    "    wohin = 'right'\n",
    "\n",
    "def animate_pendulum(times, x1, y1, z1):\n",
    "    \n",
    "    fig, ax = plt.subplots(figsize=(15, 15), subplot_kw={'aspect': 'equal'})\n",
    "    \n",
    "    ax.axis('on')\n",
    "    ax.set_xlim(xmin - 1.*cc, xmax + 1.*cc)\n",
    "    ax.set_ylim(ymin - 1.*cc, ymax + 1.*cc)\n",
    "    ax.plot(strassex, strassey)\n",
    "    \n",
    "    ax.plot(test_np, test1_np, color='green')\n",
    "\n",
    "\n",
    "    line1, = ax.plot([], [], 'o-', lw=0.5)                                      # center of the ellipse\n",
    "    line2, = ax.plot([], [], 'o', color=\"black\")                                # particle on the ellipse\n",
    "    line3  = ax.axvline(resultat[0, 6], linestyle='--')                         # vertical tracking line\n",
    "    line4  = ax.axhline(gesamt_lam(resultat[0, 6], *pL_vals), linestyle = '--') # horizontal trackimg line\n",
    "    \n",
    "    elli = patches.Ellipse((x1[0], y1[0]), width=2.*a1, height=2.*b1, angle=np.rad2deg(resultat[0\n",
    "        , 0]), zorder=1, fill=True, color='red', ec='black')\n",
    "    ax.add_patch(elli)\n",
    "\n",
    "    def animate(i):\n",
    "        message = (f'running time {times[i]:.2f} sec \\n Initial speed is {np.abs(u11):.2f} radians/sec to the {wohin}'\n",
    "            f'\\n The dashed lines cross at the contact point \\n The black dot is the particle')\n",
    "        ax.set_title(message, fontsize=20)\n",
    "        ax.set_xlabel('X direction', fontsize=20)\n",
    "        ax.set_ylabel('Y direction', fontsize=20)\n",
    "        elli.set_center((x1[i], y1[i]))\n",
    "        elli.set_angle(np.rad2deg(resultat[i, 0]))\n",
    "                       \n",
    "        line1.set_data(x1[i], y1[i])                  \n",
    "        line2.set_data(z1[i, 0], z1[i, 1])            \n",
    "        line3.set_xdata([resultat[i, 6], resultat[i, 6]]) \n",
    "        wert = gesamt_lam(resultat[i, 6], *pL_vals)\n",
    "        line4.set_ydata([wert, wert])\n",
    "        return line1, line2, line3, line4,\n",
    "\n",
    "    anim = animation.FuncAnimation(fig, animate, frames=schritte,\n",
    "                                   interval=2000*np.max(times) / schritte,\n",
    "                                   blit=True)\n",
    "    plt.close(fig)\n",
    "    return anim\n",
    "\n",
    "anim = animate_pendulum(times, Dmcx, Dmcy, Po_np)\n",
    "print(f'it took {time.time() - start :.3f} sec to run the program BEFORE HTML')\n",
    "HTML(anim.to_jshtml())    # needed, when run on an iPad, I know no other way to do it. It is SLOW!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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.8.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}