From 9eb0e86e569632215dd94f51555a2f636f8e3e8b Mon Sep 17 00:00:00 2001 From: Anton Soloviev Date: Mon, 12 Oct 2026 01:50:53 -0700 Subject: [PATCH 1/3] Broadcast scalar integration steps over twist sequences --- spatialmath/twist.py | 16 +++++++++--- tests/test_twist_sequence.py | 49 ++++++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+), 4 deletions(-) create mode 100644 tests/test_twist_sequence.py diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 928235e4..d80b7df0 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -1014,7 +1014,7 @@ def SE3(self, theta=1, unit="rad"): if len(theta) == 1: # theta is a scalar - return SE3(smb.trexp(self.S * theta)) + return SE3([smb.trexp(S * theta[0]) for S in self.data]) else: # theta is a vector if len(self) == 1: @@ -1072,6 +1072,8 @@ def exp(self, theta=1, unit="rad"): if len(self) == 1: return SE3([smb.trexp(self.S * t) for t in theta], check=False) + elif len(theta) == 1: + return SE3([smb.trexp(s * theta[0]) for s in self.data], check=False) elif len(self) == len(theta): return SE3([smb.trexp(s * t) for s, t in zip(self.S, theta)], check=False) else: @@ -1530,15 +1532,19 @@ def SE2(self, theta=1, unit="rad"): """ from spatialmath.pose2d import SE2 - if unit != "rad" and self.isprismatic: + if unit != "rad" and any(s.isprismatic for s in self): print("Twist3.exp: using degree mode for a prismatic twist") theta = smb.getunit(theta, unit) if len(theta) == 1: - return SE2(smb.trexp2(self.S * theta)) - else: + return SE2([smb.trexp2(S * theta[0]) for S in self.data]) + elif len(self) == 1: return SE2([smb.trexp2(self.S * t) for t in theta]) + elif len(self) == len(theta): + return SE2([smb.trexp2(S * t) for S, t in zip(self.data, theta)]) + else: + raise ValueError("length of twist and theta not consistent") def skewa(self): """ @@ -1605,6 +1611,8 @@ def exp(self, theta=1, unit="rad"): if len(self) == 1: return SE2([smb.trexp2(self.S * t) for t in theta], check=False) + elif len(theta) == 1: + return SE2([smb.trexp2(s * theta[0]) for s in self.data], check=False) elif len(self) == len(theta): return SE2([smb.trexp2(s * t) for s, t in zip(self.S, theta)], check=False) else: diff --git a/tests/test_twist_sequence.py b/tests/test_twist_sequence.py new file mode 100644 index 00000000..7dcbee9a --- /dev/null +++ b/tests/test_twist_sequence.py @@ -0,0 +1,49 @@ +import numpy as np +import pytest +from scipy.linalg import expm + +from spatialmath import SE2, SE3, Twist2, Twist3 + + +@pytest.mark.parametrize("twist,pose", [(Twist2, SE2), (Twist3, SE3)]) +@pytest.mark.parametrize("method", ["exp", "pose"]) +@pytest.mark.parametrize("theta", [None, 0, -0.4, [0.2, -0.7]]) +def test_twist_sequence_exponential(twist, pose, method, theta): + """A shared step and individual steps both integrate every twist.""" + vectors = ( + [[1, -2, 0.3], [2, 1, -0.4]] + if twist is Twist2 + else [[1, -2, 3, 0.2, 0.3, -0.4], [2, 1, -1, -0.1, 0.5, 0.2]] + ) + sequence = twist([np.array(v) for v in vectors]) + convert = getattr(sequence, "exp" if method == "exp" else pose.__name__) + result = convert() if theta is None else convert(theta) + steps = np.broadcast_to(1 if theta is None else theta, (2,)) + assert isinstance(result, pose) + assert len(result) == 2 + for value, source, step in zip(result, sequence, steps): + np.testing.assert_allclose(value.A, expm(source.skewa() * step), atol=1e-14) + + +@pytest.mark.parametrize("twist,pose", [(Twist2, SE2), (Twist3, SE3)]) +@pytest.mark.parametrize("method", ["exp", "pose"]) +def test_twist_sequence_degrees_and_singleton(twist, pose, method): + """Degrees, singleton expansion and mismatched lengths retain their rules.""" + source = twist([1, 2, 0.3] if twist is Twist2 else [1, 2, 3, 0.2, 0.3, 0.4]) + name = "exp" if method == "exp" else pose.__name__ + angles = [15, -30] + expanded = getattr(source, name)(angles, unit="deg") + assert len(expanded) == 2 + for value, angle in zip(expanded, angles): + np.testing.assert_allclose( + value.A, expm(source.skewa() * np.deg2rad(angle)), atol=1e-14 + ) + sequence = twist([source, source]) + shared = getattr(sequence, name)(30, unit="deg") + assert len(shared) == 2 + for value in shared: + np.testing.assert_allclose( + value.A, expm(source.skewa() * np.deg2rad(30)), atol=1e-14 + ) + with pytest.raises(ValueError): + getattr(sequence, name)([0.1, 0.2, 0.3]) From 7b68d6f9836027522fe895f588559c5b9ba2a8ec Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:14:57 +1000 Subject: [PATCH 2/3] fix: twist sequence predicates, scalar left-multiply, and copied docstrings Follow-up to the twist-sequence broadcasting change. A search for cut/paste errors between Twist2 and Twist3 turned up some real bugs as well as text: - isprismatic/isrevolute raised AttributeError on a sequence because they read .w/.v from the raw ndarrays in .data instead of the Twist elements. - Twist2.__rmul had no trailing underscores so it was never called, and ``2 * Twist2`` silently fell through to UserList repetition. - Twist3.__rmul__ used right.S, which is a list for a sequence, so scalar * Twist3 failed for sequences. - Twist3.Rand used yrange[0] as the lower bound of the z range. Docstrings: Twist2.SE2 said it returned SE3 and pointed at Twist3.exp (and its degree-mode message named Twist3.exp); Twist2.__init__ said it built a prismatic twist and swapped the moment/direction sizes; Twist2.isvalid described a 6-vector/4x4; UnitRevolute said prismatic; Twist3.Rand described SE3.Rand; Rx/Ry/Rz called their results SE(3)/SO(3) rotations; the [omega, v] ordering note contradicted the [v, omega] data layout; exp() documented a default of None and left a literal unterminated; the ``runblock:`` directives in __str__/__repr__ had a single colon (and their examples would not have run once fixed). SE2.Rand's example used SE3. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/pose2d.py | 4 +- spatialmath/twist.py | 104 +++++++++++++----------- tests/test_twist_sequence_predicates.py | 58 +++++++++++++ 3 files changed, 117 insertions(+), 49 deletions(-) create mode 100644 tests/test_twist_sequence_predicates.py diff --git a/spatialmath/pose2d.py b/spatialmath/pose2d.py index 62ac8510..96583173 100644 --- a/spatialmath/pose2d.py +++ b/spatialmath/pose2d.py @@ -358,13 +358,13 @@ def Rand( Return an SE2 instance with random rotation and translation. - - ``SE2.Rand()`` is a random SE(2) rotation. + - ``SE2.Rand()`` is a random SE(2) pose. - ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random poses. Example, create random ten vehicles in the xy-plane:: - >>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) + >>> x = SE2.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) >>> len(x) 10 diff --git a/spatialmath/twist.py b/spatialmath/twist.py index d80b7df0..475e2b8c 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -105,7 +105,7 @@ def isprismatic(self): if len(self) == 1: return smb.iszerovec(self.w) else: - return [smb.iszerovec(x.w) for x in self.data] + return [smb.iszerovec(x.w) for x in self] @property def isrevolute(self): @@ -131,7 +131,7 @@ def isrevolute(self): if len(self) == 1: return smb.iszerovec(self.v) else: - return [smb.iszerovec(x.v) for x in self.data] + return [smb.iszerovec(x.v) for x in self] @property def isunit(self): @@ -302,7 +302,7 @@ class Twist3(BaseTwist): .. note:: Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational - components, ie. :math:`[\omega, \vec{v}]`. + components, ie. :math:`[\vec{v}, \omega]`. """ @@ -364,8 +364,8 @@ def isvalid(v, check=True): """ Test if matrix is valid twist - :param x: array to test - :type x: ndarray + :param v: array to test + :type v: ndarray :return: Whether the value is a 6-vector or a valid 4x4 se(3) element :rtype: bool @@ -538,7 +538,7 @@ def Rx(cls, theta, unit="rad"): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Rx(θ)`` is an SE(3) rotation of θ radians about the x-axis + - ``Twist3.Rx(θ)`` is an se(3) rotation of θ radians about the x-axis - ``Twist3.Rx(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -569,7 +569,7 @@ def Ry(cls, theta, unit="rad", t=None): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis + - ``Twist3.Ry(θ)`` is an se(3) rotation of θ radians about the y-axis - ``Twist3.Ry(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -600,7 +600,7 @@ def Rz(cls, theta, unit="rad", t=None): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis + - ``Twist3.Rz(θ)`` is an se(3) rotation of θ radians about the z-axis - ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -765,14 +765,14 @@ def Rand( :type zrange: 2-element sequence, optional :param N: number of random transforms :type N: int - :return: SE(3) matrix - :rtype: SE3 instance + :return: 3D twist + :rtype: Twist3 instance - Return an SE3 instance with random rotation and translation. + Return a Twist3 instance with random rotation and translation. - - ``SE3.Rand()`` is a random SE(3) translation. - - ``SE3.Rand(N=N)`` is an SE3 object containing a sequence of N random - poses. + - ``Twist3.Rand()`` is a random se(3) twist. + - ``Twist3.Rand(N=N)`` is a Twist3 object containing a sequence of N random + twists. Example: @@ -781,7 +781,7 @@ def Rand( >>> from spatialmath import Twist3 >>> Twist3.Rand(N=2) - :seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand` + :seealso: :meth:`~spatialmath.pose3d.SE3.Rand` """ from spatialmath.pose3d import SO3 @@ -792,7 +792,7 @@ def Rand( low=yrange[0], high=yrange[1], size=N ) # random values in the range Z = np.random.uniform( - low=yrange[0], high=zrange[1], size=N + low=zrange[0], high=zrange[1], size=N ) # random values in the range R = SO3.Rand(N=N) @@ -1028,7 +1028,7 @@ def exp(self, theta=1, unit="rad"): """ Exponentiate a 3D twist - :param theta: rotation magnitude, defaults to None + :param theta: rotation magnitude, defaults to 1 :type theta: float, optional :param units: rotational units, defaults to 'rad' :type units: str, optional @@ -1037,7 +1037,7 @@ def exp(self, theta=1, unit="rad"): - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, + - ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` If ``len(X)==1`` and ``len(θ)==N`` then the resulting SE3 object has @@ -1165,7 +1165,7 @@ def __rmul__( - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` """ if smb.isscalar(left): - return Twist3(right.S * left) + return Twist3(right.binop(left, lambda x, y: x * y)) else: raise ValueError("Twist3 *, incorrect left operand") @@ -1180,10 +1180,10 @@ def __str__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist3 - >>> x = Twist3.R([1,2,3], [4,5,6]) + >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> print(x) """ return "\n".join( @@ -1204,13 +1204,11 @@ def __repr__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist3 - >>> x = Twist3.R([1,2,3], [4,5,6]) + >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> x - >>> a.append(a) - >>> a """ if len(self) == 0: @@ -1261,8 +1259,7 @@ def __init__(self, arg=None, w=None, check=True): r""" Construct a new 2D Twist object - :type a: 2-element array-like - :return: 2D prismatic twist + :return: 2D twist :rtype: Twist2 instance - ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as @@ -1272,7 +1269,7 @@ def __init__(self, arg=None, w=None, check=True): - ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation, and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion - ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the - moment vector (1 element) and direction vector (2 elements). + moment vector (2 elements) and direction vector (1 element). :References: - Robotics, Vision & Control for Python, Section 2.2.2.4, P. Corke, Springer 2023. @@ -1280,7 +1277,7 @@ def __init__(self, arg=None, w=None, check=True): .. note:: Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational - components, ie. :math:`[\omega, \vec{v}]`. + components, ie. :math:`[\vec{v}, \omega]`. """ from spatialmath.pose2d import SE2 @@ -1330,12 +1327,12 @@ def isvalid(v, check=True): """ Test if matrix is valid twist - :param x: array to test - :type x: ndarray + :param v: array to test + :type v: ndarray :return: Whether the value is a 3-vector or a valid 3x3 se(2) element :rtype: bool - A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix, + A twist can be represented by a 3-vector or a 3x3 augmented skew symmetric matrix, for example: .. runblock:: pycon @@ -1371,7 +1368,7 @@ def UnitRevolute(cls, q): :param q: Point on the line of action :type q: array_like(2) - :return: 2D prismatic twist + :return: 2D revolute twist :rtype: Twist2 instance - ``Twist2.UnitRevolute(q)`` is a 2D Twist object representing rotation about the 2D point ``q``. @@ -1515,7 +1512,7 @@ def SE2(self, theta=1, unit="rad"): Convert 2D twist to SE(2) matrix :return: an SE(2) representation - :rtype: SE3 instance + :rtype: SE2 instance ``S.SE2()`` is an SE2 object representing the homogeneous transformation equivalent to the Twist2. This is the exponentiation of the twist vector. @@ -1528,12 +1525,12 @@ def SE2(self, theta=1, unit="rad"): >>> S = Twist2.UnitPrismatic([1,2]) >>> S.SE2() - :seealso: :func:`Twist3.exp` + :seealso: :func:`Twist2.exp` """ from spatialmath.pose2d import SE2 if unit != "rad" and any(s.isprismatic for s in self): - print("Twist3.exp: using degree mode for a prismatic twist") + print("Twist2.SE2: using degree mode for a prismatic twist") theta = smb.getunit(theta, unit) @@ -1576,7 +1573,7 @@ def exp(self, theta=1, unit="rad"): r""" Exponentiate a 2D twist - :param theta: rotation magnitude, defaults to None + :param theta: rotation magnitude, defaults to 1 :type theta: float, optional :param unit: rotational units, defaults to 'rad' :type unit: str, optional @@ -1585,7 +1582,7 @@ def exp(self, theta=1, unit="rad"): - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, + - ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` Example: @@ -1677,7 +1674,7 @@ def Tx(cls, x): :return: 2D twist vector :rtype: Twist2 instance - `Twist2.Tx(x)` is an se(2) translation of ``x`` along the x-axis + ``Twist2.Tx(x)`` is an se(2) translation of ``x`` along the x-axis Example: @@ -1703,7 +1700,7 @@ def Ty(cls, y): :return: 2D twist vector :rtype: Twist2 instance - `Twist2.Ty(y) is an se(2) translation of ``y`` along the y-axis + ``Twist2.Ty(y)`` is an se(2) translation of ``y`` along the y-axis Example: @@ -1782,11 +1779,25 @@ def __mul__( else: raise ValueError("Twist2 *, incorrect right operand") - def __rmul(self, left): + def __rmul__( + right, left + ): # lgtm[py/not-named-self] pylint: disable=no-self-argument + """ + Overloaded ``*`` operator + + :arg right: right multiplicand + :arg left: left multiplicand + :return: product + :raises: ValueError + + Left-multiplication by a scalar + + - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` + """ if smb.isscalar(left): - return Twist2(self.S * left) + return Twist2(right.binop(left, lambda x, y: x * y)) else: - raise ValueError("twist *, incorrect left operand") + raise ValueError("Twist2 *, incorrect left operand") def __str__(self): """ @@ -1799,8 +1810,9 @@ def __str__(self): Example: - .. runblock: pycon + .. runblock:: pycon + >>> from spatialmath import Twist2 >>> x = Twist2([1,2,3]) >>> print(x) """ @@ -1815,13 +1827,11 @@ def __repr__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist2 >>> x = Twist2([1,2,3]) >>> x - >>> a.append(a) - >>> a """ diff --git a/tests/test_twist_sequence_predicates.py b/tests/test_twist_sequence_predicates.py new file mode 100644 index 00000000..8508a2a6 --- /dev/null +++ b/tests/test_twist_sequence_predicates.py @@ -0,0 +1,58 @@ +import numpy as np +import pytest + +from spatialmath import Twist2, Twist3 + + +def _sequence(cls): + """A prismatic, a revolute and a general twist, in that order.""" + if cls is Twist2: + vectors = [[1, 0, 0], [0, 0, 1], [1, 2, 3]] + else: + vectors = [[1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1], [1, 2, 3, 0.1, 0.2, 0.3]] + return cls([np.array(v, dtype=float) for v in vectors]) + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_isprismatic_sequence(cls): + """isprismatic gives one result per twist, as for a single twist.""" + twists = _sequence(cls) + assert list(twists.isprismatic) == [True, False, False] + assert [t.isprismatic for t in twists] == list(twists.isprismatic) + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_isrevolute_sequence(cls): + """isrevolute gives one result per twist, as for a single twist.""" + twists = _sequence(cls) + assert list(twists.isrevolute) == [False, True, False] + assert [t.isrevolute for t in twists] == list(twists.isrevolute) + + +def test_twist2_se2_degree_warning_covers_sequence(capsys): + """Degree mode warns when any twist in the sequence is prismatic.""" + _sequence(Twist2).SE2(30, unit="deg") + assert "Twist2.SE2" in capsys.readouterr().out + + Twist2([np.array([1.0, 2.0, 3.0])] * 2).SE2(30, unit="deg") + assert capsys.readouterr().out == "" + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_scalar_times_twist(cls): + """``s * X`` scales each twist, the same as ``X * s``.""" + twists = _sequence(cls) + for left in (2 * twists, 2.0 * twists): + right = twists * 2 + assert len(left) == len(twists) + for a, b, t in zip(left, right, twists): + np.testing.assert_allclose(a.S, 2 * t.S) + np.testing.assert_allclose(a.S, b.S) + + +def test_twist3_rand_honours_zrange(): + """The z lower bound comes from zrange, not yrange.""" + np.random.seed(0) + poses = Twist3.Rand(N=200, xrange=(0, 1), yrange=(-3, -2), zrange=(5, 6)).exp() + z = np.array([p.t[2] for p in poses]) + assert z.min() >= 5 and z.max() <= 6 From 5fb579d07e4344218cbba209ebc2e0994c08522f Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:31:06 +1000 Subject: [PATCH 3/3] docs: describe the displacement and velocity usages of Twist2/Twist3 A twist is used both as the log of a pose (RVC, Modern Robotics) and as a velocity (ROS Twist). They share the vector, the [v, w] layout and the algebra, but nothing in the docstrings said so. Say so, point to SpatialVelocity as the typed counterpart, and note which properties are invariant to scaling the twist and which are not. Twist2 had no class docstring, only __init__, so give it one. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/twist.py | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 475e2b8c..2b2854bb 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -296,6 +296,23 @@ class Twist3(BaseTwist): 3D rigid body transformation which is the unique elements of the Lie algebra se(3) of the corresponding SE(3) matrix. + A twist can be used in two ways which share the same 6-vector, the same + :math:`[\vec{v}, \omega]` layout and the same algebra: + + - as a *displacement*, where ``exp(S)`` is the pose reached by following the + screw motion and ``exp(S * theta)`` scales it. ``S`` is often a unit + twist, see :meth:`unit`. This is the usage in Robotics, Vision & Control + and in Modern Robotics. + - as a *velocity*, where ``S`` has units of m/s and rad/s and ``exp(S * dt)`` + is the pose change over a time ``dt``. This is the meaning of ``Twist`` + in ROS. The typed counterpart, which also has acceleration, force and + momentum, is :class:`~spatialmath.spatialvector.SpatialVelocity`. + + Properties that describe the screw axis, such as :attr:`pitch`, + :attr:`pole` and :meth:`line`, are unchanged by scaling the twist so apply + to both. Those that depend on its magnitude, such as :attr:`theta`, are + an angle in the first usage and a rate in the second. + :References: - Robotics, Vision & Control for Python, Section 2.3.2.3, P. Corke, Springer 2023. - Modern Robotics, Lynch & Park, Cambridge 2017 @@ -1255,6 +1272,20 @@ def _repr_pretty_(self, p, cycle): class Twist2(BaseTwist): + r""" + 2D twist class + + A Twist2 object holds the parameters of a twist, a representation of a + 2D rigid body transformation which is the unique elements of the Lie + algebra se(2) of the corresponding SE(2) matrix. It is a 3-vector + :math:`[\vec{v}, \omega]`. + + As for :class:`Twist3` it can be used as a *displacement*, where ``exp(S)`` + is the pose reached by following the motion, or as a *velocity*, where + ``exp(S * dt)`` is the pose change over a time ``dt``. Properties that + describe the axis, such as :attr:`pole`, are unchanged by scaling the twist. + """ + def __init__(self, arg=None, w=None, check=True): r""" Construct a new 2D Twist object