roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/tools/data.py

@@ -0,0 +1,187 @@
+from pathlib import Path
+import sys
+import importlib
+
+
+def _candidate_rtbdata_roots():
+    """
+    Yield candidate roots where the ``rtbdata`` tree may exist.
+    """
+    roots = []
+
+    # Primary source: installed/importable rtbdata package.
+    rtbdata = importlib.import_module("rtbdata")
+    roots.append(Path(rtbdata.__path__[0]))
+
+    # Dev fallback: local sibling checkout at <repo>/rtb-data/rtbdata.
+    repo_root = Path(__file__).resolve().parents[3]
+    local_rtbdata = repo_root / "rtb-data" / "rtbdata"
+    if local_rtbdata.exists():
+        roots.append(local_rtbdata)
+
+    # Preserve order while removing duplicates.
+    seen = set()
+    for root in roots:
+        root = root.resolve()
+        if root not in seen:
+            seen.add(root)
+            yield root
+
+
+def rtb_load_matfile(filename):
+    """
+    Load toolbox mat format data file
+
+    :param filename: relative pathname of datafile
+    :type filename: str
+    :raises ValueError: File does not exist
+    :return: contents of mat data file
+    :rtype: dict
+
+    Reads a MATLAB format *mat* file which can contain multiple variables, in
+    a binary or ASCII format.  Returns a dict where the keys are the variable
+    names and the values are NumPy arrays.
+
+    .. note::
+        - Uses SciPy ``io.loadmat`` to do the work.
+        - If the filename has no path component, eg. ``map1.mat`, it will be
+          first be looked for in the folder ``roboticstoolbox/data``.
+        - MATLAB structs are converted to Python dicts, but extended so that
+          elements can be accessed using dot notation.
+
+    :seealso: :func:`path_to_datafile`
+    """
+    from scipy.io import loadmat
+    from scipy.io.matlab.mio5_params import mat_struct
+    from collections import namedtuple
+
+    # get results as a dict
+    data = rtb_load_data(filename, loadmat, squeeze_me=True, struct_as_record=False)
+
+    # if elements are a scipy.io.matlab.mio5_params.mat_struct, that is, they
+    # were a MATLAB struct, convert them to a namedtuple
+    for key, value in data.items():
+        if isinstance(value, mat_struct):
+            # extend dict with a dot access method
+            class dictx(dict):
+                def __getattr__(self, key):
+                    try:
+                        return self[key]
+                    except KeyError as k:
+                        raise AttributeError(k)
+
+            d = dictx()
+            for v in value._fieldnames:
+                d[v] = getattr(value, v)
+            data[key] = d
+
+    return data
+
+
+def rtb_load_jsonfile(filename):
+    """
+    Load toolbox JSON format data file
+
+    :param filename: relative pathname of datafile
+    :type filename: str
+    :raises ValueError: File does not exist
+    :return: contents of JSON data file
+    :rtype: dict
+
+    Reads a JSON format file which can contain multiple variables and return
+    a dict where the keys are the variable
+    names and the values are NumPy arrays.
+
+    .. note::
+        - If the filename has no path component, eg. ``map1.mat`, it will be
+          first be looked for in the folder ``roboticstoolbox/data``.
+
+    :seealso: :func:`path_to_datafile`
+    """
+    import json
+
+    return rtb_load_data(filename, lambda f: json.load(open(f, "r")))
+
+
+def rtb_load_data(filename, handler, **kwargs):
+    """
+    Load toolbox data file
+
+    :param filename: relative pathname of datafile
+    :type filename: str
+    :param handler: function to read data
+    :type handler: callable
+    :raises ValueError: File does not exist
+    :return: data object
+
+    Resolves the relative pathname to an absolute name and then invokes the
+    data reading function::
+
+        handler(abs_file_name, **kwargs)
+
+    For example::
+
+        data = rtb_load_data('data/queensland.json', lambda f: json.load(open(f, 'r')))
+
+
+    .. note:: If the filename has no path component, eg. ``foo.dat``, it will
+        first be looked for in the folder ``roboticstoolbox/data``.
+
+    :seealso: :func:`path_to_datafile`
+    """
+    path = rtb_path_to_datafile(filename, local=False)
+    return handler(path, **kwargs)
+
+
+def rtb_path_to_datafile(*filename, local=True):
+    """
+    Get absolute path to datafile
+
+    :param filename: pathname of datafile
+    :type filename: str
+    :param local: search for file locally first, default True
+    :type local: bool
+    :raises FileNotFoundError: File does not exist
+    :return: Absolute path
+    :rtype: Path
+
+    The positional arguments are joined, like ``os.path.join``.
+
+    If ``local`` is True then ``~`` is expanded and if the file exists, the
+    path is made absolute, and symlinks resolved.
+
+    Otherwise, the file is sought within the ``rtbdata`` package and if found,
+    return that absolute path.
+
+    Example::
+
+        loadmat('data/map1.mat')   # read rtbdata/data/map1.mat
+        loadmat('foo.dat')         # read ./foo.dat
+        loadmat('~/foo.dat')       # read $HOME/foo.dat
+    """
+
+    filename = Path(*filename)
+
+    if local:
+        # check if file is in user's local filesystem
+
+        p = filename.expanduser()
+        p = p.resolve()
+        if p.exists():
+            return p
+
+    # otherwise, look for it in rtbdata locations
+    for root in _candidate_rtbdata_roots():
+        path = root / filename
+        if path.exists():
+            return path.resolve()
+
+    raise ValueError(f"file {filename} not found locally or in rtbdata")
+
+
+if __name__ == "__main__":
+    house = rtb_load_matfile("data/house.mat")
+    a = rtb_loadmat("map1.mat")
+    print(a)
+    a = rtb_loadmat("data/map1.mat")
+    print(a)

