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

.. only:: html

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

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

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

.. _sphx_glr_examples_plot_betts_10_43_opty.py:


Free-Flying Robot (betts_10_43)
===============================

This is example 10.43 from *Betts, Practical Methods for Optimal Control
Using NonlinearProgramming*, 3rd edition, Chapter 10: Test Problems.

More details are in chapter 8.13, example 8.17 of this book.

**States**

- :math:`y_0, ...y_5`: state variables

**Specifieds**

- :math:`u_0, ..., u_3` : control variables

.. GENERATED FROM PYTHON SOURCE LINES 20-29

.. code-block:: Python


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









.. GENERATED FROM PYTHON SOURCE LINES 30-31

Equations of motion.

.. GENERATED FROM PYTHON SOURCE LINES 31-51

.. code-block:: Python

    t = me.dynamicsymbols._t
    y = me.dynamicsymbols(f'y:{6}')
    u = me.dynamicsymbols(f'u:{4}')
    uy = [y[i].diff(t) for i in range(6)]

    alpha, beta = sm.symbols('alpha beta', real=True)

    eom = sm.Matrix([
        -uy[0] + y[3],
        -uy[1] + y[4],
        -uy[2] + y[5],
        -uy[3] + (u[0]-u[1]+u[2]-u[3]) * sm.cos(y[2]),
        -uy[4] + (u[0]-u[1]+u[2]-u[3]) * sm.sin(y[2]),
        -uy[5] + alpha*(u[0]-u[1]) - beta*(u[2]-u[3]),
        u[0] + u[1],
        u[2] + u[3],
    ])

    MathJaxRepr(eom)






.. raw:: html

    <div class="output_subarea output_html rendered_html output_result">
    $$\left[\begin{matrix}y_{3} - \dot{y}_{0}\\y_{4} - \dot{y}_{1}\\y_{5} - \dot{y}_{2}\\\left(u_{0} - u_{1} + u_{2} - u_{3}\right) \cos{\left(y_{2} \right)} - \dot{y}_{3}\\\left(u_{0} - u_{1} + u_{2} - u_{3}\right) \sin{\left(y_{2} \right)} - \dot{y}_{4}\\\alpha \left(u_{0} - u_{1}\right) - \beta \left(u_{2} - u_{3}\right) - \dot{y}_{5}\\u_{0} + u_{1}\\u_{2} + u_{3}\end{matrix}\right]$$
    </div>
    <br />
    <br />

.. GENERATED FROM PYTHON SOURCE LINES 52-54

Set Up the Optimization Problem and Solve It
--------------------------------------------

.. GENERATED FROM PYTHON SOURCE LINES 54-61

.. code-block:: Python

    t0, tf = 0.0, 12.0
    num_nodes = 2001
    interval_value = (tf - t0)/(num_nodes - 1)

    state_symbols = y
    specified_symbols = u








.. GENERATED FROM PYTHON SOURCE LINES 62-63

Specify the objective function and form the gradient.

.. GENERATED FROM PYTHON SOURCE LINES 63-73

.. code-block:: Python

    start = time.time()
    obj_func = sm.Integral(sum([u[i] for i in range(4)]), t)
    obj, obj_grad = create_objective_function(
        obj_func,
        state_symbols,
        specified_symbols,
        tuple(),
        num_nodes,
        node_time_interval=interval_value)








.. GENERATED FROM PYTHON SOURCE LINES 74-75

Specify the symbolic instance constraints, the bounds and known parameters.

.. GENERATED FROM PYTHON SOURCE LINES 75-102

.. code-block:: Python

    instance_constraints = (
        y[0].func(t0) + 10.0,
        y[1].func(t0) + 10.0,
        y[2].func(t0) - np.pi/2,
        y[3].func(t0),
        y[4].func(t0),
        y[5].func(t0),
        y[0].func(tf),
        y[1].func(tf),
        y[2].func(tf),
        y[3].func(tf),
        y[4].func(tf),
        y[5].func(tf),
    )

    bounds = {u[i]: (0.0, 1.0) for i in range(4)}

    eom_bounds = {
        6: (-np.inf, 1.0),
        7: (-np.inf, 1.0),
    }

    par_map = {
        alpha: 0.2,
        beta: 0.2,
    }








.. GENERATED FROM PYTHON SOURCE LINES 103-104

Create the optimization problem.

.. GENERATED FROM PYTHON SOURCE LINES 104-118

.. code-block:: Python

    prob = Problem(
        obj,
        obj_grad,
        eom,
        state_symbols,
        num_nodes,
        interval_value,
        instance_constraints=instance_constraints,
        known_parameter_map=par_map,
        bounds=bounds,
        eom_bounds=eom_bounds,
        time_symbol=t,
    )








.. GENERATED FROM PYTHON SOURCE LINES 119-120

Give some rough estimates for the trajectories.

.. GENERATED FROM PYTHON SOURCE LINES 120-122

.. code-block:: Python

    initial_guess = np.zeros(prob.num_free)








.. GENERATED FROM PYTHON SOURCE LINES 123-124

Find the optimal solution.

.. GENERATED FROM PYTHON SOURCE LINES 124-134

.. code-block:: Python

    start = time.time()
    solution, info = prob.solve(initial_guess)
    end = time.time()
    print(info['status_msg'])
    Jstar = 7.91055654
    print(f"Objective value achieved: {info['obj_val']:.4f}, as per the book "
          f"it is {Jstar:.4f}, so the deviation is: "
          f"{(info['obj_val'] - Jstar)/Jstar*100:.3f} % ")
    print(f"Time taken for the simulation: {end - start:.2f} s")





.. 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: 7.6918, as per the book it is 7.9106, so the deviation is: -2.765 % 
    Time taken for the simulation: 82.41 s




.. GENERATED FROM PYTHON SOURCE LINES 135-136

Plot the optimal state and input trajectories.

.. GENERATED FROM PYTHON SOURCE LINES 136-138

.. code-block:: Python

    _ = prob.plot_trajectories(solution, show_bounds=True)




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





.. GENERATED FROM PYTHON SOURCE LINES 139-140

Plot the constraint violations.

.. GENERATED FROM PYTHON SOURCE LINES 140-142

.. code-block:: Python

    _ = prob.plot_constraint_violations(solution, subplots=True, show_bounds=True)




.. image-sg:: /examples/images/sphx_glr_plot_betts_10_43_opty_002.png
   :alt: Constraint violations  Values of bounded EoMs
   :srcset: /examples/images/sphx_glr_plot_betts_10_43_opty_002.png
   :class: sphx-glr-single-img





.. GENERATED FROM PYTHON SOURCE LINES 143-144

Plot the objective function.

.. GENERATED FROM PYTHON SOURCE LINES 144-145

.. code-block:: Python

    _ = prob.plot_objective_value()



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






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

   **Total running time of the script:** (1 minutes 31.359 seconds)


.. _sphx_glr_download_examples_plot_betts_10_43_opty.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_43_opty.ipynb <plot_betts_10_43_opty.ipynb>`

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

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

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

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


.. only:: html

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

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