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

.. only:: html

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

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

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

.. _sphx_glr_examples_planning_control_example_ilqr_obstacle.py:


iLQR with obstacle avoidance
==============================

This example shows how to use the iLQR solver with obstacle avoidance.

.. GENERATED FROM PYTHON SOURCE LINES 7-40

.. code-block:: default


    import rofunc as rf
    import numpy as np
    from rofunc.config.utils import get_config

    cfg = get_config('./planning', 'ilqr_obstacle')

    Mu = np.array([[3, 3, np.pi / 6]])  # Via-point [x1,x2,o]
    Obst = np.array([
        [1, 0.6, np.pi / 4],  # [x1,x2,o]
        [2, 2.5, -np.pi / 6]  # [x1,x2,o]
    ])

    A_obst = np.zeros((cfg.nbObstacles, 2, 2))
    S_obst = np.zeros((cfg.nbObstacles, 2, 2))
    Q_obst = np.zeros((cfg.nbObstacles, 2, 2))
    U_obst = np.zeros((cfg.nbObstacles, 2, 2))  # Q_obs[t] = U_obs[t].T @ U_obs[t]
    for i in range(cfg.nbObstacles):
        orn_t = Obst[i][-1]
        A_obst[i] = np.array([  # Orientation in matrix form
            [np.cos(orn_t), -np.sin(orn_t)],
            [np.sin(orn_t), np.cos(orn_t)]
        ])

        S_obst[i] = A_obst[i] @ np.diag(cfg.sizeObstacle) ** 2 @ A_obst[i].T  # Covariance matrix
        Q_obst[i] = np.linalg.inv(S_obst[i])  # Precision matrix
        U_obst[i] = A_obst[i] @ np.diag(
            1 / np.array(cfg.sizeObstacle))  # "Square root" of cfg.Q_obst[i]

    u0 = np.zeros(cfg.nbVarU * (cfg.nbData - 1))  # Initial control command
    x0 = np.zeros(cfg.nbVarX)  # Initial state

    rf.lqr.uni_obstacle(Mu, Obst, S_obst, U_obst, u0, x0, cfg)


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

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


.. _sphx_glr_download_examples_planning_control_example_ilqr_obstacle.py:

.. only:: html

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




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

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

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

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


.. only:: html

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

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