def list_header(format_spec) -> list:
result = []
SO3_formats = SO3.valid_list_formats()
R3_formats = R3.valid_list_formats()
for fspec in format_spec:
if fspec == "P":
result += "P11,P12,P13,P14,P21,P22,P23,P24,P31,P32,P33,P34".split()
elif fspec in R3_formats:
result += R3.list_header(fspec)
elif fspec in SO3_formats:
result += SO3.list_header(fspec)
else:
return NotImplemented
return result
def list_header(format_spec) -> list:
result = []
SO3_formats = SO3.valid_list_formats()
R3_formats = R3.valid_list_formats()
S1_formats = S1.valid_list_formats()
for fspec in format_spec:
if fspec in R3_formats:
result += R3.list_header(fspec)
elif fspec in SO3_formats:
result += SO3.list_header(fspec)
elif fspec in S1_formats:
result += S1.list_header(fspec)
else:
return NotImplemented
return result
def from_list(line, format_spec="sqx") -> 'SE3':
result = SE3()
SO3_formats = SO3.valid_list_formats()
R3_formats = R3.valid_list_formats()
S1_formats = S1.valid_list_formats()
for fspec in format_spec:
if fspec in SO3_formats:
result._R = SO3.from_list(line, fspec)
elif fspec in R3_formats:
result._x = R3.from_list(line, fspec)
elif fspec in S1_formats:
result._s = S1.from_list(line, fspec)
else:
return NotImplemented
return result
def __init__(self, R=None, x=None):
if R is None:
R = SO3()
if x is None:
x = R3()
self._R = R
self._x = x
class SE3(LieGroup.LieGroup):
def __init__(self, R = None : SO3, x = None : R3):
if R is None:
R = SO3()
if x is None:
x = R3()
self._R = R
self._x = x
def valid_list_formats() -> dict:
# Possible formats are
# q/w/R/r : SO(3) format specs
# x : 3 entry translation
# P : 12 entry homogeneous matrix (row-by-row)
result = {'P':12}
result.update(SO3.valid_list_formats())
result.update(R3.valid_list_formats())
return result
def valid_list_formats():
# Possible formats are
# SO(3) format specs
# R(3) format specs
# S(1) format specs
result = dict()
result.update(SO3.valid_list_formats())
result.update(R3.valid_list_formats())
result.update(S1.valid_list_formats())
return result
def from_list(line, format_spec="qx") -> 'SE3':
result = SE3()
SO3_formats = SO3.valid_list_formats()
R3_formats = R3.valid_list_formats()
for fspec in format_spec:
if fspec in SO3_formats:
result._R = SO3.from_list(line, fspec)
line = line[SO3_formats[fspec]:]
elif fspec in R3_formats:
result._x = R3.from_list(line, fspec)
line = line[R3_formats[fspec]:]
elif fspec == "P":
mat = np.reshape(np.array([float(line[i]) for i in range(12)]), (3,4))
result._R._rot = result._R._rot.from_matrix(mat[0:3,0:3])
result._x._trans = mat[0:3,3:4]
line = line[12:]
else:
return NotImplemented
return result
def __mul__(self, other):
if isinstance(other, S1):
result = S1()
result._scale = self._scale * other._scale
return result
elif isinstance(other, np.ndarray):
return float(self._scale) * other
elif isinstance(other, R3):
result = R3()
result._trans = self._scale * other._trans
return result
return NotImplemented
def __mul__(self, other):
if isinstance(other, SO3):
result = SO3()
result._rot = self._rot * other._rot
return result
elif isinstance(other, np.ndarray) and other.shape[0] == 3:
return self._rot.as_matrix() @ other
elif isinstance(other, R3):
result = R3()
result._trans = self._rot.as_matrix() @ other._trans
return result
return NotImplemented
def to_list(self, format_spec) -> list:
result = []
SO3_formats = SO3.valid_list_formats()
R3_formats = R3.valid_list_formats()
S1_formats = S1.valid_list_formats()
for fspec in format_spec:
if fspec in SO3_formats:
result += self._R.to_list(fspec)
elif fspec in R3_formats:
result += self._x.to_list(fspec)
elif fspec in S1_formats:
result += self._s.to_list(fspec)
else:
return NotImplemented
return result
def to_list(self, format_spec) -> list:
result = []
SO3_formats = SO3.valid_list_formats()
R3_formats = R3.valid_list_formats()
for fspec in format_spec:
if fspec in SO3_formats:
result += self._R.to_list(fspec)
elif fspec in R3_formats:
result += self._x.to_list(fspec)
elif fspec == "P":
posemat = np.hstack((self.R(), self.x()))
result += posemat.ravel().tolist()
else:
return NotImplemented
return result
def __init__(self, R=SO3(), x=R3(), s=S1()):
self._R = R
self._x = x
self._s = s
def identity():
result = SE3()
result._R = SO3.identity()
result._x = R3.identity()
return result