.. DO NOT EDIT.
.. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY.
.. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE:
.. "examples/beginner/plot_path_constraints.py"
.. LINE NUMBERS ARE GIVEN BELOW.

.. only:: html

    .. note::
        :class: sphx-glr-download-link-note

        :ref:`Go to the end <sphx_glr_download_examples_beginner_plot_path_constraints.py>`
        to download the full example code.

.. rst-class:: sphx-glr-example-title

.. _sphx_glr_examples_beginner_plot_path_constraints.py:


Path Constraints
================

Objectives
----------

- Include additional path constraints in addition to the differential
  equations.

.. GENERATED FROM PYTHON SOURCE LINES 12-19

.. code-block:: Python

    import numpy as np
    import sympy as sm
    import sympy.physics.mechanics as me
    import matplotlib.pyplot as plt
    from opty import Problem
    from opty.utils import create_objective_function, MathJaxRepr








.. GENERATED FROM PYTHON SOURCE LINES 20-30

Introduction
------------

Given a set of differential equations, any number of additional equations
that are functions of the states can be appended to constrain the
trajectories. These are typically called "path constraints". Below the 6
ordinary differential equations of motion of a particle moving in space with
an applied force are given. One algebraic path constraint is added to
restrict the particle to being on the surface of a cylinder: Pythagoras's
theorem :math:`x^2 + y^2 = r^2`.

.. GENERATED FROM PYTHON SOURCE LINES 30-50

.. code-block:: Python

    m, r = sm.symbols('m, r', real=True)
    x, y, z = me.dynamicsymbols('x, y, z', real=True)
    vx, vy, vz = me.dynamicsymbols('v_x, v_y, v_z', real=True)
    Fx, Fy, Fz = me.dynamicsymbols('F_x, F_y, F_z', real=True)
    t = me.dynamicsymbols._t

    states = (x, y, z, vx, vy, vz)
    specifieds = (Fx, Fy, Fz)

    eom = sm.Matrix([
        x.diff() - vx,
        y.diff() - vy,
        z.diff() - vz,
        m*vx.diff() - Fx,
        m*vy.diff() - Fy,
        m*vz.diff() - Fz,
        x**2 + y**2 - r**2,
    ])
    MathJaxRepr(eom)






.. raw:: html

    <div class="output_subarea output_html rendered_html output_result">
    $$\left[\begin{matrix}- v_{x} + \dot{x}\\- v_{y} + \dot{y}\\- v_{z} + \dot{z}\\m \dot{v}_{x} - F_{x}\\m \dot{v}_{y} - F_{y}\\m \dot{v}_{z} - F_{z}\\- r^{2} + x^{2} + y^{2}\end{matrix}\right]$$
    </div>
    <br />
    <br />

.. GENERATED FROM PYTHON SOURCE LINES 51-52

Define the time and constant parameter numerical values.

.. GENERATED FROM PYTHON SOURCE LINES 52-61

.. code-block:: Python

    num_nodes = 101
    dt = 0.1
    t0, tf = 0.0, dt*(num_nodes - 1)

    par_map = {
        m: 1.0,
        r: 1.0,
    }








.. GENERATED FROM PYTHON SOURCE LINES 62-63

Minimize the average force magnitude over time.

.. GENERATED FROM PYTHON SOURCE LINES 63-69

.. code-block:: Python

    obj_func = sm.Integral(Fx**2 + Fy**2 + Fz**2, t)
    obj, obj_grad = create_objective_function(
        obj_func, states, specifieds, tuple(), num_nodes,
        dt, time_symbol=t)
    MathJaxRepr(obj_func)






.. raw:: html

    <div class="output_subarea output_html rendered_html output_result">
    $$\int \left(F_{x}^{2} + F_{y}^{2} + F_{z}^{2}\right)\, dt$$
    </div>
    <br />
    <br />

.. GENERATED FROM PYTHON SOURCE LINES 70-72

Require that the particle make a half turn around the cylinder and rise a
specified distance, :math:`4r`, being stationary at start and stop.

.. GENERATED FROM PYTHON SOURCE LINES 72-87

.. code-block:: Python

    instance_constraints = (
        x.func(t0),
        y.func(t0) + r,
        z.func(t0),
        vx.func(t0),
        vy.func(t0),
        vz.func(t0),
        x.func(tf),
        y.func(tf) - r,
        z.func(tf) - 4*r,
        vx.func(tf),
        vy.func(tf),
        vz.func(tf),
    )








.. GENERATED FROM PYTHON SOURCE LINES 88-89

Setup and solve the problem.

.. GENERATED FROM PYTHON SOURCE LINES 89-105

.. code-block:: Python

    prob = Problem(
        obj,
        obj_grad,
        eom,
        states,
        num_nodes,
        dt,
        known_parameter_map=par_map,
        instance_constraints=instance_constraints,
        time_symbol=t,
        backend='numpy',
    )

    initial_guess = np.random.random(prob.num_free)
    solution, info = prob.solve(initial_guess)








.. GENERATED FROM PYTHON SOURCE LINES 106-107

Plot the solution trajectories.

.. GENERATED FROM PYTHON SOURCE LINES 107-109

.. code-block:: Python

    _ = prob.plot_trajectories(solution)




.. image-sg:: /examples/beginner/images/sphx_glr_plot_path_constraints_001.png
   :alt: State Trajectories, Input Trajectories
   :srcset: /examples/beginner/images/sphx_glr_plot_path_constraints_001.png
   :class: sphx-glr-single-img





.. GENERATED FROM PYTHON SOURCE LINES 110-111

Plot the constraint violations.

.. GENERATED FROM PYTHON SOURCE LINES 111-113

.. code-block:: Python

    _ = prob.plot_constraint_violations(solution)




.. image-sg:: /examples/beginner/images/sphx_glr_plot_path_constraints_002.png
   :alt: Constraint violations
   :srcset: /examples/beginner/images/sphx_glr_plot_path_constraints_002.png
   :class: sphx-glr-single-img





.. GENERATED FROM PYTHON SOURCE LINES 114-115

Show the path of the particle in 3D:

.. GENERATED FROM PYTHON SOURCE LINES 115-120

.. code-block:: Python

    xs, _, _ = prob.parse_free(solution)
    ax = plt.figure().add_subplot(projection='3d')
    ax.plot(xs[0], xs[1], xs[2])

    plt.show()



.. image-sg:: /examples/beginner/images/sphx_glr_plot_path_constraints_003.png
   :alt: plot path constraints
   :srcset: /examples/beginner/images/sphx_glr_plot_path_constraints_003.png
   :class: sphx-glr-single-img






.. rst-class:: sphx-glr-timing

   **Total running time of the script:** (0 minutes 6.597 seconds)


.. _sphx_glr_download_examples_beginner_plot_path_constraints.py:

.. only:: html

  .. container:: sphx-glr-footer sphx-glr-footer-example

    .. container:: sphx-glr-download sphx-glr-download-jupyter

      :download:`Download Jupyter notebook: plot_path_constraints.ipynb <plot_path_constraints.ipynb>`

    .. container:: sphx-glr-download sphx-glr-download-python

      :download:`Download Python source code: plot_path_constraints.py <plot_path_constraints.py>`

    .. container:: sphx-glr-download sphx-glr-download-zip

      :download:`Download zipped: plot_path_constraints.zip <plot_path_constraints.zip>`


.. only:: html

 .. rst-class:: sphx-glr-signature

    `Gallery generated by Sphinx-Gallery <https://sphinx-gallery.github.io>`_