.. DO NOT EDIT.
.. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY.
.. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE:
.. "examples/beginner/plot_betts_10_50.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_betts_10_50.py>`
        to download the full example code.

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

.. _sphx_glr_examples_beginner_plot_betts_10_50.py:


Delay Equation (Göllmann, Kern, and Maurer)
===========================================

Objectives
----------

- A simple example to show how to handle inequality constraints.
- Shows how instance constraints on one state variable may explicitly depend on
  an instance of another state variable at a different time.

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

This is example 10.50 from [Betts2010]_.

**States**

- :math:`x_1, x_2, x_3. x_4. x_5, x_6` : state variables

**Controls**

- :math:`u_1, u_2, u_3, u_4, u_5, u_6` : control variables

.. GENERATED FROM PYTHON SOURCE LINES 26-33

.. code-block:: Python

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








.. GENERATED FROM PYTHON SOURCE LINES 34-57

Equations of Motion
-------------------
There are six differential equations:

.. math::

   \dot{x}_1 = & x_0 u_{-1} \\
   \dot{x}_2 = & x_1 u_0 \\
   \dot{x}_3 = & x_2 u_1 \\
   \dot{x}_4 = & x_3 u_2 \\
   \dot{x}_5 = & x_4 u_3 \\
   \dot{x}_6 = & x_5 u_4

and six algebraic inequality constraints:

.. math::

   0.3 \leq u_1 + x_1 \\
   0.3 \leq u_2 + x_2 \\
   0.3 \leq u_3 + x_3 \\
   0.3 \leq u_4 + x_4 \\
   0.3 \leq u_5 + x_5 \\
   0.3 \leq u_6 + x_6

.. GENERATED FROM PYTHON SOURCE LINES 57-82

.. code-block:: Python

    x1, x2, x3, x4, x5, x6 = me.dynamicsymbols('x1, x2, x3, x4, x5, x6')
    u1, u2, u3, u4, u5, u6 = me.dynamicsymbols('u1, u2, u3, u4, u5, u6')
    t = me.dynamicsymbols._t

    x0 = 1.0
    u_minus_1, u0 = 0.0, 0.0

    eom = sm.Matrix([
        # equality constraints
        -x1.diff(t) + x0*u_minus_1,
        -x2.diff(t) + x1*u0,
        -x3.diff(t) + x2*u1,
        -x4.diff(t) + x3*u2,
        -x5.diff(t) + x4*u3,
        -x6.diff(t) + x5*u4,
        # inequality constraints
        u1 + x1,
        u2 + x2,
        u3 + x3,
        u4 + x4,
        u5 + x5,
        u6 + x6,
    ])
    MathJaxRepr(eom)






.. raw:: html

    <div class="output_subarea output_html rendered_html output_result">
    $$\left[\begin{matrix}- \dot{x}_{1}\\- \dot{x}_{2}\\u_{1} x_{2} - \dot{x}_{3}\\u_{2} x_{3} - \dot{x}_{4}\\u_{3} x_{4} - \dot{x}_{5}\\u_{4} x_{5} - \dot{x}_{6}\\u_{1} + x_{1}\\u_{2} + x_{2}\\u_{3} + x_{3}\\u_{4} + x_{4}\\u_{5} + x_{5}\\u_{6} + x_{6}\end{matrix}\right]$$
    </div>
    <br />
    <br />

.. GENERATED FROM PYTHON SOURCE LINES 83-85

Define and Solve the Optimization Problem
-----------------------------------------

.. GENERATED FROM PYTHON SOURCE LINES 85-92

.. code-block:: Python

    num_nodes = 501
    t0, tf = 0.0, 1.0
    interval_value = (tf - t0)/(num_nodes - 1)

    state_symbols = (x1, x2, x3, x4, x5, x6)
    unknown_input_trajectories = (u1, u2, u3, u4, u5, u6)








.. GENERATED FROM PYTHON SOURCE LINES 93-94

Specify the objective function and form the gradient.

.. GENERATED FROM PYTHON SOURCE LINES 94-110

.. code-block:: Python

    objective = sm.Integral(
        x1**2 + x2**2 + x3**2 + x4**2 + x5**2 + x6**2 +
        u1**2 + u2**2 + u3**2 + u4**2 + u5**2 + u6**2, t)

    obj, obj_grad = create_objective_function(
        objective,
        state_symbols,
        unknown_input_trajectories,
        tuple(),
        num_nodes,
        interval_value,
        time_symbol=t,
    )

    MathJaxRepr(objective)






