.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples/intermediate/plot_drone.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_drone.py: Drone Flight ============ Objectives ---------- - Show how to add extra algebraic contraints (in this case, the quaternion magnitude). - Demonstrate automatically generating a linear initial guess between instance constraints. Introduction ------------ Given a cuboid shaped drone of dimensions l x w x d with propellers at each corner in a uniform gravitational field, find the propeller thrust trajectories that will take it from a starting point to an ending point and through and intermediate point at a specific angular configuration with minimal total thrust. **Constants** - m : drone mass, [kg] - l : length (along body x) [m] - w : width (along body y) [m] - d : depth (along body z) [m] - c : viscous friction coefficient of air [Nms] **States** - x, y, z : position of mass center [m] - v1, v2, v3 : body fixed speed of mass center [m] - q0, q1, q2, q3 : quaternion measure numbers [rad] - wx, wy, wz: body fixed angular rates [rad/s] **Specifieds** - F1, F2, F3, F4 : propeller propulsion forces [N] .. GENERATED FROM PYTHON SOURCE LINES 42-50 .. code-block:: Python import sympy as sm import sympy.physics.mechanics as me import numpy as np from opty import Problem, create_objective_function import matplotlib.pyplot as plt import matplotlib.animation as animation .. GENERATED FROM PYTHON SOURCE LINES 51-52 Generate the equations of motion of the system. .. GENERATED FROM PYTHON SOURCE LINES 52-122 .. code-block:: Python m, l, w, d, g, c = sm.symbols('m, l, w, d, g, c', real=True) x, y, z, vx, vy, vz = me.dynamicsymbols('x, y, z, v_x, v_y v_z', real=True) q0, q1, q2, q3 = me.dynamicsymbols('q0, q1, q2, q3', real=True) u0, wx, wy, wz = me.dynamicsymbols('u0, omega_x, omega_y, omega_z', real=True) F1, F2, F3, F4 = me.dynamicsymbols('F1, F2, F3, F4', real=True) t = me.dynamicsymbols._t O, Ao, P1, P2, P3, P4 = sm.symbols('O, A_o, P1, P2, P3, P4', cls=me.Point) N, A = sm.symbols('N, A', cls=me.ReferenceFrame) A.orient_quaternion(N, (q0, q1, q2, q3)) Ao.set_pos(O, x*N.x + y*N.y + z*N.z) P1.set_pos(Ao, l/2*A.x + w/2*A.y) P2.set_pos(Ao, -l/2*A.x + w/2*A.y) P3.set_pos(Ao, l/2*A.x - w/2*A.y) P4.set_pos(Ao, -l/2*A.x - w/2*A.y) N_w_A = A.ang_vel_in(N) N_v_P = Ao.pos_from(O).dt(N) kinematical = sm.Matrix([ vx - N_v_P.dot(A.x), vy - N_v_P.dot(A.y), vz - N_v_P.dot(A.z), u0 - q0.diff(t), wx - N_w_A.dot(A.x), wy - N_w_A.dot(A.y), wz - N_w_A.dot(A.z), ]) A.set_ang_vel(N, wx*A.x + wy*A.y + wz*A.z) O.set_vel(N, 0) Ao.set_vel(N, vx*A.x + vy*A.y + vz*A.z) P1.v2pt_theory(Ao, N, A) P2.v2pt_theory(Ao, N, A) P3.v2pt_theory(Ao, N, A) P4.v2pt_theory(Ao, N, A) # x: l, y: w, z: d IA = me.inertia(A, m*(w**2 + d**2)/12, m*(l**2 + d**2)/12, m*(l**2 + w**2)/12) drone = me.RigidBody('A', Ao, A, m, (IA, Ao)) prop1 = (P1, F1*A.z) prop2 = (P2, F2*A.z) prop3 = (P3, F3*A.z) prop4 = (P4, F4*A.z) # use a linear simplification of air drag for continuous derivatives grav = (Ao, -m*g*N.z - c*Ao.vel(N)) # enforce the unit quaternion holonomic = sm.Matrix([q0**2 + q1**2 + q2**2 + q3**2 - 1]) kane = me.KanesMethod( N, (x, y, z, q1, q2, q3), (vx, vy, vz, wx, wy, wz), kd_eqs=kinematical, q_dependent=(q0,), u_dependent=(u0,), configuration_constraints=holonomic, velocity_constraints=holonomic.diff(t), ) fr, frstar = kane.kanes_equations([drone], [prop1, prop2, prop3, prop4, grav]) eom = kinematical.col_join(fr + frstar).col_join(holonomic) sm.pprint(eom) .. rst-class:: sphx-glr-script-out .. code-block:: none ⎡ d ↪ ⎢- (-2⋅q₀(t)⋅q₂(t) + 2⋅q₁(t)⋅q₃(t))⋅──(z(t)) - (2⋅q₀(t)⋅q₃(t) + 2⋅q₁(t)⋅q₂(t)) ↪ ⎢ dt ↪ ⎢ ↪ ⎢ d ↪ ⎢- (2⋅q₀(t)⋅q₁(t) + 2⋅q₂(t)⋅q₃(t))⋅──(z(t)) - (-2⋅q₀(t)⋅q₃(t) + 2⋅q₁(t)⋅q₂(t)) ↪ ⎢ dt ↪ ⎢ ↪ ⎢ d ↪ ⎢- (-2⋅q₀(t)⋅q₁(t) + 2⋅q₂(t)⋅q₃(t))⋅──(y(t)) - (2⋅q₀(t)⋅q₂(t) + 2⋅q₁(t)⋅q₃(t)) ↪ ⎢ dt ↪ ⎢ ↪ ⎢ d ↪ ⎢ u₀(t) - ──(q₀(t ↪ ⎢ dt ↪ ⎢ ↪ ⎢ d d ↪ ⎢ ωₓ(t) - 2⋅q₀(t)⋅──(q₁(t)) + 2⋅q₁(t)⋅──(q₀(t)) + 2 ↪ ⎢ dt dt ↪ ⎢ ↪ ⎢ d d ↪ ⎢ ω_y(t) - 2⋅q₀(t)⋅──(q₂(t)) - 2⋅q₁(t)⋅──(q₃(t)) + ↪ ⎢ dt dt ↪ ⎢ ↪ ⎢ d d ↪ ⎢ ω_z(t) - 2⋅q₀(t)⋅──(q₃(t)) + 2⋅q₁(t)⋅──(q₂(t)) - ↪ ⎢ dt dt ↪ ⎢ ↪ ⎢ ↪ ⎢ -c⋅vₓ(t) - g⋅m⋅(-2⋅q₀(t)⋅q₂(t) + 2⋅q₁(t)⋅q₃(t)) - m⋅(ω_ ↪ ⎢ ↪ ⎢ ↪ ⎢ ↪ ⎢ -c⋅v_y(t) - g⋅m⋅(2⋅q₀(t)⋅q₁(t) + 2⋅q₂(t)⋅q₃(t)) - m⋅(-ω ↪ ⎢ ↪ ⎢ ↪ ⎢ ⎛ 2 2 2 2 ⎞ ↪ ⎢ -c⋅v_z(t) - g⋅m⋅⎝q₀ (t) - q₁ (t) - q₂ (t) + q₃ (t)⎠ - m⋅(ωₓ(t)⋅v_y(t) - ↪ ⎢ ↪ ⎢ ↪ ⎢ ⎛ 2 2⎞ d ↪ ⎢ ⎛ 2 2⎞ m⋅⎝d + w ⎠⋅──(ωₓ(t)) ⎛ 2 2⎞ ↪ ⎢ m⋅⎝d + l ⎠⋅ω_y(t)⋅ω_z(t) dt m⋅⎝l + w ⎠⋅ω ↪ ⎢ ───────────────────────── - ───────────────────── - ───────────── ↪ ⎢ 12 12 12 ↪ ⎢ ↪ ⎢ ⎛ 2 2⎞ d ↪ ⎢ m⋅⎝d + l ⎠⋅──(ω_y(t)) ↪ ⎢ l⋅F₁(t) l⋅F₂(t) l⋅F₃(t) l⋅F₄(t) dt ↪ ⎢ - ─────── + ─────── - ─────── + ─────── - ────────────────────── ↪ ⎢ 2 2 2 2 12 ↪ ⎢ ↪ ⎢ ↪ ⎢ ⎛ 2 2⎞ ⎛ 2 2⎞ ↪ ⎢ m⋅⎝d + l ⎠⋅ωₓ(t)⋅ω_y(t) m⋅⎝d + w ⎠⋅ωₓ(t ↪ ⎢ - ──────────────────────── + ──────────────── ↪ ⎢ 12 12 ↪ ⎢ ↪ ⎢ 2 2 2 ↪ ⎣ q₀ (t) + q₁ (t) + q₂ (t) ↪ ↪ d ⎛ 2 2 2 2 ⎞ d ⎤ ↪ ⋅──(y(t)) - ⎝q₀ (t) + q₁ (t) - q₂ (t) - q₃ (t)⎠⋅──(x(t)) + vₓ(t) ⎥ ↪ dt dt ⎥ ↪ ⎥ ↪ d ⎛ 2 2 2 2 ⎞ d ⎥ ↪ ⋅──(x(t)) - ⎝q₀ (t) - q₁ (t) + q₂ (t) - q₃ (t)⎠⋅──(y(t)) + v_y(t)⎥ ↪ dt dt ⎥ ↪ ⎥ ↪ d ⎛ 2 2 2 2 ⎞ d ⎥ ↪ ⋅──(x(t)) - ⎝q₀ (t) - q₁ (t) - q₂ (t) + q₃ (t)⎠⋅──(z(t)) + v_z(t)⎥ ↪ dt dt ⎥ ↪ ⎥ ↪ ⎥ ↪ )) ⎥ ↪ ⎥ ↪ ⎥ ↪ d d ⎥ ↪ ⋅q₂(t)⋅──(q₃(t)) - 2⋅q₃(t)⋅──(q₂(t)) ⎥ ↪ dt dt ⎥ ↪ ⎥ ↪ d d ⎥ ↪ 2⋅q₂(t)⋅──(q₀(t)) + 2⋅q₃(t)⋅──(q₁(t)) ⎥ ↪ dt dt ⎥ ↪ ⎥ ↪ d d ⎥ ↪ 2⋅q₂(t)⋅──(q₁(t)) + 2⋅q₃(t)⋅──(q₀(t)) ⎥ ↪ dt dt ⎥ ↪ ⎥ ↪ d ⎥ ↪ y(t)⋅v_z(t) - ω_z(t)⋅v_y(t)) - m⋅──(vₓ(t)) ⎥ ↪ dt ⎥ ↪ ⎥ ↪ d ⎥ ↪ ₓ(t)⋅v_z(t) + ω_z(t)⋅vₓ(t)) - m⋅──(v_y(t)) ⎥ ↪ dt ⎥ ↪ ⎥ ↪ d ⎥ ↪ ω_y(t)⋅vₓ(t)) - m⋅──(v_z(t)) + F₁(t) + F₂(t) + F₃(t) + F₄(t) ⎥ ↪ dt ⎥ ↪ ⎥ ↪ ⎥ ↪ ⎥ ↪ _y(t)⋅ω_z(t) w⋅F₁(t) w⋅F₂(t) w⋅F₃(t) w⋅F₄(t) ⎥ ↪ ──────────── + ─────── + ─────── - ─────── - ─────── ⎥ ↪ 2 2 2 2 ⎥ ↪ ⎥ ↪ ⎥ ↪ ⎛ 2 2⎞ ⎛ 2 2⎞ ⎥ ↪ m⋅⎝d + w ⎠⋅ωₓ(t)⋅ω_z(t) m⋅⎝l + w ⎠⋅ωₓ(t)⋅ω_z(t) ⎥ ↪ - ──────────────────────── + ──────────────────────── ⎥ ↪ 12 12 ⎥ ↪ ⎥ ↪ ⎛ 2 2⎞ d ⎥ ↪ m⋅⎝l + w ⎠⋅──(ω_z(t)) ⎥ ↪ )⋅ω_y(t) dt ⎥ ↪ ──────── - ────────────────────── ⎥ ↪ 12 ⎥ ↪ ⎥ ↪ 2 ⎥ ↪ + q₃ (t) - 1 ⎦ .. GENERATED FROM PYTHON SOURCE LINES 123-124 Set up the time discretization. .. GENERATED FROM PYTHON SOURCE LINES 124-128 .. code-block:: Python duration = 10.0 # seconds num_nodes = 301 interval_value = duration/(num_nodes - 1) .. GENERATED FROM PYTHON SOURCE LINES 129-130 Provide some values for the constants. .. GENERATED FROM PYTHON SOURCE LINES 130-142 .. code-block:: Python par_map = { c: 0.5*0.1*1.2, d: 0.1, g: 9.81, l: 1.0, m: 2.0, w: 0.5, } state_symbols = (x, y, z, q0, q1, q2, q3, vx, vy, vz, u0, wx, wy, wz) specified_symbols = (F1, F2, F3, F4) .. GENERATED FROM PYTHON SOURCE LINES 143-144 Specify the objective function and form the gradient. .. GENERATED FROM PYTHON SOURCE LINES 144-154 .. code-block:: Python obj_func = sm.Integral(F1**2 + F2**2 + F3**2 + F4**2, t) sm.pprint(obj_func) obj, obj_grad = create_objective_function(obj_func, state_symbols, specified_symbols, tuple(), num_nodes, interval_value, time_symbol=t) .. rst-class:: sphx-glr-script-out .. code-block:: none ⌠ ⎮ ⎛ 2 2 2 2 ⎞ ⎮ ⎝F₁ (t) + F₂ (t) + F₃ (t) + F₄ (t)⎠ dt ⌡ .. GENERATED FROM PYTHON SOURCE LINES 155-156 Specify the symbolic instance constraints. .. GENERATED FROM PYTHON SOURCE LINES 156-196 .. code-block:: Python instance_constraints = ( # move from (0, 0, 0) to (10, 10, 10) meters x.func(0.0), y.func(0.0), z.func(0.0), x.func(duration) - 10.0, y.func(duration) - 10.0, z.func(duration) - 10.0, # start level q0.func(0.0) - 1.0, q1.func(0.0), q2.func(0.0), q3.func(0.0), # rotate 90 degrees about x at midpoint in time q0.func(duration/2) - np.cos(np.pi/4), q1.func(duration/2) - np.sin(np.pi/4), q2.func(duration/2), q3.func(duration/2), # end level q0.func(duration) - 1.0, q1.func(duration), q2.func(duration), q3.func(duration), # stationary at start and finish vx.func(0.0), vy.func(0.0), vz.func(0.0), u0.func(0.0), wx.func(0.0), wy.func(0.0), wz.func(0.0), vx.func(duration), vy.func(duration), vz.func(duration), u0.func(duration), wx.func(duration), wy.func(duration), wz.func(duration), ) .. GENERATED FROM PYTHON SOURCE LINES 197-198 Add some physical limits to the propeller thrust. .. GENERATED FROM PYTHON SOURCE LINES 198-205 .. code-block:: Python bounds = { F1: (-100.0, 100.0), F2: (-100.0, 100.0), F3: (-100.0, 100.0), F4: (-100.0, 100.0), } .. GENERATED FROM PYTHON SOURCE LINES 206-207 Create the optimization problem and set any options. .. GENERATED FROM PYTHON SOURCE LINES 207-215 .. code-block:: Python prob = Problem(obj, obj_grad, eom, state_symbols, num_nodes, interval_value, known_parameter_map=par_map, instance_constraints=instance_constraints, bounds=bounds) prob.add_option('nlp_scaling_method', 'gradient-based') .. GENERATED FROM PYTHON SOURCE LINES 216-217 Give a guess of a direct route with constant thrust. .. GENERATED FROM PYTHON SOURCE LINES 217-225 .. code-block:: Python initial_guess = prob.create_linear_initial_guess() initial_guess[-4*num_nodes:] = 10.0 # constant thrust fig, axes = plt.subplots(18, 1, sharex=True, figsize=(6.4, 0.8*18), layout='compressed') _ = prob.plot_trajectories(initial_guess, axes=axes) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_drone_001.png :alt: State Trajectories, Input Trajectories :srcset: /examples/intermediate/images/sphx_glr_plot_drone_001.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 226-227 Find an optimal solution. .. GENERATED FROM PYTHON SOURCE LINES 227-232 .. code-block:: Python solution, info = prob.solve(initial_guess) time = prob.time_vector() print(info['status_msg']) print(info['obj_val']) .. 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).' 1015.4067147739491 .. GENERATED FROM PYTHON SOURCE LINES 233-234 Plot the optimal state and input trajectories. .. GENERATED FROM PYTHON SOURCE LINES 234-239 .. code-block:: Python fig, axes = plt.subplots(18, 1, sharex=True, figsize=(6.4, 0.8*18), layout='compressed') _ = prob.plot_trajectories(solution, axes=axes) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_drone_002.png :alt: State Trajectories, Input Trajectories :srcset: /examples/intermediate/images/sphx_glr_plot_drone_002.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 240-241 Plot the constraint violations. .. GENERATED FROM PYTHON SOURCE LINES 241-245 .. code-block:: Python fig, axes = plt.subplots(5, 1, figsize=(12.8, 10), layout='constrained') _ = prob.plot_constraint_violations(solution, axes=axes) .. image-sg:: /examples/intermediate/images/sphx_glr_plot_drone_003.png :alt: Constraint violations :srcset: /examples/intermediate/images/sphx_glr_plot_drone_003.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 246-247 Plot the objective function as a function of optimizer iteration. .. GENERATED FROM PYTHON SOURCE LINES 247-249 .. code-block:: Python _ = prob.plot_objective_value() .. image-sg:: /examples/intermediate/images/sphx_glr_plot_drone_004.png :alt: Objective Value :srcset: /examples/intermediate/images/sphx_glr_plot_drone_004.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 250-251 Animate the motion of the drone. .. GENERATED FROM PYTHON SOURCE LINES 251-310 .. code-block:: Python coordinates = Ao.pos_from(O).to_matrix(N) for point in [P1, Ao, P2, Ao, P3, Ao, P4]: coordinates = coordinates.row_join(point.pos_from(O).to_matrix(N)) eval_point_coords = sm.lambdify((state_symbols, specified_symbols, list(par_map.keys())), coordinates, cse=True) xs, us, ps = prob.parse_free(solution) coords = [] for xi, ui in zip(xs.T, us.T): coords.append(eval_point_coords(xi, ui, list(par_map.values()))) coords = np.array(coords) # shape(n, 3, 8) def frame(i): fig = plt.figure() ax = fig.add_subplot(projection='3d') x, y, z = eval_point_coords(xs[:, i], us[:, i], list(par_map.values())) drone_lines, = ax.plot(x, y, z, color='black', marker='o', markerfacecolor='blue', markersize=4) P1_path, = ax.plot(coords[:i, 0, 1], coords[:i, 1, 1], coords[:i, 2, 1]) P2_path, = ax.plot(coords[:i, 0, 3], coords[:i, 1, 3], coords[:i, 2, 3]) P3_path, = ax.plot(coords[:i, 0, 5], coords[:i, 1, 5], coords[:i, 2, 5]) P4_path, = ax.plot(coords[:i, 0, 7], coords[:i, 1, 7], coords[:i, 2, 7]) title_template = 'Time = {:1.2f} s' title_text = ax.set_title(title_template.format(time[i])) ax.set_xlim((np.min(coords[:, 0, :]) - 0.2, np.max(coords[:, 0, :]) + 0.2)) ax.set_ylim((np.min(coords[:, 1, :]) - 0.2, np.max(coords[:, 1, :]) + 0.2)) ax.set_zlim((np.min(coords[:, 2, :]) - 0.2, np.max(coords[:, 2, :]) + 0.2)) ax.set_xlabel(r'$x$ [m]') ax.set_ylabel(r'$y$ [m]') ax.set_zlabel(r'$z$ [m]') return fig, title_text, drone_lines, P1_path, P2_path, P3_path, P4_path fig, title_text, drone_lines, P1_path, P2_path, P3_path, P4_path = frame(0) def animate(i): title_text.set_text('Time = {:1.2f} s'.format(time[i])) drone_lines.set_data_3d(coords[i, 0, :], coords[i, 1, :], coords[i, 2, :]) P1_path.set_data_3d(coords[:i, 0, 1], coords[:i, 1, 1], coords[:i, 2, 1]) P2_path.set_data_3d(coords[:i, 0, 3], coords[:i, 1, 3], coords[:i, 2, 3]) P3_path.set_data_3d(coords[:i, 0, 5], coords[:i, 1, 5], coords[:i, 2, 5]) P4_path.set_data_3d(coords[:i, 0, 7], coords[:i, 1, 7], coords[:i, 2, 7]) ani = animation.FuncAnimation(fig, animate, range(0, len(time), 2), interval=int(interval_value*2000)) .. container:: sphx-glr-animation .. raw:: html
.. GENERATED FROM PYTHON SOURCE LINES 311-312 A frame from the animation. .. GENERATED FROM PYTHON SOURCE LINES 312-316 .. code-block:: Python frame(num_nodes - num_nodes//4) plt.show() .. image-sg:: /examples/intermediate/images/sphx_glr_plot_drone_006.png :alt: Time = 7.53 s :srcset: /examples/intermediate/images/sphx_glr_plot_drone_006.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-timing **Total running time of the script:** (1 minutes 49.013 seconds) .. _sphx_glr_download_examples_intermediate_plot_drone.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: plot_drone.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: plot_drone.py ` .. container:: sphx-glr-download sphx-glr-download-zip :download:`Download zipped: plot_drone.zip ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_