Webots VRML conversion script

convert.py

# Converts rotation and translation VRML fields.
# It takes the clipboard content, applies transforms, and pastes the transformed VRML to the clipboard.
#
# Dependencies:
#
#   pip3 install clipboard numpy transforms3d
#   sudo apt install xclip
#
# GNOME SHORTCUT
# In GNOME, you can run the script as a keyboard shortcut.
# Go to `Settings > Keyboard Shortcuts > +`

import clipboard
import numpy as np
import transforms3d
import re


GEOMETRY = False
TRANSLATE_ONLY = False
TRANSLATE_VECTOR = [-2, 0, 0]

ROTATION = [np.pi/2, -np.pi/2, 0]   # RUB
# ROTATION = [np.pi/2, np.pi/2, 0]    # RUB with x-inverted
# ROTATION = [np.pi/2, 0, 0]
if GEOMETRY:
    ROTATION = [-np.pi/2, 0, 0]     # Plane for sure
ROTATION_MATRIX = transforms3d.euler.euler2mat(ROTATION[0], ROTATION[1], ROTATION[2], 'rxyz')


def vector_to_string_array(vector, decimals, zero_one_decimals=None):
    if zero_one_decimals is None:
        # Zero and one are special cases and typically it is fine to be more agressive when rounding
        zero_one_decimals = int(0.7 * decimals)
    new_str = []
    for value in vector:
        if abs(value) < 1 / (10**zero_one_decimals):
            new_str.append('0')
        elif abs(value - 1) < 1 / (10**zero_one_decimals):
            new_str.append('1')
        elif abs(value + 1) < 1 / (10**zero_one_decimals):
            new_str.append('-1')
        else:
            new_str.append(str(round(value, decimals)))
    return new_str


def convert_translation(translation):
    translation = [float(value) for value in translation]
    if TRANSLATE_ONLY:
        return (np.array(translation) + TRANSLATE_VECTOR).flatten()
    return (ROTATION_MATRIX @ np.array(translation)).flatten()


def convert_mesh(geometry_points):
    for i in range(0, len(geometry_points), 3):
        new_point = convert_translation(geometry_points[i:i+3])
        geometry_points[i:i+3] = vector_to_string_array(new_point, 7)
    return ' '.join(geometry_points)


def convert_orientation(rotation_angle_axis):
    rotation_angle_axis = [float(value) for value in rotation_angle_axis]
    orientation = transforms3d.axangles.axangle2mat(rotation_angle_axis[:3], rotation_angle_axis[3])
    if GEOMETRY:
        new_rotation = orientation @ ROTATION_MATRIX
    else:
        new_rotation = ROTATION_MATRIX @ orientation
    new_rotation_axis, new_rotation_angle = transforms3d.axangles.mat2axangle(new_rotation)
    return ' '.join(vector_to_string_array(list(new_rotation_axis) + list([new_rotation_angle]), 6))


def is_number(string):
    try:
        float(string)
        return True
    except ValueError:
        return False


def main():
    text = clipboard.paste()
    vector = [x for x in re.compile('\n|,| ').split(text) if is_number(x)]
    out = 'ERROR'

    if len(vector) == 0:
        new_rotation_axis, new_rotation_angle = transforms3d.axangles.mat2axangle(ROTATION_MATRIX)
        out = ' '.join(vector_to_string_array(list(new_rotation_axis) + list([new_rotation_angle]), 6))
    elif len(vector) == 3:
        out = convert_translation(vector)
        out = ' '.join(vector_to_string_array(out, 4))
    elif len(vector) == 4:
        out = convert_orientation(vector)
    else:
        out = convert_mesh(vector)

    clipboard.copy(out)


if __name__ == '__main__':
    main()