Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit fd9e6a0

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

File tree

Expand file treeCollapse file tree

8 files changed

+65
-45
lines changed
Filter options
Expand file treeCollapse file tree

8 files changed

+65
-45
lines changed

‎spatialmath/base/quaternions.py

Copy file name to clipboardExpand all lines: spatialmath/base/quaternions.py
+11-9Lines 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

Copy file name to clipboardExpand all lines: spatialmath/base/transforms2d.py
+11-7Lines 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

Copy file name to clipboardExpand all lines: spatialmath/base/transforms3d.py
+1-1Lines 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

Copy file name to clipboardExpand all lines: spatialmath/base/transformsNd.py
+4-4Lines 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

Copy file name to clipboardExpand all lines: spatialmath/base/vectors.py
+25-17Lines 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
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

Copy file name to clipboardExpand all lines: spatialmath/baseposematrix.py
-2Lines 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)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.