chain-link-closed.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\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib as mp\n",
    "from matplotlib import patches\n",
    "%matplotlib inline\n",
    "from IPython.display import HTML\n",
    "mp.rcParams['animation.embed_limit'] = 2**128\n",
    "from matplotlib import animation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**A chain**\n",
    "\n",
    "I want to simulate a hanging chain, which I model as a simple closed 2D n-link pendulum, where each link is modelled a a thin rod.\\\n",
    "The two ends of the chain are fixed at $(0., 0.)$ and at $(EP_x, EP_y)$.\\\n",
    "I saw this some time ago as an example of some other modelling library, and I want to see, how this will work with sympy mechanics.\\\n",
    "In the animation, I also draw the **catenary** corresponding to the chain I model. With friction, the modelled chain should end close to this catenary.\n",
    "\n",
    "\n",
    "I noticed the following:\n",
    "- If I define the rotation of frame $A[i]$ relative to frame $A[i-1]$ the number of operations shoot up: for $n = 12$ the **mass matrix has 25 mio operations**. If I define the rotation of $A[i]$ relative to the inertial frame $O$, the numbers become very reasonable. \n",
    "\n",
    "**Symbols**\n",
    "\n",
    "- $O$: Reference frame\n",
    "- $PO$: point fixed in $O$\n",
    "- $A[i]$: body fixed frame of link $i$ with $0 \\leq n < n$\n",
    "- $Dmc[i]$: center of gravitiy of link $i$\n",
    "- $P[i]$: point, where frame $A[i]$ joins frame $A[i+1]$\n",
    "- $l$: length of the pendulum, that is each link has length = $\\dfrac{l}{n}$\n",
    "\n",
    "- $m$: mass of each link\n",
    "- $iZZ$: moment of inertial of each link around $A[i].z$, relative to $Dmc[i]$\n",
    "- $reibung$: speed dependent friction in each joint.\n",
    "- $auxx, auxy, fx, fy$: virtual speeds and reaction forces on point $O$\n",
    "- $rhs$: 'place holders' for the $rhs = MM^{-1} \\cdot force$ to be evaluated numerically later\n",
    "\n",
    "\n",
    "- $q[i]$: generalized coordinate of frame $A[i]$ relative to the inertial frame $O$.\n",
    "- $u[i]$: angular speed dto.\n",
    "\n",
    "\n",
    "- $EP$: end point of the chain, fixed in place. That is $P[n-1]$ should be at $EP$.\n",
    "- $EP_x, EP_y$: its coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#==================\n",
    "n = 30\n",
    "term_info = True\n",
    "#==================\n",
    "start = time.time()\n",
    "\n",
    "m, g, iZZ, l, reibung = sm.symbols('m, g, iZZ, l, reibung')\n",
    "q   = me.dynamicsymbols(f'q:{n}')\n",
    "u   = me.dynamicsymbols(f'u:{n}')\n",
    "\n",
    "auxx, auxy, fx, fy = me.dynamicsymbols('auxx, auxy, fx, fy')\n",
    "t = me.dynamicsymbols._t\n",
    "\n",
    "A   = sm.symbols(f'A:{n}', cls=me.ReferenceFrame)\n",
    "Dmc = sm.symbols(f'Dmc:{n}', cls=me.Point)\n",
    "P   = sm.symbols(f'P:{n}', cls=me.Point)\n",
    "rhs = list(sm.symbols(f'rhs:{n}'))\n",
    "\n",
    "O   = me.ReferenceFrame('O')\n",
    "PO  = me.Point('PO')\n",
    "PO.set_vel(O, auxx*O.x + auxy*O.y)\n",
    "\n",
    "l1 = l/n\n",
    "l2 = l/(2 * n)\n",
    "\n",
    "A[0].orient_axis(O, q[0], O.z)\n",
    "A[0].set_ang_vel(O, u[0] * O.z)\n",
    "\n",
    "Dmc[0].set_pos(PO, l2*A[0].x)\n",
    "Dmc[0].v2pt_theory(PO, O, A[0])\n",
    "P[0].set_pos(PO, l1*A[0].x)\n",
    "P[0].v2pt_theory(PO, O, A[0])\n",
    "\n",
    "for i in range(1, n):\n",
    "    A[i].orient_axis(O, q[i], O.z)         #(A[i-1], q[i], A[i-1].z)\n",
    "    A[i].set_ang_vel(O, u[i] * O.z)    #(A[i-1], u[i] * A[i-1].z)\n",
    "\n",
    "    Dmc[i].set_pos(P[i-1], l2*A[i].x)\n",
    "    Dmc[i].v2pt_theory(P[i-1], O, A[i])\n",
    "    P[i].set_pos(P[i-1], l1*A[i].x)\n",
    "    P[i].v2pt_theory(P[i-1], O, A[i])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "*Find the configuration constraint and derive the speed constraints from it*\\\n",
    "The last point $P[n-1]$ is required to be fixed at $(EP_x / EP_y)$. I do it exactly like JM does it in his lecture.\\\n",
    "*laenge* is used to check how well the speed constraints are fulfilled. If I use *.magnitude()* I run into numerical issues sometimes. This was discussed on Github a while ago. The way I do it avoids these issues.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "EP = me.Point('EP')\n",
    "EPx, EPy = sm.symbols('EPx, EPy')\n",
    "\n",
    "EP_pos     = EP.set_pos(PO, EPx*O.x + EPy*O.y)\n",
    "EP_pos     = EP.pos_from(PO)\n",
    "Pn_pos     = P[0].pos_from(PO) + sum([P[i+1].pos_from(P[i]) for i in range(n-1)])\n",
    "\n",
    "constraint = EP_pos - Pn_pos\n",
    "\n",
    "constraintX       = me.dot(constraint, O.x)\n",
    "constraintY       = me.dot(constraint, O.y)\n",
    "constraint_matrix = sm.Matrix([constraintX, constraintY])\n",
    "laenge            = sm.sqrt(constraintX**2 + constraintY**2) # using magnitude() here gives errors at time\n",
    "\n",
    "constraint_dict     = {sm.Derivative(q[i], t): u[i] for i in range(n)}\n",
    "constraintXdt       = constraintX.diff(t).subs(constraint_dict)\n",
    "constraintYdt       = constraintY.diff(t).subs(constraint_dict)\n",
    "constraintdt_matrix = sm.Matrix([constraintXdt, constraintYdt])\n",
    "\n",
    "matrix_A = constraintdt_matrix.jacobian((u[n-2], u[n-1]))\n",
    "vector_b = constraintdt_matrix.subs({u[n-2]: 0., u[n-1]: 0.})\n",
    "loesung  = matrix_A.LUsolve(-vector_b)\n",
    "\n",
    "if term_info == True:\n",
    "    print('loesung DS', me.find_dynamicsymbols(loesung))\n",
    "    print('loesung FS', loesung.free_symbols)\n",
    "    print('loesung has {} operations'.format(sum([loesung[i].count_ops(visual=False) for i in (0, 1)])))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Kane's equations**\n",
    "\n",
    "nothing special here."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "BODY = []\n",
    "for i in range(n):\n",
    "    inertia = me.inertia(A[i], 0., 0., iZZ)\n",
    "    body    = me.RigidBody('body' + str(i), Dmc[i], A[i], m, (inertia, Dmc[i]))\n",
    "    BODY.append(body)\n",
    "\n",
    "FL1    = [(Dmc[i], -m*g*O.y) for i in range(n)] + [(PO, fx*O.x + fy*O.y)]\n",
    "Torque = [(A[i], - u[i] * reibung * A[i].z) for i in range(n)]\n",
    "FL     = FL1 + Torque\n",
    "\n",
    "kd     = [u[i] - q[i].diff(t) for i in range(n)]\n",
    "aux    = [auxx, auxy]\n",
    "speed_constr = [u[n-2] - loesung[0], u[n-1] - loesung[1]]\n",
    "\n",
    "u_ind  = [u[i] for i in range(n-2)]\n",
    "u_dep  = [u[n-2], u[n-1]]\n",
    "\n",
    "KM = me.KanesMethod(O, q_ind=q, u_ind=u_ind, u_dependent=u_dep, kd_eqs=kd, u_auxiliary=aux, \n",
    "            velocity_constraints=speed_constr)\n",
    "(fr, frstar) = KM.kanes_equations(BODY, FL)\n",
    "\n",
    "MM = KM.mass_matrix_full\n",
    "if term_info == True:\n",
    "    print('MM DS', me.find_dynamicsymbols(MM))\n",
    "    print('MM free symbols', MM.free_symbols)\n",
    "    print('MM contains {} 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",
    "force = KM.forcing_full.subs({fx: 0., fy: 0.})\n",
    "if term_info == True:\n",
    "    print('force DS', me.find_dynamicsymbols(force))\n",
    "    print('force free symbols', force.free_symbols)\n",
    "    print('force contains {} operations'.format(sum([force[i].count_ops(visual=False) for i in range(force.shape[0])])), '\\n')\n",
    "\n",
    "eingepraegt_dict = {sm.Derivative(i, t): j for i, j in zip(u, rhs)} \n",
    "eingepraegt      = KM.auxiliary_eqs.subs(eingepraegt_dict)\n",
    "if term_info == True:\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",
    "print(f'it took {time.time() - start:.3f} sec to set up Kanes equations for {n} links')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Functions for the kinetic and the potential energies.\\\n",
    "Always useful to detect mistakes.\\\n",
    "orte is needed for the animation only."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "kin_energie = sum([koerper.kinetic_energy(O) for koerper in BODY]).subs({i: 0. for i in aux})\n",
    "pot_energie = sum([m*g*me.dot(koerper.pos_from(PO), O.y) for koerper in Dmc])\n",
    "        \n",
    "orte        = [[me.dot(P[i].pos_from(PO), uv) for uv in (O.x, O.y)] for i in range(n) ]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lambdification."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start1 = time.time()\n",
    "\n",
    "qL  = q + u_ind + u_dep\n",
    "qL1 = q + u_ind\n",
    "qL2 = [q[i] for i in range(n-2)]\n",
    "pL = [m, g, l, iZZ, reibung, EPx, EPy]\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",
    "eingepraegt_lam = sm.lambdify(F + qL + pL + rhs, eingepraegt, cse=True)\n",
    "\n",
    "kin_lam    = sm.lambdify(qL + pL, kin_energie, cse=True)\n",
    "pot_lam    = sm.lambdify(qL + pL, pot_energie, cse=True)\n",
    "\n",
    "ort_lam = sm.lambdify(qL + pL, orte, cse=True)\n",
    "\n",
    "loesung_lam = sm.lambdify(qL1 + pL, loesung, cse=True)\n",
    "constraint_lam = sm.lambdify([q[n-2], q[n-1]] + qL2 + pL, constraint_matrix, cse=True)\n",
    "laenge_lam     = sm.lambdify(q + pL, laenge, cse=True)\n",
    "constraintdt_lam = sm.lambdify(qL + pL, constraintdt_matrix, cse=True)\n",
    "\n",
    "print(f'it took {time.time()-start1:.3f} sec to do the lambdification')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical Integration**\n",
    "\n",
    "A.\\\n",
    "Define the initial values and the parameters.\\\n",
    "For more than a few links, it is difficult to get initial generalized coordinates $q[i]$ which fit the configuration constraint: if I just 'randomly set $q[0]....q[n-3]$ there may not be a solution for the dependent generalized coordinates $q[n-2], q[n-1]$.\\\n",
    "So I get approximate generalized coordinates by numerically solving $\\min \\limits_{q[0]...q[n-1]} (| config. constraint |)$ for $q[0]....q[n-1]$. *method = 'Nelder-Meat'* in *minimize(..)* finds good minimal values even for larger n, while *no method* even failed with n = 10, depending on where the fixed point of $P[n-1]$ was located. There are many feasible solutions, the one picked likely depends on the initial conditions given to *minimize(..)*, but I did not study this.\\\n",
    "The values produced by *minimize(..)* serve as ininital conditions to numerically solve for $q[n-2], q[n-1]$\\\n",
    "Finally I calculate how well the initial generalized coordinates and speeds fulfill the configuaration constraint and the resulting speed constraints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "#==========================\n",
    "# Input variables\n",
    "#==========================\n",
    "m1       = 1.\n",
    "g1       = 9.8\n",
    "l1       = 30.\n",
    "reibung1 = 2.\n",
    "\n",
    "q1      = [np.random.choice((1, -1)) for _ in range(n)] # just starting guess for below.\n",
    "u1      = [0. for _ in range(n)] # setting the angular velocities != 0 ,increases the integration time a lot.\n",
    "\n",
    "EPx1    =  10.  # Must be > 0, else the catenary will not work.\n",
    "EPy1    =   3.\n",
    "\n",
    "intervall = 20.\n",
    "punkte    = 25\n",
    "#==========================\n",
    "\n",
    "schritte = int(intervall * punkte)\n",
    "times    = np.linspace(0., intervall, schritte)\n",
    "t_span   = (0., intervall)\n",
    "\n",
    "iZZ1 = 1./12. * m1 * (l1/n)**2        # from the internet\n",
    "\n",
    "pL_vals = [m1, g1, l1, iZZ1, reibung1, EPx1, EPy1]\n",
    "\n",
    "if np.sqrt(EPx1**2 + EPy1**2) > l1:\n",
    "    raise Exception('endpoint to far away from (0/0)')\n",
    "\n",
    "# find generalized coordinates to fullfill the configuration constraints\n",
    "def func1(x0, args):\n",
    "    return laenge_lam(*x0, *args)\n",
    "X0 = tuple((q1))\n",
    "X0 = tuple(([1.] * n))\n",
    "for _ in range(6):\n",
    "    t0 = minimize(func1, X0, pL_vals, method='Nelder-Mead')\n",
    "    X0 = t0.x\n",
    "\n",
    "print(f'error of initial guess is  {laenge_lam(*X0, *pL_vals):.3e} achieved by minimizing')\n",
    "      \n",
    "def func2(X0, args):\n",
    "    return constraint_lam(*X0, *args).reshape(2)\n",
    "args = [X0[i] for i in range(n-2)] + pL_vals\n",
    "Z0   = (X0[n-2], X0[n-1])\n",
    "for _ in range(6):\n",
    "    t0 = fsolve(func2, Z0, args)\n",
    "    Z0 = t0\n",
    "    Q0 = [Z0[0], Z0[1]] + [X0[i] for i in range(n-2)]\n",
    "print(f'error of improved guess is {laenge_lam(*Q0, *pL_vals):.3e}'\n",
    "     + ' achieved by nuerically solve for the dependent coordinates', '\\n')\n",
    "\n",
    "# find the dependent speeds to match the independet speeds\n",
    "U0 = [u1[i] for i in range(n-2)]\n",
    "t0 = loesung_lam(*Q0, *U0, *pL_vals)\n",
    "u1[n-2] = t0[0][0]\n",
    "u1[n-1] = t0[1][0]\n",
    "\n",
    "y0 = Q0 + U0 + [u1[n-2], u1[n-1]]\n",
    "\n",
    "print(f'error in the initial speed constraints are X: {constraintdt_lam(*y0, *pL_vals)[0][0]:.3e} /' +\n",
    "     f' Y: {constraintdt_lam(*y0, *pL_vals)[0][0]:.3e}')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "B.\\\n",
    "Numerical integration.\\\n",
    "method = $Radau$ seems to keep the total energy closer to constant, absent any friction, that is $reibung_1 = 0$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start2 = time.time()\n",
    "\n",
    "y0 = Q0 + U0 + [u1[n-2], u1[n-1]]\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",
    "resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,), method='Radau'\n",
    "                , atol=1.e-5, rtol=1.e-5)\n",
    "\n",
    "resultat = resultat1.y.T\n",
    "print('resultat shape', resultat.shape, '\\n')\n",
    "event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}\n",
    "print(event_dict[resultat1.status], ', message is:', resultat1.message, '\\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() - start2), '\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the **energies**."
   ]
  },
  {
   "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",
    "\n",
    "if reibung1 == 0.:\n",
    "    max_total = np.max(np.abs(total_np))\n",
    "    min_total = np.min(np.abs(total_np))\n",
    "    delta = max_total - min_total\n",
    "    print(f'max deviation of total energy from zero is {delta/max_total * 100:.3e} % of max. total energy')\n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "for i, j in zip((kin_np, pot_np, total_np), ('kinetic energy', 'potential energy', \n",
    "                'total energy')):\n",
    "    ax.plot(times, i, label=j)\n",
    "ax.set_title(f'Energies of the system, friction = {reibung1}')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel('energy (Nm)')\n",
    "ax.legend();\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check, how well the **configuration constraint** and the **speed constraints** are kept\\\n",
    "Ideally, of course, they should be zero."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "X_np = np.empty(schritte)\n",
    "Y_np = np.empty(schritte)\n",
    "\n",
    "for i in range(schritte):\n",
    "    X_np[i], Y_np[i] = constraint_lam(resultat[i, n-2], resultat[i, n-1],\n",
    "        *[resultat[i, j] for j in range(n-2)], *pL_vals)\n",
    "    \n",
    "fig, ax = plt.subplots(figsize=(10,5)) \n",
    "for i, j in zip((X_np, Y_np), ('X direction', 'Y-direction')):\n",
    "    ax.plot(times, i, label=j)\n",
    "ax.set_title('violation of configuration constraint')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel('meter')\n",
    "ax.legend();\n",
    "\n",
    "for i in range(schritte):\n",
    "    X_np[i], Y_np[i] = constraintdt_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((X_np, Y_np), ('u[n-2]', 'u[n-1]')):\n",
    "    ax.plot(times, i, label=j)\n",
    "ax.set_title('violation of speed constraints')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel('rad / sec')\n",
    "ax.legend();\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Reaction force on $PO$**\n",
    "\n",
    "First I solve the equation $rhs = MM^{-1} \\cdot force$ numerically.\\\n",
    "Then I solve $eingepraegt = 0$ for $fx, fy$ numerically.\\\n",
    "I iterate over fsolve twice, this sometimes improves the accuracy.\\\n",
    "I iterate over the whole process twice: Sometimes there is a message that convergence is slow. If I run it again, usually this message does not come again. I do not know, why.\n"
   ]
  },
  {
   "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",
    "\n",
    "# this is only needed because I basically copied it from some other program, where resultat2, etc \n",
    "# was different from resultat.\n",
    "resultat2 = resultat\n",
    "schritte2 = schritte\n",
    "times2    = times\n",
    "\n",
    "RHS1 = np.empty((schritte2, resultat2.shape[1]))\n",
    "for i in range(schritte2):\n",
    "    RHS1[i, :] = np.linalg.solve(MM_lam(*[resultat2[i, j]for j in range(resultat2.shape[1])], *pL_vals), \n",
    "        force_lam(*[resultat2[i, j] for j in range(resultat2.shape[1])], \n",
    "        *pL_vals)).reshape(resultat2.shape[1])\n",
    "print('RHS1 shape', RHS1.shape)\n",
    "\n",
    "#calculate implied forces numerically\n",
    "def func (x, *args):\n",
    "# just serves to 'modify' the arguments for fsolve.\n",
    "    return eingepraegt_lam(*x, *args).reshape(2)\n",
    "\n",
    "for _ in range(2):\n",
    "    kraftx  = np.empty(schritte2)\n",
    "    krafty  = np.empty(schritte2)\n",
    "    \n",
    "    x0 = tuple((1., 1.))   # initial guess\n",
    "\n",
    "    for i in range(schritte2):\n",
    "        for _ in range(2):\n",
    "            y0 = [resultat2[i, j] for j in range(resultat2.shape[1])]\n",
    "            rhs = [RHS1[i, j] for j in range(n, 2*n)]\n",
    "            args = tuple(y0 + pL_vals + rhs)\n",
    "            AAA = fsolve(func, x0, args=args).reshape(2)\n",
    "            x0 = tuple(AAA)      # improved guess. Should speed up convergence of fsolve\n",
    "        kraftx[i]  = AAA[0]\n",
    "        krafty[i]  = AAA[1]\n",
    "      \n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "for i, j in zip((kraftx, krafty), ('reaction force on PO in X direction', \n",
    "    'reaction force on PO in Y direction', 'reaction force on P[n-1] in X direction')):\n",
    "\n",
    "    plt.plot(times2, i, label=j)\n",
    "ax.set_title('Reaction Forces on suspension point PO')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel('force (N)')\n",
    "plt.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Catenary**\n",
    "\n",
    "The shape of a chain of uniform linear density hanging symmetric to the vertical Y axis is described by\\\n",
    "$y(x) = factor \\cdot \\cosh(\\dfrac{x}{factor})$ $\\space$ From the internet\\\n",
    "A chain suspended at $(x_1, y_1)$ and $(x_2, y_2)$ and length $l$ has this transcendental equation for $factor$\n",
    "- $v = y_2 - y_1$\n",
    "- $h = x_2 - x_1$\n",
    "- $l$: length of the chain\n",
    "\n",
    "\n",
    "$\\sqrt{l^2 - v^2} = 2 \\cdot factor \\cdot \\sinh(\\dfrac{h}{2. \\cdot factor})$ $\\space$ From the internet :\\\n",
    "https://math.stackexchange.com/questions/1000447/finding-the-catenary-curve-with-given-arclength-through-two-given-points\n",
    "\n",
    "I numerically solve this equation for *factor*, called $t0$ in my program\\\n",
    "If I put this $factor$ in the equation for the suspended chain, I get the graph of a symmetrically hanging chain.\\\n",
    "Next I calculate the value of *delta*, called $d0$ in my program, where the chain is at $EP_y$.\\\n",
    "Then I have to appropriately shift this symmetric chain so it matches the chain I simulate.\\\n",
    "If I use friction, that is $reibung_1 > 0$, the chain should approach this catenary.\n",
    "\n",
    "It seems that if the catenary becomes 'extreme' in some way, the equation to solve for *factor* does not converge well. The longer the chain, the better the result seems to be.\n",
    "\n",
    "I iterate from $EP_y = 0$, where it always seems to calculate correctly, to $EP_y$, not sure if this helps much.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "faktor, xx, yy, delta = sm.symbols('faktor, xx, yy, delta')\n",
    "catenary     = 2. * faktor * sm.sinh(EPx / (2. * faktor)) - sm.sqrt(l**2 - EPy**2)\n",
    "catenary_lam = sm.lambdify([faktor, EPx, EPy, l], catenary, cse=True)\n",
    "\n",
    "def func3(X0, args):\n",
    "    return catenary_lam(*X0, *args)\n",
    "\n",
    "\n",
    "catenaryY = faktor * sm.cosh((xx)/(faktor))\n",
    "catenaryY_lam = sm.lambdify([xx, faktor, EPx], catenaryY, cse=True)\n",
    "\n",
    "catenaryZ = faktor * sm.cosh((xx + delta)/(faktor))\n",
    "catenaryZ_lam = sm.lambdify([delta, xx, faktor], catenaryZ, cse=True)\n",
    "\n",
    "def func4(X0):\n",
    "    return catenaryZ_lam(X0, EPx1/2., t0) - catenaryZ_lam(0., -EPx1/2, t0 ) - EPy2\n",
    "\n",
    "Delta = []\n",
    "Factor = []\n",
    "zahl = 20\n",
    "EPy2 = 0.\n",
    "X01 = 0.1\n",
    "z01 = 0.1\n",
    "delta = EPy1/zahl\n",
    "for _ in range(zahl):\n",
    "    EPy2 += delta\n",
    "    args = [EPx1, EPy2, l1]\n",
    "    t0   = fsolve(func3, X01, args)\n",
    "    t0   = t0[0]\n",
    "    X01  = t0\n",
    "    Factor.append(t0)\n",
    "\n",
    "    d0 = fsolve(func4, z01)\n",
    "    z01 = d0[0] \n",
    "    Delta.append(z01)\n",
    "\n",
    "# calculate the values of the catenary to be used below in the animation\n",
    "ergebnis = []\n",
    "for xx1 in np.linspace(0., EPx1, 100):\n",
    "    ergebnis.append(catenaryY_lam(xx1-(EPx1-d0)/2., t0, EPx1))\n",
    "abzug = ergebnis[0]\n",
    "for i in range(100):\n",
    "    ergebnis[i] = ergebnis[i] - abzug"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Animation**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# get Carthesian coordinates\n",
    "x_coords = np.empty((len(times), n+1))\n",
    "y_coords = np.empty((len(times), n+1))\n",
    "\n",
    "for j in range(schritte):\n",
    "    x_coords[j] = [0.] + [ort_lam(*[resultat[j, k] for k in range(resultat.shape[1])], *pL_vals)[k11][0] \n",
    "                         for k11 in range(n) ]\n",
    "    y_coords[j] = [0.] + [ort_lam(*[resultat[j, k] for k in range(resultat.shape[1])], *pL_vals)[k11][1] \n",
    "                         for k11 in range(n)]\n",
    "\n",
    "max_x = max([abs(x_coords[i, j])for i in range(schritte) for j in range(n+1)])\n",
    "max_y = max([abs(y_coords[i, j])for i in range(schritte) for j in range(n+1)])\n",
    "max_xy = max(max_x, max_y)\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(7, 7))\n",
    "ax.axis('on')\n",
    "ax.set(xlim=(-max_xy-1., max_xy+1.), ylim=(-max_xy-1., max_xy+1.))\n",
    "ax.plot(0., 0.4, marker='v', markersize=15, color='red')\n",
    "ax.plot(EPx1, EPy1 + 0.4, marker='v', markersize=15, color='red')\n",
    "ax.plot(np.linspace(0., EPx1*1.01, 100), ergebnis, color='red', lw=0.3)\n",
    "\n",
    "line, = ax.plot([], [], 'o-', lw=0.5, color='blue', markersize=0) #connects the points\n",
    "   \n",
    "def animate_pendulum(times, x, y):    \n",
    "    def animate(i):\n",
    "        ax.set_title(f'Running time is {i / schritte * intervall:.2f} sec \\n' + \n",
    "                    f'The red curve is the catenary', fontsize=12)\n",
    "        line.set_data(x[i], y[i])\n",
    "        return line,\n",
    "\n",
    "    anim = animation.FuncAnimation(fig, animate, frames=len(times),\n",
    "                                   interval=1000*times.max() / len(times),\n",
    "                                   blit=True) #, init_func=init)\n",
    "    plt.close(fig)\n",
    "    return anim\n",
    "\n",
    "anim = animate_pendulum(times, x_coords, y_coords)\n",
    "print('to run the program before HTML(..) took {:.3f} sec'.format(time.time() - start))\n",
    "HTML(anim.to_jshtml())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is to check, how well the catenary is calculated, given $l_1, EP_x, EP_y$, meaning how close to $(EP_x, EP_y)$ it ends.\\\n",
    "I also try to iterate, starting from $EP_y = 0$, as it seems to alway work for this.\\\n",
    "I do not know, why it does not alway end at $(EP_x, EP_y)$\\\n",
    "The larger $l_1$, the better it seems to be."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "EPx1 = 10.\n",
    "EPy1 = -2.\n",
    "l1   = 20.\n",
    "if EPx1**2 + EPy1**2 > l1**2:\n",
    "    raise Exception('ss')\n",
    "faktor, xx, yy, delta = sm.symbols('faktor, xx, yy, delta')\n",
    "catenary     = 2. * faktor * sm.sinh(EPx / (2. * faktor)) - sm.sqrt(l**2 - EPy**2)\n",
    "catenary_lam = sm.lambdify([faktor, EPx, EPy, l], catenary, cse=True)\n",
    "\n",
    "def func3(X0, args):\n",
    "    return catenary_lam(*X0, *args)\n",
    "\n",
    "\n",
    "catenaryY = faktor * sm.cosh((xx)/(faktor))\n",
    "catenaryY_lam = sm.lambdify([xx, faktor, EPx], catenaryY, cse=True)\n",
    "\n",
    "catenaryZ = faktor * sm.cosh((xx + delta)/(faktor))\n",
    "catenaryZ_lam = sm.lambdify([delta, xx, faktor], catenaryZ, cse=True)\n",
    "\n",
    "def func4(X0):\n",
    "    return catenaryZ_lam(X0, EPx1/2., t0) - catenaryZ_lam(0., -EPx1/2, t0 ) - EPy2\n",
    "\n",
    "Delta = []\n",
    "Factor = []\n",
    "zahl = 20\n",
    "EPy2 = 0.\n",
    "X01 = 0.1\n",
    "z01 = 0.1\n",
    "delta = EPy1/zahl\n",
    "for _ in range(zahl):\n",
    "    EPy2 += delta\n",
    "    args = [EPx1, EPy2, l1]\n",
    "    t0   = fsolve(func3, X01, args)\n",
    "    t0   = t0[0]\n",
    "    X01  = t0\n",
    "    Factor.append(t0)\n",
    "\n",
    "    d0 = fsolve(func4, z01)\n",
    "    z01 = d0[0] \n",
    "    Delta.append(z01)\n",
    "\n",
    "# calculate the values of the catenary to be used below in the animation\n",
    "ergebnis = []\n",
    "for xx1 in np.linspace(0., EPx1, 100):\n",
    "    ergebnis.append(catenaryY_lam(xx1-(EPx1-d0)/2., t0, EPx1))\n",
    "abzug = ergebnis[0]\n",
    "for i in range(100):\n",
    "    ergebnis[i] = ergebnis[i] - abzug\n",
    "print(EPx1, EPy2)    \n",
    "fig, ax = plt.subplots(figsize=(10,5))\n",
    "ax.plot(0., 0., marker='v', color='red')\n",
    "ax.plot(EPx1, EPy2, marker='v', color='red')\n",
    "ax.plot(np.linspace(0., EPx1, 100), ergebnis);\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(10,4))\n",
    "ax.plot(range(zahl), Delta, label='delta')\n",
    "ax.plot(range(zahl), Factor, label='factor')\n",
    "ax.legend();"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}