From 529b34e2211b4abb4ce2d7f23e0601dc8c3891ac Mon Sep 17 00:00:00 2001
From: Franziska Krebs <franziska.krebs@kit.edu>
Date: Fri, 14 Feb 2025 11:23:43 +0100
Subject: [PATCH] added quaternion hemisphere checks
---
armarx_control/utils/mirror_trajectory.py | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/armarx_control/utils/mirror_trajectory.py b/armarx_control/utils/mirror_trajectory.py
index 01213eb7..98ffa64a 100644
--- a/armarx_control/utils/mirror_trajectory.py
+++ b/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([
--
GitLab