Skip to content

Commit bcc0d6b

Browse files
petercorkeclaude
authored andcommitted
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 <noreply@anthropic.com>
1 parent 824a459 commit bcc0d6b

3 files changed

Lines changed: 117 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: 57 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):
@@ -302,7 +302,7 @@ class Twist3(BaseTwist):
302302
303303
.. note:: Compared to Lynch & Park this module implements twist vectors
304304
with the translational components first, followed by rotational
305-
components, ie. :math:`[\omega, \vec{v}]`.
305+
components, ie. :math:`[\vec{v}, \omega]`.
306306
307307
"""
308308

@@ -364,8 +364,8 @@ def isvalid(v, check=True):
364364
"""
365365
Test if matrix is valid twist
366366
367-
:param x: array to test
368-
:type x: ndarray
367+
:param v: array to test
368+
:type v: ndarray
369369
:return: Whether the value is a 6-vector or a valid 4x4 se(3) element
370370
:rtype: bool
371371
@@ -538,7 +538,7 @@ def Rx(cls, theta, unit="rad"):
538538
:return: 3D twist vector
539539
:rtype: Twist3 instance
540540
541-
- ``Twist3.Rx(θ)`` is an SE(3) rotation of θ radians about the x-axis
541+
- ``Twist3.Rx(θ)`` is an se(3) rotation of θ radians about the x-axis
542542
- ``Twist3.Rx(θ, "deg")`` as above but θ is in degrees
543543
544544
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):
569569
:return: 3D twist vector
570570
:rtype: Twist3 instance
571571
572-
- ``Twist3.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis
572+
- ``Twist3.Ry(θ)`` is an se(3) rotation of θ radians about the y-axis
573573
- ``Twist3.Ry(θ, "deg")`` as above but θ is in degrees
574574
575575
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):
600600
:return: 3D twist vector
601601
:rtype: Twist3 instance
602602
603-
- ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis
603+
- ``Twist3.Rz(θ)`` is an se(3) rotation of θ radians about the z-axis
604604
- ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees
605605
606606
If ``θ`` is an array then the result is a sequence of rotations defined
@@ -765,14 +765,14 @@ def Rand(
765765
:type zrange: 2-element sequence, optional
766766
:param N: number of random transforms
767767
:type N: int
768-
:return: SE(3) matrix
769-
:rtype: SE3 instance
768+
:return: 3D twist
769+
:rtype: Twist3 instance
770770
771-
Return an SE3 instance with random rotation and translation.
771+
Return a Twist3 instance with random rotation and translation.
772772
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.
773+
- ``Twist3.Rand()`` is a random se(3) twist.
774+
- ``Twist3.Rand(N=N)`` is a Twist3 object containing a sequence of N random
775+
twists.
776776
777777
Example:
778778
@@ -781,7 +781,7 @@ def Rand(
781781
>>> from spatialmath import Twist3
782782
>>> Twist3.Rand(N=2)
783783
784-
:seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand`
784+
:seealso: :meth:`~spatialmath.pose3d.SE3.Rand`
785785
"""
786786
from spatialmath.pose3d import SO3
787787

@@ -792,7 +792,7 @@ def Rand(
792792
low=yrange[0], high=yrange[1], size=N
793793
) # random values in the range
794794
Z = np.random.uniform(
795-
low=yrange[0], high=zrange[1], size=N
795+
low=zrange[0], high=zrange[1], size=N
796796
) # random values in the range
797797
R = SO3.Rand(N=N)
798798

@@ -1028,7 +1028,7 @@ def exp(self, theta=1, unit="rad"):
10281028
"""
10291029
Exponentiate a 3D twist
10301030
1031-
:param theta: rotation magnitude, defaults to None
1031+
:param theta: rotation magnitude, defaults to 1
10321032
:type theta: float, optional
10331033
:param units: rotational units, defaults to 'rad'
10341034
:type units: str, optional
@@ -1037,7 +1037,7 @@ def exp(self, theta=1, unit="rad"):
10371037
10381038
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
10391039
:math:`e^{[S]}`
1040-
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
1040+
- ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis,
10411041
:math:`e^{\theta[S]}`
10421042
10431043
If ``len(X)==1`` and ``len(θ)==N`` then the resulting SE3 object has
@@ -1165,7 +1165,7 @@ def __rmul__(
11651165
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
11661166
"""
11671167
if smb.isscalar(left):
1168-
return Twist3(right.S * left)
1168+
return Twist3(right.binop(left, lambda x, y: x * y))
11691169
else:
11701170
raise ValueError("Twist3 *, incorrect left operand")
11711171

@@ -1180,10 +1180,10 @@ def __str__(self):
11801180
11811181
Example:
11821182
1183-
.. runblock: pycon
1183+
.. runblock:: pycon
11841184
11851185
>>> from spatialmath import Twist3
1186-
>>> x = Twist3.R([1,2,3], [4,5,6])
1186+
>>> x = Twist3.UnitRevolute([1,2,3], [4,5,6])
11871187
>>> print(x)
11881188
"""
11891189
return "\n".join(
@@ -1204,13 +1204,11 @@ def __repr__(self):
12041204
12051205
Example:
12061206
1207-
.. runblock: pycon
1207+
.. runblock:: pycon
12081208
12091209
>>> from spatialmath import Twist3
1210-
>>> x = Twist3.R([1,2,3], [4,5,6])
1210+
>>> x = Twist3.UnitRevolute([1,2,3], [4,5,6])
12111211
>>> x
1212-
>>> a.append(a)
1213-
>>> a
12141212
12151213
"""
12161214
if len(self) == 0:
@@ -1261,8 +1259,7 @@ def __init__(self, arg=None, w=None, check=True):
12611259
r"""
12621260
Construct a new 2D Twist object
12631261
1264-
:type a: 2-element array-like
1265-
:return: 2D prismatic twist
1262+
:return: 2D twist
12661263
:rtype: Twist2 instance
12671264
12681265
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
@@ -1272,15 +1269,15 @@ def __init__(self, arg=None, w=None, check=True):
12721269
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
12731270
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
12741271
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
1275-
moment vector (1 element) and direction vector (2 elements).
1272+
moment vector (2 elements) and direction vector (1 element).
12761273
12771274
:References:
12781275
- Robotics, Vision & Control for Python, Section 2.2.2.4, P. Corke, Springer 2023.
12791276
- Modern Robotics, Lynch & Park, Cambridge 2017
12801277
12811278
.. note:: Compared to Lynch & Park this module implements twist vectors
12821279
with the translational components first, followed by rotational
1283-
components, ie. :math:`[\omega, \vec{v}]`.
1280+
components, ie. :math:`[\vec{v}, \omega]`.
12841281
"""
12851282
from spatialmath.pose2d import SE2
12861283

@@ -1330,12 +1327,12 @@ def isvalid(v, check=True):
13301327
"""
13311328
Test if matrix is valid twist
13321329
1333-
:param x: array to test
1334-
:type x: ndarray
1330+
:param v: array to test
1331+
:type v: ndarray
13351332
:return: Whether the value is a 3-vector or a valid 3x3 se(2) element
13361333
:rtype: bool
13371334
1338-
A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix,
1335+
A twist can be represented by a 3-vector or a 3x3 augmented skew symmetric matrix,
13391336
for example:
13401337
13411338
.. runblock:: pycon
@@ -1371,7 +1368,7 @@ def UnitRevolute(cls, q):
13711368
13721369
:param q: Point on the line of action
13731370
:type q: array_like(2)
1374-
:return: 2D prismatic twist
1371+
:return: 2D revolute twist
13751372
:rtype: Twist2 instance
13761373
13771374
- ``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"):
15151512
Convert 2D twist to SE(2) matrix
15161513
15171514
:return: an SE(2) representation
1518-
:rtype: SE3 instance
1515+
:rtype: SE2 instance
15191516
15201517
``S.SE2()`` is an SE2 object representing the homogeneous transformation
15211518
equivalent to the Twist2. This is the exponentiation of the twist vector.
@@ -1528,12 +1525,12 @@ def SE2(self, theta=1, unit="rad"):
15281525
>>> S = Twist2.UnitPrismatic([1,2])
15291526
>>> S.SE2()
15301527
1531-
:seealso: :func:`Twist3.exp`
1528+
:seealso: :func:`Twist2.exp`
15321529
"""
15331530
from spatialmath.pose2d import SE2
15341531

15351532
if unit != "rad" and any(s.isprismatic for s in self):
1536-
print("Twist3.exp: using degree mode for a prismatic twist")
1533+
print("Twist2.SE2: using degree mode for a prismatic twist")
15371534

15381535
theta = smb.getunit(theta, unit)
15391536

@@ -1576,7 +1573,7 @@ def exp(self, theta=1, unit="rad"):
15761573
r"""
15771574
Exponentiate a 2D twist
15781575
1579-
:param theta: rotation magnitude, defaults to None
1576+
:param theta: rotation magnitude, defaults to 1
15801577
:type theta: float, optional
15811578
:param unit: rotational units, defaults to 'rad'
15821579
:type unit: str, optional
@@ -1585,7 +1582,7 @@ def exp(self, theta=1, unit="rad"):
15851582
15861583
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
15871584
:math:`e^{[S]}`
1588-
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
1585+
- ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis,
15891586
:math:`e^{\theta[S]}`
15901587
15911588
Example:
@@ -1720,7 +1717,7 @@ def Tx(cls, x):
17201717
:return: 2D twist vector
17211718
:rtype: Twist2 instance
17221719
1723-
`Twist2.Tx(x)` is an se(2) translation of ``x`` along the x-axis
1720+
``Twist2.Tx(x)`` is an se(2) translation of ``x`` along the x-axis
17241721
17251722
Example:
17261723
@@ -1746,7 +1743,7 @@ def Ty(cls, y):
17461743
:return: 2D twist vector
17471744
:rtype: Twist2 instance
17481745
1749-
`Twist2.Ty(y) is an se(2) translation of ``y`` along the y-axis
1746+
``Twist2.Ty(y)`` is an se(2) translation of ``y`` along the y-axis
17501747
17511748
Example:
17521749
@@ -1825,11 +1822,25 @@ def __mul__(
18251822
else:
18261823
raise ValueError("Twist2 *, incorrect right operand")
18271824

1828-
def __rmul(self, left):
1825+
def __rmul__(
1826+
right, left
1827+
): # lgtm[py/not-named-self] pylint: disable=no-self-argument
1828+
"""
1829+
Overloaded ``*`` operator
1830+
1831+
:arg right: right multiplicand
1832+
:arg left: left multiplicand
1833+
:return: product
1834+
:raises: ValueError
1835+
1836+
Left-multiplication by a scalar
1837+
1838+
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
1839+
"""
18291840
if smb.isscalar(left):
1830-
return Twist2(self.S * left)
1841+
return Twist2(right.binop(left, lambda x, y: x * y))
18311842
else:
1832-
raise ValueError("twist *, incorrect left operand")
1843+
raise ValueError("Twist2 *, incorrect left operand")
18331844

18341845
def __str__(self):
18351846
"""
@@ -1842,8 +1853,9 @@ def __str__(self):
18421853
18431854
Example:
18441855
1845-
.. runblock: pycon
1856+
.. runblock:: pycon
18461857
1858+
>>> from spatialmath import Twist2
18471859
>>> x = Twist2([1,2,3])
18481860
>>> print(x)
18491861
"""
@@ -1858,13 +1870,11 @@ def __repr__(self):
18581870
18591871
Example:
18601872
1861-
.. runblock: pycon
1873+
.. runblock:: pycon
18621874
18631875
>>> from spatialmath import Twist2
18641876
>>> x = Twist2([1,2,3])
18651877
>>> x
1866-
>>> a.append(a)
1867-
>>> a
18681878
18691879
"""
18701880

0 commit comments

Comments
 (0)