Source code for roboticstoolbox.robot.DHLink

#!/usr/bin/env python
"""
@author: Jesse Haviland
"""

from __future__ import annotations

from copy import deepcopy
from functools import wraps
from typing import Any

from numpy import array, cos, sin
from spatialmath import SE3

# import numpy as np
# from spatialmath import SE3
import roboticstoolbox as rp
from roboticstoolbox.ets.ET import ET
from roboticstoolbox.ets.ETS import ETS
from roboticstoolbox.robot.Link import Link, _copy_shapes, _dirties_frne
from roboticstoolbox.tools.types import ArrayLike


def _check_rne(func: Any) -> Any:
    """
    @_check_rne decorator

    Decorator applied to any method to calls to C RNE code.  Works in
    conjunction with::

        @_dirties_frne
        def dyn_param_setter(self, value):

    which marks the dynamic parameters as having changed using the robot's
    ``.dynchanged()`` method.

    If this is the case, then the parameters are re-serialized prior to
    invoking inverse dynamics.

    .. seealso:: :meth:`~roboticstoolbox.robot.Link.Link._listen_dyn`
    """

    @wraps(func)
    def wrapper_check_rne(*args: Any, **kwargs: Any) -> Any:
        if args[0]._frne is None or args[0]._frne_stale:
            args[0]._copy_to_cpp()
        return func(*args, **kwargs)

    return wrapper_check_rne


# --------------------------------------------------------------#

try:  # pragma: no cover
    # print('Using SymPy')
    import sympy as sym

    def _issymbol(x: Any) -> Any:
        return isinstance(x, sym.Expr)

except ImportError:

    def _issymbol(x: Any) -> Any:  # pylint: disable=unused-argument
        return False


def _cos(theta: Any) -> float:
    if _issymbol(theta):
        return sym.cos(theta)
    else:
        return cos(theta)


def _sin(theta: Any) -> float:
    if _issymbol(theta):
        return sym.sin(theta)
    else:
        return sin(theta)


# --------------------------------------------------------------#






# -------------------------------------------------------------------------- #


