def test_jacobians_of_exponential(self): t = np.random.uniform(-10.0, 10.0, size=2) theta = np.random.uniform(-np.pi, np.pi) tau = np.array([*t, theta]) T, Jr = SE2.Exp(tau, Jr=np.eye(3)) _, Jl = SE2.Exp(-tau, Jl=np.eye(3)) np.testing.assert_allclose(Jr, Jl)
def testTaylorExp(self): for i in range(100): v = np.random.uniform(-10, 10, size=2) theta = np.random.uniform(-1e-8, 1e-8) arr = np.array([v[0], v[1], theta]) T = SE2.Exp(arr) T_true = spl.expm(SE2.hat(arr)) np.testing.assert_allclose(T_true, T.arr)
def test_boxplusl(self): for i in range(100): T = SE2.random() u = np.random.uniform(-10., 10., size=2) theta = np.random.uniform(-np.pi, np.pi) vec = np.array([*u, theta]) T2 = T.boxplusl(vec) T2_true = SE2.Exp(vec) * T np.testing.assert_allclose(T2_true.T, T2.T)
def testBoxPlusR(self): for i in range(100): T = SE2.random() u = np.random.uniform(-10, 10, size=2) theta = np.random.uniform(-np.pi, np.pi) vec = np.array([*u, theta]) T3 = T.boxplusr(vec) T3_true = T * SE2.Exp(vec) np.testing.assert_allclose(T3_true.T, T3.T)
def testAdjoint(self): for i in range(100): t = np.random.uniform(-10, 10, size=2) theta = np.random.uniform(-np.pi, np.pi) u = np.random.uniform(-1, 1, size=2) phi = np.random.uniform(-np.pi, np.pi) # delta = np.array([phi, u[0], u[1]]) delta = np.array([u[0], u[1], phi]) ct = np.cos(theta) st = np.sin(theta) R = np.array([[ct, -st], [st, ct]]) T = SE2.fromRandt(R, t) adj = T.Adj T2_true = T * SE2.Exp(delta) T2 = SE2.Exp(adj @ delta) * T np.testing.assert_allclose(T2_true.arr, T2.arr)
def test_left_jacobian_or_logarithm(self): T = SE2.random() tau, Jl_inv = SE2.Log(T, Jl=np.eye(3)) _, Jl = SE2.Exp(tau, Jl=np.eye(3)) np.testing.assert_allclose(np.linalg.inv(Jl), Jl_inv)
def test_right_jacobian_of_logarithm(self): T = SE2.random() tau, Jr_inv = SE2.Log(T, Jr=np.eye(3)) _, Jr = SE2.Exp(tau, Jr=np.eye(3)) np.testing.assert_allclose(np.linalg.inv(Jr), Jr_inv)
def propogateDynamics(self, robot, u, dt): U, G = SE2.Exp(u*dt, Jr=np.eye(3)) _, F = robot.state.compose(U, Jr=np.eye(3)) self._cov = F @ self._cov @ F.T + G @ robot.odom_cov @ G.T