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 = Vector3(u0, u1, u2)
v = Vector3(v0, v1, v2)
self.a = Quaternion(x, u)
self.b = Quaternion(y, v)
def hat(v):
""" R^6 => R^4x4 """
""" returns 4x4-matrix representation ``Omega`` """
upsilon = Vector3(v[0], v[1], v[2])
omega = Vector3(v[3], v[4], v[5])
return So3.hat(omega).\
row_join(upsilon).\
col_join(sympy.Matrix.zeros(1, 4))
def test_exp_log(self):
for o in [
Vector3(0., 1, 0.5),
Vector3(0.1, 0.1, 0.1),
Vector3(0.01, 0.2, 0.03)
]:
w = So3.exp(o).log()
for i in range(0, 3):
self.assertAlmostEqual(o[i], w[i])
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 = Vector3(v0, v1, v2)
self.omega = Vector3(omega0, omega1, omega2)
self.a = So3(Quaternion(x, v))
self.p = 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 = Vector3(v0, v1, v2)
self.upsilon_omega = Vector6(
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2)
self.t = Vector3(t0, t1, t2)
self.a = Se3(So3(Quaternion(x, v)), self.t)
self.p = 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 = Vector3(s0, s1, s2)
t = Vector3(t0, t1, t2)
u = Vector3(u0, u1, u2)
v = Vector3(v0, v1, v2)
self.a = DualQuaternion(Quaternion(w, s),
Quaternion(x, t))
self.b = DualQuaternion(Quaternion(y, u),
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 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 = Vector3(Omega[0, 3], Omega[1, 3], Omega[2, 3])
tail = So3.vee(Omega[0:3, 0:3])
upsilon_omega = \
Vector6(head[0], head[1], head[2], tail[0], tail[1], tail[2])
return upsilon_omega
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 = Vector3(upsilon0, upsilon1, theta)
self.t = Vector2(t0, t1)
self.a = Se2(So2(Complex(x, y)), self.t)
self.p = Vector2(p0, p1)
def exp(v):
""" exponential map """
upsilon = v[0:3, :]
omega = Vector3(v[3], v[4], v[5])
so3 = So3.exp(omega)
Omega = So3.hat(omega)
Omega_sq = Omega * Omega
theta = sympy.sqrt(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 vee(Omega):
v = Vector3(
Omega.row(2).col(1),
Omega.row(0).col(2),
Omega.row(1).col(0))
return v
def simplify(self):
v = sympy.simplify(self.vec)
return Quaternion(sympy.simplify(self.real), Vector3(v[0], v[1], v[2]))
def zero():
return Quaternion(0, Vector3(0, 0, 0))
def identity():
return Quaternion(1, Vector3(0, 0, 0))