[docs] class RevoluteDH(DHLink): r""" Class for revolute links using standard DH convention. :param d: kinematic - link offset :param alpha: kinematic - link twist :param a: kinematic - link length :param offset: kinematic - joint variable offset :param qlim: joint variable limits [min, max] :type qlim: ndarray(2,) :param flip: joint moves in opposite direction :param m: dynamic - link mass :type m: float :param r: dynamic - position of COM with respect to link frame :type r: ndarray(3,) :param I: dynamic - inertia of link with respect to COM :type I: ndarray :param Jm: dynamic - motor inertia :type Jm: float :param B: dynamic - motor viscous friction: B=B⁺=B⁻, [B⁺, B⁻] :type B: float, or ndarray(2,) :param Tc: dynamic - motor Coulomb friction [Tc⁺, Tc⁻] :type Tc: ndarray(2,) :param G: dynamic - gear ratio :type G: float A subclass of the :class:`DHLink` class for a revolute joint that holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters. The link transform is :math:`\underbrace{\mathbf{T}_{rz}(q_i)}_{\mbox{variable}} \cdot \mathbf{T}_{tz}(d_i) \cdot \mathbf{T}_{tx}(a_i) \cdot \mathbf{T}_{rx}(\alpha_i)` where :math:`q_i` is the joint variable. :references: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. .. seealso:: :func:`PrismaticDH`, :func:`DHLink`, :func:`RevoluteMDH` """ def __init__( self, d: float = 0.0, a: float = 0.0, alpha: float = 0.0, offset: float = 0.0, qlim: ArrayLike | None = None, flip: bool = False, **kwargs: Any, ) -> None: theta = 0.0 sigma = 0 mdh = False super().__init__( d=d, alpha=alpha, theta=theta, a=a, sigma=sigma, mdh=mdh, offset=offset, qlim=qlim, flip=flip, **kwargs, )
[docs] class PrismaticDH(DHLink): r""" Class for prismatic link using standard DH convention. :param theta: kinematic: joint angle :param alpha: kinematic - link twist :param a: kinematic - link length :param offset: kinematic - joint variable offset :param qlim: joint variable limits [min, max] :type qlim: ndarray(2,) :param flip: joint moves in opposite direction :param m: dynamic - link mass :type m: float :param r: dynamic - position of COM with respect to link frame :type r: ndarray(3,) :param I: dynamic - inertia of link with respect to COM :type I: ndarray :param Jm: dynamic - motor inertia :type Jm: float :param B: dynamic - motor viscous friction: B=B⁺=B⁻, [B⁺, B⁻] :type B: float, or ndarray(2,) :param Tc: dynamic - motor Coulomb friction [Tc⁺, Tc⁻] :type Tc: ndarray(2,) :param G: dynamic - gear ratio :type G: float A subclass of the DHLink class for a prismatic joint that holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters. The link transform is :math:`\mathbf{T}_{rz}(\theta_i) \cdot \underbrace{\mathbf{T}_{tz}(q_i)}_{\mbox{variable}} \cdot \mathbf{T}_{tx}(a_i) \cdot \mathbf{T}_{rx}(\alpha_i)` where :math:`q_i` is the joint variable. :references: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. .. seealso:: :func:`RevoluteDH`, :func:`DHLink`, :func:`PrismaticMDH` """ def __init__( self, theta: float = 0.0, a: float = 0.0, alpha: float = 0.0, offset: float = 0.0, qlim: ArrayLike | None = None, flip: bool = False, **kwargs: Any, ) -> None: d = 0.0 sigma = 1 mdh = False super().__init__( theta=theta, d=d, a=a, alpha=alpha, sigma=sigma, mdh=mdh, offset=offset, qlim=qlim, flip=flip, **kwargs, )
[docs] class RevoluteMDH(DHLink): r""" Class for revolute links using modified DH convention. :param d: kinematic - link offset :param alpha: kinematic - link twist :param a: kinematic - link length :param offset: kinematic - joint variable offset :param qlim: joint variable limits [min, max] :type qlim: ndarray(2,) :param flip: joint moves in opposite direction :param m: dynamic - link mass :type m: float :param r: dynamic - position of COM with respect to link frame :type r: ndarray(3,) :param I: dynamic - inertia of link with respect to COM :type I: ndarray :param Jm: dynamic - motor inertia :type Jm: float :param B: dynamic - motor viscous friction: B=B⁺=B⁻, [B⁺, B⁻] :type B: float, or ndarray(2,) :param Tc: dynamic - motor Coulomb friction [Tc⁺, Tc⁻] :type Tc: ndarray(2,) :param G: dynamic - gear ratio :type G: float A subclass of the DHLink class for a revolute joint that holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters. The link transform is :math:`\mathbf{T}_{tx}(a_{i-1}) \cdot \mathbf{T}_{rx}(\alpha_{i-1}) \cdot \underbrace{\mathbf{T}_{rz}(q_i)}_{\mbox{variable}} \cdot \mathbf{T}_{tz}(d_i)` where :math:`q_i` is the joint variable. :references: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. .. seealso:: :func:`PrismaticMDH`, :func:`DHLink`, :func:`RevoluteDH` """ def __init__( self, d: float = 0.0, a: float = 0.0, alpha: float = 0.0, offset: float = 0.0, qlim: ArrayLike | None = None, flip: bool = False, **kwargs: Any, ) -> None: theta = 0.0 sigma = 0 mdh = True super().__init__( d=d, alpha=alpha, theta=theta, a=a, sigma=sigma, mdh=mdh, offset=offset, qlim=qlim, flip=flip, **kwargs, )
[docs] class PrismaticMDH(DHLink): r""" Class for prismatic link using modified DH convention. :param theta: kinematic: joint angle :param alpha: kinematic - link twist :param a: kinematic - link length :param offset: kinematic - joint variable offset :param qlim: joint variable limits [min, max] :type qlim: ndarray(2,) :param flip: joint moves in opposite direction :param m: dynamic - link mass :type m: float :param r: dynamic - position of COM with respect to link frame :type r: ndarray(3,) :param I: dynamic - inertia of link with respect to COM :type I: ndarray :param Jm: dynamic - motor inertia :type Jm: float :param B: dynamic - motor viscous friction: B=B⁺=B⁻, [B⁺, B⁻] :type B: float, or ndarray(2,) :param Tc: dynamic - motor Coulomb friction [Tc⁺, Tc⁻] :type Tc: ndarray(2,) :param G: dynamic - gear ratio :type G: float A subclass of the DHLink class for a prismatic joint that holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters. The link transform is :math:`\mathbf{T}_{tx}(a_{i-1}) \cdot \mathbf{T}_{rx}(\alpha_{i-1}) \cdot \mathbf{T}_{rz}(\theta_i) \cdot \underbrace{\mathbf{T}_{tz}(q_i)}_{\mbox{variable}}` where :math:`q_i` is the joint variable. :references: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. .. seealso:: :func:`RevoluteMDH`, :func:`DHLink`, :func:`PrismaticDH` """ def __init__( self, theta: float = 0.0, a: float = 0.0, alpha: float = 0.0, offset: float = 0.0, qlim: ArrayLike | None = None, flip: bool = False, **kwargs: Any, ) -> None: d = 0.0 sigma = 1 mdh = True super().__init__( theta=theta, d=d, a=a, alpha=alpha, sigma=sigma, mdh=mdh, offset=offset, qlim=qlim, flip=flip, **kwargs, )