3
3
"""
4
4
This modules contains functions to create and transform rotation matrices
5
5
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
6
15
"""
7
16
8
17
import sys
@@ -24,7 +33,8 @@ def issymbol(x):
24
33
except :
25
34
def issymbol (x ):
26
35
return False
27
-
36
+
37
+ _eps = np .finfo (np .float64 ).eps
28
38
29
39
def colvec (v ):
30
40
return np .array (v ).reshape ((len (v ), 1 ))
@@ -137,8 +147,8 @@ def trotx(theta, unit="rad", t=None):
137
147
:type theta: float
138
148
:param unit: angular units: 'rad' [default], or 'deg'
139
149
: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
142
152
:rtype: numpy.ndarray, shape=(4,4)
143
153
144
154
- ``trotx(THETA)`` is a homogeneous transformation (4x4) representing a rotation
@@ -162,7 +172,8 @@ def troty(theta, unit="rad", t=None):
162
172
:type theta: float
163
173
:param unit: angular units: 'rad' [default], or 'deg'
164
174
: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
166
177
:return: 4x4 homogeneous transformation matrix as a numpy array
167
178
:rtype: numpy.ndarray, shape=(4,4)
168
179
@@ -187,7 +198,8 @@ def trotz(theta, unit="rad", t=None):
187
198
:type theta: float
188
199
:param unit: angular units: 'rad' [default], or 'deg'
189
200
: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
191
203
:return: 4x4 homogeneous transformation matrix
192
204
:rtype: numpy.ndarray, shape=(4,4)
193
205
@@ -279,8 +291,8 @@ def trot2(theta, unit='rad', t=None):
279
291
:type theta: float
280
292
:param unit: angular units: 'rad' [default], or 'deg'
281
293
: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
284
296
:rtype: numpy.ndarray, shape=(3,3)
285
297
286
298
- ``TROT2(THETA)`` is a homogeneous transformation (3x3) representing a rotation of
@@ -343,7 +355,8 @@ def unit(v):
343
355
"""
344
356
Create a unit vector
345
357
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
347
360
:return: a unit-vector parallel to V.
348
361
:rtype: numpy.ndarray
349
362
:raises ValueError: for zero length vector
@@ -357,7 +370,7 @@ def unit(v):
357
370
v = argcheck .getvector (v )
358
371
n = np .linalg .norm (v )
359
372
360
- if n > 100 * np . finfo ( np . float64 ). eps : # if greater than eps
373
+ if n > 100 * _eps : # if greater than eps
361
374
return v / n
362
375
else :
363
376
raise ValueError ("Vector has zero norm" )
@@ -390,7 +403,7 @@ def isunitvec(v, tol=10):
390
403
391
404
:seealso: unit, isunittwist
392
405
"""
393
- return abs (np .linalg .norm (v )- 1 ) < tol * np . finfo ( np . float64 ). eps
406
+ return abs (np .linalg .norm (v )- 1 ) < tol * _eps
394
407
395
408
def iszerovec (v , tol = 10 ):
396
409
"""
@@ -405,15 +418,15 @@ def iszerovec(v, tol=10):
405
418
406
419
:seealso: unit, isunittwist
407
420
"""
408
- return np .linalg .norm (v ) < tol * np . finfo ( np . float64 ). eps
421
+ return np .linalg .norm (v ) < tol * _eps
409
422
410
423
411
424
def isunittwist (v , tol = 10 ):
412
425
r"""
413
426
Test if vector represents a unit twist in SE(2) or SE(3)
414
427
415
428
:param v: vector to test
416
- :type v: list, tuple, or numpy.ndarray
429
+ :type v: array_like
417
430
:param tol: tolerance in units of eps
418
431
:type tol: float
419
432
:return: whether vector has unit length
@@ -433,10 +446,10 @@ def isunittwist(v, tol=10):
433
446
434
447
if len (v ) == 3 :
435
448
# 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 ))
437
450
elif len (v ) == 6 :
438
451
# 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 ))
440
453
else :
441
454
raise ValueError
442
455
@@ -464,7 +477,7 @@ def r2t(R, check=False):
464
477
dim = R .shape
465
478
assert dim [0 ] == dim [1 ], 'Matrix must be square'
466
479
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 :
468
481
raise ValueError ('Invalid rotation matrix ' )
469
482
470
483
T = np .pad ( R , (0 ,1 ) )
@@ -565,7 +578,7 @@ def rt2tr(R, t, check=False):
565
578
t = argcheck .getvector (t , dim = None , out = 'array' )
566
579
if R .shape [0 ] != t .shape [0 ]:
567
580
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 :
569
582
raise ValueError ('Invalid rotation matrix' )
570
583
571
584
if R .shape == (2 , 2 ):
@@ -599,7 +612,7 @@ def isR(R, tol = 10):
599
612
600
613
:seealso: isrot2, isrot
601
614
"""
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 \
603
616
and np .linalg .det ( R @R .T ) > 0
604
617
605
618
def ishom (T , check = False ):
@@ -692,7 +705,7 @@ def isskew(S, tol = 10):
692
705
693
706
:seealso: isskewa
694
707
"""
695
- return np .linalg .norm (S + S .T ) < tol * np . finfo ( np . float64 ). eps
708
+ return np .linalg .norm (S + S .T ) < tol * _eps
696
709
697
710
def isskewa (S , tol = 10 ):
698
711
r"""
@@ -711,7 +724,7 @@ def isskewa(S, tol = 10):
711
724
712
725
:seealso: isskew
713
726
"""
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 \
715
728
and np .all (S [- 1 ,:] == 0 )
716
729
717
730
def iseye (S , tol = 10 ):
@@ -733,7 +746,7 @@ def iseye(S, tol = 10):
733
746
s = S .shape
734
747
if len (s ) != 2 or s [0 ] != s [1 ]:
735
748
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
737
750
738
751
739
752
@@ -772,7 +785,7 @@ def rpy2r(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
772
785
to the optic axis and x-axis parallel to the pixel rows.
773
786
774
787
- ``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
776
789
(ROLL, PITCH, YAW).
777
790
"""
778
791
@@ -821,14 +834,14 @@ def rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
821
834
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
822
835
and y-axis sideways.
823
836
- '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
825
838
and y-axis between the gripper fingers.
826
839
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
827
840
then by roll about the new z-axis. Convention for a camera with z-axis parallel
828
841
to the optic axis and x-axis parallel to the pixel rows.
829
842
830
843
- ``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
832
845
(ROLL, PITCH, YAW).
833
846
834
847
Notes:
@@ -859,7 +872,7 @@ def eul2r(phi, theta=None, psi=None, unit='rad'):
859
872
matrix equivalent to the specified Euler angles. These correspond
860
873
to rotations about the Z, Y, Z axes respectively.
861
874
- ``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
863
876
(PHI THETA PSI).
864
877
"""
865
878
@@ -893,7 +906,7 @@ def eul2tr(phi, theta=None, psi=None, unit='rad'):
893
906
matrix equivalent to the specified Euler angles. These correspond
894
907
to rotations about the Z, Y, Z axes respectively.
895
908
- ``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
897
910
(PHI THETA PSI).
898
911
899
912
Notes:
@@ -913,8 +926,8 @@ def angvec2r(theta, v, unit='rad'):
913
926
:type theta: float
914
927
:param unit: angular units: 'rad' [default], or 'deg'
915
928
: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
918
931
:return: 3x3 rotation matrix
919
932
:rtype: numpdy.ndarray, shape=(3,3)
920
933
@@ -928,7 +941,7 @@ def angvec2r(theta, v, unit='rad'):
928
941
"""
929
942
assert np .isscalar (theta ) and argcheck .isvector (v , 3 ), "Arguments must be theta and vector"
930
943
931
- if np .linalg .norm (v ) < 10 * np . finfo ( np . float64 ). eps :
944
+ if np .linalg .norm (v ) < 10 * _eps :
932
945
return np .eye (3 )
933
946
934
947
theta = argcheck .getunit (theta , unit )
@@ -949,8 +962,8 @@ def angvec2tr(theta, v, unit='rad'):
949
962
:type theta: float
950
963
:param unit: angular units: 'rad' [default], or 'deg'
951
964
: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
954
967
:return: 4x4 homogeneous transformation matrix
955
968
:rtype: numpdy.ndarray, shape=(4,4)
956
969
@@ -972,9 +985,9 @@ def oa2r(o, a=None):
972
985
Create SO(3) rotation matrix from two vectors
973
986
974
987
:param o: 3-vector parallel to Y- axis
975
- :type o: list, tuple, numpy.ndarray
988
+ :type o: array_like
976
989
:param a: 3-vector parallel to the Z-axis
977
- :type o: list, tuple, numpy.ndarray
990
+ :type o: array_like
978
991
:return: 3x3 rotation matrix
979
992
:rtype: numpy.ndarray, shape=(3,3)
980
993
@@ -1012,9 +1025,9 @@ def oa2tr(o, a=None):
1012
1025
Create SE(3) pure rotation from two vectors
1013
1026
1014
1027
:param o: 3-vector parallel to Y- axis
1015
- :type o: list, tuple, numpy.ndarray
1028
+ :type o: array_like
1016
1029
:param a: 3-vector parallel to the Z-axis
1017
- :type o: list, tuple, numpy.ndarray
1030
+ :type o: array_like
1018
1031
:return: 4x4 homogeneous transformation matrix
1019
1032
:rtype: numpy.ndarray, shape=(4,4)
1020
1033
@@ -1125,7 +1138,7 @@ def tr2eul(T, unit='rad', flip=False, check=False):
1125
1138
assert isrot (R , check = check )
1126
1139
1127
1140
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 :
1129
1142
eul [0 ] = 0
1130
1143
sp = 0
1131
1144
cp = 1
@@ -1192,7 +1205,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
1192
1205
if order == 'xyz' or order == 'arm' :
1193
1206
1194
1207
# 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
1196
1209
# singularity
1197
1210
rpy [0 ] = 0 # roll is zero
1198
1211
if R [0 ,2 ] > 0 :
@@ -1218,7 +1231,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
1218
1231
elif order == 'zyx' or order == 'vehicle' :
1219
1232
1220
1233
# 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
1222
1235
# singularity
1223
1236
rpy [0 ] = 0 # roll is zero
1224
1237
if R [2 ,0 ] < 0 :
@@ -1242,7 +1255,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
1242
1255
1243
1256
elif order == 'yxz' or order == 'camera' :
1244
1257
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
1246
1259
# singularity
1247
1260
rpy [0 ] = 0
1248
1261
if R [1 ,2 ] < 0 :
@@ -1279,7 +1292,7 @@ def skew(v):
1279
1292
Create skew-symmetric metrix from vector
1280
1293
1281
1294
:param v: 1- or 3-vector
1282
- :type v: list, tuple or numpy.ndarray
1295
+ :type v: array_like
1283
1296
:return: skew-symmetric matrix in so(2) or so(3)
1284
1297
:rtype: numpy.ndarray, shape=(2,2) or (3,3)
1285
1298
:raises: ValueError
@@ -1350,7 +1363,7 @@ def skewa(v):
1350
1363
Create augmented skew-symmetric metrix from vector
1351
1364
1352
1365
:param v: 3- or 6-vector
1353
- :type v: list, tuple or numpy.ndarray
1366
+ :type v: array_like
1354
1367
:return: augmented skew-symmetric matrix in se(2) or se(3)
1355
1368
:rtype: numpy.ndarray, shape=(3,3) or (4,4)
1356
1369
:raises: ValueError
@@ -1465,7 +1478,7 @@ def trlog(T, check=True):
1465
1478
if iseye (R ):
1466
1479
# matrix is identity
1467
1480
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 :
1469
1482
# tr R = -1
1470
1483
# rotation by +/- pi, +/- 3pi etc.
1471
1484
mx = R .diagonal ().max ()
@@ -1506,10 +1519,10 @@ def trexp(S, theta=None):
1506
1519
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
1507
1520
given by ``THETA``.
1508
1521
- ``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 ).
1510
1523
- ``trexp(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
1511
1524
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 ).
1513
1526
1514
1527
1515
1528
For se(3) the results is an SE(3) homogeneous transformation matrix:
@@ -1589,7 +1602,7 @@ def trexp2(S, theta=None):
1589
1602
Exponential of so(2) or se(2) matrix
1590
1603
1591
1604
: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
1593
1606
:param theta: motion
1594
1607
:type theta: float
1595
1608
:return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2)
@@ -1606,10 +1619,10 @@ def trexp2(S, theta=None):
1606
1619
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
1607
1620
given by ``THETA``.
1608
1621
- ``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 ).
1610
1623
- ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
1611
1624
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 ).
1613
1626
1614
1627
1615
1628
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'):
1839
1852
return s
1840
1853
1841
1854
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 ]
1843
1856
return ', ' .join ([fmt .format (x ) for x in v ])
1844
1857
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
1848
1861
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" ) )
1851
1863
1852
1864
0 commit comments