def setUp(self):
x, u0, u1, u2 = sympy.symbols('x u0 u1 u2', real=True)
y, v0, v1, v2 = sympy.symbols('y v0 v1 v2', real=True)
u = sophus.Vector3(u0, u1, u2)
v = sophus.Vector3(v0, v1, v2)
self.a = Quaternion(x, u)
self.b = Quaternion(y, v)
def test_exp_log(self):
for v in [sophus.Vector3(0., 1, 0.5),
sophus.Vector3(0.1, 0.1, 0.1),
sophus.Vector3(0.01, 0.2, 0.03)]:
w = Se2.exp(v).log()
for i in range(0, 3):
self.assertAlmostEqual(v[i], w[i])
def hat(v):
""" R^6 => R^4x4 """
""" returns 4x4-matrix representation ``Omega`` """
upsilon = sophus.Vector3(v[0], v[1], v[2])
omega = sophus.Vector3(v[3], v[4], v[5])
return sophus.So3.hat(omega).\
row_join(upsilon).\
col_join(sympy.Matrix.zeros(1, 4))
def setUp(self):
omega0, omega1, omega2 = sympy.symbols('omega[0], omega[1], omega[2]',
real=True)
x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True)
p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True)
v = sophus.Vector3(v0, v1, v2)
self.omega = sophus.Vector3(omega0, omega1, omega2)
self.a = So3(sophus.Quaternion(x, v))
self.p = sophus.Vector3(p0, p1, p2)
def setUp(self):
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 = sympy.symbols(
'upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]',
real=True)
x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True)
p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True)
t0, t1, t2 = sympy.symbols('t[0] t[1] t[2]', real=True)
v = sophus.Vector3(v0, v1, v2)
self.upsilon_omega = sophus.Vector6(
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2)
self.t = sophus.Vector3(t0, t1, t2)
self.a = Se3(sophus.So3(sophus.Quaternion(x, v)), self.t)
self.p = sophus.Vector3(p0, p1, p2)
def setUp(self):
w, s0, s1, s2 = sympy.symbols('w s0 s1 s2', real=True)
x, t0, t1, t2 = sympy.symbols('x t0 t1 t2', real=True)
y, u0, u1, u2 = sympy.symbols('y u0 u1 u2', real=True)
z, v0, v1, v2 = sympy.symbols('z v0 v1 v2', real=True)
s = sophus.Vector3(s0, s1, s2)
t = sophus.Vector3(t0, t1, t2)
u = sophus.Vector3(u0, u1, u2)
v = sophus.Vector3(v0, v1, v2)
self.a = DualQuaternion(sophus.Quaternion(w, s),
sophus.Quaternion(x, t))
self.b = DualQuaternion(sophus.Quaternion(y, u),
sophus.Quaternion(z, v))
def log(self):
theta = self.so2.log()
halftheta = 0.5 * theta
a = -(halftheta * self.so2.z.imag) / (self.so2.z.real - 1)
V_inv = sympy.Matrix([[a, halftheta],
[-halftheta, a]])
upsilon = V_inv * self.t
return sophus.Vector3(upsilon[0], upsilon[1], theta)
def vee(Omega):
""" R^4x4 => R^6 """
""" returns 6-vector representation of Lie algebra """
""" This is the inverse of the hat-operator """
head = sophus.Vector3(Omega[0,3], Omega[1,3], Omega[2,3])
tail = sophus.So3.vee(Omega[0:3,0:3])
upsilon_omega = \
sophus.Vector6(head[0], head[1], head[2], tail[0], tail[1], tail[2])
return upsilon_omega
def exp(v):
""" exponential map """
upsilon = v[0:3, :]
omega = sophus.Vector3(v[3], v[4], v[5])
so3 = sophus.So3.exp(omega)
Omega = sophus.So3.hat(omega)
Omega_sq = Omega * Omega
theta = sympy.sqrt(sophus.squared_norm(omega))
V = (sympy.Matrix.eye(3) +
(1 - sympy.cos(theta)) / (theta**2) * Omega +
(theta - sympy.sin(theta)) / (theta**3) * Omega_sq)
return Se3(so3, V * upsilon)
def setUp(self):
upsilon0, upsilon1, theta = sympy.symbols(
'upsilon[0], upsilon[1], theta',
real=True)
x, y = sympy.symbols('c[0] c[1]', real=True)
p0, p1 = sympy.symbols('p0 p1', real=True)
t0, t1 = sympy.symbols('t[0] t[1]', real=True)
self.upsilon_theta = sophus.Vector3(
upsilon0, upsilon1, theta)
self.t = sophus.Vector2(t0, t1)
self.a = Se2(sophus.So2(sophus.Complex(x, y)), self.t)
self.p = sophus.Vector2(p0, p1)
def simplify(self):
v = sympy.simplify(self.vec)
return Quaternion(sympy.simplify(self.real),
sophus.Vector3(v[0], v[1], v[2]))
def zero():
return Quaternion(0, sophus.Vector3(0, 0, 0))
def identity():
return Quaternion(1, sophus.Vector3(0, 0, 0))
def vee(Omega):
v = sophus.Vector3(
Omega.row(2).col(1),
Omega.row(0).col(2),
Omega.row(1).col(0))
return v