def algebra_from_vector(self, v):
""" Note that the first element is (omega, vx, vy) or
(w1,w2,w3,vx,vy,vz) """
if self.n == 3:
assert v.size == 3
omega = v[0]
vel = v[1:3]
return self.algebra_from_velocities_2d(avel=omega, lvel=vel)
elif self.n == 4:
assert v.size == 6
omega = v[0:3]
vel = v[3:6]
W = hat_map(omega)
return combine_pieces(W, vel, vel * 0, 0)
else:
assert False, 'Not implemented for n=%d.' % self.n
def algebra_from_vector(self, v):
""" Note that the first element is (omega, vx, vy) or
(w1,w2,w3,vx,vy,vz) """
if self.n == 3:
assert v.size == 3
omega = v[0]
vel = v[1:3]
return self.algebra_from_velocities_2d(avel=omega, lvel=vel)
elif self.n == 4:
assert v.size == 6
omega = v[0:3]
vel = v[3:6]
W = hat_map(omega)
return combine_pieces(W, vel, vel * 0, 0)
else:
assert False, 'Not implemented for n=%d.' % self.n
def algebra_from_velocities_2d(self, avel, lvel):
W = hat_map_2d(avel)
return combine_pieces(W, lvel, np.array(lvel) * 0, 0)
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
W = self.son.project(W)
return combine_pieces(W, v, v * 0, 0)
def se2_project_from_se3(b):
W, v, zero, one = extract_pieces(b) # @UnusedVariable
W = so2_project_from_so3(W)
v = v[0:2]
return combine_pieces(W, v, v * 0, 0)
def SE2_project_from_SE3(b):
R, t, zero, one = extract_pieces(b) # @UnusedVariable
R = SO2_project_from_SO3(R)
t = t[0:2]
return combine_pieces(R, t, t * 0, 1)
def se3_from_se2(a):
W, v, zero, one = extract_pieces(a) # @UnusedVariable
W = so3_from_so2(W)
v = np.array([v[0], v[1], 0])
return combine_pieces(W, v, v * 0, 0)
def se3_from_so3(a):
return combine_pieces(a, np.array([0, 0, 0]), np.array([0, 0, 0]), 0)
def algebra_from_vector(self, v):
assert v.size == self.n - 1
return combine_pieces(np.zeros((self.n - 1, self.n - 1)), v, v * 0, 0)
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
return combine_pieces(W * 0, v, v * 0, 0)
def algebra_from_vector(self, v):
assert v.size == self.n - 1
return combine_pieces(np.zeros((self.n - 1, self.n - 1)), v, v * 0, 0)
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
return combine_pieces(W * 0, v, v * 0, 0)
def algebra_from_velocities_2d(self, avel, lvel):
W = hat_map_2d(avel)
return combine_pieces(W, lvel, np.array(lvel) * 0, 0)
def project(self, X):
W, v, zero, zero = extract_pieces(X) # @UnusedVariable
W = self.son.project(W)
return combine_pieces(W, v, v * 0, 0)