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

.. only:: html

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

        Click :ref:`here <sphx_glr_download_auto_examples_plot_kinematics.py>`
        to download the full example code

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

.. _sphx_glr_auto_examples_plot_kinematics.py:

Retime a path subject to kinematic constraints
=================================================

In this example, we will see how can we retime a generic spline-based
path subject to kinematic constraints. This is very simple to do with
`toppra`, as we shall see below. First import the library.

.. GENERATED FROM PYTHON SOURCE LINES 9-19

.. code-block:: default
   :lineno-start: 10


    import toppra as ta
    import toppra.constraint as constraint
    import toppra.algorithm as algo
    import numpy as np
    import matplotlib.pyplot as plt
    import time

    ta.setup_logging("INFO")








.. GENERATED FROM PYTHON SOURCE LINES 20-21

We generate a path with some random waypoints.

.. GENERATED FROM PYTHON SOURCE LINES 21-36

.. code-block:: default
   :lineno-start: 22


    def generate_new_problem(seed=9):
        # Parameters
        N_samples = 5
        dof = 7
        np.random.seed(seed)
        way_pts = np.random.randn(N_samples, dof)
        return (
            np.linspace(0, 1, 5),
            way_pts,
            10 + np.random.rand(dof) * 20,
            10 + np.random.rand(dof) * 2,
        )
    ss, way_pts, vlims, alims = generate_new_problem()








.. GENERATED FROM PYTHON SOURCE LINES 37-38

Define the geometric path and two constraints.

.. GENERATED FROM PYTHON SOURCE LINES 38-42

.. code-block:: default
   :lineno-start: 38

    path = ta.SplineInterpolator(ss, way_pts)
    pc_vel = constraint.JointVelocityConstraint(vlims)
    pc_acc = constraint.JointAccelerationConstraint(alims)








.. GENERATED FROM PYTHON SOURCE LINES 43-47

We solve the parametrization problem using the
`ParametrizeConstAccel` parametrizer. This parametrizer is the
classical solution, guarantee constraint and boundary conditions
satisfaction.

.. GENERATED FROM PYTHON SOURCE LINES 47-50

.. code-block:: default
   :lineno-start: 47

    instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
    jnt_traj = instance.compute_trajectory()





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

 Out:

 .. code-block:: none

     INFO [algorithm.py : 104] No gridpoint specified. Automatically choose a gridpoint with 290 points
     INFO [reachability_algorithm.py : 65] Solver wrapper not supplied. Choose solver wrapper automatically!
     INFO [reachability_algorithm.py : 75] Select solver seidel
     INFO [algorithm.py : 191] Successfully parametrize path. Duration: 3.525, previously 1.000)
     INFO [algorithm.py : 193] Finish parametrization in 0.013 secs




.. GENERATED FROM PYTHON SOURCE LINES 51-53

The output trajectory is an instance of
:class:`toppra.interpolator.AbstractGeometricPath`.

.. GENERATED FROM PYTHON SOURCE LINES 53-70

.. code-block:: default
   :lineno-start: 53

    ts_sample = np.linspace(0, jnt_traj.duration, 100)
    qs_sample = jnt_traj(ts_sample)
    qds_sample = jnt_traj(ts_sample, 1)
    qdds_sample = jnt_traj(ts_sample, 2)
    fig, axs = plt.subplots(3, 1, sharex=True)
    for i in range(path.dof):
        # plot the i-th joint trajectory
        axs[0].plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
        axs[1].plot(ts_sample, qds_sample[:, i], c="C{:d}".format(i))
        axs[2].plot(ts_sample, qdds_sample[:, i], c="C{:d}".format(i))
    axs[2].set_xlabel("Time (s)")
    axs[0].set_ylabel("Position (rad)")
    axs[1].set_ylabel("Velocity (rad/s)")
    axs[2].set_ylabel("Acceleration (rad/s2)")
    plt.show()





.. image-sg:: /auto_examples/images/sphx_glr_plot_kinematics_001.png
   :alt: plot kinematics
   :srcset: /auto_examples/images/sphx_glr_plot_kinematics_001.png
   :class: sphx-glr-single-img





.. GENERATED FROM PYTHON SOURCE LINES 71-72

Optionally, we can inspect the output.

.. GENERATED FROM PYTHON SOURCE LINES 72-74

.. code-block:: default
   :lineno-start: 72

    instance.compute_feasible_sets()
    instance.inspect()



.. image-sg:: /auto_examples/images/sphx_glr_plot_kinematics_002.png
   :alt: Path-position path-velocity plot
   :srcset: /auto_examples/images/sphx_glr_plot_kinematics_002.png
   :class: sphx-glr-single-img






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

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


.. _sphx_glr_download_auto_examples_plot_kinematics.py:


.. only :: html

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



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

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



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

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


.. only:: html

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

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