def make_motion(v): A = se2.algebra_from_vector(v) Q = SE2.group_from_algebra(A) return 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)