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]))