roboticstoolbox/tools/jsingu.py

@@ -0,0 +1,51 @@
+import numpy as np
+from scipy.linalg import lu
+
+
+# def jsingu(J):
+
+#     # convert to row-echelon form
+#     P, L, U = lu(J)
+#     U = np.where(abs(U) < 100 * np.finfo(np.float64).eps, False, True)
+
+#     for j in range(J.shape[0]):
+#         if not U[j, j]:
+#             print(
+#                 f'joint {j} is dependent on joint ' +
+#                 ', '.join(
+#                     [str(i) for i in range(j) if all(U[:, j] == U[:, i])]))
+
+
+def jsingu(J):
+
+    indep_columns = np.empty((J.shape[0], 0))
+    rank = 0
+    for j in range(J.shape[1]):
+        temp = np.column_stack((indep_columns, J[:, j]))
+        temp_rank = np.linalg.matrix_rank(temp)
+        if temp_rank > rank:
+            # this column is independent
+            rank = temp_rank
+            indep_columns = temp
+
+        else:
+            s = f"column {j} ="
+            c = np.linalg.pinv(indep_columns) @ J[:, j]
+            for i, ci in enumerate(c):
+                if abs(ci) > 10 * np.finfo(np.float64).eps:
+                    if ci < 0:
+                        s += " - "
+                    elif i > 0:
+                        s += " + "
+                    s += f"{abs(ci):.3g} column_{i}"
+            print(s)
+
+
+if __name__ == "__main__":  # pragma nocover
+    import roboticstoolbox as rtb
+
+    # robot = rtb.models.DH.Puma560()
+    robot = rtb.models.URDF.UR5()
+    J = robot.jacob0(robot.qr)
+
+    jsingu(J)

roboticstoolbox/tools/null.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import numpy as np
+
+
+def null(A, atol=1e-13, rtol=0):
+    """
+    Compute an approximate basis for the nullspace of A.
+
+    The algorithm used by this function is based on the singular value
+    decomposition of A.
+
+    :param A: A should be at most 2D. A 1-D array with length k will be
+        treated as a 2D with shape (1, k)
+    :type A: ndarray
+    :param atol: The absolute tolerance for a zero singular value. Singular
+        values smaller than `atol` are considered to be zero.
+    :type atol: float
+    :param rtol: The relative tolerance. Singular values less than rtol*smax
+        are considered to be zero, where smax is the largest singular value.
+    :type rtol: float
+
+    :notes:
+        - If both `atol` and `rtol` are positive, the combined tolerance is
+          the maximum of the two; that is:
+          tol = max(atol, rtol * smax)
+        - Singular values smaller than `tol` are considered to be zero.
+
+    Return value
+    ------------
+    :return ns: If A is an array with shape (m, k), then ns will be an array
+        with shape (k, n), where n is the estimated dimension of the
+        nullspace of A.  The columns of ns are a basis for the
+        nullspace; each element in numpy.dot(A, ns) will be approximately
+        zero.
+    :rtype ns: ndarray
+
+    """
+
+    A = np.atleast_2d(A)
+    u, s, vh = np.linalg.svd(A)
+    tol = max(atol, rtol * s[0])
+    nnz = (s >= tol).sum()
+    ns = vh[nnz:].conj().T
+    return ns

