Source code for roboticstoolbox.robot.Link

from __future__ import annotations

from abc import ABC
from copy import deepcopy
from functools import wraps
from typing import Any
from warnings import warn

# from multiprocessing.sharedctypes import Value
import numpy as np
from ansitable import ANSITable, Column
from spatialgeometry import SceneGroup, SceneNode, Shape
from spatialmath import SE2, SE3
from spatialmath.base import getvector, ismatrix, isscalar, isvector
from typing_extensions import Self

import roboticstoolbox as rtb
from roboticstoolbox.ets._ET import BaseET
from roboticstoolbox.ets.ET import ET
from roboticstoolbox.ets.ET2 import ET2
from roboticstoolbox.ets.ETS import ETS
from roboticstoolbox.ets.ETS2 import ETS2
from roboticstoolbox.tools.types import ArrayLike, NDArray

# A generic type variable representing any subclass of BaseETS
# ETSType = TypeVar("ETSType", bound=BaseETS)
# ETType = TypeVar("ETType", bound=BaseET)


def _copy_shapes(shapes: Any, link_name: Any) -> Any:
    """Copy a list of coal/geometry shape objects, warning and skipping any that
    do not support deepcopy (coal CollisionObject instances cannot be pickled)."""
    import warnings
    from copy import deepcopy

    result = []
    warned = False
    for shape in shapes:
        try:
            result.append(deepcopy(shape))
        except RuntimeError:
            if not warned:
                warnings.warn(
                    f"Collision/geometry shape on link '{link_name}' could not be "
                    "copied (coal objects do not support deepcopy). "
                    "Shapes will be absent from the copied link. "
                    "Reload the robot from URDF if collision checking is needed.",
                    UserWarning,
                    stacklevel=5,
                )
                warned = True
    return result


def _dirties_frne(func: Any) -> Any:
    """
    @_dirties_frne

    Decorator for property setters

    Use this decorator for any property setter that updates a parameter that
    affects the result of inverse dynamics.  This allows the C version of the
    parameters only having to be updated when they change, rather than on
    every call.  This decorator signals the change by:

    - invoking the ``.dynchanged()`` method of the robot that owns the link.
      This assumes that the Link object is owned by a robot, this happens
      when the Link object is passed to a robot constructor.

    It does not say whether the link has dynamics, since many of the parameters
    that it decorates are kinematic.  That is the job of the setters of the mass
    and the motor inertia, see :meth:`Link._update_hasdynamics`.

    Example::

        @m.setter
        @_dirties_frne
        def m(self, m_new):
            self._m = m_new

    .. seealso:: :func:`DHLink._dyn_changed`
    """

    @wraps(func)
    def wrapper_dirties_frne(*args: Any) -> Any:
        if args[0]._robot is not None:
            args[0]._robot.dynchanged()
        return func(*args)

    return wrapper_dirties_frne


# Counts the changes to the mass or motor inertia of any link.  A robot caches the
# answer to "does any link have dynamics?" and checks the counter to know whether the
# answer can be stale.  A counter is used rather than a notification because a link
# can be shared by several robots but only knows about one of them.
_dynamics_version = 0


def _dyn_version() -> int:
    return _dynamics_version


def _nonzero(x: Any) -> bool:
    """True if a mass or inertia is not zero, or is symbolic"""
    try:
        return bool(np.any(np.asarray(x) != 0))
    except Exception:
        return True  # something we cannot compare with zero, such as a symbol


def _readonly(a: NDArray) -> NDArray:
    """A view of an array that cannot be written to"""
    view = a.view()
    view.flags.writeable = False
    return view












[docs]
class Link2(BaseLink):
    def __init__(
        self, ets: ETS2 = ETS2(), jindex: int | None = None, **kwargs: Any
    ) -> None:
        # process common options
        super().__init__(ets=ets, **kwargs)

        # check we have an ETS
        if not isinstance(self._ets, ETS2):  # pragma: nocover
            raise TypeError("The self._ets argument must be of type ETS2")

        # Set the jindex
        if len(self._ets) > 0 and self._ets[-1].isjoint:
            if jindex is not None:
                self._ets[-1].jindex = jindex  # pragma: nocover



[docs]
    def A(self, q: float = 0.0) -> SE2:
        """
        Link transform matrix

        :param q: Joint coordinate (radians or metres). Not required for links
            with no variable
        :returns: link frame transformation matrix

        ``link.A(q)`` is an SE(2) matrix that describes the rigid-body
        transformation from the previous to the current link frame to
        the next, which depends on the joint coordinate ``q``.
        """

        if self.isjoint:
            if self._Ts is not None:
                return SE2(self._Ts @ self._ets[-1].A(q), check=False)
            else:
                return SE2(self._ets[-1].A(q), check=False)

        elif self._Ts is not None:
            return SE2(self._Ts, check=False)
        else:
            return SE2()