.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples\plot_particle_different_eoms.py" .. LINE NUMBERS ARE GIVEN BELOW. .. only:: html .. note:: :class: sphx-glr-download-link-note :ref:`Go to the end ` to download the full example code. .. rst-class:: sphx-glr-example-title .. _sphx_glr_examples_plot_particle_different_eoms.py: Particle in Changing Environment ================================ Objective --------- - Show how it is possible to change the equations of motion during the motion using smooth hump functions. Description ----------- A particle is moving along a street defined by a sine function from 0.0 m to 20.0 m in the x direction as fast as possible. The particle has a rocket engine that provides thrust. The particle's mass changes during the motion, as does the friction acting on it and the exhaust speed. This is very artificial, of course, just to show how it can be done. So, in a way the equations of motion change during the motion. Notes ----- - Even with this simple example, one has to 'play around' a bit to get results. For example, if the steepness of the hump functions is too high, one gets seemingly unreasonable results. - Very strange: If :math:`T` is included as a ``state``, the optimization works fine. If it is made an ``input``, the result is very different, much less reasonable. No idea why. **States** - :math:`x` : x position of the particle - :math:`y` : y position of the particle - :math:`q` : direction of the force/thrust - :math:`u_x` : x velocity of the particle - :math:`u_y` : y velocity of the particle - :math:`u` : angular velocity of the direction of the force/thrust - :math:`m` : mass of the fuel - :math:`T` : Torque to change the direction of the thrust, of no importance **Inputs** - :math:`\alpha` : burn rate of the fuel: :math:`\dot{m} = - \alpha m` **Parameters** - :math:`x_1, x_2, x_3` : positions along the street where the changes happen - :math:`a, b` : parameters defining the street shape - :math:`v_{gas}` : exhaust speed of the rocket engine - :math:`m_0` : payload mass (net weight without fuel ) .. GENERATED FROM PYTHON SOURCE LINES 60-69 .. code-block:: Python import numpy as np import matplotlib.pyplot as plt import sympy as sm import sympy.physics.mechanics as me from opty import Problem from scipy.interpolate import CubicSpline from matplotlib.patches import FancyArrowPatch from matplotlib.animation import FuncAnimation .. GENERATED FROM PYTHON SOURCE LINES 70-72 Equations of Motion ------------------- .. GENERATED FROM PYTHON SOURCE LINES 72-99 .. code-block:: Python N, A = me.ReferenceFrame('N'), me.ReferenceFrame('A') O, P = me.Point('O'), me.Point('P') O.set_vel(N, 0) t = me.dynamicsymbols._t steep = sm.symbols('steep') steepness = 5.0 x1, x2, x3 = sm.symbols('x1 x2 x3') a, b = sm.symbols('a b') x, y, ux, uy, q, u = me.dynamicsymbols('x y ux uy q u') m, mdt = me.dynamicsymbols('m mdt') alpha = me.dynamicsymbols('alpha') v_gas = sm.symbols('v_gas') m0 = sm.symbols('m0') mu = sm.symbols('mu') T = me.dynamicsymbols('T') def strasse(x, a, b): return a * sm.sin(b * x) .. GENERATED FROM PYTHON SOURCE LINES 100-101 Define the smooth hump function, and plot it. .. GENERATED FROM PYTHON SOURCE LINES 101-117 .. code-block:: Python def smooth_hump(x, x1, x2, steepness): return 0.5 * (sm.tanh(steepness * (x - x1)) - sm.tanh(steepness * (x - x2))) hump_lambda = sm.lambdify((x, x1, x2, steep), smooth_hump(x, x1, x2, steep), cse=True) xx = np.linspace(-10, 10, 100) fig, ax = plt.subplots(figsize=(6.4, 2.5)) ax.plot(xx, hump_lambda(xx, -5, 5, steepness)) ax.axvline(-5, color='red', linestyle='--') ax.axvline(5, color='red', linestyle='--') _ = ax.set_title('Smooth hump function, steepness={}'.format(steepness)) .. image-sg:: /examples/images/sphx_glr_plot_particle_different_eoms_001.png :alt: Smooth hump function, steepness=5.0 :srcset: /examples/images/sphx_glr_plot_particle_different_eoms_001.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 118-119 Define the particle system. .. GENERATED FROM PYTHON SOURCE LINES 119-126 .. code-block:: Python A.orient_axis(N, q, N.z) A.set_ang_vel(N, u * N.z) P.set_pos(O, x * N.x + y * N.y) P.set_vel(N, ux * N.x + uy * N.y) .. GENERATED FROM PYTHON SOURCE LINES 127-129 Between x1 and x2 the mass of the payload increases from :math:`m + m_0` to :math:`m + 4 m_0`. .. GENERATED FROM PYTHON SOURCE LINES 129-131 .. code-block:: Python Pa = me.Particle('Pa', P, m + m0 + 3*m0 * smooth_hump(x, x1, x2, steepness)) .. GENERATED FROM PYTHON SOURCE LINES 132-133 Friction and exhaust speed change between x2 and x3 .. GENERATED FROM PYTHON SOURCE LINES 133-139 .. code-block:: Python forces = [(P, -m.diff(t) * (v_gas - 0.5 * v_gas * smooth_hump(x, x2, x3, steepness)) * A.x - (mu + 5.0 * mu * smooth_hump(x, x2, x3, steepness)) * (ux * N.x + uy * N.y)), (A, T * N.z - 0.01 * u * N.z)] .. GENERATED FROM PYTHON SOURCE LINES 140-141 Set up Kane's Method and get the equations of motion. .. GENERATED FROM PYTHON SOURCE LINES 141-158 .. code-block:: Python kd = sm.Matrix([ x.diff(t) - ux, y.diff(t) - uy, q.diff(t) - u, ]) kane = me.KanesMethod( N, q_ind=[x, y, q], u_ind=[ux, uy, u], kd_eqs=kd ) fr, frstar = kane.kanes_equations([Pa], forces) eom = kd.col_join(fr + frstar) .. GENERATED FROM PYTHON SOURCE LINES 159-161 Add the fuel consumption equation. The assumption is, that the the exhaust is proportional to the fuel left: :math:`\dfrac{dm(t)}{dt} = - \alpha m(t)` .. GENERATED FROM PYTHON SOURCE LINES 161-163 .. code-block:: Python eom = eom.col_join(sm.Matrix([m.diff(t) + alpha * m])) .. GENERATED FROM PYTHON SOURCE LINES 164-168 Particle to stay on the street. mdt - m.diff(t) is added, so m.diff(t) is available and the force may be calculated for the animation. .. GENERATED FROM PYTHON SOURCE LINES 168-170 .. code-block:: Python eom = eom.col_join(sm.Matrix([y - strasse(x, a, b), mdt - m.diff(t)])) .. GENERATED FROM PYTHON SOURCE LINES 171-173 Set Up the Optimization ----------------------- .. GENERATED FROM PYTHON SOURCE LINES 173-204 .. code-block:: Python h = sm.symbols('h') num_nodes = 201 t0, tb, tf = 0.0, h * int((num_nodes - 1)/2), h * (num_nodes - 1) interval = h state_symbols = (x, y, q, ux, uy, u, m, T) instance_constraint = ( x.func(t0), y.func(t0), q.func(t0), ux.func(t0), uy.func(t0), u.func(t0), m.func(t0) - 5.0, ux.func(tb) - 0.0, uy.func(tb) - 0.0, x.func(tf) - 20.0, ux.func(tf) - 0.0, uy.func(tf) - 0.0, ) bounds = { h: (0.0, 1.0), alpha: (0.0, 0.5), T: (-5.0, 5.0), q: (-np.pi, np.pi), x: (0.0, 20.0), } .. GENERATED FROM PYTHON SOURCE LINES 205-206 Keep the particle on the street, within + / - 0.5 units. .. GENERATED FROM PYTHON SOURCE LINES 206-209 .. code-block:: Python eom_bounds = {7: (-0.5, 0.5)} .. GENERATED FROM PYTHON SOURCE LINES 210-211 Define the known parameters, the objective function and the Problem. .. GENERATED FROM PYTHON SOURCE LINES 211-249 .. code-block:: Python par_map = { x1: 5.0, x2: 10.0, x3: 15.0, m0: 1.0, a: 2.0, b: np.pi / 10, v_gas: 100.0, mu: 1.0, } def obj(free): return free[-1] def obj_grad(free): grad = np.zeros_like(free) grad[-1] = 1.0 return grad prob = Problem( obj, obj_grad, eom, state_symbols, num_nodes, interval, known_parameter_map=par_map, instance_constraints=instance_constraint, bounds=bounds, eom_bounds=eom_bounds, backend='cython', time_symbol=t, ) .. GENERATED FROM PYTHON SOURCE LINES 250-251 Solve the problem. .. GENERATED FROM PYTHON SOURCE LINES 251-259 .. code-block:: Python prob.add_option('max_iter', 5000) initial_guess = np.ones(prob.num_free) * 0.1 for _ in range(1): solution, info = prob.solve(initial_guess) initial_guess = solution print(info['status_msg']) .. rst-class:: sphx-glr-script-out .. code-block:: none b'Algorithm terminated successfully at a locally optimal point, satisfying the convergence tolerances (can be specified by options).' .. GENERATED FROM PYTHON SOURCE LINES 260-261 Plot the objective value. .. GENERATED FROM PYTHON SOURCE LINES 261-264 .. code-block:: Python _ = prob.plot_objective_value() .. image-sg:: /examples/images/sphx_glr_plot_particle_different_eoms_002.png :alt: Objective Value :srcset: /examples/images/sphx_glr_plot_particle_different_eoms_002.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 265-266 Plot the trajectories. .. GENERATED FROM PYTHON SOURCE LINES 266-269 .. code-block:: Python _ = prob.plot_trajectories(solution, show_bounds=True) .. image-sg:: /examples/images/sphx_glr_plot_particle_different_eoms_003.png :alt: State Trajectories, Input Trajectories :srcset: /examples/images/sphx_glr_plot_particle_different_eoms_003.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 270-271 Plot the constraint violations. .. GENERATED FROM PYTHON SOURCE LINES 271-275 .. code-block:: Python _ = prob.plot_constraint_violations(solution, show_bounds=True, subplots=True) .. image-sg:: /examples/images/sphx_glr_plot_particle_different_eoms_004.png :alt: Constraint violations Values of bounded EoMs :srcset: /examples/images/sphx_glr_plot_particle_different_eoms_004.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 276-278 Animation --------- .. GENERATED FROM PYTHON SOURCE LINES 280-401 .. code-block:: Python fps = 25 street_lam = sm.lambdify((x, a, b), strasse(x, a, b)) state_vals, input_vals, _, h_val = prob.parse_free(solution) tf = h_val * (num_nodes - 1) kraft_vals = input_vals.T[:, 0] * input_vals.T[:, 1] t_arr = np.linspace(t0, tf, num_nodes) state_sol = CubicSpline(t_arr, state_vals.T) input_sol = CubicSpline(t_arr, input_vals.T) kraft_sol = CubicSpline(t_arr, kraft_vals) # create additional point for the force Pf = sm.symbols('Pf', cls=me.Point) kraft = sm.symbols('kraft') Pf.set_pos(P, kraft * A.x) coordinates = P.pos_from(O).to_matrix(N) coordinates = coordinates.row_join(Pf.pos_from(P).to_matrix(N)) pL, pL_vals = zip(*par_map.items()) coords_lam = sm.lambdify((*state_symbols, kraft, *pL), coordinates, cse=True) def init(): xmin, xmax = -1.0, 21.0 ymin, ymax = -par_map[a]-1.0, par_map[a]+1.0 fig = plt.figure(figsize=(8, 8)) ax = fig.add_subplot(111) ax.set_xlim(xmin, xmax) ax.set_ylim(ymin, ymax) ax.set_aspect('equal') ax.grid() XX = np.linspace(xmin, xmax, 200) ax.plot(XX, street_lam(XX, par_map[a], par_map[b]) + 0.5, color='black', linestyle='-') ax.plot(XX, street_lam(XX, par_map[a], par_map[b]) - 0.5, color='black', linestyle='-') ax.axvline(par_map[x1], color='red', linestyle='--') ax.axvline(par_map[x2], color='red', linestyle='--') ax.axvline(par_map[x3], color='red', linestyle='--') ax.set_xlabel('x position [m]') ax.set_ylabel('y position [m]') arrow0 = FancyArrowPatch( posA=(6, -5.5), posB=(7.5, -1), arrowstyle='->', # arrow head connectionstyle='arc3,rad=-0.3', color='blue', mutation_scale=20, lw=0.5, ) ax.add_patch(arrow0) ax.text(5.0, -5.5, f"Here the mass \njumps from $m(t) + m_0$ to " "$m(t) + 4 m_0$", fontsize=8, color='blue') ax.fill_between( [par_map[x1], par_map[x2]], y1=ax.get_ylim()[0], y2=ax.get_ylim()[1], hatch='//', facecolor='none', # important: no solid fill edgecolor='blue', alpha=0.5 ) arrow1 = FancyArrowPatch( posA=(15, -5.5), posB=(12.5, -1), arrowstyle='->', # arrow head connectionstyle='arc3,rad=-0.3', color='red', mutation_scale=20, lw=0.5, ) ax.add_patch(arrow1) ax.text(12.5, -5.5, f"Here the friction \njumps from $\\mu$ to 6$\\mu$ \n" f"the exhaust speed drops 50%", fontsize=8, color='red') ax.fill_between( [par_map[x2], par_map[x3]], y1=ax.get_ylim()[0], y2=ax.get_ylim()[1], hatch='\\', facecolor='none', # important: no solid fill edgecolor='red', alpha=0.5, ) line1 = ax.scatter([], [], color='red', s=100) pfeil = ax.quiver([], [], [], [], color='green', scale=5, width=0.004, headwidth=8) return fig, ax, line1, pfeil # Function to update the plot for each animation frame fig, ax, line1, pfeil = init() def update(t): message = (f'running time {t:.2f} sec \n ' 'The driving/breaking force is green') ax.set_title(message, fontsize=12) coords = coords_lam(*state_sol(t), -kraft_sol(t), *pL_vals) line1.set_offsets([coords[0, 0], coords[1, 0]]) pfeil.set_offsets([coords[0, 0], coords[1, 0]]) pfeil.set_UVC(coords[0, 1], coords[1, 1]) frames = np.linspace(t0, tf, int(fps * (tf - t0))) animation = FuncAnimation(fig, update, frames=frames, interval=2000 / fps) plt.show() .. container:: sphx-glr-animation .. raw:: html
.. rst-class:: sphx-glr-timing **Total running time of the script:** (0 minutes 25.084 seconds) .. _sphx_glr_download_examples_plot_particle_different_eoms.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: plot_particle_different_eoms.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: plot_particle_different_eoms.py ` .. container:: sphx-glr-download sphx-glr-download-zip :download:`Download zipped: plot_particle_different_eoms.zip ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_