(refactor) Removes numpy-quaternion dependency and implements transformations using numpy

Author: mkabtoul

pyproject.toml

@@ -21,7 +21,6 @@ keywords = ["robotics", "gpgpu", "navigation", "control", "planning", "mapping"]
 
 dependencies = [
     "numpy<=1.26.4",
-    "numpy-quaternion>=2023.0.3",
     "toml",
     "attrs>=23.2.0",
     "pyyaml",

src/kompass_core/datatypes/pose.py

@@ -1,35 +1,30 @@
 from typing import Union
 
 import numpy as np
-import quaternion
-from quaternion import quaternion as quat
 
 
 def _equal_approx(
-    v1, v2, is_quaternion=False, absolute_tolerance=0.01
+    v1: np.ndarray,
+    v2: np.ndarray,
+    is_quaternion: bool = False,
+    absolute_tolerance: float = 0.01,
 ) -> Union[bool, np.bool_]:
     """
-        check if two vector/quaternion arrays are approximately equals
-        to within absolute_tolerance
-
-    :param      v1:                 vector 1
-    :type       v1:                 numpy/ numpy-quaternion
-    :param      v2:                 vector 2
-    :type       v2:                 vector numpy / numpy-quaternion
-    :param      is_quaternion:      is it quaternion check?, defaults to False
-    :type       is_quaternion:      bool, optional
-    :param      absolute_tolerance: tolerate some difference if exist, defaults to 0.01
-    :type       absolute_tolerance: float, optional
-
-    :return:    is both vectors are equal?
-    :rtype:     bool
+    Check if two vectors or quaternions are approximately equal within a tolerance.
+
+    :param v1: First vector or quaternion [w, x, y, z]
+    :param v2: Second vector or quaternion
+    :param is_quaternion: Set to True if comparing quaternions
+    :param absolute_tolerance: Absolute tolerance for comparison
+    :return: True if approximately equal
     """
     if is_quaternion:
-        equal = quaternion.allclose(a=v1, b=v2, rtol=0.0, atol=absolute_tolerance)
+        # q and -q represent the same rotation
+        return np.allclose(v1, v2, rtol=0.0, atol=absolute_tolerance) or np.allclose(
+            v1, -v2, rtol=0.0, atol=absolute_tolerance
+        )
     else:
-        equal = np.allclose(a=v1, b=v2, rtol=0.0, atol=absolute_tolerance)
-
-    return equal
+        return np.allclose(v1, v2, rtol=0.0, atol=absolute_tolerance)
 
 
 class PoseData:
@@ -134,14 +129,14 @@ def get_position(self) -> np.ndarray:
         """
         return np.array([self.x, self.y, self.z], dtype=np.float32)
 
-    def get_orientation(self) -> quaternion.quaternion:
+    def get_orientation(self) -> np.ndarray:
         """
         Get the orientation represented as quaternion
 
         :return: orientation represented as quaternion
-        :rtype: quaternion.quaternion
+        :rtype: np.ndarray
         """
-        return quat(self.qw, self.qx, self.qy, self.qz)
+        return np.array([self.qw, self.qx, self.qy, self.qz])
 
     def get_yaw(self) -> np.float64:
         """

src/kompass_core/utils/geometry.py

@@ -2,8 +2,6 @@
 from typing import List, Union, Tuple
 
 import numpy as np
-import quaternion
-from quaternion import quaternion as quat
 
 from ..datatypes.pose import PoseData
 from ..datatypes.laserscan import LaserScanData
@@ -81,21 +79,41 @@ def probability_of_collision(
         return prop_col
 
 
-def _rotate_vector_by_quaternion(q: quaternion.quaternion, v: List) -> List:
+def _np_quaternion_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
     """
-    rotate a vector v by a rotation quaternion q
+    Multiplies two quaternions q1 * q2
+    Each quaternion is an array [w, x, y, z]
+    """
+    w0, x0, y0, z0 = q1
+    w1, x1, y1, z1 = q2
+    return np.array([
+        w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1,
+        w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1,
+        w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1,
+        w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1,
+    ])
+
+
+def _np_quaternion_conjugate(q: np.ndarray) -> np.ndarray:
+    """
+    Returns the conjugate of a quaternion
+    """
+    w, x, y, z = q
+    return np.array([w, -x, -y, -z])
+
 
-    :param      q: the rotation to perform
-    :type       q: quaternion.quaternion
-    :param      v: the vector to be rotated
-    :type       v: List
+def _rotate_vector_by_quaternion(q: np.ndarray, v: List[float]) -> List[float]:
+    """
+    Rotate a 3D vector v by a quaternion q.
 
-    :return:    the rotated position of the vector
-    :rtype:     List
+    :param      q: quaternion [w, x, y, z]
+    :param      v: vector [x, y, z]
+    :return:    rotated vector
     """
-    vq = quat(0, 0, 0, 0)
-    vq.imag = v
-    return (q * vq * q.inverse()).imag
+    vq = np.array([0.0, *v])
+    q_conj = _np_quaternion_conjugate(q)
+    rotated_vq = _np_quaternion_multiply(_np_quaternion_multiply(q, vq), q_conj)
+    return rotated_vq[1:].tolist()
 
 
 def get_pose_target_in_reference_frame(
@@ -119,21 +137,24 @@ def get_pose_target_in_reference_frame(
     reference_position = reference_pose.get_position()
     reference_rotation = reference_pose.get_orientation()
 
-    orientation_target_in_ref = reference_rotation.inverse() * orientation_target
+    orientation_target_in_ref = (
+        _np_quaternion_conjugate(reference_rotation) * orientation_target
+    )
 
     position_target_in_ref = _rotate_vector_by_quaternion(
-        reference_rotation.inverse(), (position_target - reference_position).tolist()
+        _np_quaternion_conjugate(reference_rotation),
+        (position_target - reference_position).tolist(),
     )
 
     target_pose_in_ref = PoseData()
     target_pose_in_ref.set_pose(
         x=position_target_in_ref[0],
         y=position_target_in_ref[1],
         z=position_target_in_ref[2],
-        qw=orientation_target_in_ref.w,
-        qx=orientation_target_in_ref.x,
-        qy=orientation_target_in_ref.y,
-        qz=orientation_target_in_ref.z,
+        qw=orientation_target_in_ref[0],
+        qx=orientation_target_in_ref[1],
+        qy=orientation_target_in_ref[2],
+        qz=orientation_target_in_ref[3],
     )
 
     return target_pose_in_ref
@@ -209,7 +230,7 @@ def from_2d_to_PoseData(x: float, y: float, heading: float) -> PoseData:
     :rtype: PoseData
     """
     pose_data = PoseData()
-    quat_data: quat = from_euler_to_quaternion(heading, 0.0, 0.0)
+    quat_data: np.ndarray = from_euler_to_quaternion(heading, 0.0, 0.0)
     pose_data.set_position(x, y, 0.0)
     pose_data.set_orientation(
         qw=quat_data[0], qx=quat_data[1], qy=quat_data[2], qz=quat_data[3]

src/kompass_cpp/tests/trajectory_sampler_plt.py

@@ -32,7 +32,9 @@ def plot_samples(figure_name, trajectories, reference=None):
     try:
         import matplotlib.pyplot as plt
     except ImportError:
-        print("Matplotlib is not installed. Test figures will not be generated. To generate figures run 'pip install matplotlib'")
+        print(
+            "Matplotlib is not installed. Test figures will not be generated. To generate figures run 'pip install matplotlib'"
+        )
         return
 
     if reference: