8000 added eps as a module constant · navrobot/robotics-toolbox-python@99667bf · GitHub
[go: up one dir, main page]

Skip to content

Commit 99667bf

Browse files
committed
added eps as a module constant
changed doco to include array_like added unit tests at the end
1 parent 47cd4e3 commit 99667bf

File tree

1 file changed

+64
-52
lines changed

1 file changed

+64
-52
lines changed

spatialmath/base/transforms.py

Lines changed: 64 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,15 @@
33
"""
44
This modules contains functions to create and transform rotation matrices
55
and homogeneous tranformation matrices.
6+
7+
Vector arguments are what numpy refers to as ``array_like`` and can be a list,
8+
tuple, numpy array, numpy row vector or numpy column vector.
9+
10+
Versions:
11+
12+
1. Luis Fernando Lara Tobar and Peter Corke, 2008
13+
2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017
14+
3. Peter Corke, 2020
615
"""
716

817
import sys
@@ -24,7 +33,8 @@ def issymbol(x):
2433
except:
2534
def issymbol(x):
2635
return False
27-
36+
37+
_eps = np.finfo(np.float64).eps
2838

2939
def colvec(v):
3040
return np.array(v).reshape((len(v), 1))
@@ -137,8 +147,8 @@ def trotx(theta, unit="rad", t=None):
137147
:type theta: float
138148
:param unit: angular units: 'rad' [default], or 'deg'
139149
:type unit: str
140-
:param t: the translation, defaults to [0,0,0]
141-
:return: 4x4 homogeneous transformation matrix
150+
:param t: translation 3-vector, defaults to [0,0,0]
151+
:type t: array_like :return: 4x4 homogeneous transformation matrix
142152
:rtype: numpy.ndarray, shape=(4,4)
143153
144154
- ``trotx(THETA)`` is a homogeneous transformation (4x4) representing a rotation
@@ -162,7 +172,8 @@ def troty(theta, unit="rad", t=None):
162172
:type theta: float
163173
:param unit: angular units: 'rad' [default], or 'deg'
164174
:type unit: str
165-
:param t: the translation, defaults to [0,0,0]
175+
:param t: translation 3-vector, defaults to [0,0,0]
176+
:type t: array_like
166177
:return: 4x4 homogeneous transformation matrix as a numpy array
167178
:rtype: numpy.ndarray, shape=(4,4)
168179
@@ -187,7 +198,8 @@ def trotz(theta, unit="rad", t=None):
187198
:type theta: float
188199
:param unit: angular units: 'rad' [default], or 'deg'
189200
:type unit: str
190-
:param t: the translation, defaults to [0,0,0]
201+
:param t: translation 3-vector, defaults to [0,0,0]
202+
:type t: array_like
191203
:return: 4x4 homogeneous transformation matrix
192204
:rtype: numpy.ndarray, shape=(4,4)
193205
@@ -279,8 +291,8 @@ def trot2(theta, unit='rad', t=None):
279291
:type theta: float
280292
:param unit: angular units: 'rad' [default], or 'deg'
281293
:type unit: str
282-
:param t: the translation, defaults to [0,0]
283-
:return: 3x3 homogeneous transformation matrix
294+
:param t: translation 2-vector, defaults to [0,0]
295+
:type t: array_like :return: 3x3 homogeneous transformation matrix
284296
:rtype: numpy.ndarray, shape=(3,3)
285297
286298
- ``TROT2(THETA)`` is a homogeneous transformation (3x3) representing a rotation of
@@ -343,7 +355,8 @@ def unit(v):
343355
"""
344356
Create a unit vector
345357
346-
:param v: n-dimensional vector as a list, dict, or a numpy array, row or column vector
358+
:param v: n-dimensional vector
359+
:type v: array_like
347360
:return: a unit-vector parallel to V.
348361
:rtype: numpy.ndarray
349362
:raises ValueError: for zero length vector
@@ -357,7 +370,7 @@ def unit(v):
357370
v = argcheck.getvector(v)
358371
n = np.linalg.norm(v)
359372

360-
if n > 100*np.finfo(np.float64).eps: # if greater than eps
373+
if n > 100*_eps: # if greater than eps
361374
return v / n
362375
else:
363376
raise ValueError("Vector has zero norm")
@@ -390,7 +403,7 @@ def isunitvec(v, tol=10):
390403
391404
:seealso: unit, isunittwist
392405
"""
393-
return abs(np.linalg.norm(v)-1) < tol*np.finfo(np.float64).eps
406+
return abs(np.linalg.norm(v)-1) < tol*_eps
394407

395408
def iszerovec(v, tol=10):
396409
"""
@@ -405,15 +418,15 @@ def iszerovec(v, tol=10):
405418
406419
:seealso: unit, isunittwist
407420
"""
408-
return np.linalg.norm(v) < tol*np.finfo(np.float64).eps
421+
return np.linalg.norm(v) < tol*_eps
409422

410423

411424
def isunittwist(v, tol=10):
412425
r"""
413426
Test if vector represents a unit twist in SE(2) or SE(3)
414427
415428
:param v: vector to test
416-
:type v: list, tuple, or numpy.ndarray
429+
:type v: array_like
417430
:param tol: tolerance in units of eps
418431
:type tol: float
419432
:return: whether vector has unit length
@@ -433,10 +446,10 @@ def isunittwist(v, tol=10):
433446

434447
if len(v) == 3:
435448
# test for SE(2) twist
436-
return isunitvec(v[2], tol=tol) or (np.abs(v[2]) < tol*np.finfo(np.float64).eps and isunitvec(v[0:2], tol=tol))
449+
return isunitvec(v[2], tol=tol) or (np.abs(v[2]) < tol*_eps and isunitvec(v[0:2], tol=tol))
437450
elif len(v) == 6:
438451
# test for SE(3) twist
439-
return isunitvec(v[3:6], tol=tol) or (np.linalg.norm(v[3:6]) < tol*np.finfo(np.float64).eps and isunitvec(v[0:3], tol=tol))
452+
return isunitvec(v[3:6], tol=tol) or (np.linalg.norm(v[3:6]) < tol*_eps and isunitvec(v[0:3], tol=tol))
440453
else:
441454
raise ValueError
442455

@@ -464,7 +477,7 @@ def r2t(R, check=False):
464477
dim = R.shape
465478
assert dim[0] == dim[1], 'Matrix must be square'
466479

467-
if check and np.abs(np.linalg.det(R) - 1) < 100*np.finfo(np.float64).eps:
480+
if check and np.abs(np.linalg.det(R) - 1) < 100*_eps:
468481
raise ValueError('Invalid rotation matrix ')
469482

470483
T = np.pad( R, (0,1) )
@@ -565,7 +578,7 @@ def rt2tr(R, t, check=False):
565578
t = argcheck.getvector(t, dim=None, out='array')
566579
if R.shape[0] != t.shape[0]:
567580
raise ValueError("R and t must have the same number of rows")
568-
if check and np.abs(np.linalg.det(R) - 1) < 100*np.finfo(np.float64).eps:
581+
if check and np.abs(np.linalg.det(R) - 1) < 100*_eps:
569582
raise ValueError('Invalid rotation matrix')
570583

571584
if R.shape == (2, 2):
@@ -599,7 +612,7 @@ def isR(R, tol = 10):
599612
600613
:seealso: isrot2, isrot
601614
"""
602-
return np.linalg.norm( R@R.T - np.eye(R.shape[0]) ) < tol*np.finfo(np.float64).eps \
615+
return np.linalg.norm( R@R.T - np.eye(R.shape[0]) ) < tol*_eps \
603616
and np.linalg.det( R@R.T ) > 0
604617

