def adjoint(se3vec : np.ndarray) -> np.ndarray:
assert isinstance(se3vec, np.ndarray)
assert se3vec.shape == (6,1)
ad = np.zeros((6,6))
OmegaCross = SO3.skew(se3vec[0:3,:])
VCross = SO3.skew(se3vec[3:6,:])
ad[0:3,0:3] = OmegaCross
ad[3:6,0:3] = VCross
ad[3:6,3:6] = OmegaCross
return ad
def list_header(format_spec) -> list:
result = []
SO3_formats = SO3.valid_list_formats()
S1_formats = S1.valid_list_formats()
for fspec in format_spec:
if fspec == "Q":
result += "Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33".split()
elif fspec in S1_formats:
result += S1.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()
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 test_setslice(self):
ft1 = FrameTrajectory("blargh")
ft2 = FrameTrajectory("blargh")
for i in range(20):
ft1.append(SE3element(SO3.uniform_random(), np.random.rand(3)))
for i in range(10):
ft2.append(SE3element(SO3.uniform_random(), np.random.rand(3)))
ft1[:10] = ft2
self.assertTrue(ft1[:10].all_frames.shape == ft2.all_frames.shape)
self.assertTrue(np.allclose(ft1.all_frames[:10], ft2.all_frames))
ft1[10:] = ft2
self.assertTrue(ft1[10:].all_frames.shape == ft2.all_frames.shape)
self.assertTrue(np.allclose(ft1.all_frames[10:], ft2.all_frames))
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 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 degrees_of_freedom(self, g1: SE3element,
g2: SE3element) -> List[float]:
"""
Calculates the six degrees of freedom between two SE3 elements.
"""
g1R = g1.orientation
g2R = g2.orientation
rot_dof = list(so3.vec(SO3.sqrt(g1R.inv() * g2R)))
midR = g1R * SO3.sqrt(g1R.inv() * g2R)
trans_dof = np.dot(midR.inv().representation,
g2.position - g1.position)
rot_dof.extend(trans_dof)
#print('r', rot_dof)
#print('t', trans_dof)
return rot_dof
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
def __init__(self,
_or: SO3element = SO3.identity(),
_pos: np.ndarray = np.zeros(3)):
self.group = SE3()
self.representation = np.eye(4)
self.orientation = _or
self.position = _pos
def Adjoint(self) -> np.ndarray:
Ad = np.zeros((6,6))
R = self.R()
Ad[0:3,0:3] = R
Ad[3:6,0:3] = SO3.skew(self.x()) @ R
Ad[3:6,3:6] = R
return Ad
def exp(se3arr):
if not isinstance(se3arr, np.ndarray):
raise TypeError
if se3arr.shape == (4,4):
se3arr = SE3.vee(se3arr)
elif not se3arr.shape == (6,1):
raise ValueError
w = se3arr[0:3,0:1]
u = se3arr[3:6,0:1]
theta = np.linalg.norm(w)
if theta > 1e-6:
A = np.sin(theta) / theta
B = (1.0 - np.cos(theta)) / theta**2.0
C = (1.0 - A) / theta**2.0
else:
A = 1.0
B = 1.0 / 2.0
C = 1.0 / 6.0
wx = SO3.skew(w)
wx2 = wx @ wx
R = np.eye(3) + A * wx + B * wx2
V = np.eye(3) + B * wx + C * wx2
mat = np.block([[R, V@u],[np.zeros((1,4))]])
result = SE3.from_matrix(mat)
return result
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 test_right_multiply(self):
ft1 = FrameTrajectory("blorgh")
for i in range(10):
v = SE3element()
v.orientation = SO3.uniform_random()
#ft1.append(v)
ft1.append(SE3element())
w = SE3element()
w.orientation = SO3.uniform_random()
ft2 = ft1.right_multiply(w)
longw = np.array([w.representation for i in range(10)])
self.assertTrue(
np.allclose(ft2.frames[:ft2.n_timeframes],
longw[:ft2.n_timeframes]))
def midframe(self, g1: SE3element, g2: SE3element) -> SE3element:
"""
Calculates the midframe of two SE3 elements.
"""
g1R = g1.orientation
g2R = g2.orientation
midR = g1R * SO3.sqrt(g1R.inv() * g2R)
return SE3element(midR, 0.5 * (g1.position + g2.position))
def wedge(vec : np.ndarray) -> np.ndarray:
if not isinstance(vec, np.ndarray):
raise TypeError
if not vec.shape == (6,1):
raise ValueError
mat = np.zeros((4,4))
mat[0:3,0:3] = SO3.skew(vec[0:3,0:1])
mat[0:3,3:4] = vec[3:6, 0:1]
return mat
def vee(mat : np.ndarray) -> np.ndarray:
if not isinstance(mat, np.ndarray):
raise TypeError
if not mat.shape == (4,4):
raise ValueError
vecOmega = SO3.vex(mat[0:3,0:3])
vecV = mat[0:3,3:4]
vec = np.vstack((vecOmega, vecV))
return vec
def __init__(self, R=None, s=None):
if R is None:
R = SO3()
if s is None:
s = S1()
assert isinstance(R, SO3)
assert isinstance(s, S1)
self._R = R
self._s = s
def valid_list_formats() -> dict:
# Possible formats are
# q/w/R/r : SO(3) format specs
# s : 1 entry scale
# Q : 9 entry matrix (row-by-row)
result = {'Q': 9}
result.update(SO3.valid_list_formats())
result.update(S1.valid_list_formats())
return result
def test_relative_to(self):
ft1 = FrameTrajectory("blargh")
ft2 = FrameTrajectory("blorgh")
frames1 = []
frames2 = []
for i in range(10):
v = SE3element()
v.orientation = SO3.uniform_random()
ft1.append(v)
frames1.append(v)
w = SE3element()
w.orientation = SO3.uniform_random()
ft2.append(w)
frames2.append(w)
rel_ft = ft1.relative_to(ft2)
self.assertTrue(np.allclose(rel_ft.frames, ft2.inv() * ft1))
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 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 log(self) -> np.ndarray:
w = self._R.log()
theta = np.linalg.norm(w)
wx = SO3.skew(w)
if theta > 1e-6:
Vinv = np.eye(3) - 0.5 * wx + theta**(-2.0) * (1.0 - (theta*np.sin(theta))/(2*(1-np.cos(theta)))) * wx @ wx
else:
Vinv = np.eye(3) - 0.5*wx
u = Vinv @ self._x._trans
return np.vstack((w,u))
def from_list(line, format_spec="qs") -> 'SOT3':
result = SOT3()
SO3_formats = SO3.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)
line = line[SO3_formats[fspec]:]
elif fspec in S1_formats:
result._s = S1.from_list(line, fspec)
line = line[S1_formats[fspec]:]
elif fspec == "Q":
mat = np.reshape(np.array([float(line[i]) for i in range(9)]),
(3, 3))
result = SOT3.from_matrix(mat)
line = line[9:]
else:
return NotImplemented
return result
def from_matrix(mat : np.ndarray) -> 'SE3':
if not isinstance(mat, np.ndarray):
raise TypeError
if not mat.shape == (4,4):
raise ValueError
result = SE3()
result._R = SO3.from_matrix(mat[0:3,0:3])
result._x._trans = mat[0:3,3:4]
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 test_positions(self):
ft = FrameTrajectory("blargh")
for _ in range(1):
v = SE3element()
v.orientation = SO3.uniform_random()
v.position = np.random.rand(3)
ft.append(v)
print()
print()
print(np.transpose(ft.positions, (0, 2, 1)))
print(json.dumps(ft.toJSON(), indent=4))
def degrees_of_freedomSE3(self, g1: SE3element,
g2: SE3element) -> SE3element:
"""
Calculates the six degrees of freedom between two SE3 elements.
"""
g1R = g1.orientation
g2R = g2.orientation
rot_dof = g1R.inv() * g2R
midR = g1R * SO3.sqrt(g1R.inv() * g2R)
trans_dof = np.dot(midR.inv().representation,
g2.position - g1.position)
return SE3element(rot_dof, trans_dof)
def test_dof(self):
ft1 = FrameTrajectory("blargh")
ft2 = FrameTrajectory("blorgh")
for i in range(10):
v = SE3element()
v.orientation = SO3.uniform_random()
v.position = np.random.rand(3)
#print('R1')
#print(v.orientation)
#print('r1')
#print(v.position)
ft1.append(v)
w = SE3element()
w.orientation = SO3.uniform_random()
w.position = np.random.rand(3)
#print('R2')
#print(w.orientation)
#print('r2')
#print(w.position)
ft2.append(w)
dof_trajs = degrees_of_freedom(ft1, ft2)
def test_sqrt(self):
ft = FrameTrajectory("blargh")
for i in range(10):
v = SE3element()
v.orientation = SO3.uniform_random()
ft.append(v)
sqrt_ft = sqrt(ft.orientations)
or2 = np.matmul(sqrt_ft, sqrt_ft)
#print(or2)
#print(ft.orientations)
self.assertTrue(np.allclose(ft.orientations, or2))
def from_matrix(mat: np.ndarray) -> 'SOT3':
if not isinstance(mat, np.ndarray):
raise TypeError
if not mat.shape == (4, 4) or mat.shape == (3, 3):
raise ValueError
result = SOT3()
m = mat[0:3, 0:3]
Q = m.T @ m
# Q is s^2 I_3.
result._s = S1(Q[0, 0]**0.5)
result._R = SO3.from_matrix(m / result._s)
return result