added quaternion hemisphere checks

armarx_control/utils/mirror_trajectory.py

@@ -127,7 +127,11 @@ def mirror_pose(line: List[float], space: Space, conversion_direction: Conversio
         ], dtype=float)
         # on purpose coefficient-wise, and not a matrix multiplication
         converted_pose = coeff_wise_factors * pose
-        converted_line += list(matrix_to_pose7(converted_pose))
+        quaternion = matrix_to_pose7(converted_pose)
+        # preventing unwanted hemisphere flips of quaternions
+        if (float(line[4])>=0 and quaternion[3]<0) or (float(line[4])<0 and quaternion[3]>=0):
+            quaternion = quaternion*np.array([1, 1, 1, -1, -1, -1, -1], dtype=float)
+        converted_line += list(quaternion)
     elif space == Space.JOINT_SPACE:
         # for ARMAR-6, determined using RobotViewer
         coeff_wise_factors = np.array([