8000 Standardizing tolerance uses (#104) · petercorke/spatialmath-python@fd9e6a0 · GitHub
[go: up one dir, main page]
Skip to content

Commit fd9e6a0

Browse files
jcao-bdaijcao-bdai
authored
Standardizing tolerance uses (bdaiinstitute#104)
1 parent f57437a commit fd9e6a0

File tree

8 files changed

+65
-45
lines changed

8 files changed

+65
-45
lines changed

‎spatialmath/base/quaternions.py

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -106,12 +106,14 @@ def qnorm(q: ArrayLike4) -> float:
106106
return np.linalg.norm(q)
107107

108108

109-
def qunit(q: ArrayLike4, tol: Optional[float] = 10) -> UnitQuaternionArray:
109+
def qunit(q: ArrayLike4, tol: float = 10) -> UnitQuaternionArray:
110110
"""
111111
Create a unit quaternion
112112
113113
:arg v: quaterion
114114
:type v: array_like(4)
115+
:param tol: Tolerance in multiples of eps, defaults to 10
116+
:type tol: float, optional
115117
:return: a pure quaternion
116118
:rtype: ndarray(4)
117119
:raises ValueError: quaternion has (near) zero norm
@@ -144,13 +146,13 @@ def qunit(q: ArrayLike4, tol: Optional[float] = 10) -> UnitQuaternionArray:
144146
return -q
145147

146148

147-
def qisunit(q: ArrayLike4, tol: Optional[float] = 100) -> bool:
149+
def qisunit(q: ArrayLike4, tol: float = 100) -> bool:
148150
"""
149151
Test if quaternion has unit length
150152
151153
:param v: quaternion
152154
:type v: array_like(4)
153-
:param tol: tolerance in units of eps
155+
:param tol: tolerance in units of eps, defaults to 100
154156
:type tol: float
155157
:return: whether quaternion has unit length
156158
:rtype: bool
@@ -172,7 +174,7 @@ def qisunit(q: ArrayLike4, tol: Optional[float] = 100) -> bool:
172174
def qisequal(
173175
q1: ArrayLike4,
174176
q2: ArrayLike4,
175-
tol: Optional[float] = 100,
177+
tol: float = 100,
176178
unitq: Optional[bool] = False,
177179
) -> bool:
178180
...
@@ -182,13 +184,13 @@ def qisequal(
182184
def qisequal(
183185
q1: ArrayLike4,
184186
q2: ArrayLike4,
185-
tol: Optional[float] = 100,
187+
tol: float = 100,
186188
unitq: Optional[bool] = True,
187189
) -> bool:
188190
...
189191

190192

191-
def qisequal(q1, q2, tol: Optional[float] = 100, unitq: Optional[bool] = False):
193+
def qisequal(q1, q2, tol: float = 100, unitq: Optional[bool] = False):
192194
"""
193195
Test if quaternions are equal
194196
@@ -198,7 +200,7 @@ def qisequal(q1, q2, tol: Optional[float] = 100, unitq: Optional[bool] = False):
198200
:type q2: array_like(4)
199201
:param unitq: quaternions are unit quaternions
200202
:type unitq: bool
201-
:param tol: tolerance in units of eps
203+
:param tol: tolerance in units of eps, defaults to 100
202204
:type tol: float
203205
:return: whether quaternions are equal
204206
:rtype: bool
@@ -545,7 +547,7 @@ def q2r(
545547
def r2q(
546548
R: SO3Array,
547549
check: Optional[bool] = False,
548-
tol: Optional[float] = 100,
550+
tol: float = 100,
549551
order: Optional[str] = "sxyz",
550552
) -> UnitQuaternionArray:
551553
"""
@@ -555,7 +557,7 @@ def r2q(
555557
:type R: ndarray(3,3)
556558
:param check: check validity of rotation matrix, default False
557559
:type check: bool
558-
:param tol: tolerance in units of eps
560+
:param tol: tolerance in units of eps, defaults to 100
559561
:type tol: float
560562
:param order: the order of the returned quaternion elements. Must be 'sxyz' or
561563
'xyzs'. Defaults to 'sxyz'.

‎spatialmath/base/transforms2d.py

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -324,14 +324,16 @@ def pos2tr2(x, y=None):
324324
return T
325325

326326

327-
def ishom2(T: Any, check: bool = False) -> bool: # TypeGuard(SE2):
327+
def ishom2(T: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard(SE2):
328328
"""
329329
Test if matrix belongs to SE(2)
330330
331331
:param T: SE(2) matrix to test
332332
:type T: ndarray(3,3)
333333
:param check: check validity of rotation submatrix
334334
:type check: bool
335+
:param tol: Tolerance in units of eps for zero-rotation case, defaults to 100
336+
:type: float
335337
:return: whether matrix is an SE(2) homogeneous transformation matrix
336338
:rtype: bool
337339
@@ -356,18 +358,20 @@ def ishom2(T: Any, check: bool = False) -> bool: # TypeGuard(SE2):
356358
return (
357359
isinstance(T, np.ndarray)
358360
and T.shape == (3, 3)
359-
and (not check or (smb.isR(T[:2, :2]) and all(T[2, :] == np.array([0, 0, 1]))))
361+
and (not check or (smb.isR(T[:2, :2], tol=tol) and all(T[2, :] == np.array([0, 0, 1]))))
360362
)
361363

362364

363-
def isrot2(R: Any, check: bool = False) -> bool: # TypeGuard(SO2):
365+
def isrot2(R: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard(SO2):
364366
"""
365367
Test if matrix belongs to SO(2)
366368
367369
:param R: SO(2) matrix to test
368370
:type R: ndarray(3,3)
369371
:param check: check validity of rotation submatrix
370372
:type check: bool
373+
:param tol: Tolerance in units of eps for zero-rotation case, defaults to 100
374+
:type: float
371375
:return: whether matrix is an SO(2) rotation matrix
372376
:rtype: bool
373377
@@ -388,7 +392,7 @@ def isrot2(R: Any, check: bool = False) -> bool: # TypeGuard(SO2):
388392
389393
:seealso: isR, ishom2, isrot
390394
"""
391-
return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or smb.isR(R))
395+
return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or smb.isR(R, tol=tol))
392396

393397

394398
# ---------------------------------------------------------------------------------------#
@@ -514,10 +518,10 @@ def trlog2(
514518
:func:`~spatialmath.base.transformsNd.vexa`
515519
"""
516520

517-
if ishom2(T, check=check):
521+
if ishom2(T, check=check, tol=tol):
518522
# SE(2) matrix
519523

520-
if smb.iseye(T, tol):
524+
if smb.iseye(T, tol=tol):
521525
# is identity matrix
522526
if twist:
523527
return np.zeros((3,))
@@ -539,7 +543,7 @@ def trlog2(
539543
[[smb.skew(theta), tr[:, np.newaxis]], [np.zeros((1, 3))]]
540544
)
541545

542-
elif isrot2(T, check=check):
546+
elif isrot2(T, check=check, tol=tol):
543547
# SO(2) rotation matrix
544548
theta = math.atan(T[1, 0] / T[0, 0])
545549
if twist:

‎spatialmath/base/transforms3d.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1335,7 +1335,7 @@ def trlog(
13351335
[R, t] = tr2rt(T)
13361336

13371337
# S = trlog(R, check=False) # recurse
1338-
S = trlog(cast(SO3Array, R), check=False) # recurse
1338+
S = trlog(cast(SO3Array, R), check=False, tol=tol) # recurse
13391339
w = vex(S)
13401340
theta = norm(w)
13411341
if theta == 0:

‎spatialmath/base/transformsNd.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -359,7 +359,7 @@ def isR(R: NDArray, tol: float = 100) -> bool: # -> TypeGuard[SOnArray]:
359359
360360
:param R: matrix to test
361361
:type R: ndarray(2,2) or ndarray(3,3)
362-
:param tol: tolerance in units of eps
362+
:param tol: tolerance in units of eps, defaults to 100
363363
:type tol: float
364364
:return: whether matrix is a proper orthonormal rotation matrix
365365
:rtype: bool
@@ -388,7 +388,7 @@ def isskew(S: NDArray, tol: float = 10) -> bool: # -> TypeGuard[sonArray]:
388388
389389
:param S: matrix to test
390390
:type S: ndarray(2,2) or ndarray(3,3)
391-
:param tol: tolerance in units of eps
391+
:param tol: tolerance in units of eps, defaults to 10
392392
:type tol: float
393393
:return: whether matrix is a proper skew-symmetric matrix
394394
:rtype: bool
@@ -415,7 +415,7 @@ def isskewa(S: NDArray, tol: float = 10) -> bool: # -> TypeGuard[senArray]:
415415
416416
:param S: matrix to test
417417
:type S: ndarray(3,3) or ndarray(4,4)
418-
:param tol: tolerance in units of eps
418+
:param tol: tolerance in units of eps, defaults to 10
419419
:type tol: float
420420
:return: whether matrix is a proper skew-symmetric matrix
421421
:rtype: bool
@@ -445,7 +445,7 @@ def iseye(S: NDArray, tol: float = 10) -> bool:
445445
446446
:param S: matrix to test
447447
:type S: ndarray(n,n)
448-
:param tol: tolerance in units of eps
448+
:param tol: tolerance in units of eps, defaults to 10
449449
:type tol: float
450450
:return: whether matrix is a proper skew-symmetric matrix
451451
:rtype: bool

‎spatialmath/base/vectors.py

Lines changed: 25 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -142,12 +142,14 @@ def colvec(v: ArrayLike) -> NDArray:
142142
return np.array(v).reshape((len(v), 1))
143143

144144

145-
def unitvec(v: ArrayLike) -> NDArray:
145+
def unitvec(v: ArrayLike, tol: float = 100) -> NDArray:
146146
"""
147147
Create a unit vector
148148
149149
:param v: any vector
150150
:type v: array_like(n)
151+
:param tol: Tolerance in units of eps for zero-norm case, defaults to 100
152+
:type: float
151153
:return: a unit-vector parallel to ``v``.
152154
:rtype: ndarray(n)
153155
:raises ValueError: for zero length vector
@@ -166,7 +168,7 @@ def unitvec(v: ArrayLike) -> NDArray:
166168
v = getvector(v)
167169
n = norm(v)
168170

169-
if n > 100 * _eps: # if greater than eps
171+
if n > tol * _eps: # if greater than eps
170172
return v / n
171173
else:
172174
raise ValueError("zero norm vector")
@@ -178,6 +180,8 @@ def unitvec_norm(v: ArrayLike, tol: float = 100) -> Tuple[NDArray, float]:
178180
179181
:param v: any vector
180182
:type v: array_like(n)
183+
:param tol: Tolerance in units of eps for zero-norm case, defaults to 100
184+
:type: float
181185
:return: a unit-vector parallel to ``v`` and the norm
182186
:rtype: (ndarray(n), float)
183187
:raises ValueError: for zero length vector
@@ -208,7 +212,7 @@ def isunitvec(v: ArrayLike, tol: float = 10) -> bool:
208212
209213
:param v: vector to test
210214
:type v: ndarray(n)
211-
:param tol: tolerance in units of eps
215+
:param tol: tolerance in units of eps, defaults to 10
212216
:type tol: float
213217
:return: whether vector has unit length
214218
:rtype: bool
@@ -230,7 +234,7 @@ def iszerovec(v: ArrayLike, tol: float = 10) -> bool:
230234
231235
:param v: vector to test
232236
:type v: ndarray(n)
233-
:param tol: tolerance in units of eps
237+
:param tol: tolerance in units of eps, defaults to 10
234238
:type tol: float
235239
:return: whether vector has zero length
236240
:rtype: bool
@@ -252,7 +256,7 @@ def iszero(v: float, tol: float = 10) -> bool:
252256
253257
:param v: value to test
254258
:type v: float
255-
:param tol: tolerance in units of eps
259+
:param tol: tolerance in units of eps, defaults to 10
256260
:type tol: float
257261
:return: whether value is zero
258262
:rtype: bool
@@ -274,7 +278,7 @@ def isunittwist(v: ArrayLike6, tol: float = 10) -> bool:
274278
275279
:param v: twist vector to test
276280
:type v: array_like(6)
277-
:param tol: tolerance in units of eps
281+
:param tol: tolerance in units of eps, defaults to 10
278282
:type tol: float
279283
:return: whether twist has unit length
280284
:rtype: bool
@@ -302,7 +306,7 @@ def isunittwist(v: ArrayLike6, tol: float = 10) -> bool:
302306
if len(v) == 6:
303307
# test for SE(3) twist
304308
return isunitvec(v[3:6], tol=tol) or (
305-
bool(np.linalg.norm(v[3:6]) < tol * _eps) and isunitvec(v[0:3], tol=tol)
309+
iszerovec(v[3:6], tol=tol) and isunitvec(v[0:3], tol=tol)
306310
)
307311
else:
308312
raise ValueError
@@ -314,7 +318,7 @@ def isunittwist2(v: ArrayLike3, tol: float = 10) -> bool:
314318
315319
:param v: twist vector to test
316320
:type v: array_like(3)
317-
:param tol: tolerance in units of eps
321+
:param tol: tolerance in units of eps, defaults to 10
318322
:type tol: float
319323
:return: whether vector has unit length
320324
:rtype: bool
@@ -341,7 +345,7 @@ def isunittwist2(v: ArrayLike3, tol: float = 10) -> bool:
341345
if len(v) == 3:
342346
# test for SE(2) twist
343347
return isunitvec(v[2], tol=tol) or (
344-
np.abs(v[2]) < tol * _eps and isunitvec(v[0:2], tol=tol)
348+
iszero(v[2], tol=tol) and isunitvec(v[0:2], tol=tol)
345349
)
346350
else:
347351
raise ValueError
@@ -353,7 +357,7 @@ def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]:
353357
354358
:param S: twist vector
355359
:type S: array_like(6)
356-
:param tol: tolerance in units of eps
360+
:param tol: tolerance in units of eps, defaults to 10
357361
:type tol: float
358362
:return: unit twist
359363
:rtype: ndarray(6)
@@ -380,7 +384,7 @@ def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]:
380384
v = S[0:3]
381385
w = S[3:6]
382386

383-
if iszerovec(w):
387+
if iszerovec(w, tol=tol):
384388
th = norm(v)
385389
else:
386390
th = norm(w)
@@ -394,7 +398,7 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float
394398
395399
:param S: twist vector
396400
:type S: array_like(6)
397-
:param tol: tolerance in units of eps
401+
:param tol: tolerance in units of eps, defaults to 10
398402
:type tol: float
399403
:return: unit twist and scalar motion
400404
:rtype: tuple (ndarray(6), float)
@@ -425,20 +429,22 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float
425429
v = S[0:3]
426430
w = S[3:6]
427431

428-
if iszerovec(w):
432+
if iszerovec(w, tol=tol):
429433< 341A /td>
th = norm(v)
430434
else:
431435
th = norm(w)
432436

433437
return (S / th, th)
434438

435439

436-
def unittwist2(S: ArrayLike3) -> R3:
440+
def unittwist2(S: ArrayLike3, tol: float = 10) -> R3:
437441
"""
438442
Convert twist to unit twist
439443
440444
:param S: twist vector
441445
:type S: array_like(3)
446+
:param tol: tolerance in units of eps, defaults to 10
447+
:type tol: float
442448
:return: unit twist
443449
:rtype: ndarray(3)
444450
@@ -459,20 +465,22 @@ def unittwist2(S: ArrayLike3) -> R3:
459465
v = S[0:2]
460466
w = S[2]
461467

462-
if iszero(w):
468+
if iszero(w, tol=tol):
463469
th = norm(v)
464470
else:
465471
th = abs(w)
466472

467473
return S / th
468474

469475

470-
def unittwist2_norm(S: ArrayLike3) -> Tuple[R3, float]:
476+
def unittwist2_norm(S: ArrayLike3, tol: float = 10) -> Tuple[R3, float]:
471477
"""
472478
Convert twist to unit twist
473479
474480
:param S: twist vector
475481
:type S: array_like(3)
482+
:param tol: tolerance in units of eps, defaults to 10
483+
:type tol: float
476484
:return: unit twist and scalar motion
477485
:rtype: tuple (ndarray(3), float)
478486
@@ -493,7 +501,7 @@ def unittwist2_norm(S: ArrayLike3) -> Tuple[R3, float]:
493501
v = S[0:2]
494502
w = S[2]
495503

496-
if iszero(w):
504+
if iszero(w, tol=tol):
497505
th = norm(v)
498506
else:
499507
th = abs(w)

‎spatialmath/baseposematrix.py

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -882,8 +882,6 @@ def _string_color(self, color: Optional[bool] = False) -> str:
882882
883883
:param color: colorise the output, defaults to False
884884
:type color: bool, optional
885-
:param tol: zero values smaller than tol*eps, defaults to 10
886-
:type tol: float, optional
887885
:return: multiline matrix representation
888886
:rtype: str
889887

0 commit comments

Comments
 (0)
0