10
10
import numpy as np
11
11
import math
12
12
13
- from spatialmath .base import argcheck
13
+ from spatialmath .base import argcheck
14
14
import spatialmath .base as tr
15
15
from spatialmath import super_pose as sp
16
16
17
+
17
18
class SO2 (sp .SMPose ):
18
-
19
+
19
20
# SO2() identity matrix
20
21
# SO2(angle, unit)
21
22
# SO2( obj ) # deep copy
22
23
# SO2( np ) # make numpy object
23
24
# SO2( nplist ) # make from list of numpy objects
24
-
25
+
25
26
# constructor needs to take ndarray -> SO2, or list of ndarray -> SO2
26
- def __init__ (self , arg = None , * , unit = 'rad' ):
27
+ def __init__ (self , arg = None , * , unit = 'rad' ):
27
28
super ().__init__ () # activate the UserList semantics
28
-
29
+
29
30
if arg is None :
30
31
# empty constructor
31
32
self .data = [np .eye (2 )]
32
-
33
+
33
34
elif argcheck .isvector (arg ):
34
35
# SO2(value)
35
36
# SO2(list of values)
36
37
self .data = [tr .rot2 (x , unit ) for x in argcheck .getvector (arg )]
37
-
38
+
38
39
else :
39
40
super ().arghandler (arg )
40
41
41
42
@classmethod
42
- def rand (cls , * , range = [0 , 2 * math .pi ], unit = 'rad' , N = 1 ):
43
+ def rand (cls , * , range = [0 , 2 * math .pi ], unit = 'rad' , N = 1 ):
43
44
rand = np .random .uniform (low = range [0 ], high = range [1 ], size = N ) # random values in the range
44
45
return cls ([tr .rot2 (x ) for x in argcheck .getunit (rand , unit )])
45
-
46
+
46
47
@classmethod
47
48
def isvalid (self , x ):
48
49
return tr .isrot2 (x , check = True )
49
50
50
51
@property
51
52
def T (self ):
52
53
return SO2 (self .A .T )
53
-
54
+
54
55
def inv (self ):
55
56
return SO2 (self .A .T )
56
-
57
- # for symmetry with other
57
+
58
+ # for symmetry with other
58
59
@classmethod
59
60
def R (cls , theta , unit = 'rad' ):
60
61
return SO2 ([tr .rot1 (x , unit ) for x in argcheck .getvector (theta )])
61
-
62
+
62
63
@property
63
64
def angle (self ):
64
65
"""Returns angle of SO2 object matrices in unit radians"""
@@ -71,30 +72,31 @@ def angle(self):
71
72
elif len (angles ) > 1 :
72
73
return angles
73
74
75
+
74
76
class SE2 (sp .SMPose ):
75
77
# constructor needs to take ndarray -> SO2, or list of ndarray -> SO2
76
- def __init__ (self , x = None , y = None , theta = 0 , * , unit = 'rad' ):
78
+ def __init__ (self , x = None , y = None , theta = 0 , * , unit = 'rad' ):
77
79
super ().__init__ () # activate the UserList semantics
78
-
80
+
79
81
if x is None :
80
82
# empty constructor
81
83
self .data = [np .eye (3 )]
82
-
83
- elif all (map (lambda x : isinstance (x , (int ,float )), [x , y , theta ])):
84
+
85
+ elif all (map (lambda x : isinstance (x , (int , float )), [x , y , theta ])):
84
86
# SE2(x, y, theta)
85
- self .data = [tr .trot2 (theta , t = [x ,y ], unit = unit )]
86
-
87
+ self .data = [tr .trot2 (theta , t = [x , y ], unit = unit )]
88
+
87
89
elif argcheck .isvector (x ) and argcheck .isvector (y ) and argcheck .isvector (theta ):
88
90
# SE2(xvec, yvec, tvec)
89
91
xvec = argcheck .getvector (x )
90
92
yvec = argcheck .getvector (y , dim = len (xvec ))
91
93
tvec = argcheck .getvector (theta , dim = len (xvec ))
92
94
self .data = [tr .trot2 (_t , t = [_x , _y ]) for (_x , _y , _t ) in zip (xvec , yvec , argcheck .getunit (tvec , unit ))]
93
-
95
+
94
96
elif isinstance (x , np .ndarray ) and y is None and theta is None :
95
97
assert x .shape [1 ] == 3 , 'array argument must be Nx3'
96
98
self .data = [tr .trot2 (_t , t = [_x , _y ], unit = unit ) for (_x , _y , _t ) in x ]
97
-
99
+
98
100
else :
99
101
super ().arghandler (x )
100
102
@@ -103,33 +105,32 @@ def T(self):
103
105
raise NotImplemented ('transpose is not meaningful for SE3 object' )
104
106
105
107
@classmethod
106
- def rand (cls , * , xrange = [- 1 , 1 ], yrange = [- 1 , 1 ], trange = [0 , 2 * math .pi ], unit = 'rad' , N = 1 ):
108
+ def rand (cls , * , xrange = [- 1 , 1 ], yrange = [- 1 , 1 ], trange = [0 , 2 * math .pi ], unit = 'rad' , N = 1 ):
107
109
x = np .random .uniform (low = xrange [0 ], high = xrange [1 ], size = N ) # random values in the range
108
110
y = np .random .uniform (low = yrange [0 ], high = yrange [1 ], size = N ) # random values in the range
109
111
theta = np .random .uniform (low = trange [0 ], high = trange [1 ], size = N ) # random values in the range
110
- return cls ([tr .trot2 (t , t = [x ,y ]) for (t ,x , y ) in zip (x , y , argcheck .getunit (theta , unit ))])
111
-
112
+ return cls ([tr .trot2 (t , t = [x , y ]) for (t , x , y ) in zip (x , y , argcheck .getunit (theta , unit ))])
113
+
112
114
@classmethod
113
115
def isvalid (self , x ):
114
116
return tr .ishom2 (x , check = True )
115
117
116
118
@property
117
119
def t (self ):
118
- return self .A [:2 ,2 ]
119
-
120
+ return self .A [:2 , 2 ]
121
+
120
122
@property
121
123
def R (self ):
122
- return SO2 (self .A [:2 ,:2 ])
123
-
124
+ return SO2 (self .A [:2 , :2 ])
125
+
124
126
def inv (self ):
125
127
return SO2 (self .A .T )
126
128
ArithmeticError ()
127
-
129
+
128
130
129
131
if __name__ == '__main__' : # pragma: no cover
130
-
132
+
131
133
import pathlib
132
134
import os .path
133
-
134
- exec (open (os .path .join (pathlib .Path (__file__ ).parent .absolute (), "test_pose2d.py" )).read () )
135
-
135
+
136
+ exec (open (os .path .join (pathlib .Path (__file__ ).parent .absolute (), "test_pose2d.py" )).read ())
0 commit comments