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
