def test_se3_se2(): for pose in SE2.interesting_points(): pose3 = SE3_from_SE2(pose) vel3 = SE3.algebra_from_group(pose3) vel2 = se2_from_se3(vel3) pose2 = SE2.group_from_algebra(vel2) assert_allclose(pose2, pose, atol=1e-8)
def __init__(self, lvel, avel, topology='plane', interval=1): ''' :param lvel: Instantaneous linear velocity :param avel: Instantaneous angular velocity :param topology: Topology of the domain (plane/torus) :param interval: Length of motion. ''' self.topology_s = topology SymbolicDiffeo.__init__(self, topology) self.lvel = np.asarray(lvel) self.avel = float(avel) self.interval = interval self.v = se2.algebra_from_velocities(avel=self.avel, lvel=self.lvel) self.q = SE2.group_from_algebra(self.v) self.R, self.t = geometry.rotation_translation_from_SE2(self.q)
def integrate(self, state, command): v = se2.algebra_from_vector(command) delta = SE2.group_from_algebra(v * self.dt) return np.dot(state, delta)
def make_motion(v): A = se2.algebra_from_vector(v) Q = SE2.group_from_algebra(A) return Q