605618
def ishom(T, check=False):
@@ -692,7 +705,7 @@ def isskew(S, tol = 10):
692705
693706
:seealso: isskewa
694707
"""
695-
return np.linalg.norm(S + S.T) < tol*np.finfo(np.float64).eps
708+
return np.linalg.norm(S + S.T) < tol*_eps
696709

697710
def isskewa(S, tol = 10):
698711
r"""
@@ -711,7 +724,7 @@ def isskewa(S, tol = 10):
711724
712725
:seealso: isskew
713726
"""
714-
return np.linalg.norm(S[0:-1,0:-1] + S[0:-1,0:-1].T) < tol*np.finfo(np.float64).eps \
727+
return np.linalg.norm(S[0:-1,0:-1] + S[0:-1,0:-1].T) < tol*_eps \
715728
and np.all(S[-1,:] == 0)
716729

717730
def iseye(S, tol = 10):
@@ -733,7 +746,7 @@ def iseye(S, tol = 10):
733746
s = S.shape
734747
if len(s) != 2 or s[0] != s[1]:
735748
return False # not a square matrix
736-
return abs(S.trace() - s[0]) < tol * np.finfo(np.float64).eps
749+
return abs(S.trace() - s[0]) < tol * _eps
737750

738751

739752

@@ -772,7 +785,7 @@ def rpy2r(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
772785
to the optic axis and x-axis parallel to the pixel rows.
773786
774787
- ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
775-
from ``RPY`` which is a 3-vector (list, tuple, numpy.ndarray) with values
788+
from ``RPY`` which is a 3-vector (array_like) with values
776789
(ROLL, PITCH, YAW).
777790
"""
778791

@@ -821,14 +834,14 @@ def rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
821834
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
822835
and y-axis sideways.
823836
- 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
824-
then by roll about the new z-axis. Covention for a robot gripper with z-axis forward
837+
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
825838
and y-axis between the gripper fingers.
826839
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
827840
then by roll about the new z-axis. Convention for a camera with z-axis parallel
828841
to the optic axis and x-axis parallel to the pixel rows.
829842
830843
- ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken
831-
from ``RPY`` which is a 3-vector (list, tuple, numpy.ndarray) with values
844+
from ``RPY`` which is a 3-vector (array_like) with values
832845
(ROLL, PITCH, YAW).
833846
834847
Notes:
@@ -859,7 +872,7 @@ def eul2r(phi, theta=None, psi=None, unit='rad'):
859872
matrix equivalent to the specified Euler angles. These correspond
860873
to rotations about the Z, Y, Z axes respectively.
861874
- ``R = eul2r(EUL)`` as above but the Euler angles are taken from
862-
``EUL`` which is a 3-vector (list, tuple, numpy.ndarray) with values
875+
``EUL`` which is a 3-vector (array_like) with values
863876
(PHI THETA PSI).
864877
"""
865878

@@ -893,7 +906,7 @@ def eul2tr(phi, theta=None, psi=None, unit='rad'):
893906
matrix equivalent to the specified Euler angles. These correspond
894907
to rotations about the Z, Y, Z axes respectively.
895908
- ``R = eul2tr(EUL)`` as above but the Euler angles are taken from
896-
``EUL`` which is a 3-vector (list, tuple, numpy.ndarray) with values
909+
``EUL`` which is a 3-vector (array_like) with values
897910
(PHI THETA PSI).
898911
899912
Notes:
@@ -913,8 +926,8 @@ def angvec2r(theta, v, unit='rad'):
913926
:type theta: float
914927
:param unit: angular units: 'rad' [default], or 'deg'
915928
:type unit: str
916-
:param v: rotation axis
917-
:type v: 3-vector: list, tuple, numpy.ndarray
929+
:param v: rotation axis, 3-vector
930+
:type v: array_like
918931
:return: 3x3 rotation matrix
919932
:rtype: numpdy.ndarray, shape=(3,3)
920933
@@ -928,7 +941,7 @@ def angvec2r(theta, v, unit='rad'):
928941
"""
929942
assert np.isscalar(theta) and argcheck.isvector(v, 3), "Arguments must be theta and vector"
930943

