car_on_street_gate_opty.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0a527636",
   "metadata": {},
   "source": [
    "I try to model a **conventional car**: The rear axle is driven, the front axle does the steering.\\\n",
    "No speed possible perpendicular to the wheels.\\\n",
    "The car must drive from A to B, using minimal energy, and it must stay on a road.\\\n",
    "It should stop at an interval (gate) $((B|final_u), (B|final_o))$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "f7cc216b",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy.physics.mechanics as me\n",
    "import time\n",
    "import numpy as np\n",
    "import sympy as sm\n",
    "from scipy.interpolate import CubicSpline\n",
    "\n",
    "from opty.direct_collocation import Problem\n",
    "from opty.utils import parse_free, create_objective_function\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib\n",
    "from IPython.display import HTML\n",
    "matplotlib.rcParams['animation.embed_limit'] = 2**128\n",
    "from matplotlib.animation import FuncAnimation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "090b386b",
   "metadata": {},
   "source": [
    "Set up **Kane's equations of motion**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "c665b199",
   "metadata": {},
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "\n",
    "N, A0, Ab, Af = sm.symbols('N A0 Ab Af', cls= me.ReferenceFrame)\n",
    "t = me.dynamicsymbols._t\n",
    "O, Pb, Dmc, Pf = sm.symbols('O Pb Dmc Pf', cls= me.Point)\n",
    "O.set_vel(N, 0)\n",
    "\n",
    "q0, qf  = me.dynamicsymbols('q_0 q_f')\n",
    "u0, uf  = me.dynamicsymbols('u_0 u_f')\n",
    "x, y    = me.dynamicsymbols('x y')\n",
    "ux, uy  = me.dynamicsymbols('u_x u_y')\n",
    "Tf, Fb  = me.dynamicsymbols('T_f F_b')\n",
    "\n",
    "reibung = sm.symbols('reibung')\n",
    "\n",
    "l, m0, mb, mf, iZZ0, iZZb, iZZf = sm.symbols('l m0 mb mf iZZ0, iZZb, iZZf')\n",
    "\n",
    "A0.orient_axis(N, q0, N.z)\n",
    "A0.set_ang_vel(N, u0 * N.z)\n",
    "\n",
    "Ab.orient_axis(A0, 0, N.z)\n",
    "\n",
    "Af.orient_axis(A0, qf, N.z)\n",
    "rot = Af.ang_vel_in(N)\n",
    "Af.set_ang_vel(N, uf * N.z)\n",
    "rot1 = Af.ang_vel_in(N)\n",
    "\n",
    "Pf.set_pos(O, x * N.x + y * N.y)\n",
    "Pf.set_vel(N, ux * N.x + uy * N.y)\n",
    "\n",
    "Pb.set_pos(Pf, -l * A0.y)\n",
    "Pb.v2pt_theory(Pf, N, A0)\n",
    "\n",
    "Dmc.set_pos(Pf, -l/2 * A0.y)\n",
    "Dmc.v2pt_theory(Pf, N, A0)\n",
    "\n",
    "# No speed perpendicular to the wheels\n",
    "vel1 = me.dot(Pb.vel(N), Ab.x) - 0\n",
    "vel2 = me.dot(Pf.vel(N), Af.x) - 0\n",
    "\n",
    "I0 = me.inertia(A0, 0, 0, iZZ0)\n",
    "body0 = me.RigidBody('body0', Dmc, A0, m0, (I0, Dmc))\n",
    "Ib = me.inertia(Ab, 0, 0, iZZb)\n",
    "bodyb = me.RigidBody('bodyb', Pb, Ab, mb, (Ib, Pb))\n",
    "If = me.inertia(Af, 0, 0, iZZf)\n",
    "bodyf = me.RigidBody('bodyf', Pf, Af, mf, (If, Pf))\n",
    "BODY = [body0, bodyb, bodyf]\n",
    "\n",
    "\n",
    "# Forces\n",
    "FL = [(Pb, Fb * Ab.y), (Af, Tf * N.z), (Dmc, -reibung * Dmc.vel(N))]\n",
    "\n",
    "kd = sm.Matrix([ux - x.diff(t), uy - y.diff(t), u0 - q0.diff(t),\n",
    "                me.dot(rot1- rot, N.z)])\n",
    "speed_constr = sm.Matrix([vel1, vel2])\n",
    "\n",
    "q_ind = [x, y, q0, qf]\n",
    "u_ind = [u0, uf]\n",
    "u_dep = [ux, uy]\n",
    "\n",
    "# Create the KanesMethod object\n",
    "KM = me.KanesMethod(\n",
    "                    N, q_ind=q_ind, u_ind=u_ind,\n",
    "                    kd_eqs=kd,\n",
    "                    u_dependent=u_dep,\n",
    "                    velocity_constraints=speed_constr,\n",
    "                    )\n",
    "(fr, frstar) = KM.kanes_equations(BODY, FL)\n",
    "eom = fr + frstar\n",
    "eom = kd.col_join(eom)\n",
    "eom = eom.col_join(speed_constr)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9545c291",
   "metadata": {},
   "source": [
    "Set up so the car stays on the street\\\n",
    "The street is described by two functions in the X / Y plane f(x), g(x)\n",
    "with f(x) < g(x) for all x. For *number* of points $P(x_i. y_i) \\in $ car it must hold:\\\n",
    "$ f(x_i) < y_i < g(x_i)$\\\n",
    "As *opty* at present does not allow inequalities in the eoms, I circumvent this by\n",
    "introducing additional state variables."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "c76a2b1d",
   "metadata": {},
   "outputs": [],
   "source": [
    "XX, a, b, c, d = sm.symbols('XX a b c d')\n",
    "def street(XX, a, b, c):\n",
    "    return a*sm.sin(b*XX) + c\n",
    "\n",
    "number = 4\n",
    "py_upper = me.dynamicsymbols(f'py_upper:{number}')\n",
    "py_lower = me.dynamicsymbols(f'py_lower:{number}')\n",
    "\n",
    "park1y = Pf.pos_from(O).dot(N.y)\n",
    "park2y = Pb.pos_from(O).dot(N.y)\n",
    "park1x = Pf.pos_from(O).dot(N.x)\n",
    "park2x = Pb.pos_from(O).dot(N.x)\n",
    "\n",
    "delta_x = np.linspace(park1x, park2x, number)\n",
    "delta_y = np.linspace(park1y, park2y, number)\n",
    "\n",
    "delta_p_u = [delta_y[i] - street(delta_x[i], a, b, c) for i in range(number)]\n",
    "delta_p_l = [-delta_y[i] + street(delta_x[i], a, b, c+d) for i in range(number)]\n",
    "\n",
    "eom_add = sm.Matrix([\n",
    "    *[-py_upper[i] + delta_p_u[i] for i in range(number)],\n",
    "    *[-py_lower[i] + delta_p_l[i] for i in range(number)],\n",
    "])\n",
    "\n",
    "eom = eom.col_join(eom_add)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "272b91c6",
   "metadata": {},
   "source": [
    "*opty* presently does not support bounds for instance constraints.\\\n",
    "I circumvent this as follows:\\\n",
    "I add two state variables $top$ and $bottom$ and two specified symbolds $top_h$\n",
    "and $bottom_h$\\\n",
    "Say, we need $a \\leq y(t_x) \\leq b, a, b > 0 $, then I add this to the equations of motion:\n",
    "\n",
    "$top = top_h \\cdot y(t_x)$\\\n",
    "$bottom = bottom_h \\cdot y(t_x)$\n",
    "\n",
    "Then I bound $top_h \\in (1.0, \\inf)$ and \n",
    "$bottom_h \\in (0.0, 1.0)$, and put instance constraints $top(t_x) = b, \n",
    "bottom(t_x) = a$\\\n",
    "This forces $a \\leq y(t_x) \\leq b$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "61af42cc",
   "metadata": {},
   "outputs": [],
   "source": [
    "top, bottom = me.dynamicsymbols('top, bottom')\n",
    "top_h, bottom_h = me.dynamicsymbols('top_h, bottom_h')\n",
    "\n",
    "eom_add = sm.Matrix([\n",
    "    -top + park1y * top_h,\n",
    "    -bottom + park1y * bottom_h,\n",
    "])\n",
    "\n",
    "eom = eom.col_join(eom_add)\n",
    "\n",
    "print(f'eom too large to print out. Its shape is {eom.shape} and it has ' +\n",
    "      f'{sm.count_ops(eom)} operations')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e0685c32",
   "metadata": {},
   "source": [
    "Set up the machinery for **opty** and **solve** the optimization.\\\n",
    "If needed, one can iterate, using the solution of iteration i as initial_guess for iteration i + 1.\\\n",
    "As the convergence is very slow, I use the result of a previous run as initial guess."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc7e7050",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "state_symbols = [x, y, q0, qf, ux, uy, u0, uf] + py_upper + py_lower + [top, bottom]\n",
    "\n",
    "laenge = len(state_symbols)\n",
    "constant_symbols = (l, m0, mb, mf, iZZ0, iZZb, iZZf, reibung, a, b, c, d)\n",
    "specified_symbols = [Fb, Tf] +[top_h, bottom_h]\n",
    "unknown_symbols = ()\n",
    "\n",
    "# Specify the known system parameters.\n",
    "final_o = 4.0\n",
    "final_u = 2.0\n",
    "par_map = {}\n",
    "par_map[m0] = 1.0\n",
    "par_map[mb] = 0.5\n",
    "par_map[mf] = 0.5\n",
    "par_map[iZZ0] = 1.\n",
    "par_map[iZZb] = 0.5\n",
    "par_map[iZZf] = 0.5\n",
    "par_map[l] = 3.0\n",
    "par_map[reibung] = 0.5\n",
    "# below for the shape of the street\n",
    "par_map[a] = 3.5\n",
    "par_map[b] = 0.5\n",
    "par_map[c] = 4.0\n",
    "par_map[d] = 4.5\n",
    "\n",
    "t0, tf = 0.0, 10.0\n",
    "num_nodes = 601\n",
    "interval_value = (tf - t0)/(num_nodes - 1)\n",
    "\n",
    "objective = sm.Integral(Tf**2 + 1.e2*Fb**2, t)\n",
    "obj, obj_grad = create_objective_function(\n",
    "    objective,\n",
    "    state_symbols,\n",
    "    specified_symbols,\n",
    "    tuple(),\n",
    "    num_nodes,\n",
    "    interval_value,\n",
    ")\n",
    "\n",
    "instance_constraints = (\n",
    "        x.func(t0) + 10.0,\n",
    "        ux.func(t0),\n",
    "        uy.func(t0),\n",
    "        u0.func(t0),\n",
    "        uf.func(t0),\n",
    "\n",
    "        x.func(tf) - 10.0,\n",
    "        ux.func(tf),\n",
    "        uy.func(tf),\n",
    "        top.func(tf) - final_o,\n",
    "        bottom.func(tf) - final_u,\n",
    ")\n",
    "\n",
    "grenze = 100.0\n",
    "grenze1 = 5.0\n",
    "delta = np.pi/4.0\n",
    "bounds1 = {\n",
    "        Fb: (-grenze, grenze),\n",
    "        Tf: (-grenze, grenze),\n",
    "        # restrict the steering angle to avoid locking\n",
    "        qf: (-np.pi/2. + delta, np.pi/2. - delta),\n",
    "        x: (-14, 14),\n",
    "        y: (0.0, 25),\n",
    "        top_h: (1.0, 10.0),\n",
    "        bottom_h: (0.0, 1.0),\n",
    "}\n",
    "\n",
    "bounds2 = {py_upper[i]: (0, 10.0) for i in range(number)}\n",
    "bounds3 = {py_lower[i]: (0.0, 10.0) for i in range(number)}\n",
    "bounds = {**bounds1, **bounds2, **bounds3}\n",
    "\n",
    "prob = Problem(\n",
    "        obj,\n",
    "        obj_grad,\n",
    "        eom,\n",
    "        state_symbols,\n",
    "        num_nodes,\n",
    "        interval_value,\n",
    "        known_parameter_map=par_map,\n",
    "        instance_constraints=instance_constraints,\n",
    "        bounds=bounds,\n",
    ")\n",
    "\n",
    "initial_guess = np.ones(prob.num_free) * 0.01\n",
    "initial_guess = np.load('car_on_street_gate_solution.npy')\n",
    "prob.add_option('max_iter', 3000)\n",
    "for i in range(1):\n",
    "# Find the optimal solution.\n",
    "    solution, info = prob.solve(initial_guess)\n",
    "    initial_guess = solution\n",
    "    print(f'{i+1} - th iteration')\n",
    "    print('message from optimizer:', info['status_msg'])\n",
    "    print('Iterations needed',len(prob.obj_value))\n",
    "    print(f\"objective value {info['obj_val']:.3e} \\n\")\n",
    "\n",
    "prob.plot_objective_value()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "87dfce60",
   "metadata": {},
   "source": [
    "Plot the constraint violations."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9954e90e",
   "metadata": {},
   "outputs": [],
   "source": [
    "prob.plot_constraint_violations(solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6fb77b13",
   "metadata": {},
   "source": [
    "Plot generalized coordinates / speeds and forces / torques"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a6f0398b",
   "metadata": {},
   "outputs": [],
   "source": [
    "prob.plot_trajectories(solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c384f66b",
   "metadata": {},
   "source": [
    "aminate the **car**\\\n",
    "The green arrows symbolize the forces which opty calculated to drive the car. They are perpendicular to the rear axle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5546cec9",
   "metadata": {},
   "outputs": [],
   "source": [
    "fps = 15\n",
    "street_lam = sm.lambdify((x, a, b, c), street(x, a, b, c))\n",
    "\n",
    "def add_point_to_data(line, x, y):\n",
    "# to trace the path of the point. Copied from Timo.\n",
    "    old_x, old_y = line.get_data()\n",
    "    line.set_data(np.append(old_x, x), np.append(old_y, y))\n",
    "\n",
    "\n",
    "state_vals, input_vals, _ = parse_free(solution, len(state_symbols),\n",
    "                len(specified_symbols), num_nodes)\n",
    "t_arr = np.linspace(t0, tf, num_nodes)\n",
    "state_sol = CubicSpline(t_arr, state_vals.T)\n",
    "input_sol = CubicSpline(t_arr, input_vals.T)\n",
    "\n",
    "# create additional points for the axles\n",
    "Pbl, Pbr, Pfl, Pfr = sm.symbols('Pbl Pbr Pfl Pfr', cls= me.Point)\n",
    "\n",
    "# end points of the force, length of the axles\n",
    "Fbq = me.Point('Fbq')\n",
    "la = sm.symbols('la')\n",
    "fb, tq = sm.symbols('f_b, t_q')\n",
    "\n",
    "Pbl.set_pos(Pb, -la/2 * Ab.x)\n",
    "Pbr.set_pos(Pb, la/2 * Ab.x)\n",
    "Pfl.set_pos(Pf, -la/2 * Af.x)\n",
    "Pfr.set_pos(Pf, la/2 * Af.x)\n",
    "\n",
    "Fbq.set_pos(Pb, fb * Ab.y)\n",
    "\n",
    "coordinates = Pb.pos_from(O).to_matrix(N)\n",
    "for point in (Dmc, Pf, Pbl, Pbr, Pfl, Pfr, Fbq):\n",
    "    coordinates = coordinates.row_join(point.pos_from(O).to_matrix(N))\n",
    "\n",
    "pL, pL_vals = zip(*par_map.items())\n",
    "la1 = par_map[l] / 4.                      # length of an axle\n",
    "la2 = la1/1.8\n",
    "coords_lam = sm.lambdify((*state_symbols, fb, *pL, la), coordinates,\n",
    "        cse=True)\n",
    "\n",
    "def init():\n",
    "        xmin, xmax = -15, 15.\n",
    "        ymin, ymax = -0.75, 12.5\n",
    "\n",
    "        fig = plt.figure(figsize=(8, 8))\n",
    "        ax  = fig.add_subplot(111)\n",
    "        ax.set_xlim(xmin, xmax)\n",
    "        ax.set_ylim(ymin, ymax)\n",
    "        ax.set_aspect('equal')\n",
    "        ax.grid()\n",
    "\n",
    "        XX = np.linspace(xmin, xmax,  200)\n",
    "        ax.fill_between(XX, ymin, street_lam(XX, par_map[a], par_map[b],\n",
    "                par_map[c]-la2), color='grey', alpha=0.25)\n",
    "        ax.fill_between(XX, street_lam(XX, par_map[a], par_map[b],\n",
    "                par_map[c]+par_map[d]+la2), ymax, color='grey', alpha=0.25)\n",
    "\n",
    "        ax.plot(XX, street_lam(XX, par_map[a], par_map[b], par_map[c]-la2),\n",
    "                color='black')\n",
    "        ax.plot(XX, street_lam(XX, par_map[a],\n",
    "                par_map[b], par_map[c]+par_map[d]+la2), color='black')\n",
    "        ax.vlines(-10.0, street_lam(-10.0, par_map[a], par_map[b],\n",
    "                par_map[c]-la2), street_lam(-10, par_map[a], par_map[b],\n",
    "                par_map[c]+par_map[d]+la2), color='red', linestyle='dotted')\n",
    "        ax.vlines(10.0, street_lam(10, par_map[a], par_map[b], par_map[c]-la2),\n",
    "                final_u, color='green', linestyle='-')\n",
    "        ax.vlines(10.0, final_o,\n",
    "                street_lam(10, par_map[a], par_map[b], par_map[c]+par_map[d]\n",
    "                +la2), color='green', linestyle='-')\n",
    "        ax.vlines(10.0, final_u, final_o, color='green', linestyle='dotted')\n",
    "\n",
    "\n",
    "        line1, = ax.plot([], [], color='orange', lw=2)\n",
    "        line2, = ax.plot([], [], color='red', lw=2)\n",
    "        line3, = ax.plot([], [], color='magenta', lw=2)\n",
    "        line4  = ax.quiver([], [], [], [], color='green', scale=10, width=0.004,\n",
    "                headwidth=8)\n",
    "\n",
    "        return fig, ax, line1, line2, line3, line4\n",
    "\n",
    "# Function to update the plot for each animation frame\n",
    "fig, ax, line1, line2, line3, line4 = init()\n",
    "def update(t):\n",
    "    message = (f'running time {t:.2f} sec \\n The rear axle is red, the ' +\n",
    "               f'front axle is magenta \\n The driving/breaking force is green')\n",
    "    ax.set_title(message, fontsize=12)\n",
    "\n",
    "    coords = coords_lam(*state_sol(t), input_sol(t)[0], *pL_vals, la1)\n",
    "\n",
    "    #   Pb, Dmc, Pf, Pbl, Pbr, Pfl, Pfr, Fbq\n",
    "    line1.set_data([coords[0, 0], coords[0, 2]], [coords[1, 0], coords[1, 2]])\n",
    "    line2.set_data([coords[0, 3], coords[0, 4]], [coords[1, 3], coords[1, 4]])\n",
    "    line3.set_data([coords[0, 5], coords[0, 6]], [coords[1, 5], coords[1, 6]])\n",
    "\n",
    "    line4.set_offsets([coords[0, 0], coords[1, 0]])\n",
    "    line4.set_UVC(coords[0, 7] - coords[0, 0] , coords[1, 7] - coords[1, 0])\n",
    "    return line1, line2, line3, line4, #line5, line6,\n",
    "\n",
    "frames = np.linspace(t0, tf, int(fps * (tf - t0)))\n",
    "animation = FuncAnimation(fig, update, frames=frames, interval=1000 / fps)\n",
    "\n",
    "\n",
    "# Show the animation\n",
    "display(HTML(animation.to_jshtml()))\n",
    "plt.close()\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "opty-dev",
   "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.12.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}