qplot has gone to plot.py

petercorke · petercorke · commit f8acf2000dd5 · 2021-09-21T09:06:18.000+10:00
diff --git a/roboticstoolbox/tools/trajectory.py b/roboticstoolbox/tools/trajectory.py
@@ -453,7 +453,11 @@ def lspbfunc(t):
         if isscalar(t):
             t = [t]
         for tk in t:
-            if tk <= tb:
+            if tk < 0:
+                pk = q0
+                pdk = 0
+                pddk = 0
+            elif tk <= tb:
                 # initial blend
                 pk = q0 + a / 2 * tk ** 2
                 pdk = a * tk
@@ -463,11 +467,15 @@ def lspbfunc(t):
                 pk = (qf + q0 - V * tf) / 2 + V * tk
                 pdk = V
                 pddk = 0
-            else:
+            elif tk <= tf:
                 # final blend
                 pk = qf - a / 2 * tf ** 2 + a * tf * tk - a / 2 * tk ** 2
                 pdk = a * tf - a * tk
                 pddk = -a
+            else:
+                pk = qf
+                pdk = 0
+                pddk = 0
             p.append(pk)
             pd.append(pdk)
             pdd.append(pddk)
@@ -638,107 +646,6 @@ def mtraj(tfunc, q0, qf, t):
     return Trajectory("mtraj", x, y, yd, ydd, istime)
 
 
-def qplot(
-    x,
-    y=None,
-    wrist=False,
-    unwrap=False,
-    block=False,
-    labels=None,
-    loc=None,
-    grid=True,
-    stack=False,
-    **kwargs,
-):
-    """
-    Plot trajectory data
-
-    :param q: trajectory, one row per timestep
-    :type q: ndarray(m,n)
-    :param t: time vector, optional
-    :type t: numpy ndarray, shape=(M,)
-    :param wrist: distinguish arm and wrist joints with line styles
-    :type wrist: bool
-    :param unwrap: unwrap joint angles so that they smoothly increase or
-        decrease when they pass through :math:`\pm \pi`
-    :type unwrap: bool
-    :param block: block until the plot is closed
-    :type block: bool
-    :param labels: legend labels
-    :type labels: list of str, or single string with space separated labels
-    :param kwargs: options passed to pyplot.plot
-    :param loc: legend location as per pyplot.legend
-    :type loc: str
-
-    This is a convenience function to plot trajectories, where each row represents one time step.
-
-    - ``qplot(q)`` plots the joint angles versus row number.  If N==6 a
-      conventional 6-axis robot is assumed, and the first three joints are
-      shown as solid lines, the last three joints (wrist) are shown as dashed
-      lines. A legend is also displayed.
-
-    - ``qplot(t, q)`` as above but displays the joint angle trajectory versus
-      time given the time vector T (Mx1).
-
-    Example::
-
-        >>> qplot(q, x, labels='x y z')
-
-    :seealso: :func:`jtraj`, :func:`numpy.unwrap`
-    """
-    if y is None:
-        q = x
-        t = np.arange(0, q.shape[0])
-    else:
-        t = x
-        q = y
-
-    if t.ndim != 1 or q.shape[0] != t.shape[0]:
-        raise ValueError("dimensions of arguments are not consistent")
-
-    if unwrap:
-        q = np.unwrap(q, axis=0)
-
-    n = q.shape[1]
-
-    if labels is None:
-        labels = [f"q{i}" for i in range(n)]
-    elif isinstance(labels, str):
-        labels = labels.split(" ")
-    elif not isinstance(labels, (tuple, list)):
-        raise TypeError("wrong type for labels")
-
-    fig, ax = plt.subplots()
-
-    if stack:
-        for i in range(n):
-            ax = plt.subplot(n, 1, i + 1)
-
-            plt.plot(t, q[:, i], **kwargs)
-
-            plt.grid(grid)
-            ax.set_ylabel(labels[i])
-            ax.set_xlim(t[0], t[-1])
-
-        ax.set_xlabel("Time (s)")
-
-    else:
-        if n == 6 and wrist:
-            plt.plot(t, q[:, 0:3], **kwargs)
-            plt.plot(t, q[:, 3:6], "--", **kwargs)
-        else:
-            plt.plot(t, q, **kwargs)
-
-        ax.legend(labels, loc=loc)
-
-        plt.grid(grid)
-        ax.set_xlabel("Time (s)")
-        ax.set_ylabel("Joint coordinates (rad,m)")
-        ax.set_xlim(t[0], t[-1])
-
-    plt.show(block=block)
-
-    return fig.get_axes()
 
 
 # -------------------------------------------------------------------------- #
