def algebra_from_vector(self, v):
if self.n == 2:
return hat_map_2d(v[0])
elif self.n == 3:
return hat_map(v)
else:
assert False, 'Not implemented for n=%s.' % self.n
def algebra_from_vector(self, v):
if self.n == 2:
return hat_map_2d(v[0])
elif self.n == 3:
return hat_map(v)
else:
assert False, 'Not implemented for n=%s.' % self.n
def interesting_points(self):
points = []
points.append(self.zero())
if self.n == 2:
points.append(hat_map_2d(np.pi))
points.append(hat_map_2d(np.pi / 2))
points.append(hat_map_2d(-np.pi))
elif self.n == 3:
points.append(hat_map(np.array([0, 0, 1]) * np.pi / 2))
points.append(hat_map(np.array([0, 0, 1]) * np.pi))
points.append(hat_map(np.array([0, 1, 0]) * np.pi / 2))
points.append(hat_map(np.array([0, 1, 0]) * np.pi))
points.append(hat_map(np.array([1, 0, 0]) * np.pi / 2))
points.append(hat_map(np.array([1, 0, 0]) * np.pi))
else:
assert False, 'Not implemented for n=%s' % self.n
return points
def interesting_points(self):
points = []
points.append(self.zero())
if self.n == 2:
points.append(hat_map_2d(np.pi))
points.append(hat_map_2d(np.pi / 2))
points.append(hat_map_2d(-np.pi))
elif self.n == 3:
points.append(hat_map(np.array([0, 0, 1]) * np.pi / 2))
points.append(hat_map(np.array([0, 0, 1]) * np.pi))
points.append(hat_map(np.array([0, 1, 0]) * np.pi / 2))
points.append(hat_map(np.array([0, 1, 0]) * np.pi))
points.append(hat_map(np.array([1, 0, 0]) * np.pi / 2))
points.append(hat_map(np.array([1, 0, 0]) * np.pi))
else:
assert False, 'Not implemented for n=%s' % self.n
return points
def se2_from_SE2_slow(pose):
''' Converts a pose to its Lie algebra representation. '''
R, t, zero, one = extract_pieces(pose) # @UnusedVariable
# FIXME: this still doesn't work well for singularity
W = np.array(logm(pose).real)
M, v, Z, zero = extract_pieces(W) # @UnusedVariable
M = 0.5 * (M - M.T)
if np.abs(R[0, 0] - (-1)) < 1e-10: # XXX: threshold
# cannot use logarithm for log(-I), it gives imaginary solution
M = hat_map_2d(np.pi)
return combine_pieces(M, v, v * 0, 0)
def se2_from_SE2_slow(pose):
''' Converts a pose to its Lie algebra representation. '''
R, t, zero, one = extract_pieces(pose) # @UnusedVariable
# FIXME: this still doesn't work well for singularity
W = np.array(logm(pose).real)
M, v, Z, zero = extract_pieces(W) # @UnusedVariable
M = 0.5 * (M - M.T)
if np.abs(R[0, 0] - (-1)) < 1e-10: # XXX: threshold
# cannot use logarithm for log(-I), it gives imaginary solution
M = hat_map_2d(np.pi)
return combine_pieces(M, v, v * 0, 0)
def se2_from_SE2(pose):
'''
Converts a pose to its Lie algebra representation.
See Bullo, Murray "PD control on the euclidean group" for proofs.
'''
R, t, zero, one = extract_pieces(pose) # @UnusedVariable
w = angle_from_rot2d(R)
w_abs = np.abs(w)
# FIXME: singularity
if w_abs < 1e-8: # XXX: threshold
a = 1
else:
a = (w_abs / 2) / np.tan(w_abs / 2)
A = np.array([[a, w / 2], [-w / 2, a]])
v = np.dot(A, t)
w_hat = hat_map_2d(w)
return combine_pieces(w_hat, v, v * 0, 0)
def se2_from_SE2(pose):
'''
Converts a pose to its Lie algebra representation.
See Bullo, Murray "PD control on the euclidean group" for proofs.
'''
R, t, zero, one = extract_pieces(pose) # @UnusedVariable
w = angle_from_rot2d(R)
w_abs = np.abs(w)
# FIXME: singularity
if w_abs < 1e-8: # XXX: threshold
a = 1
else:
a = (w_abs / 2) / np.tan(w_abs / 2)
A = np.array([[a, w / 2], [-w / 2, a]])
v = np.dot(A, t)
w_hat = hat_map_2d(w)
return combine_pieces(w_hat, 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 se2_from_linear_angular(linear, angular):
''' Returns an element of se2 from linear and angular velocity. '''
linear = np.array(linear)
M = hat_map_2d(angular)
return combine_pieces(M, linear, linear * 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 se2_from_linear_angular(linear, angular):
''' Returns an element of se2 from linear and angular velocity. '''
linear = np.array(linear)
M = hat_map_2d(angular)
return combine_pieces(M, linear, linear * 0, 0)