10000 continue refactoring · Robertleoj/spatialmath-python@9a8ab0f · GitHub
[go: up one dir, main page]
Skip to content

Commit 9a8ab0f

Browse files
RobertleojRobertleoj
committed
continue refactoring
1 parent bd8b82d commit 9a8ab0f

File tree

2 files changed

+53
-47
lines changed

2 files changed

+53
-47
lines changed

‎spatialmath/DualQuaternion.py

Lines changed: 29 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
from spatialmath import base
55
import spatialmath.pose3d as pose3d
66
from spatialmath.base.types import ArrayLike3, R8x8, R8
7-
from typing import Self, overload
7+
from typing import overload
88

99
# TODO scalar multiplication
1010

@@ -77,7 +77,7 @@ def __init__(
7777
raise ValueError("expecting zero or two parameters")
7878

7979
@classmethod
80-
def Pure(cls, x: ArrayLike3) -> Self:
80+
def Pure(cls, x: ArrayLike3) -> DualQuaternion:
8181
x = base.getvector(x, 3)
8282
return cls(UnitQuaternion(), Quaternion.Pure(x))
8383

@@ -123,7 +123,7 @@ def norm(self) -> tuple[float, float]:
123123
b = self.real * self.dual.conj() + self.dual * self.real.conj()
124124
return (base.sqrt(a.s), base.sqrt(b.s))
125125

126-
def conj(self) -> Self:
126+
def conj(self) -> DualQuaternion:
127127
r"""
128128
Conjugate of dual quaternion
129129
@@ -144,7 +144,7 @@ def conj(self) -> Self:
144144
"""
145145
return DualQuaternion(self.real.conj(), self.dual.conj())
146146

147-
def __add__(left, right: DualQuaternion) -> Self: # pylint: disable=no-self-argument
147+
def __add__(self: DualQuaternion, right: DualQuaternion) -> DualQuaternion:
148148
"""
149149
Sum of two dual quaternions
150150
@@ -159,9 +159,12 @@ def __add__(left, right: DualQuaternion) -> Self: # pylint: disable=no-self-arg
159159
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
160160
>>> d + d
161161
"""
162-
return DualQuaternion(left.real + right.real, left.dual + right.dual)
162+
assert self.real is not None and self.dual is not None
163+
assert right.real is not None and right.dual is not None
163164

164-
def __sub__(left, right: DualQuaternion) -> Self: # pylint: disable=no-self-argument
165+
return DualQuaternion(self.real + right.real, self.dual + right.dual)
166+
167+
def __sub__(self, right: DualQuaternion) -> DualQuaternion:
165168
"""
166169
Difference of two dual quaternions
167170
@@ -176,9 +179,12 @@ def __sub__(left, right: DualQuaternion) -> Self: # pylint: disable=no-self-arg
176179
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
177180
>>> d - d
178181
"""
179-
return DualQuaternion(left.real - right.real, left.dual - right.dual)
182+
assert self.real is not None and self.dual is not None
183+
assert right.real is not None and right.dual is not None
184+
185+
return DualQuaternion(self.real - right.real, self.dual - right.dual)
180186

181-
def __mul__(left, right: Self) -> Self: # pylint: disable=no-self-argument
187+
def __mul__(self: DualQuaternion, right: DualQuaternion) -> DualQuaternion:
182188
"""
183189
Product of dual quaternion
184190
@@ -196,19 +202,23 @@ def __mul__(left, right: Self) -> Self: # pylint: disable=no-self-argument
196202
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
197203
>>> d * d
198204
"""
205+
assert self.real is not None and self.dual is not None
206+
assert right.real is not None and right.dual is not None
207+
199208
if isinstance(right, DualQuaternion):
200-
real = left.real * right.real
201-
dual = left.real * right.dual + left.dual * right.real
209+
real = self.real * right.real
210+
dual = self.real * right.dual + self.dual * right.real
202211

203-
if isinstance(left, UnitDualQuaternion) and isinstance(
204-
left, UnitDualQuaternion
212+
if isinstance(self, UnitDualQuaternion) and isinstance(
213+
self, UnitDualQuaternion
205214
):
206215
return UnitDualQuaternion(real, dual)
207216
else:
208217
return DualQuaternion(real, dual)
209-
elif isinstance(left, UnitDualQuaternion) and base.isvector(right, 3):
218+
219+
elif isinstance(self, UnitDualQuaternion) and base.isvector(right, 3):
210220
v = base.getvector(right, 3)
211-
vp = left * DualQuaternion.Pure(v) * left.conj()
221+
vp = self * DualQuaternion.Pure(v) * self.conj()
212222
return vp.dual.v
213223

214224
def matrix(self) -> R8x8:
@@ -231,6 +241,8 @@ def matrix(self) -> R8x8:
231241
>>> d.matrix() @ d.vec
232242
>>> d * d
233243
"""
244+
assert self.real is not None and self.dual is not None
245+
234246
return np.block(
235247
[[self.real.matrix, np.zeros((4, 4))], [self.dual.matrix, self.real.matrix]]
236248
)
@@ -251,10 +263,9 @@ def vec(self) -> R8:
251263
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
252264
>>> d.vec
253265
"""
254-
return np.r_[self.real.vec, self.dual.vec]
266+
assert self.real is not None and self.dual is not None
255267

256-
# def log(self):
257-
# pass
268+
return np.r_[self.real.vec, self.dual.vec]
258269

259270

260271
class UnitDualQuaternion(DualQuaternion):
@@ -270,13 +281,7 @@ class UnitDualQuaternion(DualQuaternion):
270281
:seealso: :func:`UnitDualQuaternion`
271282
"""
272283

273-
@overload
274-
def __init__(self, T: pose3d.SE3): ...
275-
276-
@overload
277-
def __init__(self, real: Quaternion, dual: Quaternion): ...
278-
279-
def __init__(self, real=None, dual=None):
284+
def __init__(self, real: Quaternion | None = None, dual: Quaternion | None = None) -> None:
280285
r"""
281286
Create new unit dual quaternion
282287

‎spatialmath/quaternion.py

Lines changed: 24 additions & 23 deletions
Original F438 file line numberDiff line numberDiff line change
@@ -483,8 +483,8 @@ def inner(self, other) -> float:
483483
# -------------------------------------------- operators
484484

485485
def __eq__(
486-
left, right: Quaternion
487-
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
486+
self: Quaternion, right: Quaternion
487+
) -> bool:
488488
"""
489489
Overloaded ``==`` operator
490490
@@ -507,12 +507,12 @@ def __eq__(
507507
508508
:seealso: :func:`__ne__` :func:`~spatialmath.base.quaternions.qisequal`
509509
"""
510-
assert isinstance(left, type(right)), "operands to == are of different types"
511-
return left.binop(right, smb.qisequal, list1=False)
510+
assert isinstance(self, type(right)), "operands to == are of different types"
511+
return self.binop(right, smb.qisequal, list1=False)
512512

513513
def __ne__(
514-
left, right: Quaternion
515-
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
514+
self: Quaternion, right: Quaternion
515+
) -> bool:
516516
"""
517517
Overloaded ``!=`` operator
518518
@@ -534,12 +534,12 @@ def __ne__(
534534
535535
:seealso: :func:`__ne__` :func:`~spatialmath.base.quaternions.qisequal`
536536
"""
537-
assert isinstance(left, type(right)), "operands to == are of different types"
538-
return left.binop(right, lambda x, y: not smb.qisequal(x, y), list1=False)
537+
assert isinstance(self, type(right)), "operands to == are of different types"
538+
return self.binop(right, lambda x, y: not smb.qisequal(x, y), list1=False)
539539

540540
def __mul__(
541-
left, right: Quaternion
542-
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
541+
self: Quaternion, right: Quaternion
542+
) -> Quaternion:
543543
"""
544544
Overloaded ``*`` operator
545545
@@ -588,21 +588,21 @@ def __mul__(
588588
589589
:seealso: :func:`__rmul__` :func:`__imul__` :func:`~spatialmath.base.quaternions.qqmul`
590590
"""
591-
if isinstance(right, left.__class__):
591+
if isinstance(right, self.__class__):
592592
# quaternion * [unit]quaternion case
593-
return Quaternion(left.binop(right, smb.qqmul))
593+
return Quaternion(self.binop(right, smb.qqmul))
594594

595595
elif smb.isscalar(right):
596596
# quaternion * scalar case
597597
# print('scalar * quat')
598-
return Quaternion([right * q._A for q in left])
598+
return Quaternion([right * q._A for q in self])
599599

600600
else:
601601
raise ValueError("operands to * are of different types")
602602

603603
def __rmul__(
604-
right, left: Quaternion
605-
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
604+
self: Quaternion, right: Quaternion
605+
) -> Quaternion:
606606
"""
607607
Overloaded ``*`` operator
608608
@@ -623,11 +623,11 @@ def __rmul__(
623623
:seealso: :func:`__mul__`
624624
"""
625625
# scalar * quaternion case
626-
return Quaternion([left * q._A for q in right])
626+
return Quaternion([right * q._A for q in self])
627627

628628
def __imul__(
629-
left, right: Quaternion
630-
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
629+
self: Quaternion, right: Quaternion
630+
) -> Quaternion:
631631
"""
632632
Overloaded ``*=`` operator
633633
@@ -651,7 +651,7 @@ def __imul__(
651651
652652
:seealso: :func:`__mul__`
653653
"""
654-
return left.__mul__(right)
654+
return self.__mul__(right)
655655

656656
def __pow__(self, n: int) -> Quaternion:
657657
"""
@@ -758,8 +758,8 @@ def __add__(
758758
return Quaternion(left.binop(right, lambda x, y: x + y))
759759

760760
def __sub__(
761-
left, right: Quaternion
762-
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
761+
self: Quaternion, right: Quaternion
762+
) -> Quaternion:
763763
"""
764764
Overloaded ``-`` operator
765765
@@ -809,8 +809,8 @@ def __sub__(
809809
"""
810810
# results is not in the group, return an array, not a class
811811
# TODO allow class +/- a conformant array
812-
assert isinstance(left, type(right)), "operands to - are of different types"
813-
return Quaternion(left.binop(right, lambda x, y: x - y))
812+
assert isinstance(self, type(right)), "operands to - are of different types"
813+
return Quaternion(self.binop(right, lambda x, y: x - y))
814814

815815
def __neg__(self) -> Quaternion:
816816
r"""
@@ -1076,6 +1076,7 @@ def isvalid(x: ArrayLike, check: bool = True) -> bool:
10761076
>>> UnitQuaternion.isvalid(np.r_[1, 0, 0, 0])
10771077
>>> UnitQuaternion.isvalid(np.r_[1, 2, 3, 4])
10781078
"""
1079+
x = np.array(x)
10791080
return x.shape == (4,) and (not check or smb.isunitvec(x))
10801081

10811082
@property

0 commit comments

Comments
 (0)
0