Source code for roboticstoolbox.robot.Link

from __future__ import annotations

from copy import deepcopy
from abc import ABC
from typing_extensions import Self

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

import roboticstoolbox as rtb
from roboticstoolbox.ets.ETS import ETS
from roboticstoolbox.ets.ETS2 import ETS2
from roboticstoolbox.ets.ET import ET
from roboticstoolbox.ets.ET2 import ET2
from roboticstoolbox.ets._ET import BaseET
from warnings import warn

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, link_name):
    """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):
    """
    @_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.
    - setting the ``._hasdynamics`` attribute of the Link

    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):
        if args[0]._robot is not None:
            args[0]._robot.dynchanged()
        args[0]._hasdynamics = True
        return func(*args)

    return wrapper_dirties_frne












[docs]
class Link2(BaseLink):
    def __init__(self, ets: ETS2 = ETS2(), jindex: int | None = None, **kwargs):
        # 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()