.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples/intermediate/plot_crane_moving_a_load.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_intermediate_plot_crane_moving_a_load.py: Crane Moving a Load =================== Objectives ---------- - Show the use of opty's variable node time interval feature to solve a relatively simple problem. - Show how to use additional specifieds to enforce instance constraints on :math:`\dfrac{d^2}{dt^2}(\textrm{state variable})` Introduction ------------ A load is moved by a crane. The load is connected to the crane by a massless rod and a pin joint. The goal is to move the load from the initial position to the final position in the shortest possible time. The load must not over-swing the final position. The load is moved by a force applied to its suspension point. The jib of the crane is extended in the horizontal X direction. In order to ensure that the load is at rest at its final location, its velocity **and** its acceleration must be zero at the final time. Notes ----- The solution ``opty`` finds may not what one would expect intuitively. **Constants** - :math:`l` : length of the rod attaching the load to the crane [m] - :math:`m_1` : mass of mover attached to the arm of the crane [kg] - :math:`m_2` : mass of the load [kg] - :math:`g` : acceleration due to gravity [m/s²] **States** - :math:`x_c` : x-coordinate of the mover [m] - :math:`x_l` : x-coordinate of the load [m] - :math:`y_l` : y-coordinate of the load [m] - :math:`q` : angle of the rod [rad] - :math:`u_{xc}` : velocity of the mover in x-direction [m/s] - :math:`u_{xl}` : velocity of the load in x-direction [m/s] - :math:`u_{yl}` : velocity of the load in y-direction [m/s] - :math:`u` : angular velocity of the rod [rad/s] **Specifieds** - :math:`F` : force applied to the mover [N] - :math:`h_1` : needed to enforce instance constraints - :math:`h_2` : needed to enforce instance constraints .. GENERATED FROM PYTHON SOURCE LINES 58-69 .. code-block:: Python import os import sympy.physics.mechanics as me import numpy as np import sympy as sm from scipy.interpolate import CubicSpline from opty.direct_collocation import Problem from opty.utils import MathJaxRepr import matplotlib.pyplot as plt import matplotlib.animation as animation from matplotlib import patches .. GENERATED FROM PYTHON SOURCE LINES 70-73 Set up Kane's Equations of Motion --------------------------------- .. GENERATED FROM PYTHON SOURCE LINES 73-129 .. code-block:: Python N, A = sm.symbols('N A', cls=me.ReferenceFrame) t = me.dynamicsymbols._t O, P1, P2 = sm.symbols('O P1 P2', cls=me.Point) O.set_vel(N, 0) xc, xl, yl, q, uxc, uxl, uyl, u, F, h1, h2 = \ me.dynamicsymbols('xc, xl, yl, q, uxc, uxl, uyl, uq, F, h1, h2') l, m1, m2, g = sm.symbols('l, m1, m2, g') A.orient_axis(N, q, N.z) A.set_ang_vel(N, u*N.z) P1.set_pos(O, xc*N.x) P1.set_vel(N, uxc*N.x) P2.set_pos(P1, -l*A.y) P2.v2pt_theory(P1, N, A) P1a = me.Particle('P1a', P1, m1) P2a = me.Particle('P2a', P2, m2) bodies = [P1a, P2a] forces = [(P1, F*N.x - m1*g*N.y), (P2, -m2*g*N.y)] kd = sm.Matrix([ uxc - xc.diff(t), u - q.diff(t), uxl - xl.diff(t), uyl - yl.diff(t), ]) config_constr = sm.Matrix([xl - xc - l*sm.sin(q), yl + l*sm.cos(q)]) speed_constr = config_constr.diff(t) q_ind = [xc, q] q_dep = [xl, yl] u_ind = [uxc, u] u_dep = [uxl, uyl] KM = me.KanesMethod( N, q_ind=q_ind, q_dependent=q_dep, u_ind=u_ind, u_dependent=u_dep, kd_eqs=kd, configuration_constraints=config_constr, velocity_constraints=speed_constr, ) fr, frstar = KM.kanes_equations(bodies, forces) eom = kd.col_join(fr + frstar) eom = eom.col_join(config_constr) eom = eom.col_join(sm.Matrix([h1 - u.diff(t), h2 - uxc.diff(t)])) MathJaxRepr(eom) .. raw:: html
$$\left[\begin{matrix}uxc - \dot{xc}\\uq - \dot{q}\\uxl - \dot{xl}\\uyl - \dot{yl}\\l m_{2} uq^{2} \sin{\left(q \right)} - l m_{2} \cos{\left(q \right)} \dot{uq} - \left(m_{1} + m_{2}\right) \dot{uxc} + F\\- g l m_{2} \sin{\left(q \right)} - l^{2} m_{2} \dot{uq} - l m_{2} \cos{\left(q \right)} \dot{uxc}\\- l \sin{\left(q \right)} - xc + xl\\l \cos{\left(q \right)} + yl\\h_{1} - \dot{uq}\\h_{2} - \dot{uxc}\end{matrix}\right]$$