931-
if np.linalg.norm(v) < 10*np.finfo(np.float64).eps:
944+
if np.linalg.norm(v) < 10*_eps:
932945
return np.eye(3)
933946

934947
theta = argcheck.getunit(theta, unit)
@@ -949,8 +962,8 @@ def angvec2tr(theta, v, unit='rad'):
949962
:type theta: float
950963
:param unit: angular units: 'rad' [default], or 'deg'
951964
:type unit: str
952-
:param v: rotation axis
953-
:type v: 3-vector: list, tuple, numpy.ndarray
965+
:param v: rotation axis, 3-vector
966+
:type v: : array_like
954967
:return: 4x4 homogeneous transformation matrix
955968
:rtype: numpdy.ndarray, shape=(4,4)
956969
@@ -972,9 +985,9 @@ def oa2r(o, a=None):
972985
Create SO(3) rotation matrix from two vectors
973986
974987
:param o: 3-vector parallel to Y- axis
975-
:type o: list, tuple, numpy.ndarray
988+
:type o: array_like
976989
:param a: 3-vector parallel to the Z-axis
977-
:type o: list, tuple, numpy.ndarray
990+
:type o: array_like
978991
:return: 3x3 rotation matrix
979992
:rtype: numpy.ndarray, shape=(3,3)
980993
@@ -1012,9 +1025,9 @@ def oa2tr(o, a=None):
10121025
Create SE(3) pure rotation from two vectors
10131026
10141027
:param o: 3-vector parallel to Y- axis
1015-
:type o: list, tuple, numpy.ndarray
1028+
:type o: array_like
10161029
:param a: 3-vector parallel to the Z-axis
1017-
:type o: list, tuple, numpy.ndarray
1030+
:type o: array_like
10181031
:return: 4x4 homogeneous transformation matrix
10191032
:rtype: numpy.ndarray, shape=(4,4)
10201033
@@ -1125,7 +1138,7 @@ def tr2eul(T, unit='rad', flip=False, check=False):
11251138
assert isrot(R, check=check)
11261139

11271140
eul = np.zeros((3,))
1128-
if abs(R[0,2]) < 10*np.finfo(np.float64).eps and abs(R[1,2]) < 10*np.finfo(np.float64).eps:
1141+
if abs(R[0,2]) < 10*_eps and abs(R[1,2]) < 10*_eps:
11291142
eul[0] = 0
11301143
sp = 0
11311144
cp = 1
@@ -1192,7 +1205,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
11921205
if order == 'xyz' or order == 'arm':
11931206

11941207
# XYZ order
1195-
if abs(abs(R[0,2]) - 1) < 10*np.finfo(np.float64).eps: # when |R13| == 1
1208+
if abs(abs(R[0,2]) - 1) < 10*_eps: # when |R13| == 1
11961209
# singularity
11971210
rpy[0] = 0 # roll is zero
11981211
if R[0,2] > 0:
@@ -1218,7 +1231,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
12181231
elif order == 'zyx' or order == 'vehicle':
12191232

12201233
# old ZYX order (as per Paul book)
1221-
if abs(abs(R[2,0]) - 1) < 10*np.finfo(np.float64).eps: # when |R31| == 1
1234+
if abs(abs(R[2,0]) - 1) < 10*_eps: # when |R31| == 1
12221235
# singularity
12231236
rpy[0] = 0 # roll is zero
12241237
if R[2,0] < 0:
@@ -1242,7 +1255,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
12421255

12431256
elif order == 'yxz' or order == 'camera':
12441257

