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

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

.. _sphx_glr_auto_examples_plot_robust_kinematics.py:


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

.. GENERATED FROM PYTHON SOURCE LINES 5-74

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

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


    parser = argparse.ArgumentParser(
        description="An example showcasing the usage of robust constraints."
        "A velocity constraint and a robust acceleration constraint"
        "are considered in this script."
    )
    parser.add_argument(
        "-N",
        "--N",
        type=int,
        help="Number of segments in the discretization.",
        default=100,
    )
    parser.add_argument("-v", "--verbose", action="store_true", default=False)
    parser.add_argument("-du", "--du", default=1e-3, type=float)
    parser.add_argument("-dx", "--dx", default=5e-2, type=float)
    parser.add_argument("-dc", "--dc", default=9e-3, type=float)
    parser.add_argument("-so", "--solver_wrapper", default="ecos")
    parser.add_argument("-i", "--interpolation_scheme", default=1, type=int)
    args = parser.parse_args()
    if args.verbose:
        ta.setup_logging("DEBUG")
    else:
        ta.setup_logging("INFO")

    # Parameters
    N_samples = 5
    dof = 7

    # Random waypoints used to obtain a random geometric path.
    np.random.seed(9)
    way_pts = np.random.randn(N_samples, dof)

    # Create velocity bounds, then velocity constraint object
    vlim_ = np.random.rand(dof) * 20
    vlim = np.vstack((-vlim_, vlim_)).T
    # Create acceleration bounds, then acceleration constraint object
    alim_ = np.random.rand(dof) * 2
    alim = np.vstack((-alim_, alim_)).T

    path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts)
    pc_vel = constraint.JointVelocityConstraint(vlim)
    pc_acc = constraint.JointAccelerationConstraint(
        alim, discretization_scheme=constraint.DiscretizationType.Interpolation
    )
    robust_pc_acc = constraint.RobustLinearConstraint(
        pc_acc, [args.du, args.dx, args.dc], args.interpolation_scheme
    )
    instance = algo.TOPPRA(
        [pc_vel, robust_pc_acc],
        path,
        gridpoints=np.linspace(0, 1, args.N + 1),
        solver_wrapper=args.solver_wrapper,
    )

    X = instance.compute_feasible_sets()
    K = instance.compute_controllable_sets(0, 0)

    _, sd_vec, _ = instance.compute_parameterization(0, 0)

    X = np.sqrt(X)
    K = np.sqrt(K)







.. GENERATED FROM PYTHON SOURCE LINES 75-84

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

    plt.plot(X[:, 0], c="green", label="Feasible sets")
    plt.plot(X[:, 1], c="green")
    plt.plot(K[:, 0], "--", c="red", label="Controllable sets")
    plt.plot(K[:, 1], "--", c="red")
    plt.plot(sd_vec, label="Velocity profile")
    plt.legend()
    plt.title("Path-position path-velocity plot")
    plt.show()




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





.. GENERATED FROM PYTHON SOURCE LINES 85-92

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

    jnt_traj = instance.compute_trajectory(0, 0)
    ts_sample = np.linspace(0, jnt_traj.duration, 100)
    qs_sample = jnt_traj.evaldd(ts_sample)

    plt.plot(ts_sample, qs_sample)
    plt.show()




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


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

 Out:

 .. code-block:: none

     INFO [algorithm.py : 191] Successfully parametrize path. Duration: 37.417, previously 1.000)
     INFO [algorithm.py : 193] Finish parametrization in 1.927 secs





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

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


.. _sphx_glr_download_auto_examples_plot_robust_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_robust_kinematics.py <plot_robust_kinematics.py>`



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

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


.. only:: html

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

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