roboticstoolbox/tools/numerical.py

@@ -0,0 +1,96 @@
+import numpy as np
+from spatialmath import base
+
+
+def jacobian_numerical(f, x, dx=1e-8, N=0):
+    r"""
+    Numerically compute Jacobian of function
+
+    :param f: the function, returns an m-vector
+    :type f: callable
+    :param x: function argument
+    :type x: ndarray(n)
+    :param dx: the numerical perturbation, defaults to 1e-8
+    :type dx: float, optional
+    :param N: function returns SE(N) matrix, defaults to 0
+    :type N: int, optional
+    :return: Jacobian matrix
+    :rtype: ndarray(m,n)
+
+    Computes a numerical approximation to the Jacobian for ``f(x)`` where
+    :math:`f: \mathbb{R}^n \mapsto \mathbb{R}^m`.
+
+    Uses first-order difference :math:`J[:,i] = (f(x + dx) - f(x)) / dx`.
+
+    If ``N`` is 2 or 3, then it is assumed that the function returns
+    an SE(N) matrix which is converted into a Jacobian column comprising the
+    translational Jacobian followed by the rotational Jacobian.
+    """
+
+    Jcol = []
+    J0 = f(x)
+    I = np.eye(len(x))
+    f0 = f(x)
+    for i in range(len(x)):
+        fi = f(x + I[:, i] * dx)
+        Ji = (fi - f0) / dx
+
+        if N > 0:
+            t = Ji[:N, N]
+            r = base.vex(Ji[:N, :N] @ J0[:N, :N].T)
+            Ji = np.r_[t, r]
+
+        Jcol.append(Ji)
+
+    return np.c_[Jcol].T
+
+
+def hessian_numerical(J, x, dx=1e-8):
+    r"""
+    Numerically compute Hessian of Jacobian function
+
+    :param J: the Jacobian function, returns an ndarray(m,n)
+    :type J: callable
+    :param x: function argument
+    :type x: ndarray(n)
+    :param dx: the numerical perturbation, defaults to 1e-8
+    :type dx: float, optional
+    :return: Hessian matrix
+    :rtype: ndarray(m,n,n)
+
+    Computes a numerical approximation to the Hessian for ``J(x)`` where
+    :math:`f: \mathbb{R}^n  \mapsto \mathbb{R}^{m \times n}`
+
+    Uses first-order difference :math:`H[:,:,i] = (J(x + dx) - J(x)) / dx`.
+    """
+
+    I = np.eye(len(x))
+    Hcol = []
+    J0 = J(x)
+    for i in range(len(x)):
+        Ji = J(x + I[:, i] * dx)
+        Hi = (Ji - J0) / dx
+
+        Hcol.append(Hi)
+
+    return np.stack(Hcol, axis=2)
+
+
+if __name__ == "__main__":
+    import roboticstoolbox as rtb
+
+    np.set_printoptions(
+        linewidth=120,
+        formatter={"float": lambda x: f"{x:8.4g}" if abs(x) > 1e-10 else f"{0:8.4g}"},
+    )
+
+    robot = rtb.models.DH.Puma560()
+    q = robot.qn
+
+    J = jacobian_numerical(lambda q: robot.fkine(q).A, q, N=3)
+    print(J)
+    print(robot.jacob0(q))
+
+    H = hessian_numerical(robot.jacob0, q)
+    print(H)
+    print(robot.ets().hessian0(q))

