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

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

.. _sphx_glr_examples_planning_control_plot_example_lqt_cp_dmp.py:


LQT with control primitives and DMP
==================================

This example shows how to use the LQT controller with control primitives and DMP to track a trajectory.

.. GENERATED FROM PYTHON SOURCE LINES 7-28


.. rst-class:: sphx-glr-script-out

.. code-block:: pytb

    Traceback (most recent call last):
      File "/home/docs/checkouts/readthedocs.org/user_builds/rofunc/checkouts/stable/examples/planning_control/plot_example_lqt_cp_dmp.py", line 12, in <module>
        x = np.load('../data/LQT_LQR/S.npy')[0, :, :2].T
      File "/home/docs/checkouts/readthedocs.org/user_builds/rofunc/envs/stable/lib/python3.8/site-packages/numpy/lib/npyio.py", line 417, in load
        fid = stack.enter_context(open(os_fspath(file), "rb"))
    FileNotFoundError: [Errno 2] No such file or directory: '../data/LQT_LQR/S.npy'






|

.. code-block:: default


    import numpy as np
    import rofunc as rf
    from scipy.interpolate import interp1d

    x = np.load('../data/LQT_LQR/S.npy')[0, :, :2].T

    cfg = rf.config.utils.get_config('./planning', 'lqt_cp_dmp')

    f_pos = interp1d(np.linspace(0, np.size(x, 1) - 1, np.size(x, 1), dtype=int), x, kind='cubic')
    MuPos = f_pos(np.linspace(0, np.size(x, 1) - 1, cfg.nbData))  # Position
    MuVel = np.gradient(MuPos)[1] / cfg.dt
    MuAcc = np.gradient(MuVel)[1] / cfg.dt
    # Position, velocity and acceleration profiles as references
    via_points = np.vstack((MuPos, MuVel, MuAcc))
    cfg.nbData = len(via_points[0])

    state_noise = np.vstack((np.array([[3], [-0.5]]), np.zeros((cfg.nbVarX - cfg.nbVarU, 1))))

    controller = rf.planning_control.lqt.lqt_cp_dmp.LQTCPDMP(via_points, cfg)
    u_hat, x_hat = controller.solve(state_noise=state_noise)


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

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


.. _sphx_glr_download_examples_planning_control_plot_example_lqt_cp_dmp.py:

.. only:: html

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




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

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

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

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


.. only:: html

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

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