disc-on-very-uneven-street.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "import numpy as np\n",
    "from scipy.integrate import solve_ivp\n",
    "from scipy.optimize import fsolve, minimize\n",
    "\n",
    "from matplotlib import animation\n",
    "import matplotlib\n",
    "from matplotlib import patches\n",
    "import matplotlib.pyplot as plt\n",
    "from IPython.display import HTML\n",
    "matplotlib.rcParams['animation.embed_limit'] = 2**128\n",
    "%matplotlib inline\n",
    "\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Needed to exit the loop in the function *event* when a second contact point was found."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Rausspringen(Exception):\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A homogeneous disc with radius r and mass m is running on an uneven 'street' without sliding. \n",
    "The disc is not allowed to jump, hence I can calculate the reaction forces needed to hold it on the\n",
    "street at all times\n",
    "\n",
    "The 'overall' shape of the street is modelled as a parabola (called strassen_form), the unevenness is \n",
    "modelled as a sum of sin functions (called strasse) with each term having a smaller amplitude and higher \n",
    "frequency as the previous one.\n",
    "the 'street itself' is the sum of strasse and strassen_form, called gesamt.\n",
    "\n",
    "When $r < | \\dfrac{(1 + \\left(\\frac{d}{dx}(gesamt(x(t)))\\right)^{3/2}} {\\frac{d^2}{dx^2}\\left(gesamt(x(t))\\right)}|$, the disc will always have only one contact point. If this inequality does not hold, there may be a second contact point. (The formula for osculating radius (Krümmungsradius) is from Wikipedia.)\\\n",
    "The disc is supposed to run over these 'pot holes'. I use the key word **events** in solve_ivp to get the event: a second contact point is there.\n",
    "\n",
    "\n",
    "**Variables**\n",
    "\n",
    "- $q_1, u_1$: rotation of the disc and its speed\n",
    "- $x, u_x$: loction of the contact point and the speed of successive contact points\n",
    "\n",
    "- N: inertial frame\n",
    "- $P_0$ point fixed in N\n",
    "- $A_2$ body fixed frame of disc\n",
    "- $Dmc$: center of disc\n",
    "- $Dmc_o$: location of observer\n",
    "\n",
    "- $m$: mass of the disc\n",
    "- $m_o$: mass of the observer\n",
    "- $i_{ZZ}$: moment of inertia of the disc aroud the Z axis\n",
    "- $\\alpha, \\beta$: location of the observer relative to the center of the disc\n",
    "- $amplitude, frequenz$: parameters of the street\n",
    "- $reibung$: friction\n",
    "- $rhs_1$: just a place holder for $MM^{-1}\\cdot force[1]$ to be calculated later numerically. Needed for the rection force at CP\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "\n",
    "# q1 is the angle of the disc, x is the horizontal position of contact point CP\n",
    "q1, x = me.dynamicsymbols('q1 x')  \n",
    "u1, ux = me.dynamicsymbols('u1 ux')\n",
    "\n",
    "auxx, auxy, fx, fy, rhs1 = me.dynamicsymbols('auxx auxy fx fy rhs1')   # for the reaction forces at contact point CP\n",
    "\n",
    "m, mo, g, r, iZZ, alpha, beta = sm.symbols('m, mo, g, r, iZZ, alpha, beta')\n",
    "amplitude, frequenz, reibung, t = sm.symbols('amplitude frequenz reibung t') # reibung = friction in German\n",
    "\n",
    "N = me.ReferenceFrame('N')         # fixed inertial frame\n",
    "A2 = me.ReferenceFrame('A2')       # fixed to the disc\n",
    "P0, CP, Dmc, Dmco = sm.symbols('P0, CP, Dmc, Dmco', cls=me.Point)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Determine the street and its osculating radius (Schmiegekreis). As the disc should contact the street at exactly one point, this osculating radius must be larger than the radius of the disc. I found the formula for the osculating radius of a function in the internet."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#============================================\n",
    "rumpel = 4    # the higher the number the more 'uneven the street'\n",
    "#============================================\n",
    "def gesamt1(x, amplitude, frequenz):\n",
    "    strasse = sum([amplitude/j * sm.sin(j*frequenz * x) for j in range(1, rumpel)])\n",
    "    strassen_form = (frequenz/4. * x)**2\n",
    "    return strassen_form + strasse\n",
    "gesamt = gesamt1(x, amplitude, frequenz)\n",
    "\n",
    "r_max = (sm.S(1.) + (gesamt.diff(x))**2 )**sm.S(3/2)/gesamt.diff(x, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Relationship of x(t) to q(t)**:\n",
    "\n",
    "Obviously, $ x(t) = function(q(t), gesamt(x(t), r) $.\n",
    "When the disc is rotated through an angle $q$, the arc length is $r\\cdot q(t)$.\n",
    "\n",
    "The arc length of a function f(k(t)) from 0 to $x(t)$ is: $ \\int_{0}^{x(t)} \\sqrt{1 +  \\left(\\frac{d}{dx}(f(k(t)\\right)^2} \\,dk \\ $ (I found this in the internet)\n",
    "\n",
    "This gives the sought after relationship between $q(t)$ and $x(t)$:\n",
    "$ r \\cdot (-q(t))  =  \\int_{0}^{x(t)} \\sqrt{1 + \\left( \\frac{d}{dk}(gesamt(k(t) \\right)^2}\\,dk \\ $, differentiated w.r.t *t*:\n",
    "- $ r \\cdot (-u)  = \\sqrt{1 + \\left( \\frac{d}{dx}(gesamt(x(t))\\right)^2} \\cdot \\frac{d}{dt}(x(t) $, that is solved for $\\frac{d}{dt}\\left(x(t)\\right)$:\n",
    "\n",
    "- $\\frac{d}{dt}(x(t)) = \\dfrac{-(r \\cdot u)} {\\sqrt{1 + \\left(\\frac{d}{dx}(gesamt(x(t)\\right)^2}}\\$\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)$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "rhs3 = (-u1 * r / sm.sqrt(1. + (gesamt1(x, amplitude, frequenz).diff(x))**2)).simplify()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The vector perpendicular to the strasse is -($\\frac{d}{dx}gesamt(x), - 1$). I found this in the internet.\\\n",
    "The leading minus sign, because directed 'inward'. It points from the contact point CP to the geometric center of the disc Dmc. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "#The center of the wheel is at distance r from CP, perpendicular to the surface of the street.\n",
    "vector = (-(gesamt1(x, amplitude, frequenz).diff(x)*N.x - N.y)).simplify()\n",
    "\n",
    "A2.orient_axis(N, q1, N.z)\n",
    "A2.set_ang_vel(N, u1 * N.z)\n",
    "\n",
    "CP.set_pos(P0, x*N.x + gesamt1(x, amplitude, frequenz)*N.y)    # location of contact point\n",
    "Dmc.set_pos(CP, r * (vector.normalize()).simplify())\n",
    "TEST = Dmc.pos_from(P0)\n",
    "print('Dmc DS', me.find_dynamicsymbols(TEST, reference_frame=N))\n",
    "print('Dmc FS', TEST.free_symbols(reference_frame=N))\n",
    "\n",
    "CP.set_vel(N, (CP.pos_from(P0).diff(t, N)).subs({sm.Derivative(x, t): rhs3}))\n",
    "Dmc.set_vel(N, Dmc.pos_from(P0).diff(t, N).subs({sm.Derivative(x, t): rhs3}) + auxx*N.x + auxy*N.y)\n",
    "print('Dmc DS', me.find_dynamicsymbols(Dmc.vel(N), reference_frame=N))\n",
    "print('Dmc FS', Dmc.vel(N).free_symbols(reference_frame=N))\n",
    "\n",
    "\n",
    "Dmco.set_pos(Dmc, r * (alpha*A2.x + beta*A2.y))\n",
    "Dmco.set_vel(N, Dmco.pos_from(P0).diff(t, N).subs({sm.Derivative(x, t): rhs3, sm.Derivative(q1, t): u1}))\n",
    "\n",
    "# just needed for potting later\n",
    "Dmco_pos = [me.dot(Dmco.pos_from(P0), uv) for uv in (N.x, N.y)]\n",
    "Dmc_pos = [me.dot(Dmc.pos_from(P0), uv) for uv in (N.x, N.y)]\n",
    "CP_pos = [me.dot(CP.pos_from(P0), uv) for uv in (N.x, N.y)] # just for the plotting"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "I need this simple function to later see, if there is a **second contact point**, $CP_2$, at position ($xh, gesamt(xh, frequenz, amplitude, rumpel)$\\\n",
    "I only need to look into the direction the disc is moving, and only the interval $(x, 2\\cdot r]$ "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xh  = sm.symbols('xh')\n",
    "CP2 = me.Point('CP2')\n",
    "CP2.set_pos(P0, xh*N.x + gesamt1(xh, amplitude, frequenz)*N.y)\n",
    "abstand2 = Dmc.pos_from(CP2).magnitude()\n",
    "print('abstand2 DS', me.find_dynamicsymbols(abstand2))\n",
    "print('abstand2 FS', abstand2.free_symbols)\n",
    "\n",
    "abstand2_lam = sm.lambdify([x, xh, r, amplitude, frequenz], abstand2, cse=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define the bodies and the energies.\n",
    "I find it useful to calculate and plot the energies: They may give hints, that there may be a mistake in setting up Kane's equations - like it happened with almost every example I try to set up."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "I = me.inertia(A2, 0., 0., iZZ)                                              \n",
    "Body = me.RigidBody('Body', Dmc, A2, m, (I, Dmc))\n",
    "observer = me.Particle('observer', Dmco, mo)\n",
    "BODY = [Body, observer]\n",
    "kin_energie = (Body.kinetic_energy(N) + observer.kinetic_energy(N)).subs({auxx: 0., auxy:  0.})\n",
    "pot_energie = m * g * me.dot(Dmc.pos_from(P0), N.y) + mo * g * me.dot(Dmco.pos_from(P0), N.y) \n",
    "\n",
    "print('kinetic energy DS: ', me.find_dynamicsymbols(kin_energie))\n",
    "print('kinetic energy free symbols', kin_energie.free_symbols)\n",
    "print('potential energy DS: ', me.find_dynamicsymbols(pot_energie))\n",
    "print('potential energy free symbols', pot_energie.free_symbols)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Kane's equations**\n",
    "\n",
    "- determine the external forces, here only gravitational forces\n",
    "- get the equation for the reaction forces. They (of course) depend on the accelerations of the masses, hence on $rhs = MM^{-1} \\cdot force$. It is calculated numerically later on.\n",
    "- add the term to calculate the  X - position of CP at the bottom of force. Recall we got a differential equation for x(t).\n",
    "- enlarge the mass matrix appropriately\n",
    "- convert the sympy functions to numpy functions, using *lambdify(..)*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "FL = [(Dmc, -m*g*N.y), (Dmco, -mo*g*N.y), (Dmc, fx*N.x + fy*N.y), (A2, -reibung*u1*A2.z)]\n",
    "kd = [u1 - q1.diff(t)]\n",
    "q = [q1]\n",
    "u = [u1]\n",
    "aux = [auxx, auxy]\n",
    "\n",
    "KM = me.KanesMethod(N, q_ind=q, u_ind=u, kd_eqs=kd, u_auxiliary=aux)\n",
    "(fr, frstar) = KM.kanes_equations(BODY, FL)\n",
    "MM = KM.mass_matrix_full\n",
    "force = KM.forcing_full\n",
    "\n",
    "# Reaction forces\n",
    "eingepraegt = KM.auxiliary_eqs.subs({sm.Derivative(u1, t): rhs1, sm.Derivative(x, t): rhs3})\n",
    "print('eingepraegt DS', me.find_dynamicsymbols(eingepraegt))\n",
    "print('eingepraegt free symbols', eingepraegt.free_symbols)\n",
    "print('eingepraegt has {} operations'.format(sum([eingepraegt[i].count_ops(visual=False) for i in range(len(eingepraegt))])), '\\n')\n",
    "\n",
    "# Add rhs3 at the bottom of force, to get d/dt(x) = rhs3. This is to numerically integrate x(t)\n",
    "force = sm.Matrix.vstack(force, sm.Matrix([rhs3])).subs({sm.Derivative(x, t): rhs3, fx: 0., fy: 0.}) \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) for i in range(len(force))])), '\\n')\n",
    "\n",
    "# Enlarge MM properly\n",
    "MM = sm.Matrix.hstack(MM, sm.Matrix([0., 0.])).subs({sm.Derivative(x, t): rhs3})\n",
    "MM = sm.Matrix.vstack(MM, sm.Matrix([0., 0., 1.]).T)\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]) for j in range(MM.shape[1])])), '\\n')\n",
    "\n",
    "# Lambdification. Turning symbolic expressions into numpy functions.\n",
    "pL = [m, mo, g, r, iZZ, alpha, beta, amplitude, frequenz, reibung]\n",
    "qL = q + u + [x]\n",
    "F = [fx, fy]\n",
    "\n",
    "MM_lam = sm.lambdify(qL + pL, MM, cse=True)\n",
    "force_lam = sm.lambdify(qL + pL, force, cse=True)\n",
    "\n",
    "CP_pos_lam = sm.lambdify(qL + pL, CP_pos, cse=True)\n",
    "Dmc_pos_lam = sm.lambdify(qL + pL, Dmc_pos, cse=True)\n",
    "Dmco_pos_lam = sm.lambdify(qL + pL, Dmco_pos, cse=True)\n",
    "\n",
    "# will be solved for F numerically later.\n",
    "eingepraegt_lam = sm.lambdify(F + qL + pL + [rhs1], eingepraegt, cse=False) \n",
    "\n",
    "#this is needed to plot the shape of the street\n",
    "strasse_lam = sm.lambdify([x] + pL,  gesamt, cse=True)\n",
    "\n",
    "kin_lam = sm.lambdify(qL + pL, kin_energie, cse=True)\n",
    "# kin1_lam is needed further down for fsolve, where I have to solve for u1\n",
    "kin1_lam = sm.lambdify([u1] + [q1, x] + pL, kin_energie, cse=True)\n",
    "pot_lam = sm.lambdify(qL + pL, pot_energie, cse=True)\n",
    "\n",
    "r_max_lam = sm.lambdify([x] + pL, r_max, cse=True)\n",
    "\n",
    "print('it took {:.3f} sec to establish Kanes equations'.format(time.time() - start))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical integration**\n",
    "\n",
    "A.\\\n",
    "Parameter / initial values\n",
    "- $q_{11}, u_{11}$: rotation of the disc and its speed\n",
    "- $x_1, u_{x1}$: loction of the contact point and the spped of successive contact points\n",
    "\n",
    "- $m_1$: mass of the disc\n",
    "- $m_{o1}$: mass of the observer\n",
    "- $i_{ZZ1}$: moment of inertia of the disc aroud the Z axis\n",
    "- $\\alpha_1, \\beta_1$: location of the observer relative to the center of the disc\n",
    "- $amplitude_1, frequenz_1$: parameters of the street\n",
    "- $reibung_1$: friction\n",
    "\n",
    "While it makes sense to call these values similar to their names when setting up Kane's equations, **avoid** the **same** name: The symbols / dynamic symbols get overwritten, with unintended consequences.\\\n",
    "In order to find a second contaxct point, at least the way I could come up with, *step_size* and *max_step* have to be small, slowing down the integration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# Input parameters \n",
    "#==========================================================\n",
    "m1 = 1.\n",
    "mo1 = 1.\n",
    "r1 =  2.\n",
    "alpha1, beta1 = 0., 0.99\n",
    "amplitude1 = 1.\n",
    "frequenz1 = .9   # the smaller this number, the more 'even' the street   \n",
    "reibung1 = 0.        # Friction\n",
    "intervall = 10.     # time inverval of integration is [0., intervall]\n",
    "\n",
    "q11 = 0.             # starting angle. As the disc is symmetric about this angle, it plays no real role\n",
    "u11 = 3.5             # starting angular velocity of disc.\n",
    "x1  = 7.5             # Starting X position of disc. \n",
    "\n",
    "step_size = 0.01    # Stepsize to be used to search for the second CP\n",
    "max_step  = 0.01     # Max. stepsize for solve_ivp \n",
    "\n",
    "punkte    = 500      # Determines the number of times given out by the solution of solve_ivp\n",
    "#==========================================================\n",
    "schritte = int(intervall * punkte)\n",
    "\n",
    "iZZ1 = 1/2 * m1 * r1**2\n",
    "pL_vals = [m1, mo1,  9.8, r1, iZZ1, alpha1, beta1, amplitude1, frequenz1, reibung1]\n",
    "\n",
    "y0 = [q11, u11, x1]\n",
    "print('Arguments')\n",
    "print('[m, mo1,  g, r, iZZ, iXY, alpha, beta, amplitude, frequenz, reibung]')\n",
    "print(pL_vals, '\\n')\n",
    "print('[q11, u11, x1]')\n",
    "print(y0, '\\n')\n",
    "\n",
    "startwert = y0[2]   # just needed for the plots below\n",
    "startomega = y0[1]  #  dto.\n",
    "\n",
    "#find the largest admissible r, given strasse, amplitude, frequenz\n",
    "def func(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(func, x0, pL_vals)\n",
    "\n",
    "if pL_vals[3] < (x111 := minimal.get('fun')):\n",
    "    print('selected radius = {} is less than maximally admissible radius = {:.2f}, hence o.k.'\n",
    "          .format(pL_vals[3], x111), '\\n')\n",
    "else:\n",
    "    print('selected radius {} is larger than admissible radius {:.2f}, hence a second contact point may happen.'\n",
    "          .format(pL_vals[3], x111), '\\n')\n",
    "\n",
    "# At the initial position, I do not want a second contact point nearby.\n",
    "for vorzeichen in (-1., 1.):   # determines, which direction we must look at for CP2\n",
    "    bereich    = np.linspace(x1 - vorzeichen*step_size, x1 - vorzeichen * 2. * (r1+1.), int(2.*r1/step_size))\n",
    "    \n",
    "    for xh1 in bereich:\n",
    "        if abstand2_lam(x1, xh1, r1, amplitude1, frequenz1) <= r1:\n",
    "            raise Exception('change starting point')\n",
    "        else:\n",
    "            pass      "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When a new contact point is found, the moment of inertia w.r.t. this new contact point is in general different from the moment of inertia w.r.t. the old contact point. As the total energy must remain constant, $\\frac{d}{dt}q(t)$ must in general change discontinuously.\\\n",
    "This function is used to calculate the new angular velocity $\\frac{d}{dt}q(t)$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def funcCP(x0, args):\n",
    "    return kin1_lam(x0, args[0], args[1], *args[5]) - kin_lam(args[2], args[3], args[4], *args[5])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "B.\\\n",
    "Actual **numerical integration** starts here.\n",
    "\n",
    "The function *event* is needed to stop *solve_ivp* when a second contact point is found, see the documentation of *solve_ivp* for details.\\\n",
    "Of course, I only have to look for second contact points in the direction of the movement at the time.\\\n",
    "Once a second contact point is found, the numerical integration is stopped, and its results stored in *ergebnis*. Then a new numrical integration is started with new initial conditions.\\\n",
    "I need $CP_{2x}$ for the next integration, but I cannot return it, as *solve_ivp* needs a defined output of the *event* function. Hence I make it a global variable.\n",
    "\n",
    "NOTES:\\\n",
    "The *events* keyword of *solve_ivp* requires a continuous function $f(t, y, args)$ with {event happened at $t_0, y_0$} <=> $f(t_0, y_0, args) = 0.$\\\n",
    "I my case, with a numerical criterium, the right way to do it is like this, **a helpful person in *Stack Overflow* explained it to me**:\\\n",
    "As long as the event has not occured, the function returns $-1$. When the event did occur, it returns $+1$. This seems to work fine."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start1 = time.time()\n",
    "schritte = int(intervall * punkte)\n",
    "times = np.linspace(0, intervall, schritte)\n",
    "CP2x = 0.\n",
    "y0 = [q11, u11, x1]\n",
    "zaehl_event = 0\n",
    "#==============================================\n",
    "def event(t, y, args):\n",
    "    global CP2x, zaehl_event\n",
    "    zaehl_event += 1\n",
    "    vorzeichen = np.sign(y[1])  # determines, which direction we must look at for CP2\n",
    "    bereich    = np.linspace(y[2] - 5. * vorzeichen*step_size, y[2] - vorzeichen * 3. * r1, \n",
    "                    int(2.*r1/step_size))\n",
    "    \n",
    "    for xh1 in bereich:\n",
    "        try:\n",
    "            if abstand2_lam(y[2], xh1, r1, amplitude1, frequenz1) <= r1:\n",
    "                raise Rausspringen()\n",
    "            else:\n",
    "                pass\n",
    "                \n",
    "        except:\n",
    "# event_info = True shows, how solve_ivp searches for the exact location where the even took place\n",
    "            if event_info == True:\n",
    "                print(f'second CP, as {(abstand2_lam(y[2], xh1, r1, amplitude1, frequenz1) - r1):.3f} <=0, at time {t:.3f} and location {xh1:.3f}')\n",
    "            CP2x = xh1\n",
    "            return 1.   # 0 a second contact point was found\n",
    "        \n",
    "    return -1. # 1 no second contact point\n",
    "\n",
    "#===============================================\n",
    "event.terminal = True   # if True, this stops the integration if event occurs\n",
    "event_info     = False  # if True, data related to the occurence of events are printed\n",
    "#===============================================\n",
    "\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",
    "runtime    = 0.\n",
    "starttime  = 0.\n",
    "starttime1 = 0.\n",
    "ergebnis   = []\n",
    "event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', \n",
    "            1: 'some termination event'}\n",
    "\n",
    "# here the 'piecewise' integration starts.\n",
    "while starttime < intervall:\n",
    "    resultat1 = solve_ivp(gradient, (starttime, float(intervall)), y0, t_eval=times, args=(pL_vals,), \n",
    "            atol=1.e-7, rtol=1.e-7, max_step=max_step, events=event, method='Radau')\n",
    "\n",
    "    resultat = resultat1.y.T\n",
    "    if event_info == True:\n",
    "        print(event_dict[resultat1.status], ' im loop message is:', resultat1.message)\n",
    "        print('resultat shape', resultat.shape)\n",
    "    \n",
    "    if resultat1.y_events[0].size > 0.:\n",
    "        height = strasse_lam(resultat1.y_events[0][0][2], *pL_vals )\n",
    "        if event_info  == True:\n",
    "            print(f'generalized coordinates at exit time, height of CP        {resultat1.y_events[0][0][0]:.3f} ' +\n",
    "                 f'{resultat1.y_events[0][0][1]:.3f}  {resultat1.y_events[0][0][2]:.3f}  {height:.3f}')\n",
    "            \n",
    "        hilfs0 = resultat1.y_events[0][0][0]\n",
    "        hilfs1 = resultat1.y_events[0][0][1]\n",
    "        hilfs2 = resultat1.y_events[0][0][2]\n",
    "        args1  = [hilfs0, CP2x] + [hilfs0, hilfs1, hilfs2, pL_vals]\n",
    "        x0     = hilfs1\n",
    "\n",
    "# Force the total energy to be constant.        \n",
    "# I iterate here, this sometimes improves the accuracy, as can be seen if you print the x0.\n",
    "        for _ in range(3):\n",
    "            hilfs3 = fsolve(funcCP, x0, args1)\n",
    "            hilfs3 = hilfs3[0]\n",
    "            x0     = hilfs3\n",
    "\n",
    "        y0 = [resultat1.y_events[0][0][0], hilfs3, CP2x]\n",
    "        height = strasse_lam(CP2x, *pL_vals)\n",
    "        if event_info == True:\n",
    "            print(f'initial values for the next integration, height of new CP {hilfs0:.3f} {hilfs3:.3f}' + \n",
    "                  f' {CP2x:.3F}  {height:.3f}' + '\\n')\n",
    "        starttime = resultat1.t_events[0][0]\n",
    "        \n",
    "        schritte = int((intervall - starttime) * punkte)\n",
    "        times    = np.linspace(starttime, intervall, schritte)\n",
    "    \n",
    "        ergebnis.append(resultat)\n",
    "    else:\n",
    "        break\n",
    "\n",
    "print(\"To numerically integrate an intervall of {} sec took {:.5f} sec \"\n",
    "      .format(intervall, time.time() - start1))\n",
    "print('message is', resultat1.message)\n",
    "# stack the individual results of the various integrations, to get the complete results.\n",
    "resultat = np.vstack(ergebnis)\n",
    "print(resultat.shape)\n",
    "\n",
    "# set these values for the subsequent plots below.\n",
    "schritte = resultat.shape[0]\n",
    "times = np.linspace(0., intervall, schritte)\n",
    "print('how often did solve_ivp call event:', zaehl_event)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the generalized coordinates and speeds"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "Dmc_X = np.empty(schritte)\n",
    "Dmc_Y =np.empty(schritte)\n",
    "for i in range(schritte):\n",
    "    Dmc_X[i], Dmc_Y[i] = Dmc_pos_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals)\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "for i, j in zip(range((resultat.shape[1])), ('rotational angle', 'rotational speed', 'displacement')):\n",
    "    ax.plot(times, resultat[:, i], label=j)\n",
    "ax.set_title('Coordinates, friction = {}'.format(reibung1))\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the reaction forces "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# RHS calculated numerically, too large to do it symbolically. Needed for reaction forces.\n",
    "RHS1 = np.zeros((schritte, resultat.shape[1]))\n",
    "for i in range(schritte):\n",
    "    RHS1[i, :] = np.linalg.solve(MM_lam(*[resultat[i, j]for j in range(resultat.shape[1])], *pL_vals), \n",
    "        force_lam(*[resultat[i, j] for j in range(resultat.shape[1])], \n",
    "        *pL_vals)).reshape(resultat.shape[1])\n",
    "print('RHS1 shape', RHS1.shape)\n",
    "\n",
    "def func (x11, *args):\n",
    "# just serves to make the arguments compatible between fsolve and eingepraegt_lam\n",
    "    return eingepraegt_lam(*x11, *args).reshape(len(F))\n",
    "\n",
    "kraft = np.empty((schritte, len(F)))\n",
    "x0 = tuple([1. for i in range(len(F))])   # initial guess\n",
    "for i in range(schritte):\n",
    "    for _ in range(2):\n",
    "        y00 = [resultat[i, j] for j in range(resultat.shape[1])]\n",
    "        args = tuple((y00 + pL_vals + [RHS1[i, 1]]))\n",
    "        A = fsolve(func, x0, args=args).reshape(len(F)) # numerically find fx, fy\n",
    "        x0 = tuple(A)      # updated initial guess, should improve convergence\n",
    "    kraft[i] = A        \n",
    "        \n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "ax.plot(times, kraft[:, 0], label = 'Fx')\n",
    "ax.plot(times, kraft[:, 1], label = 'Fy')\n",
    "ax.set_title('Reaction forces on contact point, friction = {}'.format(reibung1))\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the **energies** of the system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "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",
    "if pL_vals[-1] == 0.:\n",
    "    print('Max deviation from constant of total energy is {:.2e} % of max total energy'\n",
    "          .format((max(total_np) - min(total_np))/max(total_np) * 100.))\n",
    "    print('Max absolute deviation from constant of total energy is {:.2e} Nm'\n",
    "          .format((np.max(total_np) - np.min(total_np))))\n",
    "fig, ax = plt. subplots(figsize=(10, 5))\n",
    "ax.plot(times, kin_np, label='kinetic energy')\n",
    "ax.plot(times, pot_np, label='pos energy')\n",
    "ax.plot(times, total_np, label='total energy')\n",
    "ax.set_title('Energy of the disc, friction = {} frequenz = {}'.format(reibung1,frequenz1))\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the street and the extreme positions of the disc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# plot the street, and the extremes, of the position of the disc.\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "links = np.min(resultat[:, 2])\n",
    "rechts = np.max(resultat[:, 2])\n",
    "ruhe = np.mean([resultat[-30::, 2]])    # get approx. rest position of wheel\n",
    "maximal = max(np.abs(links), np.max(rechts))\n",
    "times1 = np.linspace(-maximal-5, maximal+5, schritte)\n",
    "ax.plot(times1, strasse_lam(times1, *pL_vals)  , label='Strasse')\n",
    "if pL_vals[-1] != 0.:\n",
    "    ax.axvline(ruhe,ls = '--', color='red', label='approx. fimal pos. of wheel')\n",
    "ax.axvline(links,ls = '--', color='green', label='leftmost pos. of wheel')\n",
    "ax.axvline(rechts,ls = '--', color='black', label='rightmost pos. of wheel');\n",
    "ax.axvline(startwert, ls='--', color='orange', label='starting position of wheel')\n",
    "if startomega > 0.:\n",
    "    richtung = 'left'\n",
    "else:\n",
    "    richtung = 'right'\n",
    "text = 'Wheel has speed ' + str(np.abs(startomega)) + ' units to the ' + richtung\n",
    "plt.title(text + ', friction = {} '.format(reibung1))\n",
    "plt.xlabel('horizontal distance (m)')\n",
    "plt.ylabel('elevation (m)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Animation**\n",
    "\n",
    "The blue dot represents the observer.\n",
    "HTML(...) is needed for the animation to show on an iPad, no idea, whether needed on other machines. It is SLOW!\\\n",
    "The number of points considered are *zeitpunkte*. If it builds too slowly, this number may be reduced - possibly at the loss of some accuracy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# reduce the number of points of time to around zeitpunkte\n",
    "times2 = []\n",
    "resultat2 = []\n",
    "index2 = []\n",
    "\n",
    "#=======================\n",
    "zeitpunkte = 400\n",
    "#=======================\n",
    "\n",
    "reduction = max(1, int(len(times)/zeitpunkte))\n",
    "\n",
    "for i in range(len(times)):\n",
    "    if i % reduction == 0:\n",
    "        times2.append(times[i])\n",
    "        resultat2. append(resultat[i])\n",
    " \n",
    "schritte2 = len(times2)\n",
    "print(f'animation used {schritte2} points in time')\n",
    "resultat2 = np.array(resultat2)\n",
    "times2 = np.array(times2)\n",
    "\n",
    "# Location of the center of the disc\n",
    "Dmcx = np.empty(schritte2)\n",
    "Dmcy =np.empty(schritte2)\n",
    "Dmcox = np.empty(schritte2)\n",
    "Dmcoy =np.empty(schritte2)\n",
    "\n",
    "for i in range(schritte2):\n",
    "    Dmcx[i], Dmcy[i] = Dmc_pos_lam(*[resultat2[i, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "    Dmcox[i], Dmcoy[i] = Dmco_pos_lam(*[resultat2[i, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "\n",
    "# needed to give the picture the right size.\n",
    "xmin = min([resultat2[i, 2] for i in range(schritte2)])\n",
    "xmax = max([resultat2[i, 2] for i in range(schritte2)])\n",
    "\n",
    "ymin = min([strasse_lam(resultat2[i, 2], *pL_vals) for i in range(schritte2)]) \n",
    "ymax = max([strasse_lam(resultat2[i, 2], *pL_vals) for i in range(schritte2)]) \n",
    "\n",
    "# Data to draw the uneven street\n",
    "cc = r1\n",
    "strassex = np.linspace(xmin - 3*cc, xmax + 3.*cc, schritte2)\n",
    "strassey = [strasse_lam(strassex[i], *pL_vals) for i in range(len(strassex))]\n",
    "\n",
    "def animate_pendulum(times, x1, y1, x2, y2):    \n",
    "    fig, ax = plt.subplots(figsize=(8, 8), subplot_kw={'aspect': 'equal'})\n",
    "    \n",
    "    ax.axis('on')\n",
    "    ax.set_xlim(xmin - 3.*cc, xmax + 3.*cc)\n",
    "    ax.set_ylim(ymin - 3.*cc, ymax + 3.*cc)\n",
    "    ax.plot(strassex, strassey)\n",
    "    ax.set_xlabel('horizontal distance (m)')\n",
    "    ax.set_ylabel('elevation (m)')\n",
    "    \n",
    "    line1, = ax.plot([], [], 'o-', lw=0.5)\n",
    "    line2  = ax.axvline(resultat2[0, 2], linestyle='--')                         # vertical tracking line\n",
    "    line3  = ax.axhline(strasse_lam(resultat2[0, 2], *pL_vals), linestyle = '--') # horizontal trackimg line\n",
    "    line4, = ax.plot([], [], 'bo', markersize=5) # the dot on the disc, to show it is rotating\n",
    "    \n",
    "    elli = patches.Circle((x1[0], y1[0]), radius = r1, fill=True, color='red', ec='black')\n",
    "    ax.add_patch(elli)\n",
    "\n",
    "    def animate(i):        \n",
    "        ax.set_title('running time {:.2f} sec, friction ={}'.format(times2[i], reibung1), fontsize=15)\n",
    "        \n",
    "        elli.set_center((x1[i], y1[i]))\n",
    "        elli.set_height(2.*r1)\n",
    "        elli.set_width(2.*r1)\n",
    "        elli.set_angle(np.rad2deg(resultat[i, 0]))\n",
    "                       \n",
    "        line1.set_data([x1[i]], [y1[i]])                  # center of the disc\n",
    "        line2.set_xdata([resultat2[i, 2]])  # dashed line to mark the contact point\n",
    "        line3.set_ydata([strasse_lam(resultat2[i, 2], *pL_vals)])    #            dto. \n",
    "        line4.set_data([x2[i]], [y2[i]])\n",
    "        return line1, line2, line3, line4, \n",
    "\n",
    "    anim = animation.FuncAnimation(fig, animate, frames=len(times),\n",
    "                                   interval=1000*max(times) / len(times),\n",
    "                                   blit=True)\n",
    "    plt.close(fig)\n",
    "    return anim\n",
    "\n",
    "anim = animate_pendulum(times2, Dmcx, Dmcy, Dmcox, Dmcoy)\n",
    "print('it took {:.3f} sec to run the program, before HTML(..)'.format(time.time() - start))\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.11.5"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": true,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": false,
   "report_style_numbering": false,
   "user_envs_cfg": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}