From 7892398b49e68b9749deafb1dc657d370fde4ea2 Mon Sep 17 00:00:00 2001
From: Tilman Daab <tilman.daab@kit.edu>
Date: Tue, 26 Nov 2024 10:31:48 +0100
Subject: [PATCH] add tool to mirror trajectories (currently for ARMAR-6)
---
armarx_control/utils/mirror_trajectory.py | 197 ++++++++++++++++++++++
1 file changed, 197 insertions(+)
create mode 100644 armarx_control/utils/mirror_trajectory.py
diff --git a/armarx_control/utils/mirror_trajectory.py b/armarx_control/utils/mirror_trajectory.py
new file mode 100644
index 00000000..01213eb7
--- /dev/null
+++ b/armarx_control/utils/mirror_trajectory.py
@@ -0,0 +1,197 @@
+import csv
+import os.path
+import sys
+
+from enum import Enum, auto
+from typing import List, Tuple
+import numpy as np
+
+from scipy.spatial.transform import Rotation
+
+
+class ConversionDirection(Enum):
+ """
+ From which arm to which arm to convert
+ """
+ LEFT_TO_RIGHT = auto()
+ RIGHT_TO_LEFT = auto()
+
+
+class Space(Enum):
+ """
+ Space in which the trajectory is represented (joint space or task space, i.e., end effector pose)
+ """
+ JOINT_SPACE = auto()
+ TASK_SPACE = auto()
+
+
+def resolve_path(
+ source_path_string: str, conversion_direction: ConversionDirection or None = None
+) -> Tuple[str, str, ConversionDirection]:
+ """
+ Resolve path without having this script depending on ArmarX (which would offer the ArmarX package path);
+ e.g., use $ARMARX_WORKSPACE or even the package dir variable.
+ The function also detects in which direction to convert, and determines the target path.
+
+ :param source_path_string: Path to the csv file to be converted,
+ may contain environment variables like $ARMARX_WORKSPACE
+ :param conversion_direction: LEFT_TO_RIGHT or RIGHT_TO_LEFT, or None for auto-detection based on 'LeftArm' and
+ 'RightArm' in the source_path
+ :return: [source path, target path, conversion direction]
+ """
+ source_path = os.path.expandvars(source_path_string)
+
+ if 'LeftArm' in source_path:
+ if conversion_direction is None:
+ conversion_direction = ConversionDirection.LEFT_TO_RIGHT
+ target_path = source_path.replace('LeftArm', 'RightArm')
+ elif 'RightArm' in source_path:
+ if conversion_direction is None:
+ conversion_direction = ConversionDirection.RIGHT_TO_LEFT
+ target_path = source_path.replace('RightArm', 'LeftArm')
+ else:
+ assert conversion_direction is not None, 'Could not detect whether to convert from left to right, or vice '\
+ 'versa; please specify manually'
+ assert source_path.endswith('.csv')
+ if conversion_direction is ConversionDirection.LEFT_TO_RIGHT:
+ target_path = source_path.replace('.csv', '-right.csv')
+ elif conversion_direction is ConversionDirection.RIGHT_TO_LEFT:
+ target_path = source_path.replace('.csv', '-left.csv')
+ else:
+ raise KeyError
+ return source_path, target_path, conversion_direction
+
+
+def detect_space(header: List[str]):
+ """
+ Evaluate the header of the csv file to detect the space
+ :param header: column labels
+ :return: space in which the poses are represented
+ """
+ if header == ['timestamp', 'x', 'y', 'z', 'qw', 'qx', 'qy', 'qz']:
+ space = Space.TASK_SPACE
+ # TODO: potentially extend the detection code for joint space
+ elif header[1] == 'ArmL1_Cla1' or header[1] == 'ArmR1_Cla1':
+ space = Space.JOINT_SPACE
+ else:
+ raise RuntimeError('Could not detect the space')
+ return space
+
+
+def pose7_to_matrix(pose7: np.ndarray) -> np.ndarray:
+ """
+ convert the x/y/z/qw/qx/qy/qz task space pose to a homogeneous matrix
+ :param pose7: pose in task space as x/y/z/qw/qx/qy/qz
+ :return: Pose in task space as homogeneous matrix
+ """
+ position = np.array(pose7[:3])
+ orientation = Rotation.from_quat(pose7[3:7], scalar_first=True)
+ pose = np.identity(4)
+ pose[:3, 3] = position
+ pose[:3, :3] = orientation.as_matrix()
+ return pose
+
+
+def matrix_to_pose7(matrix: np.ndarray) -> np.ndarray:
+ """
+ convert the task space pose provided as a homogeneous matrix to a x/y/z/qw/qx/qy/qz task space pose
+ :param matrix: Pose in task space as homogeneous matrix
+ :return: pose in task space as x/y/z/qw/qx/qy/qz
+ """
+ assert matrix.shape == (4, 4)
+ pose7 = np.empty(7)
+ pose7[:3] = matrix[:3, 3]
+ orientation = Rotation.from_matrix(matrix[:3, :3])
+ pose7[3:] = orientation.as_quat(scalar_first=True)
+ return pose7
+
+
+def mirror_pose(line: List[float], space: Space, conversion_direction: ConversionDirection):
+ """
+ Convert an entry of the trajectory csv file, corresponding to a pose
+ :param line: entries of the csv line
+ :param space: space in which the poses are represented
+ :param conversion_direction: from which arm to which one to convert
+ :return: modified entries of the csv line, corresponding to the mirrored pose
+ """
+ converted_line = []
+ # timestamp remains unchanged
+ converted_line.append(line[0])
+ if space == Space.TASK_SPACE:
+ pose = pose7_to_matrix(line[1:])
+ coeff_wise_factors = np.array([
+ [1, -1, -1, -1],
+ [-1, 1, 1, 1],
+ [-1, 1, 1, 1],
+ [1, 1, 1, 1]
+ ], 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))
+ elif space == Space.JOINT_SPACE:
+ # for ARMAR-6, determined using RobotViewer
+ coeff_wise_factors = np.array([
+ -1, # Cla
+ -1, # Sho1
+ +1, # Sho2
+ -1, # Sho3
+ +1, # Elb1
+ -1, # Elb2
+ -1, # Wri1
+ +1 # Wri2
+ ], dtype=float)
+ converted_line += list(coeff_wise_factors * np.array(line[1:], dtype=float))
+ else:
+ raise TypeError
+ return converted_line
+
+
+def mirror_header(header: List[str], conversion_direction: ConversionDirection) -> List[str]:
+ """
+ Adapt the header of the csv file. For task space trajectories, nothing changes. For joint space trajectories, the
+ joint names might need to be adapted. Here, it is assumed that all "L" and "R" characters are to be swapped in these
+ joint names, which is a hard-coded solution for the given robots.
+ TODO: extend if needed
+ :param header: column labels
+ :param conversion_direction: from which arm to which one to convert
+ :return: column labels for mirrored motion
+ """
+ if conversion_direction is conversion_direction.LEFT_TO_RIGHT:
+ converted_header = [entry.replace('L', 'R') for entry in header]
+ elif conversion_direction is conversion_direction.RIGHT_TO_LEFT:
+ converted_header = [entry.replace('R', 'L') for entry in header]
+ else:
+ KeyError
+ return converted_header
+
+
+def main(path: str, conversion_direction: ConversionDirection or None = None):
+ source_path, target_path, conversion_direction = resolve_path(path, conversion_direction)
+ print(f'converting from\n\t{source_path}\nto\n\t{target_path}')
+ converted_lines = []
+ assert os.path.isfile(source_path), f'Input file does not exist: {source_path}'
+ with open(source_path, 'r') as f:
+ reader = csv.reader(f)
+ header = next(reader)
+ space = detect_space(header)
+ for line in reader:
+ converted_lines.append(mirror_pose(line, space, conversion_direction))
+
+ converted_header = mirror_header(header, conversion_direction)
+ with open(target_path, 'w') as f:
+ writer = csv.writer(f)
+ f.write(','.join(converted_header) + '\n')
+ writer.writerows(converted_lines)
+
+
+if __name__ == '__main__':
+ if len(sys.argv) > 1:
+ # use command line parameter
+ path = sys.argv[1]
+ else:
+ dir_path = '$ARMARX_WORKSPACE/armarx_integration/robots/armar7/motions/data/armar7_motions/mobile_kitchen/RightArm/'
+ file_name_base = 'through_door_pose_arm_primary_lower_01'
+ for suffix in ['-ts-forward', '-ts-backward', '-update-js-forward', '-update-js-backward']:
+ file_name = file_name_base + suffix + '.csv'
+ path = os.path.join(dir_path, file_name)
+ main(path, None)
--
GitLab