add tool to mirror trajectories (currently for ARMAR-6)

add a script to mirror motions, currently intended for ARMAR-6 joints (might directly work for ARMAR-DE and ARMAR-7, but not sure)

moved from https://git.h2t.iar.kit.edu/research-projects/eurobin/wp2-skills-nancy/-/tree/main/python/trajectory-tools/trajectory-tools?ref_type=heads based on a discussion with @gao

armarx_control/utils/mirror_trajectory.py

+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)