def test_Quaternion_str_printer():
q = Quaternion(x, y, z, t)
assert str(q) == "x + y*i + z*j + t*k"
q = Quaternion(x,y,z,x*t)
assert str(q) == "x + y*i + z*j + t*x*k"
q = Quaternion(x,y,z,x+t)
assert str(q) == "x + y*i + z*j + (t + x)*k"
def from_screw(cls, l, m, theta, d):
"""Returns the unit dual quaternion corresponding to a screw.
Parameters
==========
l : tuple
unit vector parallel to the screw axis
m : tuple
moment of l
theta : number
d : number
Returns
=======
DualQuaternion
"""
(x, y, z) = l
(a, b, c) = m
if trigsimp(x**2 + y**2 + z**2) != 1 or trigsimp(x * a + y * b +
z * c) != 0:
raise ValueError(
"Expected l to be a unit vector and m perpendicular to l!")
q_r = Quaternion(cos(theta * S.Half),
*(sin(theta * S.Half) * i for i in l))
q_d = Quaternion(
-d * S.Half * sin(theta * S.Half),
*(d * S.Half * cos(theta * S.Half) * i + sin(theta * S.Half) * j
for i, j in zip(l, m)))
return DualQuaternion(q_r, q_d)
def test_quaternion_conversions():
q1 = Quaternion(1, 2, 3, 4)
assert q1.to_axis_angle() == ((2 * sqrt(29)/29,
3 * sqrt(29)/29,
4 * sqrt(29)/29),
2 * acos(sqrt(30)/30))
assert q1.to_rotation_matrix() == Matrix([[Rational(-2, 3), Rational(2, 15), Rational(11, 15)],
[Rational(2, 3), Rational(-1, 3), Rational(2, 3)],
[Rational(1, 3), Rational(14, 15), Rational(2, 15)]])
assert q1.to_rotation_matrix((1, 1, 1)) == Matrix([[Rational(-2, 3), Rational(2, 15), Rational(11, 15), Rational(4, 5)],
[Rational(2, 3), Rational(-1, 3), Rational(2, 3), S.Zero],
[Rational(1, 3), Rational(14, 15), Rational(2, 15), Rational(-2, 5)],
[S.Zero, S.Zero, S.Zero, S.One]])
theta = symbols("theta", real=True)
q2 = Quaternion(cos(theta/2), 0, 0, sin(theta/2))
assert trigsimp(q2.to_rotation_matrix()) == Matrix([
[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1]])
assert q2.to_axis_angle() == ((0, 0, sin(theta/2)/Abs(sin(theta/2))),
2*acos(cos(theta/2)))
assert trigsimp(q2.to_rotation_matrix((1, 1, 1))) == Matrix([
[cos(theta), -sin(theta), 0, sin(theta) - cos(theta) + 1],
[sin(theta), cos(theta), 0, -sin(theta) - cos(theta) + 1],
[0, 0, 1, 0],
[0, 0, 0, 1]])
def test_quaternion_conversions():
q1 = Quaternion(1, 2, 3, 4)
assert q1.to_axis_angle() == ((2 * sqrt(29)/29,
3 * sqrt(29)/29,
4 * sqrt(29)/29),
2 * acos(sqrt(30)/30))
assert q1.to_rotation_matrix() == Matrix([[-S(2)/3, S(2)/15, S(11)/15],
[S(2)/3, -S(1)/3, S(2)/3],
[S(1)/3, S(14)/15, S(2)/15]])
assert q1.to_rotation_matrix((1, 1, 1)) == Matrix([[-S(2)/3, S(2)/15, S(11)/15, S(4)/5],
[S(2)/3, -S(1)/3, S(2)/3, S(0)],
[S(1)/3, S(14)/15, S(2)/15, -S(2)/5],
[S(0), S(0), S(0), S(1)]])
theta = symbols("theta", real=True)
q2 = Quaternion(cos(theta/2), 0, 0, sin(theta/2))
assert trigsimp(q2.to_rotation_matrix()) == Matrix([
[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1]])
assert q2.to_axis_angle() == ((0, 0, sin(theta/2)/Abs(sin(theta/2))),
2*acos(cos(theta/2)))
assert trigsimp(q2.to_rotation_matrix((1, 1, 1))) == Matrix([
[cos(theta), -sin(theta), 0, sin(theta) - cos(theta) + 1],
[sin(theta), cos(theta), 0, -sin(theta) - cos(theta) + 1],
[0, 0, 1, 0],
[0, 0, 0, 1]])
def test_should_construct_from_screw(self):
l = (0, 0, 1)
m = (0, -x, 0)
dq = DualQuaternion.from_screw(l, m, theta, 0)
assert dq.real == Quaternion(cos(theta * S.Half), 0, 0,
sin(theta * S.Half))
assert dq.dual == Quaternion(0, 0, -x * sin(theta * S.Half), 0)
def __new__(cls, p=Quaternion(0), q=Quaternion(0)):
if not isinstance(p, Quaternion):
p = Quaternion(p)
if not isinstance(q, Quaternion):
q = Quaternion(q)
obj = Expr.__new__(cls, p, q)
obj._p = p
obj._q = q
return obj
def test_quaternion_construction():
q = Quaternion(x, y, z, w)
assert q + q == Quaternion(2*x, 2*y, 2*z, 2*w)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
assert q2 == Quaternion(Rational(1/2), Rational(1/2),
Rational(1/2), Rational(1,2))
M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(x) + 1)/2, 0, 0, sqrt(-2*cos(x) + 2)/2)
def test_issue_16318():
#for rtruediv
q0 = Quaternion(0, 0, 0, 0)
raises(ValueError, lambda: 1/q0)
#for rotate_point
q = Quaternion(1, 2, 3, 4)
(axis, angle) = q.to_axis_angle()
assert Quaternion.rotate_point((1, 1, 1), (axis, angle)) == (S.One / 5, 1, S(7) / 5)
#test for to_axis_angle
q = Quaternion(-1, 1, 1, 1)
axis = (-sqrt(3)/3, -sqrt(3)/3, -sqrt(3)/3)
angle = 2*pi/3
assert (axis, angle) == q.to_axis_angle()
def test_quaternion_construction():
q = Quaternion(x, y, z, w)
assert q + q == Quaternion(2*x, 2*y, 2*z, 2*w)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
assert q2 == Quaternion(Rational(1, 2), Rational(1, 2),
Rational(1, 2), Rational(1, 2))
M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(x) + 1)/2, 0, 0, sqrt(-2*cos(x) + 2)/2)
nc = Symbol('nc', commutative=False)
raises(ValueError, lambda: Quaternion(x, y, nc, w))
def test_quaternion_construction():
q = Quaternion(w, x, y, z)
assert q + q == Quaternion(2*w, 2*x, 2*y, 2*z)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3),
pi*Rational(2, 3))
assert q2 == Quaternion(S.Half, S.Half,
S.Half, S.Half)
M = Matrix([[cos(phi), -sin(phi), 0], [sin(phi), cos(phi), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(phi) + 1)/2, 0, 0, sqrt(2 - 2*cos(phi))*sign(sin(phi))/2)
nc = Symbol('nc', commutative=False)
raises(ValueError, lambda: Quaternion(w, x, nc, z))
def rotated(self, nx: float, ny: float, nz: float, theta: float):
"""
Returns a rotated version of this object
:param nx: x-component of the rotation axis
:param ny: y-component of the rotation axis
:param nz: z-component of the rotation axis
:param theta: rotation angle
:return: a rotated version of this object
"""
nrm = N(sqrt(nx**2 + ny**2 + nz**2))
sn2 = N(sin(theta / 2))
cs2 = N(cos(theta / 2))
q = Quaternion(cs2, sn2 * nx / nrm, sn2 * ny / nrm, sn2 * nz / nrm)
q_inv = q.conjugate()
r = q_inv * Quaternion(0, x, y, z) * q
return Geometry3D(self.subs_3d(self.f, r.b, r.c, r.d))
def test_quaternion_rotation_iss1593():
"""
There was a sign mistake in the definition,
of the rotation matrix. This tests that particular sign mistake.
See issue 1593 for reference.
See wikipedia
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
for the correct definition
"""
q = Quaternion(cos(phi / 2), sin(phi / 2), 0, 0)
assert (trigsimp(q.to_rotation_matrix()) == Matrix(
[[1, 0, 0], [0, cos(phi), -sin(phi)], [0, sin(phi),
cos(phi)]]))
def test_quaternion_multiplication():
q1 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
q2 = Quaternion(1, 2, 3, 5)
q3 = Quaternion(1, 1, 1, y)
assert Quaternion._generic_mul(4, 1) == 4
assert Quaternion._generic_mul(4, q1) == Quaternion(12 + 16*I, 8 + 20*I, 0, 28 + 32*I)
assert q2.mul(2) == Quaternion(2, 4, 6, 10)
assert q2.mul(q3) == Quaternion(-5*y - 4, 3*y - 2, 9 - 2*y, y + 4)
assert q2.mul(q3) == q2*q3
z = symbols('z', complex=True)
z_quat = Quaternion(re(z), im(z), 0, 0)
q = Quaternion(*symbols('q:4', real=True))
assert z * q == z_quat * q
assert q * z == q * z_quat
def test_quaternion_nultiplication():
q1 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
q2 = Quaternion(1, 2, 3, 5)
q3 = Quaternion(1, 1, 1, y)
assert Quaternion._generic_mul(4, 1) == 4
assert Quaternion._generic_mul(4, q1) == Quaternion(12 + 16*I, 8 + 20*I, 0, 28 + 32*I)
assert q2.mul(2) == Quaternion(2, 4, 6, 10)
assert q2.mul(q3) == Quaternion(-5*y - 4, 3*y - 2, 9 - 2*y, y + 4)
assert q2.mul(q3) == q2*q3
def test_quaternion_axis_angle():
test_data = [ # axis, angle, expected_quaternion
((1, 0, 0), 0, (1, 0, 0, 0)),
((1, 0, 0), pi/2, (sqrt(2)/2, sqrt(2)/2, 0, 0)),
((0, 1, 0), pi/2, (sqrt(2)/2, 0, sqrt(2)/2, 0)),
((0, 0, 1), pi/2, (sqrt(2)/2, 0, 0, sqrt(2)/2)),
((1, 0, 0), pi, (0, 1, 0, 0)),
((0, 1, 0), pi, (0, 0, 1, 0)),
((0, 0, 1), pi, (0, 0, 0, 1)),
((1, 1, 1), pi, (0, 1/sqrt(3),1/sqrt(3),1/sqrt(3))),
((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), pi*2/3, (S.Half, S.Half, S.Half, S.Half))
]
for axis, angle, expected in test_data:
assert Quaternion.from_axis_angle(axis, angle) == Quaternion(*expected)
def transform_point(pin, t):
"""Returns the coordinates of the point pin(a 3 tuple) after transformation.
Parameters
==========
pin : tuple
A 3-element tuple of coordinates of a point which needs to be
transformed.
r : DualQuaternion
Screw axis and dual angle of rotation.
Returns
=======
tuple
The coordinates of the point after transformation.
"""
pout = (t * DualQuaternion(1, Quaternion(0, *pin)) *
t.combined_conjugate()).dual
return (pout.b, pout.c, pout.d)
import sympy as sp
from sympy.algebras.quaternion import Quaternion
"""
Remember! This assumes r i j k and not i j k r
"""
pi = sp.symbols('myvar')
q = Quaternion(0, 1, 0, 0).normalize()
omega = sp.Matrix([0, pi, 0])
omega_quat = Quaternion(0, *omega)
q_dot = (1 / 2) * q * omega_quat
print(q_dot)
def test_quaternion_functions():
q = Quaternion(w, x, y, z)
q1 = Quaternion(1, 2, 3, 4)
q0 = Quaternion(0, 0, 0, 0)
assert conjugate(q) == Quaternion(w, -x, -y, -z)
assert q.norm() == sqrt(w**2 + x**2 + y**2 + z**2)
assert q.normalize() == Quaternion(w, x, y,
z) / sqrt(w**2 + x**2 + y**2 + z**2)
assert q.inverse() == Quaternion(w, -x, -y,
-z) / (w**2 + x**2 + y**2 + z**2)
assert q.inverse() == q.pow(-1)
raises(ValueError, lambda: q0.inverse())
assert q.pow(2) == Quaternion(w**2 - x**2 - y**2 - z**2, 2 * w * x,
2 * w * y, 2 * w * z)
assert q**(2) == Quaternion(w**2 - x**2 - y**2 - z**2, 2 * w * x,
2 * w * y, 2 * w * z)
assert q1.pow(-2) == Quaternion(Rational(-7, 225), Rational(-1, 225),
Rational(-1, 150), Rational(-2, 225))
assert q1**(-2) == Quaternion(Rational(-7, 225), Rational(-1, 225),
Rational(-1, 150), Rational(-2, 225))
assert q1.pow(-0.5) == NotImplemented
raises(TypeError, lambda: q1**(-0.5))
assert q1.exp() == \
Quaternion(E * cos(sqrt(29)),
2 * sqrt(29) * E * sin(sqrt(29)) / 29,
3 * sqrt(29) * E * sin(sqrt(29)) / 29,
4 * sqrt(29) * E * sin(sqrt(29)) / 29)
assert q1._ln() == \
Quaternion(log(sqrt(30)),
2 * sqrt(29) * acos(sqrt(30)/30) / 29,
3 * sqrt(29) * acos(sqrt(30)/30) / 29,
4 * sqrt(29) * acos(sqrt(30)/30) / 29)
assert q1.pow_cos_sin(2) == \
Quaternion(30 * cos(2 * acos(sqrt(30)/30)),
60 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
90 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
120 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29)
assert diff(Quaternion(x, x, x, x), x) == Quaternion(1, 1, 1, 1)
assert integrate(Quaternion(x, x, x, x), x) == \
Quaternion(x**2 / 2, x**2 / 2, x**2 / 2, x**2 / 2)
assert Quaternion.rotate_point((1, 1, 1), q1) == (S.One / 5, 1, S(7) / 5)
n = Symbol('n')
raises(TypeError, lambda: q1**n)
n = Symbol('n', integer=True)
raises(TypeError, lambda: q1**n)
assert Quaternion(22, 23, 55, 8).scalar_part() == 22
assert Quaternion(w, x, y, z).scalar_part() == w
assert Quaternion(22, 23, 55, 8).vector_part() == Quaternion(0, 23, 55, 8)
assert Quaternion(w, x, y, z).vector_part() == Quaternion(0, x, y, z)
assert q1.axis() == Quaternion(0, 2 * sqrt(29) / 29, 3 * sqrt(29) / 29,
4 * sqrt(29) / 29)
assert q1.axis().pow(2) == Quaternion(-1, 0, 0, 0)
assert q0.axis().scalar_part() == 0
assert q.axis() == Quaternion(0, x / sqrt(x**2 + y**2 + z**2),
y / sqrt(x**2 + y**2 + z**2),
z / sqrt(x**2 + y**2 + z**2))
assert q0.is_pure() == True
assert q1.is_pure() == False
assert Quaternion(0, 0, 0, 3).is_pure() == True
assert Quaternion(0, 2, 10, 3).is_pure() == True
assert Quaternion(w, 2, 10, 3).is_pure() == None
assert q1.angle() == atan(sqrt(29))
assert q.angle() == atan2(sqrt(x**2 + y**2 + z**2), w)
assert Quaternion.arc_coplanar(q1, Quaternion(2, 4, 6, 8)) == True
assert Quaternion.arc_coplanar(q1, Quaternion(1, -2, -3, -4)) == True
assert Quaternion.arc_coplanar(q1, Quaternion(1, 8, 12, 16)) == True
assert Quaternion.arc_coplanar(q1, Quaternion(1, 2, 3, 4)) == True
assert Quaternion.arc_coplanar(q1, Quaternion(w, 4, 6, 8)) == True
assert Quaternion.arc_coplanar(q1, Quaternion(2, 7, 4, 1)) == False
assert Quaternion.arc_coplanar(q1, Quaternion(w, x, y, z)) == None
raises(ValueError, lambda: Quaternion.arc_coplanar(q1, q0))
assert Quaternion.vector_coplanar(Quaternion(0, 8, 12, 16),
Quaternion(0, 4, 6, 8),
Quaternion(0, 2, 3, 4)) == True
assert Quaternion.vector_coplanar(Quaternion(0, 0, 0, 0),
Quaternion(0, 4, 6, 8),
Quaternion(0, 2, 3, 4)) == True
assert Quaternion.vector_coplanar(Quaternion(0, 8, 2, 6),
Quaternion(0, 1, 6, 6),
Quaternion(0, 0, 3, 4)) == False
assert Quaternion.vector_coplanar(Quaternion(0, 1, 3, 4),
Quaternion(0, 4, w, 6),
Quaternion(0, 6, 8, 1)) == None
raises(ValueError,
lambda: Quaternion.vector_coplanar(q0, Quaternion(0, 4, 6, 8), q1))
assert Quaternion(0, 1, 2, 3).parallel(Quaternion(0, 2, 4, 6)) == True
assert Quaternion(0, 1, 2, 3).parallel(Quaternion(0, 2, 2, 6)) == False
assert Quaternion(0, 1, 2, 3).parallel(Quaternion(w, x, y, 6)) == None
raises(ValueError, lambda: q0.parallel(q1))
assert Quaternion(0, 1, 2, 3).orthogonal(Quaternion(0, -2, 1, 0)) == True
assert Quaternion(0, 2, 4, 7).orthogonal(Quaternion(0, 2, 2, 6)) == False
assert Quaternion(0, 2, 4, 7).orthogonal(Quaternion(w, x, y, 6)) == None
raises(ValueError, lambda: q0.orthogonal(q1))
assert q1.index_vector() == Quaternion(0, 2 * sqrt(870) / 29,
3 * sqrt(870) / 29,
4 * sqrt(870) / 29)
assert Quaternion(0, 3, 9, 4).index_vector() == Quaternion(0, 3, 9, 4)
assert Quaternion(4, 3, 9, 4).mensor() == log(sqrt(122))
assert Quaternion(3, 3, 0, 2).mensor() == log(sqrt(22))
assert q0.is_zero_quaternion() == True
assert q1.is_zero_quaternion() == False
assert Quaternion(w, 0, 0, 0).is_zero_quaternion() == None
# Update the list of state variable and control variable # Update the definition of f and h # # Customize this script for using oldstate and newstate instead of Qx and NQx # since macro for Qx and NQx are only defined for x in [0,3] # Or update the C code with new macros for the additional state variables # # State variables in the code stateVars = [q0, q1, q2, q3] # Updated state variables in the code (predictino step) newStateVars = [nq0, nq1, nq2, nq3] controlVars = [wx, wy, wz] q = Quaternion(q0, q1, q2, q3) w = Quaternion(0, wx, wy, wz) # This formula is expression how the quaternion is changed # for a given rotation speed. It is taking into account the fact that # the rotation speed is in local coordinates. delta = dt / 2.0 * q * w # Equation 2.0.1 f = delta + q # Convert the quaternion to a vector fv = Matrix([f.a, f.b, f.c, f.d]) # Equation 2.0.4 (F_k in documentation) FM = fv.jacobian(stateVars)
def quaternion_derivative(quaterion, omega):
omega_quat = Quaternion(0, *omega)
q_dot = (1/2) * quaterion * omega_quat
return [getattr(q_dot, field) for field in 'abcd']
def test_quaternion_evalf():
assert Quaternion(sqrt(2), 0, 0, sqrt(3)).evalf() == Quaternion(sqrt(2).evalf(), 0, 0, sqrt(3).evalf())
assert Quaternion(1/sqrt(2), 0, 0, 1/sqrt(2)).evalf() == Quaternion((1/sqrt(2)).evalf(), 0, 0, (1/sqrt(2)).evalf())
class TestDualQuaternion():
q1 = Quaternion(a, b, c, d)
q2 = Quaternion(x, y, z, w)
q3 = Quaternion(z, b, a, d)
q4 = Quaternion(c, w, x, y)
def test_should_create_dual_quaternion(self):
dq = DualQuaternion(self.q1, self.q2)
assert isinstance(dq, DualQuaternion)
assert dq.real == self.q1
assert dq.dual == self.q2
def test_should_create_from_real_number(self):
dq = DualQuaternion(a, b)
assert dq.real == Quaternion(a)
assert dq.dual == Quaternion(b)
def test_should_create_from_default(self):
dq = DualQuaternion()
assert dq.real == Quaternion(0)
assert dq.dual == Quaternion(0)
def test_should_implement_addition(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq2 = DualQuaternion(self.q2, self.q1)
dq = dq1 + dq2
assert dq.real == self.q1 + self.q2
assert dq.dual == self.q2 + self.q1
def test_should_handle_dq_and_quat_add(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1 + self.q2
assert dq.real == self.q1 + self.q2
assert dq.dual == self.q2
def test_should_handle_dq_and_real_add(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1 + 1
assert dq.real == self.q1 + 1
assert dq.dual == self.q2
def test_should_implement_neg(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = -dq1
assert dq.real == -self.q1
assert dq.dual == -self.q2
def test_should_handle_dq_substraction(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq2 = DualQuaternion(self.q3, self.q4)
dq = dq1 - dq2
assert dq.real == self.q1 - self.q3
assert dq.dual == self.q2 - self.q4
def test_dq_should_sub_real(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1 - 1
assert dq.real == self.q1 - 1
assert dq.dual == self.q2
def test_dq_should_sub_quat(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1 - self.q3
assert dq.real == self.q1 - self.q3
assert dq.dual == self.q2
# fails since it calls __add__ of Quaternion instead of __radd__ of
# DualQuaternion in this condition
#def test_should_handle_quat_and_dq_radd(self):
#dq1 = DualQuaternion(self.q1, self.q2)
#dq = self.q2 + dq1
#assert dq.real == self.q1 + self.q2
#assert dq.dual == self.q2
def test_should_handle_real_and_dq_radd(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = 1 + dq1
assert dq.real == self.q1 + 1
assert dq.dual == self.q2
def test_should_implement_multiplication(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq2 = DualQuaternion(self.q3, self.q4)
dq = dq1 * dq2
assert dq.real == self.q1 * self.q3
assert dq.dual == self.q1 * self.q4 + self.q2 * self.q3
def test_should_have_quaternion_conj(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1.quaternion_conjugate()
assert dq.real == conjugate(self.q1)
assert dq.dual == conjugate(self.q2)
def test_should_have_dual_conj(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1.dual_number_conjugate()
assert dq.real == self.q1
assert dq.dual == -self.q2
def test_should_have_combined_conj(self):
dq1 = DualQuaternion(self.q1, self.q2)
dq = dq1.combined_conjugate()
assert dq.real == conjugate(self.q1)
assert dq.dual == -conjugate(self.q2)
def test_should_implement_norm(self):
dq = DualQuaternion(Quaternion(0, 0, 0, 1), Quaternion(0, 0, -x, 0))
nm = dq.norm()
assert nm.real == Quaternion(1)
assert nm.dual == Quaternion(0)
def test_should_construct_from_screw(self):
l = (0, 0, 1)
m = (0, -x, 0)
dq = DualQuaternion.from_screw(l, m, theta, 0)
assert dq.real == Quaternion(cos(theta * S.Half), 0, 0,
sin(theta * S.Half))
assert dq.dual == Quaternion(0, 0, -x * sin(theta * S.Half), 0)
def test_from_screw_should_raise_exception_if_l_not_unit(self):
l = (0, 0, 2)
m = (0, -x, 0)
with pytest.raises(ValueError):
DualQuaternion.from_screw(l, m, theta, 0)
def test_from_screw_should_raise_exception_if_l_m_not_orthogonal(self):
l = (0, 0, 1)
m = (0, 0, x)
with pytest.raises(ValueError):
DualQuaternion.from_screw(l, m, theta, 0)
def test_quaternion_functions():
q = Quaternion(x, y, z, w)
q1 = Quaternion(1, 2, 3, 4)
q0 = Quaternion(0, 0, 0, 0)
assert conjugate(q) == Quaternion(x, -y, -z, -w)
assert q.norm() == sqrt(w**2 + x**2 + y**2 + z**2)
assert q.normalize() == Quaternion(x, y, z,
w) / sqrt(w**2 + x**2 + y**2 + z**2)
assert q.inverse() == Quaternion(x, -y, -z,
-w) / (w**2 + x**2 + y**2 + z**2)
raises(ValueError, lambda: q0.inverse())
assert q.pow(2) == Quaternion(-w**2 + x**2 - y**2 - z**2, 2 * x * y,
2 * x * z, 2 * w * x)
assert q1.pow(-2) == Quaternion(-S(7) / 225, -S(1) / 225, -S(1) / 150,
-S(2) / 225)
assert q1.pow(-0.5) == NotImplemented
assert q1.exp() == \
Quaternion(E * cos(sqrt(29)),
2 * sqrt(29) * E * sin(sqrt(29)) / 29,
3 * sqrt(29) * E * sin(sqrt(29)) / 29,
4 * sqrt(29) * E * sin(sqrt(29)) / 29)
assert q1._ln() == \
Quaternion(log(sqrt(30)),
2 * sqrt(29) * acos(sqrt(30)/30) / 29,
3 * sqrt(29) * acos(sqrt(30)/30) / 29,
4 * sqrt(29) * acos(sqrt(30)/30) / 29)
assert q1.pow_cos_sin(2) == \
Quaternion(30 * cos(2 * acos(sqrt(30)/30)),
60 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
90 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
120 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29)
assert diff(Quaternion(x, x, x, x), x) == Quaternion(1, 1, 1, 1)
assert integrate(Quaternion(x, x, x, x), x) == \
Quaternion(x**2 / 2, x**2 / 2, x**2 / 2, x**2 / 2)
assert Quaternion.rotate_point((1, 1, 1), q1) == (1 / 5, 1, 7 / 5)
import sympy as sym from sympy.algebras.quaternion import Quaternion c0 = sym.sqrt(3) q = Quaternion(2, c0, c0, c0) p = (1, 2, -3) pp = Quaternion.rotate_point((1, 2, -3), q) print(q) print(p) print(pp) print(q * Quaternion(0, 1, 2, -3))
# https://tel.archives-ouvertes.fr/tel-00641678/document
import pytest
from sympy import *
from sympy import symbols, conjugate
from sympy import sin, cos
from sympy.abc import a, b, c, d, x, y, z, w, theta
from sympy.algebras.quaternion import Quaternion
from context import DualQuaternion
from sympy import simplify
from sympy.abc import theta, alpha
print("test")
d, a = symbols('d a')
q1 = Quaternion(1, 0, 0, 0)
q0 = Quaternion(0, 0, 0, 0)
rdi = Quaternion(0, 0, 0, d)
qti = Quaternion(cos(theta * 0.5), 0, 0, sin(theta * 0.5))
rai = Quaternion(0, a, 0, 0)
qai = Quaternion(cos(alpha * 0.5), 0, 0, sin(alpha * 0.5))
# dqti = diff(qti, theta)
# print(dqti)
dq1 = DualQuaternion(q1, 0.5 * rdi)
dq2 = DualQuaternion(qti, q0)
dq3 = DualQuaternion(q1, 0.5 * rai)
dq4 = DualQuaternion(qai, q0)
dq12 = dq1 * dq2
dq123 = dq12 * dq3
#!/usr/bin/env python3
from sympy import *
from sympy.algebras.quaternion import Quaternion
import sys
var("q0 q1 q2 q3 p0 p1 p2 p3 i j k x y z yz xz psi phi")
#Q = Quaternion(q0, 0, 0, q3)
#P = Quaternion(p0, p1, 0, 0)
Q = Quaternion(q0, 0, 0, q3)
P = Quaternion(p0, p1, 0, 0)
Qs = conjugate(Q)
Ps = conjugate(P)
#pprint(Qs)
R = P*Q
ee = (R)*Quaternion(0,0,1,0)*conjugate(R)
#a = (P)*Quaternion(0,0,1,0)*conjugate(P)
e = ee.a + ee.b * i + ee.c * j + ee.d * k
#pprint(simplify(a))
pprint(simplify(e))
#eee = solve(Eq(2*p1*sqrt(1-p1), z), (p1))
#pprint(eee)
#pprint(solve(Eq(4*q0**4 - 4*yz*q0**2 - xz**2), (q0)))
Q = Quaternion(cos(phi/2), x*sin(phi/2), y*sin(phi/2), 0)
def test_quaternion_complex_real_addition():
a = symbols("a", complex=True)
b = symbols("b", real=True)
# This symbol is not complex:
c = symbols("c", commutative=False)
q = Quaternion(w, x, y, z)
assert a + q == Quaternion(w + re(a), x + im(a), y, z)
assert 1 + q == Quaternion(1 + w, x, y, z)
assert I + q == Quaternion(w, 1 + x, y, z)
assert b + q == Quaternion(w + b, x, y, z)
raises(ValueError, lambda: c + q)
raises(ValueError, lambda: q * c)
raises(ValueError, lambda: c * q)
assert -q == Quaternion(-w, -x, -y, -z)
q1 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
q2 = Quaternion(1, 4, 7, 8)
assert q1 + (2 + 3*I) == Quaternion(5 + 7*I, 2 + 5*I, 0, 7 + 8*I)
assert q2 + (2 + 3*I) == Quaternion(3, 7, 7, 8)
assert q1 * (2 + 3*I) == \
Quaternion((2 + 3*I)*(3 + 4*I), (2 + 3*I)*(2 + 5*I), 0, (2 + 3*I)*(7 + 8*I))
assert q2 * (2 + 3*I) == Quaternion(-10, 11, 38, -5)
q1 = Quaternion(1, 2, 3, 4)
q0 = Quaternion(0, 0, 0, 0)
assert q1 + q0 == q1
assert q1 - q0 == q1
assert q1 - q1 == q0
def test_quaternion_complex_real_addition():
a = symbols("a", complex=True)
b = symbols("b", real=True)
# This symbol is not complex:
c = symbols("c", commutative=False)
q = Quaternion(x, y, z, w)
assert a + q == Quaternion(x + re(a), y + im(a), z, w)
assert 1 + q == Quaternion(1 + x, y, z, w)
assert I + q == Quaternion(x, 1 + y, z, w)
assert b + q == Quaternion(x + b, y, z, w)
assert c + q == Add(c, Quaternion(x, y, z, w), evaluate=False)
assert q * c == Mul(Quaternion(x, y, z, w), c, evaluate=False)
assert c * q == Mul(c, Quaternion(x, y, z, w), evaluate=False)
assert -q == Quaternion(-x, -y, -z, -w)
q1 = Quaternion(3 + 4 * I, 2 + 5 * I, 0, 7 + 8 * I, real_field=False)
q2 = Quaternion(1, 4, 7, 8)
assert q1 + (2 + 3 * I) == Quaternion(5 + 7 * I, 2 + 5 * I, 0, 7 + 8 * I)
assert q2 + (2 + 3 * I) == Quaternion(3, 7, 7, 8)
assert q1 * (2 + 3*I) == \
Quaternion((2 + 3*I)*(3 + 4*I), (2 + 3*I)*(2 + 5*I), 0, (2 + 3*I)*(7 + 8*I))
assert q2 * (2 + 3 * I) == Quaternion(-10, 11, 38, -5)
def test_quaternion_functions():
q = Quaternion(w, x, y, z)
q1 = Quaternion(1, 2, 3, 4)
q0 = Quaternion(0, 0, 0, 0)
assert conjugate(q) == Quaternion(w, -x, -y, -z)
assert q.norm() == sqrt(w**2 + x**2 + y**2 + z**2)
assert q.normalize() == Quaternion(w, x, y, z) / sqrt(w**2 + x**2 + y**2 + z**2)
assert q.inverse() == Quaternion(w, -x, -y, -z) / (w**2 + x**2 + y**2 + z**2)
assert q.inverse() == q.pow(-1)
raises(ValueError, lambda: q0.inverse())
assert q.pow(2) == Quaternion(w**2 - x**2 - y**2 - z**2, 2*w*x, 2*w*y, 2*w*z)
assert q**(2) == Quaternion(w**2 - x**2 - y**2 - z**2, 2*w*x, 2*w*y, 2*w*z)
assert q1.pow(-2) == Quaternion(Rational(-7, 225), Rational(-1, 225), Rational(-1, 150), Rational(-2, 225))
assert q1**(-2) == Quaternion(Rational(-7, 225), Rational(-1, 225), Rational(-1, 150), Rational(-2, 225))
assert q1.pow(-0.5) == NotImplemented
raises(TypeError, lambda: q1**(-0.5))
assert q1.exp() == \
Quaternion(E * cos(sqrt(29)),
2 * sqrt(29) * E * sin(sqrt(29)) / 29,
3 * sqrt(29) * E * sin(sqrt(29)) / 29,
4 * sqrt(29) * E * sin(sqrt(29)) / 29)
assert q1._ln() == \
Quaternion(log(sqrt(30)),
2 * sqrt(29) * acos(sqrt(30)/30) / 29,
3 * sqrt(29) * acos(sqrt(30)/30) / 29,
4 * sqrt(29) * acos(sqrt(30)/30) / 29)
assert q1.pow_cos_sin(2) == \
Quaternion(30 * cos(2 * acos(sqrt(30)/30)),
60 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
90 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
120 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29)
assert diff(Quaternion(x, x, x, x), x) == Quaternion(1, 1, 1, 1)
assert integrate(Quaternion(x, x, x, x), x) == \
Quaternion(x**2 / 2, x**2 / 2, x**2 / 2, x**2 / 2)
assert Quaternion.rotate_point((1, 1, 1), q1) == (S.One / 5, 1, S(7) / 5)
n = Symbol('n')
raises(TypeError, lambda: q1**n)
n = Symbol('n', integer=True)
raises(TypeError, lambda: q1**n)
def test_quaternion_functions():
q = Quaternion(x, y, z, w)
q1 = Quaternion(1, 2, 3, 4)
assert conjugate(q) == Quaternion(x, -y, -z, -w)
assert q.norm() == sqrt(w**2 + x**2 + y**2 + z**2)
assert q.normalize() == Quaternion(x, y, z,
w) / sqrt(w**2 + x**2 + y**2 + z**2)
assert q.inverse() == Quaternion(x, -y, -z,
-w) / (w**2 + x**2 + y**2 + z**2)
assert q.pow(2) == Quaternion(-w**2 + x**2 - y**2 - z**2, 2 * x * y,
2 * x * z, 2 * w * x)
assert q1.exp() == \
Quaternion(E * cos(sqrt(29)),
2 * sqrt(29) * E * sin(sqrt(29)) / 29,
3 * sqrt(29) * E * sin(sqrt(29)) / 29,
4 * sqrt(29) * E * sin(sqrt(29)) / 29)
assert q1._ln() == \
Quaternion(log(sqrt(30)),
2 * sqrt(29) * acos(sqrt(30)/30) / 29,
3 * sqrt(29) * acos(sqrt(30)/30) / 29,
4 * sqrt(29) * acos(sqrt(30)/30) / 29)
assert q1.pow_cos_sin(2) == \
Quaternion(30 * cos(2 * acos(sqrt(30)/30)),
60 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
90 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29,
120 * sqrt(29) * sin(2 * acos(sqrt(30)/30)) / 29)
assert diff(Quaternion(x, x, x, x), x) == Quaternion(1, 1, 1, 1)
assert integrate(Quaternion(x, x, x, x), x) == \
Quaternion(x**2 / 2, x**2 / 2, x**2 / 2, x**2 / 2)
assert Quaternion.rotate_point((1, 1, 1), q1) == (1 / 5, 1, 7 / 5)
