8000 add property for theta, xyt · suddrey-qut/spatialmath-python@462be1b · GitHub
[go: up one dir, main page]
Skip to content

Commit 462be1b

Browse files
petercorkepetercorke
committed
add property for theta, xyt
more constructor options SE2 is now subclass of SO2 isvalid becomes staticmethod
1 parent 24cdbc7 commit 462be1b

File tree

1 file changed

+54
-42
lines changed

1 file changed

+54
-42
lines changed

‎spatialmath/pose2d.py

Lines changed: 54 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -29,13 +29,16 @@ def __init__(self, arg=None, *, unit='rad'):
2929

3030
if arg is None:
3131
# empty constructor
32-
self.data = [np.eye(2)]
32+
if type(self) is SO2:
33+
self.data = [np.eye(2)]
3334

3435
elif argcheck.isvector(arg):
3536
# SO2(value)
3637
# SO2(list of values)
3738
self.data = [tr.rot2(x, unit) for x in argcheck.getvector(arg)]
3839

40+
elif isinstance(arg, np.ndarray) and arg.shape == (2,2):
41+
self.data = [arg]
3942
else:
4043
super().arghandler(arg)
4144

@@ -44,61 +47,64 @@ def rand(cls, *, range=[0, 2 * math.pi], unit='rad', N=1):
4447
rand = np.random.uniform(low=range[0], high=range[1], size=N) # random values in the range
4548
return cls([tr.rot2(x) for x in argcheck.getunit(rand, unit)])
4649

47-
@classmethod
48-
def isvalid(self, x):
50+
@staticmethod
51+
def isvalid(x):
4952
return tr.isrot2(x, check=True)
5053

5154
@property
5255
def T(self):
5356
return SO2(self.A.T)
5457

58+
@property
5559
def inv(self):
56-
return SO2(self.A.T)
60+
if len(self) == 1:
61+
return SO2(self.A.T)
62+
else:
63+
return SO2([x.T for x in self.A])
5764

58-
# for symmetry with other
59-
@classmethod
60-
def R(cls, theta, unit='rad'):
61-
return SO2([tr.rot1(x, unit) for x in argcheck.getvector(theta)])
65+
@property
66+
def R(self):
67+
return self.A[:2, :2]
6268

6369
@property
64-
def angle(self):
70+
def theta(self):
6571
"""Returns angle of SO2 object matrices in unit radians"""
66-
angles = []
67-
for each_matrix in self:
68-
angles.append(math.atan2(each_matrix[1, 0], each_matrix[0, 0]))
69-
# TODO !! Return list be default ?
70-
if len(angles) == 1:
71-
return angles[0]
72-
elif len(angles) > 1:
73-
return angles
72+
if len(self) == 1:
73+
return math.atan2(self.A[1,0], self.A[0,0])
74+
else:
75+
return [math.atan2(x.A[1,0], x.A[0,0]) for x in self]
7476

7577

76-
class SE2(sp.SMPose):
78+
class SE2(SO2):
7779
# constructor needs to take ndarray -> SO2, or list of ndarray -> SO2
78-
def __init__(self, x=None, y=None, theta=0, *, unit='rad'):
80+
def __init__(self, x=None, y=None, theta=None, *, unit='rad'):
7981
super().__init__() # activate the UserList semantics
8082

81-
if x is None:
83+
if x is None and y is None and theta is None:
84+
# SE2()
8285
# empty constructor
8386
self.data = [np.eye(3)]
8487

85-
elif all(map(lambda x: isinstance(x, (int, float)), [x, y, theta])):
86-
# SE2(x, y, theta)
87-
self.data = [tr.trot2(theta, t=[x, y], unit=unit)]
88-
89-
elif argcheck.isvector(x) and argcheck.isvector(y) and argcheck.isvector(theta):
90-
# SE2(xvec, yvec, tvec)
91-
xvec = argcheck.getvector(x)
92-
yvec = argcheck.getvector(y, dim=len(xvec))
93-
tvec = argcheck.getvector(theta, dim=len(xvec))
94-
self.data = [tr.trot2(_t, t=[_x, _y]) for (_x, _y, _t) in zip(xvec, yvec, argcheck.getunit(tvec, unit))]
95-
96-
elif isinstance(x, np.ndarray) and y is None and theta is None:
97-
assert x.shape[1] == 3, 'array argument must be Nx3'
98-
self.data = [tr.trot2(_t, t=[_x, _y], unit=unit) for (_x, _y, _t) in x]
99-
88+
elif x is not None:
89+
if y is not None and theta is not None:
90+
# SE2(x, y, theta)
91+
self.data = [tr.trot2(theta, t=[x, y], unit=unit)]
92+
93+
elif y is None and theta is None:
94+
if argcheck.isvector(x, 3):
95+
# SE2( [x,y,theta])
96+
self.data = [tr.trot2(x[2], t=x[:2], unit=unit)]
97+
elif isinstance(x, np.ndarray):
98+
if x.shape == (3,3):
99+
# SE2( 3x3 matrix )
100+
self.data = [x]
101+
elif x.shape[1] == 3:
102+
# SE2( Nx3 )
103+
self.data = [tr.trot2(T.theta, t=T.t) for T in x]
104+
else:
105+
super().arghandler(x)
100106
else:
101-
super().arghandler(x)
107+
raise ValueError('bad arguments to constructor')
102108

103109
@property
104110
def T(self):
@@ -111,21 +117,27 @@ def rand(cls, *, xrange=[-1, 1], yrange=[-1, 1], trange=[0, 2 * math.pi], unit='
111117
theta = np.random.uniform(low=trange[0], high=trange[1], size=N) # random values in the range
112118
return cls([tr.trot2(t, t=[x, y]) for (t, x, y) in zip(x, y, argcheck.getunit(theta, unit))])
113119

114-
@classmethod
115-
def isvalid(self, x):
120+
@staticmethod
121+
def isvalid(x):
116122
return tr.ishom2(x, check=True)
117123

118124
@property
119125
def t(self):
120126
return self.A[:2, 2]
121127

122128
@property
123-
def R(self):
124-
return SO2(self.A[:2, :2])
129+
def xyt(self):
130+
if len(self) == 1:
131+
return np.r_[self.t, self.theta]
132+
else:
133+
return [np.r_[x.t, x.theta] for x in self]
125134

135+
@property
126136
def inv(self):
127-
return SO2(self.A.T)
128-
ArithmeticError()
137+
if len(self) == 1:
138+
return SE2(tr.rt2tr(self.R.T, -self.R.T @ self.t))
139+
else:
140+
return SE2([tr.rt2tr(x.R.T, -x.R.T @ x.t) for x in self])
129141

130142

131143
if __name__ == '__main__': # pragma: no cover

0 commit comments

Comments
 (0)
0