roboticstoolbox/tools/p_servo.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+
+import numpy as np
+from spatialmath import SE3, base
+import math
+from typing import Union
+from roboticstoolbox.fknm import Angle_Axis
+from roboticstoolbox.tools.types import NDArray, ArrayLike
+
+# ArrayLike = Union[list, np.ndarray, tuple, set]
+
+
+def angle_axis(T, Td) -> NDArray:
+
+    try:
+        e: NDArray = Angle_Axis(T, Td)
+    except BaseException:
+        e = angle_axis_python(T, Td)
+
+    return e
+
+
+def angle_axis_python(T, Td):
+    e = np.empty(6)
+    e[:3] = Td[:3, -1] - T[:3, -1]
+    R = Td[:3, :3] @ T[:3, :3].T
+    li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
+
+    if base.iszerovec(li):
+        # diagonal matrix case
+        if np.trace(R) > 0:
+            # (1,1,1) case
+            a = np.zeros((3,))
+        else:
+            a = np.pi / 2 * (np.diag(R) + 1)
+    else:
+        # non-diagonal matrix case
+        ln = base.norm(li)
+        a = math.atan2(ln, np.trace(R) - 1) * li / ln
+
+    e[3:] = a
+
+    return e
+
+
+def p_servo(
+    wTe, wTep, gain: Union[float, ArrayLike] = 1.0, threshold=0.1, method="rpy"
+):
+    """
+    Position-based servoing.
+
+    Returns the end-effector velocity which will cause the robot to approach
+    the desired pose.
+
+    :param wTe: The current pose of the end-effecor in the base frame.
+    :type wTe: SE3 or ndarray
+    :param wTep: The desired pose of the end-effecor in the base frame.
+    :type wTep: SE3 or ndarray
+    :param gain: The gain for the controller. Can be vector corresponding to each
+        axis, or scalar corresponding to all axes.
+    :type gain: float, or array-like
+    :param threshold: The threshold or tolerance of the final error between
+        the robot's pose and desired pose
+    :type threshold: float
+    :param method: The method used to calculate the error. Default is 'rpy' -
+        error in the end-effector frame. 'angle-axis' - error in the base frame
+        using angle-axis method.
+    :type method: string: 'rpy' or 'angle-axis'
+
+    :returns v: The velocity of the end-effecotr which will casue the robot
+        to approach wTep
+    :rtype v: ndarray(6)
+    :returns arrived: True if the robot is within the threshold of the final
+        pose
+    :rtype arrived: bool
+
+    """
+
+    if isinstance(wTe, SE3):
+        wTe = wTe.A
+
+    if isinstance(wTep, SE3):
+        wTep = wTep.A
+
+    if method == "rpy":
+        # Pose difference
+        eTep = np.linalg.inv(wTe) @ wTep
+        e = np.empty(6)
+
+        # Translational error
+        e[:3] = eTep[:3, -1]
+
+        # Angular error
+        e[3:] = base.tr2rpy(eTep, unit="rad", order="zyx", check=False)
+    else:
+        e = angle_axis(wTe, wTep)
+
+    if base.isscalar(gain):
+        k = gain * np.eye(6)
+    else:
+        k = np.diag(gain)
+
+    v = k @ e
+    arrived = True if np.sum(np.abs(e)) < threshold else False
+
+    return v, arrived

roboticstoolbox/tools/params.py

@@ -0,0 +1,11 @@
+_params = {
+    "unicode": True,
+}
+
+
+def rtb_set_param(param, value):
+    _params[param] = value
+
+
+def rtb_get_param(param):
+    return _params[param]

roboticstoolbox/tools/plot.py

@@ -0,0 +1,109 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+def xplot(
+    x,
+    y=None,
+    wrist=False,
+    unwrap=False,
+    block=False,
+    labels=None,
+    loc=None,
+    grid=True,
+    stack=False,
+    **kwargs,
+):
+    r"""
+    Plot trajectory data
+
+    :param x: trajectory, one row per timestep
+    :type x: 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.
+
+    - ``xplot(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.
+
+    - ``xplot(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 q.ndim == 1:
+        # if 1D, make it Nx1
+        q = q[:, np.newaxis]
+
+    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])  fails with RVC3 Sec 3.3.3
+
+    plt.show(block=block)
+
+    return fig.get_axes()