def test_quaternion_multiply(self):
q0 = [1, 0, 0, 0]
q = quaternion_multiply(q0, q0)
testing.assert_array_equal(q, [1, 0, 0, 0])
q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
testing.assert_array_equal(q, [28, -44, -14, 48])
def test_quaternion_inverse(self):
q0 = [1, 0, 0, 0]
q1 = quaternion_inverse(q0)
q = quaternion_multiply(q0, q1)
testing.assert_array_equal(q, [1, 0, 0, 0])
q0 = [1, 2, 3, 4]
q1 = quaternion_inverse(q0)
q = quaternion_multiply(q0, q1)
testing.assert_almost_equal(q, [1, 0, 0, 0])
def test_quaternion_conjugate(self):
q0 = [1, 0, 0, 0]
q1 = quaternion_conjugate(q0)
q = quaternion_multiply(q0, q1)
testing.assert_array_equal(q, [1, 0, 0, 0])
# batch
q0 = [[1, 0, 0, 0], [0, 1, 0, 0]]
q1 = quaternion_conjugate(q0)
q = quaternion_multiply(q0, q1)
testing.assert_array_equal(q, [[1, 0, 0, 0], [1, 0, 0, 0]])
def test_mul(self):
qr1 = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd1 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr1)
dq1 = DualQuaternion(qr1, qd1)
qr2 = normalize_vector(np.array([4, 3, 2, 1]))
x, y, z = np.array([3, 2, 1])
qd2 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr2)
dq2 = DualQuaternion(qr2, qd2)
dq = dq1 * dq2
testing.assert_almost_equal(dq.translation, [0, 4, 6])
testing.assert_almost_equal(dq.quaternion.q, [-0.4, 0.2, 0.8, 0.4])
def test_add(self):
qr1 = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd1 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr1)
dq1 = DualQuaternion(qr1, qd1)
qr2 = normalize_vector(np.array([4, 3, 2, 1]))
x, y, z = np.array([3, 2, 1])
qd2 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr2)
dq2 = DualQuaternion(qr2, qd2)
dq = (dq1 + dq2).normalize()
testing.assert_almost_equal(dq.translation, [2.0, 2.6, 2.0])
testing.assert_almost_equal(dq.quaternion.q, [0.5, 0.5, 0.5, 0.5])
def test_rotation(self):
qr = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr)
dq = DualQuaternion(qr, qd)
testing.assert_almost_equal(dq.rotation, quaternion2matrix(qr))
def __mul__(self, cls):
if isinstance(cls, Quaternion):
q = quaternion_multiply(self.q, cls.q)
return Quaternion(q=q)
elif isinstance(cls, Number):
q = self.q.copy()
return Quaternion(q=q * cls)
else:
raise TypeError("Quaternion's multiplication is only supported "
'Number or Quaternion. get {}'.format(type(cls)))
def test_normalize(self):
qr1 = normalize_vector(np.array([1, 2, 3, 4]))
x, y, z = np.array([1, 2, 3])
qd1 = 0.5 * quaternion_multiply(np.array([0, x, y, z]), qr1)
dq1 = DualQuaternion(qr1, qd1)
dq1.normalize()
testing.assert_almost_equal(dq1.dq, [
0.18257419, 0.36514837, 0.54772256, 0.73029674, -1.82574186, 0.,
0.36514837, 0.18257419
])
def dual_quaternion(self):
"""Property of DualQuaternion
Return DualQuaternion representation of this coordinate.
Returns
-------
DualQuaternion : skrobot.coordinates.dual_quaternion.DualQuaternion
DualQuaternion representation of this coordinate
"""
qr = normalize_vector(self.quaternion)
x, y, z = self.translation
qd = quaternion_multiply(np.array([0, x, y, z]), qr) * 0.5
return DualQuaternion(qr, qd)
def transform_coords(c1, c2, out=None):
"""Return Coordinates by applying c1 to c2 from the left
Parameters
----------
c1 : skrobot.coordinates.Coordinates
c2 : skrobot.coordinates.Coordinates
Coordinates
c3 : skrobot.coordinates.Coordinates or None
Output argument. If this value is specified, the results will be
in-placed.
Returns
-------
Coordinates(pos=translation, rot=q) : skrobot.coordinates.Coordinates
new coordinates
Examples
--------
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates import transform_coords
>>> from numpy import pi
>>> c1 = Coordinates()
>>> c2 = Coordinates()
>>> c3 = transform_coords(c1, c2)
>>> c3.translation
array([0., 0., 0.])
>>> c3.rotation
array([[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]])
>>> c1 = Coordinates().translate([0.1, 0.2, 0.3]).rotate(pi / 3.0, 'x')
>>> c2 = Coordinates().translate([0.3, -0.3, 0.1]).rotate(pi / 2.0, 'y')
>>> c3 = transform_coords(c1, c2)
>>> c3.translation
array([ 0.4 , -0.03660254, 0.09019238])
>>> c3.rotation
>>> c3.rotation
array([[ 1.94289029e-16, 0.00000000e+00, 1.00000000e+00],
[ 8.66025404e-01, 5.00000000e-01, -1.66533454e-16],
[-5.00000000e-01, 8.66025404e-01, 2.77555756e-17]])
"""
if out is None:
out = Coordinates()
elif not isinstance(out, Coordinates):
raise TypeError("Input type should be skrobot.coordinates.Coordinates")
out.translation = c1.translation + np.dot(c1.rotation, c2.translation)
out.rotation = quaternion_normalize(
quaternion_multiply(c1.quaternion, c2.quaternion))
return out
def __init__(self,
qr=[1, 0, 0, 0],
qd=[0, 0, 0, 0],
enforce_unit_norm=False):
if (isinstance(qd, list) or isinstance(qd, np.ndarray)) and \
len(qd) == 3:
x, y, z = qd
qr = quaternion_normalize(qr)
qd = 0.5 * quaternion_multiply([0, x, y, z], qr)
self.qr = qr
self.qd = qd
if enforce_unit_norm:
norm = self.norm
if not np.allclose(norm[0], [1]):
raise ValueError("Dual quaternoin's norm "
'should be 1, but gives {}'.format(norm[0]))
