rolling-axle-uneven-street-spring 3.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "import time\n",
    "import numpy as np\n",
    "from scipy.integrate import solve_ivp\n",
    "from scipy.optimize import fsolve, minimize, root\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib as mp\n",
    "from matplotlib.patches import Ellipse\n",
    "%matplotlib inline\n",
    "from IPython.display import HTML\n",
    "mp.rcParams['animation.embed_limit'] = 2**128 # I need this for large animations on my iPad\n",
    "from matplotlib import animation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Two homogeneous wheels of mass m and radius r are attached to a massless axle of length l and roll on an uneven street. The wheel is like a disc, no width. This means, the wheels are always perpenticular to the axis. This is used further down to locate $Dmc_L$ and also for the configuration constraint\\\n",
    "The wheels may rotate indenpendently of each other.\n",
    "\n",
    "The basic shape of the street is modeled by strassen_form, the superimposed unevenness is \n",
    "modeled by strasse. (Strasse = Street in German, gesamt = combined in German), that is gesamt = strasse + strassen_form.\n",
    "\n",
    "An 'observer', a particle of mass $m_o$, may be attached anywhere inside the wheel. \n",
    "\n",
    "=================================================================================================================================\n",
    "Define the sympy symbols and the dynamic symbols needed for the calculation\n",
    "- $q_L, q_R$ are the angle of the wheels w.r.t. AX.\n",
    "- $u_L, u_R$ are the speeds\n",
    "- $x_L, z_L, x_R z_R$ are the coordinates of the contact points \n",
    "- $u_{xL}, u_{zL}, u_{xR}, u_{zR}$  the speeds.\n",
    "\n",
    "\n",
    "- the rest are constants needed to describe the physical properties of the system:\n",
    "    - m is the mass of the wheels\n",
    "    - $m_o$ is the mass of the observer\n",
    "    - r is the diameter of the wheel\n",
    "    - l is the distance of the wheel from each other, the length of the axle\n",
    "    - $i_{XX}$, etc are the moments of inertial of the wheels\n",
    "    - reibung is the friction\n",
    "    - amplitude and frequenz determine the shape of the street\n",
    "    - $ \\alpha$ determines the location of the observer w.r.t. the center of the wheel. \n",
    "    \n",
    "    \n",
    "*length_config_constr* allows you to select the method of calculating the lemgth of the configuration constraint further down.\\\n",
    "( This is a 'left over' from a discussion a few months ago on Github, as to why $\\bar v.normalize()$ may throw an error if $\\bar v \\approx 0$.\n",
    "- 1: *config_constr.magnitude() is* used\n",
    "- 2: *me.dot(config_constr, config_constr)* is used\n",
    "- 3: *sm.sqrt(configX^2 + configY^2+ configZ^2)* is used, where configX := me.dot(config_constr, N.x), etc.\n",
    "\n",
    "The printing of the information about the mass matrix and of the force vector takes time. By setting *information = False* this printing my be avoided.\n",
    "\n",
    "This is a bit of a **monster**:\\\n",
    "With rempel = 1, the force vector has about 25 mio operations, the mass matrix has about 4.8 mio operations, running time on my iPad about 2 hours.\\\n",
    "With rempel = 2, the force vector has about 32 mio operations, the mass matrix has about 5.5 mio operations, running time on my iPad about 2.5 hours.\\\n",
    "With rempel = 3, the force vector has about 40 mio operations, the mass matrix has about 7.8 mio operations, running time on my iPad about 3 hours.\\\n",
    "With rempel = 4, the force vector has about 49 mio operations, the mass matrix has about 8.9 mio operations, running time on my iPad about 4 hours."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "#========================\n",
    "length_config_constr = 2\n",
    "information = True\n",
    "#========================\n",
    "\n",
    "if length_config_constr not in [1, 2, 3] or isinstance(length_config_constr, int) == False:\n",
    "    raise Exception('this must be an integer in (1, 2, 3)')\n",
    "\n",
    "qL, qR, q2, q3 = me.dynamicsymbols('qL qR q2 q3', real=True) \n",
    "uL, uR, u2, u3 = me.dynamicsymbols('uL uR u2 u3', real=True)\n",
    "\n",
    "xL, zL, xR, zR = me.dynamicsymbols('xL zL xR zR', real=True)           # coordinates of the contact points of the left / right wheel            \n",
    "uxL, uzL, uxR, uzR = me.dynamicsymbols('uxL uzL uxR uzR', real=True)   # their 'speeds'\n",
    "\n",
    "\n",
    "m, mo, g, r, l, iXX, iYY, iZZ = sm.symbols('m, mo, g, r, l, iXX, iYY, iZZ')\n",
    "amplitude, frequenz, reibung, alpha = sm.symbols('amplitude frequenz reibung alpha')\n",
    "t = me.dynamicsymbols._t"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define the frames and the points as follows:\n",
    "- $N$ = inertial frame\n",
    "- $A_L$ = frame attached to the axle and body fixed frame of the left wheel\n",
    "- $A_R$ = frame attached to the right wheel\n",
    "- $AX_h$ = auxiliary frame, needed for the rotational speed $u_R$ of the right wheel\n",
    "- $P_0$ = fixed reference point\n",
    "- $CP_L$ = contact point, where the left wheel touches the street.\n",
    "- $CP_R$ =   dto for right wheel.\n",
    "- $Dmc_L$ = center of the left wheel\n",
    "- $Dmc_R$ = center of the right wheel\n",
    "- $Po_L, Po_R$ = particles anywhwere within the wheel\n",
    "\n",
    "At the bottom, I solve the linear equations for sm.Derivative(qL, t), etc.\\\n",
    "This is needed below, where these terms must be expressed as functions of $u_L$, $u_2$, etc.\\\n",
    "These equations are also used for the kinematic equations further down."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "N, AL, ALh, AR = sm.symbols('N, AL, ALh, AR', cls = me.ReferenceFrame)         \n",
    "P0, CPL, CPR, DmcL, DmcR, PoL, PoR = sm.symbols('P0, CPL, CPR, DmcL, DmcR, PoL, PoR', cls = me.Point)\n",
    "\n",
    "P0.set_vel(N, 0)\n",
    "\n",
    "AL.orient_body_fixed(N, (q2, qL, q3), '213')\n",
    "rot = AL.ang_vel_in(N)\n",
    "AL.set_ang_vel(N, uL*AL.x + u2*AL.y + u3*AL.z)\n",
    "rot1 = AL.ang_vel_in(N)\n",
    "\n",
    "ALh.orient_axis(AL, -qL, AL.x)\n",
    "ALh.set_ang_vel(AL, -uL * AL.x)\n",
    "\n",
    "AR.orient_axis(ALh, qR, ALh.x)\n",
    "rot11 = AR.ang_vel_in(N)\n",
    "AR.set_ang_vel(ALh, uR * ALh.x)\n",
    "rot12 = AR.ang_vel_in(N)\n",
    "\n",
    "test = [me.dot(rot - rot1, uv) for uv in ALh]\n",
    "print('Dynamic symbols of test', set().union(*[me.find_dynamicsymbols(test[i])for i in range(3)]), '\\n')\n",
    "test1, test2, test3 = sm.symbols('test1, test2, test3')\n",
    "test_dict = {sm.Derivative(qL, t): test1, sm.Derivative(q2, t): test2, sm.Derivative(q3, t): test3}\n",
    "test = [test[i].subs(test_dict) for i in range(3)]\n",
    "antwort = sm.solve(test, (test1, test2, test3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the street is modelled. The larger rumpel, the more 'uneven' the street will be. The smaller frequenz, the 'flatter' the street.\n",
    "However larger rumple values will increase the force terms. rumpel must be an integer."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#============================================\n",
    "rumpel = 4                 \n",
    "#============================================\n",
    "if isinstance(rumpel, int) == False or rumpel < 1:\n",
    "    raise Exception('rumpel must be an integer larger than zero')\n",
    "    \n",
    "def gesamt(x, z, amplitude, frequenz, rumpel):\n",
    "    strasse = sum([amplitude/j * ((sm.sin(j*frequenz * x) + sm.sin(j*frequenz * z))) for j in range(1, rumpel)])\n",
    "    strassen_form = (frequenz/2. * x)**2  + (frequenz/2. * z)**2\n",
    "    return strasse + strassen_form "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is to determine the maximum radius of the wheel so anywhere on the street it has only one contact point.\n",
    "the radius of the wheel must be smaller than the smallest osculating circle (Schmiegekreis) of the road. \n",
    "Here I calculate this for the X and for the Z direction and simply take the smaller one of the two.\\\n",
    "(I did not bother to try and find the correct formula for higher dimensional functions)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "r_max_xL = (sm.S(1.) + (gesamt(xL, zL, amplitude, frequenz, rumpel).diff(xL))**2 )**sm.S(3/2)/gesamt(xL, zL, amplitude, frequenz, rumpel).diff(xL, 2)\n",
    "r_max_zL = (sm.S(1.) + (gesamt(xL, zL, amplitude, frequenz, rumpel).diff(zL))**2 )**sm.S(3/2)/gesamt(xL, zL, amplitude, frequenz, rumpel).diff(zL, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$CP_L$ and $CP_R$ are the contact points, where the wheels touch the street."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "CPL.set_pos(P0, xL*N.x + gesamt(xL, zL, amplitude, frequenz, rumpel)*N.y + zL*N.z)\n",
    "CPR.set_pos(P0, xR*N.x + gesamt(xR, zR, amplitude, frequenz, rumpel)*N.y + zR*N.z)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**set the positions of** $Dmc_Lm Dmc_R, Po_L, Po_R$\\\n",
    "vektorL is the normal to the streel at point ($x_L, z_L$), the X/Z coordinates of the contact point $CP_L$. \n",
    "I found this formula in the internet.\n",
    "It points 'inwards', hence the leading minus sign.\n",
    "$Dmc_L$ is in the direction of this normal at distance *r* from $CP_L$. The wheel is 'thin', hence this vektor can be perpendicular to AX.x, that is, it has not component in AX.x direction. This is how I choose vektorL1."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "vektorL = -(gesamt(xL, zL, amplitude, frequenz, rumpel).diff(xL)*N.x - N.y + \n",
    "            gesamt(xL, zL, amplitude, frequenz, rumpel).diff(zL)*N.z)\n",
    "vektorL1 = (me.dot(vektorL, AL.y) * AL.y + me.dot(vektorL, AL.z) * AL.z).simplify()\n",
    "DmcL.set_pos(CPL, r * vektorL1.normalize())\n",
    "DmcR.set_pos(DmcL, l * ALh.x)\n",
    "\n",
    "PoL.set_pos(DmcL, r*alpha*AL.z)\n",
    "PoR.set_pos(DmcR, r*alpha*AR.z)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**find the 'speed' of contact point $CP_L$**\n",
    "\n",
    "The instantaneous contact point has no speed, as it is part of the street, too. We need $x(t), z(t)$ to get the location of subsequent contact points. These subsequent contact points will be located at $(x(t), gesamt(x(t), z(t)), z(t))$ .\n",
    "\n",
    "Relationship of x(t) to q(t):\n",
    "Obviously, $ x(t) = function(q(t), gesamt(x(t), z(t)), r) $.\n",
    "When the ball 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 +  \\frac{d}{dx}(f(k(t)))^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 + \\frac{d}{dk}(gesamt(k(t), z(t)))^2)} \\,dk \\ $, differentiated:\n",
    "- $ r \\cdot (-u)  = \\sqrt{(1 + \\frac{d}{dx}(gesamt(x(t), z(t)))^2)} \\cdot \\frac{d}{dt}(x(t)) $, that is solved for $\\frac{d}{dt}(x(t))$:\n",
    "- $ \\frac{d}{dt}(x(t)) = \\dfrac{-(r \\cdot u)} {\\sqrt{(1 + \\frac{d}{dx}(gesamt(x(t), z(t)))^2)})} $\n",
    "\n",
    "A similar formula holds for the speed in Z direction\n",
    "\n",
    "$ \\frac{d}{dt}(x(t)) = \\dfrac{- (u3 \\cdot r)} {\\sqrt{(1 + \\frac{d}{dx}(gesamt(x(t), z(t)))^2)}} $\n",
    "\n",
    "$ \\frac{d}{dt}(z(t)) =  \\dfrac{(u1 \\cdot r)} {\\sqrt{(1 + \\frac{d}{dz}(gesamt(x(t), z(t))))^2)}} $\n",
    "\n",
    "As speeds are vectors, they can be added to give the resultant speed.\n",
    "\n",
    "The + / - signs are a consequence of the 'right hand rule' for frames. These are the sought after first order differential equations for $(x(t), z(t))$.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "OMEGA = AL.ang_vel_in(N)\n",
    "u3_wirk = me.dot(OMEGA, N.z)\n",
    "u1_wirk = me.dot(OMEGA, N.x)\n",
    "rhsx =  -u3_wirk * r / sm.sqrt(1. + (gesamt(xL, zL, amplitude, frequenz, rumpel).diff(xL))**2)\n",
    "rhsz =   u1_wirk * r / sm.sqrt(1. + (gesamt(xL, zL, amplitude, frequenz, rumpel).diff(zL))**2)\n",
    "print('rhsx DS: ', me.find_dynamicsymbols(rhsx))\n",
    "print('rhsz DS: ', me.find_dynamicsymbols(rhsz))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Set the speed of** $Dmc_L, Dmc_R, Po_L, Po_R$\\\n",
    "I use $v_{DmcL}$ = $D_{mcL}$.pos_from($P_0$).diff(t, N) to get the speed.\\\n",
    "*configdt_dict* is a dictionary needed here and also further down. It in turn needs the dictionary *antwort* caclulated above.\\\n",
    "For some reason, not clear to me, *v2pt_theory* did not give the correct results for $Po_L, Po_R$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "configdt_dict = {sm.Derivative(qL, t): antwort[test1], sm.Derivative(q2, t): antwort[test2], \n",
    "    sm.Derivative(q3, t): antwort[test3], sm.Derivative(xL, t): rhsx, sm.Derivative(zL, t): rhsz,\n",
    "    sm.Derivative(xR, t): uxR, sm.Derivative(zR, t): uzR, uxL: rhsx, uzL: rhsz, sm.Derivative(qR, t): uR}\n",
    "\n",
    "DmcL.set_vel(N, DmcL.pos_from(P0).diff(t, N).subs(configdt_dict))\n",
    "DmcR.set_vel(N, DmcR.pos_from(P0).diff(t, N).subs(configdt_dict))\n",
    "\n",
    "CPL.set_vel(N, CPL.pos_from(P0).diff(t, N).subs(configdt_dict))\n",
    "CPR.set_vel(N, CPR.pos_from(P0).diff(t, N).subs(configdt_dict))\n",
    "\n",
    "# for whatever reason v2pt_theory does not give the correct resilts\n",
    "#PoL.v2pt_theory(DmcL, N, AL)\n",
    "#PoR.v2pt_theory(DmcR, N, AR)\n",
    "\n",
    "# This seems to work\n",
    "PoL.set_vel(N, PoL.pos_from(P0).diff(t, N).subs(configdt_dict))\n",
    "PoR.set_vel(N, PoR.pos_from(P0).diff(t, N).subs(configdt_dict))\n",
    "\n",
    "# as per my understanding, this is equivalent to v2py_theory, and also does not work\n",
    "#PoL.set_vel(N, DmcL.pos_from(P0).diff(t, N).subs(configdt_dict) + \n",
    "#             (AL.ang_vel_in(N).cross(PoL.pos_from(DmcL))).subs(configdt_dict))\n",
    "#PoR.set_vel(N, DmcR.pos_from(P0).diff(t, N).subs(configdt_dict) + \n",
    "#             (AR.ang_vel_in(N).cross(PoR.pos_from(DmcR))).subs(configdt_dict))\n",
    "\n",
    "print('v(DmcL) DS viewed in N',me.find_dynamicsymbols(DmcL.vel(N), reference_frame=N))\n",
    "print('v(DmcR) DS viewed in N',me.find_dynamicsymbols(DmcR.vel(N), reference_frame=N), '\\n')\n",
    "print('v(CPL)  DS viewed in N',me.find_dynamicsymbols(CPL.vel(N), reference_frame=N))\n",
    "print('v(CPR)  DS viewed in N',me.find_dynamicsymbols(CPR.vel(N), reference_frame=N), '\\n')\n",
    "print('v(PoL)  DS viewed in N',me.find_dynamicsymbols(PoL.vel(N), reference_frame=N))\n",
    "print('v(PoR)  DS viewed in N',me.find_dynamicsymbols(PoR.vel(N), reference_frame=N))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set up the **configuration constraint for the axle**, get the corresponding **speed constraints**, and solve for $u_3, u_{xR}, u_{zR}$.\\\n",
    "The vector DmcR.pos_from(CPR) must not have a component in AX.x direction. The distance DmcR to CPR must be r.\n",
    "\n",
    "Note: vektorL1 above and vektorR1 below are in the same plane, perpendicular to AX.x\\\n",
    "If in $vektor_{R1}$ the equivalent frame AXh (equivalent for the purpose here) is used, the number of operations in force shoot up."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vektorR =  -(gesamt(xR, zR, amplitude, frequenz, rumpel).diff(xR)*N.x - N.y + \n",
    "             gesamt(xR, zR, amplitude, frequenz, rumpel).diff(zR)*N.z)\n",
    "vektorR1 = (me.dot(vektorR, AL.y) * AL.y + me.dot(vektorR, AL.z) * AL.z).normalize()\n",
    "\n",
    "config_constr = (DmcR.pos_from(CPR) - r * vektorR1)\n",
    "\n",
    "print('config_constr DS', me.find_dynamicsymbols(config_constr, reference_frame=N),'\\n')\n",
    "print('config_constr FS', config_constr.free_symbols(reference_frame=N),'\\n')\n",
    "configX = me.dot(config_constr, N.x)\n",
    "configY = me.dot(config_constr, N.y)\n",
    "configZ = me.dot(config_constr, N.z)\n",
    "\n",
    "if length_config_constr == 1:\n",
    "    config_mag = config_constr.magnitude()\n",
    "elif length_config_constr == 2:\n",
    "    config_mag = me.dot(config_constr, config_constr) \n",
    "else:\n",
    "    config_mag = sm.sqrt(configX**2 + configY**2 + configZ**2)\n",
    "\n",
    "config_constr_matrix = sm.Matrix([configX, configY, configZ])  # needed to find the initial values of q3, xR, zR\n",
    "\n",
    "configXdt = configX.diff(t).subs(configdt_dict)\n",
    "configYdt = configY.diff(t).subs(configdt_dict)\n",
    "configZdt = configZ.diff(t).subs(configdt_dict)\n",
    "\n",
    "print('configXdt DS', me.find_dynamicsymbols(configXdt))\n",
    "print('configXdt contains {} operations'.format(sum([configXdt.count_ops(visual=False) \n",
    "                                                     for i in range(3)])), '\\n')\n",
    "print('configYdt DS', me.find_dynamicsymbols(configYdt))\n",
    "print('configYdt contains {} operations'.format(sum([configYdt.count_ops(visual=False) \n",
    "                                                     for i in range(3)])), '\\n')\n",
    "print('configZdt DS', me.find_dynamicsymbols(configZdt))\n",
    "print('configZdt contains {} operations'.format(sum([configZdt.count_ops(visual=False) \n",
    "                                                     for i in range(3)])), '\\n')\n",
    "\n",
    "# Solve the linear equation as per Jason Moore's idea.\n",
    "speed_mat = sm. Matrix([configXdt, configYdt, configZdt])\n",
    "matrix_A = (speed_mat.jacobian([u3, uxR, uzR]))\n",
    "vektor_b = (speed_mat.subs({i : 0. for i in [u3, uxR, uzR]}))\n",
    "loesung1 = (matrix_A.LUsolve(-vektor_b))\n",
    "\n",
    "print('loesung DS', me.find_dynamicsymbols(loesung1))\n",
    "print('loesung FS', set.union(*[loesung1[i].free_symbols for i in range(3)]))\n",
    "for i in range(len(loesung1)):\n",
    "    print('loesung1 {} contains {} operations'.format(i, loesung1[i].count_ops(visual=False)))\n",
    "loesung1 = [loesung1[0], loesung1[1], loesung1[2]]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**find $u_R$**\n",
    "\n",
    "\n",
    "Obviously, $u_R$ is not independent.\\\n",
    "As $u_R$ is the rotation of the right wheel ($A_R$) around $AXh.x$, in can only be affected by the speed of $CP_R$ in the Z-direction.\\\n",
    "Above I found, now for the right wheel:\n",
    "\n",
    "\n",
    "$ \\frac{d}{dt}(z_R(t)) =  \\dfrac{(u_R \\cdot r)} {\\sqrt{(1 + \\frac{d}{dz_R}(gesamt(x_R(t), z_R(t))))^2)}} $\n",
    "\n",
    "Solved for $u_R$:\n",
    "\n",
    "$u_R$ = $\\dfrac{1.}{r} \\cdot \\frac{d}{dt}(z_R(t)) \\cdot \\sqrt{(1 + \\frac{d}{dz_R}(gesamt(x_R(t), z_R(t))))^2)}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vCPRz = me.dot(CPR.vel(N), ALh.z)\n",
    "\n",
    "bedingung = (1. / r) * vCPRz * sm.sqrt(1. + (gesamt(xR, zR, amplitude, frequenz, rumpel).diff(zR))**2)\n",
    "\n",
    "print('bedingung DS', me.find_dynamicsymbols(bedingung))\n",
    "print('bedingung has {} operations'.format(bedingung.count_ops(visual=False)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One has to solve for all four dependent generalized speeds, that is $u_3, u_{xR}, u_{zR}, u_R$ simultaneously. I will do it numerically later in the integration program. This is needed for the initial conditions. I also use it in the integration, following an idea by JM."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed_matrix = sm.Matrix([loesung1[0] - u3, loesung1[1] - uxR, loesung1[2]- uzR, bedingung - uR])\n",
    "\n",
    "print('speed matrix DS',me.find_dynamicsymbols(speed_matrix))\n",
    "print('speed_matrix has {} operations'.format(sum([speed_matrix[i].count_ops() for i in range(4)])))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The standard machinery to set up **Kane's equation** starts.\n",
    "\n",
    "As the energy of a system may give hints that maybe a mistake was made setting up Kane's equations, e.g. absent any friction or applied forces/torques the total energy must be constant, I like to calculate them and look at them."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "IL = me.inertia(AL, iXX, iYY, iZZ)\n",
    "IR = me.inertia(AR, iXX, iYY, iZZ)\n",
    "\n",
    "BodyL = me.RigidBody('BodyL', DmcL, AL, m, (IL, DmcL))\n",
    "BodyR = me.RigidBody('BodyR', DmcR, AR, m, (IR, DmcR))\n",
    "PoLp  = me.Particle('PoLp', PoL, mo)\n",
    "PoRp  = me.Particle('PoRp', PoR, mo)\n",
    "BODY = [BodyL, BodyR, PoLp, PoRp]\n",
    "\n",
    "punkte = [DmcL, DmcR, PoL, PoR]\n",
    "massen = [m, m, mo, mo]\n",
    "pot_energie = sum([masse*g * me.dot(punkt.pos_from(P0), N.y) for masse, punkt in zip(massen, punkte)])\n",
    "kin_energie = sum([koerper.kinetic_energy(N).subs(configdt_dict) for koerper in BODY]) \n",
    "\n",
    "print('kinetic energy DS:', me.find_dynamicsymbols(kin_energie))\n",
    "print('potential energy DS:', me.find_dynamicsymbols(pot_energie))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set the **external forces** acting on the system.\\\n",
    "Here only the gravitational forces and possibly friction in the bearings of the wheels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "FL1 = [(DmcL, -m*g*N.y), (DmcR, -m*g*N.y), (PoL, -mo*g*N.y), (PoR, -mo*g*N.y)]\n",
    "Torque = [(AL, -reibung * uL*AL.x), (AR, -reibung * uR*AL.x)  ]\n",
    "FL = FL1 + Torque"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is very **important to use AX** when forming the kinematic equations of motion. Using N will increase the number of operations in force substantially. There is a discussion about this somewhere in GitHub, where Timo explains why this is so."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start2 = time.time()\n",
    "kd = [me.dot(rot - rot1, uv) for uv in AL] + [me.dot(rot11 - rot12, ALh.x)] + [\n",
    "    uxR - xR.diff(t), uzR - zR.diff(t)]\n",
    "\n",
    "speed_constraint = [uR - bedingung, u3 - loesung1[0], uxR - loesung1[1], uzR - loesung1[2]]\n",
    "q_ind = [q2, q3, qL, qR] + [xR, zR]\n",
    "u_ind = [u2, uL]\n",
    "u_dep = [u3, uxR, uzR, uR]\n",
    "\n",
    "\n",
    "KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, u_dependent=u_dep, kd_eqs=kd, \n",
    "                    velocity_constraints=speed_constraint)\n",
    "(fr, frstar) = KM.kanes_equations(BODY, FL)\n",
    "\n",
    "MM1 = KM.mass_matrix_full\n",
    "force1 = KM.forcing_full.subs(configdt_dict)\n",
    "\n",
    "if information == True:\n",
    "    print('force1 DS', me.find_dynamicsymbols(force1))\n",
    "    print('force1 free symbols', force1.free_symbols)\n",
    "    print(f'force1 has {sm.count_ops(force1)} operations, {sm.count_ops(sm.cse(force1))} after cse', '\\n')\n",
    "    print(MM1.shape)\n",
    "\n",
    "print('it took {:.3f} sec to establish Kanes equations'.format(time.time() - start2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add $rhsx, rhsz$ at the bottom of force, to get $\\dfrac{d}{dt}(x) = rhsx$, same for $z(t)$. \n",
    "This is to get $x_L(t), z_L(t)$ by numeric integration. \n",
    "  \n",
    "- $ \\dfrac{d}{dt}(x_L(t)) = rhsx $\n",
    "\n",
    "\n",
    "- $ \\dfrac{d}{dt}(z_L(t)) = rhsz $\n",
    "\n",
    "I find it useful to print the dynamic symbols and the free symbols of such functions. This may give hints that something was wrong in describing the system. The number of operations is also useful to know, I think."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "force = sm.Matrix.vstack(force1, sm.Matrix([rhsx, rhsz]))\n",
    "if information == True:\n",
    "    print('force DS', me.find_dynamicsymbols(force))\n",
    "    print('force free symbols', force.free_symbols)\n",
    "    print(f'force has {sm.count_ops(force)} operations, {sm.count_ops(sm.cse(force))} after cse', '\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The mass matrix MM must be enlarged accordingly"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "MM = sm.Matrix.hstack(MM1, sm.zeros(12, 2))\n",
    "hilfs = sm.Matrix.hstack(sm.zeros(2, 12), sm.eye(2))\n",
    "MM = sm.Matrix.vstack(MM, hilfs )\n",
    "if information == True:\n",
    "    print('MM DS', me.find_dynamicsymbols(MM))\n",
    "    print('MM free symbols', MM.free_symbols)\n",
    "    print(f'MM has {sm.count_ops(MM)} operations, {sm.count_ops(sm.cse(MM))} after cse', '\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Convert all the sympy functions to numpy functions, so numerical calculations may be done."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start2 = time.time()\n",
    "pL = [m, mo, g, r, l, iXX, iYY, iZZ, amplitude, frequenz, reibung, alpha]\n",
    "qL1 = q_ind + u_ind + u_dep + [xL, zL]\n",
    "print('vector of variables is:', qL1)\n",
    "MM_lam = sm.lambdify(qL1 + pL, MM, cse=True)\n",
    "force_lam = sm.lambdify(qL1 + pL, force, cse=True)\n",
    "\n",
    "print('it took {:.3f} sec to perform the lambdification'.format(time.time() - start2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As it takes long to lambdify MM and force, I calculate the rest separately."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start2 = time.time()\n",
    "\n",
    "pot_lam = sm.lambdify(qL1 + pL, pot_energie, cse=True)\n",
    "kin_lam = sm.lambdify(qL1 + pL, kin_energie, cse=True)\n",
    "\n",
    "r_max_lam = sm.lambdify([xL, zL] + pL, [r_max_xL, r_max_zL], cse=True)\n",
    "\n",
    "config_lam = sm.lambdify([q3, xR, zR] + [xL, zL, q2, qL] + pL, config_constr_matrix, cse=True)\n",
    "config_mag_lam = sm.lambdify([q2, q3, qL, qR, xR, zR, xL, zL] + pL, config_mag, cse=True)\n",
    "speed_lam = sm.lambdify([u3, uxR, uzR, uR] + q_ind + u_ind + [xL, zL] + pL, speed_matrix, cse=True )\n",
    "speed_lam1 = sm.lambdify(qL1 + pL, speed_matrix, cse=True)\n",
    "\n",
    "print('it took {:.3f} sec to perform the lambdification'.format(time.time() - start2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set the **parameters** and the **independent initial values**\\\n",
    "Their meaning is described above.\\\n",
    "I like to take names here which are similar to the ones used to get Kane\"s equations, but the **same** names **must** be avoided. Otherwise, the symbols will be overwritten, with mostly uninteded consequences.\\\n",
    "If the street is almost flat, that is if $frequenz1 \\approx 0.$ an error ('divide by zero') occurs. I am unsure why."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#================================================================\n",
    "m1 = 1.\n",
    "mo1 = 1.\n",
    "l1 = 4.\n",
    "r1 = 2.\n",
    "\n",
    "amplitude1 = 1.\n",
    "frequenz1 = 0.25\n",
    "reibung1 = 0.\n",
    "alpha1 = 0.99\n",
    "\n",
    "q21 = 0.0\n",
    "qL11, qR1 = 0.0, 0.0\n",
    "xL1, zL1 = -5., 5.\n",
    "u21, uL1 = -2., 2.\n",
    "intervall = 7.5\n",
    "#================================================================\n",
    "schritte = int(intervall * 1000.)\n",
    "\n",
    "iXX1 = 0.5 * m1 * r1**2   # from the internet\n",
    "iYY1 = 0.5 * iXX1         # dto.\n",
    "iZZ1 = 0.5 * iXX1         # dto.\n",
    "\n",
    "print('Arguments:')\n",
    "print('[m, mo, g, r, l, iXX, iYY, iZZ, amplitude, frequenz, reibung, alpha] = ')\n",
    "pL_vals = [m1, mo1, 9.81, r1, l1, iXX1, iYY1, iZZ1, amplitude1, frequenz1, reibung1, alpha1]\n",
    "print(pL_vals)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Find the initial values of the dependent variables and speeds:\n",
    "- $q_3, x_R, z_R$\n",
    "- $u_3, u_{xR}, u_{zR}, u_R$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# find q3, xR, zR numerically, to satisfy the holonomic constraints.\n",
    "def func(x0, args):  # this is only to get the arguments in a way to match fsolve / hol_mat_lam\n",
    "    return config_lam(*x0, *args).reshape(3)\n",
    "\n",
    "args = [xL1, zL1, q21, qL11] + pL_vals\n",
    "x0 = tuple((1., 1., 1.))\n",
    "for _ in range(3):\n",
    "    loesung = root(func, x0, args)\n",
    "    q31, xR1, zR1 = loesung.x\n",
    "    x0 = tuple((q31, xR1, zR1))\n",
    "fehler = config_mag_lam(q21, q31, qL11, qR1, xR1, zR1, xL1, zL1, *pL_vals)\n",
    "print('error in initial configuration constraint: ', fehler)\n",
    "\n",
    "#numerically find u3, uxR, uzR, uR\n",
    "def func1(x0, *args):\n",
    "    return speed_lam(*x0, *args).reshape(4)\n",
    "x0 = tuple((0., 0., 0., 0.))\n",
    "args = tuple([q21, q31, qL11, qR1, xR1, zR1] + [u21, uL1] + [xL1, zL1] + pL_vals)\n",
    "loesung = root(func1, x0, args)\n",
    "u31, uxR1, uzR1, uR1 = loesung.x\n",
    "print('error in initial speed constraints         ', [speed_lam(*[u31, uxR1, uzR1, uR1], \n",
    "        *[q21, q31, qL11, qR1, xR1, zR1], *[u21, uL1], *[xL1, zL1], *pL_vals)[i][0] for i in range(3)], '\\n')\n",
    "\n",
    "y0 = [q21, q31, qL11, qR1, xR1, zR1] + [u21, uL1] + [u31, uxR1, uzR1, uR1] + [xL1, zL1]\n",
    "print('starting vector', [f'{j} = {i:.3f}' for i, j in zip(y0, \n",
    "        ('q21', 'q31', 'qL11', 'qR1', 'xR1', 'zR1', 'u21', 'uL1', 'u31', \n",
    "         'uxR1', 'uzR1', 'uR1', 'xL1', 'zL1'))])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The wheel must touch the street at exactly one point only. The radius of the wheel r must be smaller than the smallest bevel radius.\n",
    "If the smallest bevel curve radius (Schmiegekreis) is smaller than the radius of the wheel, an exception is raised. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if frequenz1 != 0.:\n",
    "    def func2(x, args):\n",
    "# just needed to get the arguments matching for minimuze\n",
    "        return np.abs(r_max_lam(*x, *args)[0])\n",
    "\n",
    "    def func3(x, args):\n",
    "# just needed to get the arguments matching for minimuze\n",
    "        return np.abs(r_max_lam(*x, *args)[1])\n",
    "\n",
    "    x0 = (0.1, 0.1)      # initial guess\n",
    "    minimal1 = minimize(func2, x0, pL_vals)\n",
    "    minimal2 = minimize(func3, x0, pL_vals)\n",
    "\n",
    "    minimal = min(minimal1.get('fun'), minimal2.get('fun'))\n",
    "\n",
    "    if pL_vals[3] < minimal:\n",
    "        print('selected radius = {} is less than minimally admissible radius = {:.4f}, hence o.k.'\n",
    "          .format(pL_vals[3], minimal), '\\n')\n",
    "    else:\n",
    "        print('selected radius {} is larger than admissible radius {:.4f}, hence NOT o.k.'\n",
    "          .format(pL_vals[3], minimal), '\\n')\n",
    "        raise Exception('Radius of wheel is too large')\n",
    "else:\n",
    "    print('the street is flat')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical integration starts.**\n",
    "\n",
    "Following an idea of Jason Moore, I numerically calculate the dependent generalized coordinates and so *force* them to be closer to the *correct* value. I loop over the numerical solution a few times, as this seems to improve the accuracy of the solution. I do the same with the speed constraints, seems to help."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start1 = time.time()\n",
    "times = np.linspace(0, intervall, schritte)\n",
    "x0 = [q31, xR1, zR1]           # initial guesses\n",
    "x01 = [u31, uxR1, uzR1, uR1]   # dto.\n",
    "\n",
    "def gradient(t, y, args):\n",
    "    if time.time() - start1 > 1800:\n",
    "        raise Exception(f'running for {(time.time() - start1):.3f} CPU sec. Hence aborted at {t:.3f} sec')\n",
    "    global x0, x01\n",
    "    args1 = [y[-2], y[-1], y[0], y[2]] + args\n",
    "    for _ in range(2):\n",
    "        loesung = root(func, x0, args1)\n",
    "        y[1], y[4], y[5] = loesung.x\n",
    "        \n",
    "        x0 = [y[1], y[4], y[5]]  # improved guess\n",
    "        \n",
    "        args2 = tuple([y[k] for k in range(8)] + [y[-2], y[-1]] + args)\n",
    "        loesung = root(func1, x01, args2)\n",
    "        y[8], y[9], y[10], y[11] = loesung.x\n",
    "        \n",
    "        x01 = [y[8], y[9], y[10], y[11]]   # improved guess\n",
    "    \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",
    "t_span = (0., intervall)\n",
    "resultat1 = solve_ivp(gradient, t_span, y0, t_eval=times, args=(pL_vals,), atol=1.e-8\n",
    "    , rtol=1.e-8, method='Radau')\n",
    "\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], 'message was:', resultat1.message)\n",
    "    \n",
    "print('resultat shape', resultat.shape, '\\n')\n",
    "\n",
    "print(\"To numerically integrate an intervall of {} sec, the routine cycled {} times and it took {:.3f} sec \"\n",
    "      .format(intervall, resultat1.nfev, time.time() - start1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the **deviations of the configuration constraint** and of the resulting **speed constraints.**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "schritte1 = resultat.shape[0]\n",
    "times1 = [times[i] for i in range(schritte1)]\n",
    "config_np = np.empty(schritte1)\n",
    "zaehler = 0\n",
    "for i in range(schritte1):\n",
    "    config_np[i] = config_mag_lam(*[resultat[i, j] for j in range(6)], resultat[i, -2], \n",
    "        resultat[i, -1], *pL_vals)\n",
    "    if config_np[i] < 0.:\n",
    "        zaehler += 1\n",
    "minimum = np.min(config_np)\n",
    "print('There are {} times, when dot(config, config) < 0. Mimimum value was {}'.format(zaehler, minimum))\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "ax.plot(times1, config_np)\n",
    "ax.set_title('Violation of configuration constraint')\n",
    "label_dict = {1: 'config_constr.magnitude', 2: 'me.dot(config_constr, config_constr)', \n",
    "             3: ' sm.sqrt(configX**2 + configY**2 + configZ**2)'}\n",
    "ax.set_xlabel(\"time (sec)\")\n",
    "ax.set_ylabel('method used to get config_mag is: \\n' + label_dict[length_config_constr])\n",
    "\n",
    "speed_np = np.empty((schritte1, 4))\n",
    "for i in range(schritte1):\n",
    "    speed_np[i] = speed_lam1(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals).reshape(4)\n",
    "    \n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "bezeichnung = ['u3', 'uxR', 'uzR', 'uR']\n",
    "for i in range(4):\n",
    "    ax.plot(times1, speed_np[:, i], label=bezeichnung[i])\n",
    "ax.set_title('violation of speed constraints')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot any generalized coordinates / speeds you want to see."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "bezeichnung = ['q21', 'q31'] + ['qL1', 'qR1'] + ['xR1', 'zR1'] + ['u21', 'uL1'] + ['u31', 'uxR1', 'uzR1', \n",
    "               'uR1'] + ['xL1', 'zL1']\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "for i in (7, 11):      #(resultat.shape[1]):\n",
    "    ax.plot(times[: resultat.shape[0]], resultat[:, i], label = bezeichnung[i])\n",
    "ax.set_title('generalized coordinates')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Plot the energies of the system**\\\n",
    "Total energy must be constant, absent any friction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "pot_np = np.empty(resultat.shape[0])\n",
    "kin_np = np.empty(resultat.shape[0])\n",
    "\n",
    "total_np = np.empty(resultat.shape[0])\n",
    "for k1 in range(resultat.shape[0]):\n",
    "    pot_np[k1]    = pot_lam(*[resultat[k1, j] for j in range(resultat.shape[1])], *pL_vals)\n",
    "    kin_np[k1]    = kin_lam(*[resultat[k1, j] for j in range(resultat.shape[1])], *pL_vals)\n",
    "    total_np[k1] = pot_np[k1] + kin_np[k1]\n",
    "\n",
    "if reibung1 == 0.:\n",
    "    fehler = ((xyz:=max(total_np)) - min(total_np)) / xyz * 100.\n",
    "    print('max deviation from total energy = constant is {:.3f} % of max total energy'.format(fehler))\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "ax.plot(times[: resultat.shape[0]], kin_np, label = 'kin energy')\n",
    "ax.plot(times[: resultat.shape[0]], pot_np, label = 'pot energy')\n",
    "#ax.plot(times[: resultat.shape[0]], spring_np, label = 'spring energy')\n",
    "ax.plot(times[: resultat.shape[0]], total_np, label = 'total energy')\n",
    "ax.set_title('Energy of the system, friction = {}'.format(reibung1))\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.legend();\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This gives the movement of the system, projected on the X / Z horizontal plane. The height above the lowest point of the street is indicated by its color.\\\n",
    "When the axle is not parallel to the X/Z plane, the projection of the discs are ellipses. The axle 'gets shorter'.\\\n",
    "As HTML is so slow, I restrict the points in time to be considered to around *zeitpunkte*."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#================\n",
    "zeitpunkte = 500\n",
    "#================\n",
    "\n",
    "DmcL_pos = [(me.dot(DmcL.pos_from(P0), uv)) for uv in (N.x, N.y, N.z)]\n",
    "DmcR_pos = [(me.dot(DmcR.pos_from(P0), uv)) for uv in (N.x, N.y, N.z)]\n",
    "PoL_pos  = [(me.dot(PoL.pos_from(P0), uv)) for uv in (N.x, N.z)]\n",
    "PoR_pos  = [(me.dot(PoR.pos_from(P0), uv)) for uv in (N.x, N.z)]\n",
    "\n",
    "DmcL_lam = sm.lambdify(qL1 + pL, DmcL_pos, cse=True)\n",
    "DmcR_lam = sm.lambdify(qL1 + pL, DmcR_pos, cse=True)\n",
    "PoL_lam  = sm.lambdify(qL1 + pL, PoL_pos, cse=True)\n",
    "PoR_lam  = sm.lambdify(qL1 + pL, PoR_pos, cse=True)\n",
    "\n",
    "\n",
    "# auxiliary points for the wheels\n",
    "richtung1 = DmcR.pos_from(DmcL).cross(N.y)\n",
    "richtung  = richtung1.normalize()\n",
    "\n",
    "richtungE = (me.dot(richtung1, N.x) * N.x - me.dot(richtung, N.z) * N.z)\n",
    "\n",
    "\n",
    "# angle for the direction of the ellipses.\n",
    "hilfs1 = me.dot(richtung1, N.x)\n",
    "angle1 = sm.atan(me.dot(richtung1, N.z) / hilfs1)\n",
    "angle1_lam = sm.lambdify(qL1 + pL, angle1, cse=True)\n",
    "\n",
    "# calculate the angle the wheels are tilted. winkel actually is sinus(angle), which is needed.\n",
    "komp       = [me.dot(DmcR.pos_from(DmcL), uv) for uv in N]\n",
    "hypothe    = DmcR.pos_from(DmcL).magnitude()\n",
    "winkel     = komp[1] / hypothe\n",
    "winkel_lam = sm.lambdify(qL1 + pL, winkel, cse=True)\n",
    "\n",
    "resultat2 = []\n",
    "times2    = []\n",
    "reduction = int(resultat.shape[0]/zeitpunkte)\n",
    "for l11 in range(resultat.shape[0]):\n",
    "    if l11 % reduction == 0:\n",
    "        resultat2.append(resultat[l11])\n",
    "        times2.append(times[l11])\n",
    "schritte2  = len(times2)\n",
    "resultat2  = np.array(resultat2)\n",
    "times2     = np.array(times2)\n",
    "print(f'number of time points considered is {schritte2}')\n",
    "\n",
    "DmcL_np = np.empty((schritte2, 3))\n",
    "DmcR_np = np.empty((schritte2, 3))\n",
    "PoL_np  = np.empty((schritte2, 2))\n",
    "PoR_np  = np.empty((schritte2, 2))\n",
    "\n",
    "winkel_np = np.empty(schritte2)\n",
    "angle1_np = np.empty(schritte2)\n",
    "\n",
    "for l11 in range(schritte2):\n",
    "    DmcL_np[l11]     = DmcL_lam(*[resultat2[l11, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "    DmcR_np[l11]     = DmcR_lam(*[resultat2[l11, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "    PoL_np[l11]      =  PoL_lam(*[resultat2[l11, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "    PoR_np[l11]      =  PoR_lam(*[resultat2[l11, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "        \n",
    "    winkel_np[l11] = winkel_lam(*[resultat2[l11, j] for j in range(resultat2.shape[1])], *pL_vals)\n",
    "    angle1_np[l11] = np.rad2deg(angle1_lam(*[resultat2[l11, j] for j in range(resultat2.shape[1])], \n",
    "       *pL_vals))\n",
    "    \n",
    "# get the size of the picture\n",
    "xmin = min(np.min(DmcL_np[:, 0]), np.min(DmcR_np[:, 0]), np.min(PoL_np[:, 0]), np.min(PoR_np[:, 0]), \n",
    "           np.min(PoL_np[:, 0]), np.min(PoR_np[:, 0]))    \n",
    "xmax = max(np.max(DmcL_np[:, 0]), np.max(DmcR_np[:, 0]), np.max(PoL_np[:, 0]), np.max(PoR_np[:, 0]), \n",
    "           np.max(PoL_np[:, 0]), np.max(PoR_np[:, 0]))\n",
    "\n",
    "ymin = min(np.min(DmcL_np[:, 2]), np.min(DmcR_np[:, 2]), np.min(PoL_np[:, 1]), np.min(PoR_np[:, 1]), \n",
    "           np.min(PoL_np[:, 1]), np.min(PoR_np[:, 1]))    \n",
    "ymax = max(np.max(DmcL_np[:, 2]), np.max(DmcR_np[:, 2]), np.max(PoL_np[:, 1]), np.max(PoR_np[:, 1]), \n",
    "           np.max(PoL_np[:, 1]), np.max(PoR_np[:, 1]))\n",
    "\n",
    "maximum = max(np.abs(xmin), np.abs(xmax), np.abs(ymin), np.abs(ymax))\n",
    "\n",
    "DmcLF = np.empty(schritte2, dtype=int)\n",
    "DmcRF = np.empty(schritte2, dtype=int)\n",
    "\n",
    "# the color mappings below require integers. As the height variation of the mass centers of the wheels\n",
    "# is not much, I multiply by a factor of 500 to get a decent number of different colors.\n",
    "faktor = 1000.\n",
    "for i in range(schritte2):\n",
    "    DmcLF[i] = int(DmcL_np[i, 1] * faktor)\n",
    "    DmcRF[i] = int(DmcR_np[i, 1] * faktor)\n",
    "\n",
    "farbe_max = max(np.max(DmcLF), np.max(DmcRF))\n",
    "farbe_min = min(np.min(DmcLF), np.min(DmcRF))\n",
    "\n",
    "Test = mp.colors.Normalize(farbe_min, farbe_max)\n",
    "Farbe = mp.cm.ScalarMappable(Test, cmap='plasma')\n",
    "farbeL1 = Farbe.to_rgba(DmcLF[0])    # color of the starting position of left center of wheel\n",
    "farbeR1 = Farbe.to_rgba(DmcRF[0])    # color of the starting position of left center of wheel\n",
    "\n",
    "def animate_pendulum(times, DmcL_np, DmcR_np, PoL_np, PoR_np, DmcLF, DmcRF, winkel_np):\n",
    "    \n",
    "    fig, ax = plt.subplots(figsize=(8, 8), subplot_kw={'aspect': 'equal'})\n",
    "    fig.colorbar(Farbe, label='Height of the ball above ground level\\n times {}'.format(faktor), \n",
    "                 shrink=0.9, ax=ax)\n",
    "    ax.axis('on')\n",
    "    ax.set_xlim(-maximum - 1., maximum + 1.)\n",
    "    ax.set_ylim(-maximum - 1., maximum + 1.)\n",
    "    ax.set_xlabel(' X Axis', fontsize=15)\n",
    "    ax.set_ylabel('Z Axis', fontsize=15)\n",
    "\n",
    "    line1, = ax.plot([], [], 'o', markersize=1, color=farbeL1)      # DmcL\n",
    "    line1a,= ax.plot([], [], linestyle='-', color='red', linewidth=0.5) # trace DmcL\n",
    "    line2, = ax.plot([], [], 'o', markersize=1, color=farbeR1)      # DmcR\n",
    "    line2a,= ax.plot([], [], linestyle='-', color='blue', linewidth=0.5)  # trace DmcR \n",
    "    line3, = ax.plot([], [], 'o', markersize=5, color='red')         # PoL\n",
    "    line4, = ax.plot([], [], 'o', markersize=5, color='green')       # PoR\n",
    "    line7, = ax.plot([], [], 'bo', markersize=1, linestyle='-')      # axle\n",
    "    \n",
    "    elliL = Ellipse((DmcL_np[0, 0], DmcL_np[0, 2]), width=2.*r1, height=2.*winkel_np[0]*r1, \n",
    "            angle=angle1_np[0], fill=True, color=farbeL1, ec=farbeL1, alpha=0.5, linewidth=3)\n",
    "    ax.add_patch(elliL)\n",
    "    \n",
    "    elliR = Ellipse((DmcR_np[0, 0], DmcR_np[0, 2]), width=2.*r1, height=2.*winkel_np[0]*r1, \n",
    "            angle=angle1_np[0], fill=True, color=farbeR1, ec=farbeR1,alpha=0.5, linewidth=3)\n",
    "    ax.add_patch(elliR)\n",
    "    \n",
    "    \n",
    "    def animate(i):\n",
    "        farbeLi = Farbe.to_rgba(DmcLF[i])                            # color of the actual point at time i\n",
    "        farbeRi = Farbe.to_rgba(DmcRF[i])                            # color of the actual point at time i\n",
    "        ax.set_title('running time {:.1f} sec'.format(times[i]), fontsize=15)\n",
    "        line1.set_data(DmcL_np[i, 0], DmcL_np[i, 2])\n",
    "        line1.set_color(farbeLi)\n",
    "        line1a.set_data(DmcL_np[: i, 0], DmcL_np[: i, 2])\n",
    "        line2.set_data(DmcR_np[i, 0], DmcR_np[i, 2])\n",
    "        line2.set_color(farbeRi)\n",
    "        line2a.set_data(DmcR_np[: i, 0], DmcR_np[: i, 2])\n",
    "        line3.set_data(PoL_np[i, 0], PoL_np[i, 1])\n",
    "        line4.set_data(PoR_np[i, 0], PoR_np[i, 1])\n",
    "        \n",
    "        x_werte = [DmcL_np[i, 0], DmcR_np[i, 0]]\n",
    "        y_werte = [DmcL_np[i, 2], DmcR_np[i, 2]]\n",
    "        line7.set_data(x_werte, y_werte)\n",
    "        \n",
    "        elliL.set_center((DmcL_np[i, 0], DmcL_np[i, 2]))\n",
    "        elliL.set_height(2.*winkel_np[i]*r1)\n",
    "        elliL.set_angle(angle1_np[i])\n",
    "        elliL.set_color(farbeLi)\n",
    "        \n",
    "        elliR.set_center((DmcR_np[i, 0], DmcR_np[i, 2]))\n",
    "        elliR.set_height(2.*winkel_np[i]*r1)\n",
    "        elliR.set_angle(angle1_np[i])\n",
    "        elliR.set_color(farbeRi)\n",
    "        \n",
    "        return line1, line1a, line2, line2a, line3, line4, line7, elliL, elliR,\n",
    "\n",
    "    anim = animation.FuncAnimation(fig, animate, frames=len(times2),\n",
    "                                   interval=1000*max(times2) / len(times2),\n",
    "                                   blit=True)\n",
    "    plt.close(fig)\n",
    "    return anim\n",
    "\n",
    "anim = animate_pendulum(times2, DmcL_np, DmcR_np, PoL_np, PoR_np, DmcLF, DmcRF, winkel_np)\n",
    "\n",
    "HTML(anim.to_jshtml())    "
   ]
  },
  {
   "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.0"
  },
  "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
}