1245-
if abs(abs(R[1,2]) - 1) < 10*np.finfo(np.float64).eps: # when |R23| == 1
1258+
if abs(abs(R[1,2]) - 1) < 10*_eps: # when |R23| == 1
12461259
# singularity
12471260
rpy[0] = 0
12481261
if R[1,2] < 0:
@@ -1279,7 +1292,7 @@ def skew(v):
12791292
Create skew-symmetric metrix from vector
12801293
12811294
:param v: 1- or 3-vector
1282-
:type v: list, tuple or numpy.ndarray
1295+
:type v: array_like
12831296
:return: skew-symmetric matrix in so(2) or so(3)
12841297
:rtype: numpy.ndarray, shape=(2,2) or (3,3)
12851298
:raises: ValueError
@@ -1350,7 +1363,7 @@ def skewa(v):
13501363
Create augmented skew-symmetric metrix from vector
13511364
13521365
:param v: 3- or 6-vector
1353-
:type v: list, tuple or numpy.ndarray
1366+
:type v: array_like
13541367
:return: augmented skew-symmetric matrix in se(2) or se(3)
13551368
:rtype: numpy.ndarray, shape=(3,3) or (4,4)
13561369
:raises: ValueError
@@ -1465,7 +1478,7 @@ def trlog(T, check=True):
14651478
if iseye(R):
14661479
# matrix is identity
14671480
return np.zeros((3,3))
1468-
elif abs(R[0,0] + 1) < 100 * np.finfo(np.float64).eps:
1481+
elif abs(R[0,0] + 1) < 100 * _eps:
14691482
# tr R = -1
14701483
# rotation by +/- pi, +/- 3pi etc.
14711484
mx = R.diagonal().max()
@@ -1506,10 +1519,10 @@ def trexp(S, theta=None):
15061519
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
15071520
given by ``THETA``.
15081521
- ``trexp(W)`` is the matrix exponential of the so(3) element ``W`` expressed as
1509-
a 3-vector (list, tuple, numpy.ndarray).
1522+
a 3-vector (array_like).
15101523
- ``trexp(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
15111524
unit-norm vector representing a rotation axis and a rotation magnitude
1512-
given by ``THETA``. ``W`` is expressed as a 3-vector (list, tuple, numpy.ndarray).
1525+
given by ``THETA``. ``W`` is expressed as a 3-vector (array_like).
15131526
15141527
15151528
For se(3) the results is an SE(3) homogeneous transformation matrix:
@@ -1589,7 +1602,7 @@ def trexp2(S, theta=None):
15891602
Exponential of so(2) or se(2) matrix
15901603
15911604
:param S: so(2), se(2) matrix or equivalent velctor
1592-
:type T: numpy.ndarray, shape=(2,2), (1,), (3,3), or (3,)
1605+
:type T: numpy.ndarray, shape=(2,2) or (3,3); array_like
15931606
:param theta: motion
15941607
:type theta: float
15951608
:return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2)
@@ -1606,10 +1619,10 @@ def trexp2(S, theta=None):
16061619
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
16071620
given by ``THETA``.
16081621
- ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as
1609-
a 1-vector (list, tuple, numpy.ndarray).
1622+
a 1-vector (array_like).
16101623
- ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
16111624
unit-norm vector representing a rotation axis and a rotation magnitude
1612-
given by ``THETA``. ``W`` is expressed as a 1-vector (list, tuple, numpy.ndarray).
1625+
given by ``THETA``. ``W`` is expressed as a 1-vector (array_like).
16131626
16141627
16151628
For se(2) the results is an SE(2) homogeneous transformation matrix:
@@ -1839,14 +1852,13 @@ def trprint2(T, label=None, file=sys.stdout, fmt='{:8.2g}', unit='deg'):
18391852
return s
18401853

18411854
def _vec2s(fmt, v):
1842-
v = [x if np.abs(x) > 100*np.finfo(np.float64).eps else 0.0 for x in v ]
1855+
v = [x if np.abs(x) > 100*_eps else 0.0 for x in v ]
18431856
return ', '.join([fmt.format(x) for x in v])
18441857

1845-
if __name__ == "main":
1846-
a=transl2(1,2)@trot2(0.3)
1847-
trprint2(a, file=None, label='T')
1858+
if __name__ == '__main__':
1859+
import pathlib
1860+
import os.path
18481861

1849-
T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg')
1850-
trprint(T, file=None, label='T')
1862+
runfile(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_transforms.py") )
18511863

18521864

0 commit comments

Comments
 (0)
0