Skip to content

Commit a3b6492

Browse files
petercorkeclaude
andauthored
fix: twist sequence predicates, scalar left-multiply, copied docstrings; document twist usages (builds on #248) (#258)
## What this does A follow-up to #248. Reviewing it, and searching `twist.py` for the same copy-and-paste pattern between `Twist2` and `Twist3`, turned up some real bugs as well as wrong docstrings. ### Bugs | Where | Problem | |---|---| | `isprismatic`, `isrevolute` | Raised `AttributeError` for any sequence of twists. They read `.w`/`.v` from the raw ndarrays in `.data` instead of the elements. | | `Twist2.__rmul` | Missing trailing underscores, so it was never called. `2 * Twist2(...)` silently did `UserList` repetition (two copies) instead of scaling. | | `Twist3.__rmul__` | Used `right.S`, which is a list for a sequence, so scalar × sequence failed. | | `Twist3.Rand` | The z range used `yrange[0]` as its lower bound. | ### Docstrings copied between classes - `Twist2.SE2` said it returned `SE3` and pointed at `Twist3.exp`, and its degree-mode message named `Twist3.exp`. - `Twist2.__init__` described a prismatic twist and had the moment and direction sizes swapped. - `Twist2.isvalid` described a 6-vector / 4x4 matrix. - `Twist2.UnitRevolute` said "prismatic". - `Twist3.Rand` described `SE3.Rand`, and `Rx`/`Ry`/`Rz` called their results SE(3) or SO(3) rotations. - The class notes claimed `[ω, v]` ordering while the data is `[v, ω]`. - `exp()` documented a default of `None` and left a literal unterminated. - `.. runblock: pycon` (single colon) in `__str__`/`__repr__` is a comment, not the directive. I fixed the colon and also the examples, which would have failed once they ran (`Twist3.R` does not exist, `a` was undefined). - `SE2.Rand`'s example called `SE3.Rand`. ### Documentation A paragraph on `Twist3`, and a new class docstring on `Twist2` (it had none), describing the two usages of a twist: as a displacement and as a velocity. It says which properties are scale-invariant and points to `SpatialVelocity`. The open questions are in #257. ## Not included - The `unit()` bugs and the `pole()` docstring parens, which #225 already fixes. - `Twist2.ad`, which is broken and needs a design decision. ## Testing - New `tests/test_twist_sequence_predicates.py` has 8 cases. All 8 fail on #248's code and pass here. - Full suite: 375 passed, 3 skipped. - Black is clean on the changed files. - Rebased onto `master`, which now includes #248, #225 and #249. 🤖 Generated with [Claude Code](https://claude.com/claude-code) --------- Co-authored-by: Claude Sonnet 5.5 <noreply@anthropic.com>
1 parent 3c42f04 commit a3b6492

3 files changed

Lines changed: 148 additions & 49 deletions

File tree

‎spatialmath/pose2d.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -358,13 +358,13 @@ def Rand(
358358
359359
Return an SE2 instance with random rotation and translation.
360360
361-
- ``SE2.Rand()`` is a random SE(2) rotation.
361+
- ``SE2.Rand()`` is a random SE(2) pose.
362362
- ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random
363363
poses.
364364
365365
Example, create random ten vehicles in the xy-plane::
366366
367-
>>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2])
367+
>>> x = SE2.Rand(N=10, xrange=[-2,2], yrange=[-2,2])
368368
>>> len(x)
369369
10
370370

‎spatialmath/twist.py‎

Lines changed: 88 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,7 @@ def isprismatic(self):
105105
if len(self) == 1:
106106
return smb.iszerovec(self.w)
107107
else:
108-
return [smb.iszerovec(x.w) for x in self.data]
108+
return [smb.iszerovec(x.w) for x in self]
109109

110110
@property
111111
def isrevolute(self):
@@ -131,7 +131,7 @@ def isrevolute(self):
131131
if len(self) == 1:
132132
return smb.iszerovec(self.v)
133133
else:
134-
return [smb.iszerovec(x.v) for x in self.data]
134+
return [smb.iszerovec(x.v) for x in self]
135135

