diff --git a/CMakeLists.txt b/CMakeLists.txt index 48a5bfe90..ee719ddf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,17 +30,17 @@ target_include_directories(_frne_c PRIVATE # --------------------------------------------------------------------------- # _fknm_c — forward kinematics, Jacobian, Hessian, IK -# Eigen is vendored as header-only in src/roboticstoolbox/robot/cpp-extensions/Eigen/ +# Eigen is vendored as header-only in src/roboticstoolbox/ets/cpp-extensions/Eigen/ # fknm_nb.cpp is the nanobind glue; maths lives in methods/ik/linalg. # --------------------------------------------------------------------------- nanobind_add_module(_fknm_c - src/roboticstoolbox/robot/cpp-extensions/methods.cpp - src/roboticstoolbox/robot/cpp-extensions/ik.cpp - src/roboticstoolbox/robot/cpp-extensions/linalg.cpp - src/roboticstoolbox/robot/cpp-extensions/fknm_nb.cpp + src/roboticstoolbox/ets/cpp-extensions/methods.cpp + src/roboticstoolbox/ets/cpp-extensions/ik.cpp + src/roboticstoolbox/ets/cpp-extensions/linalg.cpp + src/roboticstoolbox/ets/cpp-extensions/fknm_nb.cpp ) target_include_directories(_fknm_c PRIVATE - src/roboticstoolbox/robot/cpp-extensions + src/roboticstoolbox/ets/cpp-extensions ) install(TARGETS _frne_c _fknm_c DESTINATION roboticstoolbox) diff --git a/src/roboticstoolbox/backends/PyPlot/RobotPlot.py b/src/roboticstoolbox/backends/PyPlot/RobotPlot.py index 5bed8c96f..8aab3115a 100644 --- a/src/roboticstoolbox/backends/PyPlot/RobotPlot.py +++ b/src/roboticstoolbox/backends/PyPlot/RobotPlot.py @@ -169,11 +169,11 @@ def draw(self): elif link.isjoint: Tj = T[link.number] R = Tj.R - if link.v.axis[1] == "z": + if link.v.ax == "z": direction = R[:, 2] # z direction - elif link.v.axis[1] == "y": + elif link.v.ax == "y": direction = R[:, 1] # y direction - elif link.v.axis[1] == "x": + elif link.v.ax == "x": direction = R[:, 0] # direction if direction is not None: diff --git a/src/roboticstoolbox/ets/ET.py b/src/roboticstoolbox/ets/ET.py new file mode 100644 index 000000000..cef5d2492 --- /dev/null +++ b/src/roboticstoolbox/ets/ET.py @@ -0,0 +1,439 @@ +#!/usr/bin/env python3 + +""" +@author: Jesse Haviland +""" + +import roboticstoolbox as rtb +from numpy import array, ndarray, pi +from spatialmath.base import trotx, troty, trotz +# Aliased: the bottom of this module rebinds the bare name `SE3` to the +# ET.SE3 classmethod (the free-function alias), so the spatialmath class +# needs its own name here. +from spatialmath import SE3 as SE3T + +from roboticstoolbox.ets._ET import BaseET, Sym, _AXIS_TO_INT, _resolve_param +from roboticstoolbox.ets.fknm import ET_T, ET_init, ET_update + + +class ET(BaseET): + # See BaseET._deepcopy_skip: the compiled acceleration handle is + # rebuilt by _accel_init() rather than deep-copied. + _deepcopy_skip = ("_ET__fknm",) + + def __init__(self, **kwargs): + # Set before super().__init__() runs: BaseET.__init__ may invoke + # the `param` setter (for a static ET), which calls _accel_update() + # below. `None` here tells _accel_update() the compiled struct + # doesn't exist yet, so it skips the sync instead of touching an + # attribute that isn't there yet. + self.__fknm = None + super().__init__(**kwargs) + # Now that BaseET.__init__ has finished (axis/param/T/joint/etc. are + # all final), do the one real build of the compiled struct. + self._accel_init() + + def __mul__(self, other: "ET") -> "rtb.ETS": + return rtb.ETS([self, other]) + + def __add__(self, other: "ET") -> "rtb.ETS": + return self.__mul__(other) + + # ------------------------------------------------------------------ + # Compiled C++ acceleration. ET2 has none of this - see BaseET's + # no-op _accel_init/_accel_update and pure-Python A(). + # ------------------------------------------------------------------ + def __axis_to_number(self, axis: str) -> int: + """ + Private convenience function which converts the axis string to an + integer for faster processing in the C extensions + """ + return _AXIS_TO_INT.get(axis, 0) + + def _accel_init(self) -> None: + """ + Build the compiled struct that holds this ET's data, from the + current (final) Python-side state. + """ + if self.jindex is None: + jindex = 0 + else: + jindex = self.jindex + + if self.qlim is None: + if self.kind[0] == "R": + qlim = array([-pi, pi]) + else: + qlim = array([0, 1]) + else: + qlim = self.qlim + + self.__fknm = ET_init( + self._isstaticsym, + self.isjoint, + self.isflip, + jindex, + self.__axis_to_number(self.kind), + self._T, + qlim, + ) + + def _accel_update(self) -> None: + """ + Push current Python-side state to the compiled struct. Called + whenever param/qlim/jindex change after construction. A no-op while + the struct doesn't exist yet (i.e. mid-__init__, before + _accel_init() has run for the first time). + """ + if self.__fknm is None: + return + + if self.jindex is None: + jindex = 0 + else: + jindex = self.jindex + + if self.qlim is None: + if self.kind[0] == "R": + qlim = array([-pi, pi]) + else: + qlim = array([0, 1]) + else: + qlim = self.qlim + + ET_update( + self.__fknm, + self._isstaticsym, + self.isjoint, + self.isflip, + jindex, + self.__axis_to_number(self.kind), + self._T, + qlim, + ) + + @property + def fknm(self): + return self.__fknm + + def A(self, q: float | Sym = 0.0) -> ndarray: + """ + Evaluate an elementary transformation + + :param q: Is used if this ET is variable (a joint) + :returns: The SE(3) matrix value of the ET + :rtype: ndarray + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.A() + >>> e = ET.tx() + >>> e.A(0.7) + + """ + try: + # Try and use the C implementation, flip is handled in C + return ET_T(self.__fknm, q) + except TypeError: + # We can't use the fast version (e.g. symbolic q), fall back + # to the pure-Python evaluation shared with ET2 + return super().A(q) + + @property + def s(self) -> ndarray: # pragma: nocover + if self.kind[1] == "x": + if self.kind[0] == "R": + return array([0, 0, 0, 1, 0, 0]) + else: + return array([1, 0, 0, 0, 0, 0]) + elif self.kind[1] == "y": + if self.kind[0] == "R": + return array([0, 0, 0, 0, 1, 0]) + else: + return array([0, 1, 0, 0, 0, 0]) + else: + if self.kind[0] == "R": + return array([0, 0, 0, 0, 0, 1]) + else: + return array([0, 0, 1, 0, 0, 0]) + + @classmethod + def Rx( + cls, + param: float | Sym | str | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, + ) -> "ET": + """ + Pure rotation about the x-axis + + :param param: rotation about the x-axis + :param unit: angular unit, "rad" [default] or "deg" + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET + + - ``ET.Rx(param)`` is an elementary rotation about the x-axis by a + constant angle + - ``ET.Rx()`` is an elementary rotation about the x-axis by a variable + angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in + this case. + + See Also + -------- + :func:`ET` + :func:`isrotation` + + :SymPy: supported + """ + param = _resolve_param(param, eta) + return cls(axis="Rx", param=param, axis_func=trotx, unit=unit, **kwargs) + + @classmethod + def Ry( + cls, + param: float | Sym | str | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, + ) -> "ET": + """ + Pure rotation about the y-axis + + :param param: rotation about the y-axis + :param unit: angular unit, "rad" [default] or "deg" + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET + + - ``ET.Ry(param)`` is an elementary rotation about the y-axis by a + constant angle + - ``ET.Ry()`` is an elementary rotation about the y-axis by a variable + angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in + this case. + + See Also + -------- + :func:`ET` + :func:`isrotation` + + :SymPy: supported + """ + param = _resolve_param(param, eta) + return cls(axis="Ry", param=param, axis_func=troty, unit=unit, **kwargs) + + @classmethod + def Rz( + cls, + param: float | Sym | str | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, + ) -> "ET": + """ + Pure rotation about the z-axis + + :param param: rotation about the z-axis + :param unit: angular unit, "rad" [default] or "deg" + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET + + - ``ET.Rz(param)`` is an elementary rotation about the z-axis by a + constant angle + - ``ET.Rz()`` is an elementary rotation about the z-axis by a variable + angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in + this case. + + See Also + -------- + :func:`ET` + :func:`isrotation` + + :SymPy: supported + """ + param = _resolve_param(param, eta) + return cls(axis="Rz", param=param, axis_func=trotz, unit=unit, **kwargs) + + @classmethod + def tx( + cls, + param: float | Sym | str | None = None, + *, + eta: float | None = None, + **kwargs, + ) -> "ET": + """ + Pure translation along the x-axis + + :param param: translation distance along the x-axis + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET + + - ``ET.tx(param)`` is an elementary translation along the x-axis by a + distance constant + - ``ET.tx()`` is an elementary translation along the x-axis by a + variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` + can be set in this case. + + See Also + -------- + :func:`ET` + :func:`istranslation` + + :SymPy: supported + """ + param = _resolve_param(param, eta) + + # this method is 3x faster than using lambda x: transl(x, 0, 0) + def axis_func(param): + # fmt: off + return array([ + [1, 0, 0, param], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + # fmt: on + + return cls(axis="tx", axis_func=axis_func, param=param, **kwargs) + + @classmethod + def ty( + cls, + param: float | Sym | str | None = None, + *, + eta: float | None = None, + **kwargs, + ) -> "ET": + """ + Pure translation along the y-axis + + :param param: translation distance along the y-axis + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET + + - ``ET.ty(param)`` is an elementary translation along the y-axis by a + distance constant + - ``ET.ty()`` is an elementary translation along the y-axis by a + variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` + can be set in this case. + + See Also + -------- + :func:`ET` + :func:`istranslation` + + :SymPy: supported + """ + param = _resolve_param(param, eta) + + def axis_func(param): + # fmt: off + return array([ + [1, 0, 0, 0], + [0, 1, 0, param], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + # fmt: on + + return cls(axis="ty", param=param, axis_func=axis_func, **kwargs) + + @classmethod + def tz( + cls, + param: float | Sym | str | None = None, + *, + eta: float | None = None, + **kwargs, + ) -> "ET": + """ + Pure translation along the z-axis + + :param param: translation distance along the z-axis + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET + + - ``ET.tz(param)`` is an elementary translation along the z-axis by a + distance constant + - ``ET.tz()`` is an elementary translation along the z-axis by a + variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` + can be set in this case. + + See Also + -------- + :func:`ET` + :func:`istranslation` + + :SymPy: supported + """ + param = _resolve_param(param, eta) + + def axis_func(param): + # fmt: off + return array([ + [1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, param], + [0, 0, 0, 1] + ]) + # fmt: on + + return cls(axis="tz", axis_func=axis_func, param=param, **kwargs) + + @classmethod + def SE3(cls, T: ndarray | SE3T, **kwargs) -> "ET": + """ + A static SE3 + + :param T: The SE3 transformation matrix + :returns: An elementary transform + :rtype: ET + + See Also + -------- + :func:`ET` + :func:`istranslation` + + :SymPy: supported + """ + + trans = T.A if isinstance(T, SE3T) else T + + return cls(axis="SE3", T=trans, **kwargs) + + +# --------------------------------------------------------------------------- +# Bare free-function aliases, so `from roboticstoolbox.ets.ET import *` pulls +# in exactly the 3D names (no ET2 tx/ty collision - see roboticstoolbox.ets.ET2 +# for the 2D equivalents, which can't be wildcard-imported alongside these +# since they share the tx/ty names with different meanings). +# --------------------------------------------------------------------------- +Rx = ET.Rx +Ry = ET.Ry +Rz = ET.Rz +tx = ET.tx +ty = ET.ty +tz = ET.tz +SE3 = ET.SE3 + +__all__ = ["ET", "Rx", "Ry", "Rz", "tx", "ty", "tz", "SE3"] diff --git a/src/roboticstoolbox/ets/ET2.py b/src/roboticstoolbox/ets/ET2.py new file mode 100644 index 000000000..68a42c822 --- /dev/null +++ b/src/roboticstoolbox/ets/ET2.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +""" +@author: Jesse Haviland +""" + +import roboticstoolbox as rtb +from numpy import array, ndarray +from spatialmath.base import trot2, transl2 +# Aliased: the bottom of this module rebinds the bare name `SE2` to the +# ET2.SE2 classmethod (the free-function alias), so the spatialmath class +# needs its own name here. +from spatialmath import SE2 as SE2T + +from roboticstoolbox.ets._ET import BaseET, Sym, _resolve_param + + +class ET2(BaseET): + def __init__(self, **kwargs): + super().__init__(**kwargs) + + def __mul__(self, other: "ET2") -> "rtb.ETS2": + return rtb.ETS2([self, other]) + + def __add__(self, other: "ET2") -> "rtb.ETS2": + return self.__mul__(other) + + @property + def s(self) -> ndarray: # pragma: nocover + if self.kind[0] == "R": + return array([0, 0, 0, 1]) + if self.kind[1] == "x": + return array([1, 0, 0, 0]) + elif self.kind[1] == "y": + return array([0, 1, 0, 0]) + else: + return array([0, 0, 1, 0]) + + @classmethod + def R( + cls, + param: float | Sym | str | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, + ) -> "ET2": + """ + Pure rotation + + :param param: rotation angle + :param unit: angular unit, "rad" [default] or "deg" + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET2 + + - ``ET2.R(param)`` is an elementary rotation by a constant angle + - ``ET2.R()`` is an elementary rotation by a variable angle, i.e. a + revolute robot joint. ``j`` or ``flip`` can be set in + this case. + + .. rubric:: Notes + + - In the 2D case this is rotation around the normal to the + xy-plane. + + See Also + -------- + :func:`ET2`, :func:`isrotation` + + """ + param = _resolve_param(param, eta) + return cls( + axis="R", param=param, axis_func=lambda theta: trot2(theta), unit=unit, **kwargs + ) + + @classmethod + def tx( + cls, + param: float | Sym | str | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, + ) -> "ET2": + """ + Pure translation along the x-axis + + :param param: translation distance along the x-axis + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET2 + + - ``ET2.tx(param)`` is an elementary translation along the x-axis by a + distance constant + - ``ET2.tx()`` is an elementary translation along the x-axis by a + variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` + can be set in this case. + + See Also + -------- + :func:`ET2` + :func:`istranslation` + + """ + param = _resolve_param(param, eta) + return cls(axis="tx", param=param, axis_func=lambda x: transl2(x, 0), **kwargs) + + @classmethod + def ty( + cls, + param: float | Sym | str | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, + ) -> "ET2": + """ + Pure translation along the y-axis + + :param param: translation distance along the y-axis + :param j: Explicit joint number within the robot + :param flip: Joint moves in opposite direction + :returns: An elementary transform + :rtype: ET2 + + - ``ET2.ty(param)`` is an elementary translation along the y-axis by a + distance constant + - ``ET2.ty()`` is an elementary translation along the y-axis by a + variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` + can be set in this case. + + See Also + -------- + :func:`ET2` + + """ + param = _resolve_param(param, eta) + return cls(axis="ty", param=param, axis_func=lambda y: transl2(0, y), **kwargs) + + @classmethod + def SE2(cls, T: ndarray | SE2T, **kwargs) -> "ET2": + """ + A static SE2 + + :param T: The SE2 transformation matrix + :returns: An elementary transform + :rtype: ET2 + + See Also + -------- + :func:`ET2` + :func:`istranslation` + + :SymPy: supported + """ + + trans = T.A if isinstance(T, SE2T) else T + + return cls(axis="SE2", T=trans, **kwargs) + + # A() is inherited from BaseET: ET2 has no compiled acceleration, so + # the shared pure-Python evaluation is all it ever needed. + + +# --------------------------------------------------------------------------- +# Bare free-function aliases, so `from roboticstoolbox.ets.ET2 import *` pulls +# in exactly the 2D names. Note tx/ty here are 2D and mean something +# different to roboticstoolbox.ets.ET's tx/ty (3D) - importing both modules' +# wildcards into the same namespace will have the second import's tx/ty win. +# --------------------------------------------------------------------------- +R = ET2.R +tx = ET2.tx +ty = ET2.ty +SE2 = ET2.SE2 + +__all__ = ["ET2", "R", "tx", "ty", "SE2"] diff --git a/src/roboticstoolbox/robot/ETS.py b/src/roboticstoolbox/ets/ETS.py similarity index 69% rename from src/roboticstoolbox/robot/ETS.py rename to src/roboticstoolbox/ets/ETS.py index 8dd4b0a25..aaa71e3fd 100644 --- a/src/roboticstoolbox/robot/ETS.py +++ b/src/roboticstoolbox/ets/ETS.py @@ -6,8 +6,7 @@ """ from __future__ import annotations -from collections.abc import MutableSequence -from functools import wraps, cached_property +from functools import cached_property import numpy as np from numpy.random import uniform from numpy.linalg import inv, det, cond, svd @@ -26,7 +25,7 @@ from roboticstoolbox.tools.params import rtb_get_param from roboticstoolbox.robot.IK import IK_GN, IK_LM, IK_NR, IK_QP -from roboticstoolbox.robot.fknm import ( +from roboticstoolbox.ets.fknm import ( ETS_init, ETS_fkine, ETS_jacob0, @@ -38,618 +37,13 @@ IK_LM_c, ) from copy import deepcopy -from roboticstoolbox.robot.ET import ET, ET2, BaseET +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets._ET import BaseET +from roboticstoolbox.ets._ETS import BaseETS, T, _dirties_fknm from typing import overload, TypeVar from typing import Literal as L from roboticstoolbox.tools.types import ArrayLike, NDArray -T = TypeVar("T", bound="BaseETS") - - -def _dirties_fknm(func): - @wraps(func) - def wrapper(self, *args, **kwargs): - result = func(self, *args, **kwargs) - self._fknm_stale = True - return result - return wrapper - - -class BaseETS(MutableSequence): - def __init__(self): - self._data: list = [] - self._fknm_stale = True - self._BaseETS__fknm = None - - # ------------------------------------------------------------------ - # MutableSequence abstract methods - # ------------------------------------------------------------------ - - def __len__(self) -> int: - return len(self._data) - - def __getitem__(self, i): - return self._data[i] - - @_dirties_fknm - def __setitem__(self, i, value): - self._data[i] = value - - @_dirties_fknm - def __delitem__(self, i): - del self._data[i] - - @_dirties_fknm - def insert(self, index: int, value) -> None: - self._data.insert(index, value) - - def __repr__(self) -> str: - return repr(self._data) - - def __eq__(self, other: object) -> bool: - if isinstance(other, BaseETS): - return self._data == other._data - return NotImplemented - - __hash__ = None # type: ignore[assignment] - - # ------------------------------------------------------------------ - # C handle: lazy build on first use after any mutation - # ------------------------------------------------------------------ - - @property - def _fknm(self): - if self._fknm_stale: - self._copy_to_cpp() - return self._BaseETS__fknm - - def _copy_to_cpp(self): - self._BaseETS__fknm = ETS_init( - [et.fknm for et in self._data], - self.n, - self.m, - ) - self._fknm_stale = False - - def __str__(self, q: str | None = None): - """ - Pretty prints the ETS - - ``q`` controls how the joint variables are displayed: - - - None, format depends on number of joint variables - - one, display joint variable as q - - more, display joint variables as q0, q1, ... - - if a joint index was provided, use this value - - "", display all joint variables as empty parentheses ``()`` - - "θ", display all joint variables as ``(θ)`` - - format string with passed joint variables ``(j, j+1)``, so "θ{0}" - would display joint variables as θ0, θ1, ... while "θ{1}" would - display joint variables as θ1, θ2, ... ``j`` is either the joint - index, if provided, otherwise a sequential value. - - :param q: control how joint variables are displayed - :returns: Pretty printed ETS - :rtype: str - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() - >>> print(e[:2]) - >>> print(e) - >>> print(e.__str__("")) - >>> print(e.__str__("θ{0}")) # numbering from 0 - >>> print(e.__str__("θ{1}")) # numbering from 1 - >>> # explicit joint indices - >>> e = ET.Rz(jindex=3) * ET.tx(1) * ET.Rz(jindex=4) - >>> print(e) - >>> print(e.__str__("θ{0}")) - - Angular parameters are converted to degrees, except if they - are symbolic. - - .. runblock:: pycon - >>> from roboticstoolbox import ET - >>> from spatialmath.base import symbol - >>> theta, d = symbol('theta, d') - >>> e = ET.Rx(theta) * ET.tx(2) * ET.Rx(45, 'deg') * ET.Ry(0.2) * ET.ty(d) - >>> str(e) - - """ - - es = [] - j = 0 - c = 0 - s = None - unicode = rtb_get_param("unicode") - - # An empty SE3 - if len(self._data) == 0: - return "SE3()" - - if q is None: - if len(self.joints()) > 1: - q = "q{0}" - else: - q = "q" - - # For et in the object, display it, data comes from properties - # which come from the named tuple - for et in self._data: - if et.isjoint: - if q is not None: - if et.jindex is None: # pragma: nocover this is no longer possible - _j = j - else: - _j = et.jindex - qvar = q.format( - _j, _j + 1 - ) - # else: - # qvar = "" - - if et.isflip: - s = f"{et.axis}(-{qvar})" - else: - s = f"{et.axis}({qvar})" - j += 1 - - elif et.isrotation: - if issymbol(et.eta): - s = f"{et.axis}({et.eta})" - else: - s = f"{et.axis}({et.eta * 180 / np.pi:.4g}°)" - - elif et.istranslation: - try: - s = f"{et.axis}({et.eta:.4g})" - except TypeError: # pragma: nocover - s = f"{et.axis}({et.eta})" - - elif not et.iselementary: - s = str(et) - c += 1 - - es.append(s) - - if unicode: - return " \u2295 ".join(es) - else: # pragma: nocover - return " * ".join(es) - - def _repr_pretty_(self, p, cycle): - """ - Pretty string for IPython - - Print stringified version when variable is displayed in IPython, ie. on - a line by itself. - - :param p: pretty printer handle (ignored) - :param cycle: pretty printer flag (ignored) - - Examples - -------- - - In [1]: e - Out [1]: R(q0) ⊕ tx(1) ⊕ R(q1) ⊕ tx(1) - - """ - - print(self.__str__()) # pragma: nocover - - def joint_idx(self) -> list[int]: - """ - Get index of joint transforms - - :returns: indices of transforms that are joints - :rtype: ndarray - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e.joint_idx() - - """ - - return np.where([e.isjoint for e in self])[0] # type: ignore - - def joints(self) -> list[ET]: - """ - Get a list of the variable ETs with this ETS - - :returns: list of ETs that are joints - :rtype: list[ET] - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e.joints() - - """ - - return [e for e in self if e.isjoint] - - def jindex_set(self) -> set[int]: # - """ - Get set of joint indices - - :returns: set of unique joint indices - :rtype: set[int] - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) - >>> e.jointset() - - """ - - return set([self[j].jindex for j in self.joint_idx()]) # type: ignore - - @cached_property - def jindices(self) -> NDArray: - """ - Get an array of joint indices - - :returns: array of unique joint indices - :rtype: ndarray - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) - >>> e.jointset() - - """ - - return np.array([j.jindex for j in self.joints()]) # type: ignore - - @property - def qlim(self): - r""" - Get/Set Joint limits - - Limits are extracted from the link objects. If joints limits are - not set for: - - - a revolute joint [-𝜋. 𝜋] is returned - - a prismatic joint an exception is raised - - :param new_qlim: new joint limits to set - :type new_qlim: ndarray(2,n) - :returns: array of joint limit values - :rtype: ndarray(2,n) - :raises ValueError: unset limits for a prismatic joint - - Examples - -------- - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.qlim - - """ - - limits = np.zeros((2, self.n)) - - for i, et in enumerate(self.joints()): - if et.isrotation: - if et.qlim is None: - v = [-np.pi, np.pi] - else: - v = et.qlim - elif et.istranslation: - if et.qlim is None: - raise ValueError("undefined prismatic joint limit") - else: - v = et.qlim - else: - raise ValueError("Undefined Joint Type") # pragma: nocover - limits[:, i] = v - - return limits - - @qlim.setter - def qlim(self, new_qlim: ArrayLike): - new_qlim = np.array(new_qlim) - - if new_qlim.shape == (2,) and self.n == 1: - new_qlim = new_qlim.reshape(2, 1) - - if new_qlim.shape != (2, self.n): - raise ValueError("new_qlim must be of shape (2, n)") - - for j, i in enumerate(self.joint_idx()): - et = self[i] - et.qlim = new_qlim[:, j] - self[i] = et - - @property - def structure(self) -> str: - """ - Joint structure string - - A string comprising the characters 'R' or 'P' which indicate the types - of joints in order from left to right. - - :returns: a string indicating the joint types - :rtype: str - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e.structure - - """ - - return "".join( - ["R" if self._data[i].isrotation else "P" for i in self.joint_idx()] - ) - - @property - def n(self) -> int: - """ - Number of joints - - :returns: the number of joints in the ETS - :rtype: int - - Counts the number of joints in the ETS. - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rx() * ET.tx(1) * ET.tz() - >>> e.n - - See Also - -------- - :func:`joints` - - """ - - return sum(1 for et in self._data if et.isjoint) - - @property - def m(self) -> int: - """ - Number of transforms - - :returns: the number of transforms in the ETS - :rtype: int - - Counts the number of transforms in the ETS. - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rx() * ET.tx(1) * ET.tz() - >>> e.m - - """ - - return len(self._data) - - @overload - def data(self: "ETS") -> list[ET]: ... # pragma: nocover - - @overload - def data(self: "ETS2") -> list[ET2]: ... # pragma: nocover - - @property - def data(self): - return self._data - - @data.setter - @overload - def data(self: "ETS", new_data: list[ET]): ... # pragma: nocover - - @data.setter - @overload - def data(self: "ETS", new_data: list[ET2]): ... # pragma: nocover - - @data.setter - def data(self, new_data): - self._data = new_data - self._fknm_stale = True - - @overload - def split(self: "ETS") -> list["ETS"]: ... # pragma: nocover - - @overload - def split(self: "ETS2") -> list["ETS2"]: ... # pragma: nocover - - def split(self): - """ - Split ETS into link segments - - :returns: a list of ETS, each one, apart from the last, ends with a variable ET. - - """ - - segments = [] - start = 0 - - for j, k in enumerate(self.joint_idx()): - ets_j = self._data[start : k + 1] - start = k + 1 - segments.append(self.__class__(ets_j)) - - tail = self._data[start:] - - if len(tail) > 0: - segments.append(self.__class__(tail)) - - return segments - - def inv(self: T) -> T: - r""" - Inverse of ETS - - The inverse of a given ETS. It is computed as the inverse of the - individual ETs in the reverse order. - - .. math:: - - (\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} ) - - :returns: Inverse of the ETS - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz(jindex=2) * ET.tx(1) * ET.Rx(jindex=3,flip=True) * ET.tx(1) - >>> print(e) - >>> print(e.inv()) - - .. rubric:: Notes - - - It is essential to use explicit joint indices to account for - the reversed order of the transforms. - - """ - - return self.__class__([et.inv() for et in reversed(self._data)]) # type: ignore[call-arg] - - @overload - def __getitem__(self: "BaseETS", i: int) -> BaseET: ... - - @overload - def __getitem__(self: "ETS", i: int) -> ET: ... - - @overload - def __getitem__(self: "ETS", i: slice) -> list[ET]: ... - - @overload - def __getitem__(self: "ETS2", i: int) -> ET2: ... - - @overload - def __getitem__(self: "ETS2", i: slice) -> list[ET2]: ... - - def __getitem__(self, i): - """ - Index or slice an ETS - - :param i: the index or slice - :returns: elementary transform - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e[0] - >>> e[1] - >>> e[1:3] - - """ - return self._data[i] # can be [2] or slice, eg. [3:5] - - def __deepcopy__(self, memo): - new_data = [] - - for data in self: - new_data.append(deepcopy(data)) - - cls = self.__class__ - result = cls(new_data) - memo[id(self)] = result - return result - - def plot(self, *args, **kwargs): - from roboticstoolbox.robot.Robot import Robot, Robot2 - - if isinstance(self, ETS): - robot = Robot(self) - else: - robot = Robot2(self) - - robot.plot(*args, **kwargs) - - def teach(self, *args, **kwargs): - from roboticstoolbox.robot.Robot import Robot, Robot2 - - if isinstance(self, ETS): - robot = Robot(self) - else: - robot = Robot2(self) - - robot.teach(*args, **kwargs) - - def random_q(self, i: int = 1) -> NDArray: - """ - Generate a random valid joint configuration - - :param i: number of configurations to generate - :returns: random joint configuration - :rtype: ndarray(n,) or ndarray(i,n) - - Generates a random q vector within the joint limits defined by - ``self.qlim``. - - Examples - -------- - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.Panda() - >>> ets = robot.ets() - >>> q = ets.random_q() - >>> q - - """ - - if i == 1: - q = np.zeros(self.n) - - for i in range(self.n): - q[i] = uniform(self.qlim[0, i], self.qlim[1, i]) - - else: - q = np.zeros((i, self.n)) - - for j in range(i): - for i in range(self.n): - q[j, i] = uniform(self.qlim[0, i], self.qlim[1, i]) - - return q - class ETS(BaseETS): """ @@ -2466,406 +1860,154 @@ def ikine_QP( return solver.solve(ets=self, Tep=Tep, q0=q0) - -class ETS2(BaseETS): - """ - This class implements an elementary transform sequence (ETS) for 2D - - :param arg: Function to compute ET value - - An instance can contain an elementary transform (ET) or an elementary - transform sequence (ETS). It has list-like properties by subclassing - UserList, which means we can perform indexing, slicing pop, insert, as well - as using it as an iterator over its values. - - - ``ETS()`` an empty ETS list - - ``ET2.XY(η)`` is a constant elementary transform - - ``ET2.XY(η, 'deg')`` as above but the angle is expressed in degrees - - ``ET2.XY()`` is a joint variable, the value is left free until evaluation - time - - ``ET2.XY(j=J)`` as above but the joint index is explicitly given, this - might correspond to the joint number of a multi-joint robot. - - ``ET2.XY(flip=True)`` as above but the joint moves in the opposite sense - - where ``XY`` is one of ``R``, ``tx``, ``ty``. - - Example: - - .. runblock:: pycon - - >>> from roboticstoolbox import ETS2 as ET2 - >>> e = ET2.R(0.3) # a single ET, rotation about z - >>> len(e) - >>> e = ET2.R(0.3) * ET2.tx(2) # an ETS - >>> len(e) # of length 2 - >>> e[1] # an ET sliced from the ETS - - :references: - - Kinematic Derivatives using the Elementary Transform Sequence, - J. Haviland and P. Corke - - :seealso: :func:`r`, :func:`tx`, :func:`ty` - """ - - def __init__( - self, - arg: list[ETS2 | ET2] | list[ET2] | list[ETS2] | ET2 | ETS2 | None = None, - ): - super().__init__() - if isinstance(arg, list): - for item in arg: - if isinstance(item, ET2): - self._data.append(deepcopy(item)) - elif isinstance(item, ETS2): - for ets_item in item: - self._data.append(deepcopy(ets_item)) - else: - raise TypeError("bad arg") - elif isinstance(arg, ET2): - self._data.append(deepcopy(arg)) - elif isinstance(arg, ETS2): - for ets_item in arg: - self._data.append(deepcopy(ets_item)) - elif arg is not None: - raise TypeError("bad arg") - self._ndims = 2 - self._auto_jindex = False - - # Check if jindices are set - joints = self.joints() - - # Number of joints with a jindex - jindices = 0 - - # Number of joints with a sequential jindex (j[2] -> jindex = 2) - seq_jindex = 0 - - # Count them up - for j, joint in enumerate(joints): - if joint.jindex is not None: - jindices += 1 - if joint.jindex == j: - seq_jindex += 1 - - if ( - jindices == self.n - 1 - and seq_jindex == self.n - 1 - and joints[-1].jindex is None - ): - # ets has sequential jindicies, except for the last. - joints[-1].jindex = self.n - 1 - self._auto_jindex = True - elif jindices > 0 and not jindices == self.n: - raise ValueError( - "You can not have some jindices set for the ET's in arg. It must be all" - " or none" - ) # pragma: nocover - elif jindices == 0 and self.n > 0: - # Set them ourself - for j, joint in enumerate(joints): - joint.jindex = j - self._auto_jindex = True - - def __mul__(self, other: ET2 | ETS2) -> "ETS2": - if isinstance(other, ET2): - return ETS2([*self._data, other]) - else: - return ETS2([*self._data, *other._data]) # pragma: nocover - - def __rmul__(self, other: ET2 | ETS2) -> "ETS2": - return ETS2([other, *self._data]) # pragma: nocover - - def __imul__(self, rest: "ETS2"): - return self + rest # pragma: nocover - - def __add__(self, rest) -> "ETS2": - return self.__mul__(rest) # pragma: nocover - - def compile(self) -> "ETS2": + @staticmethod + def _template_consume(elements: list["BaseET"], template: list[str]) -> int: """ - Compile an ETS2 - - :return: optimised ETS2 - - Perform constant folding for faster evaluation. Consecutive constant - ETs are compounded, leading to a constant ET which is denoted by - ``SE3`` when displayed. - - :seealso: :func:`isconstant` + Greedily match a run of ETs against an ordered template of ``kind`` + strings, where each template slot is optional but present slots must + occur in the given order. + + :param elements: ETs to match, in the order they occur in the ETS + :param template: ordered ``kind`` strings the elements may occupy + :returns: number of leading ``elements`` consumed before the first + one that fits no remaining template slot """ - const = None - ets = ETS2() - - for et in self: - if et.isjoint: - # a joint - if const is not None: - # flush the constant - if not np.array_equal(const, np.eye(3)): - ets *= ET2.SE2(const) - const = None - ets *= et # emit the joint ET + slot = 0 + consumed = 0 + for et in elements: + if et.kind in template[slot:]: + slot = template.index(et.kind, slot) + 1 + consumed += 1 else: - # not a joint - if const is None: - const = et.A() - else: - const = const @ et.A() - - if const is not None: - # flush the constant, tool transform - if not np.array_equal(const, np.eye(3)): - ets *= ET2.SE2(const) - return ets - - def insert( # type: ignore[override] - self, - i: int, - arg: ET2 | ETS2, - ) -> None: - """ - Insert value - - :param i: position to insert at - :param arg: the elementary transform or sequence to insert - - Inserts an ET or ETS into the ET sequence. The inserted value is at position - ``i``. - - Example: - - .. runblock:: pycon - - >>> from roboticstoolbox import ET2 - >>> e = ET2.R() * ET2.tx(1) * ET2.R() * ET2.tx(1) - >>> f = ET2.R() - >>> e.insert(2, f) - >>> e - """ - - if isinstance(arg, ET2): - self._data.insert(i, arg) - elif isinstance(arg, ETS2): - for j, et in enumerate(arg): - self._data.insert(i + j, et) - self._fknm_stale = True + break + return consumed - def fkine( - self, - q: ArrayLike, - base: NDArray | SE2 | None = None, - tool: NDArray | SE2 | None = None, - include_base: bool = True, - ) -> SE2: + def _split_convention(self, template: list[str], joint_slots: set[int], exposed: str): """ - Forward kinematics - - :param q: joint coordinates - :param base: base transform, optional - :param tool: tool transform, optional - :returns: transformation matrix representing the end-effector pose - :rtype: SE2 - - ``T = ets.fkine(q)`` evaluates forward kinematics for the robot at - joint configuration ``q``. - - **Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a - trajectory and the result is an ``SE2`` instance with ``m`` values. - - .. note:: - - - The robot's base tool transform, if set, is incorporated into the result. - - A tool transform, if provided, is incorporated into the result. - - Works from the end-effector link to the base - - .. rubric:: References - - - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke + Split according to a DH-like convention. + + A segment is a 4-slot ordered ``template`` (e.g. Rz, tz, tx, Rx for + DH); each slot is optional except that exactly one of the two + ``joint_slots`` must be occupied by the segment's joint. Content + between two joints must fully account for the gap in template + order, else ``ValueError``. The end named by ``exposed`` (``"head"`` + or ``"tail"``) is reported separately and unvalidated; the other end + is folded into the boundary segment. """ - - ret = SE2.Empty() - fk = self.eval(q, base, tool, include_base) - - if fk.dtype == "O": - # symbolic - fk = np.array(simplify(fk)) - - if fk.ndim == 3: - for T in fk: - ret.append(SE2(T, check=False)) # type: ignore - else: - ret = SE2(fk, check=False) - - return ret - - def eval( - self, - q: ArrayLike, - base: NDArray | SE2 | None = None, - tool: NDArray | SE2 | None = None, - include_base: bool = True, - ) -> NDArray: - """ - Forward kinematics (returns raw ndarray) - - :param q: joint coordinates - :param base: base transform, optional - :param tool: tool transform, optional - :returns: transformation matrix representing the end-effector pose - :rtype: ndarray(3,3) or ndarray(m,3,3) - - ``T = ets.eval(q)`` evaluates forward kinematics for the robot at - joint configuration ``q``, returning the raw ndarray. - - **Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a - trajectory and the result is an ndarray of shape (m,3,3). - - .. note:: - - - The robot's base tool transform, if set, is incorporated into the result. - - A tool transform, if provided, is incorporated into the result. - - Works from the end-effector link to the base - - .. rubric:: References - - - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke - """ - - q = getmatrix(q, (None, None)) - l, _ = q.shape # type: ignore - end = self[-1] - - if base is None: - bases = None - elif isinstance(base, SE2): - bases = np.array(base.A) - elif np.array_equal(base, np.eye(3)): # pragma: nocover - bases = None - else: # pragma: nocover - bases = base - - if tool is None: - tools = None - elif isinstance(tool, SE2): - tools = np.array(tool.A) - elif np.array_equal(tool, np.eye(3)): # pragma: nocover - tools = None - else: # pragma: nocover - tools = tool - - if l > 1: - T = np.zeros((l, 3, 3), dtype=object) + idx = list(self.joint_idx()) + if len(idx) == 0: + raise ValueError("ETS has no joints") + + def joint_slot(et: "ET") -> int: + if et.kind not in template or template.index(et.kind) not in joint_slots: + raise ValueError( + "ETS is not a valid DH/MDH parameterisation: " + f"{et} is not a permitted joint for this convention" + ) + return template.index(et.kind) + + slots = [joint_slot(self[k]) for k in idx] + start = [0] * len(idx) + end = [0] * len(idx) + + if exposed == "head": + gap = list(reversed(self[0 : idx[0]])) + sub = list(reversed(template[0 : slots[0]])) + consumed = self._template_consume(gap, sub) + start[0] = idx[0] - consumed + head = self[0 : start[0]] else: - T = np.zeros((3, 3), dtype=object) - - for k, qk in enumerate(q): # type: ignore - link = end # start with last link - - jindex = 0 if link.jindex is None and link.isjoint else link.jindex - Tk = link.A(qk[jindex]) - - if tools is not None: - Tk = Tk @ tools - - # add remaining links, back toward the base - for i in range(self.m - 2, -1, -1): - link = self._data[i] - - jindex = 0 if link.jindex is None and link.isjoint else link.jindex - A = link.A(qk[jindex]) - - if A is not None: - Tk = A @ Tk - - # add base transform if it is set - if include_base is True and bases is not None: - Tk = bases @ Tk - - # append - if l > 1: - T[k, :, :] = Tk - # ret.append(SE2(Tk, check=False)) # type: ignore - else: - T = Tk - # ret = SE2(Tk, check=False) - - return T - - def jacob0( - self, - q: ArrayLike, - ) -> NDArray: - # very inefficient implementation, just put a 1 in last row - # if its a rotation joint - q = getvector(q) - - j = 0 - J = np.zeros((3, self.n)) - etjoints = self.joint_idx() - - if not np.all(np.array([self[i].jindex for i in etjoints])): - # not all joints have a jindex it is required, set them - for j in range(self.n): - i = etjoints[j] - self[i].jindex = j - - for j in range(self.n): - i = etjoints[j] - - if self[i].jindex is not None: - jindex = self[i].jindex - else: - jindex = 0 # pragma: nocover - - # jindex = 0 if self[i].jindex is None else self[i].jindex - - axis = self[i].axis - if axis == "R": - dTdq = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) @ self[i].A( - q[jindex] # type: ignore + head = self[0:0] + + for i, k in enumerate(idx): + hi_bound = idx[i + 1] if i + 1 < len(idx) else len(self) + consumed = self._template_consume( + list(self[k + 1 : hi_bound]), template[slots[i] + 1 :] + ) + end[i] = k + 1 + consumed + + if i + 1 < len(idx): + gap = self[end[i] : idx[i + 1]] + gap_consumed = self._template_consume( + list(gap), template[0 : slots[i + 1]] ) - elif axis == "tx": - dTdq = np.array([[0, 0, 1], [0, 0, 0], [0, 0, 0]]) - elif axis == "ty": - dTdq = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 0]]) - else: # pragma: nocover - raise TypeError("Invalid axes") - - E0 = ETS2(self[:i]) - if len(E0) > 0: - dTdq = E0.fkine(q).A @ dTdq - - Ef = ETS2(self[i + 1 :]) - if len(Ef) > 0: - dTdq = dTdq @ Ef.fkine(q).A - - T = self.fkine(q).A - dRdt = dTdq[:2, :2] @ T[:2, :2].T + if gap_consumed != len(gap): + raise ValueError( + "ETS is not a valid DH/MDH parameterisation: " + f"unexpected term at index {end[i] + gap_consumed}" + ) + start[i + 1] = end[i] - J[:2, j] = dTdq[:2, 2] - J[2, j] = dRdt[1, 0] + if exposed == "tail": + tail = self[end[-1] :] + else: + end[-1] = len(self) + tail = self[0:0] - return J + segments = [self.__class__(self[start[i] : end[i]]) for i in range(len(idx))] + return segments, head, tail - def jacobe( - self, - q: ArrayLike, - ): + def split(self, method: str = "last") -> list["ETS"]: r""" - Jacobian in end-effector frame + Split ETS into link segments - :param q: joint coordinates - :returns: Jacobian matrix - :rtype: ndarray(3,n) + :param method: one of ``"first"``, ``"last"`` (default), ``"dh"``, or ``"mdh"``. + :returns: ``[base, *segments, gripper]`` -- a list of length + ``n_joints + 2``. ``base``/``gripper`` are empty ETS when the + method has no concept of one (e.g. ``"dh"`` never populates + ``gripper``, ``"mdh"`` never populates ``base``). + + Split an ETS into segments representing links, plus a base and gripper segment. + Unpack with ``base, *segments, gripper = ets.split(method)``. + + The behaviour depends on the ``method`` argument: + + * ``"first"``: each link segment begins with a joint and continues upto, but not + including the next joint. Any constant ET before the first joint are part of + the base. There are no gripper ET, they are included in the last link segment. + * ``"last"``: each link segment ends with a joint and include all ET after the + previous joint. Any constant ET after the last joint are part of the gripper. + There are no base ET, they are included in the first link segment. + * ``"dh"``: similar to ``"first"`` but each segment is validated against the + Denavit-Hartenberg convention: an ordered, 4-slot template + Rz($θ_j$) tz($d_j$) tx($a_j$) Rx($α_j$), exactly one of Rz/tz being the + segment's joint and the rest optional (but present slots must occur in + this order). Content between two joints that cannot be accounted for by + the template raises ``ValueError``. The base (content before the first + joint's template slots) is unvalidated and reported separately; trailing + content past the last joint's template is folded into the last segment. + * ``"mdh"``: similar to ``"last"`` but validated against the modified + Denavit-Hartenberg convention, template tx($a_{j-1}$) Rx($α_{j-1}$) + Rz($θ_j$) tz($d_j$), same rules as ``"dh"`` mirrored: the gripper + (content past the last joint's template slots) is unvalidated and + reported separately; leading content before the first joint's template + is folded into the first segment. - ``jacobe(q)`` is the manipulator Jacobian matrix which maps joint - velocity to end-effector spatial velocity. + .. runblock:: pycon + >>> from roboticstoolbox.ets.ETS import * + >>> e = tz(1) * Rx("q1") * tx(2) * Ry("q2") * ty(3) * Rz("q3") * tz(4) + >>> base, *segments, gripper = e.split("first") + >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") + >>> base, *segments, gripper = e.split("last") + >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") + >>> e = tz(1) * Rz("q1") * tx(2) * Rx(90, 'deg') * Rz("q2") * tz(4) * Rz(180, 'deg') * tz("q3") * Rz(270, 'deg') * tz(4) + >>> base, *segments, gripper = e.split("dh") + >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") + >>> base, *segments, gripper = e.split("mdh") + >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") + """ - End-effector spatial velocity :math:`\nu = (v_x, v_y, \omega)^T` - is related to joint velocity by :math:`{}^{e}\nu = {}^{e}\mathbf{J}_0(q) \dot{q}`. + match method.lower(): + case "dh": + segments, head, tail = self._split_convention( + ["Rz", "tz", "tx", "Rx"], {0, 1}, exposed="head" + ) + case "mdh": + segments, head, tail = self._split_convention( + ["tx", "Rx", "Rz", "tz"], {2, 3}, exposed="tail" + ) + case _: + return super().split(method) # type: ignore - :seealso: :func:`jacob0`, :func:`hessian0` - """ + return [head] + segments + [tail] # type: ignore - T = self.fkine(q, include_base=False).A - return tr2jac2(T.T) @ self.jacob0(q) diff --git a/src/roboticstoolbox/ets/ETS2.py b/src/roboticstoolbox/ets/ETS2.py new file mode 100644 index 000000000..2b7db3d5c --- /dev/null +++ b/src/roboticstoolbox/ets/ETS2.py @@ -0,0 +1,449 @@ +#!/usr/bin/env python3 + +""" +@author: Jesse Haviland +@author: Peter Corke +""" + +from __future__ import annotations +from functools import cached_property +import numpy as np +from numpy.random import uniform +from numpy.linalg import inv, det, cond, svd +from spatialmath import SE3, SE2 +from spatialmath.base import ( + getvector, + issymbol, + tr2jac, + verifymatrix, + tr2jac2, + t2r, + rotvelxform, + simplify, + getmatrix, +) +from roboticstoolbox.tools.params import rtb_get_param +from roboticstoolbox.robot.IK import IK_GN, IK_LM, IK_NR, IK_QP + +from roboticstoolbox.ets.fknm import ( + ETS_init, + ETS_fkine, + ETS_jacob0, + ETS_jacobe, + ETS_hessian0, + ETS_hessiane, + IK_NR_c, + IK_GN_c, + IK_LM_c, +) +from copy import deepcopy +from roboticstoolbox.ets.ET2 import ET2 +from roboticstoolbox.ets._ET import BaseET +from roboticstoolbox.ets._ETS import BaseETS, T, _dirties_fknm +from typing import overload, TypeVar +from typing import Literal as L +from roboticstoolbox.tools.types import ArrayLike, NDArray + + +class ETS2(BaseETS): + """ + This class implements an elementary transform sequence (ETS) for 2D + + :param arg: Function to compute ET value + + An instance can contain an elementary transform (ET) or an elementary + transform sequence (ETS). It has list-like properties by subclassing + UserList, which means we can perform indexing, slicing pop, insert, as well + as using it as an iterator over its values. + + - ``ETS()`` an empty ETS list + - ``ET2.XY(η)`` is a constant elementary transform + - ``ET2.XY(η, 'deg')`` as above but the angle is expressed in degrees + - ``ET2.XY()`` is a joint variable, the value is left free until evaluation + time + - ``ET2.XY(j=J)`` as above but the joint index is explicitly given, this + might correspond to the joint number of a multi-joint robot. + - ``ET2.XY(flip=True)`` as above but the joint moves in the opposite sense + + where ``XY`` is one of ``R``, ``tx``, ``ty``. + + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import ETS2 as ET2 + >>> e = ET2.R(0.3) # a single ET, rotation about z + >>> len(e) + >>> e = ET2.R(0.3) * ET2.tx(2) # an ETS + >>> len(e) # of length 2 + >>> e[1] # an ET sliced from the ETS + + :references: + - Kinematic Derivatives using the Elementary Transform Sequence, + J. Haviland and P. Corke + + :seealso: :func:`r`, :func:`tx`, :func:`ty` + """ + + def __init__( + self, + arg: list[ETS2 | ET2] | list[ET2] | list[ETS2] | ET2 | ETS2 | None = None, + ): + super().__init__() + if isinstance(arg, list): + for item in arg: + if isinstance(item, ET2): + self._data.append(deepcopy(item)) + elif isinstance(item, ETS2): + for ets_item in item: + self._data.append(deepcopy(ets_item)) + else: + raise TypeError("bad arg") + elif isinstance(arg, ET2): + self._data.append(deepcopy(arg)) + elif isinstance(arg, ETS2): + for ets_item in arg: + self._data.append(deepcopy(ets_item)) + elif arg is not None: + raise TypeError("bad arg") + self._ndims = 2 + self._auto_jindex = False + + # Check if jindices are set + joints = self.joints() + + # Number of joints with a jindex + jindices = 0 + + # Number of joints with a sequential jindex (j[2] -> jindex = 2) + seq_jindex = 0 + + # Count them up + for j, joint in enumerate(joints): + if joint.jindex is not None: + jindices += 1 + if joint.jindex == j: + seq_jindex += 1 + + if ( + jindices == self.n - 1 + and seq_jindex == self.n - 1 + and joints[-1].jindex is None + ): + # ets has sequential jindicies, except for the last. + joints[-1].jindex = self.n - 1 + self._auto_jindex = True + elif jindices > 0 and not jindices == self.n: + raise ValueError( + "You can not have some jindices set for the ET's in arg. It must be all" + " or none" + ) # pragma: nocover + elif jindices == 0 and self.n > 0: + # Set them ourself + for j, joint in enumerate(joints): + joint.jindex = j + self._auto_jindex = True + + def __mul__(self, other: ET2 | ETS2) -> "ETS2": + if isinstance(other, ET2): + return ETS2([*self._data, other]) + else: + return ETS2([*self._data, *other._data]) # pragma: nocover + + def __rmul__(self, other: ET2 | ETS2) -> "ETS2": + return ETS2([other, *self._data]) # pragma: nocover + + def __imul__(self, rest: "ETS2"): + return self + rest # pragma: nocover + + def __add__(self, rest) -> "ETS2": + return self.__mul__(rest) # pragma: nocover + + def compile(self) -> "ETS2": + """ + Compile an ETS2 + + :return: optimised ETS2 + + Perform constant folding for faster evaluation. Consecutive constant + ETs are compounded, leading to a constant ET which is denoted by + ``SE3`` when displayed. + + :seealso: :func:`isconstant` + """ + const = None + ets = ETS2() + + for et in self: + if et.isjoint: + # a joint + if const is not None: + # flush the constant + if not np.array_equal(const, np.eye(3)): + ets *= ET2.SE2(const) + const = None + ets *= et # emit the joint ET + else: + # not a joint + if const is None: + const = et.A() + else: + const = const @ et.A() + + if const is not None: + # flush the constant, tool transform + if not np.array_equal(const, np.eye(3)): + ets *= ET2.SE2(const) + return ets + + def insert( # type: ignore[override] + self, + i: int, + arg: ET2 | ETS2, + ) -> None: + """ + Insert value + + :param i: position to insert at + :param arg: the elementary transform or sequence to insert + + Inserts an ET or ETS into the ET sequence. The inserted value is at position + ``i``. + + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import ET2 + >>> e = ET2.R() * ET2.tx(1) * ET2.R() * ET2.tx(1) + >>> f = ET2.R() + >>> e.insert(2, f) + >>> e + """ + + if isinstance(arg, ET2): + self._data.insert(i, arg) + elif isinstance(arg, ETS2): + for j, et in enumerate(arg): + self._data.insert(i + j, et) + self._fknm_stale = True + + def fkine( + self, + q: ArrayLike, + base: NDArray | SE2 | None = None, + tool: NDArray | SE2 | None = None, + include_base: bool = True, + ) -> SE2: + """ + Forward kinematics + + :param q: joint coordinates + :param base: base transform, optional + :param tool: tool transform, optional + :returns: transformation matrix representing the end-effector pose + :rtype: SE2 + + ``T = ets.fkine(q)`` evaluates forward kinematics for the robot at + joint configuration ``q``. + + **Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a + trajectory and the result is an ``SE2`` instance with ``m`` values. + + .. note:: + + - The robot's base tool transform, if set, is incorporated into the result. + - A tool transform, if provided, is incorporated into the result. + - Works from the end-effector link to the base + + .. rubric:: References + + - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke + """ + + ret = SE2.Empty() + fk = self.eval(q, base, tool, include_base) + + if fk.dtype == "O": + # symbolic + fk = np.array(simplify(fk)) + + if fk.ndim == 3: + for T in fk: + ret.append(SE2(T, check=False)) # type: ignore + else: + ret = SE2(fk, check=False) + + return ret + + def eval( + self, + q: ArrayLike, + base: NDArray | SE2 | None = None, + tool: NDArray | SE2 | None = None, + include_base: bool = True, + ) -> NDArray: + """ + Forward kinematics (returns raw ndarray) + + :param q: joint coordinates + :param base: base transform, optional + :param tool: tool transform, optional + :returns: transformation matrix representing the end-effector pose + :rtype: ndarray(3,3) or ndarray(m,3,3) + + ``T = ets.eval(q)`` evaluates forward kinematics for the robot at + joint configuration ``q``, returning the raw ndarray. + + **Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a + trajectory and the result is an ndarray of shape (m,3,3). + + .. note:: + + - The robot's base tool transform, if set, is incorporated into the result. + - A tool transform, if provided, is incorporated into the result. + - Works from the end-effector link to the base + + .. rubric:: References + + - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke + """ + + q = getmatrix(q, (None, None)) + l, _ = q.shape # type: ignore + end = self[-1] + + if base is None: + bases = None + elif isinstance(base, SE2): + bases = np.array(base.A) + elif np.array_equal(base, np.eye(3)): # pragma: nocover + bases = None + else: # pragma: nocover + bases = base + + if tool is None: + tools = None + elif isinstance(tool, SE2): + tools = np.array(tool.A) + elif np.array_equal(tool, np.eye(3)): # pragma: nocover + tools = None + else: # pragma: nocover + tools = tool + + if l > 1: + T = np.zeros((l, 3, 3), dtype=object) + else: + T = np.zeros((3, 3), dtype=object) + + for k, qk in enumerate(q): # type: ignore + link = end # start with last link + + jindex = 0 if link.jindex is None and link.isjoint else link.jindex + Tk = link.A(qk[jindex]) + + if tools is not None: + Tk = Tk @ tools + + # add remaining links, back toward the base + for i in range(self.m - 2, -1, -1): + link = self._data[i] + + jindex = 0 if link.jindex is None and link.isjoint else link.jindex + A = link.A(qk[jindex]) + + if A is not None: + Tk = A @ Tk + + # add base transform if it is set + if include_base is True and bases is not None: + Tk = bases @ Tk + + # append + if l > 1: + T[k, :, :] = Tk + # ret.append(SE2(Tk, check=False)) # type: ignore + else: + T = Tk + # ret = SE2(Tk, check=False) + + return T + + def jacob0( + self, + q: ArrayLike, + ) -> NDArray: + # very inefficient implementation, just put a 1 in last row + # if its a rotation joint + q = getvector(q) + + j = 0 + J = np.zeros((3, self.n)) + etjoints = self.joint_idx() + + if not np.all(np.array([self[i].jindex for i in etjoints])): + # not all joints have a jindex it is required, set them + for j in range(self.n): + i = etjoints[j] + self[i].jindex = j + + for j in range(self.n): + i = etjoints[j] + + if self[i].jindex is not None: + jindex = self[i].jindex + else: + jindex = 0 # pragma: nocover + + # jindex = 0 if self[i].jindex is None else self[i].jindex + + axis = self[i].kind + if axis == "R": + dTdq = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) @ self[i].A( + q[jindex] # type: ignore + ) + elif axis == "tx": + dTdq = np.array([[0, 0, 1], [0, 0, 0], [0, 0, 0]]) + elif axis == "ty": + dTdq = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 0]]) + else: # pragma: nocover + raise TypeError("Invalid axes") + + E0 = ETS2(self[:i]) + if len(E0) > 0: + dTdq = E0.fkine(q).A @ dTdq + + Ef = ETS2(self[i + 1 :]) + if len(Ef) > 0: + dTdq = dTdq @ Ef.fkine(q).A + + T = self.fkine(q).A + dRdt = dTdq[:2, :2] @ T[:2, :2].T + + J[:2, j] = dTdq[:2, 2] + J[2, j] = dRdt[1, 0] + + return J + + def jacobe( + self, + q: ArrayLike, + ): + r""" + Jacobian in end-effector frame + + :param q: joint coordinates + :returns: Jacobian matrix + :rtype: ndarray(3,n) + + ``jacobe(q)`` is the manipulator Jacobian matrix which maps joint + velocity to end-effector spatial velocity. + + End-effector spatial velocity :math:`\nu = (v_x, v_y, \omega)^T` + is related to joint velocity by :math:`{}^{e}\nu = {}^{e}\mathbf{J}_0(q) \dot{q}`. + + :seealso: :func:`jacob0`, :func:`hessian0` + """ + + T = self.fkine(q, include_base=False).A + return tr2jac2(T.T) @ self.jacob0(q) diff --git a/src/roboticstoolbox/ets/_ET.py b/src/roboticstoolbox/ets/_ET.py new file mode 100644 index 000000000..a3f2e61fe --- /dev/null +++ b/src/roboticstoolbox/ets/_ET.py @@ -0,0 +1,668 @@ +#!/usr/bin/env python3 + +""" +@author: Jesse Haviland + +Shared base for ET (3D) and ET2 (2D) - see roboticstoolbox.ets.ET / +roboticstoolbox.ets.ET2 for the concrete classes. This module exists +separately so ET.py and ET2.py can each import BaseET without importing +each other (BaseET is depended on by both, so it can't depend on either +without a cycle). +""" + +import re +import warnings +from copy import deepcopy + +from numpy import array, ndarray, deg2rad, eye, pi +from numpy.linalg import inv as npinv +from spatialmath.base import getvector, issymbol, tr2rpy, tr2xyt +from typing import Callable, TYPE_CHECKING + +from roboticstoolbox.tools.types import ArrayLike, NDArray + +_AXIS_TO_INT: dict[str, int] = {"Rx": 0, "Ry": 1, "Rz": 2, "tx": 3, "ty": 4, "tz": 5} + +if TYPE_CHECKING: # pragma: nocover + import sympy + + Sym = sympy.core.symbol.Symbol # type: ignore +else: # pragma: nocover + Sym = None + + +def _resolve_param( + param: "float | Sym | None", eta: "float | None" +) -> "float | Sym | None": + """ + Merge the `param` kwarg with the deprecated `eta` kwarg. + + `eta` (η) is the name used in the original Elementary Transform Sequence + paper; `param` is its replacement. If `eta` is passed, warn and use it + as `param` - this keeps every existing `eta=...` call working + unchanged, since `param` didn't exist before 1.4.0. + """ + if eta is not None: + warnings.warn( + "the `eta` keyword is deprecated since 1.4.0, use `param` instead", + DeprecationWarning, + stacklevel=3, + ) + return eta + return param + + +def _parse_joint_descriptor(s: str) -> "tuple[int | None, bool]": + """ + Parse a joint descriptor string into (jindex, flip). + + A leading '-' sets flip (and is stripped); a leading '+' is stripped and + ignored. The first run of digits found anywhere in what remains becomes + the joint index - this treats 'theta2', 'q2', 'q(3)' and 'θ_3' the same + regardless of how the index is set off from the rest of the name. If no + digit is found, jindex is None, so the joint falls through to ETS's + existing auto-numbering for unassigned joints. + """ + flip = s.startswith("-") + if s[:1] in "+-": + s = s[1:] + + match = re.search(r"\d+", s) + jindex = int(match.group()) if match else None + + return jindex, flip + + +class BaseET: + def __init__( + self, + axis: str, + param: float | Sym | str | None = None, + axis_func: Callable[[float | Sym], ndarray] | None = None, + T: ndarray | None = None, + jindex: int | None = None, + unit: str = "rad", + flip: bool | None = None, + qlim: ArrayLike | None = None, + *, + eta: float | None = None, + ): + param = _resolve_param(param, eta) + + self._kind = axis + + # A custom joint display name, e.g. "theta2" from a string `param` + # descriptor below - printed by __str__ in place of the generic + # "q2" when set. + self._joint_name = None + + # A string `param` that doesn't parse as a plain number is a joint + # descriptor (e.g. "theta2", "-q(3)", "θ_3"): regex-parsed for + # jindex/flip and remembered for __str__, then treated as a + # variable joint (param=None) from here on. `jindex`/`flip` must + # not also be given explicitly in this case - one form or the + # other, not a silent merge of both. + if isinstance(param, str): + try: + param = float(param) + except ValueError: + if jindex is not None or flip is not None: + raise ValueError( + "cannot specify `jindex` or `flip` alongside a string " + "joint descriptor for `param`" + ) + jindex, flip = _parse_joint_descriptor(param) + self._joint_name = param + param = None + + flip = bool(flip) # None (not given, and not set above) -> False + + # A flag to check if the ET is a static joint with a symbolic value + # Defaults to False as is set to True if param is a symbol below + self._isstaticsym = False + + # axis_func/flip/jindex/qlim must all be set before `param` below: + # the `param` setter (for the static, param-is-not-None case) reads + # `self.axis_func` to (re)compute `self._T`, and subclasses that add + # compiled acceleration (see ET._accel_update) need jindex/qlim/flip + # already in place too. + self._axis_func = axis_func + self._flip = flip + self._jindex = jindex + + if qlim is not None: + self._qlim: NDArray | None = getvector(qlim, 2, out="array") + else: + self._qlim: NDArray | None = None + + if param is None: + self._param = None + if T is None: + self._joint = True + self._T = eye(4).copy(order="F") + if axis_func is None: + raise TypeError("For a variable joint, axis_func must be specified") + else: + self._joint = False + self._T = T.copy(order="F") + else: + if axis[0] == "R" and unit.lower().startswith("deg"): + if not issymbol(param): + param = deg2rad(float(param)) + # This is a static joint. The `param` setter validates axis_func, + # computes `_T`, sets `_isstaticsym`/`_joint`, and (for ET) + # syncs the compiled acceleration struct. + self.param = param + + def __str__(self): + param_str = "" + + if self.isjoint: + if self._joint_name is not None: + param_str = self._joint_name + elif self.jindex is None: + param_str = "q" + else: + param_str = f"q{self.jindex}" + elif issymbol(self.param): + # Check if symbolic + param_str = f"{self.param}" + elif self.isrotation and self.param is not None: + param_str = f"{self.param * (180.0 / pi):.4g}°" + elif not self.iselementary: + # Compound/arbitrary transform - kind is guaranteed to be + # exactly "SE3" or "SE2" here (the only "S"-prefixed kinds). + if self.kind == "SE3": + T = self.A() + rpy = tr2rpy(T) * 180.0 / pi + if T[:3, -1].any() and rpy.any(): + param_str = ( + f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g};" + f" {rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" + ) + elif T[:3, -1].any(): + param_str = f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g}" + elif rpy.any(): + param_str = f"{rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" + else: + param_str = "" # pragma: nocover + elif self.kind == "SE2": + T = self.A() + xyt = tr2xyt(T) + xyt[2] *= 180 / pi + param_str = f"{xyt[0]:.4g}, {xyt[1]:.4g}; {xyt[2]:.4g}°" + + else: + param_str = f"{self.param:.4g}" + + return f"{self.kind}({param_str})" + + def __repr__(self): + s_param = "" if self.param is None else f"param={self.param}" + s_T = ( + f"T={repr(self._T)}" + if (self.param is None and self.axis_func is None) + else "" + ) + s_flip = "" if not self.isflip else f"flip={self.isflip}" + s_qlim = "" if self.qlim is None else f"qlim={repr(self.qlim)}" + s_jindex = "" if self.jindex is None else f"jindex={self.jindex}" + + kwargs = [s_param, s_T, s_jindex, s_flip, s_qlim] + s_kwargs = ", ".join(filter(None, kwargs)) + + # self.__class__.__name__ rather than isinstance(self, ET)/(self, ET2): + # ET/ET2 can't be imported here without a circular import (they both + # depend on BaseET), and this is a display label anyway - it's also + # more correct for any future subclass than a hardcoded "ET"/"ET2". + start = self.__class__.__name__ + + return f"{start}.{self.kind}({s_kwargs})" + + def _repr_pretty_(self, p, cycle): + """ + Pretty string for IPython + + :param p: pretty printer handle (ignored) + :param cycle: pretty printer flag (ignored) + + Print stringified version when variable is displayed in IPython, ie. on + a line by itself. + + Example:: + + [In [1]: e + Out [1]: tx(1) + """ + p.text(str(self)) # pragma: nocover + + # Attribute names to exclude from the generic __dict__ copy below, e.g. + # an opaque compiled-acceleration handle that can't be deep-copied and + # must instead be rebuilt fresh by _accel_init(). Empty for the plain + # Python ET2; overridden by ET. + _deepcopy_skip: tuple[str, ...] = () + + def __deepcopy__(self, memo): + cls = self.__class__ + result = cls.__new__(cls) + memo[id(self)] = result + + for k, v in self.__dict__.items(): + if k not in self._deepcopy_skip: + setattr(result, k, deepcopy(v, memo)) + + result._accel_init() + return result + + def __eq__(self, other): + return repr(self) == repr(other) + + def __radd__(self, other): + # lets sum() work without an explicit start value, since its + # default start is the int 0, which has no idea how to compose + # with an ET/ET2 + if other == 0: + return self + return NotImplemented + + # ------------------------------------------------------------------ + # Compiled-acceleration hooks. BaseET (and so ET2) is pure Python; ET + # overrides both to build/refresh the compiled C++ struct. Keeping + # these as no-op hooks here means the param/qlim/jindex setters and + # inv()/__deepcopy__ below don't need to know or care whether the + # concrete class has acceleration at all. + # ------------------------------------------------------------------ + def _accel_init(self) -> None: + pass + + def _accel_update(self) -> None: + pass + + @property + def param(self) -> float | Sym | None: + """ + Get the transform constant + + :returns: The constant value if set + :rtype: float or Sym or None + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.param + >>> e = ET.Rx(90, 'deg') + >>> e.param + >>> e = ET.ty() + >>> e.param + + .. rubric:: Notes + + - If the value was given in degrees it will be converted and + stored internally in radians + - Historically called `eta` (η), after the notation used in the + original Elementary Transform Sequence paper (Haviland & Corke, + "Manipulator Differential Kinematics"). `eta` is kept as a + deprecated alias below. + """ + return self._param + + @param.setter + def param(self, value: float | Sym) -> None: + """ + Set the transform constant + + :param value: The transform constant + + .. rubric:: Notes + + - No unit conversions are applied, it is assumed to be in + radians. + - Setting `param` always makes the ET a static (non-joint) transform: + `_T` is recomputed from `axis_func`, and (for ET) the compiled + acceleration struct is refreshed. This is also what ETS.merge() + relies on when it combines two adjacent static ETs. + """ + if self.axis_func is None: + raise TypeError( + "For a static joint either both `param` and `axis_func` " + "must be specified otherwise `T` must be supplied" + ) + + self._param = value if issymbol(value) else float(value) + self._isstaticsym = issymbol(value) + self._joint = False + self._T = self.axis_func(self._param).copy(order="F") + + self._accel_update() + + @property + def eta(self) -> float | Sym | None: + """ + Get the transform constant + + .. deprecated:: 1.4.0 + `eta` (η) is the name used in the original Elementary Transform + Sequence paper; kept as a permanent alias for :attr:`param`, + which is otherwise identical. + + :returns: The constant value if set + :rtype: float or Sym or None + """ + warnings.warn( + "ET.eta is deprecated since 1.4.0, use .param instead", + DeprecationWarning, + stacklevel=2, + ) + return self._param + + @eta.setter + def eta(self, value: float | Sym) -> None: + warnings.warn( + "ET.eta is deprecated since 1.4.0, use .param instead", + DeprecationWarning, + stacklevel=2, + ) + self.param = value + + @property + def axis_func( + self, + ) -> Callable[[float | Sym], ndarray] | None: + return self._axis_func + + @property + def kind(self) -> str: + """ + The transform type and axis + + :returns: The transform type and axis, e.g. ``"Rx"``, ``"tx"``, ``"SE3"`` + :rtype: str + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.kind + >>> e = ET.Rx(90, 'deg') + >>> e.kind + + """ + return self._kind + + @property + def axis(self) -> str: + """ + The transform type and axis + + .. deprecated:: 1.4.0 + Use :attr:`kind` instead. ``axis`` is kept as an alias and will + not be repurposed to mean something else in a future release. + + :returns: The transform type and axis + :rtype: str + """ + warnings.warn( + "ET.axis is deprecated since 1.4.0, use .kind instead", + DeprecationWarning, + stacklevel=2, + ) + return self._kind + + @property + def ax(self) -> str | None: + """ + The Cartesian axis this transform acts along/about + + :returns: ``"x"``, ``"y"``, or ``"z"`` for an elementary transform, + otherwise ``None`` (e.g. ``ET2``'s rotation, which has no axis + letter, or a compound/arbitrary ``SE3``/``SE2`` transform) + :rtype: str or None + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.ax + >>> e = ET.Rx(90, 'deg') + >>> e.ax + + """ + letter = self._kind[-1] + return letter if letter in "xyz" else None + + @property + def isjoint(self) -> bool: + """ + Test if ET is a joint + + :returns: True if a joint + :rtype: bool + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.isjoint + >>> e = ET.tx() + >>> e.isjoint + + """ + return self._joint + + @property + def isflip(self) -> bool: + """ + Test if ET joint is flipped + + :returns: True if joint is flipped + :rtype: bool + + A flipped joint uses the negative of the joint variable, ie. it rotates + or moves in the opposite direction. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx() + >>> e.T(1) + >>> eflip = ET.tx(flip=True) + >>> eflip.T(1) + + """ + + return self._flip + + @property + def isrotation(self) -> bool: + """ + Test if ET is a rotation + + :returns: True if a rotation + :rtype: bool + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.isrotation + >>> e = ET.rx() + >>> e.isrotation + + """ + + return self.kind[0] == "R" + + @property + def istranslation(self) -> bool: + """ + Test if ET is a translation + + :returns: True if a translation + :rtype: bool + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.istranslation + >>> e = ET.rx() + >>> e.istranslation + + """ + + return self.kind[0] == "t" + + @property + def qlim(self) -> ndarray | None: + return self._qlim + + @qlim.setter + def qlim(self, qlim_new: ArrayLike | None) -> None: + if qlim_new is not None: + qlim_new = getvector(qlim_new, 2, out="array") + self._qlim = qlim_new + self._accel_update() + + @property + def jindex(self) -> int | None: + """ + Get ET joint index + + :returns: The assigned joint index + :rtype: int or None + + Allows an ET to be associated with a numbered joint in a robot. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx() + >>> print(e) + >>> e = ET.tx(j=3) + >>> print(e) + >>> print(e.jindex) + + """ + + return self._jindex + + @jindex.setter + def jindex(self, j): + if not isinstance(j, int) or j < 0: + raise ValueError(f"jindex is {j}, must be an int >= 0") + self._jindex = j + self._accel_update() + + @property + def iselementary(self) -> bool: + """ + Test if ET is an elementary transform + + :returns: True if an elementary transform + :rtype: bool + + .. rubric:: Notes + + - ET's may not actually be "elementary", it can be a complex + mix of rotations and translations. + + See Also + -------- + :func:`compile` + + """ + + return self.kind[0] != "S" + + def inv(self): + r""" + Inverse of ET + + :returns: Inverse of the ET + :rtype: ET + + The inverse of a given ET. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz(2.5) + >>> print(e) + >>> print(e.inv()) + + """ # noqa + + inv = deepcopy(self) + + if inv.isjoint: + inv._flip ^= True + elif not inv.iselementary: + inv._T = npinv(inv._T).copy(order="F") + elif inv._param is not None: + inv._T = npinv(inv._T).copy(order="F") + inv._param = -inv._param + + inv._accel_update() + + return inv + + def A(self, q: float | Sym = 0.0) -> ndarray: + """ + Evaluate an elementary transformation + + :param q: Is used if this ET is variable (a joint) + :returns: The SE(3) or SE(2) matrix value of the ET + :rtype: ndarray + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.A() + >>> e = ET.tx() + >>> e.A(0.7) + + Pure-Python evaluation, shared by ET2 and used by ET as the + fallback when the compiled acceleration struct can't be used. + """ + if self.isjoint: + if self.isflip: + q = -q # type: ignore + + if self.axis_func is not None: + return self.axis_func(q) + else: # pragma: no cover + raise TypeError("axis_func not defined") + else: # pragma: no cover + return self._T diff --git a/src/roboticstoolbox/ets/_ETS.py b/src/roboticstoolbox/ets/_ETS.py new file mode 100644 index 000000000..a6699f915 --- /dev/null +++ b/src/roboticstoolbox/ets/_ETS.py @@ -0,0 +1,781 @@ +#!/usr/bin/env python3 + +""" +@author: Jesse Haviland +@author: Peter Corke + +Shared base for ETS (3D) and ETS2 (2D) - see roboticstoolbox.ets.ETS / +roboticstoolbox.ets.ETS2 for the concrete classes. This module exists +separately so ETS.py and ETS2.py can each import BaseETS without importing +each other (BaseETS is depended on by both, so it can't depend on either +without a cycle). +""" + +from __future__ import annotations +from collections.abc import MutableSequence +from functools import wraps, cached_property +import numpy as np +from numpy.random import uniform +from numpy.linalg import inv, det, cond, svd +from spatialmath import SE3, SE2 +from spatialmath.base import ( + getvector, + issymbol, + tr2jac, + verifymatrix, + tr2jac2, + t2r, + rotvelxform, + simplify, + getmatrix, +) +from roboticstoolbox.tools.params import rtb_get_param + +from roboticstoolbox.ets.fknm import ( + ETS_init, + ETS_fkine, + ETS_jacob0, + ETS_jacobe, + ETS_hessian0, + ETS_hessiane, + IK_NR_c, + IK_GN_c, + IK_LM_c, +) +from copy import deepcopy +from roboticstoolbox.ets._ET import BaseET +from typing import overload, TypeVar +from typing import Literal as L +from roboticstoolbox.tools.types import ArrayLike, NDArray + +T = TypeVar("T", bound="BaseETS") + + +def _dirties_fknm(func): + @wraps(func) + def wrapper(self, *args, **kwargs): + result = func(self, *args, **kwargs) + self._fknm_stale = True + return result + return wrapper + + +class BaseETS(MutableSequence): + def __init__(self): + self._data: list = [] + self._fknm_stale = True + self._BaseETS__fknm = None + + # ------------------------------------------------------------------ + # MutableSequence abstract methods + # ------------------------------------------------------------------ + + def __len__(self) -> int: + return len(self._data) + + @_dirties_fknm + def __setitem__(self, i, value): + self._data[i] = value + + @_dirties_fknm + def __delitem__(self, i): + del self._data[i] + + @_dirties_fknm + def insert(self, index: int, value) -> None: + self._data.insert(index, value) + + def __repr__(self) -> str: + return repr(self._data) + + def __eq__(self, other: object) -> bool: + if isinstance(other, BaseETS): + return self._data == other._data + return NotImplemented + + __hash__ = None # type: ignore[assignment] + + def __radd__(self, other): + # lets sum() work without an explicit start value, since its + # default start is the int 0, which has no idea how to compose + # with an ETS/ETS2 + if other == 0: + return self + return NotImplemented + + # ------------------------------------------------------------------ + # C handle: lazy build on first use after any mutation + # ------------------------------------------------------------------ + + @property + def _fknm(self): + if self._fknm_stale: + self._copy_to_cpp() + return self._BaseETS__fknm + + def _copy_to_cpp(self): + self._BaseETS__fknm = ETS_init( + [et.fknm for et in self._data], + self.n, + self.m, + ) + self._fknm_stale = False + + def __str__(self, q: str | None = None): + """ + Pretty prints the ETS + + ``q`` controls how the joint variables are displayed: + + - None, format depends on number of joint variables + - one, display joint variable as q + - more, display joint variables as q0, q1, ... + - if a joint index was provided, use this value + - "", display all joint variables as empty parentheses ``()`` + - "θ", display all joint variables as ``(θ)`` + - format string with passed joint variables ``(j, j+1)``, so "θ{0}" + would display joint variables as θ0, θ1, ... while "θ{1}" would + display joint variables as θ1, θ2, ... ``j`` is either the joint + index, if provided, otherwise a sequential value. + + :param q: control how joint variables are displayed + :returns: Pretty printed ETS + :rtype: str + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() + >>> print(e[:2]) + >>> print(e) + >>> print(e.__str__("")) + >>> print(e.__str__("θ{0}")) # numbering from 0 + >>> print(e.__str__("θ{1}")) # numbering from 1 + >>> # explicit joint indices + >>> e = ET.Rz(jindex=3) * ET.tx(1) * ET.Rz(jindex=4) + >>> print(e) + >>> print(e.__str__("θ{0}")) + + Angular parameters are converted to degrees, except if they + are symbolic. + + .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> from spatialmath.base import symbol + >>> theta, d = symbol('theta, d') + >>> e = ET.Rx(theta) * ET.tx(2) * ET.Rx(45, 'deg') * ET.Ry(0.2) * ET.ty(d) + >>> str(e) + + """ + + es = [] + j = 0 + c = 0 + s = None + unicode = rtb_get_param("unicode") + + # An empty SE3 + if len(self._data) == 0: + return "SE3()" + + if q is None: + if len(self.joints()) > 1: + q = "q{0}" + else: + q = "q" + + # For et in the object, display it, data comes from properties + # which come from the named tuple + for et in self._data: + if et.isjoint: + # A custom name from a string `param` descriptor (e.g. + # "theta2") already encodes any leading sign itself, so it + # takes over the whole "q0"/"-q0" formatting below rather + # than combining with it. + if et._joint_name is not None: + s = f"{et.kind}({et._joint_name})" + j += 1 + es.append(s) + continue + + if q is not None: + if et.jindex is None: # pragma: nocover this is no longer possible + _j = j + else: + _j = et.jindex + qvar = q.format( + _j, _j + 1 + ) + # else: + # qvar = "" + + if et.isflip: + s = f"{et.kind}(-{qvar})" + else: + s = f"{et.kind}({qvar})" + j += 1 + + elif et.isrotation: + if issymbol(et.param): + s = f"{et.kind}({et.param})" + else: + s = f"{et.kind}({et.param * 180 / np.pi:.4g}°)" + + elif et.istranslation: + try: + s = f"{et.kind}({et.param:.4g})" + except TypeError: # pragma: nocover + s = f"{et.kind}({et.param})" + + elif not et.iselementary: + s = str(et) + c += 1 + + es.append(s) + + if unicode: + return " \u2295 ".join(es) + else: # pragma: nocover + return " * ".join(es) + + def _repr_pretty_(self, p, cycle): + """ + Pretty string for IPython + + Print stringified version when variable is displayed in IPython, ie. on + a line by itself. + + :param p: pretty printer handle (ignored) + :param cycle: pretty printer flag (ignored) + + Examples + -------- + + In [1]: e + Out [1]: R(q0) ⊕ tx(1) ⊕ R(q1) ⊕ tx(1) + + """ + + print(self.__str__()) # pragma: nocover + + def joint_idx(self) -> list[int]: + """ + Get index of joint transforms + + :returns: indices of transforms that are joints + :rtype: ndarray + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e.joint_idx() + + """ + + return np.where([e.isjoint for e in self])[0] # type: ignore + + def joints(self) -> list[ET]: + """ + Get a list of the variable ETs with this ETS + + :returns: list of ETs that are joints + :rtype: list[ET] + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e.joints() + + """ + + return [e for e in self if e.isjoint] + + def split(self, method: str = "last") -> list["BaseETS"]: + """ + Split ETS into link segments + + :param method: one of ``"first"`` or ``"last"`` (default). + :returns: ``[base, *segments, gripper]`` -- a list of length + ``n_joints + 2``. ``base``/``gripper`` are empty ETS when the + method has no concept of one. + + Split an ETS into segments representing links, plus a base and gripper segment. + Unpack with ``base, *segments, gripper = ets.split(method)``. + + The behaviour depends on the ``method`` argument: + + * ``"first"``: each link segment begins with a joint and continues upto, but not + including the next joint. Any constant ET before the first joint are part of + the base. There are no gripper ET, they are included in the last link segment. + * ``"last"``: each link segment ends with a joint and include all ET after the + previous joint. Any constant ET after the last joint are part of the gripper. + There are no base ET, they are included in the first link segment. + + .. runblock:: pycon + >>> from roboticstoolbox.ets.ETS import * + >>> e = tz(1) * Rx("q1") * tx(2) * Ry("q2") * ty(3) * Rz("q3") * tz(4) + >>> base, *segments, gripper = e.split("first") + >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") + >>> base, *segments, gripper = e.split("last") + >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") + """ + + segments = [] + joint_idx = self.joint_idx() + head = self[0:0] + tail = self[0:0] + + match method.lower(): + + case "first": + head = self[0:joint_idx[0]] + start = len(head) + for k in joint_idx[1:]: + ets_j = self[start: k] + start = k + segments.append(self.__class__(ets_j)) + segments.append(self.__class__(self[joint_idx[-1]:])) + case "last": + start = 0 + for k in joint_idx: + ets_j = self[start : k + 1] + start = k + 1 + segments.append(self.__class__(ets_j)) + tail = self[start:] + case _: + raise ValueError(f"unknown split method '{method}'") + + return [head] + segments + [tail] # type: ignore + + def jindex_set(self) -> set[int]: # + """ + Get set of joint indices + + :returns: set of unique joint indices + :rtype: set[int] + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) + >>> e.jointset() + + """ + + return set([self[j].jindex for j in self.joint_idx()]) # type: ignore + + @cached_property + def jindices(self) -> NDArray: + """ + Get an array of joint indices + + :returns: array of unique joint indices + :rtype: ndarray + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) + >>> e.jointset() + + """ + + return np.array([j.jindex for j in self.joints()]) # type: ignore + + @property + def qlim(self): + r""" + Get/Set Joint limits + + Limits are extracted from the link objects. If joints limits are + not set for: + + - a revolute joint [-𝜋. 𝜋] is returned + - a prismatic joint an exception is raised + + :param new_qlim: new joint limits to set + :type new_qlim: ndarray(2,n) + :returns: array of joint limit values + :rtype: ndarray(2,n) + :raises ValueError: unset limits for a prismatic joint + + Examples + -------- + + .. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.qlim + + """ + + limits = np.zeros((2, self.n)) + + for i, et in enumerate(self.joints()): + if et.isrotation: + if et.qlim is None: + v = [-np.pi, np.pi] + else: + v = et.qlim + elif et.istranslation: + if et.qlim is None: + raise ValueError("undefined prismatic joint limit") + else: + v = et.qlim + else: + raise ValueError("Undefined Joint Type") # pragma: nocover + limits[:, i] = v + + return limits + + @qlim.setter + def qlim(self, new_qlim: ArrayLike): + new_qlim = np.array(new_qlim) + + if new_qlim.shape == (2,) and self.n == 1: + new_qlim = new_qlim.reshape(2, 1) + + if new_qlim.shape != (2, self.n): + raise ValueError("new_qlim must be of shape (2, n)") + + for j, i in enumerate(self.joint_idx()): + et = self[i] + et.qlim = new_qlim[:, j] + self[i] = et + + @property + def structure(self) -> str: + """ + Joint structure string + + A string comprising the characters 'R' or 'P' which indicate the types + of joints in order from left to right. + + :returns: a string indicating the joint types + :rtype: str + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.tz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e.structure + + """ + + return "".join( + ["R" if self._data[i].isrotation else "P" for i in self.joint_idx()] + ) + + @property + def n(self) -> int: + """ + Number of joints + + :returns: the number of joints in the ETS + :rtype: int + + Counts the number of joints in the ETS. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rx() * ET.tx(1) * ET.tz() + >>> e.n + + See Also + -------- + :func:`joints` + + """ + + return sum(1 for et in self._data if et.isjoint) + + @property + def m(self) -> int: + """ + Number of transforms + + :returns: the number of transforms in the ETS + :rtype: int + + Counts the number of transforms in the ETS. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rx() * ET.tx(1) * ET.tz() + >>> e.m + + """ + + return len(self._data) + + @overload + def data(self: "ETS") -> list[ET]: ... # pragma: nocover + + @overload + def data(self: "ETS2") -> list[ET2]: ... # pragma: nocover + + @property + def data(self): + return self._data + + @data.setter + @overload + def data(self: "ETS", new_data: list[ET]): ... # pragma: nocover + + @data.setter + @overload + def data(self: "ETS", new_data: list[ET2]): ... # pragma: nocover + + @data.setter + def data(self, new_data): + self._data = new_data + self._fknm_stale = True + + def inv(self: T) -> T: + r""" + Inverse of ETS + + The inverse of a given ETS. It is computed as the inverse of the + individual ETs in the reverse order. + + .. math:: + + (\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} ) + + :returns: Inverse of the ETS + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz(jindex=2) * ET.tx(1) * ET.Rx(jindex=3,flip=True) * ET.tx(1) + >>> print(e) + >>> print(e.inv()) + + .. rubric:: Notes + + - It is essential to use explicit joint indices to account for + the reversed order of the transforms. + + """ + + return self.__class__([et.inv() for et in reversed(self._data)]) # type: ignore[call-arg] + + @overload + def __getitem__(self: "BaseETS", i: int) -> BaseET: ... + + @overload + def __getitem__(self: "ETS", i: int) -> ET: ... + + @overload + def __getitem__(self: "ETS", i: slice) -> "ETS": ... + + @overload + def __getitem__(self: "ETS2", i: int) -> ET2: ... + + @overload + def __getitem__(self: "ETS2", i: slice) -> "ETS2": ... + + def __getitem__(self, i): + """ + Index or slice an ETS + + :param i: the index or slice + :returns: elementary transform if ``i`` is an int, otherwise an ETS + of the same concrete type as ``self`` (a slice of an ETS is + still an ETS) + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e[0] + >>> e[1] + >>> e[1:3] + + """ + if isinstance(i, slice): + return self.__class__(self._data[i]) + return self._data[i] + + def __deepcopy__(self, memo): + new_data = [] + + for data in self: + new_data.append(deepcopy(data)) + + cls = self.__class__ + result = cls(new_data) + memo[id(self)] = result + return result + + def plot(self, *args, **kwargs): + from roboticstoolbox.robot.Robot import Robot, Robot2 + # Deferred (like Robot/Robot2 above): BaseETS can't import the + # concrete ETS without a cycle (ETS.py imports BaseETS from here). + from roboticstoolbox.ets.ETS import ETS + + if isinstance(self, ETS): + robot = Robot(self) + else: + robot = Robot2(self) + + robot.plot(*args, **kwargs) + + def teach(self, *args, **kwargs): + from roboticstoolbox.robot.Robot import Robot, Robot2 + from roboticstoolbox.ets.ETS import ETS + + if isinstance(self, ETS): + robot = Robot(self) + else: + robot = Robot2(self) + + robot.teach(*args, **kwargs) + + def random_q(self, i: int = 1) -> NDArray: + """ + Generate a random valid joint configuration + + :param i: number of configurations to generate + :returns: random joint configuration + :rtype: ndarray(n,) or ndarray(i,n) + + Generates a random q vector within the joint limits defined by + ``self.qlim``. + + Examples + -------- + + .. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.Panda() + >>> ets = robot.ets() + >>> q = ets.random_q() + >>> q + + """ + + if i == 1: + q = np.zeros(self.n) + + for i in range(self.n): + q[i] = uniform(self.qlim[0, i], self.qlim[1, i]) + + else: + q = np.zeros((i, self.n)) + + for j in range(i): + for i in range(self.n): + q[j, i] = uniform(self.qlim[0, i], self.qlim[1, i]) + + return q + + def swap(self, i: int) -> None: + """ + Swap two transforms in the ETS + + :param i: index of first transform + + Swaps the two transforms at indices ``i`` and ``i+1``. This is useful for + changing the order of commutative transforms in an ETS. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rx() * ET.Rz(1) + >>> print(e) + >>> e.swap(1) + >>> print(e) + + """ + if i < 0 or i >= len(self._data) - 1: + raise IndexError("Index out of range") # pragma: nocover + + e1 = self._data[i] + e2 = self._data[i + 1] + if e1.kind == e2.kind: + self._data[i], self._data[i + 1] = self._data[i + 1], self._data[i] + self._fknm_stale = True + else: + raise ValueError("Transforms are not commutative") # pragma: nocover + + def merge(self, i: int) -> None: + """ + Merge two transforms in the ETS + + :param i: index of first transform + + Merges the two transforms at indices ``i`` and ``i+1``. This is useful for + reducing the number of transforms in an ETS. + + Examples + -------- + + .. runblock:: pycon + + >>> from roboticstoolbox import ET + >>> from math import pi + >>> e = ET.Rz() * ET.tx(1) * ET.tx(2) * ET.Rz(1) + >>> print(e) + >>> e.merge(1) + >>> print(e) + >>> e = ET.tx(1) * ET.Rx() * ET.Rx(pi/2) * ET.tx(2) + >>> print(e) + >>> e.merge(1) + >>> print(e) + + """ + if i < 0 or i >= len(self._data) - 1: + raise IndexError("Index out of range") # pragma: nocover + e1 = self._data[i] + e2 = self._data[i + 1] + if e1.kind != e2.kind: + raise ValueError("Transforms are not the same type") # pragma: nocover + + elif (e1.isjoint + e2.isjoint) == 2: + raise ValueError("Transforms are both joints") # pragma: nocover + + else: + self._data[i].param = e1.param + e2.param + del self._data[i + 1] + self._fknm_stale = True + diff --git a/src/roboticstoolbox/ets/__init__.py b/src/roboticstoolbox/ets/__init__.py new file mode 100644 index 000000000..9579b9956 --- /dev/null +++ b/src/roboticstoolbox/ets/__init__.py @@ -0,0 +1,11 @@ +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ET2 import ET2 +from roboticstoolbox.ets.ETS import ETS +from roboticstoolbox.ets.ETS2 import ETS2 + +__all__ = [ + "ET", + "ET2", + "ETS", + "ETS2", +] diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Cholesky b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Cholesky similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Cholesky rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Cholesky diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/CholmodSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/CholmodSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/CholmodSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/CholmodSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Core b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Core similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Core rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Core diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Dense b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Dense similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Dense rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Dense diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Eigen b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Eigen similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Eigen rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Eigen diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Eigenvalues b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Eigenvalues similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Eigenvalues rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Eigenvalues diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Geometry b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Geometry similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Geometry rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Geometry diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Householder b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Householder similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Householder rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Householder diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/IterativeLinearSolvers b/src/roboticstoolbox/ets/cpp-extensions/Eigen/IterativeLinearSolvers similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/IterativeLinearSolvers rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/IterativeLinearSolvers diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Jacobi b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Jacobi similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Jacobi rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Jacobi diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/KLUSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/KLUSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/KLUSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/KLUSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/LU b/src/roboticstoolbox/ets/cpp-extensions/Eigen/LU similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/LU rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/LU diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/MetisSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/MetisSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/MetisSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/MetisSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/OrderingMethods b/src/roboticstoolbox/ets/cpp-extensions/Eigen/OrderingMethods similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/OrderingMethods rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/OrderingMethods diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/PaStiXSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/PaStiXSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/PaStiXSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/PaStiXSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/PardisoSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/PardisoSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/PardisoSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/PardisoSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/QR b/src/roboticstoolbox/ets/cpp-extensions/Eigen/QR similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/QR rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/QR diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/QtAlignedMalloc b/src/roboticstoolbox/ets/cpp-extensions/Eigen/QtAlignedMalloc similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/QtAlignedMalloc rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/QtAlignedMalloc diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SPQRSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SPQRSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SPQRSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SPQRSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SVD b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SVD similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SVD rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SVD diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/Sparse b/src/roboticstoolbox/ets/cpp-extensions/Eigen/Sparse similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/Sparse rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/Sparse diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseCholesky b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseCholesky similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseCholesky rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseCholesky diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseCore b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseCore similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseCore rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseCore diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseLU b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseLU similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseLU rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseLU diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseQR b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseQR similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SparseQR rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SparseQR diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/StdDeque b/src/roboticstoolbox/ets/cpp-extensions/Eigen/StdDeque similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/StdDeque rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/StdDeque diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/StdList b/src/roboticstoolbox/ets/cpp-extensions/Eigen/StdList similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/StdList rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/StdList diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/StdVector b/src/roboticstoolbox/ets/cpp-extensions/Eigen/StdVector similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/StdVector rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/StdVector diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/SuperLUSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/SuperLUSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/SuperLUSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/SuperLUSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/UmfPackSupport b/src/roboticstoolbox/ets/cpp-extensions/Eigen/UmfPackSupport similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/UmfPackSupport rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/UmfPackSupport diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Cholesky/LDLT.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Cholesky/LDLT.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Cholesky/LDLT.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Cholesky/LDLT.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Cholesky/LLT.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Cholesky/LLT.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Cholesky/LLT.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Cholesky/LLT.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Cholesky/LLT_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Cholesky/LLT_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Cholesky/LLT_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Cholesky/LLT_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/CholmodSupport/CholmodSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/CholmodSupport/CholmodSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/CholmodSupport/CholmodSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/CholmodSupport/CholmodSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ArithmeticSequence.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ArithmeticSequence.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ArithmeticSequence.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ArithmeticSequence.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Array.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Array.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Array.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Array.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ArrayBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ArrayBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ArrayBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ArrayBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ArrayWrapper.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ArrayWrapper.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ArrayWrapper.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ArrayWrapper.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Assign.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Assign.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Assign.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Assign.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/AssignEvaluator.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/AssignEvaluator.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/AssignEvaluator.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/AssignEvaluator.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Assign_MKL.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Assign_MKL.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Assign_MKL.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Assign_MKL.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/BandMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/BandMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/BandMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/BandMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Block.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Block.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Block.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Block.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/BooleanRedux.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/BooleanRedux.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/BooleanRedux.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/BooleanRedux.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CommaInitializer.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CommaInitializer.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CommaInitializer.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CommaInitializer.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ConditionEstimator.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ConditionEstimator.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ConditionEstimator.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ConditionEstimator.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CoreEvaluators.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CoreEvaluators.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CoreEvaluators.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CoreEvaluators.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CoreIterators.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CoreIterators.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CoreIterators.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CoreIterators.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseBinaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseBinaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseBinaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseBinaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseNullaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseNullaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseNullaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseNullaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseTernaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseTernaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseTernaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseTernaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseUnaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseUnaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseUnaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseUnaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseUnaryView.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseUnaryView.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/CwiseUnaryView.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/CwiseUnaryView.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DenseBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DenseBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DenseBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DenseBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DenseCoeffsBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DenseCoeffsBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DenseCoeffsBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DenseCoeffsBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DenseStorage.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DenseStorage.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DenseStorage.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DenseStorage.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Diagonal.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Diagonal.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Diagonal.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Diagonal.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DiagonalMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DiagonalMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DiagonalMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DiagonalMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DiagonalProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DiagonalProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/DiagonalProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/DiagonalProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Dot.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Dot.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Dot.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Dot.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/EigenBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/EigenBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/EigenBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/EigenBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ForceAlignedAccess.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ForceAlignedAccess.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ForceAlignedAccess.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ForceAlignedAccess.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Fuzzy.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Fuzzy.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Fuzzy.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Fuzzy.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/GeneralProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/GeneralProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/GeneralProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/GeneralProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/GenericPacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/GenericPacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/GenericPacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/GenericPacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/GlobalFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/GlobalFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/GlobalFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/GlobalFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/IO.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/IO.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/IO.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/IO.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/IndexedView.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/IndexedView.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/IndexedView.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/IndexedView.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Inverse.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Inverse.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Inverse.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Inverse.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Map.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Map.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Map.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Map.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MapBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MapBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MapBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MapBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MathFunctionsImpl.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MathFunctionsImpl.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MathFunctionsImpl.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MathFunctionsImpl.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Matrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Matrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Matrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Matrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MatrixBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MatrixBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/MatrixBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/MatrixBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/NestByValue.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/NestByValue.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/NestByValue.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/NestByValue.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/NoAlias.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/NoAlias.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/NoAlias.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/NoAlias.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/NumTraits.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/NumTraits.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/NumTraits.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/NumTraits.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/PartialReduxEvaluator.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/PartialReduxEvaluator.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/PartialReduxEvaluator.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/PartialReduxEvaluator.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/PermutationMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/PermutationMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/PermutationMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/PermutationMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/PlainObjectBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/PlainObjectBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/PlainObjectBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/PlainObjectBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Product.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Product.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Product.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Product.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ProductEvaluators.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ProductEvaluators.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ProductEvaluators.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ProductEvaluators.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Random.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Random.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Random.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Random.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Redux.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Redux.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Redux.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Redux.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Ref.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Ref.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Ref.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Ref.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Replicate.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Replicate.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Replicate.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Replicate.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Reshaped.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Reshaped.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Reshaped.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Reshaped.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ReturnByValue.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ReturnByValue.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/ReturnByValue.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/ReturnByValue.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Reverse.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Reverse.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Reverse.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Reverse.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Select.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Select.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Select.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Select.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SelfAdjointView.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SelfAdjointView.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SelfAdjointView.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SelfAdjointView.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SelfCwiseBinaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SelfCwiseBinaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SelfCwiseBinaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SelfCwiseBinaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Solve.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Solve.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Solve.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Solve.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SolveTriangular.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SolveTriangular.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SolveTriangular.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SolveTriangular.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SolverBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SolverBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/SolverBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/SolverBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/StableNorm.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/StableNorm.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/StableNorm.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/StableNorm.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/StlIterators.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/StlIterators.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/StlIterators.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/StlIterators.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Stride.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Stride.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Stride.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Stride.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Swap.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Swap.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Swap.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Swap.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Transpose.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Transpose.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Transpose.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Transpose.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Transpositions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Transpositions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Transpositions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Transpositions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/TriangularMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/TriangularMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/TriangularMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/TriangularMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/VectorBlock.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/VectorBlock.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/VectorBlock.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/VectorBlock.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/VectorwiseOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/VectorwiseOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/VectorwiseOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/VectorwiseOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Visitor.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Visitor.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/Visitor.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/Visitor.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AVX512/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AVX512/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductCommon.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductCommon.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductCommon.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductCommon.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductMMA.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductMMA.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductMMA.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductMMA.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/AltiVec/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/AltiVec/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/CUDA/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/CUDA/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/CUDA/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/CUDA/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/BFloat16.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/BFloat16.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/BFloat16.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/BFloat16.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/ConjHelper.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/ConjHelper.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/ConjHelper.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/ConjHelper.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/Half.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/Half.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/Half.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/Half.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/Settings.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/Settings.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/Settings.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/Settings.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/Default/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/Default/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/GPU/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/GPU/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/GPU/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/GPU/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/GPU/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/GPU/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/GPU/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/GPU/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/GPU/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/GPU/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/GPU/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/GPU/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/HIP/hcc/math_constants.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/HIP/hcc/math_constants.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/HIP/hcc/math_constants.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/HIP/hcc/math_constants.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/MSA/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/MSA/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/MSA/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/MSA/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/MSA/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/MSA/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/MSA/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/MSA/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/MSA/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/MSA/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/MSA/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/MSA/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/NEON/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/NEON/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SSE/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SSE/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SVE/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SVE/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SVE/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SVE/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SVE/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SVE/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SVE/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SVE/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SVE/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SVE/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SVE/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SVE/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/InteropHeaders.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/InteropHeaders.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/InteropHeaders.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/InteropHeaders.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/SyclMemoryModel.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/SyclMemoryModel.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/SyclMemoryModel.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/SyclMemoryModel.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/TypeCasting.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/TypeCasting.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/SYCL/TypeCasting.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/SYCL/TypeCasting.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/ZVector/Complex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/ZVector/Complex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/ZVector/Complex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/ZVector/Complex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/ZVector/MathFunctions.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/ZVector/MathFunctions.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/ZVector/MathFunctions.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/ZVector/MathFunctions.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/ZVector/PacketMath.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/ZVector/PacketMath.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/arch/ZVector/PacketMath.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/arch/ZVector/PacketMath.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/AssignmentFunctors.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/AssignmentFunctors.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/AssignmentFunctors.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/AssignmentFunctors.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/BinaryFunctors.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/BinaryFunctors.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/BinaryFunctors.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/BinaryFunctors.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/NullaryFunctors.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/NullaryFunctors.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/NullaryFunctors.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/NullaryFunctors.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/StlFunctors.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/StlFunctors.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/StlFunctors.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/StlFunctors.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/TernaryFunctors.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/TernaryFunctors.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/TernaryFunctors.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/TernaryFunctors.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/UnaryFunctors.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/UnaryFunctors.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/functors/UnaryFunctors.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/functors/UnaryFunctors.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralBlockPanelKernel.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralBlockPanelKernel.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralBlockPanelKernel.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/Parallelizer.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/Parallelizer.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/Parallelizer.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/Parallelizer.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointRank2Update.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointRank2Update.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/SelfadjointRank2Update.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/SelfadjointRank2Update.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularSolverVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularSolverVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/products/TriangularSolverVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/products/TriangularSolverVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/BlasUtil.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/BlasUtil.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/BlasUtil.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/BlasUtil.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ConfigureVectorization.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ConfigureVectorization.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ConfigureVectorization.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ConfigureVectorization.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Constants.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Constants.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Constants.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Constants.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/DisableStupidWarnings.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/DisableStupidWarnings.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/DisableStupidWarnings.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/DisableStupidWarnings.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ForwardDeclarations.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ForwardDeclarations.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ForwardDeclarations.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ForwardDeclarations.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/IndexedViewHelper.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/IndexedViewHelper.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/IndexedViewHelper.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/IndexedViewHelper.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/IntegralConstant.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/IntegralConstant.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/IntegralConstant.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/IntegralConstant.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/MKL_support.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/MKL_support.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/MKL_support.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/MKL_support.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Macros.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Macros.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Macros.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Macros.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Memory.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Memory.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Memory.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Memory.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Meta.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Meta.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/Meta.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/Meta.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/NonMPL2.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/NonMPL2.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/NonMPL2.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/NonMPL2.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ReenableStupidWarnings.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ReenableStupidWarnings.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ReenableStupidWarnings.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ReenableStupidWarnings.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ReshapedHelper.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ReshapedHelper.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/ReshapedHelper.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/ReshapedHelper.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/StaticAssert.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/StaticAssert.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/StaticAssert.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/StaticAssert.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/SymbolicIndex.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/SymbolicIndex.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/SymbolicIndex.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/SymbolicIndex.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/XprHelper.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/XprHelper.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Core/util/XprHelper.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Core/util/XprHelper.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/ComplexEigenSolver.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/ComplexEigenSolver.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/ComplexEigenSolver.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/EigenSolver.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/EigenSolver.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/EigenSolver.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/EigenSolver.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/HessenbergDecomposition.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/HessenbergDecomposition.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/HessenbergDecomposition.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/RealQZ.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/RealQZ.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/RealQZ.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/RealQZ.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/RealSchur.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/RealSchur.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/RealSchur.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/RealSchur.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/Tridiagonalization.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/Tridiagonalization.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Eigenvalues/Tridiagonalization.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Eigenvalues/Tridiagonalization.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/AlignedBox.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/AlignedBox.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/AlignedBox.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/AlignedBox.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/AngleAxis.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/AngleAxis.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/AngleAxis.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/AngleAxis.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/EulerAngles.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/EulerAngles.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/EulerAngles.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/EulerAngles.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Homogeneous.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Homogeneous.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Homogeneous.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Homogeneous.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Hyperplane.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Hyperplane.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Hyperplane.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Hyperplane.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/OrthoMethods.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/OrthoMethods.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/OrthoMethods.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/OrthoMethods.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/ParametrizedLine.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/ParametrizedLine.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/ParametrizedLine.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/ParametrizedLine.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Quaternion.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Quaternion.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Quaternion.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Quaternion.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Rotation2D.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Rotation2D.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Rotation2D.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Rotation2D.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/RotationBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/RotationBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/RotationBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/RotationBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Scaling.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Scaling.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Scaling.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Scaling.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Transform.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Transform.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Transform.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Transform.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Translation.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Translation.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Translation.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Translation.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Umeyama.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Umeyama.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/Umeyama.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/Umeyama.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/arch/Geometry_SIMD.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/arch/Geometry_SIMD.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Geometry/arch/Geometry_SIMD.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Geometry/arch/Geometry_SIMD.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Householder/BlockHouseholder.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Householder/BlockHouseholder.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Householder/BlockHouseholder.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Householder/BlockHouseholder.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Householder/Householder.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Householder/Householder.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Householder/Householder.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Householder/Householder.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Householder/HouseholderSequence.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Householder/HouseholderSequence.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Householder/HouseholderSequence.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Householder/HouseholderSequence.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Jacobi/Jacobi.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Jacobi/Jacobi.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/Jacobi/Jacobi.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/Jacobi/Jacobi.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/KLUSupport/KLUSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/KLUSupport/KLUSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/KLUSupport/KLUSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/KLUSupport/KLUSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/Determinant.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/Determinant.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/Determinant.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/Determinant.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/FullPivLU.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/FullPivLU.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/FullPivLU.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/FullPivLU.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/InverseImpl.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/InverseImpl.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/InverseImpl.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/InverseImpl.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/PartialPivLU.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/PartialPivLU.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/PartialPivLU.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/PartialPivLU.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/PartialPivLU_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/PartialPivLU_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/PartialPivLU_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/PartialPivLU_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/arch/InverseSize4.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/arch/InverseSize4.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/LU/arch/InverseSize4.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/LU/arch/InverseSize4.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/MetisSupport/MetisSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/MetisSupport/MetisSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/MetisSupport/MetisSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/MetisSupport/MetisSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/OrderingMethods/Amd.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/OrderingMethods/Amd.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/OrderingMethods/Amd.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/OrderingMethods/Amd.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/OrderingMethods/Eigen_Colamd.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/OrderingMethods/Eigen_Colamd.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/OrderingMethods/Eigen_Colamd.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/OrderingMethods/Eigen_Colamd.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/OrderingMethods/Ordering.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/OrderingMethods/Ordering.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/OrderingMethods/Ordering.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/OrderingMethods/Ordering.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/PaStiXSupport/PaStiXSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/PaStiXSupport/PaStiXSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/PaStiXSupport/PaStiXSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/PaStiXSupport/PaStiXSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/PardisoSupport/PardisoSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/PardisoSupport/PardisoSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/PardisoSupport/PardisoSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/PardisoSupport/PardisoSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/CompleteOrthogonalDecomposition.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/CompleteOrthogonalDecomposition.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/CompleteOrthogonalDecomposition.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/FullPivHouseholderQR.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/FullPivHouseholderQR.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/FullPivHouseholderQR.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/FullPivHouseholderQR.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/HouseholderQR.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/HouseholderQR.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/HouseholderQR.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/HouseholderQR.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/HouseholderQR_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/HouseholderQR_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/QR/HouseholderQR_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/QR/HouseholderQR_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/BDCSVD.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/BDCSVD.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/BDCSVD.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/BDCSVD.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/JacobiSVD.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/JacobiSVD.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/JacobiSVD.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/JacobiSVD.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/JacobiSVD_LAPACKE.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/JacobiSVD_LAPACKE.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/JacobiSVD_LAPACKE.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/JacobiSVD_LAPACKE.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/SVDBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/SVDBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/SVDBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/SVDBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/UpperBidiagonalization.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/UpperBidiagonalization.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SVD/UpperBidiagonalization.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SVD/UpperBidiagonalization.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/AmbiVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/AmbiVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/AmbiVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/AmbiVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/CompressedStorage.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/CompressedStorage.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/CompressedStorage.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/CompressedStorage.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/MappedSparseMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/MappedSparseMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/MappedSparseMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/MappedSparseMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseAssign.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseAssign.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseAssign.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseAssign.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseBlock.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseBlock.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseBlock.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseBlock.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseColEtree.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseColEtree.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseColEtree.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseColEtree.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseCompressedBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseCompressedBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseCompressedBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseCompressedBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseCwiseBinaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseCwiseBinaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseCwiseBinaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseCwiseUnaryOp.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseCwiseUnaryOp.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseCwiseUnaryOp.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseDenseProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseDenseProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseDenseProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseDenseProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseDiagonalProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseDiagonalProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseDiagonalProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseDiagonalProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseDot.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseDot.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseDot.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseDot.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseFuzzy.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseFuzzy.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseFuzzy.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseFuzzy.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseMap.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseMap.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseMap.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseMap.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseMatrixBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseMatrixBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseMatrixBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseMatrixBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparsePermutation.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparsePermutation.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparsePermutation.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparsePermutation.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseProduct.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseProduct.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseProduct.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseProduct.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseRedux.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseRedux.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseRedux.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseRedux.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseRef.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseRef.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseRef.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseRef.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseSelfAdjointView.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseSelfAdjointView.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseSelfAdjointView.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseSelfAdjointView.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseSolverBase.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseSolverBase.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseSolverBase.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseSolverBase.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseSparseProductWithPruning.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseSparseProductWithPruning.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseSparseProductWithPruning.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseTranspose.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseTranspose.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseTranspose.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseTranspose.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseTriangularView.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseTriangularView.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseTriangularView.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseTriangularView.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseUtil.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseUtil.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseUtil.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseUtil.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseView.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseView.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/SparseView.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/SparseView.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/TriangularSolver.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/TriangularSolver.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseCore/TriangularSolver.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseCore/TriangularSolver.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLUImpl.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLUImpl.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLUImpl.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLUImpl.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_Memory.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_Memory.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_Memory.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_Memory.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_Structs.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_Structs.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_Structs.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_Structs.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_Utils.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_Utils.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_Utils.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_Utils.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_bmod.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_bmod.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_bmod.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_bmod.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_dfs.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_dfs.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_dfs.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_dfs.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_gemm_kernel.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_gemm_kernel.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_gemm_kernel.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_gemm_kernel.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_kernel_bmod.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_kernel_bmod.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_kernel_bmod.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_bmod.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_bmod.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_bmod.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_bmod.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_dfs.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_dfs.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_dfs.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_dfs.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_pivotL.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_pivotL.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_pivotL.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_pivotL.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_pruneL.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_pruneL.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_pruneL.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_pruneL.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_relax_snode.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_relax_snode.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseLU/SparseLU_relax_snode.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseLU/SparseLU_relax_snode.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseQR/SparseQR.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseQR/SparseQR.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SparseQR/SparseQR.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SparseQR/SparseQR.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/StdDeque.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/StdDeque.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/StdDeque.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/StdDeque.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/StdList.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/StdList.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/StdList.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/StdList.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/StdVector.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/StdVector.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/StdVector.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/StdVector.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/details.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/details.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/StlSupport/details.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/StlSupport/details.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SuperLUSupport/SuperLUSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SuperLUSupport/SuperLUSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/SuperLUSupport/SuperLUSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/SuperLUSupport/SuperLUSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/UmfPackSupport/UmfPackSupport.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/UmfPackSupport/UmfPackSupport.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/UmfPackSupport/UmfPackSupport.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/UmfPackSupport/UmfPackSupport.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/Image.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/Image.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/Image.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/Image.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/Kernel.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/Kernel.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/Kernel.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/Kernel.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/RealSvd2x2.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/RealSvd2x2.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/RealSvd2x2.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/RealSvd2x2.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/blas.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/blas.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/blas.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/blas.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/lapack.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/lapack.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/lapack.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/lapack.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/lapacke.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/lapacke.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/lapacke.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/lapacke.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/lapacke_mangling.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/lapacke_mangling.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/misc/lapacke_mangling.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/misc/lapacke_mangling.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ArrayCwiseBinaryOps.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/ArrayCwiseBinaryOps.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ArrayCwiseBinaryOps.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ArrayCwiseUnaryOps.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/ArrayCwiseUnaryOps.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ArrayCwiseUnaryOps.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/BlockMethods.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/BlockMethods.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/BlockMethods.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/BlockMethods.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/CommonCwiseBinaryOps.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/CommonCwiseBinaryOps.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/CommonCwiseBinaryOps.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/CommonCwiseBinaryOps.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/CommonCwiseUnaryOps.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/CommonCwiseUnaryOps.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/CommonCwiseUnaryOps.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/CommonCwiseUnaryOps.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/IndexedViewMethods.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/IndexedViewMethods.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/IndexedViewMethods.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/IndexedViewMethods.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/MatrixCwiseBinaryOps.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/MatrixCwiseBinaryOps.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/MatrixCwiseBinaryOps.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/MatrixCwiseUnaryOps.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/MatrixCwiseUnaryOps.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/MatrixCwiseUnaryOps.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/ReshapedMethods.h b/src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ReshapedMethods.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/Eigen/src/plugins/ReshapedMethods.h rename to src/roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ReshapedMethods.h diff --git a/src/roboticstoolbox/ets/cpp-extensions/README.md b/src/roboticstoolbox/ets/cpp-extensions/README.md new file mode 100644 index 000000000..8bb8d4671 --- /dev/null +++ b/src/roboticstoolbox/ets/cpp-extensions/README.md @@ -0,0 +1,73 @@ +# Fast/optimized kinematics (fknm) + +## Synopsis + +nanobind C++/C extension that accelerates the ETS hot path: forward +kinematics, Jacobians, Hessians, and IK (Newton-Raphson, Gauss-Newton, +Levenberg-Marquardt), used by `ETS`/`Robot`. + +This is one of two such extensions in the codebase — the other, +**frne** (`_frne_c`, recursive Newton-Euler inverse dynamics, used by +`DHRobot.rne()`), lives at `robot/cpp-extensions/` since it's a +dynamics concern, not an ETS one. See that directory's own README for +frne specifics; this file covers fknm plus the architecture patterns +shared by both. + +It's optional: `ets/fknm.py` is a pure-Python facade that tries to +import the compiled extension and transparently falls back to an +equivalent pure-Python implementation when it's unavailable +(Pyodide/WASM builds without the extension compiled, CI paths that force +the Python implementation, or symbolic/SymPy inputs, which the C++ side +can't handle). + +## How it works + +* Built via nanobind, driven by the top-level `CMakeLists.txt` / + scikit-build-core. The compiled modules install as + `roboticstoolbox._fknm_c` / `roboticstoolbox._frne_c` at the top level of + the installed package (`CMakeLists.txt`: `DESTINATION roboticstoolbox`) — + that install location is independent of where this source directory lives. +* `fknm.py` / `frne.py` each do `from roboticstoolbox._fknm_c import ...` + (or `_frne_c`) inside a `try/except ImportError`, and provide a pure-Python + equivalent for every function on the `except` path. Symbolic inputs are + detected per-call and routed to the Python path even when the extension is + available. +* **Lazy serialization into the extension.** The C++ side needs its own copy + of the robot's state (`ETS`/`ET` structs for fknm, a `Robot` struct for + frne) — Python objects can't be handed across the boundary directly. Rather + than re-serializing on every mutation, `ETS`/`BaseETS` and `DHRobot` each + keep a C++-side mirror object (`_fknm`, `_frne`) plus a dirty flag + (`_fknm_stale`, `_frne_stale`). Mutating methods are wrapped with the + `@_dirties_fknm` / `@_dirties_frne` decorators, which just set the flag + after the wrapped call — they don't touch the mirror themselves. The + mirror is only actually rebuilt (`_copy_to_cpp()`) the next time a C++ + function is about to be called and the flag is set. This means a whole + sequence of mutations (e.g. building up an `ETS` element by element, or + setting several dynamic parameters on a `DHLink`) costs one rebuild total, + not one per mutation, and a mirror that's never used for a C++ call is + never built at all. + +## Gotcha! + +Internal transform matrices (ET/ETS results, Jacobians) are stored +**column-major** — Eigen's native layout, not NumPy's default row-major +layout. This is intentional (it's what `Eigen::Map` wants) and callers don't +need to do anything about it: the nanobind bindings (`EigenRef4d`, +`EigenRefJd`) accept C-contiguous, F-contiguous, or arbitrary-stride NumPy +arrays and let Eigen copy element-by-element using the real strides, so no +Python-side layout conversion is ever required before calling into C++. + +# Files + +| File | Purpose | +| ---- | ------- | +| Eigen | Vendored Eigen headers (header-only) | +| fknm_nb.cpp | nanobind glue for `_fknm_c` — binds FK, Jacobian, Hessian, IK, and ET init/update | +| ik.cpp | Fast inverse kinematics (Newton-Raphson, Gauss-Newton, Levenberg-Marquardt) | +| ik.h | " | +| linalg.cpp | SE(3) matrix operations | +| linalg.h | " | +| methods.cpp | Jacobians, Hessians | +| methods.h | " | +| structs.cpp | ET, ETS mirror (currently unused/dead — not part of the `_fknm_c` CMake sources) | +| structs.h | " | diff --git a/src/roboticstoolbox/robot/cpp-extensions/fknm_nb.cpp b/src/roboticstoolbox/ets/cpp-extensions/fknm_nb.cpp similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/fknm_nb.cpp rename to src/roboticstoolbox/ets/cpp-extensions/fknm_nb.cpp diff --git a/src/roboticstoolbox/robot/cpp-extensions/ik.cpp b/src/roboticstoolbox/ets/cpp-extensions/ik.cpp similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/ik.cpp rename to src/roboticstoolbox/ets/cpp-extensions/ik.cpp diff --git a/src/roboticstoolbox/robot/cpp-extensions/ik.h b/src/roboticstoolbox/ets/cpp-extensions/ik.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/ik.h rename to src/roboticstoolbox/ets/cpp-extensions/ik.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/linalg.cpp b/src/roboticstoolbox/ets/cpp-extensions/linalg.cpp similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/linalg.cpp rename to src/roboticstoolbox/ets/cpp-extensions/linalg.cpp diff --git a/src/roboticstoolbox/robot/cpp-extensions/linalg.h b/src/roboticstoolbox/ets/cpp-extensions/linalg.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/linalg.h rename to src/roboticstoolbox/ets/cpp-extensions/linalg.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/methods.cpp b/src/roboticstoolbox/ets/cpp-extensions/methods.cpp similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/methods.cpp rename to src/roboticstoolbox/ets/cpp-extensions/methods.cpp diff --git a/src/roboticstoolbox/robot/cpp-extensions/methods.h b/src/roboticstoolbox/ets/cpp-extensions/methods.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/methods.h rename to src/roboticstoolbox/ets/cpp-extensions/methods.h diff --git a/src/roboticstoolbox/robot/cpp-extensions/structs.cpp b/src/roboticstoolbox/ets/cpp-extensions/structs.cpp similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/structs.cpp rename to src/roboticstoolbox/ets/cpp-extensions/structs.cpp diff --git a/src/roboticstoolbox/robot/cpp-extensions/structs.h b/src/roboticstoolbox/ets/cpp-extensions/structs.h similarity index 100% rename from src/roboticstoolbox/robot/cpp-extensions/structs.h rename to src/roboticstoolbox/ets/cpp-extensions/structs.h diff --git a/src/roboticstoolbox/robot/fknm.py b/src/roboticstoolbox/ets/fknm.py similarity index 98% rename from src/roboticstoolbox/robot/fknm.py rename to src/roboticstoolbox/ets/fknm.py index 610bf0c5e..1a212e8ce 100644 --- a/src/roboticstoolbox/robot/fknm.py +++ b/src/roboticstoolbox/ets/fknm.py @@ -139,22 +139,22 @@ def _python_jacob0(data, n, q, tool): y = Tu[1, 3] z = Tu[2, 3] - if link.axis == "Rz": + if link.kind == "Rz": J[:3, j] = (o * x) - (n_vec * y) J[3:, j] = a - elif link.axis == "Ry": + elif link.kind == "Ry": J[:3, j] = (n_vec * z) - (a * x) J[3:, j] = o - elif link.axis == "Rx": + elif link.kind == "Rx": J[:3, j] = (a * y) - (o * z) J[3:, j] = n_vec - elif link.axis == "tx": + elif link.kind == "tx": J[:3, j] = n_vec J[3:, j] = zero - elif link.axis == "ty": + elif link.kind == "ty": J[:3, j] = o J[3:, j] = zero - elif link.axis == "tz": + elif link.kind == "tz": J[:3, j] = a J[3:, j] = zero diff --git a/src/roboticstoolbox/models/ETS/Frankie.py b/src/roboticstoolbox/models/ETS/Frankie.py index 348df7d81..1d9fa1ac6 100644 --- a/src/roboticstoolbox/models/ETS/Frankie.py +++ b/src/roboticstoolbox/models/ETS/Frankie.py @@ -1,8 +1,8 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link diff --git a/src/roboticstoolbox/models/ETS/GenericSeven.py b/src/roboticstoolbox/models/ETS/GenericSeven.py index 91bb31276..42347105c 100644 --- a/src/roboticstoolbox/models/ETS/GenericSeven.py +++ b/src/roboticstoolbox/models/ETS/GenericSeven.py @@ -1,8 +1,8 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link diff --git a/src/roboticstoolbox/models/ETS/Omni.py b/src/roboticstoolbox/models/ETS/Omni.py index ad0a3d4a4..e37765d70 100644 --- a/src/roboticstoolbox/models/ETS/Omni.py +++ b/src/roboticstoolbox/models/ETS/Omni.py @@ -1,8 +1,8 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link import spatialgeometry as sg diff --git a/src/roboticstoolbox/models/ETS/Panda.py b/src/roboticstoolbox/models/ETS/Panda.py index 441ae6514..1bd0aa8e9 100644 --- a/src/roboticstoolbox/models/ETS/Panda.py +++ b/src/roboticstoolbox/models/ETS/Panda.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET +from roboticstoolbox.ets.ET import ET from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link diff --git a/src/roboticstoolbox/models/ETS/Planar2.py b/src/roboticstoolbox/models/ETS/Planar2.py index a5ae0d985..6401cf356 100644 --- a/src/roboticstoolbox/models/ETS/Planar2.py +++ b/src/roboticstoolbox/models/ETS/Planar2.py @@ -1,8 +1,8 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET2 -from roboticstoolbox.robot.ETS import ETS2 +from roboticstoolbox.ets.ET2 import ET2 +from roboticstoolbox.ets.ETS2 import ETS2 from roboticstoolbox.robot.Robot import Robot2 from roboticstoolbox.robot.Link import Link2 diff --git a/src/roboticstoolbox/models/ETS/Planar_Y.py b/src/roboticstoolbox/models/ETS/Planar_Y.py index 6ca6dc10b..c85ace1b3 100644 --- a/src/roboticstoolbox/models/ETS/Planar_Y.py +++ b/src/roboticstoolbox/models/ETS/Planar_Y.py @@ -1,8 +1,8 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link diff --git a/src/roboticstoolbox/models/ETS/XYPanda.py b/src/roboticstoolbox/models/ETS/XYPanda.py index ef4ce8443..8029c9a6f 100644 --- a/src/roboticstoolbox/models/ETS/XYPanda.py +++ b/src/roboticstoolbox/models/ETS/XYPanda.py @@ -1,8 +1,8 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link diff --git a/src/roboticstoolbox/models/URDF/FrankieOmni.py b/src/roboticstoolbox/models/URDF/FrankieOmni.py index 80d4e5c84..201705247 100644 --- a/src/roboticstoolbox/models/URDF/FrankieOmni.py +++ b/src/roboticstoolbox/models/URDF/FrankieOmni.py @@ -3,8 +3,8 @@ import numpy as np from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link -from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ET import ET +from roboticstoolbox.ets.ETS import ETS +from roboticstoolbox.ets.ET import ET from roboticstoolbox.models.URDF.URDFRobot import URDF_read from spatialmath import SE3 diff --git a/src/roboticstoolbox/models/URDF/URDFRobot.py b/src/roboticstoolbox/models/URDF/URDFRobot.py index 50f7de280..da7243c58 100644 --- a/src/roboticstoolbox/models/URDF/URDFRobot.py +++ b/src/roboticstoolbox/models/URDF/URDFRobot.py @@ -22,8 +22,8 @@ from roboticstoolbox.tools.urdf import URDF from roboticstoolbox.robot.Link import Link -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS from roboticstoolbox.robot.Robot import Robot diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 01ff118da..8ebc703f2 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -35,12 +35,12 @@ from spatialgeometry import SceneNode from roboticstoolbox.backends.Connector import Connector -from roboticstoolbox.robot.fknm import Robot_link_T +from roboticstoolbox.ets.fknm import Robot_link_T import roboticstoolbox as rtb from roboticstoolbox.robot.Gripper import Gripper from roboticstoolbox.robot.Link import BaseLink, Link -from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ET import ET +from roboticstoolbox.ets.ETS import ETS +from roboticstoolbox.ets.ET import ET from roboticstoolbox.robot.Dynamics import DynamicsMixin from roboticstoolbox.tools.types import ArrayLike, NDArray from roboticstoolbox.tools.params import rtb_get_param diff --git a/src/roboticstoolbox/robot/DHLink.py b/src/roboticstoolbox/robot/DHLink.py index d847d485c..4b570901e 100644 --- a/src/roboticstoolbox/robot/DHLink.py +++ b/src/roboticstoolbox/robot/DHLink.py @@ -7,8 +7,8 @@ # from spatialmath import SE3 import roboticstoolbox as rp from roboticstoolbox.robot.Link import Link, _dirties_frne, _copy_shapes -from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ET import ET +from roboticstoolbox.ets.ETS import ETS +from roboticstoolbox.ets.ET import ET from spatialmath import SE3 from functools import wraps from numpy import ndarray, cos, sin, array diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index 1ccacc679..eeec5f343 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -11,7 +11,7 @@ import copy import numpy as np from roboticstoolbox.robot.Robot import Robot # DHLink -from roboticstoolbox.robot.ETS import ETS, ET +from roboticstoolbox.ets.ETS import ETS, ET from roboticstoolbox.robot.DHLink import DHLink from roboticstoolbox.tools.params import rtb_set_param from spatialmath.base.argcheck import getvector, isscalar, verifymatrix, getmatrix diff --git a/src/roboticstoolbox/robot/ET.py b/src/roboticstoolbox/robot/ET.py deleted file mode 100644 index 363c7442e..000000000 --- a/src/roboticstoolbox/robot/ET.py +++ /dev/null @@ -1,971 +0,0 @@ -#!/usr/bin/env python3 - -""" -@author: Jesse Haviland -""" - -from numpy import array, ndarray, deg2rad, eye, pi -from numpy.linalg import inv as npinv -import roboticstoolbox as rtb -from spatialmath.base import ( - trotx, - troty, - trotz, - issymbol, - tr2rpy, - trot2, - transl2, - tr2xyt, -) -from copy import deepcopy -from roboticstoolbox.robot.fknm import ET_T, ET_init, ET_update -from spatialmath.base import getvector -from spatialmath import SE3, SE2 -from typing import Callable, TYPE_CHECKING - -# from spatialmath.base.types import ArrayLike -from roboticstoolbox.tools.types import ArrayLike, NDArray - -_AXIS_TO_INT: dict[str, int] = {"Rx": 0, "Ry": 1, "Rz": 2, "tx": 3, "ty": 4, "tz": 5} - -if TYPE_CHECKING: # pragma: nocover - import sympy - - Sym = sympy.core.symbol.Symbol # type: ignore -else: # pragma: nocover - Sym = None - - -class BaseET: - def __init__( - self, - axis: str, - eta: float | Sym | None = None, - axis_func: Callable[[float | Sym], ndarray] | None = None, - T: ndarray | None = None, - jindex: int | None = None, - unit: str = "rad", - flip: bool = False, - qlim: ArrayLike | None = None, - ): - self._axis = axis - - # A flag to check if the ET is a static joint with a symbolic value - # Defaults to False as is set to True if eta is a symbol below - self._isstaticsym = False - - if eta is None: - self._eta = None - else: - if axis[0] == "R" and unit.lower().startswith("deg"): - if not issymbol(eta): - self.eta = deg2rad(float(eta)) - else: - self.eta = eta - - self._axis_func = axis_func - self._flip = flip - self._jindex = jindex - - if qlim is not None: - self._qlim: NDArray | None = getvector(qlim, 2, out="array") - else: - self._qlim: NDArray | None = None - - if self.eta is None: - if T is None: - self._joint = True - self._T = eye(4).copy(order="F") - if axis_func is None: - raise TypeError("For a variable joint, axis_func must be specified") - else: - self._joint = False - self._T = T.copy(order="F") - else: - # This is a static joint - if issymbol(eta): - self._isstaticsym = True - - self._joint = False - if axis_func is not None: - self._T = axis_func(self.eta).copy(order="F") - else: - raise TypeError( - "For a static joint either both `eta` and `axis_func` " - "must be specified otherwise `T` must be supplied" - ) - - # Initialise the C object which holds ET data - # This returns a reference to said C data - self.__fknm = self.__init_c() - - def __init_c(self): - """ - Super Private method which initialises a C object to hold ET Data - """ - if self.jindex is None: - jindex = 0 - else: - jindex = self.jindex - - if self.qlim is None: - if self.axis[0] == "R": - qlim = array([-pi, pi]) - else: - qlim = array([0, 1]) - else: - qlim = self.qlim - - return ET_init( - self._isstaticsym, - self.isjoint, - self.isflip, - jindex, - self.__axis_to_number(self.axis), - self._T, - qlim, - ) - - def __update_c(self): - """ - Super Private method which updates the C object which holds ET Data - """ - if self.jindex is None: - jindex = 0 - else: - jindex = self.jindex - - if self.qlim is None: - if self.axis[0] == "R": - qlim = array([-pi, pi]) - else: - qlim = array([0, 1]) - else: - qlim = self.qlim - - ET_update( - self.fknm, - self._isstaticsym, - self.isjoint, - self.isflip, - jindex, - self.__axis_to_number(self.axis), - self._T, - qlim, - ) - - def __str__(self): - eta_str = "" - - if self.isjoint: - if self.jindex is None: - eta_str = "q" - else: - eta_str = f"q{self.jindex}" - elif issymbol(self.eta): - # Check if symbolic - eta_str = f"{self.eta}" - elif self.isrotation and self.eta is not None: - eta_str = f"{self.eta * (180.0 / pi):.4g}°" - elif not self.iselementary: - if isinstance(self, ET): - T = self.A() - rpy = tr2rpy(T) * 180.0 / pi - if T[:3, -1].any() and rpy.any(): - eta_str = ( - f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g};" - f" {rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" - ) - elif T[:3, -1].any(): - eta_str = f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g}" - elif rpy.any(): - eta_str = f"{rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" - else: - eta_str = "" # pragma: nocover - elif isinstance(self, ET2): - T = self.A() - xyt = tr2xyt(T) - xyt[2] *= 180 / pi - eta_str = f"{xyt[0]:.4g}, {xyt[1]:.4g}; {xyt[2]:.4g}°" - - else: - eta_str = f"{self.eta:.4g}" - - return f"{self.axis}({eta_str})" - - def __repr__(self): - s_eta = "" if self.eta is None else f"eta={self.eta}" - s_T = ( - f"T={repr(self._T)}" - if (self.eta is None and self.axis_func is None) - else "" - ) - s_flip = "" if not self.isflip else f"flip={self.isflip}" - s_qlim = "" if self.qlim is None else f"qlim={repr(self.qlim)}" - s_jindex = "" if self.jindex is None else f"jindex={self.jindex}" - - kwargs = [s_eta, s_T, s_jindex, s_flip, s_qlim] - s_kwargs = ", ".join(filter(None, kwargs)) - - start = "ET" if isinstance(self, ET) else "ET2" - - return f"{start}.{self.axis}({s_kwargs})" - - def _repr_pretty_(self, p, cycle): - """ - Pretty string for IPython - - :param p: pretty printer handle (ignored) - :param cycle: pretty printer flag (ignored) - - Print stringified version when variable is displayed in IPython, ie. on - a line by itself. - - Example:: - - [In [1]: e - Out [1]: tx(1) - """ - p.text(str(self)) # pragma: nocover - - def __deepcopy__(self, memo): - cls = self.__class__ - result = cls.__new__(cls) - memo[id(self)] = result - - for k, v in self.__dict__.items(): - if k != "_BaseET__fknm": - setattr(result, k, deepcopy(v, memo)) - - result.__fknm = result.__init_c() - return result - - def __eq__(self, other): - return repr(self) == repr(other) - - def __axis_to_number(self, axis: str) -> int: - """ - Private convenience function which converts the axis string to an - integer for faster processing in the C extensions - """ - if isinstance(self, ET2): - return 0 - return _AXIS_TO_INT.get(axis, 0) - - @property - def fknm(self): - return self.__fknm - - @property - def eta(self) -> float | Sym | None: - """ - Get the transform constant - - :returns: The constant η if set - :rtype: float or Sym or None - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.eta - >>> e = ET.Rx(90, 'deg') - >>> e.eta - >>> e = ET.ty() - >>> e.eta - - .. rubric:: Notes - - - If the value was given in degrees it will be converted and - stored internally in radians - """ - return self._eta - - @eta.setter - def eta(self, value: float | Sym) -> None: - """ - Set the transform constant - - :param value: The transform constant η - - .. rubric:: Notes - - - No unit conversions are applied, it is assumed to be in - radians. - """ - self._eta = value if issymbol(value) else float(value) - - @property - def axis_func( - self, - ) -> Callable[[float | Sym], ndarray] | None: - return self._axis_func - - @property - def axis(self) -> str: - """ - The transform type and axis - - :returns: The transform type and axis - :rtype: str - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.axis - >>> e = ET.Rx(90, 'deg') - >>> e.axis - - """ - return self._axis - - @property - def isjoint(self) -> bool: - """ - Test if ET is a joint - - :returns: True if a joint - :rtype: bool - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.isjoint - >>> e = ET.tx() - >>> e.isjoint - - """ - return self._joint - - @property - def isflip(self) -> bool: - """ - Test if ET joint is flipped - - :returns: True if joint is flipped - :rtype: bool - - A flipped joint uses the negative of the joint variable, ie. it rotates - or moves in the opposite direction. - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx() - >>> e.T(1) - >>> eflip = ET.tx(flip=True) - >>> eflip.T(1) - - """ - - return self._flip - - @property - def isrotation(self) -> bool: - """ - Test if ET is a rotation - - :returns: True if a rotation - :rtype: bool - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.isrotation - >>> e = ET.rx() - >>> e.isrotation - - """ - - return self.axis[0] == "R" - - @property - def istranslation(self) -> bool: - """ - Test if ET is a translation - - :returns: True if a translation - :rtype: bool - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.istranslation - >>> e = ET.rx() - >>> e.istranslation - - """ - - return self.axis[0] == "t" - - @property - def qlim(self) -> ndarray | None: - return self._qlim - - @qlim.setter - def qlim(self, qlim_new: ArrayLike | None) -> None: - if qlim_new is not None: - qlim_new = getvector(qlim_new, 2, out="array") - self._qlim = qlim_new - self.__update_c() - - @property - def jindex(self) -> int | None: - """ - Get ET joint index - - :returns: The assigned joint index - :rtype: int or None - - Allows an ET to be associated with a numbered joint in a robot. - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx() - >>> print(e) - >>> e = ET.tx(j=3) - >>> print(e) - >>> print(e.jindex) - - """ - - return self._jindex - - @jindex.setter - def jindex(self, j): - if not isinstance(j, int) or j < 0: - raise ValueError(f"jindex is {j}, must be an int >= 0") - self._jindex = j - self.__update_c() - - @property - def iselementary(self) -> bool: - """ - Test if ET is an elementary transform - - :returns: True if an elementary transform - :rtype: bool - - .. rubric:: Notes - - - ET's may not actually be "elementary", it can be a complex - mix of rotations and translations. - - See Also - -------- - :func:`compile` - - """ - - return self.axis[0] != "S" - - def inv(self): - r""" - Inverse of ET - - :returns: Inverse of the ET - :rtype: ET - - The inverse of a given ET. - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz(2.5) - >>> print(e) - >>> print(e.inv()) - - """ # noqa - - inv = deepcopy(self) - - if inv.isjoint: - inv._flip ^= True - elif not inv.iselementary: - inv._T = npinv(inv._T).copy(order="F") - elif inv._eta is not None: - inv._T = npinv(inv._T).copy(order="F") - inv._eta = -inv._eta - - inv.__update_c() - - return inv - - def A(self, q: float | Sym = 0.0) -> ndarray: - """ - Evaluate an elementary transformation - - :param q: Is used if this ET is variable (a joint) - :returns: The SE(3) or SE(2) matrix value of the ET - :rtype: ndarray - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.A() - >>> e = ET.tx() - >>> e.A(0.7) - - """ - try: - # Try and use the C implementation, flip is handled in C - return ET_T(self.__fknm, q) - except TypeError: - # We can't use the fast version, lets use Python instead - if self.isjoint: - if self.isflip: - q = -q # type: ignore - - if self.axis_func is not None: - return self.axis_func(q) - else: # pragma: no cover - raise TypeError("axis_func not defined") - else: # pragma: no cover - return self._T - - -class ET(BaseET): - def __init__(self, **kwargs): - super().__init__(**kwargs) - - def __mul__(self, other: "ET") -> "rtb.ETS": - return rtb.ETS([self, other]) - - def __add__(self, other: "ET") -> "rtb.ETS": - return self.__mul__(other) - - @property - def s(self) -> ndarray: # pragma: nocover - if self.axis[1] == "x": - if self.axis[0] == "R": - return array([0, 0, 0, 1, 0, 0]) - else: - return array([1, 0, 0, 0, 0, 0]) - elif self.axis[1] == "y": - if self.axis[0] == "R": - return array([0, 0, 0, 0, 1, 0]) - else: - return array([0, 1, 0, 0, 0, 0]) - else: - if self.axis[0] == "R": - return array([0, 0, 0, 0, 0, 1]) - else: - return array([0, 0, 1, 0, 0, 0]) - - @classmethod - def Rx( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs - ) -> "ET": - """ - Pure rotation about the x-axis - - :param η: rotation about the x-axis - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET - - - ``ET.Rx(η)`` is an elementary rotation about the x-axis by a - constant angle η - - ``ET.Rx()`` is an elementary rotation about the x-axis by a variable - angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in - this case. - - See Also - -------- - :func:`ET` - :func:`isrotation` - - :SymPy: supported - """ - - return cls(axis="Rx", eta=eta, axis_func=trotx, unit=unit, **kwargs) - - @classmethod - def Ry( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs - ) -> "ET": - """ - Pure rotation about the y-axis - - :param η: rotation about the y-axis - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET - - - ``ET.Ry(η)`` is an elementary rotation about the y-axis by a - constant angle η - - ``ET.Ry()`` is an elementary rotation about the y-axis by a variable - angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in - this case. - - See Also - -------- - :func:`ET` - :func:`isrotation` - - :SymPy: supported - """ - return cls(axis="Ry", eta=eta, axis_func=troty, unit=unit, **kwargs) - - @classmethod - def Rz( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs - ) -> "ET": - """ - Pure rotation about the z-axis - - :param η: rotation about the z-axis - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET - - - ``ET.Rz(η)`` is an elementary rotation about the z-axis by a - constant angle η - - ``ET.Rz()`` is an elementary rotation about the z-axis by a variable - angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in - this case. - - See Also - -------- - :func:`ET` - :func:`isrotation` - - :SymPy: supported - """ - return cls(axis="Rz", eta=eta, axis_func=trotz, unit=unit, **kwargs) - - @classmethod - def tx(cls, eta: float | Sym | None = None, **kwargs) -> "ET": - """ - Pure translation along the x-axis - - :param η: translation distance along the x-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET - - - ``ET.tx(η)`` is an elementary translation along the x-axis by a - distance constant η - - ``ET.tx()`` is an elementary translation along the x-axis by a - variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` - can be set in this case. - - See Also - -------- - :func:`ET` - :func:`istranslation` - - :SymPy: supported - """ - - # this method is 3x faster than using lambda x: transl(x, 0, 0) - def axis_func(eta): - # fmt: off - return array([ - [1, 0, 0, eta], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - # fmt: on - - return cls(axis="tx", axis_func=axis_func, eta=eta, **kwargs) - - @classmethod - def ty(cls, eta: float | Sym | None = None, **kwargs) -> "ET": - """ - Pure translation along the y-axis - - :param η: translation distance along the y-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET - - - ``ET.ty(η)`` is an elementary translation along the y-axis by a - distance constant η - - ``ET.ty()`` is an elementary translation along the y-axis by a - variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` - can be set in this case. - - See Also - -------- - :func:`ET` - :func:`istranslation` - - :SymPy: supported - """ - - def axis_func(eta): - # fmt: off - return array([ - [1, 0, 0, 0], - [0, 1, 0, eta], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - # fmt: on - - return cls(axis="ty", eta=eta, axis_func=axis_func, **kwargs) - - @classmethod - def tz(cls, eta: float | Sym | None = None, **kwargs) -> "ET": - """ - Pure translation along the z-axis - - :param η: translation distance along the z-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET - - - ``ET.tz(η)`` is an elementary translation along the z-axis by a - distance constant η - - ``ET.tz()`` is an elementary translation along the z-axis by a - variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` - can be set in this case. - - See Also - -------- - :func:`ET` - :func:`istranslation` - - :SymPy: supported - """ - - def axis_func(eta): - # fmt: off - return array([ - [1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, eta], - [0, 0, 0, 1] - ]) - # fmt: on - - return cls(axis="tz", axis_func=axis_func, eta=eta, **kwargs) - - @classmethod - def SE3(cls, T: ndarray | SE3, **kwargs) -> "ET": - """ - A static SE3 - - :param T: The SE3 transformation matrix - :returns: An elementary transform - :rtype: ET - - See Also - -------- - :func:`ET` - :func:`istranslation` - - :SymPy: supported - """ - - trans = T.A if isinstance(T, SE3) else T - - return cls(axis="SE3", T=trans, **kwargs) - - -class ET2(BaseET): - def __init__(self, **kwargs): - super().__init__(**kwargs) - - def __mul__(self, other: "ET2") -> "rtb.ETS2": - return rtb.ETS2([self, other]) - - def __add__(self, other: "ET2") -> "rtb.ETS2": - return self.__mul__(other) - - @property - def s(self) -> ndarray: # pragma: nocover - if self.axis[0] == "R": - return array([0, 0, 0, 1]) - if self.axis[1] == "x": - return array([1, 0, 0, 0]) - elif self.axis[1] == "y": - return array([0, 1, 0, 0]) - else: - return array([0, 0, 1, 0]) - - @classmethod - def R( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs - ) -> "ET2": - """ - Pure rotation - - :param η: rotation angle - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET2 - - - ``ET2.R(η)`` is an elementary rotation by a constant angle η - - ``ET2.R()`` is an elementary rotation by a variable angle, i.e. a - revolute robot joint. ``j`` or ``flip`` can be set in - this case. - - .. rubric:: Notes - - - In the 2D case this is rotation around the normal to the - xy-plane. - - See Also - -------- - :func:`ET2`, :func:`isrotation` - - """ - - return cls( - axis="R", eta=eta, axis_func=lambda theta: trot2(theta), unit=unit, **kwargs - ) - - @classmethod - def tx( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs - ) -> "ET2": - """ - Pure translation along the x-axis - - :param η: translation distance along the x-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET2 - - - ``ET2.tx(η)`` is an elementary translation along the x-axis by a - distance constant η - - ``ET2.tx()`` is an elementary translation along the x-axis by a - variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` - can be set in this case. - - See Also - -------- - :func:`ET2` - :func:`istranslation` - - """ - - return cls(axis="tx", eta=eta, axis_func=lambda x: transl2(x, 0), **kwargs) - - @classmethod - def ty( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs - ) -> "ET2": - """ - Pure translation along the y-axis - - :param η: translation distance along the y-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - :returns: An elementary transform - :rtype: ET2 - - - ``ET2.ty(η)`` is an elementary translation along the y-axis by a - distance constant η - - ``ET2.ty()`` is an elementary translation along the y-axis by a - variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` - can be set in this case. - - See Also - -------- - :func:`ET2` - - """ - - return cls(axis="ty", eta=eta, axis_func=lambda y: transl2(0, y), **kwargs) - - @classmethod - def SE2(cls, T: ndarray | SE2, **kwargs) -> "ET2": - """ - A static SE2 - - :param T: The SE2 transformation matrix - :returns: An elementary transform - :rtype: ET2 - - See Also - -------- - :func:`ET2` - :func:`istranslation` - - :SymPy: supported - """ - - trans = T.A if isinstance(T, SE2) else T - - return cls(axis="SE2", T=trans, **kwargs) - - def A(self, q: float | Sym = 0.0) -> ndarray: - """ - Evaluate an elementary transformation - - :param q: Is used if this ET2 is variable (a joint) - :returns: The SE(2) matrix value of the ET2 - :rtype: ndarray - - Examples - -------- - - .. runblock:: pycon - - >>> from roboticstoolbox import ET2 - >>> e = ET2.tx(1) - >>> e.A() - >>> e = ET2.tx() - >>> e.A(0.7) - - """ - - if self.isjoint: - if self.isflip: - q = -1.0 * q # type: ignore[assignment] # Sym*float yields Expr, not Sym - - if self.axis_func is not None: - return self.axis_func(q) - else: # pragma: no cover - raise TypeError("axis_func not defined") - else: # pragma: no cover - return self._T diff --git a/src/roboticstoolbox/robot/Gripper.py b/src/roboticstoolbox/robot/Gripper.py index 2477c40b9..b4f0fb9ec 100644 --- a/src/roboticstoolbox/robot/Gripper.py +++ b/src/roboticstoolbox/robot/Gripper.py @@ -10,7 +10,7 @@ from roboticstoolbox.robot.Link import Link from functools import lru_cache from typing import TypeVar, Generic, Callable -from roboticstoolbox.robot.fknm import Robot_link_T +from roboticstoolbox.ets.fknm import Robot_link_T from roboticstoolbox.tools.types import ArrayLike, NDArray from roboticstoolbox.robot.Link import BaseLink diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 430079242..b87257bc2 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -102,7 +102,7 @@ def __str__(self): class IKSolver(ABC): - """ + r""" An abstract super class for numerical inverse kinematics (IK) This class provides basic functionality to perform numerical IK. Superclasses @@ -113,7 +113,15 @@ class IKSolver(ABC): :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful - :param tol: Maximum allowed residual error E + :param tol: Maximum allowed residual error E, where + :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form + in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`). + Because `E` is quadratic, `tol` does not bound the linear-scale position/ + orientation error directly — with the default unit weighting, components of + :math:`\vec{e}` are only guaranteed to be within roughly + :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on + the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific + linear-scale accuracy :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority :param joint_limits: Reject solutions with joint limit violations @@ -517,7 +525,7 @@ def _calc_qnull( class IK_NR(IKSolver): - """ + r""" Newton-Raphson Numerical Inverse Kinematics Solver A class which provides functionality to perform numerical inverse kinematics (IK) @@ -531,7 +539,15 @@ class IK_NR(IKSolver): :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful - :param tol: Maximum allowed residual error E + :param tol: Maximum allowed residual error E, where + :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form + in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`). + Because `E` is quadratic, `tol` does not bound the linear-scale position/ + orientation error directly — with the default unit weighting, components of + :math:`\vec{e}` are only guaranteed to be within roughly + :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on + the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific + linear-scale accuracy :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority :param joint_limits: Reject solutions with joint limit violations @@ -674,7 +690,7 @@ def step( class IK_LM(IKSolver): - """ + r""" Levemberg-Marquadt Numerical Inverse Kinematics Solver A class which provides functionality to perform numerical inverse kinematics (IK) @@ -684,7 +700,15 @@ class IK_LM(IKSolver): :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful - :param tol: Maximum allowed residual error E + :param tol: Maximum allowed residual error E, where + :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form + in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`). + Because `E` is quadratic, `tol` does not bound the linear-scale position/ + orientation error directly — with the default unit weighting, components of + :math:`\vec{e}` are only guaranteed to be within roughly + :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on + the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific + linear-scale accuracy :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority :param joint_limits: Reject solutions with joint limit violations @@ -898,7 +922,7 @@ def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray): class IK_GN(IKSolver): - """ + r""" Gauss-Newton Numerical Inverse Kinematics Solver A class which provides functionality to perform numerical inverse kinematics (IK) @@ -912,7 +936,15 @@ class IK_GN(IKSolver): :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful - :param tol: Maximum allowed residual error E + :param tol: Maximum allowed residual error E, where + :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form + in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`). + Because `E` is quadratic, `tol` does not bound the linear-scale position/ + orientation error directly — with the default unit weighting, components of + :math:`\vec{e}` are only guaranteed to be within roughly + :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on + the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific + linear-scale accuracy :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority :param joint_limits: Reject solutions with joint limit violations @@ -1070,7 +1102,7 @@ def step( class IK_QP(IKSolver): - """ + r""" Quadratic Progamming Numerical Inverse Kinematics Solver A class which provides functionality to perform numerical inverse kinematics (IK) @@ -1081,7 +1113,15 @@ class IK_QP(IKSolver): :param ilimit: How many iterations are allowed within a search before a new search is started :param slimit: How many searches are allowed before being deemed unsuccessful - :param tol: Maximum allowed residual error E + :param tol: Maximum allowed residual error E, where + :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form + in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`). + Because `E` is quadratic, `tol` does not bound the linear-scale position/ + orientation error directly — with the default unit weighting, components of + :math:`\vec{e}` are only guaranteed to be within roughly + :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on + the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific + linear-scale accuracy :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority :param joint_limits: Reject solutions with joint limit violations diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index 2736fb586..9753593ed 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -14,8 +14,11 @@ from typing import overload import roboticstoolbox as rtb -from roboticstoolbox.robot.ETS import ETS, ETS2 -from roboticstoolbox.robot.ET import ET, ET2, BaseET +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 diff --git a/src/roboticstoolbox/robot/PoERobot.py b/src/roboticstoolbox/robot/PoERobot.py index 98bd4d1a6..0a6a32ac8 100644 --- a/src/roboticstoolbox/robot/PoERobot.py +++ b/src/roboticstoolbox/robot/PoERobot.py @@ -3,8 +3,8 @@ from spatialmath import Twist3, SE3 from spatialmath.base import skew from roboticstoolbox.robot import Link, Robot -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS class PoELink(Link): @@ -108,8 +108,8 @@ def _ets_world(self, twist: Twist3) -> ETS: ET.Ry(rpy[1]), ET.Rx(rpy[0]), ] - # remove ETs with empty transform (eta=None means joint variable, skip) - et_list = [et for et in et_list if et.eta is None or not np.isclose(et.eta, 0.0)] # type: ignore[arg-type] + # remove ETs with empty transform (param=None means joint variable, skip) + et_list = [et for et in et_list if et.param is None or not np.isclose(et.param, 0.0)] # type: ignore[arg-type] # assign joint variable at the end of list (if the frame is not base or tool # frame) @@ -315,7 +315,7 @@ def _update_ets(self): ET.Rx(rpy[0]), ] # remove ETs with empty transform - et_list = [et for et in et_list if et.eta is None or not np.isclose(et.eta, 0.0)] # type: ignore[arg-type] + et_list = [et for et in et_list if et.param is None or not np.isclose(et.param, 0.0)] # type: ignore[arg-type] # assign joint variable with corresponding index if self.links[i].isrevolute: diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 507c0f3f8..685c3d761 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -40,7 +40,8 @@ from roboticstoolbox.robot.RobotKinematics import RobotKinematicsMixin from roboticstoolbox.robot.Gripper import Gripper from roboticstoolbox.robot.Link import BaseLink, Link, Link2 -from roboticstoolbox.robot.ETS import ETS, ETS2 +from roboticstoolbox.ets.ETS import ETS +from roboticstoolbox.ets.ETS2 import ETS2 from roboticstoolbox.tools import URDF from roboticstoolbox.tools.types import ArrayLike, NDArray from roboticstoolbox.tools.data import rtb_path_to_datafile @@ -108,8 +109,15 @@ def __init__( # We're passed an ETS string links = [] # chop it up into segments, a link frame after every joint + # split()'s default "last" method folds any base content into + # the first segment, so `base` is always empty and dropped; + # `gripper` holds trailing constant content, if any, and + # becomes one extra static (non-joint) link. + _, *segs, gripper = arg.split() + if gripper: + segs.append(gripper) parent = None - for j, ets_j in enumerate(arg.split()): + for j, ets_j in enumerate(segs): elink = Link(ETS(ets_j), parent=parent, name=f"link{j:d}") if ( elink.qlim is None @@ -637,7 +645,7 @@ def reach(self) -> float: elif et.qlim is not None: # pragma nocover d += max(et.qlim) else: - d += abs(et.eta) + d += abs(et.param) link = link.parent if link is None or isinstance(link, str): d_all.append(d) @@ -1804,8 +1812,15 @@ def __init__(self, arg, **kwargs): # we're passed an ETS string links = [] # chop it up into segments, a link frame after every joint + # split()'s default "last" method folds any base content into + # the first segment, so `base` is always empty and dropped; + # `gripper` holds trailing constant content, if any, and + # becomes one extra static (non-joint) link. + _, *segs, gripper = arg.split() + if gripper: + segs.append(gripper) parent = None - for j, ets_j in enumerate(arg.split()): + for j, ets_j in enumerate(segs): elink = Link2(ETS2(ets_j), parent=parent, name=f"link{j:d}") parent = elink if ( @@ -1911,7 +1926,7 @@ def reach(self) -> float: elif et.qlim is not None: # pragma nocover d += max(et.qlim) else: - d += abs(et.eta) + d += abs(et.param) link = link.parent if link is None or isinstance(link, str): d_all.append(d) diff --git a/src/roboticstoolbox/robot/RobotProto.py b/src/roboticstoolbox/robot/RobotProto.py index fc4045e2a..7424c0d0c 100644 --- a/src/roboticstoolbox/robot/RobotProto.py +++ b/src/roboticstoolbox/robot/RobotProto.py @@ -4,7 +4,7 @@ from roboticstoolbox.robot.Link import Link, BaseLink from roboticstoolbox.robot.Gripper import Gripper -from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.ets.ETS import ETS from spatialmath import SE3 diff --git a/src/roboticstoolbox/robot/__init__.py b/src/roboticstoolbox/robot/__init__.py index 8862bd7bb..317ce23eb 100644 --- a/src/roboticstoolbox/robot/__init__.py +++ b/src/roboticstoolbox/robot/__init__.py @@ -12,9 +12,8 @@ from roboticstoolbox.robot.ERobot import ERobot, ERobot2 from roboticstoolbox.robot.ELink import ELink, ELink2 -from roboticstoolbox.robot.ETS import ETS, ETS2 +from roboticstoolbox.ets import ET, ET2, ETS, ETS2 from roboticstoolbox.robot.Gripper import Gripper -from roboticstoolbox.robot.ET import ET, ET2 from roboticstoolbox.robot.IK import IKSolution, IKSolver, IK_LM, IK_NR, IK_GN, IK_QP diff --git a/src/roboticstoolbox/robot/cpp-extensions/README.md b/src/roboticstoolbox/robot/cpp-extensions/README.md index a0209bbe3..0cc8f5288 100644 --- a/src/roboticstoolbox/robot/cpp-extensions/README.md +++ b/src/roboticstoolbox/robot/cpp-extensions/README.md @@ -1,81 +1,25 @@ -# Fast/optimized kinematics and dynamics +# Fast recursive Newton-Euler (frne) ## Synopsis -nanobind C++/C extensions that accelerate the two hot paths of robot -computation: +nanobind C++/C extension that accelerates `DHRobot.rne()`: recursive +Newton-Euler inverse dynamics. These files were developed for a "MEX +version" of robot dynamics in the MATLAB version of the Robotics Toolbox +— `ne.c` is virtually unchanged. -* **fknm** (`_fknm_c`) — ETS forward kinematics, Jacobians, Hessians, and IK - (Newton-Raphson, Gauss-Newton, Levenberg-Marquardt), used by `ETS`/`Robot`. -* **frne** (`_frne_c`) — recursive Newton-Euler inverse dynamics, used by - `DHRobot.rne()`. +This is one of two such extensions in the codebase — the other, **fknm** +(`_fknm_c`, ETS forward kinematics/Jacobians/Hessians/IK), lives at +`ets/cpp-extensions/` since it's an ETS concern, not a dynamics one. See +that directory's README for the architecture patterns shared by both +extensions (build/install mechanics, the Python facade + pure-Python +fallback pattern, lazy serialization into the C++ mirror object). -Both are optional: `robot/fknm.py` and `robot/frne.py` are pure-Python -facades that try to import the compiled extension and transparently fall -back to an equivalent pure-Python implementation when it's unavailable -(Pyodide/WASM builds without the extension compiled, CI paths that force -the Python implementation, or symbolic/SymPy inputs, which the C++ side -can't handle). - -## How it works - -* Built via nanobind, driven by the top-level `CMakeLists.txt` / - scikit-build-core. The compiled modules install as - `roboticstoolbox._fknm_c` / `roboticstoolbox._frne_c` at the top level of - the installed package (`CMakeLists.txt`: `DESTINATION roboticstoolbox`) — - that install location is independent of where this source directory lives. -* `fknm.py` / `frne.py` each do `from roboticstoolbox._fknm_c import ...` - (or `_frne_c`) inside a `try/except ImportError`, and provide a pure-Python - equivalent for every function on the `except` path. Symbolic inputs are - detected per-call and routed to the Python path even when the extension is - available. -* **Lazy serialization into the extension.** The C++ side needs its own copy - of the robot's state (`ETS`/`ET` structs for fknm, a `Robot` struct for - frne) — Python objects can't be handed across the boundary directly. Rather - than re-serializing on every mutation, `ETS`/`BaseETS` and `DHRobot` each - keep a C++-side mirror object (`_fknm`, `_frne`) plus a dirty flag - (`_fknm_stale`, `_frne_stale`). Mutating methods are wrapped with the - `@_dirties_fknm` / `@_dirties_frne` decorators, which just set the flag - after the wrapped call — they don't touch the mirror themselves. The - mirror is only actually rebuilt (`_copy_to_cpp()`) the next time a C++ - function is about to be called and the flag is set. This means a whole - sequence of mutations (e.g. building up an `ETS` element by element, or - setting several dynamic parameters on a `DHLink`) costs one rebuild total, - not one per mutation, and a mirror that's never used for a C++ call is - never built at all. - -## Gotcha! - -Internal transform matrices (ET/ETS results, Jacobians) are stored -**column-major** — Eigen's native layout, not NumPy's default row-major -layout. This is intentional (it's what `Eigen::Map` wants) and callers don't -need to do anything about it: the nanobind bindings (`EigenRef4d`, -`EigenRefJd`) accept C-contiguous, F-contiguous, or arbitrary-stride NumPy -arrays and let Eigen copy element-by-element using the real strides, so no -Python-side layout conversion is ever required before calling into C++. +It's optional: `robot/frne.py` is a pure-Python facade that tries to +import the compiled extension and transparently falls back to an +equivalent pure-Python implementation when it's unavailable. # Files -## Fast Forward Kinematics (fknm) - -| File | Purpose | -| ---- | ------- | -| Eigen | Vendored Eigen headers (header-only) | -| fknm_nb.cpp | nanobind glue for `_fknm_c` — binds FK, Jacobian, Hessian, IK, and ET init/update | -| ik.cpp | Fast inverse kinematics (Newton-Raphson, Gauss-Newton, Levenberg-Marquardt) | -| ik.h | " | -| linalg.cpp | SE(3) matrix operations | -| linalg.h | " | -| methods.cpp | Jacobians, Hessians | -| methods.h | " | -| structs.cpp | ET, ETS mirror | -| structs.h | " | - -## Fast Recursive Newton-Euler (frne) - -These files were developed for a "MEX version" of robot dynamics in the -MATLAB version of the Robotics Toolbox. `ne.c` is virtually unchanged. - | File | Purpose | | ---- | ------- | | frne_nb.cpp | nanobind glue for `_frne_c` — binds `init`/`frne`/`delete` | diff --git a/src/roboticstoolbox/tools/p_servo.py b/src/roboticstoolbox/tools/p_servo.py index b3d50560e..d787ba543 100644 --- a/src/roboticstoolbox/tools/p_servo.py +++ b/src/roboticstoolbox/tools/p_servo.py @@ -10,9 +10,9 @@ def angle_axis(T, Td) -> NDArray: - # deferred: roboticstoolbox.robot.fknm imports the robot package, which + # deferred: roboticstoolbox.ets.fknm imports the ets package, which # depends on roboticstoolbox.tools being fully initialised first - from roboticstoolbox.robot.fknm import Angle_Axis + from roboticstoolbox.ets.fknm import Angle_Axis try: e: NDArray = Angle_Axis(T, Td) diff --git a/tech-debt.md b/tech-debt.md index 359a02350..612bdccb3 100644 --- a/tech-debt.md +++ b/tech-debt.md @@ -726,6 +726,39 @@ handed back — matching the C++ semantics. See the fix on --- +## Fixed: `tol` is a quadratic residual, so default IK tests under-guaranteed pose accuracy + +### Background + +`test_DHRobot.py::test_ikine_LM` and `test_blocks.py::RobotBlockTest::test_ikine` +intermittently (the former) or deterministically at `seed=0` (the latter) +failed a `places=4` (~5e-5) check on `np.linalg.norm(T - fkine(sol.q))`, even +though `sol.success` was `True`. Root cause: `IKSolver`'s `tol` bounds the +*quadratic* weighted angle-axis error +:math:`E = \tfrac{1}{2}\vec{e}^\top\mat{W}_e\vec{e}`, not the linear-scale +pose error. With the default `tol=1e-6`, the actual pose error is only +guaranteed to be on the order of :math:`\sqrt{2\cdot\text{tol}} \approx 1.4 +\times 10^{-3}` — looser than the `places=4` checks assumed. Confirmed +concretely on the Panda `test_ikine` case: `sol.residual = 2.87e-8` (well +under `tol=1e-6`) still coincided with a raw pose difference of `3.27e-4`, +consistent with the quadratic/linear relationship, not a solver defect. + +`test_DHRobot.py::test_ikine_LM` additionally had no pinned `seed`, so it +was genuinely non-deterministic between runs (unlike `test_blocks.py`'s +`seed=0`, which just deterministically landed on the same marginal +solution every time — a reproducible near-miss, not a random flake). + +### Fix + +- Documented the quadratic-vs-linear relationship directly on `tol`'s + docstring (`IKSolver` and all four solver subclasses in + `src/roboticstoolbox/robot/IK.py`). +- Both tests now pass `tol=1e-10` explicitly (comfortably bounding pose + error under `places=4`'s ~5e-5 threshold), and `test_ikine_LM` now pins + `seed=0` for reproducibility. + +--- + ## Removed: `robot_descriptions` CI caching (was solving the wrong problem) ### Background @@ -1014,3 +1047,52 @@ Worth first confirming whether this is purely cosmetic (a shutdown-time diagnostic with no real-world consequence for a long-running process) or an actual growing-memory leak in a long-lived Swift session — the repro above only demonstrates the message, not measured memory growth. + +--- + +## `IK.py`'s solvers are typed against `ETS` but only need a small FK/Jacobian surface + +### Background + +`IKSolver._solve`/`step`/`_random_q`/`_check_jl`/`_null_Σ` (`src/roboticstoolbox/robot/IK.py`) +all take an `ets: "rtb.ETS"` parameter, but only ever call a small, fixed +set of things on it: + +- `n` (int) — joint count +- `qlim` (2×n array) — joint limits +- `jindices` — joint index array +- `joints()` — iterate joint ETs (used once, to find max jindex) +- `eval(q)` — forward kinematics, raw matrix +- `jacob0(q)` — base-frame Jacobian +- `jacobm(q)` — manipulability Jacobian (only for the null-space `kq`/`km` terms) + +Typing this as literally `ETS` overstates the coupling: nothing in `IK.py` +needs the rest of `ETS`'s surface (`merge`/`swap`/`split`/etc.), and the +solvers would work unchanged against anything else exposing this same +shape. (Note: this isn't a circular-import problem today — `IK.py` +already does `import roboticstoolbox as rtb` and only references +`rtb.ETS` inside string-quoted type hints, deferred and never evaluated +at runtime. This is about honestly documenting the dependency, not +working around an import cycle.) + +`RobotProto.py` already establishes exactly this pattern for two other +mixins: `KinematicsProtocol` (needs just `_T` + `.ets()`, used by +`RobotKinematicsMixin`) and the larger `RobotProto` (used by the +`Dynamics` mixin). Both are `typing.Protocol` classes specifically so +mixins can be type-checked against a structural contract without +importing the concrete class. + +### Proposed fix + +Add a third protocol alongside those two — proposed name `IKProtocol` +(matches the existing `KinematicsProtocol`/`RobotProto` naming, and says +what it's for without the discarded, less-precise "IK-capable" framing) +— declaring exactly the seven-item surface above. Change every +`ets: "rtb.ETS"` in `IK.py` to `ets: "IKProtocol"`. `ETS` already +structurally satisfies it, so no change needed there; the payoff is that +the real dependency becomes explicit rather than implicit. + +**Deferred** while the `/ets` package split (moving `ET`/`ETS` out of +`/robot`, see the ETS/fknm refactor entry above) is underway — worth +revisiting together since the import surface `IK.py` needs from the new +package location is the same shape this protocol would formalize. diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index 3e755811f..56f12308b 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -968,7 +968,12 @@ def test_ikine_LM(self): T = puma.fkine(puma.qn) - sol = puma.ikine_LM(T) + # `tol` bounds the quadratic angle-axis residual E, not the linear + # pose error directly (see IKSolver's `tol` docs) - the default + # tol=1e-6 only guarantees pose error on the order of 1e-3, not the + # 1e-4-ish precision `places=4` below checks for. Tighten tol + # accordingly and pin a seed so the search is reproducible. + sol = puma.ikine_LM(T, tol=1e-10, seed=0) self.assertTrue(sol.success) self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=4) diff --git a/tests/test_ET.py b/tests/test_ET.py index e2aa2677f..563f676ba 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -11,7 +11,7 @@ import spatialmath.base as sm from spatialmath import SE3 import unittest -from roboticstoolbox.robot.ET import BaseET +from roboticstoolbox.ets._ET import BaseET import sympy from copy import copy, deepcopy @@ -113,8 +113,8 @@ def test_repr(self): tx = rtb.ET.tx(1.543, jindex=5, flip=True, qlim=[-1, 1]) se = rtb.ET.SE3(SE3.Rx(0.3) * SE3.Ry(0.5), jindex=5, flip=True, qlim=[-1, 1]) - arx = "ET.Rx(eta=1.543, jindex=5, flip=True, qlim=array([-1., 1.]))" - atx = "ET.tx(eta=1.543, jindex=5, flip=True, qlim=array([-1., 1.]))" + arx = "ET.Rx(param=1.543, jindex=5, flip=True, qlim=array([-1., 1.]))" + atx = "ET.tx(param=1.543, jindex=5, flip=True, qlim=array([-1., 1.]))" ase = "ET.SE3(T=array([[ 0.87758256, 0. , 0.47942554, 0. ]," print(repr(se)) @@ -191,7 +191,7 @@ def test_axis_error(self): BaseET("Rx") with nt.assert_raises(TypeError): - BaseET("Rx", eta=0.5) + BaseET("Rx", param=0.5) def test_jindex(self): et1 = rtb.ET.Rx(1.5, jindex=2) @@ -339,6 +339,49 @@ def test_jindex_error(self): with self.assertRaises(ValueError): r1.jindex = -2 + def test_param_setter_updates_transform(self): + # ETS.merge() reassigns `.param` on an already-constructed ET (to + # combine two adjacent static transforms). The compiled fast path + # (`.A()` -> ET_T) and the qlim/jindex the C struct also carries + # must reflect the new value, not the value at construction time. + r1 = rtb.ET.tx(1.0) + nt.assert_almost_equal(r1.A(), sm.transl(1.0, 0, 0)) + + r1.param = 3.0 + self.assertEqual(r1.param, 3.0) + nt.assert_almost_equal(r1.A(), sm.transl(3.0, 0, 0)) + + # deepcopy must rebuild its own compiled struct from the updated + # state, not the stale one from construction + r2 = deepcopy(r1) + nt.assert_almost_equal(r2.A(), sm.transl(3.0, 0, 0)) + + def test_et2_no_compiled_accel(self): + # ET2 is pure Python and must never build/hold a compiled + # acceleration handle: calling the C fast path (ET_T) directly on + # an ET2's data is undefined behaviour (it assumes a 4x4 SE(3) + # buffer, but ET2 stores 3x3 SE(2) matrices). Asserting `.fknm` + # doesn't exist keeps this structurally impossible rather than + # relying on nothing ever calling the fast path by accident. + e = rtb.ET2.tx(1.0) + + self.assertFalse(hasattr(e, "fknm")) + self.assertFalse(hasattr(e, "_ET__fknm")) + + # param/qlim/jindex updates on ET2 must not attempt to touch a + # compiled struct that doesn't exist + e.param = 2.0 + nt.assert_almost_equal(e.A(), sm.transl2(2.0, 0)) + e.qlim = (-1, 1) + e.jindex = 0 + + def test_et_has_compiled_accel(self): + # Counterpart to test_et2_no_compiled_accel: ET (3D) does build a + # compiled struct, and it survives deepcopy as a distinct object + # (see also test_copy). + r1 = rtb.ET.Rx(1.0) + self.assertIsNotNone(r1.fknm) + def test_et2_T(self): fl = 1.543 rx = rtb.ET2.R() @@ -353,6 +396,200 @@ def test_et2_T(self): nt.assert_array_almost_equal(se.A(), sm.trot2(fl) @ sm.transl2(fl, 0)) nt.assert_array_almost_equal(tyf.A(fl), sm.transl2(0, -fl)) + def test_kind(self): + self.assertEqual(rtb.ET.Rx(1.0).kind, "Rx") + self.assertEqual(rtb.ET.Ry(1.0).kind, "Ry") + self.assertEqual(rtb.ET.Rz(1.0).kind, "Rz") + self.assertEqual(rtb.ET.tx(1.0).kind, "tx") + self.assertEqual(rtb.ET.ty(1.0).kind, "ty") + self.assertEqual(rtb.ET.tz(1.0).kind, "tz") + self.assertEqual(rtb.ET.SE3(SE3.Rx(0.5)).kind, "SE3") + + self.assertEqual(rtb.ET2.R(1.0).kind, "R") + self.assertEqual(rtb.ET2.tx(1.0).kind, "tx") + self.assertEqual(rtb.ET2.ty(1.0).kind, "ty") + self.assertEqual(rtb.ET2.SE2(sm.trot2(0.5)).kind, "SE2") + + def test_axis_deprecated(self): + # .axis is a permanent deprecated alias for .kind (never repurposed + # for the x/y/z meaning - see .ax below) + e = rtb.ET.Rx(1.0) + + with self.assertWarns(DeprecationWarning): + axis = e.axis + + self.assertEqual(axis, e.kind) + self.assertEqual(axis, "Rx") + + def test_ax(self): + self.assertEqual(rtb.ET.Rx(1.0).ax, "x") + self.assertEqual(rtb.ET.Ry(1.0).ax, "y") + self.assertEqual(rtb.ET.Rz(1.0).ax, "z") + self.assertEqual(rtb.ET.tx(1.0).ax, "x") + self.assertEqual(rtb.ET.ty(1.0).ax, "y") + self.assertEqual(rtb.ET.tz(1.0).ax, "z") + self.assertIsNone(rtb.ET.SE3(SE3.Rx(0.5)).ax) + + self.assertIsNone(rtb.ET2.R(1.0).ax) + self.assertEqual(rtb.ET2.tx(1.0).ax, "x") + self.assertEqual(rtb.ET2.ty(1.0).ax, "y") + self.assertIsNone(rtb.ET2.SE2(sm.trot2(0.5)).ax) + + def test_eta_property_deprecated(self): + # .eta is a permanent deprecated alias for .param - both getter and + # setter must warn and behave identically to .param + e = rtb.ET.tx(1.0) + + with self.assertWarns(DeprecationWarning): + value = e.eta + + self.assertEqual(value, e.param) + self.assertEqual(value, 1.0) + + with self.assertWarns(DeprecationWarning): + e.eta = 2.0 + + self.assertEqual(e.param, 2.0) + nt.assert_almost_equal(e.A(), sm.transl(2.0, 0, 0)) + + def test_eta_kwarg_deprecated(self): + # eta= is a permanent deprecated alias for param= on every factory + # classmethod and on BaseET.__init__ directly + with self.assertWarns(DeprecationWarning): + e = rtb.ET.tx(eta=1.5) + + self.assertEqual(e.param, 1.5) + nt.assert_almost_equal(e.A(), sm.transl(1.5, 0, 0)) + + with self.assertWarns(DeprecationWarning): + e2 = BaseET("tx", eta=1.5, axis_func=lambda x: sm.transl(x, 0, 0)) + + self.assertEqual(e2.param, 1.5) + + def test_joint_descriptor_string(self): + cases = [ + ("theta2", 2, False), + ("q2", 2, False), + ("-q(3)", 3, True), + ("θ_3", 3, False), + ] + for s, jindex, flip in cases: + e = rtb.ET.Rx(s) + self.assertTrue(e.isjoint) + self.assertEqual(e.jindex, jindex, s) + self.assertEqual(e.isflip, flip, s) + self.assertEqual(str(e), f"Rx({s})") + + # ET2 gets the same treatment, no special-casing needed + e2 = rtb.ET2.R("-q(4)") + self.assertEqual(e2.jindex, 4) + self.assertTrue(e2.isflip) + + def test_joint_descriptor_kinematics(self): + # the parsed descriptor must behave exactly like a normal joint + e = rtb.ET.Rx("-q(3)") + nt.assert_almost_equal(e.A(0.5), sm.trotx(-0.5)) + + e2 = rtb.ET.Rx("theta2") + nt.assert_almost_equal(e2.A(0.5), sm.trotx(0.5)) + + def test_joint_descriptor_no_digit_falls_back_to_auto_numbering(self): + e = rtb.ET.Rx("theta") + self.assertTrue(e.isjoint) + self.assertIsNone(e.jindex) + self.assertFalse(e.isflip) + + def test_joint_descriptor_numeric_string_is_static_value(self): + # a string that parses as a plain number is a static value, not a + # joint descriptor + e = rtb.ET.tx("1.5") + self.assertFalse(e.isjoint) + self.assertEqual(e.param, 1.5) + nt.assert_almost_equal(e.A(), sm.transl(1.5, 0, 0)) + + def test_joint_descriptor_conflict_raises(self): + with self.assertRaises(ValueError): + rtb.ET.Rx("theta2", jindex=5) + + with self.assertRaises(ValueError): + rtb.ET.Rx("theta2", flip=True) + + def test_free_functions(self): + # roboticstoolbox.ets.ET/.ET2 expose Rx/Ry/Rz/tx/ty/tz/SE3 and + # R/tx/ty/SE2 respectively as bare module-level functions (not just + # ET.Rx/ET2.tx classmethods), so `from roboticstoolbox.ets.ET import *` + # works. tx/ty deliberately mean different things (3D vs 2D) between + # the two modules - that's the one thing wildcard-importing both at + # once can't avoid. + from roboticstoolbox.ets import ET as ET_module + from roboticstoolbox.ets import ET2 as ET2_module + + # Rx/tx/etc are bound classmethods, so a fresh attribute access + # (ET_module.Rx vs rtb.ET.Rx) produces a distinct-but-equal bound + # method object each time - compare with == (same __func__/__self__), + # not `is`. + self.assertEqual(ET_module.Rx, rtb.ET.Rx) + self.assertEqual(ET_module.tx, rtb.ET.tx) + self.assertEqual(ET_module.SE3, rtb.ET.SE3) + + self.assertEqual(ET2_module.R, rtb.ET2.R) + self.assertEqual(ET2_module.tx, rtb.ET2.tx) + self.assertEqual(ET2_module.SE2, rtb.ET2.SE2) + + nt.assert_almost_equal( + ET_module.tx(1.5).A(), rtb.ET.tx(1.5).A() + ) + nt.assert_almost_equal( + ET2_module.tx(1.5).A(), rtb.ET2.tx(1.5).A() + ) + # confirm they really are different (3D vs 2D), not the same object + self.assertNotEqual(ET_module.tx(1.5).A().shape, ET2_module.tx(1.5).A().shape) + + def test_sum(self): + # __add__ is an alias for __mul__ (composition) on ET/ET2, and + # __radd__ (treating a start value of 0 as identity) is what lets + # sum() work without an explicit start + e1 = rtb.ET.Rz(jindex=0) + e2 = rtb.ET.tx(1) + e3 = rtb.ET.Rz(jindex=1) + expected = e1 * e2 * e3 + + r_add = e1 + e2 + e3 + self.assertIsInstance(r_add, rtb.ETS) + self.assertEqual(r_add, expected) + + r_sum = sum([e1, e2, e3]) + self.assertIsInstance(r_sum, rtb.ETS) + self.assertEqual(r_sum, expected) + + f1 = rtb.ET2.R(jindex=0) + f2 = rtb.ET2.tx(1) + f3 = rtb.ET2.R(jindex=1) + expected2 = f1 * f2 * f3 + + s_add = f1 + f2 + f3 + self.assertIsInstance(s_add, rtb.ETS2) + self.assertEqual(s_add, expected2) + + s_sum = sum([f1, f2, f3]) + self.assertIsInstance(s_sum, rtb.ETS2) + self.assertEqual(s_sum, expected2) + + # BaseETS.__radd__ makes sum() work on a list of ETS/ETS2 too, not + # just their individual elements + ets_sum = sum([e1 * e2, e3]) + self.assertIsInstance(ets_sum, rtb.ETS) + self.assertEqual(ets_sum, expected) + + ets2_sum = sum([f1 * f2, f3]) + self.assertIsInstance(ets2_sum, rtb.ETS2) + self.assertEqual(ets2_sum, expected2) + + # a genuinely bad start value still fails loudly rather than being + # silently swallowed + with self.assertRaises(TypeError): + sum([e1, e2, e3], 5) + if __name__ == "__main__": unittest.main() diff --git a/tests/test_ETS.py b/tests/test_ETS.py index d6f3d11a7..9022f3201 100644 --- a/tests/test_ETS.py +++ b/tests/test_ETS.py @@ -7,6 +7,11 @@ import numpy.testing as nt import roboticstoolbox as rtb + +# Rx/Ry/Rz/tx/ty/tz free functions for terser ET construction; ET also +# exports SE3 (a constant-transform ET constructor) via __all__, so this +# must precede the spatialmath SE3 import below to let that one win +from roboticstoolbox.ets.ET import * import numpy as np # import spatialmath.base as sm @@ -18,8 +23,8 @@ class TestETS(unittest.TestCase): def test_bad_arg(self): - rx = rtb.ET.Rx(1.543) - ry = rtb.ET.Ry(1.543) + rx = Rx(1.543) + ry = Ry(1.543) with self.assertRaises(TypeError): rtb.ETS([rx, ry, 1.0]) # type: ignore @@ -32,36 +37,36 @@ def test_args(self): # mm = 1e-3 # tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) r1 = l0 + l1 + l2 + l3 + l3 + l4 + l5 r2 = l0 * l1 * l2 * l3 * l3 * l4 * l5 r3 = rtb.ETS(l0 + l1 + l2 + l3 + l3 + l4 + l5) r4 = rtb.ETS([l0, l1, l2, l3, l3, l4, l5]) - r5 = rtb.ETS([l0, l1, l2, l3, l3, l4, rtb.ET.Rx(90 * deg), rtb.ET.Rz(jindex=5)]) + r5 = rtb.ETS([l0, l1, l2, l3, l3, l4, Rx(90 * deg), Rz(jindex=5)]) # r6 = rtb.ETS([r1]) - r7 = rtb.ETS(rtb.ET.Rx(1.0)) + r7 = rtb.ETS(Rx(1.0)) self.assertEqual(r1, r2) self.assertEqual(r1, r3) self.assertEqual(r1, r4) self.assertEqual(r1, r5) - self.assertEqual(r7[0], rtb.ET.Rx(1.0)) + self.assertEqual(r7[0], Rx(1.0)) self.assertEqual(r1 + rtb.ETS(), r2) def test_empty(self): @@ -70,13 +75,13 @@ def test_empty(self): self.assertEqual(r.m, 0) def test_str(self): - rx = rtb.ET.Rx(1.543) - ry = rtb.ET.Ry(1.543) - rz = rtb.ET.Rz(1.543) - a = rtb.ET.Rx() - b = rtb.ET.Ry() - c = rtb.ET.Rz() - d = rtb.ET.tx(1.0) + rx = Rx(1.543) + ry = Ry(1.543) + rz = Rz(1.543) + a = Rx() + b = Ry() + c = Rz() + d = tx(1.0) e = rtb.ET.SE3(SE3.Rx(1.0)) ets = rx * ry * rz * d ets2 = rx * a * b * c @@ -90,39 +95,39 @@ def test_str(self): ) def test_str_jindex(self): - rx = rtb.ET.Rx(1.543) - a = rtb.ET.Rx(jindex=2) - b = rtb.ET.Ry(jindex=5) - c = rtb.ET.Rz(jindex=7) + rx = Rx(1.543) + a = Rx(jindex=2) + b = Ry(jindex=5) + c = Rz(jindex=7) ets = rx * a * b * c self.assertEqual(str(ets), "Rx(88.41°) ⊕ Rx(q2) ⊕ Ry(q5) ⊕ Rz(q7)") def test_str_flip(self): - rx = rtb.ET.Rx(1.543) - a = rtb.ET.Rx(jindex=2, flip=True) - b = rtb.ET.Ry(jindex=5) - c = rtb.ET.Rz(jindex=7) + rx = Rx(1.543) + a = Rx(jindex=2, flip=True) + b = Ry(jindex=5) + c = Rz(jindex=7) ets = rx * a * b * c self.assertEqual(str(ets), "Rx(88.41°) ⊕ Rx(-q2) ⊕ Ry(q5) ⊕ Rz(q7)") def test_str_sym(self): x = sympy.Symbol("x") - rx = rtb.ET.Rx(x) # type: ignore - a = rtb.ET.Rx(jindex=2) - b = rtb.ET.Ry(jindex=5) - c = rtb.ET.Rz(jindex=7) + rx = Rx(x) # type: ignore + a = Rx(jindex=2) + b = Ry(jindex=5) + c = Rz(jindex=7) ets = rx * a * b * c self.assertEqual(str(ets), "Rx(x) ⊕ Rx(q2) ⊕ Ry(q5) ⊕ Rz(q7)") def ets_mul(self): - rx = rtb.ET.Rx(1.543) - ry = rtb.ET.Ry(1.543) - rz = rtb.ET.Rz(1.543) - a = rtb.ET.Rx() - b = rtb.ET.Ry() + rx = Rx(1.543) + ry = Ry(1.543) + rz = Rz(1.543) + a = Rx() + b = Ry() ets1 = rx * ry ets2 = a * b @@ -136,11 +141,11 @@ def ets_mul(self): self.assertIsInstance(ets2, rtb.ETS) def test_n(self): - rx = rtb.ET.Rx(1.543) - ry = rtb.ET.Ry(1.543) - # rz = rtb.ET.Rz(1.543) - a = rtb.ET.Rx() - b = rtb.ET.Ry() + rx = Rx(1.543) + ry = Ry(1.543) + # rz = Rz(1.543) + a = Rx() + b = Ry() ets1 = rx * ry ets2 = a * b @@ -154,20 +159,20 @@ def test_n(self): def test_fkine(self): q = np.array([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]) - rx = rtb.ET.Rx(1.543) - ry = rtb.ET.Ry(1.543) - rz = rtb.ET.Rz(1.543) - tx = rtb.ET.tx(1.543) - ty = rtb.ET.ty(1.543) - tz = rtb.ET.tz(1.543) - a = rtb.ET.Rx(jindex=0) - b = rtb.ET.Ry(jindex=1) - c = rtb.ET.Rz(jindex=2) - d = rtb.ET.tx(jindex=3) - e = rtb.ET.ty(jindex=4) - f = rtb.ET.tz(jindex=5) - - r = rx * ry * rz * tx * ty * tz * a * b * c * d * e * f + rx = Rx(1.543) + ry = Ry(1.543) + rz = Rz(1.543) + t_x = tx(1.543) + t_y = ty(1.543) + t_z = tz(1.543) + a = Rx(jindex=0) + b = Ry(jindex=1) + c = Rz(jindex=2) + d = tx(jindex=3) + e = ty(jindex=4) + f = tz(jindex=5) + + r = rx * ry * rz * t_x * t_y * t_z * a * b * c * d * e * f ans = ( SE3.Rx(1.543) @@ -193,9 +198,9 @@ def test_fkine_sym(self): q = np.array([y, z]) qt = np.array([[1.0, y], [z, y], [x, 2.0]]) - a = rtb.ET.Rx(x) # type: ignore - b = rtb.ET.Ry(jindex=0) - c = rtb.ET.tz(jindex=1) + a = Rx(x) # type: ignore + b = Ry(jindex=0) + c = tz(jindex=1) r = a * b * c @@ -220,7 +225,7 @@ def test_fkine_sym(self): tool = SE3.Tz(0.5) ans5 = base * ans1 - r2 = rtb.ETS([rtb.ET.Rx(jindex=0)]) + r2 = rtb.ETS([Rx(jindex=0)]) nt.assert_almost_equal(r.fkine(q, base=base).A, sympy.simplify(ans5.A)) # nt.assert_almost_equal(r.fkine(q, base=base), ans5.A) # type: ignore @@ -236,12 +241,12 @@ def test_fkine_sym(self): def test_fkine_traj(self): robot = rtb.ERobot( [ - rtb.Link(rtb.ET.Rx()), - rtb.Link(rtb.ET.Ry()), - rtb.Link(rtb.ET.Rz()), - rtb.Link(rtb.ET.tx()), - rtb.Link(rtb.ET.ty()), - rtb.Link(rtb.ET.tz()), + rtb.Link(Rx()), + rtb.Link(Ry()), + rtb.Link(Rz()), + rtb.Link(tx()), + rtb.Link(ty()), + rtb.Link(tz()), ] ) @@ -264,31 +269,31 @@ def test_jacob0_panda(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee @@ -367,24 +372,24 @@ def test_jacobe_panda(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l0 = tz(0.333) * Rz(jindex=0) + l1 = Rx(-90 * deg) * Rz(jindex=1) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee @@ -399,10 +404,10 @@ def test_jacobe_panda(self): def test_pop(self): q = [1.0, 2.0, 3.0] - a = rtb.ET.Rx(jindex=0) - b = rtb.ET.Ry(jindex=1) - c = rtb.ET.Rz(jindex=2) - d = rtb.ET.tz(1.543) + a = Rx(jindex=0) + b = Ry(jindex=1) + c = Rz(jindex=2) + d = tz(1.543) ans1 = SE3.Rx(q[0]) * SE3.Ry(q[1]) * SE3.Rz(q[2]) * SE3.Tz(1.543) ans2 = SE3.Rx(q[0]) * SE3.Rz(q[2]) * SE3.Tz(1.543) @@ -422,10 +427,10 @@ def test_pop(self): def test_inv(self): q = [1.0, 2.0, 3.0] - a = rtb.ET.Rx(jindex=0) - b = rtb.ET.Ry(jindex=1) - c = rtb.ET.Rz(jindex=2) - d = rtb.ET.tz(1.543) + a = Rx(jindex=0) + b = Ry(jindex=1) + c = Rz(jindex=2) + d = tz(1.543) ans1 = SE3.Rx(q[0]) * SE3.Ry(q[1]) * SE3.Rz(q[2]) * SE3.Tz(1.543) @@ -436,10 +441,10 @@ def test_inv(self): def test_jointset(self): # q = [1.0, 2.0, 3.0] - a = rtb.ET.Rx(jindex=0) - b = rtb.ET.Ry(jindex=1) - c = rtb.ET.Rz(jindex=2) - d = rtb.ET.tz(1.543) + a = Rx(jindex=0) + b = Ry(jindex=1) + c = Rz(jindex=2) + d = tz(1.543) ans = set((0, 1, 2)) @@ -451,50 +456,57 @@ def test_split(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) - segs = [l0, l1, l2, l3, l4, l5, l6, ee] - segs3 = [l0, l1, l2, l3, l4, l5, l6, rtb.ETS([rtb.ET.Rx(0.5)])] + segs = [l0, l1, l2, l3, l4, l5, l6] r = l0 * l1 * l2 * l3 * l4 * l5 * l6 * ee r2 = l0 * l1 * l2 * l3 * l4 * l5 * l6 - r3 = l0 * l1 * l2 * l3 * l4 * l5 * l6 * rtb.ET.Rx(0.5) - - split = r.split() - split2 = r2.split() - split3 = r3.split() + r3 = l0 * l1 * l2 * l3 * l4 * l5 * l6 * Rx(0.5) + # split() returns [base, *segments, gripper]; base is always empty + # for the default "last" method (leading content is folded into the + # first segment) + base, *split, gripper = r.split() + self.assertEqual(len(base), 0) for i, link in enumerate(segs): self.assertEqual(link, split[i]) + self.assertEqual(gripper, ee) - for i, link in enumerate(split2): - self.assertEqual(link, segs[i]) + base2, *split2, gripper2 = r2.split() + self.assertEqual(len(base2), 0) + for i, link in enumerate(segs): + self.assertEqual(link, split2[i]) + self.assertEqual(len(gripper2), 0) - for i, link in enumerate(split3): - self.assertEqual(link, segs3[i]) + base3, *split3, gripper3 = r3.split() + self.assertEqual(len(base3), 0) + for i, link in enumerate(segs): + self.assertEqual(link, split3[i]) + self.assertEqual(gripper3, rtb.ETS([Rx(0.5)])) def test_compile(self): q = [0, 1.0, 2, 3, 4, 5, 6] @@ -502,31 +514,31 @@ def test_compile(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 * l1 * l2 * l3 * l4 * l5 * l6 * ee r2 = r.compile() @@ -540,31 +552,31 @@ def test_insert(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 * l1 * l2 * l3 * l4 * l5 * l6 * ee @@ -575,32 +587,117 @@ def test_insert(self): r3.append(ee) r4 = l0 * l1 * l2 * l3 * l4 * l5 * l6 - r4.append(rtb.ET.tz(tool_offset)) - r4.append(rtb.ET.Rz(-np.pi / 4)) + r4.append(tz(tool_offset)) + r4.append(Rz(-np.pi / 4)) r5 = l0 * l1 * l2 * l3 * l4 * l6 * ee - r5.insert(14, rtb.ET.Rx(90 * deg)) - r5.insert(15, rtb.ET.Rz(jindex=5)) + r5.insert(14, Rx(90 * deg)) + r5.insert(15, Rz(jindex=5)) nt.assert_almost_equal(r.eval(q), r2.eval(q)) nt.assert_almost_equal(r.eval(q), r3.eval(q)) nt.assert_almost_equal(r.eval(q), r4.eval(q)) nt.assert_almost_equal(r.fkine(q).A, r5.fkine(q).A) + def test_swap(self): + q = [1.0] + e = Rz(jindex=0) * Rz(1) * tx(2) + + ans = SE3.Rz(q[0]) * SE3.Rz(1) * SE3.Tx(2) + + self.assertTrue(e[0].isjoint) + self.assertFalse(e[1].isjoint) + nt.assert_almost_equal(e.fkine(q).A, ans.A) + + # swap two adjacent transforms that share the same axis - the + # kinematics must be unaffected since same-axis rotations commute + e.swap(0) + self.assertFalse(e[0].isjoint) + self.assertTrue(e[1].isjoint) + self.assertAlmostEqual(e[0].param, 1.0) # type: ignore + nt.assert_almost_equal(e.fkine(q).A, ans.A) + + # swap back and check we're symmetric + e.swap(0) + self.assertTrue(e[0].isjoint) + self.assertFalse(e[1].isjoint) + nt.assert_almost_equal(e.fkine(q).A, ans.A) + + def test_swap_not_commutative(self): + e = tx(1) * Rz(jindex=0) * Rz(1) + + with self.assertRaises(ValueError): + e.swap(0) + + def test_swap_bad_index(self): + e = tx(1) * tx(2) + + with self.assertRaises(IndexError): + e.swap(-1) + + with self.assertRaises(IndexError): + e.swap(1) + + def test_merge(self): + q = [1.0] + e = Rz(jindex=0) * tx(1) * tx(2) * Rz(1) + + ans = SE3.Rz(q[0]) * SE3.Tx(3) * SE3.Rz(1) + + n = len(e) + e.merge(1) + + self.assertEqual(len(e), n - 1) + self.assertEqual(e[1].kind, "tx") + self.assertAlmostEqual(e[1].param, 3.0) # type: ignore + nt.assert_almost_equal(e.fkine(q).A, ans.A) + + def test_merge_not_same_type(self): + e = tx(1) * Rz(1) + + with self.assertRaises(ValueError): + e.merge(0) + + def test_merge_both_joints(self): + e = Rz(jindex=0) * Rz(jindex=1) + + with self.assertRaises(ValueError): + e.merge(0) + + def test_merge_bad_index(self): + e = tx(1) * tx(2) + + with self.assertRaises(IndexError): + e.merge(-1) + + with self.assertRaises(IndexError): + e.merge(1) + + def test_joint_descriptor_ets_str(self): + # ETS.__str__ has its own joint-formatting logic, independent of + # each ET's own __str__ - a custom name from a string `param` + # descriptor (e.g. "theta2") must show up there too, since printing + # a full kinematic chain (not a lone ET) is the primary use case. + ets = Rx("theta0") * tx("q1") * Rz("-q(2)") * Ry() + + self.assertEqual(str(ets), "Rx(theta0) ⊕ tx(q1) ⊕ Rz(-q(2)) ⊕ Ry(q3)") + self.assertEqual([e.jindex for e in ets], [0, 1, 2, 3]) + self.assertEqual([e.isflip for e in ets], [False, False, True, False]) + def test_jacob0(self): q = [0.0] - rx = rtb.ETS(rtb.ET.Rx()) - ry = rtb.ETS(rtb.ET.Ry()) - rz = rtb.ETS(rtb.ET.Rz()) - tx = rtb.ETS(rtb.ET.tx()) - ty = rtb.ETS(rtb.ET.ty()) - tz = rtb.ETS(rtb.ET.tz()) - - r = tx + ty + tz + rx + ry + rz - - nt.assert_almost_equal(tx.jacob0(q), np.array([[1, 0, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(ty.jacob0(q), np.array([[0, 1, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(tz.jacob0(q), np.array([[0, 0, 1, 0, 0, 0]]).T) + rx = rtb.ETS(Rx()) + ry = rtb.ETS(Ry()) + rz = rtb.ETS(Rz()) + t_x = rtb.ETS(tx()) + t_y = rtb.ETS(ty()) + t_z = rtb.ETS(tz()) + + r = t_x + t_y + t_z + rx + ry + rz + + nt.assert_almost_equal(t_x.jacob0(q), np.array([[1, 0, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_y.jacob0(q), np.array([[0, 1, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_z.jacob0(q), np.array([[0, 0, 1, 0, 0, 0]]).T) nt.assert_almost_equal(rx.jacob0(q), np.array([[0, 0, 0, 1, 0, 0]]).T) nt.assert_almost_equal(ry.jacob0(q), np.array([[0, 0, 0, 0, 1, 0]]).T) nt.assert_almost_equal(rz.jacob0(q), np.array([[0, 0, 0, 0, 0, 1]]).T) @@ -608,18 +705,18 @@ def test_jacob0(self): def test_jacobe(self): q = [0.0] - rx = rtb.ETS(rtb.ET.Rx()) - ry = rtb.ETS(rtb.ET.Ry()) - rz = rtb.ETS(rtb.ET.Rz()) - tx = rtb.ETS(rtb.ET.tx()) - ty = rtb.ETS(rtb.ET.ty()) - tz = rtb.ETS(rtb.ET.tz()) - - r = tx + ty + tz + rx + ry + rz - - nt.assert_almost_equal(tx.jacobe(q), np.array([[1, 0, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(ty.jacobe(q), np.array([[0, 1, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(tz.jacobe(q), np.array([[0, 0, 1, 0, 0, 0]]).T) + rx = rtb.ETS(Rx()) + ry = rtb.ETS(Ry()) + rz = rtb.ETS(Rz()) + t_x = rtb.ETS(tx()) + t_y = rtb.ETS(ty()) + t_z = rtb.ETS(tz()) + + r = t_x + t_y + t_z + rx + ry + rz + + nt.assert_almost_equal(t_x.jacobe(q), np.array([[1, 0, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_y.jacobe(q), np.array([[0, 1, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_z.jacobe(q), np.array([[0, 0, 1, 0, 0, 0]]).T) nt.assert_almost_equal(rx.jacobe(q), np.array([[0, 0, 0, 1, 0, 0]]).T) nt.assert_almost_equal(ry.jacobe(q), np.array([[0, 0, 0, 0, 1, 0]]).T) nt.assert_almost_equal(rz.jacobe(q), np.array([[0, 0, 0, 0, 0, 1]]).T) @@ -629,21 +726,21 @@ def test_jacob0_sym(self): x = sympy.Symbol("x") q1 = np.array([x, x]) q2 = np.array([0, x]) - rx = rtb.ETS(rtb.ET.Rx(jindex=0)) - ry = rtb.ETS(rtb.ET.Ry(jindex=0)) - rz = rtb.ETS(rtb.ET.Rz(jindex=0)) - tx = rtb.ETS(rtb.ET.tx(jindex=0)) - ty = rtb.ETS(rtb.ET.ty(jindex=0)) - tz = rtb.ETS(rtb.ET.tz(jindex=1)) + rx = rtb.ETS(Rx(jindex=0)) + ry = rtb.ETS(Ry(jindex=0)) + rz = rtb.ETS(Rz(jindex=0)) + t_x = rtb.ETS(tx(jindex=0)) + t_y = rtb.ETS(ty(jindex=0)) + t_z = rtb.ETS(tz(jindex=1)) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) - r = tx + ty + tz + rx + ry + rz + a + r = t_x + t_y + t_z + rx + ry + rz + a print(r.jacob0(q2)) - nt.assert_almost_equal(tx.jacob0(q1), np.array([[1, 0, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(ty.jacob0(q1), np.array([[0, 1, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(tz.jacob0(q1), np.array([[0, 0, 1, 0, 0, 0]]).T) + nt.assert_almost_equal(t_x.jacob0(q1), np.array([[1, 0, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_y.jacob0(q1), np.array([[0, 1, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_z.jacob0(q1), np.array([[0, 0, 1, 0, 0, 0]]).T) nt.assert_almost_equal(rx.jacob0(q1), np.array([[0, 0, 0, 1, 0, 0]]).T) nt.assert_almost_equal(ry.jacob0(q1), np.array([[0, 0, 0, 0, 1, 0]]).T) nt.assert_almost_equal(rz.jacob0(q1), np.array([[0, 0, 0, 0, 0, 1]]).T) @@ -655,21 +752,21 @@ def test_jacobe_sym(self): x = sympy.Symbol("x") q1 = np.array([x, x]) q2 = np.array([0, x]) - rx = rtb.ETS(rtb.ET.Rx(jindex=0)) - ry = rtb.ETS(rtb.ET.Ry(jindex=0)) - rz = rtb.ETS(rtb.ET.Rz(jindex=0)) - tx = rtb.ETS(rtb.ET.tx(jindex=0)) - ty = rtb.ETS(rtb.ET.ty(jindex=0)) - tz = rtb.ETS(rtb.ET.tz(jindex=1)) + rx = rtb.ETS(Rx(jindex=0)) + ry = rtb.ETS(Ry(jindex=0)) + rz = rtb.ETS(Rz(jindex=0)) + t_x = rtb.ETS(tx(jindex=0)) + t_y = rtb.ETS(ty(jindex=0)) + t_z = rtb.ETS(tz(jindex=1)) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) - r = tx + ty + tz + rx + ry + rz + a + r = t_x + t_y + t_z + rx + ry + rz + a print(r.jacobe(q2)) - nt.assert_almost_equal(tx.jacobe(q1), np.array([[1, 0, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(ty.jacobe(q1), np.array([[0, 1, 0, 0, 0, 0]]).T) - nt.assert_almost_equal(tz.jacobe(q1), np.array([[0, 0, 1, 0, 0, 0]]).T) + nt.assert_almost_equal(t_x.jacobe(q1), np.array([[1, 0, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_y.jacobe(q1), np.array([[0, 1, 0, 0, 0, 0]]).T) + nt.assert_almost_equal(t_z.jacobe(q1), np.array([[0, 0, 1, 0, 0, 0]]).T) nt.assert_almost_equal(rx.jacobe(q1), np.array([[0, 0, 0, 1, 0, 0]]).T) nt.assert_almost_equal(ry.jacobe(q1), np.array([[0, 0, 0, 0, 1, 0]]).T) nt.assert_almost_equal(rz.jacobe(q1), np.array([[0, 0, 0, 0, 0, 1]]).T) @@ -682,31 +779,31 @@ def test_hessian0(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee @@ -1132,31 +1229,31 @@ def test_hessian0_tool(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) ee = ee.fkine([]) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 @@ -1583,31 +1680,31 @@ def test_hessiane(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee @@ -1640,31 +1737,31 @@ def test_hessiane_tool(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) ee = ee.fkine([]) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 @@ -1696,15 +1793,15 @@ def test_hessiane_tool(self): def test_hessian_sym(self): x = sympy.Symbol("x") q2 = np.array([0, x]) - rx = rtb.ETS(rtb.ET.Rx(jindex=0)) - ry = rtb.ETS(rtb.ET.Ry(jindex=0)) - rz = rtb.ETS(rtb.ET.Rz(jindex=0)) - tx = rtb.ETS(rtb.ET.tx(jindex=0)) - ty = rtb.ETS(rtb.ET.ty(jindex=0)) - tz = rtb.ETS(rtb.ET.tz(jindex=1)) + rx = rtb.ETS(Rx(jindex=0)) + ry = rtb.ETS(Ry(jindex=0)) + rz = rtb.ETS(Rz(jindex=0)) + t_x = rtb.ETS(tx(jindex=0)) + t_y = rtb.ETS(ty(jindex=0)) + t_z = rtb.ETS(tz(jindex=1)) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) - r = tx + ty + tz + rx + ry + rz + a + r = t_x + t_y + t_z + rx + ry + rz + a J0 = r.jacob0(q2) Je = r.jacobe(q2) @@ -1770,27 +1867,27 @@ def test_hessian_sym(self): def test_plot(self): q2 = np.array([0, 1, 2, 3, 4, 5]) - rx = rtb.ETS(rtb.ET.Rx(jindex=0)) - ry = rtb.ETS(rtb.ET.Ry(jindex=1)) - rz = rtb.ETS(rtb.ET.Rz(jindex=2)) - tx = rtb.ETS(rtb.ET.tx(jindex=3, qlim=[-1, 1])) - ty = rtb.ETS(rtb.ET.ty(jindex=4, qlim=[-1, 1])) - tz = rtb.ETS(rtb.ET.tz(jindex=5, qlim=[-1, 1])) + rx = rtb.ETS(Rx(jindex=0)) + ry = rtb.ETS(Ry(jindex=1)) + rz = rtb.ETS(Rz(jindex=2)) + t_x = rtb.ETS(tx(jindex=3, qlim=[-1, 1])) + t_y = rtb.ETS(ty(jindex=4, qlim=[-1, 1])) + t_z = rtb.ETS(tz(jindex=5, qlim=[-1, 1])) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) - r = tx + ty + tz + rx + ry + rz + a + r = t_x + t_y + t_z + rx + ry + rz + a r.plot(q=q2, block=False, backend="pyplot") def test_teach(self): # x = sympy.Symbol("x") q2 = np.array([0, 1, 2, 3, 4, 5]) - rx = rtb.ETS(rtb.ET.Rx(jindex=0)) - ry = rtb.ETS(rtb.ET.Ry(jindex=1)) - rz = rtb.ETS(rtb.ET.Rz(jindex=2)) - tx = rtb.ETS(rtb.ET.tx(jindex=3, qlim=[-1, 1])) - ty = rtb.ETS(rtb.ET.ty(jindex=4, qlim=[-1, 1])) - tz = rtb.ETS(rtb.ET.tz(jindex=5, qlim=[-1, 1])) + rx = rtb.ETS(Rx(jindex=0)) + ry = rtb.ETS(Ry(jindex=1)) + rz = rtb.ETS(Rz(jindex=2)) + t_x = rtb.ETS(tx(jindex=3, qlim=[-1, 1])) + t_y = rtb.ETS(ty(jindex=4, qlim=[-1, 1])) + t_z = rtb.ETS(tz(jindex=5, qlim=[-1, 1])) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) - r = tx + ty + tz + rx + ry + rz + a + r = t_x + t_y + t_z + rx + ry + rz + a r.teach(q=q2, block=False, backend="pyplot") def test_partial_fkine(self): @@ -1798,31 +1895,31 @@ def test_partial_fkine(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + l0 = tz(0.333) * Rz(jindex=0) - l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + l1 = Rx(-90 * deg) * Rz(jindex=1) - l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + l2 = Rx(90 * deg) * tz(0.316) * Rz(jindex=2) - l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + l3 = tx(0.0825) * Rx(90 * deg) * Rz(jindex=3) l4 = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rx(-90 * deg) - * rtb.ET.tz(0.384) - * rtb.ET.Rz(jindex=4) + tx(-0.0825) + * Rx(-90 * deg) + * tz(0.384) + * Rz(jindex=4) ) - l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + l5 = Rx(90 * deg) * Rz(jindex=5) l6 = ( - rtb.ET.tx(0.088) - * rtb.ET.Rx(90 * deg) - * rtb.ET.tz(0.107) - * rtb.ET.Rz(jindex=6) + tx(0.088) + * Rx(90 * deg) + * tz(0.107) + * Rz(jindex=6) ) - ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + ee = tz(tool_offset) * Rz(-np.pi / 4) r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee r2 = rtb.Robot(r) @@ -4263,37 +4360,37 @@ def test_partial_fkine(self): nt.assert_almost_equal(fk2, ans) def test_qlim1(self): - rx = rtb.ETS(rtb.ET.Rx()) + rx = rtb.ETS(Rx()) q = rx.qlim nt.assert_equal(q, np.array([[-np.pi], [np.pi]])) def test_qlim2(self): - rx = rtb.ETS(rtb.ET.Rx(qlim=[-1, 1])) + rx = rtb.ETS(Rx(qlim=[-1, 1])) q = rx.qlim nt.assert_equal(q, np.array([[-1], [1]])) def test_qlim3(self): - rx = rtb.ETS(rtb.ET.tx(qlim=[-1, 1])) + rx = rtb.ETS(tx(qlim=[-1, 1])) q = rx.qlim nt.assert_equal(q, np.array([[-1], [1]])) def test_qlim4(self): - rx = rtb.ETS(rtb.ET.tx()) + rx = rtb.ETS(tx()) with self.assertRaises(ValueError): rx.qlim def test_random_q(self): - rx = rtb.ETS(rtb.ET.Rx(qlim=[-1, 1])) + rx = rtb.ETS(Rx(qlim=[-1, 1])) q = rx.random_q() self.assertTrue(-1 <= q <= 1) def test_random_q2(self): - rx = rtb.ETS([rtb.ET.Rx(qlim=[-1, 1]), rtb.ET.Rx(qlim=[1, 2])]) + rx = rtb.ETS([Rx(qlim=[-1, 1]), Rx(qlim=[1, 2])]) q = rx.random_q(10) diff --git a/tests/test_ETS2.py b/tests/test_ETS2.py index 102a3bd99..562b4e46b 100644 --- a/tests/test_ETS2.py +++ b/tests/test_ETS2.py @@ -7,6 +7,11 @@ import numpy.testing as nt import roboticstoolbox as rtb + +# R/tx/ty free functions for terser ET2 construction; ET2 also exports SE2 +# (a constant-transform ET2 constructor) via __all__, so this must precede +# the spatialmath SE2 import below to let that one win +from roboticstoolbox.ets.ET2 import * import numpy as np # import spatialmath.base as sm @@ -17,8 +22,8 @@ class TestETS2(unittest.TestCase): def test_bad_arg(self): - rx = rtb.ET2.R(1.543) - ry = rtb.ET2.R(1.543) + rx = R(1.543) + ry = R(1.543) with self.assertRaises(TypeError): rtb.ETS2([rx, ry, 1.0]) # type: ignore @@ -31,38 +36,38 @@ def test_args(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET2.tx(0.333) * rtb.ET2.R(jindex=0) + l0 = tx(0.333) * R(jindex=0) - l1 = rtb.ET2.R(-90 * deg) * rtb.ET2.R(jindex=1) + l1 = R(-90 * deg) * R(jindex=1) - l2 = rtb.ET2.R(90 * deg) * rtb.ET2.tx(0.316) * rtb.ET2.R(jindex=2) + l2 = R(90 * deg) * tx(0.316) * R(jindex=2) - l3 = rtb.ET2.tx(0.0825) * rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=3) + l3 = tx(0.0825) * R(90 * deg) * R(jindex=3) l4 = ( - rtb.ET2.tx(-0.0825) - * rtb.ET2.R(-90 * deg) - * rtb.ET2.tx(0.384) - * rtb.ET2.R(jindex=4) + tx(-0.0825) + * R(-90 * deg) + * tx(0.384) + * R(jindex=4) ) - l5 = rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=5) + l5 = R(90 * deg) * R(jindex=5) r1 = l0 + l1 + l2 + l3 + l3 + l4 + l5 r2 = l0 * l1 * l2 * l3 * l3 * l4 * l5 r3 = rtb.ETS2(l0 + l1 + l2 + l3 + l3 + l4 + l5) r4 = rtb.ETS2([l0, l1, l2, l3, l3, l4, l5]) r5 = rtb.ETS2( - [l0, l1, l2, l3, l3, l4, rtb.ET2.R(90 * deg), rtb.ET2.R(jindex=5)] + [l0, l1, l2, l3, l3, l4, R(90 * deg), R(jindex=5)] ) r6 = rtb.ETS2([r1]) - r7 = rtb.ETS2(rtb.ET2.R(1.0)) + r7 = rtb.ETS2(R(1.0)) self.assertEqual(r1, r2) self.assertEqual(r1, r3) self.assertEqual(r1, r4) self.assertEqual(r1, r5) - self.assertEqual(r7[0], rtb.ET2.R(1.0)) + self.assertEqual(r7[0], R(1.0)) self.assertEqual(r1 + rtb.ETS2(), r2) def test_empty(self): @@ -71,13 +76,13 @@ def test_empty(self): self.assertEqual(r.m, 0) def test_str(self): - rx = rtb.ET2.R(1.543) - ry = rtb.ET2.R(1.543) - rz = rtb.ET2.R(1.543) - a = rtb.ET2.R() - b = rtb.ET2.R() - c = rtb.ET2.R() - d = rtb.ET2.tx(1.0) + rx = R(1.543) + ry = R(1.543) + rz = R(1.543) + a = R() + b = R() + c = R() + d = tx(1.0) e = rtb.ET2.SE2(SE2(1.0, unit="rad")) ets = rx * ry * rz * d ets2 = rx * a * b * c @@ -89,39 +94,39 @@ def test_str(self): self.assertEqual(ets2.__str__(q="θ{0}"), "R(88.41°) ⊕ R(θ0) ⊕ R(θ1) ⊕ R(θ2)") def test_str_jindex(self): - rx = rtb.ET2.R(1.543) - a = rtb.ET2.R(jindex=2) - b = rtb.ET2.R(jindex=5) - c = rtb.ET2.R(jindex=7) + rx = R(1.543) + a = R(jindex=2) + b = R(jindex=5) + c = R(jindex=7) ets = rx * a * b * c self.assertEqual(str(ets), "R(88.41°) ⊕ R(q2) ⊕ R(q5) ⊕ R(q7)") def test_str_flip(self): - rx = rtb.ET2.R(1.543) - a = rtb.ET2.R(jindex=2, flip=True) - b = rtb.ET2.R(jindex=5) - c = rtb.ET2.R(jindex=7) + rx = R(1.543) + a = R(jindex=2, flip=True) + b = R(jindex=5) + c = R(jindex=7) ets = rx * a * b * c self.assertEqual(str(ets), "R(88.41°) ⊕ R(-q2) ⊕ R(q5) ⊕ R(q7)") def test_str_sym(self): x = sympy.Symbol("x") - rx = rtb.ET2.R(x) - a = rtb.ET2.R(jindex=2) - b = rtb.ET2.R(jindex=5) - c = rtb.ET2.R(jindex=7) + rx = R(x) + a = R(jindex=2) + b = R(jindex=5) + c = R(jindex=7) ets = rx * a * b * c self.assertEqual(str(ets), "R(x) ⊕ R(q2) ⊕ R(q5) ⊕ R(q7)") def ets_mul(self): - rx = rtb.ET2.R(1.543) - ry = rtb.ET2.R(1.543) - rz = rtb.ET2.R(1.543) - a = rtb.ET2.R() - b = rtb.ET2.R() + rx = R(1.543) + ry = R(1.543) + rz = R(1.543) + a = R() + b = R() ets1 = rx * ry ets2 = a * b @@ -135,11 +140,11 @@ def ets_mul(self): self.assertIsInstance(ets2, rtb.ETS) def test_n(self): - rx = rtb.ET2.R(1.543) - ry = rtb.ET2.R(1.543) - rz = rtb.ET2.R(1.543) - a = rtb.ET2.R() - b = rtb.ET2.R() + rx = R(1.543) + ry = R(1.543) + rz = R(1.543) + a = R() + b = R() ets1 = rx * ry ets2 = a * b @@ -153,20 +158,20 @@ def test_n(self): def test_fkine(self): q = np.array([0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]) - rx = rtb.ET2.R(1.543) - ry = rtb.ET2.R(1.543) - rz = rtb.ET2.R(1.543) - tx = rtb.ET2.tx(1.543) - ty = rtb.ET2.ty(1.543) - tx = rtb.ET2.tx(1.543) - a = rtb.ET2.R(jindex=0) - b = rtb.ET2.R(jindex=1) - c = rtb.ET2.R(jindex=2) - d = rtb.ET2.tx(jindex=3) - e = rtb.ET2.ty(jindex=4) - f = rtb.ET2.tx(jindex=5) - - r = rx * ry * rz * tx * ty * tx * a * b * c * d * e * f + rx = R(1.543) + ry = R(1.543) + rz = R(1.543) + t_x = tx(1.543) + t_y = ty(1.543) + t_x = tx(1.543) + a = R(jindex=0) + b = R(jindex=1) + c = R(jindex=2) + d = tx(jindex=3) + e = ty(jindex=4) + f = tx(jindex=5) + + r = rx * ry * rz * t_x * t_y * t_x * a * b * c * d * e * f ans = ( SE2(1.543) @@ -192,9 +197,9 @@ def test_fkine2(self): q = np.array([y, z]) qt = np.array([[1.0, y], [z, y], [x, 2.0]]) - a = rtb.ET2.R(x) - b = rtb.ET2.R(jindex=0) - c = rtb.ET2.tx(jindex=1) + a = R(x) + b = R(jindex=0) + c = tx(jindex=1) r = a * b * c @@ -214,7 +219,7 @@ def test_fkine2(self): tool = SE2.Tx(0.5) ans5 = base * ans1 - r2 = rtb.ETS2([rtb.ET2.R(jindex=0)]) + r2 = rtb.ETS2([R(jindex=0)]) nt.assert_almost_equal(r.fkine(q, base=base), ans5.A) # nt.assert_almost_equal(r.fkine(q, base=base.A), ans5.A) # type: ignore @@ -226,10 +231,10 @@ def test_fkine2(self): def test_pop(self): q = [1.0, 2.0, 3.0] - a = rtb.ET2.R(jindex=0) - b = rtb.ET2.R(jindex=1) - c = rtb.ET2.R(jindex=2) - d = rtb.ET2.tx(1.543) + a = R(jindex=0) + b = R(jindex=1) + c = R(jindex=2) + d = tx(1.543) ans1 = SE2(q[0]) * SE2(q[1]) * SE2(q[2]) * SE2.Tx(1.543) ans2 = SE2(q[0]) * SE2(q[2]) * SE2.Tx(1.543) @@ -249,10 +254,10 @@ def test_pop(self): def test_inv(self): q = [1.0, 2.0, 3.0] - a = rtb.ET2.R(jindex=0) - b = rtb.ET2.R(jindex=1) - c = rtb.ET2.R(jindex=2) - d = rtb.ET2.tx(1.543) + a = R(jindex=0) + b = R(jindex=1) + c = R(jindex=2) + d = tx(1.543) ans1 = SE2(q[0]) * SE2(q[1]) * SE2(q[2]) * SE2.Tx(1.543) @@ -263,10 +268,10 @@ def test_inv(self): def test_jointset(self): q = [1.0, 2.0, 3.0] - a = rtb.ET2.R(jindex=0) - b = rtb.ET2.R(jindex=1) - c = rtb.ET2.R(jindex=2) - d = rtb.ET2.tx(1.543) + a = R(jindex=0) + b = R(jindex=1) + c = R(jindex=2) + d = tx(1.543) ans = set((0, 1, 2)) @@ -278,50 +283,57 @@ def test_split(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET2.tx(0.333) * rtb.ET2.R(jindex=0) + l0 = tx(0.333) * R(jindex=0) - l1 = rtb.ET2.R(-90 * deg) * rtb.ET2.R(jindex=1) + l1 = R(-90 * deg) * R(jindex=1) - l2 = rtb.ET2.R(90 * deg) * rtb.ET2.tx(0.316) * rtb.ET2.R(jindex=2) + l2 = R(90 * deg) * tx(0.316) * R(jindex=2) - l3 = rtb.ET2.tx(0.0825) * rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=3) + l3 = tx(0.0825) * R(90 * deg) * R(jindex=3) l4 = ( - rtb.ET2.tx(-0.0825) - * rtb.ET2.R(-90 * deg) - * rtb.ET2.tx(0.384) - * rtb.ET2.R(jindex=4) + tx(-0.0825) + * R(-90 * deg) + * tx(0.384) + * R(jindex=4) ) - l5 = rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=5) + l5 = R(90 * deg) * R(jindex=5) l6 = ( - rtb.ET2.tx(0.088) - * rtb.ET2.R(90 * deg) - * rtb.ET2.tx(0.107) - * rtb.ET2.R(jindex=6) + tx(0.088) + * R(90 * deg) + * tx(0.107) + * R(jindex=6) ) - ee = rtb.ET2.tx(tool_offset) * rtb.ET2.R(-np.pi / 4) + ee = tx(tool_offset) * R(-np.pi / 4) - segs = [l0, l1, l2, l3, l4, l5, l6, ee] - segs3 = [l0, l1, l2, l3, l4, l5, l6, rtb.ETS2([rtb.ET2.R(0.5)])] + segs = [l0, l1, l2, l3, l4, l5, l6] r = l0 * l1 * l2 * l3 * l4 * l5 * l6 * ee r2 = l0 * l1 * l2 * l3 * l4 * l5 * l6 - r3 = l0 * l1 * l2 * l3 * l4 * l5 * l6 * rtb.ET2.R(0.5) - - split = r.split() - split2 = r2.split() - split3 = r3.split() + r3 = l0 * l1 * l2 * l3 * l4 * l5 * l6 * R(0.5) + # split() returns [base, *segments, gripper]; base is always empty + # for the default "last" method (leading content is folded into the + # first segment) + base, *split, gripper = r.split() + self.assertEqual(len(base), 0) for i, link in enumerate(segs): self.assertEqual(link, split[i]) + self.assertEqual(gripper, ee) - for i, link in enumerate(split2): - self.assertEqual(link, segs[i]) + base2, *split2, gripper2 = r2.split() + self.assertEqual(len(base2), 0) + for i, link in enumerate(segs): + self.assertEqual(link, split2[i]) + self.assertEqual(len(gripper2), 0) - for i, link in enumerate(split3): - self.assertEqual(link, segs3[i]) + base3, *split3, gripper3 = r3.split() + self.assertEqual(len(base3), 0) + for i, link in enumerate(segs): + self.assertEqual(link, split3[i]) + self.assertEqual(gripper3, rtb.ETS2([R(0.5)])) def test_compile(self): q = [0, 1.0, 2, 3, 4, 5, 6] @@ -329,31 +341,31 @@ def test_compile(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET2.tx(0.333) * rtb.ET2.R(jindex=0) + l0 = tx(0.333) * R(jindex=0) - l1 = rtb.ET2.R(-90 * deg) * rtb.ET2.R(jindex=1) + l1 = R(-90 * deg) * R(jindex=1) - l2 = rtb.ET2.R(90 * deg) * rtb.ET2.tx(0.316) * rtb.ET2.R(jindex=2) + l2 = R(90 * deg) * tx(0.316) * R(jindex=2) - l3 = rtb.ET2.tx(0.0825) * rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=3) + l3 = tx(0.0825) * R(90 * deg) * R(jindex=3) l4 = ( - rtb.ET2.tx(-0.0825) - * rtb.ET2.R(-90 * deg) - * rtb.ET2.tx(0.384) - * rtb.ET2.R(jindex=4) + tx(-0.0825) + * R(-90 * deg) + * tx(0.384) + * R(jindex=4) ) - l5 = rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=5) + l5 = R(90 * deg) * R(jindex=5) l6 = ( - rtb.ET2.tx(0.088) - * rtb.ET2.R(90 * deg) - * rtb.ET2.tx(0.107) - * rtb.ET2.R(jindex=6) + tx(0.088) + * R(90 * deg) + * tx(0.107) + * R(jindex=6) ) - ee = rtb.ET2.tx(tool_offset) * rtb.ET2.R(-np.pi / 4) + ee = tx(tool_offset) * R(-np.pi / 4) r = l0 * l1 * l2 * l3 * l4 * l5 * l6 * ee r2 = r.compile() @@ -367,31 +379,31 @@ def test_insert(self): mm = 1e-3 tool_offset = (103) * mm - l0 = rtb.ET2.tx(0.333) * rtb.ET2.R(jindex=0) + l0 = tx(0.333) * R(jindex=0) - l1 = rtb.ET2.R(-90 * deg) * rtb.ET2.R(jindex=1) + l1 = R(-90 * deg) * R(jindex=1) - l2 = rtb.ET2.R(90 * deg) * rtb.ET2.tx(0.316) * rtb.ET2.R(jindex=2) + l2 = R(90 * deg) * tx(0.316) * R(jindex=2) - l3 = rtb.ET2.tx(0.0825) * rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=3) + l3 = tx(0.0825) * R(90 * deg) * R(jindex=3) l4 = ( - rtb.ET2.tx(-0.0825) - * rtb.ET2.R(-90 * deg) - * rtb.ET2.tx(0.384) - * rtb.ET2.R(jindex=4) + tx(-0.0825) + * R(-90 * deg) + * tx(0.384) + * R(jindex=4) ) - l5 = rtb.ET2.R(90 * deg) * rtb.ET2.R(jindex=5) + l5 = R(90 * deg) * R(jindex=5) l6 = ( - rtb.ET2.tx(0.088) - * rtb.ET2.R(90 * deg) - * rtb.ET2.tx(0.107) - * rtb.ET2.R(jindex=6) + tx(0.088) + * R(90 * deg) + * tx(0.107) + * R(jindex=6) ) - ee = rtb.ET2.tx(tool_offset) * rtb.ET2.R(-np.pi / 4) + ee = tx(tool_offset) * R(-np.pi / 4) r = l0 * l1 * l2 * l3 * l4 * l5 * l6 * ee @@ -402,12 +414,12 @@ def test_insert(self): r3.append(ee) r4 = l0 * l1 * l2 * l3 * l4 * l5 * l6 - r4.append(rtb.ET2.tx(tool_offset)) - r4.append(rtb.ET2.R(-np.pi / 4)) + r4.append(tx(tool_offset)) + r4.append(R(-np.pi / 4)) r5 = l0 * l1 * l2 * l3 * l4 * l6 * ee - r5.insert(14, rtb.ET2.R(90 * deg)) - r5.insert(15, rtb.ET2.R(jindex=5)) + r5.insert(14, R(90 * deg)) + r5.insert(15, R(jindex=5)) nt.assert_almost_equal(r.fkine(q).A, r2.fkine(q).A) nt.assert_almost_equal(r.fkine(q).A, r3.fkine(q).A) @@ -416,47 +428,47 @@ def test_insert(self): def test_jacob0(self): q = [0, 0, 0] - r = rtb.ETS2(rtb.ET2.R(jindex=0)) - tx = rtb.ETS2(rtb.ET2.tx(jindex=1)) - ty = rtb.ETS2(rtb.ET2.ty(jindex=2)) + r = rtb.ETS2(R(jindex=0)) + t_x = rtb.ETS2(tx(jindex=1)) + t_y = rtb.ETS2(ty(jindex=2)) - r2 = tx + ty + r + r2 = t_x + t_y + r - nt.assert_almost_equal(tx.jacob0(q), np.array([[1, 0, 0]]).T) - nt.assert_almost_equal(ty.jacob0(q), np.array([[0, 1, 0]]).T) + nt.assert_almost_equal(t_x.jacob0(q), np.array([[1, 0, 0]]).T) + nt.assert_almost_equal(t_y.jacob0(q), np.array([[0, 1, 0]]).T) nt.assert_almost_equal(r.jacob0(q), np.array([[0, 0, 1]]).T) nt.assert_almost_equal(r2.jacob0(q), np.eye(3)) def test_jacobe(self): q = [0, 0, 0] - r = rtb.ETS2(rtb.ET2.R(jindex=0)) - tx = rtb.ETS2(rtb.ET2.tx(jindex=1)) - ty = rtb.ETS2(rtb.ET2.ty(jindex=2)) + r = rtb.ETS2(R(jindex=0)) + t_x = rtb.ETS2(tx(jindex=1)) + t_y = rtb.ETS2(ty(jindex=2)) - r2 = tx + ty + r + r2 = t_x + t_y + r - nt.assert_almost_equal(tx.jacobe(q), np.array([[1, 0, 0]]).T) - nt.assert_almost_equal(ty.jacobe(q), np.array([[0, 1, 0]]).T) + nt.assert_almost_equal(t_x.jacobe(q), np.array([[1, 0, 0]]).T) + nt.assert_almost_equal(t_y.jacobe(q), np.array([[0, 1, 0]]).T) nt.assert_almost_equal(r.jacobe(q), np.array([[0, 0, 1]]).T) nt.assert_almost_equal(r2.jacobe(q), np.eye(3)) def test_plot(self): q2 = np.array([0, 1, 2]) - rz = rtb.ETS2(rtb.ET2.R(jindex=0)) - tx = rtb.ETS2(rtb.ET2.tx(jindex=1, qlim=[-1, 1])) - ty = rtb.ETS2(rtb.ET2.ty(jindex=2, qlim=[-1, 1])) + rz = rtb.ETS2(R(jindex=0)) + t_x = rtb.ETS2(tx(jindex=1, qlim=[-1, 1])) + t_y = rtb.ETS2(ty(jindex=2, qlim=[-1, 1])) a = rtb.ETS2(rtb.ET2.SE2(np.eye(3))) - r = tx + ty + rz + a + r = t_x + t_y + rz + a r.plot(q=q2, block=False) def test_teach(self): x = sympy.Symbol("x") q2 = np.array([0, 1, 2]) - rz = rtb.ETS2(rtb.ET2.R(jindex=0)) - tx = rtb.ETS2(rtb.ET2.tx(jindex=1, qlim=[-1, 1])) - ty = rtb.ETS2(rtb.ET2.ty(jindex=2, qlim=[-1, 1])) + rz = rtb.ETS2(R(jindex=0)) + t_x = rtb.ETS2(tx(jindex=1, qlim=[-1, 1])) + t_y = rtb.ETS2(ty(jindex=2, qlim=[-1, 1])) a = rtb.ETS2(rtb.ET2.SE2(np.eye(3))) - r = tx + ty + rz + a + r = t_x + t_y + rz + a r.teach(q=q2, block=False) diff --git a/tests/test_Robot.py b/tests/test_Robot.py index 9dad476cb..8cbce8a2d 100644 --- a/tests/test_Robot.py +++ b/tests/test_Robot.py @@ -652,8 +652,8 @@ def test_dotfile(self): def test_dotfile2(self): # branched robot built dynamically — two end-effectors sharing a common base from roboticstoolbox.robot.Link import Link - from roboticstoolbox.robot.ET import ET - from roboticstoolbox.robot.ETS import ETS + from roboticstoolbox.ets.ET import ET + from roboticstoolbox.ets.ETS import ETS L0 = Link(name="base") L1 = Link(ETS(ET.Rz()), name="joint1", parent=L0) diff --git a/tests/test_blocks.py b/tests/test_blocks.py index 1481f1c34..a3a01292d 100644 --- a/tests/test_blocks.py +++ b/tests/test_blocks.py @@ -43,7 +43,10 @@ def test_ikine(self): T = robot.fkine(q) sol = robot.ikine_LM(T) - block = IKine(robot, seed=0) + # `tol` bounds the quadratic angle-axis residual E, not the linear + # pose error directly - the default tol=1e-6 only guarantees pose + # error on the order of 1e-3, not tight enough for the check below. + block = IKine(robot, seed=0, args={"tol": 1e-10}) q_ik = block.test_output(T)[0] # get IK from block pass diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index 8a07f5e52..7a5ab6f91 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -24,9 +24,9 @@ import sympy import roboticstoolbox as rtb -# roboticstoolbox/robot/ETS.py defines a class also called ETS, and -# roboticstoolbox/robot/__init__.py does `from ...ETS import ETS`, which -# rebinds the "ETS" attribute on the roboticstoolbox.robot package to the +# roboticstoolbox/ets/ETS.py defines a class also called ETS, and +# roboticstoolbox/ets/__init__.py does `from ...ETS import ETS`, which +# rebinds the "ETS" attribute on the roboticstoolbox.ets package to the # class, shadowing the submodule of the same name. `import a.b.c as x` is # defined as `import a.b.c; x = a.b.c` — that second step is still # attribute access, so it hits the same shadowing and also gives the @@ -41,7 +41,7 @@ # and fails with AttributeError; 3.11+ uses pkgutil.resolve_name and # isn't fooled. patch.object() against the real module (via sys.modules) # works on every version. -_ETS_module = sys.modules["roboticstoolbox.robot.ETS"] +_ETS_module = sys.modules["roboticstoolbox.ets.ETS"] from spatialmath import SE3 @@ -71,7 +71,7 @@ def _puma(): @contextmanager def _no_c_fkine(): """Force ETS.eval() onto the Python path via the facade's Python implementation.""" - from roboticstoolbox.robot.fknm import _python_fkine + from roboticstoolbox.ets.fknm import _python_fkine def _py(fknm, q, base, tool, include_base, _data=None): return _python_fkine(_data, q, base, tool, include_base) @@ -83,7 +83,7 @@ def _py(fknm, q, base, tool, include_base, _data=None): @contextmanager def _no_c_jacob0(): """Force ETS.jacob0() onto the Python path via the facade's Python implementation.""" - from roboticstoolbox.robot.fknm import _python_jacob0 + from roboticstoolbox.ets.fknm import _python_jacob0 def _py(fknm, q, tool, _data=None, _n=None): return _python_jacob0(_data, _n, q, tool) @@ -95,7 +95,7 @@ def _py(fknm, q, tool, _data=None, _n=None): @contextmanager def _no_c_jacobe(): """Force ETS.jacobe() onto the Python path via the facade's Python implementation.""" - from roboticstoolbox.robot.fknm import _python_jacobe + from roboticstoolbox.ets.fknm import _python_jacobe def _py(fknm, q, tool, _data=None, _n=None): return _python_jacobe(_data, _n, q, tool) @@ -107,7 +107,7 @@ def _py(fknm, q, tool, _data=None, _n=None): @contextmanager def _no_c_hessian0(): """Force ETS.hessian0() onto the Python path via the facade's Python implementation.""" - from roboticstoolbox.robot.fknm import _python_jacob0, _python_hessian + from roboticstoolbox.ets.fknm import _python_jacob0, _python_hessian from spatialmath.base import getvector, verifymatrix def _py(fknm, q, J0, tool, _data=None, _n=None): @@ -127,7 +127,7 @@ def _py(fknm, q, J0, tool, _data=None, _n=None): @contextmanager def _no_c_hessiane(): """Force ETS.hessiane() onto the Python path via the facade's Python implementation.""" - from roboticstoolbox.robot.fknm import _python_jacobe, _python_hessian + from roboticstoolbox.ets.fknm import _python_jacobe, _python_hessian from spatialmath.base import getvector, verifymatrix def _py(fknm, q, Je, tool, _data=None, _n=None): @@ -549,21 +549,21 @@ def _assert_c_faster(self, t_c, t_py, factor=5): f" Python path: {t_py * 1000 / self.N:.3f} ms/call", ) - @unittest.skipIf(not __import__("roboticstoolbox.robot.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, + @unittest.skipIf(not __import__("roboticstoolbox.ets.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, "C extension not built") def test_eval_c_faster_than_python(self): t_c = self._time_c("eval", self.q) t_py = self._time_python(_no_c_fkine, "eval", self.q) self._assert_c_faster(t_c, t_py) - @unittest.skipIf(not __import__("roboticstoolbox.robot.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, + @unittest.skipIf(not __import__("roboticstoolbox.ets.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, "C extension not built") def test_jacob0_c_faster_than_python(self): t_c = self._time_c("jacob0", self.q) t_py = self._time_python(_no_c_jacob0, "jacob0", self.q) self._assert_c_faster(t_c, t_py) - @unittest.skipIf(not __import__("roboticstoolbox.robot.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, + @unittest.skipIf(not __import__("roboticstoolbox.ets.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, "C extension not built") def test_jacobe_c_faster_than_python(self): t_c = self._time_c("jacobe", self.q)