Add unit test

dfki-ric · Sep 28, 2023 · 594a765 · 594a765
1 parent e653d3b
commit 594a765
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/pytransform3d/test/test_transformations.py b/pytransform3d/test/test_transformations.py
@@ -914,3 +914,14 @@ def test_jacobian_se3():
 
         J_inv_J = np.dot(J_inv, J)
         assert_array_almost_equal(J_inv_J, np.eye(6))
+
+
+def test_pq_slerp():
+    start = np.array([0.2, 0.3, 0.4, 1.0, 0.0, 0.0, 0.0])
+    end = np.array([1.0, 0.5, 0.8, 0.0, 1.0, 0.0, 0.0])
+    pq_05 = pt.pq_slerp(start, end, 0.5)
+    assert_array_almost_equal(pq_05, [0.6, 0.4, 0.6, 0.707107, 0.707107, 0, 0])
+    pq_025 = pt.pq_slerp(start, end, 0.25)
+    assert_array_almost_equal(pq_025, [0.4, 0.35, 0.5, 0.92388, 0.382683, 0, 0])
+    pq_075 = pt.pq_slerp(start, end, 0.75)
+    assert_array_almost_equal(pq_075, [0.8, 0.45, 0.7, 0.382683, 0.92388, 0, 0])
diff --git a/pytransform3d/transformations/_pq_operations.py b/pytransform3d/transformations/_pq_operations.py
@@ -32,8 +32,8 @@ def pq_slerp(start, end, t):
     """
     start = check_pq(start)
     end = check_pq(end)
-    start_p, start_q = np.split(start, 3)
-    end_p, end_q = np.split(end, 3)
+    start_p, start_q = np.array_split(start, (3,))
+    end_p, end_q = np.array_split(end, (3,))
     q_t = rotations.quaternion_slerp(start_q, end_q, t, shortest_path=True)
     p_t = start_p + t * (end_p - start_p)
     return np.hstack((p_t, q_t))