136136
@property
137137
def isunit(self):
@@ -296,13 +296,30 @@ class Twist3(BaseTwist):
296296
3D rigid body transformation which is the unique elements of the Lie
297297
algebra se(3) of the corresponding SE(3) matrix.
298298
299+
A twist can be used in two ways which share the same 6-vector, the same
300+
:math:`[\vec{v}, \omega]` layout and the same algebra:
301+
302+
- as a *displacement*, where ``exp(S)`` is the pose reached by following the
303+
screw motion and ``exp(S * theta)`` scales it. ``S`` is often a unit
304+
twist, see :meth:`unit`. This is the usage in Robotics, Vision & Control
305+
and in Modern Robotics.
306+
- as a *velocity*, where ``S`` has units of m/s and rad/s and ``exp(S * dt)``
307+
is the pose change over a time ``dt``. This is the meaning of ``Twist``
308+
in ROS. The typed counterpart, which also has acceleration, force and
309+
momentum, is :class:`~spatialmath.spatialvector.SpatialVelocity`.
310+
311+
Properties that describe the screw axis, such as :attr:`pitch`,
312+
:attr:`pole` and :meth:`line`, are unchanged by scaling the twist so apply
313+
to both. Those that depend on its magnitude, such as :attr:`theta`, are
314+
an angle in the first usage and a rate in the second.
315+
299316
:References:
300317
- Robotics, Vision & Control for Python, Section 2.3.2.3, P. Corke, Springer 2023.
301318
- Modern Robotics, Lynch & Park, Cambridge 2017
302319
303320
.. note:: Compared to Lynch & Park this module implements twist vectors
304321
with the translational components first, followed by rotational
305-
components, ie. :math:`[\omega, \vec{v}]`.
322+
components, ie. :math:`[\vec{v}, \omega]`.
306323
307324
"""
308325

@@ -364,8 +381,8 @@ def isvalid(v, check=True):
364381
"""
365382
Test if matrix is valid twist
366383
367-
:param x: array to test
368-
:type x: ndarray
384+
:param v: array to test
385+
:type v: ndarray
369386
:return: Whether the value is a 6-vector or a valid 4x4 se(3) element
370387
:rtype: bool
371388
@@ -538,7 +555,7 @@ def Rx(cls, theta, unit="rad"):
538555
:return: 3D twist vector
539556
:rtype: Twist3 instance
540557
541-
- ``Twist3.Rx(θ)`` is an SE(3) rotation of θ radians about the x-axis
558+
- ``Twist3.Rx(θ)`` is an se(3) rotation of θ radians about the x-axis
542559
- ``Twist3.Rx(θ, "deg")`` as above but θ is in degrees
543560
544561
If ``θ`` is an array then the result is a sequence of rotations defined
@@ -569,7 +586,7 @@ def Ry(cls, theta, unit="rad", t=None):
569586
:return: 3D twist vector
570587
:rtype: Twist3 instance
571588
572-
- ``Twist3.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis
589+
- ``Twist3.Ry(θ)`` is an se(3) rotation of θ radians about the y-axis
573590
- ``Twist3.Ry(θ, "deg")`` as above but θ is in degrees
574591
575592
If ``θ`` is an array then the result is a sequence of rotations defined
@@ -600,7 +617,7 @@ def Rz(cls, theta, unit="rad", t=None):
600617
:return: 3D twist vector
601618
:rtype: Twist3 instance
602619
603-
- ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis
620+
- ``Twist3.Rz(θ)`` is an se(3) rotation of θ radians about the z-axis
604621
- ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees
605622
606623
If ``θ`` is an array then the result is a sequence of rotations defined
@@ -765,14 +782,14 @@ def Rand(
765782
:type zrange: 2-element sequence, optional
766783
:param N: number of random transforms
767784
:type N: int
768-
:return: SE(3) matrix
769-
:rtype: SE3 instance
785+
:return: 3D twist
786+
:rtype: Twist3 instance
770787
771-
Return an SE3 instance with random rotation and translation.
788+
Return a Twist3 instance with random rotation and translation.
772789
773-
- ``SE3.Rand()`` is a random SE(3) translation.
774-
- ``SE3.Rand(N=N)`` is an SE3 object containing a sequence of N random
775-
poses.
790+
- ``Twist3.Rand()`` is a random se(3) twist.
791+
- ``Twist3.Rand(N=N)`` is a Twist3 object containing a sequence of N random
792+
twists.
776793
777794
Example:
778795
@@ -781,7 +798,7 @@ def Rand(
781798
>>> from spatialmath import Twist3
782799
>>> Twist3.Rand(N=2)
783800
784-
:seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand`
801+
:seealso: :meth:`~spatialmath.pose3d.SE3.Rand`
785802
"""
786803
from spatialmath.pose3d import SO3
787804

@@ -792,7 +809,7 @@ def Rand(
792809
low=yrange[0], high=yrange[1], size=N
793810
) # random values in the range
794811
Z = np.random.uniform(
795-
low=yrange[0], high=zrange[1], size=N
812+
low=zrange[0], high=zrange[1], size=N
796813
) # random values in the range
797814
R = SO3.Rand(N=N)
798815

@@ -1028,7 +1045,7 @@ def exp(self, theta=1, unit="rad"):
10281045
"""
10291046
Exponentiate a 3D twist
10301047
1031-
:param theta: rotation magnitude, defaults to None
1048+
:param theta: rotation magnitude, defaults to 1
10321049
:type theta: float, optional
10331050
:param units: rotational units, defaults to 'rad'
10341051
:type units: str, optional
@@ -1037,7 +1054,7 @@ def exp(self, theta=1, unit="rad"):
10371054
10381055
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
10391056
:math:`e^{[S]}`
1040-
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
1057+
- ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis,
10411058
:math:`e^{\theta[S]}`
10421059
10431060
If ``len(X)==1`` and ``len(θ)==N`` then the resulting SE3 object has
@@ -1173,7 +1190,7 @@ def __rmul__(
11731190
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
11741191
"""
11751192
if smb.isscalar(left):
1176-
return Twist3(right.S * left)
1193+
return Twist3(right.binop(left, lambda x, y: x * y))
11771194
else:
11781195
raise ValueError("Twist3 *, incorrect left operand")
11791196

@@ -1188,10 +1205,10 @@ def __str__(self):
11881205
11891206
Example:
11901207
1191-
.. runblock: pycon
1208+
.. runblock:: pycon
11921209
11931210
>>> from spatialmath import Twist3
1194-
>>> x = Twist3.R([1,2,3], [4,5,6])
1211+
>>> x = Twist3.UnitRevolute([1,2,3], [4,5,6])
11951212
>>> print(x)
11961213
"""
11971214
return "\n".join(
@@ -1212,13 +1229,11 @@ def __repr__(self):
12121229
12131230
Example:
12141231
1215-
.. runblock: pycon
1232+
.. runblock:: pycon
12161233
12171234
>>> from spatialmath import Twist3
1218-
>>> x = Twist3.R([1,2,3], [4,5,6])
1235+
>>> x = Twist3.UnitRevolute([1,2,3], [4,5,6])
12191236
>>> x
1220-
>>> a.append(a)
1221-
>>> a
12221237
12231238
"""
12241239
if len(self) == 0:
@@ -1265,12 +1280,25 @@ def _repr_pretty_(self, p, cycle):
12651280

12661281

12671282
class Twist2(BaseTwist):
1283+
r"""
1284+
2D twist class
1285+
1286+
A Twist2 object holds the parameters of a twist, a representation of a
1287+
2D rigid body transformation which is the unique elements of the Lie
1288+
algebra se(2) of the corresponding SE(2) matrix. It is a 3-vector
1289+
:math:`[\vec{v}, \omega]`.
1290+
1291+
As for :class:`Twist3` it can be used as a *displacement*, where ``exp(S)``
1292+
is the pose reached by following the motion, or as a *velocity*, where
1293+
``exp(S * dt)`` is the pose change over a time ``dt``. Properties that
1294+
describe the axis, such as :attr:`pole`, are unchanged by scaling the twist.
1295+
"""
1296+
12681297
def __init__(self, arg=None, w=None, check=True):
12691298
r"""
12701299
Construct a new 2D Twist object
12711300
1272-
:type a: 2-element array-like
1273-
:return: 2D prismatic twist
1301+
:return: 2D twist
12741302
:rtype: Twist2 instance
12751303
12761304
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
@@ -1280,15 +1308,15 @@ def __init__(self, arg=None, w=None, check=True):
12801308
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
12811309
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
12821310
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
1283-
moment vector (1 element) and direction vector (2 elements).
1311+
moment vector (2 elements) and direction vector (1 element).
12841312
12851313
:References:
12861314
- Robotics, Vision & Control for Python, Section 2.2.2.4, P. Corke, Springer 2023.
12871315
- Modern Robotics, Lynch & Park, Cambridge 2017
12881316
12891317
.. note:: Compared to Lynch & Park this module implements twist vectors
12901318
with the translational components first, followed by rotational
1291-
components, ie. :math:`[\omega, \vec{v}]`.
1319+
components, ie. :math:`[\vec{v}, \omega]`.
12921320
"""
12931321
from spatialmath.pose2d import SE2
12941322

@@ -1338,12 +1366,12 @@ def isvalid(v, check=True):
13381366
"""
13391367
Test if matrix is valid twist
13401368
1341-
:param x: array to test
1342-
:type x: ndarray
1369+
:param v: array to test
1370+
:type v: ndarray
13431371
:return: Whether the value is a 3-vector or a valid 3x3 se(2) element
13441372
:rtype: bool
13451373
1346-
A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix,
1374+
A twist can be represented by a 3-vector or a 3x3 augmented skew symmetric matrix,
13471375
for example:
13481376
13491377
.. runblock:: pycon
@@ -1379,7 +1407,7 @@ def UnitRevolute(cls, q):
13791407
13801408
:param q: Point on the line of action
13811409
:type q: array_like(2)
1382-
:return: 2D prismatic twist
1410+
:return: 2D revolute twist
13831411
:rtype: Twist2 instance
13841412
13851413
- ``Twist2.UnitRevolute(q)`` is a 2D Twist object representing rotation about the 2D point ``q``.
@@ -1523,7 +1551,7 @@ def SE2(self, theta=1, unit="rad"):
15231551
Convert 2D twist to SE(2) matrix
15241552
15251553
:return: an SE(2) representation
1526-
:rtype: SE3 instance
1554+
:rtype: SE2 instance
15271555
15281556
``S.SE2()`` is an SE2 object representing the homogeneous transformation
15291557
equivalent to the Twist2. This is the exponentiation of the twist vector.
@@ -1536,12 +1564,12 @@ def SE2(self, theta=1, unit="rad"):
15361564
>>> S = Twist2.UnitPrismatic([1,2])
15371565
>>> S.SE2()
15381566
1539-
:seealso: :func:`Twist3.exp`
1567+
:seealso: :func:`Twist2.exp`
15401568
"""
15411569
from spatialmath.pose2d import SE2
15421570

15431571
if unit != "rad" and any(s.isprismatic for s in self):
1544-
print("Twist3.exp: using degree mode for a prismatic twist")
1572+
print("Twist2.SE2: using degree mode for a prismatic twist")
15451573

15461574
theta = smb.getunit(theta, unit)
15471575

@@ -1584,7 +1612,7 @@ def exp(self, theta=1, unit="rad"):
15841612
r"""
15851613
Exponentiate a 2D twist
15861614
1587-
:param theta: rotation magnitude, defaults to None
1615+
:param theta: rotation magnitude, defaults to 1
15881616
:type theta: float, optional
15891617
:param unit: rotational units, defaults to 'rad'
15901618
:type unit: str, optional
@@ -1593,7 +1621,7 @@ def exp(self, theta=1, unit="rad"):
15931621
15941622
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
15951623
:math:`e^{[S]}`
1596-
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
1624+
- ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis,
15971625
:math:`e^{\theta[S]}`
15981626
15991627
Example:
@@ -1728,7 +1756,7 @@ def Tx(cls, x):
17281756
:return: 2D twist vector
17291757
:rtype: Twist2 instance
17301758
1731-
`Twist2.Tx(x)` is an se(2) translation of ``x`` along the x-axis
1759+
``Twist2.Tx(x)`` is an se(2) translation of ``x`` along the x-axis
17321760
17331761
Example:
17341762
@@ -1754,7 +1782,7 @@ def Ty(cls, y):
17541782
:return: 2D twist vector
17551783
:rtype: Twist2 instance
17561784
1757-
`Twist2.Ty(y) is an se(2) translation of ``y`` along the y-axis
1785+
``Twist2.Ty(y)`` is an se(2) translation of ``y`` along the y-axis
17581786
17591787
Example:
17601788
@@ -1833,11 +1861,25 @@ def __mul__(
18331861
else:
18341862
raise ValueError("Twist2 *, incorrect right operand")
18351863

1836-
def __rmul(self, left):
1864+
def __rmul__(
1865+
right, left
1866+
): # lgtm[py/not-named-self] pylint: disable=no-self-argument
1867+
"""
1868+
Overloaded ``*`` operator
1869+
1870+
:arg right: right multiplicand
1871+
:arg left: left multiplicand
1872+
:return: product
1873+
:raises: ValueError
1874+
1875+
Left-multiplication by a scalar
1876+
1877+
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
1878+
"""
18371879
if smb.isscalar(left):
1838-
return Twist2(self.S * left)
1880+
return Twist2(right.binop(left, lambda x, y: x * y))
18391881
else:
1840-
raise ValueError("twist *, incorrect left operand")
1882+
raise ValueError("Twist2 *, incorrect left operand")
18411883

18421884
def __str__(self):
18431885
"""
@@ -1850,8 +1892,9 @@ def __str__(self):
18501892
18511893
Example:
18521894
1853-
.. runblock: pycon
1895+
.. runblock:: pycon
18541896
1897+
>>> from spatialmath import Twist2
18551898
>>> x = Twist2([1,2,3])
18561899
>>> print(x)
18571900
"""
@@ -1866,13 +1909,11 @@ def __repr__(self):
18661909
18671910
Example:
18681911
1869-
.. runblock: pycon
1912+
.. runblock:: pycon
18701913
18711914
>>> from spatialmath import Twist2
18721915
>>> x = Twist2([1,2,3])
18731916
>>> x
1874-
>>> a.append(a)
1875-
>>> a
18761917
18771918
"""
18781919

0 commit comments

Comments
 (0)