.. GENERATED FROM PYTHON SOURCE LINES 130-133 Set up the Optimization Problem and Solve it -------------------------------------------- .. GENERATED FROM PYTHON SOURCE LINES 133-142 .. code-block:: Python state_symbols = tuple((*q_ind, *q_dep, *u_ind, *u_dep)) constant_symbols = (l, m1, m2, g) specified_symbols = (F, h1, h2) h = sm.symbols('h') num_nodes = 150 duration = (num_nodes - 1)*h interval_value = h .. GENERATED FROM PYTHON SOURCE LINES 143-144 Specify the known system parameters. .. GENERATED FROM PYTHON SOURCE LINES 144-153 .. code-block:: Python par_map = {} par_map[l] = 5.0 par_map[m1] = 1.0 par_map[m2] = 10.0 par_map[g] = 9.81 t0, tf = 0.0, duration .. GENERATED FROM PYTHON SOURCE LINES 154-155 Set up the objective function and its gradient. .. GENERATED FROM PYTHON SOURCE LINES 155-166 .. code-block:: Python def obj(free): """Minimize h, the time interval between nodes.""" return free[-1] def obj_grad(free): grad = np.zeros_like(free) grad[-1] = 1.0 return grad .. GENERATED FROM PYTHON SOURCE LINES 167-168 Starting and final location of the load. .. GENERATED FROM PYTHON SOURCE LINES 168-171 .. code-block:: Python starting_location = 0.0 ending_location = 15.0 .. GENERATED FROM PYTHON SOURCE LINES 172-173 Form the instance constraints. .. GENERATED FROM PYTHON SOURCE LINES 173-194 .. code-block:: Python instance_constraints = ( xc.func(t0) - starting_location, xl.func(t0) - starting_location, yl.func(t0) + par_map[l], q.func(t0), uxc.func(t0), uxl.func(t0), uyl.func(t0), u.func(t0), xc.func(tf) - ending_location, xl.func(tf) - ending_location, yl.func(tf) + par_map[l], q.func(tf), uxc.func(tf), uxl.func(tf), uyl.func(tf), u.func(tf), h1.func(tf), h2.func(tf), ) .. GENERATED FROM PYTHON SOURCE LINES 195-196 Forcing h > 0.0 sometimes avoids negative 'solutions'. .. GENERATED FROM PYTHON SOURCE LINES 196-203 .. code-block:: Python bounds = { F: (-20., 20.), xl: (starting_location, ending_location), xc: (starting_location, ending_location), h: (0.0, 1.0), } .. GENERATED FROM PYTHON SOURCE LINES 204-205 Create the optimization problem. .. GENERATED FROM PYTHON SOURCE LINES 205-219 .. code-block:: Python prob = Problem( obj, obj_grad, eom, state_symbols, num_nodes, interval_value, known_parameter_map=par_map, instance_constraints=instance_constraints, time_symbol=t, bounds=bounds, backend='numpy', ) .. GENERATED FROM PYTHON SOURCE LINES 220-221 Reasonable initial guess. .. GENERATED FROM PYTHON SOURCE LINES 221-232 .. code-block:: Python i1 = [(ending_location - starting_location)/num_nodes*i for i in range(num_nodes)] i2 = [0.0 for _ in range(num_nodes)] i3 = i1 i4 = [-par_map[l] for _ in range(num_nodes)] i5 = [0.0 for _ in range(5*num_nodes)] i6 = list(np.zeros(2*num_nodes)) i7 = [0.01] initial_guess = np.array(i1 + i2 + i3 + i4 + i5 + i6 + i7) .. GENERATED FROM PYTHON SOURCE LINES 233-234 Use the the initial_guess given above and plot it. .. GENERATED FROM PYTHON SOURCE LINES 234-236 .. code-block:: Python _ = prob.plot_trajectories(initial_guess) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_001.png :alt: State Trajectories, Input Trajectories :srcset: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_001.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 237-239 Use the solution of a previous run if available, else the initial guess given above is used to solve the problem. .. GENERATED FROM PYTHON SOURCE LINES 239-243 .. code-block:: Python fname = f'crane_moving_a_load_{num_nodes}_nodes_solution.csv' if os.path.exists(fname): initial_guess = np.loadtxt(fname) .. GENERATED FROM PYTHON SOURCE LINES 244-245 Find the optimal solution. .. GENERATED FROM PYTHON SOURCE LINES 245-251 .. code-block:: Python solution, info = prob.solve(initial_guess) print('Message from optimizer:', info['status_msg']) _ = prob.plot_objective_value() print('Iterations needed', len(prob.obj_value)) print(f"Objective value {solution[-1]: .3e}") .. image-sg:: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_002.png :alt: Objective Value :srcset: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_002.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none Message from optimizer: b'Algorithm terminated successfully at a locally optimal point, satisfying the convergence tolerances (can be specified by options).' Iterations needed 205 Objective value 3.956e-02 .. GENERATED FROM PYTHON SOURCE LINES 252-253 Plot the accuracy of the solution. .. GENERATED FROM PYTHON SOURCE LINES 253-255 .. code-block:: Python _ = prob.plot_constraint_violations(solution) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_003.png :alt: Constraint violations :srcset: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_003.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 256-257 Plot the state trajectories. .. GENERATED FROM PYTHON SOURCE LINES 257-259 .. code-block:: Python _ = prob.plot_trajectories(solution, show_bounds=True) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_004.png :alt: State Trajectories, Input Trajectories :srcset: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_004.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 260-262 Animate the Simulation ---------------------- .. GENERATED FROM PYTHON SOURCE LINES 262-334 .. code-block:: Python fps = 10 state_vals, input_vals, _, h_sol = prob.parse_free(solution) tf = h_sol*(num_nodes - 1) t_arr = prob.time_vector(solution=solution) state_sol = CubicSpline(t_arr, state_vals.T) input_sol = CubicSpline(t_arr, input_vals.T) coordinates = P1.pos_from(O).to_matrix(N) coordinates = coordinates.row_join(P2.pos_from(O).to_matrix(N)) pl, pl_vals = zip(*par_map.items()) coords_lam = sm.lambdify((*state_symbols, *specified_symbols, *pl), coordinates, cse=True) width, height, radius = 0.5, 0.5, 0.5 def init_plot(): xmin, xmax = starting_location - 1, ending_location + 1 ymin, ymax = -par_map[l] - 1, 1 fig, ax = plt.subplots(figsize=(8, 8)) ax.set_xlim(xmin, xmax) ax.set_ylim(ymin, ymax) ax.set_aspect('equal') ax.set_xlabel('X-axis [m]') ax.set_ylabel('Y-axis [m]') ax.axhline(0., color='black', lw=1.5) ax.axvline(ending_location+radius, color='black', lw=1.0) ax.axvline(starting_location-radius, color='black', lw=1.0) ax.annotate('walls', xy=(ending_location+radius, -2), arrowprops=dict(arrowstyle='->', connectionstyle='arc3, rad=-.2', lw=0.25), xytext=(8, 0.5), fontsize=9) ax.annotate('', xy=(starting_location-radius, -1), arrowprops=dict(arrowstyle='->', connectionstyle='arc3, rad=-.2', lw=0.25), xytext=(8, 0.5)) line1, = ax.plot([], [], color='orange', lw=1) pfeil = ax.quiver([], [], [], [], color='red', scale=55, width=0.002, headwidth=8) recht = patches.Rectangle((-width/2, -height/2), width=width, height=height, fill=True, color='red', ec='black') ax.add_patch(recht) load = patches.Circle((0, -par_map[l]), radius=radius, fill=True, color='blue', ec='black') ax.add_patch(load) return fig, ax, line1, recht, load, pfeil def update(t): message = f'running time {t:0.2f} sec \n The red arrow shows the force.' ax.set_title(message, fontsize=12) coords = coords_lam(*state_sol(t), *input_sol(t), *pl_vals) line1.set_data([coords[0, 0], coords[0, 1]], [coords[1, 0], coords[1, 1]]) recht.set_xy((coords[0, 0] - width/2., coords[1, 0] - height/2.)) load.set_center((coords[0, 1], coords[1, 1])) pfeil.set_offsets([coords[0, 0], coords[1, 0]+0.25]) pfeil.set_UVC(input_sol(t)[0], 0.25) return line1, recht, load, pfeil .. GENERATED FROM PYTHON SOURCE LINES 335-336 A frame from the animation. .. GENERATED FROM PYTHON SOURCE LINES 336-339 .. code-block:: Python fig, ax, line1, recht, load, pfeil = init_plot() _ = update(2.0) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_005.png :alt: running time 2.00 sec The red arrow shows the force. :srcset: /examples/intermediate/images/sphx_glr_plot_crane_moving_a_load_005.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 341-347 .. code-block:: Python fig, ax, line1, recht, load, pfeil = init_plot() anim = animation.FuncAnimation(fig, update, frames=np.arange(t0, tf, 1/fps), interval=1/fps*1000) plt.show() .. container:: sphx-glr-animation .. raw:: html
.. rst-class:: sphx-glr-timing **Total running time of the script:** (0 minutes 54.132 seconds) .. _sphx_glr_download_examples_intermediate_plot_crane_moving_a_load.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: plot_crane_moving_a_load.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: plot_crane_moving_a_load.py ` .. container:: sphx-glr-download sphx-glr-download-zip :download:`Download zipped: plot_crane_moving_a_load.zip ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_