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 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 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)