From 3b3f754b5c7ca9545e81b0e4a81d436d344c7d8d Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 14 Jul 2026 21:56:35 +1000 Subject: [PATCH 01/10] refactor(et): split compiled acceleration out of BaseET into ET; fix merge()/split() bugs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BaseET/ET/ET2 previously built a compiled C++ struct for every ET, including ET2 (pure Python, 2D) — calling the fast path directly on an ET2's handle silently read out-of-bounds memory since it assumes a 4x4 SE(3) buffer, not ET2's 3x3 SE(2). ET2 now has no compiled handle at all (_accel_init/_accel_update are no-op hooks on BaseET, real on ET), making that class of bug structurally impossible rather than merely unexercised. Along the way: BaseET.__init__ now sets axis_func/flip/jindex/qlim before eta (the eta setter needs axis_func to recompute _T, and reassigning eta post-construction — as ETS.merge() does — silently left the transform matrix stale otherwise, since eta's setter never recomputed _T or synced the C struct). ETS.merge() also had `e1.type`/`e2.type`, referencing an attribute that doesn't exist on ET at all. ETS.split() had also picked up an ETS2-flavoured `split(self, index)` override on the 3D ETS class that shadowed BaseETS's real no-arg segment-splitting split(), breaking Robot() construction from an ETS string. Restored the correct split() on both ETS and ETS2. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/ET.py | 324 +++++++++++++++++-------------- src/roboticstoolbox/robot/ETS.py | 147 +++++++++++--- tests/test_ET.py | 43 ++++ tests/test_ETS.py | 74 +++++++ 4 files changed, 417 insertions(+), 171 deletions(-) diff --git a/src/roboticstoolbox/robot/ET.py b/src/roboticstoolbox/robot/ET.py index 363c7442e..7d41fc490 100644 --- a/src/roboticstoolbox/robot/ET.py +++ b/src/roboticstoolbox/robot/ET.py @@ -54,15 +54,11 @@ def __init__( # 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 - + # axis_func/flip/jindex/qlim must all be set before `eta` below: + # the `eta` setter (for the static, eta-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 @@ -72,7 +68,8 @@ def __init__( else: self._qlim: NDArray | None = None - if self.eta is None: + if eta is None: + self._eta = None if T is None: self._joint = True self._T = eye(4).copy(order="F") @@ -82,77 +79,13 @@ def __init__( 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, - ) + if axis[0] == "R" and unit.lower().startswith("deg"): + if not issymbol(eta): + eta = deg2rad(float(eta)) + # This is a static joint. The `eta` setter validates axis_func, + # computes `_T`, sets `_isstaticsym`/`_joint`, and (for ET) + # syncs the compiled acceleration struct. + self.eta = eta def __str__(self): eta_str = "" @@ -228,33 +161,39 @@ def _repr_pretty_(self, p, cycle): """ 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 != "_BaseET__fknm": + if k not in self._deepcopy_skip: setattr(result, k, deepcopy(v, memo)) - result.__fknm = result.__init_c() + result._accel_init() 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) + # ------------------------------------------------------------------ + # 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 eta/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 - @property - def fknm(self): - return self.__fknm + def _accel_update(self) -> None: + pass @property def eta(self) -> float | Sym | None: @@ -295,8 +234,23 @@ def eta(self, value: float | Sym) -> None: - No unit conversions are applied, it is assumed to be in radians. + - Setting `eta` 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 `eta` and `axis_func` " + "must be specified otherwise `T` must be supplied" + ) + self._eta = value if issymbol(value) else float(value) + self._isstaticsym = issymbol(value) + self._joint = False + self._T = self.axis_func(self._eta).copy(order="F") + + self._accel_update() @property def axis_func( @@ -429,7 +383,7 @@ 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() + self._accel_update() @property def jindex(self) -> int | None: @@ -462,7 +416,7 @@ 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() + self._accel_update() @property def iselementary(self) -> bool: @@ -516,7 +470,7 @@ def inv(self): inv._T = npinv(inv._T).copy(order="F") inv._eta = -inv._eta - inv.__update_c() + inv._accel_update() return inv @@ -539,27 +493,37 @@ def A(self, q: float | Sym = 0.0) -> ndarray: >>> 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. """ - 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") + 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 - return self._T + raise TypeError("axis_func not defined") + else: # pragma: no cover + return self._T 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 `eta` 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/eta/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]) @@ -567,6 +531,111 @@ def __mul__(self, other: "ET") -> "rtb.ETS": 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.axis[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.axis), + self._T, + qlim, + ) + + def _accel_update(self) -> None: + """ + Push current Python-side state to the compiled struct. Called + whenever eta/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.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, + ) + + @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.axis[1] == "x": @@ -938,34 +1007,5 @@ def SE2(cls, T: ndarray | SE2, **kwargs) -> "ET2": 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 + # A() is inherited from BaseET: ET2 has no compiled acceleration, so + # the shared pure-Python evaluation is all it ever needed. diff --git a/src/roboticstoolbox/robot/ETS.py b/src/roboticstoolbox/robot/ETS.py index 8dd4b0a25..94925fffe 100644 --- a/src/roboticstoolbox/robot/ETS.py +++ b/src/roboticstoolbox/robot/ETS.py @@ -483,35 +483,6 @@ 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 @@ -650,6 +621,78 @@ def random_q(self, i: int = 1) -> NDArray: 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.axis == e2.axis: + 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.axis != e2.axis: + 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].eta = e1.eta + e2.eta + del self._data[i + 1] + self._fknm_stale = True class ETS(BaseETS): """ @@ -2466,6 +2509,29 @@ def ikine_QP( return solver.solve(ets=self, Tep=Tep, q0=q0) + def split(self) -> list["ETS"]: + """ + 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 + class ETS2(BaseETS): """ @@ -2650,6 +2716,29 @@ def insert( # type: ignore[override] self._data.insert(i + j, et) self._fknm_stale = True + def split(self) -> list["ETS2"]: + """ + Split ETS2 into link segments + + :returns: a list of ETS2, 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 fkine( self, q: ArrayLike, diff --git a/tests/test_ET.py b/tests/test_ET.py index e2aa2677f..8955c6c92 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -339,6 +339,49 @@ def test_jindex_error(self): with self.assertRaises(ValueError): r1.jindex = -2 + def test_eta_setter_updates_transform(self): + # ETS.merge() reassigns `.eta` 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.eta = 3.0 + self.assertEqual(r1.eta, 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")) + + # eta/qlim/jindex updates on ET2 must not attempt to touch a + # compiled struct that doesn't exist + e.eta = 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() diff --git a/tests/test_ETS.py b/tests/test_ETS.py index d6f3d11a7..8d9230bb7 100644 --- a/tests/test_ETS.py +++ b/tests/test_ETS.py @@ -587,6 +587,80 @@ def test_insert(self): 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 = rtb.ET.Rz(jindex=0) * rtb.ET.Rz(1) * rtb.ET.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].eta, 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 = rtb.ET.tx(1) * rtb.ET.Rz(jindex=0) * rtb.ET.Rz(1) + + with self.assertRaises(ValueError): + e.swap(0) + + def test_swap_bad_index(self): + e = rtb.ET.tx(1) * rtb.ET.tx(2) + + with self.assertRaises(IndexError): + e.swap(-1) + + with self.assertRaises(IndexError): + e.swap(1) + + def test_merge(self): + q = [1.0] + e = rtb.ET.Rz(jindex=0) * rtb.ET.tx(1) * rtb.ET.tx(2) * rtb.ET.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].axis, "tx") + self.assertAlmostEqual(e[1].eta, 3.0) # type: ignore + nt.assert_almost_equal(e.fkine(q).A, ans.A) + + def test_merge_not_same_type(self): + e = rtb.ET.tx(1) * rtb.ET.Rz(1) + + with self.assertRaises(ValueError): + e.merge(0) + + def test_merge_both_joints(self): + e = rtb.ET.Rz(jindex=0) * rtb.ET.Rz(jindex=1) + + with self.assertRaises(ValueError): + e.merge(0) + + def test_merge_bad_index(self): + e = rtb.ET.tx(1) * rtb.ET.tx(2) + + with self.assertRaises(IndexError): + e.merge(-1) + + with self.assertRaises(IndexError): + e.merge(1) + def test_jacob0(self): q = [0.0] rx = rtb.ETS(rtb.ET.Rx()) From bc3770a73b73eabe08c069770f575bcd72bb80fa Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 14 Jul 2026 21:57:28 +1000 Subject: [PATCH 02/10] fix(ik): pin seed and tighten tol in flaky ikine tests, document quadratic tol MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit IKSolver.tol bounds the quadratic weighted angle-axis residual E, not linear-scale pose error directly — with the default tol=1e-6, actual pose error is only guaranteed to be on the order of sqrt(2*tol) ≈ 1.4e-3, looser than the places=4 (~5e-5) checks in test_ikine_LM/test_ikine assumed. Confirmed concretely: a solve with residual=2.87e-8 (well under tol) still had a raw pose difference of 3.27e-4. Not a resurgence of the stale-residual bug fixed on bug/ik-gn-stale-residual — that fix is intact. test_DHRobot.py::test_ikine_LM additionally had no pinned seed, so it was genuinely non-deterministic between runs. Pass tol=1e-10 in both tests (comfortably bounds pose error under places=4's threshold) and pin seed=0 for reproducibility. Document the quadratic/linear relationship on IKSolver's `tol` docstring (and its four solver subclasses) so this isn't rediscovered from scratch next time. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/IK.py | 60 +++++++++++++++++++++++++++------ tech-debt.md | 33 ++++++++++++++++++ tests/test_DHRobot.py | 7 +++- tests/test_blocks.py | 5 ++- 4 files changed, 93 insertions(+), 12 deletions(-) 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/tech-debt.md b/tech-debt.md index 359a02350..d169c380f 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 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_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 From 8ffc493719ebf846873e5395fb365bc35f8bcf34 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 11:58:59 +1000 Subject: [PATCH 03/10] feat(et): rename axis -> kind, add ax for the x/y/z-only concept ET.axis bundled two concepts into one string ("Rx"/"tx"/"SE3"): the rotation/translation/special type, and which Cartesian axis. Split these: - `kind` is the new home for today's `axis` semantics (full string). - `axis` becomes a permanent deprecated alias for `kind` (warns since 1.4.0) - it is never repurposed to mean the x/y/z-only concept, so it can never silently mean two different things across versions. - `ax` is new: returns "x"/"y"/"z" for an elementary transform, None for ET2's rotation (its axis string is just "R", no letter) and for compound/arbitrary SE3/SE2 transforms. Migrated all internal `.axis` call sites (ET.py, ETS.py, fknm.py's pure-Python Jacobian fallback, RobotPlot.py) to `.kind`/`.ax` so the library itself never triggers its own deprecation warning. DHFactor.py/DHFactor2.py are excluded (WIP, being rewritten separately). Co-Authored-By: Claude Sonnet 5 --- .../backends/PyPlot/RobotPlot.py | 6 +- src/roboticstoolbox/robot/ET.py | 87 ++++++++++++++----- src/roboticstoolbox/robot/ETS.py | 18 ++-- src/roboticstoolbox/robot/fknm.py | 12 +-- tests/test_ET.py | 39 +++++++++ tests/test_ETS.py | 2 +- 6 files changed, 124 insertions(+), 40 deletions(-) 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/robot/ET.py b/src/roboticstoolbox/robot/ET.py index 7d41fc490..bc618d8ab 100644 --- a/src/roboticstoolbox/robot/ET.py +++ b/src/roboticstoolbox/robot/ET.py @@ -17,6 +17,7 @@ transl2, tr2xyt, ) +import warnings from copy import deepcopy from roboticstoolbox.robot.fknm import ET_T, ET_init, ET_update from spatialmath.base import getvector @@ -48,7 +49,7 @@ def __init__( flip: bool = False, qlim: ArrayLike | None = None, ): - self._axis = axis + self._kind = 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 @@ -124,7 +125,7 @@ def __str__(self): else: eta_str = f"{self.eta:.4g}" - return f"{self.axis}({eta_str})" + return f"{self.kind}({eta_str})" def __repr__(self): s_eta = "" if self.eta is None else f"eta={self.eta}" @@ -142,7 +143,7 @@ def __repr__(self): start = "ET" if isinstance(self, ET) else "ET2" - return f"{start}.{self.axis}({s_kwargs})" + return f"{start}.{self.kind}({s_kwargs})" def _repr_pretty_(self, p, cycle): """ @@ -258,13 +259,56 @@ def axis_func( ) -> 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 -------- @@ -273,12 +317,13 @@ def axis(self) -> str: >>> from roboticstoolbox import ET >>> e = ET.tx(1) - >>> e.axis + >>> e.ax >>> e = ET.Rx(90, 'deg') - >>> e.axis + >>> e.ax """ - return self._axis + letter = self._kind[-1] + return letter if letter in "xyz" else None @property def isjoint(self) -> bool: @@ -349,7 +394,7 @@ def isrotation(self) -> bool: """ - return self.axis[0] == "R" + return self.kind[0] == "R" @property def istranslation(self) -> bool: @@ -372,7 +417,7 @@ def istranslation(self) -> bool: """ - return self.axis[0] == "t" + return self.kind[0] == "t" @property def qlim(self) -> ndarray | None: @@ -437,7 +482,7 @@ def iselementary(self) -> bool: """ - return self.axis[0] != "S" + return self.kind[0] != "S" def inv(self): r""" @@ -553,7 +598,7 @@ def _accel_init(self) -> None: jindex = self.jindex if self.qlim is None: - if self.axis[0] == "R": + if self.kind[0] == "R": qlim = array([-pi, pi]) else: qlim = array([0, 1]) @@ -565,7 +610,7 @@ def _accel_init(self) -> None: self.isjoint, self.isflip, jindex, - self.__axis_to_number(self.axis), + self.__axis_to_number(self.kind), self._T, qlim, ) @@ -586,7 +631,7 @@ def _accel_update(self) -> None: jindex = self.jindex if self.qlim is None: - if self.axis[0] == "R": + if self.kind[0] == "R": qlim = array([-pi, pi]) else: qlim = array([0, 1]) @@ -599,7 +644,7 @@ def _accel_update(self) -> None: self.isjoint, self.isflip, jindex, - self.__axis_to_number(self.axis), + self.__axis_to_number(self.kind), self._T, qlim, ) @@ -638,18 +683,18 @@ def A(self, q: float | Sym = 0.0) -> ndarray: @property def s(self) -> ndarray: # pragma: nocover - if self.axis[1] == "x": - if self.axis[0] == "R": + 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.axis[1] == "y": - if self.axis[0] == "R": + 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.axis[0] == "R": + if self.kind[0] == "R": return array([0, 0, 0, 0, 0, 1]) else: return array([0, 0, 1, 0, 0, 0]) @@ -888,11 +933,11 @@ def __add__(self, other: "ET2") -> "rtb.ETS2": @property def s(self) -> ndarray: # pragma: nocover - if self.axis[0] == "R": + if self.kind[0] == "R": return array([0, 0, 0, 1]) - if self.axis[1] == "x": + if self.kind[1] == "x": return array([1, 0, 0, 0]) - elif self.axis[1] == "y": + elif self.kind[1] == "y": return array([0, 1, 0, 0]) else: return array([0, 0, 1, 0]) diff --git a/src/roboticstoolbox/robot/ETS.py b/src/roboticstoolbox/robot/ETS.py index 94925fffe..4efbec003 100644 --- a/src/roboticstoolbox/robot/ETS.py +++ b/src/roboticstoolbox/robot/ETS.py @@ -193,22 +193,22 @@ def __str__(self, q: str | None = None): # qvar = "" if et.isflip: - s = f"{et.axis}(-{qvar})" + s = f"{et.kind}(-{qvar})" else: - s = f"{et.axis}({qvar})" + s = f"{et.kind}({qvar})" j += 1 elif et.isrotation: if issymbol(et.eta): - s = f"{et.axis}({et.eta})" + s = f"{et.kind}({et.eta})" else: - s = f"{et.axis}({et.eta * 180 / np.pi:.4g}°)" + s = f"{et.kind}({et.eta * 180 / np.pi:.4g}°)" elif et.istranslation: try: - s = f"{et.axis}({et.eta:.4g})" + s = f"{et.kind}({et.eta:.4g})" except TypeError: # pragma: nocover - s = f"{et.axis}({et.eta})" + s = f"{et.kind}({et.eta})" elif not et.iselementary: s = str(et) @@ -647,7 +647,7 @@ def swap(self, i: int) -> None: e1 = self._data[i] e2 = self._data[i + 1] - if e1.axis == e2.axis: + if e1.kind == e2.kind: self._data[i], self._data[i + 1] = self._data[i + 1], self._data[i] self._fknm_stale = True else: @@ -683,7 +683,7 @@ def merge(self, i: int) -> None: raise IndexError("Index out of range") # pragma: nocover e1 = self._data[i] e2 = self._data[i + 1] - if e1.axis != e2.axis: + if e1.kind != e2.kind: raise ValueError("Transforms are not the same type") # pragma: nocover elif (e1.isjoint + e2.isjoint) == 2: @@ -2908,7 +2908,7 @@ def jacob0( # jindex = 0 if self[i].jindex is None else self[i].jindex - axis = self[i].axis + 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 diff --git a/src/roboticstoolbox/robot/fknm.py b/src/roboticstoolbox/robot/fknm.py index 610bf0c5e..1a212e8ce 100644 --- a/src/roboticstoolbox/robot/fknm.py +++ b/src/roboticstoolbox/robot/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/tests/test_ET.py b/tests/test_ET.py index 8955c6c92..4b82318ec 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -396,6 +396,45 @@ 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) + if __name__ == "__main__": unittest.main() diff --git a/tests/test_ETS.py b/tests/test_ETS.py index 8d9230bb7..c378327cc 100644 --- a/tests/test_ETS.py +++ b/tests/test_ETS.py @@ -636,7 +636,7 @@ def test_merge(self): e.merge(1) self.assertEqual(len(e), n - 1) - self.assertEqual(e[1].axis, "tx") + self.assertEqual(e[1].kind, "tx") self.assertAlmostEqual(e[1].eta, 3.0) # type: ignore nt.assert_almost_equal(e.fkine(q).A, ans.A) From 7f0df78c137be82418a08b2d8716954598fd7539 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 12:53:16 +1000 Subject: [PATCH 04/10] feat(et): rename eta -> param, keep eta as a deprecated alias MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `eta` (η) is the name used in the original Elementary Transform Sequence paper (Haviland & Corke) and means nothing to a new reader. `param` is its replacement: - `param` is the new home for the transform-constant property/setter, keeping all the centralized static-transform logic added earlier this session (axis_func validation, `_T` recompute, `_accel_update()` sync). - `eta` becomes a permanent deprecated property alias (getter/setter both warn since 1.4.0, forward to `param`). - The `eta` constructor keyword is similarly renamed to `param` on `BaseET.__init__` and every factory classmethod (Rx/Ry/Rz/tx/ty/tz/SE3 on ET; R/tx/ty/SE2 on ET2), with a keyword-only `eta=` fallback (via a shared `_resolve_param()` helper) so existing `eta=...` calls keep working, warning instead of breaking. Migrated internal call sites (ETS.py's merge()/__str__, Robot.py, PoERobot.py) to `.param`/`param=`. DHFactor.py excluded (WIP, out of scope, as agreed for the axis->kind rename too). Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/ET.py | 311 +++++++++++++++++--------- src/roboticstoolbox/robot/ETS.py | 12 +- src/roboticstoolbox/robot/PoERobot.py | 6 +- src/roboticstoolbox/robot/Robot.py | 4 +- tests/test_ET.py | 49 +++- tests/test_ETS.py | 4 +- 6 files changed, 264 insertions(+), 122 deletions(-) diff --git a/src/roboticstoolbox/robot/ET.py b/src/roboticstoolbox/robot/ET.py index bc618d8ab..b001c7d82 100644 --- a/src/roboticstoolbox/robot/ET.py +++ b/src/roboticstoolbox/robot/ET.py @@ -37,26 +37,51 @@ 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 + + class BaseET: def __init__( self, axis: str, - eta: float | Sym | None = None, + param: 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, + *, + eta: float | None = None, ): + param = _resolve_param(param, eta) + self._kind = 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 + # 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 `eta` below: - # the `eta` setter (for the static, eta-is-not-None case) reads + # 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. @@ -69,8 +94,8 @@ def __init__( else: self._qlim: NDArray | None = None - if eta is None: - self._eta = None + if param is None: + self._param = None if T is None: self._joint = True self._T = eye(4).copy(order="F") @@ -81,64 +106,64 @@ def __init__( self._T = T.copy(order="F") else: if axis[0] == "R" and unit.lower().startswith("deg"): - if not issymbol(eta): - eta = deg2rad(float(eta)) - # This is a static joint. The `eta` setter validates axis_func, + 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.eta = eta + self.param = param def __str__(self): - eta_str = "" + param_str = "" if self.isjoint: if self.jindex is None: - eta_str = "q" + param_str = "q" else: - eta_str = f"q{self.jindex}" - elif issymbol(self.eta): + param_str = f"q{self.jindex}" + elif issymbol(self.param): # 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}°" + 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: if isinstance(self, ET): T = self.A() rpy = tr2rpy(T) * 180.0 / pi if T[:3, -1].any() and rpy.any(): - eta_str = ( + 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(): - eta_str = f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g}" + param_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}°" + param_str = f"{rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" else: - eta_str = "" # pragma: nocover + param_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}°" + param_str = f"{xyt[0]:.4g}, {xyt[1]:.4g}; {xyt[2]:.4g}°" else: - eta_str = f"{self.eta:.4g}" + param_str = f"{self.param:.4g}" - return f"{self.kind}({eta_str})" + return f"{self.kind}({param_str})" def __repr__(self): - s_eta = "" if self.eta is None else f"eta={self.eta}" + s_param = "" if self.param is None else f"param={self.param}" s_T = ( f"T={repr(self._T)}" - if (self.eta is None and self.axis_func is None) + 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_eta, s_T, s_jindex, s_flip, s_qlim] + kwargs = [s_param, s_T, s_jindex, s_flip, s_qlim] s_kwargs = ", ".join(filter(None, kwargs)) start = "ET" if isinstance(self, ET) else "ET2" @@ -186,7 +211,7 @@ def __eq__(self, other): # ------------------------------------------------------------------ # 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 eta/qlim/jindex setters and + # 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. # ------------------------------------------------------------------ @@ -197,11 +222,11 @@ def _accel_update(self) -> None: pass @property - def eta(self) -> float | Sym | None: + def param(self) -> float | Sym | None: """ Get the transform constant - :returns: The constant η if set + :returns: The constant value if set :rtype: float or Sym or None Examples @@ -211,48 +236,81 @@ def eta(self) -> float | Sym | None: >>> from roboticstoolbox import ET >>> e = ET.tx(1) - >>> e.eta + >>> e.param >>> e = ET.Rx(90, 'deg') - >>> e.eta + >>> e.param >>> e = ET.ty() - >>> e.eta + >>> 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._eta + return self._param - @eta.setter - def eta(self, value: float | Sym) -> None: + @param.setter + def param(self, value: float | Sym) -> None: """ Set the transform constant - :param value: The transform constant η + :param value: The transform constant .. rubric:: Notes - No unit conversions are applied, it is assumed to be in radians. - - Setting `eta` always makes the ET a static (non-joint) transform: + - 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 `eta` and `axis_func` " + "For a static joint either both `param` and `axis_func` " "must be specified otherwise `T` must be supplied" ) - self._eta = value if issymbol(value) else float(value) + self._param = value if issymbol(value) else float(value) self._isstaticsym = issymbol(value) self._joint = False - self._T = self.axis_func(self._eta).copy(order="F") + 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, @@ -511,9 +569,9 @@ def inv(self): inv._flip ^= True elif not inv.iselementary: inv._T = npinv(inv._T).copy(order="F") - elif inv._eta is not None: + elif inv._param is not None: inv._T = npinv(inv._T).copy(order="F") - inv._eta = -inv._eta + inv._param = -inv._param inv._accel_update() @@ -560,13 +618,13 @@ class ET(BaseET): def __init__(self, **kwargs): # Set before super().__init__() runs: BaseET.__init__ may invoke - # the `eta` setter (for a static ET), which calls _accel_update() + # 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/eta/T/joint/etc. are + # 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() @@ -618,7 +676,7 @@ def _accel_init(self) -> None: def _accel_update(self) -> None: """ Push current Python-side state to the compiled struct. Called - whenever eta/qlim/jindex change after construction. A no-op while + 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). """ @@ -701,20 +759,25 @@ def s(self) -> ndarray: # pragma: nocover @classmethod def Rx( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs + cls, + param: float | Sym | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, ) -> "ET": """ Pure rotation about the x-axis - :param η: 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(η)`` is an elementary rotation about the x-axis by a - constant angle η + - ``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. @@ -726,25 +789,30 @@ def Rx( :SymPy: supported """ - - return cls(axis="Rx", eta=eta, axis_func=trotx, unit=unit, **kwargs) + param = _resolve_param(param, eta) + return cls(axis="Rx", param=param, axis_func=trotx, unit=unit, **kwargs) @classmethod def Ry( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs + cls, + param: float | Sym | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, ) -> "ET": """ Pure rotation about the y-axis - :param η: 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(η)`` is an elementary rotation about the y-axis by a - constant angle η + - ``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. @@ -756,24 +824,30 @@ def Ry( :SymPy: supported """ - return cls(axis="Ry", eta=eta, axis_func=troty, unit=unit, **kwargs) + param = _resolve_param(param, eta) + return cls(axis="Ry", param=param, axis_func=troty, unit=unit, **kwargs) @classmethod def Rz( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs + cls, + param: float | Sym | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, ) -> "ET": """ Pure rotation about the z-axis - :param η: 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(η)`` is an elementary rotation about the z-axis by a - constant angle η + - ``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. @@ -785,21 +859,28 @@ def Rz( :SymPy: supported """ - return cls(axis="Rz", eta=eta, axis_func=trotz, unit=unit, **kwargs) + param = _resolve_param(param, eta) + return cls(axis="Rz", param=param, axis_func=trotz, unit=unit, **kwargs) @classmethod - def tx(cls, eta: float | Sym | None = None, **kwargs) -> "ET": + def tx( + cls, + param: float | Sym | None = None, + *, + eta: float | None = None, + **kwargs, + ) -> "ET": """ Pure translation along the x-axis - :param η: translation distance 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(η)`` is an elementary translation along the x-axis by a - distance constant η + - ``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. @@ -811,33 +892,40 @@ def tx(cls, eta: float | Sym | None = None, **kwargs) -> "ET": :SymPy: supported """ + param = _resolve_param(param, eta) # this method is 3x faster than using lambda x: transl(x, 0, 0) - def axis_func(eta): + def axis_func(param): # fmt: off return array([ - [1, 0, 0, eta], + [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, eta=eta, **kwargs) + return cls(axis="tx", axis_func=axis_func, param=param, **kwargs) @classmethod - def ty(cls, eta: float | Sym | None = None, **kwargs) -> "ET": + def ty( + cls, + param: float | Sym | None = None, + *, + eta: float | None = None, + **kwargs, + ) -> "ET": """ Pure translation along the y-axis - :param η: translation distance 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(η)`` is an elementary translation along the y-axis by a - distance constant η + - ``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. @@ -849,32 +937,39 @@ def ty(cls, eta: float | Sym | None = None, **kwargs) -> "ET": :SymPy: supported """ + param = _resolve_param(param, eta) - def axis_func(eta): + def axis_func(param): # fmt: off return array([ [1, 0, 0, 0], - [0, 1, 0, eta], + [0, 1, 0, param], [0, 0, 1, 0], [0, 0, 0, 1] ]) # fmt: on - return cls(axis="ty", eta=eta, axis_func=axis_func, **kwargs) + return cls(axis="ty", param=param, axis_func=axis_func, **kwargs) @classmethod - def tz(cls, eta: float | Sym | None = None, **kwargs) -> "ET": + def tz( + cls, + param: float | Sym | None = None, + *, + eta: float | None = None, + **kwargs, + ) -> "ET": """ Pure translation along the z-axis - :param η: translation distance 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(η)`` is an elementary translation along the z-axis by a - distance constant η + - ``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. @@ -886,18 +981,19 @@ def tz(cls, eta: float | Sym | None = None, **kwargs) -> "ET": :SymPy: supported """ + param = _resolve_param(param, eta) - def axis_func(eta): + def axis_func(param): # fmt: off return array([ [1, 0, 0, 0], [0, 1, 0, 0], - [0, 0, 1, eta], + [0, 0, 1, param], [0, 0, 0, 1] ]) # fmt: on - return cls(axis="tz", axis_func=axis_func, eta=eta, **kwargs) + return cls(axis="tz", axis_func=axis_func, param=param, **kwargs) @classmethod def SE3(cls, T: ndarray | SE3, **kwargs) -> "ET": @@ -944,19 +1040,24 @@ def s(self) -> ndarray: # pragma: nocover @classmethod def R( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs + cls, + param: float | Sym | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, ) -> "ET2": """ Pure rotation - :param η: rotation angle + :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(η)`` is an elementary rotation by a constant angle η + - ``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. @@ -971,26 +1072,31 @@ def R( :func:`ET2`, :func:`isrotation` """ - + param = _resolve_param(param, eta) return cls( - axis="R", eta=eta, axis_func=lambda theta: trot2(theta), unit=unit, **kwargs + axis="R", param=param, axis_func=lambda theta: trot2(theta), unit=unit, **kwargs ) @classmethod def tx( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs + cls, + param: float | Sym | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, ) -> "ET2": """ Pure translation along the x-axis - :param η: translation distance 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(η)`` is an elementary translation along the x-axis by a - distance constant η + - ``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. @@ -1001,24 +1107,29 @@ def tx( :func:`istranslation` """ - - return cls(axis="tx", eta=eta, axis_func=lambda x: transl2(x, 0), **kwargs) + param = _resolve_param(param, eta) + return cls(axis="tx", param=param, axis_func=lambda x: transl2(x, 0), **kwargs) @classmethod def ty( - cls, eta: float | Sym | None = None, unit: str = "rad", **kwargs + cls, + param: float | Sym | None = None, + unit: str = "rad", + *, + eta: float | None = None, + **kwargs, ) -> "ET2": """ Pure translation along the y-axis - :param η: translation distance 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(η)`` is an elementary translation along the y-axis by a - distance constant η + - ``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. @@ -1028,8 +1139,8 @@ def ty( :func:`ET2` """ - - return cls(axis="ty", eta=eta, axis_func=lambda y: transl2(0, y), **kwargs) + 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 | SE2, **kwargs) -> "ET2": diff --git a/src/roboticstoolbox/robot/ETS.py b/src/roboticstoolbox/robot/ETS.py index 4efbec003..fe29044cf 100644 --- a/src/roboticstoolbox/robot/ETS.py +++ b/src/roboticstoolbox/robot/ETS.py @@ -199,16 +199,16 @@ def __str__(self, q: str | None = None): j += 1 elif et.isrotation: - if issymbol(et.eta): - s = f"{et.kind}({et.eta})" + if issymbol(et.param): + s = f"{et.kind}({et.param})" else: - s = f"{et.kind}({et.eta * 180 / np.pi:.4g}°)" + s = f"{et.kind}({et.param * 180 / np.pi:.4g}°)" elif et.istranslation: try: - s = f"{et.kind}({et.eta:.4g})" + s = f"{et.kind}({et.param:.4g})" except TypeError: # pragma: nocover - s = f"{et.kind}({et.eta})" + s = f"{et.kind}({et.param})" elif not et.iselementary: s = str(et) @@ -690,7 +690,7 @@ def merge(self, i: int) -> None: raise ValueError("Transforms are both joints") # pragma: nocover else: - self._data[i].eta = e1.eta + e2.eta + self._data[i].param = e1.param + e2.param del self._data[i + 1] self._fknm_stale = True diff --git a/src/roboticstoolbox/robot/PoERobot.py b/src/roboticstoolbox/robot/PoERobot.py index 98bd4d1a6..c05c2755a 100644 --- a/src/roboticstoolbox/robot/PoERobot.py +++ b/src/roboticstoolbox/robot/PoERobot.py @@ -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..72cc4fa35 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -637,7 +637,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) @@ -1911,7 +1911,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/tests/test_ET.py b/tests/test_ET.py index 4b82318ec..59770819e 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -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,16 +339,16 @@ def test_jindex_error(self): with self.assertRaises(ValueError): r1.jindex = -2 - def test_eta_setter_updates_transform(self): - # ETS.merge() reassigns `.eta` on an already-constructed ET (to + 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.eta = 3.0 - self.assertEqual(r1.eta, 3.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 @@ -368,9 +368,9 @@ def test_et2_no_compiled_accel(self): self.assertFalse(hasattr(e, "fknm")) self.assertFalse(hasattr(e, "_ET__fknm")) - # eta/qlim/jindex updates on ET2 must not attempt to touch a + # param/qlim/jindex updates on ET2 must not attempt to touch a # compiled struct that doesn't exist - e.eta = 2.0 + e.param = 2.0 nt.assert_almost_equal(e.A(), sm.transl2(2.0, 0)) e.qlim = (-1, 1) e.jindex = 0 @@ -435,6 +435,37 @@ def test_ax(self): 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) + if __name__ == "__main__": unittest.main() diff --git a/tests/test_ETS.py b/tests/test_ETS.py index c378327cc..671a29af8 100644 --- a/tests/test_ETS.py +++ b/tests/test_ETS.py @@ -602,7 +602,7 @@ def test_swap(self): e.swap(0) self.assertFalse(e[0].isjoint) self.assertTrue(e[1].isjoint) - self.assertAlmostEqual(e[0].eta, 1.0) # type: ignore + 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 @@ -637,7 +637,7 @@ def test_merge(self): self.assertEqual(len(e), n - 1) self.assertEqual(e[1].kind, "tx") - self.assertAlmostEqual(e[1].eta, 3.0) # type: ignore + 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): From ab0f794e30ff4ce443994428108466909a9eddc7 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 14:11:39 +1000 Subject: [PATCH 05/10] feat(et): accept a string joint descriptor in param (jindex/flip parsing) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Constructing a joint currently means either the parameterless factory (auto-numbered) or a separate `jindex=` int kwarg. `param` now also accepts a descriptive string - `ET.Rx("theta2")`, `ET.Rx("-q(3)")` - from which jindex and flip are parsed: - A leading '-' sets flip (and is stripped); a leading '+' is stripped and ignored. - The first run of digits anywhere in what remains becomes the jindex, via `_parse_joint_descriptor()` - this treats "theta2", "q2", "q(3)" and "θ_3" the same regardless of how the index is set off from the rest of the name. - No digit found (e.g. "theta") falls through to ETS's existing auto-numbering for unassigned joints - old behaviour maintained. - A string that parses as a plain number (e.g. "1.5") is a static value, not a joint descriptor. - `jindex=`/`flip=` given explicitly alongside a string descriptor raises ValueError - the two forms are mutually exclusive, not silently merged. - The original string is remembered (`_joint_name`) and preferred by both ET.__str__ and ETS.__str__ (which has its own, independent joint-formatting logic) over the generic "q2"/"-q2". Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/ET.py | 74 ++++++++++++++++++++++++++------ src/roboticstoolbox/robot/ETS.py | 10 +++++ tests/test_ET.py | 48 +++++++++++++++++++++ tests/test_ETS.py | 11 +++++ 4 files changed, 131 insertions(+), 12 deletions(-) diff --git a/src/roboticstoolbox/robot/ET.py b/src/roboticstoolbox/robot/ET.py index b001c7d82..a04f70214 100644 --- a/src/roboticstoolbox/robot/ET.py +++ b/src/roboticstoolbox/robot/ET.py @@ -17,6 +17,7 @@ transl2, tr2xyt, ) +import re import warnings from copy import deepcopy from roboticstoolbox.robot.fknm import ET_T, ET_init, ET_update @@ -58,16 +59,37 @@ def _resolve_param( 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 | None = None, + 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 = False, + flip: bool | None = None, qlim: ArrayLike | None = None, *, eta: float | None = None, @@ -76,6 +98,32 @@ def __init__( 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 @@ -117,7 +165,9 @@ def __str__(self): param_str = "" if self.isjoint: - if self.jindex is None: + 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}" @@ -760,7 +810,7 @@ def s(self) -> ndarray: # pragma: nocover @classmethod def Rx( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, unit: str = "rad", *, eta: float | None = None, @@ -795,7 +845,7 @@ def Rx( @classmethod def Ry( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, unit: str = "rad", *, eta: float | None = None, @@ -830,7 +880,7 @@ def Ry( @classmethod def Rz( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, unit: str = "rad", *, eta: float | None = None, @@ -865,7 +915,7 @@ def Rz( @classmethod def tx( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, *, eta: float | None = None, **kwargs, @@ -910,7 +960,7 @@ def axis_func(param): @classmethod def ty( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, *, eta: float | None = None, **kwargs, @@ -954,7 +1004,7 @@ def axis_func(param): @classmethod def tz( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, *, eta: float | None = None, **kwargs, @@ -1041,7 +1091,7 @@ def s(self) -> ndarray: # pragma: nocover @classmethod def R( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, unit: str = "rad", *, eta: float | None = None, @@ -1080,7 +1130,7 @@ def R( @classmethod def tx( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, unit: str = "rad", *, eta: float | None = None, @@ -1113,7 +1163,7 @@ def tx( @classmethod def ty( cls, - param: float | Sym | None = None, + param: float | Sym | str | None = None, unit: str = "rad", *, eta: float | None = None, diff --git a/src/roboticstoolbox/robot/ETS.py b/src/roboticstoolbox/robot/ETS.py index fe29044cf..9d56109eb 100644 --- a/src/roboticstoolbox/robot/ETS.py +++ b/src/roboticstoolbox/robot/ETS.py @@ -181,6 +181,16 @@ def __str__(self, q: str | None = None): # 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 diff --git a/tests/test_ET.py b/tests/test_ET.py index 59770819e..dabbb9e1d 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -466,6 +466,54 @@ def test_eta_kwarg_deprecated(self): 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) + if __name__ == "__main__": unittest.main() diff --git a/tests/test_ETS.py b/tests/test_ETS.py index 671a29af8..4e1f99f1c 100644 --- a/tests/test_ETS.py +++ b/tests/test_ETS.py @@ -661,6 +661,17 @@ def test_merge_bad_index(self): 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 = rtb.ET.Rx("theta0") * rtb.ET.tx("q1") * rtb.ET.Rz("-q(2)") * rtb.ET.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()) From 73fee374ea7657ae3973faebc7a056c23d0bf035 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 14:42:39 +1000 Subject: [PATCH 06/10] docs(tech-debt): record deferred IKProtocol idea, ahead of the /ets split IK.py's solvers are typed against ETS but only use a small FK/Jacobian surface (n, qlim, jindices, joints(), eval(), jacob0(), jacobm()). RobotProto.py already has precedent for this (KinematicsProtocol, RobotProto) - a matching IKProtocol would make the real dependency explicit instead of overstating it as "needs a whole ETS". Deferred until after the /ets package split, since the import surface IK.py needs from the new location is the same shape this would formalize. Co-Authored-By: Claude Sonnet 5 --- tech-debt.md | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/tech-debt.md b/tech-debt.md index d169c380f..612bdccb3 100644 --- a/tech-debt.md +++ b/tech-debt.md @@ -1047,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. From b1678732382c84387be2c742cb6f6d6ee01b5852 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 15:03:05 +1000 Subject: [PATCH 07/10] refactor: move ET/ETS out of robot/ into new ets/ package (Phase 1: no split yet) ET/ET2/ETS/ETS2 and their C++ acceleration are the elementary transform sequence representation that Robot/DHRobot/PoERobot are built from, not robot models themselves - a genuinely different, more foundational layer than the rest of robot/. The C++ build already reflects this split cleanly (the _fknm_c CMake target - methods.cpp/ik.cpp/linalg.cpp/ fknm_nb.cpp/structs.h/Eigen - never overlapped with _frne_c's dynamics files), the Python package structure just hadn't caught up. This phase is a pure move, no logic change: ET.py/ETS.py/fknm.py and the _fknm_c source files relocate to src/roboticstoolbox/ets/, CMakeLists.txt updated to match, and every internal import path updated (~20 files: models/, robot/Link.py, robot/BaseRobot.py, robot/Robot.py, etc., plus tests/test_ET.py, tests/test_Robot.py, tests/test_fknm_fallback.py's sys.modules-based patch workaround). Top-level `from roboticstoolbox import ET`/`ETS` is unaffected - only the internal .robot.ET/.robot.ETS import paths moved, and those get a clean break (no deprecated shim) per this being an internal reorg, not a public rename. Also moved: the previously-dead structs.cpp (commented-out, not part of any CMake target, but #includes structs.h which just moved) and split cpp-extensions/README.md into two - the ets/ copy for fknm, a new short one in robot/ for frne - since they now live in different directories. DHFactor.py/DHFactor2.py (WIP) untouched, per earlier agreement - they import ET/ETS via the top-level roboticstoolbox re-export and keep working regardless of where the underlying files live. IK.py stays in robot/ and just updates its ETS import path. Verified: full test suite green (609 passed) against the existing compiled _fknm_c/_frne_c extensions - no C++ code changed, only source file locations and CMakeLists.txt paths, so a from-scratch rebuild wasn't exercised in this session (scikit-build-core unavailable in this environment). The updated CMakeLists.txt paths should be verified with a real build (e.g. in CI) before merging. Co-Authored-By: Claude Sonnet 5 --- CMakeLists.txt | 12 +-- src/roboticstoolbox/{robot => ets}/ET.py | 2 +- src/roboticstoolbox/{robot => ets}/ETS.py | 4 +- src/roboticstoolbox/ets/__init__.py | 9 ++ .../cpp-extensions/Eigen/Cholesky | 0 .../cpp-extensions/Eigen/CholmodSupport | 0 .../{robot => ets}/cpp-extensions/Eigen/Core | 0 .../{robot => ets}/cpp-extensions/Eigen/Dense | 0 .../{robot => ets}/cpp-extensions/Eigen/Eigen | 0 .../cpp-extensions/Eigen/Eigenvalues | 0 .../cpp-extensions/Eigen/Geometry | 0 .../cpp-extensions/Eigen/Householder | 0 .../Eigen/IterativeLinearSolvers | 0 .../cpp-extensions/Eigen/Jacobi | 0 .../cpp-extensions/Eigen/KLUSupport | 0 .../{robot => ets}/cpp-extensions/Eigen/LU | 0 .../cpp-extensions/Eigen/MetisSupport | 0 .../cpp-extensions/Eigen/OrderingMethods | 0 .../cpp-extensions/Eigen/PaStiXSupport | 0 .../cpp-extensions/Eigen/PardisoSupport | 0 .../{robot => ets}/cpp-extensions/Eigen/QR | 0 .../cpp-extensions/Eigen/QtAlignedMalloc | 0 .../cpp-extensions/Eigen/SPQRSupport | 0 .../{robot => ets}/cpp-extensions/Eigen/SVD | 0 .../cpp-extensions/Eigen/Sparse | 0 .../cpp-extensions/Eigen/SparseCholesky | 0 .../cpp-extensions/Eigen/SparseCore | 0 .../cpp-extensions/Eigen/SparseLU | 0 .../cpp-extensions/Eigen/SparseQR | 0 .../cpp-extensions/Eigen/StdDeque | 0 .../cpp-extensions/Eigen/StdList | 0 .../cpp-extensions/Eigen/StdVector | 0 .../cpp-extensions/Eigen/SuperLUSupport | 0 .../cpp-extensions/Eigen/UmfPackSupport | 0 .../cpp-extensions/Eigen/src/Cholesky/LDLT.h | 0 .../cpp-extensions/Eigen/src/Cholesky/LLT.h | 0 .../Eigen/src/Cholesky/LLT_LAPACKE.h | 0 .../Eigen/src/CholmodSupport/CholmodSupport.h | 0 .../Eigen/src/Core/ArithmeticSequence.h | 0 .../cpp-extensions/Eigen/src/Core/Array.h | 0 .../cpp-extensions/Eigen/src/Core/ArrayBase.h | 0 .../Eigen/src/Core/ArrayWrapper.h | 0 .../cpp-extensions/Eigen/src/Core/Assign.h | 0 .../Eigen/src/Core/AssignEvaluator.h | 0 .../Eigen/src/Core/Assign_MKL.h | 0 .../Eigen/src/Core/BandMatrix.h | 0 .../cpp-extensions/Eigen/src/Core/Block.h | 0 .../Eigen/src/Core/BooleanRedux.h | 0 .../Eigen/src/Core/CommaInitializer.h | 0 .../Eigen/src/Core/ConditionEstimator.h | 0 .../Eigen/src/Core/CoreEvaluators.h | 0 .../Eigen/src/Core/CoreIterators.h | 0 .../Eigen/src/Core/CwiseBinaryOp.h | 0 .../Eigen/src/Core/CwiseNullaryOp.h | 0 .../Eigen/src/Core/CwiseTernaryOp.h | 0 .../Eigen/src/Core/CwiseUnaryOp.h | 0 .../Eigen/src/Core/CwiseUnaryView.h | 0 .../cpp-extensions/Eigen/src/Core/DenseBase.h | 0 .../Eigen/src/Core/DenseCoeffsBase.h | 0 .../Eigen/src/Core/DenseStorage.h | 0 .../cpp-extensions/Eigen/src/Core/Diagonal.h | 0 .../Eigen/src/Core/DiagonalMatrix.h | 0 .../Eigen/src/Core/DiagonalProduct.h | 0 .../cpp-extensions/Eigen/src/Core/Dot.h | 0 .../cpp-extensions/Eigen/src/Core/EigenBase.h | 0 .../Eigen/src/Core/ForceAlignedAccess.h | 0 .../cpp-extensions/Eigen/src/Core/Fuzzy.h | 0 .../Eigen/src/Core/GeneralProduct.h | 0 .../Eigen/src/Core/GenericPacketMath.h | 0 .../Eigen/src/Core/GlobalFunctions.h | 0 .../cpp-extensions/Eigen/src/Core/IO.h | 0 .../Eigen/src/Core/IndexedView.h | 0 .../cpp-extensions/Eigen/src/Core/Inverse.h | 0 .../cpp-extensions/Eigen/src/Core/Map.h | 0 .../cpp-extensions/Eigen/src/Core/MapBase.h | 0 .../Eigen/src/Core/MathFunctions.h | 0 .../Eigen/src/Core/MathFunctionsImpl.h | 0 .../cpp-extensions/Eigen/src/Core/Matrix.h | 0 .../Eigen/src/Core/MatrixBase.h | 0 .../Eigen/src/Core/NestByValue.h | 0 .../cpp-extensions/Eigen/src/Core/NoAlias.h | 0 .../cpp-extensions/Eigen/src/Core/NumTraits.h | 0 .../Eigen/src/Core/PartialReduxEvaluator.h | 0 .../Eigen/src/Core/PermutationMatrix.h | 0 .../Eigen/src/Core/PlainObjectBase.h | 0 .../cpp-extensions/Eigen/src/Core/Product.h | 0 .../Eigen/src/Core/ProductEvaluators.h | 0 .../cpp-extensions/Eigen/src/Core/Random.h | 0 .../cpp-extensions/Eigen/src/Core/Redux.h | 0 .../cpp-extensions/Eigen/src/Core/Ref.h | 0 .../cpp-extensions/Eigen/src/Core/Replicate.h | 0 .../cpp-extensions/Eigen/src/Core/Reshaped.h | 0 .../Eigen/src/Core/ReturnByValue.h | 0 .../cpp-extensions/Eigen/src/Core/Reverse.h | 0 .../cpp-extensions/Eigen/src/Core/Select.h | 0 .../Eigen/src/Core/SelfAdjointView.h | 0 .../Eigen/src/Core/SelfCwiseBinaryOp.h | 0 .../cpp-extensions/Eigen/src/Core/Solve.h | 0 .../Eigen/src/Core/SolveTriangular.h | 0 .../Eigen/src/Core/SolverBase.h | 0 .../Eigen/src/Core/StableNorm.h | 0 .../Eigen/src/Core/StlIterators.h | 0 .../cpp-extensions/Eigen/src/Core/Stride.h | 0 .../cpp-extensions/Eigen/src/Core/Swap.h | 0 .../cpp-extensions/Eigen/src/Core/Transpose.h | 0 .../Eigen/src/Core/Transpositions.h | 0 .../Eigen/src/Core/TriangularMatrix.h | 0 .../Eigen/src/Core/VectorBlock.h | 0 .../Eigen/src/Core/VectorwiseOp.h | 0 .../cpp-extensions/Eigen/src/Core/Visitor.h | 0 .../Eigen/src/Core/arch/AVX/Complex.h | 0 .../Eigen/src/Core/arch/AVX/MathFunctions.h | 0 .../Eigen/src/Core/arch/AVX/PacketMath.h | 0 .../Eigen/src/Core/arch/AVX/TypeCasting.h | 0 .../Eigen/src/Core/arch/AVX512/Complex.h | 0 .../src/Core/arch/AVX512/MathFunctions.h | 0 .../Eigen/src/Core/arch/AVX512/PacketMath.h | 0 .../Eigen/src/Core/arch/AVX512/TypeCasting.h | 0 .../Eigen/src/Core/arch/AltiVec/Complex.h | 0 .../src/Core/arch/AltiVec/MathFunctions.h | 0 .../src/Core/arch/AltiVec/MatrixProduct.h | 0 .../Core/arch/AltiVec/MatrixProductCommon.h | 0 .../src/Core/arch/AltiVec/MatrixProductMMA.h | 0 .../Eigen/src/Core/arch/AltiVec/PacketMath.h | 0 .../Eigen/src/Core/arch/CUDA/Complex.h | 0 .../Eigen/src/Core/arch/Default/BFloat16.h | 0 .../Eigen/src/Core/arch/Default/ConjHelper.h | 0 .../arch/Default/GenericPacketMathFunctions.h | 0 .../Default/GenericPacketMathFunctionsFwd.h | 0 .../Eigen/src/Core/arch/Default/Half.h | 0 .../Eigen/src/Core/arch/Default/Settings.h | 0 .../Eigen/src/Core/arch/Default/TypeCasting.h | 0 .../Eigen/src/Core/arch/GPU/MathFunctions.h | 0 .../Eigen/src/Core/arch/GPU/PacketMath.h | 0 .../Eigen/src/Core/arch/GPU/TypeCasting.h | 0 .../src/Core/arch/HIP/hcc/math_constants.h | 0 .../Eigen/src/Core/arch/MSA/Complex.h | 0 .../Eigen/src/Core/arch/MSA/MathFunctions.h | 0 .../Eigen/src/Core/arch/MSA/PacketMath.h | 0 .../Eigen/src/Core/arch/NEON/Complex.h | 0 .../Core/arch/NEON/GeneralBlockPanelKernel.h | 0 .../Eigen/src/Core/arch/NEON/MathFunctions.h | 0 .../Eigen/src/Core/arch/NEON/PacketMath.h | 0 .../Eigen/src/Core/arch/NEON/TypeCasting.h | 0 .../Eigen/src/Core/arch/SSE/Complex.h | 0 .../Eigen/src/Core/arch/SSE/MathFunctions.h | 0 .../Eigen/src/Core/arch/SSE/PacketMath.h | 0 .../Eigen/src/Core/arch/SSE/TypeCasting.h | 0 .../Eigen/src/Core/arch/SVE/MathFunctions.h | 0 .../Eigen/src/Core/arch/SVE/PacketMath.h | 0 .../Eigen/src/Core/arch/SVE/TypeCasting.h | 0 .../Eigen/src/Core/arch/SYCL/InteropHeaders.h | 0 .../Eigen/src/Core/arch/SYCL/MathFunctions.h | 0 .../Eigen/src/Core/arch/SYCL/PacketMath.h | 0 .../src/Core/arch/SYCL/SyclMemoryModel.h | 0 .../Eigen/src/Core/arch/SYCL/TypeCasting.h | 0 .../Eigen/src/Core/arch/ZVector/Complex.h | 0 .../src/Core/arch/ZVector/MathFunctions.h | 0 .../Eigen/src/Core/arch/ZVector/PacketMath.h | 0 .../src/Core/functors/AssignmentFunctors.h | 0 .../Eigen/src/Core/functors/BinaryFunctors.h | 0 .../Eigen/src/Core/functors/NullaryFunctors.h | 0 .../Eigen/src/Core/functors/StlFunctors.h | 0 .../Eigen/src/Core/functors/TernaryFunctors.h | 0 .../Eigen/src/Core/functors/UnaryFunctors.h | 0 .../Core/products/GeneralBlockPanelKernel.h | 0 .../src/Core/products/GeneralMatrixMatrix.h | 0 .../products/GeneralMatrixMatrixTriangular.h | 0 .../GeneralMatrixMatrixTriangular_BLAS.h | 0 .../Core/products/GeneralMatrixMatrix_BLAS.h | 0 .../src/Core/products/GeneralMatrixVector.h | 0 .../Core/products/GeneralMatrixVector_BLAS.h | 0 .../Eigen/src/Core/products/Parallelizer.h | 0 .../Core/products/SelfadjointMatrixMatrix.h | 0 .../products/SelfadjointMatrixMatrix_BLAS.h | 0 .../Core/products/SelfadjointMatrixVector.h | 0 .../products/SelfadjointMatrixVector_BLAS.h | 0 .../src/Core/products/SelfadjointProduct.h | 0 .../Core/products/SelfadjointRank2Update.h | 0 .../Core/products/TriangularMatrixMatrix.h | 0 .../products/TriangularMatrixMatrix_BLAS.h | 0 .../Core/products/TriangularMatrixVector.h | 0 .../products/TriangularMatrixVector_BLAS.h | 0 .../Core/products/TriangularSolverMatrix.h | 0 .../products/TriangularSolverMatrix_BLAS.h | 0 .../Core/products/TriangularSolverVector.h | 0 .../Eigen/src/Core/util/BlasUtil.h | 0 .../src/Core/util/ConfigureVectorization.h | 0 .../Eigen/src/Core/util/Constants.h | 0 .../src/Core/util/DisableStupidWarnings.h | 0 .../Eigen/src/Core/util/ForwardDeclarations.h | 0 .../Eigen/src/Core/util/IndexedViewHelper.h | 0 .../Eigen/src/Core/util/IntegralConstant.h | 0 .../Eigen/src/Core/util/MKL_support.h | 0 .../Eigen/src/Core/util/Macros.h | 0 .../Eigen/src/Core/util/Memory.h | 0 .../cpp-extensions/Eigen/src/Core/util/Meta.h | 0 .../Eigen/src/Core/util/NonMPL2.h | 0 .../src/Core/util/ReenableStupidWarnings.h | 0 .../Eigen/src/Core/util/ReshapedHelper.h | 0 .../Eigen/src/Core/util/StaticAssert.h | 0 .../Eigen/src/Core/util/SymbolicIndex.h | 0 .../Eigen/src/Core/util/XprHelper.h | 0 .../src/Eigenvalues/ComplexEigenSolver.h | 0 .../Eigen/src/Eigenvalues/ComplexSchur.h | 0 .../src/Eigenvalues/ComplexSchur_LAPACKE.h | 0 .../Eigen/src/Eigenvalues/EigenSolver.h | 0 .../src/Eigenvalues/GeneralizedEigenSolver.h | 0 .../GeneralizedSelfAdjointEigenSolver.h | 0 .../src/Eigenvalues/HessenbergDecomposition.h | 0 .../src/Eigenvalues/MatrixBaseEigenvalues.h | 0 .../Eigen/src/Eigenvalues/RealQZ.h | 0 .../Eigen/src/Eigenvalues/RealSchur.h | 0 .../Eigen/src/Eigenvalues/RealSchur_LAPACKE.h | 0 .../src/Eigenvalues/SelfAdjointEigenSolver.h | 0 .../SelfAdjointEigenSolver_LAPACKE.h | 0 .../src/Eigenvalues/Tridiagonalization.h | 0 .../Eigen/src/Geometry/AlignedBox.h | 0 .../Eigen/src/Geometry/AngleAxis.h | 0 .../Eigen/src/Geometry/EulerAngles.h | 0 .../Eigen/src/Geometry/Homogeneous.h | 0 .../Eigen/src/Geometry/Hyperplane.h | 0 .../Eigen/src/Geometry/OrthoMethods.h | 0 .../Eigen/src/Geometry/ParametrizedLine.h | 0 .../Eigen/src/Geometry/Quaternion.h | 0 .../Eigen/src/Geometry/Rotation2D.h | 0 .../Eigen/src/Geometry/RotationBase.h | 0 .../Eigen/src/Geometry/Scaling.h | 0 .../Eigen/src/Geometry/Transform.h | 0 .../Eigen/src/Geometry/Translation.h | 0 .../Eigen/src/Geometry/Umeyama.h | 0 .../Eigen/src/Geometry/arch/Geometry_SIMD.h | 0 .../Eigen/src/Householder/BlockHouseholder.h | 0 .../Eigen/src/Householder/Householder.h | 0 .../src/Householder/HouseholderSequence.h | 0 .../BasicPreconditioners.h | 0 .../src/IterativeLinearSolvers/BiCGSTAB.h | 0 .../ConjugateGradient.h | 0 .../IncompleteCholesky.h | 0 .../IterativeLinearSolvers/IncompleteLUT.h | 0 .../IterativeSolverBase.h | 0 .../LeastSquareConjugateGradient.h | 0 .../IterativeLinearSolvers/SolveWithGuess.h | 0 .../cpp-extensions/Eigen/src/Jacobi/Jacobi.h | 0 .../Eigen/src/KLUSupport/KLUSupport.h | 0 .../cpp-extensions/Eigen/src/LU/Determinant.h | 0 .../cpp-extensions/Eigen/src/LU/FullPivLU.h | 0 .../cpp-extensions/Eigen/src/LU/InverseImpl.h | 0 .../Eigen/src/LU/PartialPivLU.h | 0 .../Eigen/src/LU/PartialPivLU_LAPACKE.h | 0 .../Eigen/src/LU/arch/InverseSize4.h | 0 .../Eigen/src/MetisSupport/MetisSupport.h | 0 .../Eigen/src/OrderingMethods/Amd.h | 0 .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 0 .../Eigen/src/OrderingMethods/Ordering.h | 0 .../Eigen/src/PaStiXSupport/PaStiXSupport.h | 0 .../Eigen/src/PardisoSupport/PardisoSupport.h | 0 .../Eigen/src/QR/ColPivHouseholderQR.h | 0 .../src/QR/ColPivHouseholderQR_LAPACKE.h | 0 .../src/QR/CompleteOrthogonalDecomposition.h | 0 .../Eigen/src/QR/FullPivHouseholderQR.h | 0 .../Eigen/src/QR/HouseholderQR.h | 0 .../Eigen/src/QR/HouseholderQR_LAPACKE.h | 0 .../src/SPQRSupport/SuiteSparseQRSupport.h | 0 .../cpp-extensions/Eigen/src/SVD/BDCSVD.h | 0 .../cpp-extensions/Eigen/src/SVD/JacobiSVD.h | 0 .../Eigen/src/SVD/JacobiSVD_LAPACKE.h | 0 .../cpp-extensions/Eigen/src/SVD/SVDBase.h | 0 .../Eigen/src/SVD/UpperBidiagonalization.h | 0 .../src/SparseCholesky/SimplicialCholesky.h | 0 .../SparseCholesky/SimplicialCholesky_impl.h | 0 .../Eigen/src/SparseCore/AmbiVector.h | 0 .../Eigen/src/SparseCore/CompressedStorage.h | 0 .../ConservativeSparseSparseProduct.h | 0 .../Eigen/src/SparseCore/MappedSparseMatrix.h | 0 .../Eigen/src/SparseCore/SparseAssign.h | 0 .../Eigen/src/SparseCore/SparseBlock.h | 0 .../Eigen/src/SparseCore/SparseColEtree.h | 0 .../src/SparseCore/SparseCompressedBase.h | 0 .../src/SparseCore/SparseCwiseBinaryOp.h | 0 .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 0 .../Eigen/src/SparseCore/SparseDenseProduct.h | 0 .../src/SparseCore/SparseDiagonalProduct.h | 0 .../Eigen/src/SparseCore/SparseDot.h | 0 .../Eigen/src/SparseCore/SparseFuzzy.h | 0 .../Eigen/src/SparseCore/SparseMap.h | 0 .../Eigen/src/SparseCore/SparseMatrix.h | 0 .../Eigen/src/SparseCore/SparseMatrixBase.h | 0 .../Eigen/src/SparseCore/SparsePermutation.h | 0 .../Eigen/src/SparseCore/SparseProduct.h | 0 .../Eigen/src/SparseCore/SparseRedux.h | 0 .../Eigen/src/SparseCore/SparseRef.h | 0 .../src/SparseCore/SparseSelfAdjointView.h | 0 .../Eigen/src/SparseCore/SparseSolverBase.h | 0 .../SparseSparseProductWithPruning.h | 0 .../Eigen/src/SparseCore/SparseTranspose.h | 0 .../src/SparseCore/SparseTriangularView.h | 0 .../Eigen/src/SparseCore/SparseUtil.h | 0 .../Eigen/src/SparseCore/SparseVector.h | 0 .../Eigen/src/SparseCore/SparseView.h | 0 .../Eigen/src/SparseCore/TriangularSolver.h | 0 .../Eigen/src/SparseLU/SparseLU.h | 0 .../Eigen/src/SparseLU/SparseLUImpl.h | 0 .../Eigen/src/SparseLU/SparseLU_Memory.h | 0 .../Eigen/src/SparseLU/SparseLU_Structs.h | 0 .../src/SparseLU/SparseLU_SupernodalMatrix.h | 0 .../Eigen/src/SparseLU/SparseLU_Utils.h | 0 .../Eigen/src/SparseLU/SparseLU_column_bmod.h | 0 .../Eigen/src/SparseLU/SparseLU_column_dfs.h | 0 .../src/SparseLU/SparseLU_copy_to_ucol.h | 0 .../Eigen/src/SparseLU/SparseLU_gemm_kernel.h | 0 .../src/SparseLU/SparseLU_heap_relax_snode.h | 0 .../Eigen/src/SparseLU/SparseLU_kernel_bmod.h | 0 .../Eigen/src/SparseLU/SparseLU_panel_bmod.h | 0 .../Eigen/src/SparseLU/SparseLU_panel_dfs.h | 0 .../Eigen/src/SparseLU/SparseLU_pivotL.h | 0 .../Eigen/src/SparseLU/SparseLU_pruneL.h | 0 .../Eigen/src/SparseLU/SparseLU_relax_snode.h | 0 .../Eigen/src/SparseQR/SparseQR.h | 0 .../Eigen/src/StlSupport/StdDeque.h | 0 .../Eigen/src/StlSupport/StdList.h | 0 .../Eigen/src/StlSupport/StdVector.h | 0 .../Eigen/src/StlSupport/details.h | 0 .../Eigen/src/SuperLUSupport/SuperLUSupport.h | 0 .../Eigen/src/UmfPackSupport/UmfPackSupport.h | 0 .../cpp-extensions/Eigen/src/misc/Image.h | 0 .../cpp-extensions/Eigen/src/misc/Kernel.h | 0 .../Eigen/src/misc/RealSvd2x2.h | 0 .../cpp-extensions/Eigen/src/misc/blas.h | 0 .../cpp-extensions/Eigen/src/misc/lapack.h | 0 .../cpp-extensions/Eigen/src/misc/lapacke.h | 0 .../Eigen/src/misc/lapacke_mangling.h | 0 .../Eigen/src/plugins/ArrayCwiseBinaryOps.h | 0 .../Eigen/src/plugins/ArrayCwiseUnaryOps.h | 0 .../Eigen/src/plugins/BlockMethods.h | 0 .../Eigen/src/plugins/CommonCwiseBinaryOps.h | 0 .../Eigen/src/plugins/CommonCwiseUnaryOps.h | 0 .../Eigen/src/plugins/IndexedViewMethods.h | 0 .../Eigen/src/plugins/MatrixCwiseBinaryOps.h | 0 .../Eigen/src/plugins/MatrixCwiseUnaryOps.h | 0 .../Eigen/src/plugins/ReshapedMethods.h | 0 .../ets/cpp-extensions/README.md | 73 ++++++++++++++++ .../{robot => ets}/cpp-extensions/fknm_nb.cpp | 0 .../{robot => ets}/cpp-extensions/ik.cpp | 0 .../{robot => ets}/cpp-extensions/ik.h | 0 .../{robot => ets}/cpp-extensions/linalg.cpp | 0 .../{robot => ets}/cpp-extensions/linalg.h | 0 .../{robot => ets}/cpp-extensions/methods.cpp | 0 .../{robot => ets}/cpp-extensions/methods.h | 0 .../{robot => ets}/cpp-extensions/structs.cpp | 0 .../{robot => ets}/cpp-extensions/structs.h | 0 src/roboticstoolbox/{robot => ets}/fknm.py | 0 src/roboticstoolbox/models/ETS/Frankie.py | 4 +- .../models/ETS/GenericSeven.py | 4 +- src/roboticstoolbox/models/ETS/Omni.py | 4 +- src/roboticstoolbox/models/ETS/Panda.py | 2 +- src/roboticstoolbox/models/ETS/Planar2.py | 4 +- src/roboticstoolbox/models/ETS/Planar_Y.py | 4 +- src/roboticstoolbox/models/ETS/XYPanda.py | 4 +- .../models/URDF/FrankieOmni.py | 4 +- src/roboticstoolbox/models/URDF/URDFRobot.py | 4 +- src/roboticstoolbox/robot/BaseRobot.py | 6 +- src/roboticstoolbox/robot/DHLink.py | 4 +- src/roboticstoolbox/robot/DHRobot.py | 2 +- src/roboticstoolbox/robot/Gripper.py | 2 +- src/roboticstoolbox/robot/Link.py | 4 +- src/roboticstoolbox/robot/PoERobot.py | 4 +- src/roboticstoolbox/robot/Robot.py | 2 +- src/roboticstoolbox/robot/RobotProto.py | 2 +- src/roboticstoolbox/robot/__init__.py | 3 +- .../robot/cpp-extensions/README.md | 84 ++++--------------- src/roboticstoolbox/tools/p_servo.py | 4 +- tests/test_ET.py | 2 +- tests/test_Robot.py | 4 +- tests/test_fknm_fallback.py | 24 +++--- 375 files changed, 153 insertions(+), 128 deletions(-) rename src/roboticstoolbox/{robot => ets}/ET.py (99%) rename src/roboticstoolbox/{robot => ets}/ETS.py (99%) create mode 100644 src/roboticstoolbox/ets/__init__.py rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Cholesky (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/CholmodSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Core (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Dense (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Eigen (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Eigenvalues (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Geometry (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Householder (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/IterativeLinearSolvers (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Jacobi (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/KLUSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/LU (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/MetisSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/OrderingMethods (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/PaStiXSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/PardisoSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/QR (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/QtAlignedMalloc (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SPQRSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SVD (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/Sparse (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SparseCholesky (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SparseCore (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SparseLU (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SparseQR (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/StdDeque (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/StdList (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/StdVector (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/SuperLUSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/UmfPackSupport (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Cholesky/LDLT.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Cholesky/LLT.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Cholesky/LLT_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/CholmodSupport/CholmodSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ArithmeticSequence.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Array.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ArrayBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ArrayWrapper.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Assign.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/AssignEvaluator.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Assign_MKL.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/BandMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Block.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/BooleanRedux.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CommaInitializer.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ConditionEstimator.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CoreEvaluators.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CoreIterators.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CwiseBinaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CwiseNullaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CwiseTernaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CwiseUnaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/CwiseUnaryView.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/DenseBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/DenseCoeffsBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/DenseStorage.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Diagonal.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/DiagonalMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/DiagonalProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Dot.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/EigenBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ForceAlignedAccess.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Fuzzy.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/GeneralProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/GenericPacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/GlobalFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/IO.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/IndexedView.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Inverse.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Map.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/MapBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/MathFunctionsImpl.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Matrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/MatrixBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/NestByValue.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/NoAlias.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/NumTraits.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/PartialReduxEvaluator.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/PermutationMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/PlainObjectBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Product.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ProductEvaluators.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Random.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Redux.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Ref.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Replicate.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Reshaped.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/ReturnByValue.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Reverse.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Select.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/SelfAdjointView.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/SelfCwiseBinaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Solve.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/SolveTriangular.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/SolverBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/StableNorm.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/StlIterators.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Stride.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Swap.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Transpose.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Transpositions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/TriangularMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/VectorBlock.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/VectorwiseOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/Visitor.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX512/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX512/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX512/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AVX512/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AltiVec/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AltiVec/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductCommon.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AltiVec/MatrixProductMMA.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/AltiVec/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/CUDA/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/BFloat16.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/ConjHelper.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/Half.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/Settings.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/Default/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/GPU/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/GPU/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/GPU/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/HIP/hcc/math_constants.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/MSA/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/MSA/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/MSA/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/NEON/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/NEON/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/NEON/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/NEON/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SSE/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SSE/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SSE/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SSE/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SVE/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SVE/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SVE/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SYCL/InteropHeaders.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SYCL/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SYCL/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SYCL/SyclMemoryModel.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/SYCL/TypeCasting.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/ZVector/Complex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/ZVector/MathFunctions.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/arch/ZVector/PacketMath.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/functors/AssignmentFunctors.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/functors/BinaryFunctors.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/functors/NullaryFunctors.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/functors/StlFunctors.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/functors/TernaryFunctors.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/functors/UnaryFunctors.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralBlockPanelKernel.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/Parallelizer.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/SelfadjointProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/SelfadjointRank2Update.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/products/TriangularSolverVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/BlasUtil.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/ConfigureVectorization.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/Constants.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/DisableStupidWarnings.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/ForwardDeclarations.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/IndexedViewHelper.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/IntegralConstant.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/MKL_support.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/Macros.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/Memory.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/Meta.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/NonMPL2.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/ReenableStupidWarnings.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/ReshapedHelper.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/StaticAssert.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/SymbolicIndex.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Core/util/XprHelper.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/ComplexEigenSolver.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/EigenSolver.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/HessenbergDecomposition.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/RealQZ.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/RealSchur.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Eigenvalues/Tridiagonalization.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/AlignedBox.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/AngleAxis.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/EulerAngles.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Homogeneous.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Hyperplane.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/OrthoMethods.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/ParametrizedLine.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Quaternion.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Rotation2D.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/RotationBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Scaling.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Transform.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Translation.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/Umeyama.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Geometry/arch/Geometry_SIMD.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Householder/BlockHouseholder.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Householder/Householder.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Householder/HouseholderSequence.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/Jacobi/Jacobi.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/KLUSupport/KLUSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/LU/Determinant.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/LU/FullPivLU.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/LU/InverseImpl.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/LU/PartialPivLU.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/LU/PartialPivLU_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/LU/arch/InverseSize4.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/MetisSupport/MetisSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/OrderingMethods/Amd.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/OrderingMethods/Eigen_Colamd.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/OrderingMethods/Ordering.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/PaStiXSupport/PaStiXSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/PardisoSupport/PardisoSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/QR/CompleteOrthogonalDecomposition.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/QR/FullPivHouseholderQR.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/QR/HouseholderQR.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/QR/HouseholderQR_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SVD/BDCSVD.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SVD/JacobiSVD.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SVD/JacobiSVD_LAPACKE.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SVD/SVDBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SVD/UpperBidiagonalization.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/AmbiVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/CompressedStorage.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/MappedSparseMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseAssign.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseBlock.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseColEtree.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseCompressedBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseCwiseBinaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseCwiseUnaryOp.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseDenseProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseDiagonalProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseDot.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseFuzzy.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseMap.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseMatrixBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparsePermutation.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseProduct.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseRedux.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseRef.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseSelfAdjointView.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseSolverBase.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseSparseProductWithPruning.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseTranspose.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseTriangularView.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseUtil.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/SparseView.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseCore/TriangularSolver.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLUImpl.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_Memory.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_Structs.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_Utils.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_bmod.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_column_dfs.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_gemm_kernel.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_kernel_bmod.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_bmod.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_panel_dfs.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_pivotL.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_pruneL.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseLU/SparseLU_relax_snode.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SparseQR/SparseQR.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/StlSupport/StdDeque.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/StlSupport/StdList.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/StlSupport/StdVector.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/StlSupport/details.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/SuperLUSupport/SuperLUSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/UmfPackSupport/UmfPackSupport.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/Image.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/Kernel.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/RealSvd2x2.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/blas.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/lapack.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/lapacke.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/misc/lapacke_mangling.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/ArrayCwiseBinaryOps.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/ArrayCwiseUnaryOps.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/BlockMethods.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/CommonCwiseBinaryOps.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/CommonCwiseUnaryOps.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/IndexedViewMethods.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/MatrixCwiseBinaryOps.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/MatrixCwiseUnaryOps.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/Eigen/src/plugins/ReshapedMethods.h (100%) create mode 100644 src/roboticstoolbox/ets/cpp-extensions/README.md rename src/roboticstoolbox/{robot => ets}/cpp-extensions/fknm_nb.cpp (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/ik.cpp (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/ik.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/linalg.cpp (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/linalg.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/methods.cpp (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/methods.h (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/structs.cpp (100%) rename src/roboticstoolbox/{robot => ets}/cpp-extensions/structs.h (100%) rename src/roboticstoolbox/{robot => ets}/fknm.py (100%) 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/robot/ET.py b/src/roboticstoolbox/ets/ET.py similarity index 99% rename from src/roboticstoolbox/robot/ET.py rename to src/roboticstoolbox/ets/ET.py index a04f70214..692dd0108 100644 --- a/src/roboticstoolbox/robot/ET.py +++ b/src/roboticstoolbox/ets/ET.py @@ -20,7 +20,7 @@ import re import warnings from copy import deepcopy -from roboticstoolbox.robot.fknm import ET_T, ET_init, ET_update +from roboticstoolbox.ets.fknm import ET_T, ET_init, ET_update from spatialmath.base import getvector from spatialmath import SE3, SE2 from typing import Callable, TYPE_CHECKING diff --git a/src/roboticstoolbox/robot/ETS.py b/src/roboticstoolbox/ets/ETS.py similarity index 99% rename from src/roboticstoolbox/robot/ETS.py rename to src/roboticstoolbox/ets/ETS.py index 9d56109eb..72d5617a2 100644 --- a/src/roboticstoolbox/robot/ETS.py +++ b/src/roboticstoolbox/ets/ETS.py @@ -26,7 +26,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,7 +38,7 @@ IK_LM_c, ) from copy import deepcopy -from roboticstoolbox.robot.ET import ET, ET2, BaseET +from roboticstoolbox.ets.ET import ET, ET2, BaseET from typing import overload, TypeVar from typing import Literal as L from roboticstoolbox.tools.types import ArrayLike, NDArray diff --git a/src/roboticstoolbox/ets/__init__.py b/src/roboticstoolbox/ets/__init__.py new file mode 100644 index 000000000..ab976216a --- /dev/null +++ b/src/roboticstoolbox/ets/__init__.py @@ -0,0 +1,9 @@ +from roboticstoolbox.ets.ET import ET, ET2 +from roboticstoolbox.ets.ETS import ETS, 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 100% rename from src/roboticstoolbox/robot/fknm.py rename to src/roboticstoolbox/ets/fknm.py 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..e62e16f0e 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.ET import ET2 +from roboticstoolbox.ets.ETS 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/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/Link.py b/src/roboticstoolbox/robot/Link.py index 2736fb586..8f8b0b61a 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -14,8 +14,8 @@ 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, ETS2 +from roboticstoolbox.ets.ET import ET, ET2, 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 c05c2755a..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): diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 72cc4fa35..ca2da9893 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -40,7 +40,7 @@ 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, ETS2 from roboticstoolbox.tools import URDF from roboticstoolbox.tools.types import ArrayLike, NDArray from roboticstoolbox.tools.data import rtb_path_to_datafile 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/tests/test_ET.py b/tests/test_ET.py index dabbb9e1d..05fc85104 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 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_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) From 3465dfacb1a5726a9823004908de7ba54f49fa5e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 15:11:40 +1000 Subject: [PATCH 08/10] refactor: split ET.py into ET.py/ET2.py/_ET.py, add Rx/tx/etc free functions ET2 (2D) previously shared a file with its 3D counterpart and their common base class, which blocked exposing Rx/Ry/Rz/tx/ty/tz etc as bare importable functions (tx/ty would collide between the two). Splits into: - _ET.py: BaseET only (private/shared - can't import either concrete subclass without a cycle, since both depend on it). - ET.py: the ET class + Rx/Ry/Rz/tx/ty/tz/SE3 as module-level names, so `from roboticstoolbox.ets.ET import *` pulls in exactly the 3D set. - ET2.py: the ET2 class + R/tx/ty/SE2 as module-level names, same idea for 2D. tx/ty deliberately mean something different here than in ET.py - wildcard-importing both into one namespace isn't supported, by design. isinstance(self, ET)/isinstance(self, ET2) inside BaseET's shared __str__/__repr__ (which can't import either concrete subclass) are replaced: __str__'s 3D-vs-2D compound-transform formatting now checks `self.kind == "SE3"`/`"SE2"` directly (already guaranteed at that point - no import needed, more robust than isinstance against subclassing). __repr__'s class-name label now uses `self.__class__.__name__` (idiomatic for a display label, and more correct for any future subclass than a hardcoded "ET"/"ET2" ternary). One landmine hit and fixed while writing this: `SE3 = ET.SE3` / `SE2 = ET2.SE2` (the free-function aliases) would shadow the imported spatialmath SE3/SE2 classes that the classmethod bodies reference by name at call time - fixed by importing them as SE3T/SE2T instead. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/ets/ET.py | 828 +--------------------- src/roboticstoolbox/ets/ET2.py | 179 +++++ src/roboticstoolbox/ets/ETS.py | 4 +- src/roboticstoolbox/ets/_ET.py | 660 +++++++++++++++++ src/roboticstoolbox/ets/__init__.py | 3 +- src/roboticstoolbox/models/ETS/Planar2.py | 2 +- src/roboticstoolbox/robot/Link.py | 4 +- tests/test_ET.py | 33 +- 8 files changed, 905 insertions(+), 808 deletions(-) create mode 100644 src/roboticstoolbox/ets/ET2.py create mode 100644 src/roboticstoolbox/ets/_ET.py diff --git a/src/roboticstoolbox/ets/ET.py b/src/roboticstoolbox/ets/ET.py index 692dd0108..cef5d2492 100644 --- a/src/roboticstoolbox/ets/ET.py +++ b/src/roboticstoolbox/ets/ET.py @@ -4,661 +4,16 @@ @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, -) -import re -import warnings -from copy import deepcopy +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 -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 - - -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: - if isinstance(self, ET): - 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 isinstance(self, ET2): - 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)) - - start = "ET" if isinstance(self, ET) else "ET2" - - 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) - - # ------------------------------------------------------------------ - # 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 class ET(BaseET): @@ -1046,7 +401,7 @@ def axis_func(param): return cls(axis="tz", axis_func=axis_func, param=param, **kwargs) @classmethod - def SE3(cls, T: ndarray | SE3, **kwargs) -> "ET": + def SE3(cls, T: ndarray | SE3T, **kwargs) -> "ET": """ A static SE3 @@ -1062,156 +417,23 @@ def SE3(cls, T: ndarray | SE3, **kwargs) -> "ET": :SymPy: supported """ - trans = T.A if isinstance(T, SE3) else T + trans = T.A if isinstance(T, SE3T) 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.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 | 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) - - # 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.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/ets/ETS.py b/src/roboticstoolbox/ets/ETS.py index 72d5617a2..cd086310c 100644 --- a/src/roboticstoolbox/ets/ETS.py +++ b/src/roboticstoolbox/ets/ETS.py @@ -38,7 +38,9 @@ IK_LM_c, ) from copy import deepcopy -from roboticstoolbox.ets.ET import ET, ET2, BaseET +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ET2 import ET2 +from roboticstoolbox.ets._ET import BaseET from typing import overload, TypeVar from typing import Literal as L from roboticstoolbox.tools.types import ArrayLike, NDArray diff --git a/src/roboticstoolbox/ets/_ET.py b/src/roboticstoolbox/ets/_ET.py new file mode 100644 index 000000000..70dcecb57 --- /dev/null +++ b/src/roboticstoolbox/ets/_ET.py @@ -0,0 +1,660 @@ +#!/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) + + # ------------------------------------------------------------------ + # 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/__init__.py b/src/roboticstoolbox/ets/__init__.py index ab976216a..4a4219c62 100644 --- a/src/roboticstoolbox/ets/__init__.py +++ b/src/roboticstoolbox/ets/__init__.py @@ -1,4 +1,5 @@ -from roboticstoolbox.ets.ET import ET, ET2 +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ET2 import ET2 from roboticstoolbox.ets.ETS import ETS, ETS2 __all__ = [ diff --git a/src/roboticstoolbox/models/ETS/Planar2.py b/src/roboticstoolbox/models/ETS/Planar2.py index e62e16f0e..bdb646d51 100644 --- a/src/roboticstoolbox/models/ETS/Planar2.py +++ b/src/roboticstoolbox/models/ETS/Planar2.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.ets.ET import ET2 +from roboticstoolbox.ets.ET2 import ET2 from roboticstoolbox.ets.ETS import ETS2 from roboticstoolbox.robot.Robot import Robot2 from roboticstoolbox.robot.Link import Link2 diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index 8f8b0b61a..6363774e4 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -15,7 +15,9 @@ import roboticstoolbox as rtb from roboticstoolbox.ets.ETS import ETS, ETS2 -from roboticstoolbox.ets.ET import ET, ET2, BaseET +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/tests/test_ET.py b/tests/test_ET.py index 05fc85104..938d7adf0 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.ets.ET import BaseET +from roboticstoolbox.ets._ET import BaseET import sympy from copy import copy, deepcopy @@ -514,6 +514,37 @@ def test_joint_descriptor_conflict_raises(self): 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) + if __name__ == "__main__": unittest.main() From d35ead73a7319a471083f9c5a1d189aa2e4a94e4 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 15 Jul 2026 15:18:39 +1000 Subject: [PATCH 09/10] refactor: split ETS.py into ETS.py/ETS2.py/_ETS.py Same treatment as ET/ET2/_ET (previous commit), for the sequence classes: - _ETS.py: BaseETS only (private/shared - both ETS and ETS2 depend on it, so it can't depend on either without a cycle). - ETS.py: the ETS class. - ETS2.py: the ETS2 class (imports ET2 directly from ET2.py - safe, one-directional, since ET2.py never depends on ETS2.py at load time). isinstance(self, ETS) inside BaseETS.plot()/teach() (which can't import the concrete ETS without a cycle) is fixed the same way those methods already handle Robot/Robot2: a function-local, call-time-deferred import of ETS from roboticstoolbox.ets.ETS, added alongside the existing local Robot/Robot2 import. Mechanical split of a ~2900-line file done by slicing along the existing class boundaries (BaseETS/ETS/ETS2 were already cleanly delineated) rather than hand-transcribing; each of the three new files keeps the full original import block rather than a hand-traced minimal set, trading a few unused imports per file for confidence against missing something across this much code. Verified: full test suite green (610 passed), including test_plot/ test_teach on both ETS and ETS2 specifically (the isinstance fix). Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/ets/ETS.py | 1090 +-------------------- src/roboticstoolbox/ets/ETS2.py | 472 +++++++++ src/roboticstoolbox/ets/_ETS.py | 715 ++++++++++++++ src/roboticstoolbox/ets/__init__.py | 3 +- src/roboticstoolbox/models/ETS/Planar2.py | 2 +- src/roboticstoolbox/robot/Link.py | 3 +- src/roboticstoolbox/robot/Robot.py | 3 +- 7 files changed, 1196 insertions(+), 1092 deletions(-) create mode 100644 src/roboticstoolbox/ets/ETS2.py create mode 100644 src/roboticstoolbox/ets/_ETS.py diff --git a/src/roboticstoolbox/ets/ETS.py b/src/roboticstoolbox/ets/ETS.py index cd086310c..450d03130 100644 --- a/src/roboticstoolbox/ets/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 @@ -39,672 +38,12 @@ ) from copy import deepcopy from roboticstoolbox.ets.ET import ET -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 -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: - # 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 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) -> 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 - - 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 class ETS(BaseETS): """ @@ -2545,428 +1884,3 @@ def split(self) -> list["ETS"]: return segments -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 split(self) -> list["ETS2"]: - """ - Split ETS2 into link segments - - :returns: a list of ETS2, 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 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/ETS2.py b/src/roboticstoolbox/ets/ETS2.py new file mode 100644 index 000000000..a52a4676f --- /dev/null +++ b/src/roboticstoolbox/ets/ETS2.py @@ -0,0 +1,472 @@ +#!/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 split(self) -> list["ETS2"]: + """ + Split ETS2 into link segments + + :returns: a list of ETS2, 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 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/_ETS.py b/src/roboticstoolbox/ets/_ETS.py new file mode 100644 index 000000000..7beb1de20 --- /dev/null +++ b/src/roboticstoolbox/ets/_ETS.py @@ -0,0 +1,715 @@ +#!/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) + + 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: + # 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 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) -> 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 + # 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 index 4a4219c62..9579b9956 100644 --- a/src/roboticstoolbox/ets/__init__.py +++ b/src/roboticstoolbox/ets/__init__.py @@ -1,6 +1,7 @@ from roboticstoolbox.ets.ET import ET from roboticstoolbox.ets.ET2 import ET2 -from roboticstoolbox.ets.ETS import ETS, ETS2 +from roboticstoolbox.ets.ETS import ETS +from roboticstoolbox.ets.ETS2 import ETS2 __all__ = [ "ET", diff --git a/src/roboticstoolbox/models/ETS/Planar2.py b/src/roboticstoolbox/models/ETS/Planar2.py index bdb646d51..6401cf356 100644 --- a/src/roboticstoolbox/models/ETS/Planar2.py +++ b/src/roboticstoolbox/models/ETS/Planar2.py @@ -2,7 +2,7 @@ import numpy as np from roboticstoolbox.ets.ET2 import ET2 -from roboticstoolbox.ets.ETS import ETS2 +from roboticstoolbox.ets.ETS2 import ETS2 from roboticstoolbox.robot.Robot import Robot2 from roboticstoolbox.robot.Link import Link2 diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index 6363774e4..9753593ed 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -14,7 +14,8 @@ from typing import overload import roboticstoolbox as rtb -from roboticstoolbox.ets.ETS import ETS, ETS2 +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 diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index ca2da9893..6ed864375 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.ets.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 From a0674e79a367cc03e7bc908bd74b152f92a11b13 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 17 Jul 2026 15:01:01 +1000 Subject: [PATCH 10/10] feat(ets): rigorous dh/mdh split() convention, sum() support, terser tests `ETS.split("dh"/"mdh")` (added mid-flight on this branch) previously used a heuristic that only absorbed one adjacent constant either side of a joint and never validated the rest of a segment. Replaced with a real per-segment parse against the DH/MDH 4-slot template (Rz/tz/tx/Rx or tx/Rx/Rz/tz), where exactly one of the two z-axis slots is the joint, the rest are optional but must appear in template order, and content that can't be accounted for raises ValueError - via a shared `_split_convention` helper parametrized by template/joint-slots/which end is exposed vs. folded into the boundary segment. split()'s return shape changed twice in discussion and settled on a single flat list `[base, *segments, gripper]` (always length n_joints+2, base/gripper are an empty ETS - never None - when the method has no concept of one), enabling `base, *segments, gripper = ets.split(method)` unpacking and a clean round-trip via `sum(result, ETS())`. The "first"/"last" methods were pure duplicated logic between ETS.py and ETS2.py (no DH-specific behaviour), so moved to BaseETS.split() in _ETS.py; ETS now only overrides for "dh"/"mdh", delegating everything else to super(). ETS2 inherits it unchanged, gaining base/gripper support and a correct "unknown split method" error for "dh"/"mdh" it doesn't support, with zero DH-specific code needed in the 2D class. Consequence caught by actually running Robot construction, not just tests: Robot.__init__/Robot2.__init__ built one Link per element of `arg.split()` directly - the old flat-list contract. The new fixed-shape list broke this outright (spurious empty base Link, crashed jindex bookkeeping). Fixed both to unpack base/gripper and only append gripper as an extra static link when non-empty, restoring original behaviour. Also added `BaseET.__radd__`/`BaseETS.__radd__` (start value of 0 treated as identity) so `sum([...])` works on ET/ET2/ETS/ETS2 without an explicit start, matching `+`/`__mul__` composition already there. test_ETS.py/test_ETS2.py switched to `from roboticstoolbox.ets.ET(2) import *` instead of the `rtb.ET./rtb.ET2.` prefix on every call (ET/ET2 already export a curated __all__, so this is clean) - each file is single-domain so there's no tx/ty cross-collision. Caught two real hazards by running the suite rather than trusting the mechanical strip: local variables shadowing the same-named free function within a function scope (`tx = tx(1.543)` - Python's scoping makes the whole function's `tx` local, breaking the RHS reference; fixed by renaming the locals to t_x/t_y/t_z) and `SE3`/`SE2` meaning two different things (ET's constant-transform-ET constructor vs. spatialmath's pose class) - resolved by importing ET/ET2's wildcard names before the spatialmath import so the spatialmath one wins by default, with the handful of genuine ET-constructor call sites reverted to explicit `rtb.ET.SE3(...)`. Verified: full suite green throughout (620 passed, 72 skipped). Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/ets/ETS.py | 157 ++++++- src/roboticstoolbox/ets/ETS2.py | 23 -- src/roboticstoolbox/ets/_ET.py | 8 + src/roboticstoolbox/ets/_ETS.py | 80 +++- src/roboticstoolbox/robot/Robot.py | 18 +- tests/test_ET.py | 45 ++ tests/test_ETS.py | 638 +++++++++++++++-------------- tests/test_ETS2.py | 312 +++++++------- 8 files changed, 771 insertions(+), 510 deletions(-) diff --git a/src/roboticstoolbox/ets/ETS.py b/src/roboticstoolbox/ets/ETS.py index 450d03130..aaa71e3fd 100644 --- a/src/roboticstoolbox/ets/ETS.py +++ b/src/roboticstoolbox/ets/ETS.py @@ -1860,27 +1860,154 @@ def ikine_QP( return solver.solve(ets=self, Tep=Tep, q0=q0) - def split(self) -> list["ETS"]: + @staticmethod + def _template_consume(elements: list["BaseET"], template: list[str]) -> int: """ - Split ETS into link segments - - :returns: a list of ETS, each one, apart from the last, ends with a variable ET. - + 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 """ + slot = 0 + consumed = 0 + for et in elements: + if et.kind in template[slot:]: + slot = template.index(et.kind, slot) + 1 + consumed += 1 + else: + break + return consumed - segments = [] - start = 0 + def _split_convention(self, template: list[str], joint_slots: set[int], exposed: str): + """ + 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. + """ + 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: + 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]] + ) + 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] + + if exposed == "tail": + tail = self[end[-1] :] + else: + end[-1] = len(self) + tail = self[0: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)) + segments = [self.__class__(self[start[i] : end[i]]) for i in range(len(idx))] + return segments, head, tail - tail = self._data[start:] + def split(self, method: str = "last") -> list["ETS"]: + r""" + Split ETS into link segments - if len(tail) > 0: - segments.append(self.__class__(tail)) + :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. - return segments + .. 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)}") + """ + 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 + + return [head] + segments + [tail] # type: ignore diff --git a/src/roboticstoolbox/ets/ETS2.py b/src/roboticstoolbox/ets/ETS2.py index a52a4676f..2b7db3d5c 100644 --- a/src/roboticstoolbox/ets/ETS2.py +++ b/src/roboticstoolbox/ets/ETS2.py @@ -228,29 +228,6 @@ def insert( # type: ignore[override] self._data.insert(i + j, et) self._fknm_stale = True - def split(self) -> list["ETS2"]: - """ - Split ETS2 into link segments - - :returns: a list of ETS2, 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 fkine( self, q: ArrayLike, diff --git a/src/roboticstoolbox/ets/_ET.py b/src/roboticstoolbox/ets/_ET.py index 70dcecb57..a3f2e61fe 100644 --- a/src/roboticstoolbox/ets/_ET.py +++ b/src/roboticstoolbox/ets/_ET.py @@ -257,6 +257,14 @@ def __deepcopy__(self, memo): 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 diff --git a/src/roboticstoolbox/ets/_ETS.py b/src/roboticstoolbox/ets/_ETS.py index 7beb1de20..a6699f915 100644 --- a/src/roboticstoolbox/ets/_ETS.py +++ b/src/roboticstoolbox/ets/_ETS.py @@ -73,9 +73,6 @@ def __init__(self): 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 @@ -98,6 +95,14 @@ def __eq__(self, other: object) -> bool: __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 # ------------------------------------------------------------------ @@ -296,6 +301,63 @@ def joints(self) -> list[ET]: 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 @@ -537,20 +599,22 @@ def __getitem__(self: "BaseETS", i: int) -> BaseET: ... def __getitem__(self: "ETS", i: int) -> ET: ... @overload - def __getitem__(self: "ETS", i: slice) -> list[ET]: ... + def __getitem__(self: "ETS", i: slice) -> "ETS": ... @overload def __getitem__(self: "ETS2", i: int) -> ET2: ... @overload - def __getitem__(self: "ETS2", i: slice) -> list[ET2]: ... + 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 + :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 -------- @@ -564,7 +628,9 @@ def __getitem__(self, i): >>> e[1:3] """ - return self._data[i] # can be [2] or slice, eg. [3:5] + if isinstance(i, slice): + return self.__class__(self._data[i]) + return self._data[i] def __deepcopy__(self, memo): new_data = [] diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 6ed864375..685c3d761 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -109,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 @@ -1805,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 ( diff --git a/tests/test_ET.py b/tests/test_ET.py index 938d7adf0..563f676ba 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -545,6 +545,51 @@ def test_free_functions(self): # 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 4e1f99f1c..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,12 +587,12 @@ 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)) @@ -589,7 +601,7 @@ def test_insert(self): def test_swap(self): q = [1.0] - e = rtb.ET.Rz(jindex=0) * rtb.ET.Rz(1) * rtb.ET.tx(2) + e = Rz(jindex=0) * Rz(1) * tx(2) ans = SE3.Rz(q[0]) * SE3.Rz(1) * SE3.Tx(2) @@ -612,13 +624,13 @@ def test_swap(self): nt.assert_almost_equal(e.fkine(q).A, ans.A) def test_swap_not_commutative(self): - e = rtb.ET.tx(1) * rtb.ET.Rz(jindex=0) * rtb.ET.Rz(1) + e = tx(1) * Rz(jindex=0) * Rz(1) with self.assertRaises(ValueError): e.swap(0) def test_swap_bad_index(self): - e = rtb.ET.tx(1) * rtb.ET.tx(2) + e = tx(1) * tx(2) with self.assertRaises(IndexError): e.swap(-1) @@ -628,7 +640,7 @@ def test_swap_bad_index(self): def test_merge(self): q = [1.0] - e = rtb.ET.Rz(jindex=0) * rtb.ET.tx(1) * rtb.ET.tx(2) * rtb.ET.Rz(1) + e = Rz(jindex=0) * tx(1) * tx(2) * Rz(1) ans = SE3.Rz(q[0]) * SE3.Tx(3) * SE3.Rz(1) @@ -641,19 +653,19 @@ def test_merge(self): nt.assert_almost_equal(e.fkine(q).A, ans.A) def test_merge_not_same_type(self): - e = rtb.ET.tx(1) * rtb.ET.Rz(1) + e = tx(1) * Rz(1) with self.assertRaises(ValueError): e.merge(0) def test_merge_both_joints(self): - e = rtb.ET.Rz(jindex=0) * rtb.ET.Rz(jindex=1) + e = Rz(jindex=0) * Rz(jindex=1) with self.assertRaises(ValueError): e.merge(0) def test_merge_bad_index(self): - e = rtb.ET.tx(1) * rtb.ET.tx(2) + e = tx(1) * tx(2) with self.assertRaises(IndexError): e.merge(-1) @@ -666,7 +678,7 @@ def test_joint_descriptor_ets_str(self): # 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 = rtb.ET.Rx("theta0") * rtb.ET.tx("q1") * rtb.ET.Rz("-q(2)") * rtb.ET.Ry() + 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]) @@ -674,18 +686,18 @@ def test_joint_descriptor_ets_str(self): 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) @@ -693,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) @@ -714,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) @@ -740,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) @@ -767,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 @@ -1217,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 @@ -1668,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 @@ -1725,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 @@ -1781,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) @@ -1855,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): @@ -1883,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) @@ -4348,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)