diff --git a/armarx_control/utils/mirror_trajectory.py b/armarx_control/utils/mirror_trajectory.py index 01213eb70f06768e671b8695886c6bd063ab0900..98ffa64a6ae0aaa8aa01401db291dfc5ec06b9be 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([