.. raw:: html

    <div class="output_subarea output_html rendered_html output_result">
    $$\int \left(u_{1}^{2} + u_{2}^{2} + u_{3}^{2} + u_{4}^{2} + u_{5}^{2} + u_{6}^{2} + x_{1}^{2} + x_{2}^{2} + x_{3}^{2} + x_{4}^{2} + x_{5}^{2} + x_{6}^{2}\right)\, dt$$
    </div>
    <br />
    <br />

.. GENERATED FROM PYTHON SOURCE LINES 111-112

Specify the instance constraints

.. GENERATED FROM PYTHON SOURCE LINES 112-127

.. code-block:: Python

    instance_constraints = (
        x1.func(t0) - 1.0,
        x2.func(t0) - x1.func(tf),
        x3.func(t0) - x2.func(tf),
        x4.func(t0) - x3.func(tf),
        x5.func(t0) - x4.func(tf),
        x6.func(t0) - x5.func(tf),
        u1.func(t0) + x1.func(t0) - 0.5,
        u2.func(t0) + x2.func(t0) - 0.5,
        u3.func(t0) + x3.func(t0) - 0.5,
        u4.func(t0) + x4.func(t0) - 0.5,
        u5.func(t0) + x5.func(t0) - 0.5,
        u6.func(t0) + x6.func(t0) - 0.5,
    )








.. GENERATED FROM PYTHON SOURCE LINES 128-131

Specify the bounds on the inequality constraints in the equations of motion.
The key should match the corresponding index of the equation to apply the
bounds to.

.. GENERATED FROM PYTHON SOURCE LINES 131-140

.. code-block:: Python

    eom_bounds = {
        6: (0.3, np.inf),
        7: (0.3, np.inf),
        8: (0.3, np.inf),
        9: (0.3, np.inf),
        10: (0.3, np.inf),
        11: (0.3, np.inf),
    }








.. GENERATED FROM PYTHON SOURCE LINES 141-143

Solve the Optimization Problem
------------------------------

.. GENERATED FROM PYTHON SOURCE LINES 143-168

.. code-block:: Python

    prob = Problem(
        obj,
        obj_grad,
        eom,
        state_symbols,
        num_nodes,
        interval_value,
        instance_constraints=instance_constraints,
        time_symbol=t,
        backend='numpy',
        eom_bounds=eom_bounds,
    )

    prob.add_option('max_iter', 1000)

    initial_guess = np.random.rand(prob.num_free)*0.1

    solution, info = prob.solve(initial_guess)
    initial_guess = solution
    print(info['status_msg'])
    print(f'Objective value achieved: {info["obj_val"]: .4f}, as per the book '
          f'it is {3.10812211}, so the error is: '
          f'{(info["obj_val"] - 3.10812211)/3.10812211*100: .3f} % ')
    print('\n')





.. 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).'
    Objective value achieved:  3.1073, as per the book it is 3.10812211, so the error is: -0.027 % 






.. GENERATED FROM PYTHON SOURCE LINES 169-170

Plot the optimal state and input trajectories.

.. GENERATED FROM PYTHON SOURCE LINES 170-172

.. code-block:: Python

    _ = prob.plot_trajectories(solution)




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





.. GENERATED FROM PYTHON SOURCE LINES 173-175

Plot the constraint violations.
sphinx_gallery_thumbnail_number = 2

.. GENERATED FROM PYTHON SOURCE LINES 175-177

.. code-block:: Python

    _ = prob.plot_constraint_violations(solution)




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





.. GENERATED FROM PYTHON SOURCE LINES 178-179

Plot the objective function.

.. GENERATED FROM PYTHON SOURCE LINES 179-182

.. code-block:: Python

    _ = prob.plot_objective_value()

    plt.show()



.. image-sg:: /examples/beginner/images/sphx_glr_plot_betts_10_50_003.png
   :alt: Objective Value
   :srcset: /examples/beginner/images/sphx_glr_plot_betts_10_50_003.png
   :class: sphx-glr-single-img






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

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


.. _sphx_glr_download_examples_beginner_plot_betts_10_50.py:

.. only:: html

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

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

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

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

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

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

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


.. only:: html

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

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