Python library for preprocessing of XR motion tracking data for machine learning applications.
from .resample import resample
from .to_velocity import to_velocity, compute_velocities_simple, compute_velocities_quats
from .to_body_relative import to_body_relative
from .fix_controller_mapping import fix_controller_mapping
from .to_acceleration import to_acceleration
from .canonicalize_quaternions import canonicalize_quaternions

motion_learning_toolbox/__init__.py
cschell/Motion-Learning-Toolbox
from typing import List
import numpy as np
import pandas as pd
import quaternionic


def canonicalize_quaternions(data: pd.DataFrame, joint_names: List[str], inplace=False) -> pd.DataFrame:
    """
    Canonicalize the quaternions in the DataFrame for a given list of joint names.
    
    The function modifies the quaternion rotation data to follow a canonical form. 
    Specifically, it ensures that the w-component of the quaternion is non-negative and that the returned quaternion is normalized.
    This provides a unique representation for each rotation, which is useful for machine learning applications.
    
    Parameters:
    -----------
    data : pd.DataFrame
        DataFrame containing quaternion rotation data. The DataFrame should have columns in the format "{joint_name}_rot_{wxyz}" for each joint.
        
    joint_names : List[str]
        List of joint names for which quaternion data should be canonicalized. Each joint name should correspond to quaternion columns in the DataFrame.
        
    inplace : bool, optional
        If True, modifies the DataFrame in place. Otherwise, returns a new DataFrame with the canonicalized quaternions. Default is False.
        
    Returns:
    --------
    pd.DataFrame
        DataFrame with canonicalized quaternion data for the joints specified by `joint_names`.
    """
    if not inplace:
        data = data.copy()

    for joint in joint_names:
        rotation_column_names = [f"{joint}_rot_{xyzw}" for xyzw in "wxyz"]
        canonicalized_quaternion = data[rotation_column_names].to_numpy() * np.sign(data[rotation_column_names[0]]).to_numpy()[:, None]
        data[rotation_column_names] = quaternionic.array(canonicalized_quaternion).normalized.ndarray

    return data

motion_learning_toolbox/canonicalize_quaternions.py
cschell/Motion-Learning-Toolbox
from typing import Dict
import pandas as pd

from motion_learning_toolbox.to_body_relative import to_body_relative


def fix_controller_mapping(
    data: pd.DataFrame,
    left_controller_name: str,
    right_controller_name: str,
    coordinate_system: Dict[str, str],
    reference_joint=None,
    is_br=False,
    inplace=False,
) -> pd.DataFrame:
    """
    Checks if the specified target joints are swapped and swaps them if necessary. Assumes left handed coordinate system.

    :param data: A DataFrame containing tracking data; to work correctly, this data should be encoded into body relative!
    :param left_controller_name: name of the left controller.
    :param right_controller_name: name of the right controller.
    :param right_vector_axis: the axis used for comparing target joints based on their mean values.
    :param coordinate_system: A dictionary specifying the used coordinate system.
    :param reference_joint: The reference joint used as the origin of the body-relative coordinate system.
    :param is_br: the implementation assumes that `data` has not yet been converted to body-relative, which is a necessary requirement. Should the data already be in BR, set is_br to True.
    :param inplace: If True, the result is stored in the original DataFrame (optional).

    :return: A DataFrame where the mapping of the controllers is maintained in comparison to `right_vector_axis`.
    """
    if not is_br:
        br_data = to_body_relative(
            data,
            target_joints=[left_controller_name, right_controller_name],
            coordinate_system=coordinate_system,
            reference_joint=reference_joint,
        )
    else:
        br_data = data

    l_mean = br_data[f"{left_controller_name}_pos_{coordinate_system['right']}"].mean()
    r_mean = br_data[f"{right_controller_name}_pos_{coordinate_system['right']}"].mean()

    if r_mean < l_mean:
        left_controller_name_rename_mapping = {
            c: c.replace(left_controller_name, right_controller_name)
            for c in data.columns
            if left_controller_name in c
        }
        right_controller_name_rename_mapping = {
            c: c.replace(right_controller_name, left_controller_name)
            for c in data.columns
            if right_controller_name in c
        }
        data = data.rename(
            columns={
                **left_controller_name_rename_mapping,
                **right_controller_name_rename_mapping,
            },
            inplace=inplace,
            copy=~inplace,
        )
    return data

motion_learning_toolbox/fix_controller_mapping.py
cschell/Motion-Learning-Toolbox
from typing import List
import numpy as np
import pandas as pd
from scipy.spatial.transform import Rotation
from scipy.spatial.transform import Slerp


def resample(data: pd.DataFrame, target_fps: float, joint_names: List[str]):
    """
    Resamples a recording DataFrame to a target frames-per-second (FPS) rate.

    :param data: A DataFrame containing the original tracking data; the DataFrame needs to have an index of type "timedelta64" (use `pd.to_timedelta` to convert integer indices).
    :param target_fps: The target frames-per-second (FPS) rate for resampling.
    :param joint_names: A list of joint names for which the data will be resampled.

    :return: A new DataFrame containing the resampled data with the specified target FPS.
    """

    assert data.index.inferred_type == "timedelta64", f"dataframe index has to be timedelta64, instead it was '{data.index.inferred_type}'"

    position_columns = [f"{joint}_pos_{xyz}" for joint in joint_names for xyz in "xyz"]
    orientation_columns = [f"{joint}_rot_{xyzw}" for joint in joint_names for xyzw in "xyzw"]

    feature_columns = position_columns + orientation_columns

    features = data[feature_columns].copy()

    assert len(features.columns) == len(feature_columns)

    mspf = 1000 / target_fps

    original_index = features.index.total_seconds() * 1000
    target_index = np.arange(original_index.min(), original_index.max(), mspf)

    interpolated_features = pd.DataFrame(index=pd.TimedeltaIndex(target_index, name="timestamp", unit="ms"))
    features.loc[:, position_columns] = features[position_columns].interpolate("time")

    assert not any(features[position_columns].isna().any())

    for pos_feature_name in position_columns:
        column = features[pos_feature_name]
        interpolated_features[pos_feature_name] = np.interp(x=target_index, xp=original_index, fp=column)

    for joint in joint_names:
        joint_orientation_features = [f"{joint}_rot_{c}" for c in "xyzw"]
        orientational_features = features[joint_orientation_features].dropna()
        rotations = Rotation.from_quat(orientational_features)
        dropped_na_original_index = orientational_features.index.total_seconds() * 1000
        slerp = Slerp(dropped_na_original_index, rotations)
        interpolated_features[joint_orientation_features] = slerp(target_index).as_quat()

    return interpolated_features

motion_learning_toolbox/resample.py
cschell/Motion-Learning-Toolbox
from . import to_velocity
import pandas as pd


def to_acceleration(data: pd.DataFrame, inplace=False) -> pd.DataFrame:
    """
    Calculates acceleration from position and/or rotation data.

    :param data: A DataFrame or Series containing position and/or rotation data.
    :param inplace: If True, the result is stored in the original DataFrame (optional).
    :return: A DataFrame containing the calculated acceleration.
    """

    velocities = to_velocity(data, inplace=inplace)
    acceleration = to_velocity(velocities, inplace=inplace)
    return acceleration


motion_learning_toolbox/to_acceleration.py
cschell/Motion-Learning-Toolbox
from typing import Dict, List
import quaternionic
import numpy as np
import pandas as pd


def quaternion_composition(quaternion_array1, quaternion_array2):
    w1, x1, y1, z1 = (
        quaternion_array1[:, 0],
        quaternion_array1[:, 1],
        quaternion_array1[:, 2],
        quaternion_array1[:, 3],
    )
    w2, x2, y2, z2 = (
        quaternion_array2[:, 0],
        quaternion_array2[:, 1],
        quaternion_array2[:, 2],
        quaternion_array2[:, 3],
    )

    w_composed = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x_composed = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y_composed = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z_composed = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

    composed_quaternions = quaternionic.array(
        np.column_stack((w_composed, x_composed, y_composed, z_composed))
    ).normalized
    return composed_quaternions


def to_body_relative(
    frames: pd.DataFrame,
    target_joints: List[str],
    coordinate_system: Dict[str, str],
    reference_joint="head",
):
    """
    Transforms position and rotation data into a body-relative coordinate system.

    :param frames: A DataFrame or Series containing position and/or rotation data.
    :param target_joints: A list of joints to be transformed.
    :param coordinate_system: A dictionary specifying the coordinate system for the transformation.
    :param reference_joint: The reference joint used as the origin of the body-relative coordinate system (default is "head").
    """
    reference_pos_columns = [f"{reference_joint}_pos_{xyz}" for xyz in "xyz"]
    target_dtype = frames[reference_pos_columns[0]].to_numpy().dtype
    min_float32_dtype = "float32" if target_dtype == np.float16 else target_dtype
    
    FORWARD = "xyz".index(coordinate_system["forward"])
    RIGHT = "xyz".index(coordinate_system["right"])
    UP = "xyz".index(coordinate_system["up"])

    assert FORWARD != RIGHT != UP

    FORWARD_DIRECTION = np.identity(3, dtype=target_dtype)[FORWARD]
    UP_DIRECTION = np.identity(3, dtype=target_dtype)[UP]

    num_samples = len(frames)

    ## parse rotations of the reference joint (the head)
    reference_rotation_names = [f"{reference_joint}_rot_{c}" for c in "wxyz"]
    reference_rotations = quaternionic.array(
        frames[reference_rotation_names], dtype=target_dtype
    ).normalized.astype(min_float32_dtype)

    ## retrieve projection of viewing direction of the reference joint on
    ## the horizontal plane by first applying the head rotation onto the
    ## forward vector and then zeroing out the UP axis
    horizontal_plane_projections = reference_rotations.rotate(FORWARD_DIRECTION)

    horizontal_plane_projections[:, UP] = 0

    rotations_around_up_axis = np.arccos(
        (horizontal_plane_projections @ FORWARD_DIRECTION)
        / (
            np.linalg.norm(FORWARD_DIRECTION)
            * np.linalg.norm(horizontal_plane_projections, axis=1)
        )
    )

    ## compute correction rotation
    # find out into which direction the vectors have to be rotated
    correction_rotation_directions = -np.sign(horizontal_plane_projections[:, RIGHT])

    # build euler angle rotation vector for rotation around UP axis
    # (usage of `.from_axis_angle` feels a bit hacky, but that's easier than building
    # a rotation matrix from scratch)
    correction_rotations_raw = np.zeros((num_samples, 3), dtype=target_dtype)
    correction_rotations_raw[:, UP] = (
        correction_rotation_directions * rotations_around_up_axis
    )
    correction_rotations = quaternionic.array.from_axis_angle(correction_rotations_raw).astype(min_float32_dtype)

    ## apply correction positions and rotations
    relative_positions_and_rotations = pd.DataFrame()

    for joint_name in target_joints:  # => joint_name is either `right_hand` or `left_hand`
        # apply rotations to position vector of `joint_name`
        joint_position_names = [f"{joint_name}_pos_{c}" for c in "xyz"]

        shifted_positions = (
            frames[joint_position_names].values - frames[reference_pos_columns].values
        )

        shifted_and_rotated_positions = np.einsum(
            "ijk,ik->ij", correction_rotations.to_rotation_matrix, shifted_positions
        )
        relative_positions_and_rotations[
            joint_position_names
        ] = shifted_and_rotated_positions

        # rotate the world rotation of `joint_name` by the correction rotation and save quaternion representations
        joint_rotation_names = [f"{joint_name}_rot_{c}" for c in "wxyz"]

        sr_rotations = quaternionic.array(frames[joint_rotation_names]).astype(target_dtype)
        br_rotations = quaternion_composition(
            correction_rotations.ndarray, sr_rotations.ndarray
        )

        relative_positions_and_rotations[
            joint_rotation_names
        ] = br_rotations.normalized.astype(target_dtype).ndarray

    # add horizontal rotations of reference joint
    relative_positions_and_rotations[reference_rotation_names] = (
        correction_rotations * reference_rotations
    ).normalized.astype(target_dtype).ndarray

    return relative_positions_and_rotations


motion_learning_toolbox/to_body_relative.py
cschell/Motion-Learning-Toolbox
import numpy as np
import pandas as pd
from scipy.spatial.transform import Rotation


def compute_velocities_simple(data: pd.DataFrame, inplace=False) -> pd.DataFrame:
    """
    Calculates velocities from position data using a simple differencing method.

    :param data: A DataFrame containing position data.
    :param inplace: If True, the velocities are calculated in-place.
    :return: A DataFrame containing the calculated velocities with "delta_" prefix.
    """
    velocities = data if inplace else data.copy()

    step_size = 1

    velocities.iloc[step_size:] = (
        velocities.values[step_size:] - velocities.values[:-step_size]
    )
    velocities.iloc[:step_size] = np.nan
    velocities = velocities.add_prefix("delta_")
    return velocities


def compute_velocities_quats(data: pd.DataFrame, inplace=False) -> pd.DataFrame:
    """
    Calculates velocities from rotation data using a simple differencing method.

    :param data: A DataFrame containing rotation data.
    :param inplace: If True, the velocities are calculated in-place.
    :return: A DataFrame containing the calculated velocities with "delta_" prefix.
    """
    velocities = data if inplace else data.copy()

    step_size = 1

    rotation_columns = [c for c in data.columns if "_rot_" in c]
    assert np.all(rotation_columns == data.columns), "rotation columns are wrong"

    joint_names = set([c[: -len("_rot_x")] for c in rotation_columns])

    for joint_name in joint_names:
        joint_rotation_names = [f"{joint_name}_rot_{c}" for c in "xyzw"]
        rotation_data = data[joint_rotation_names]

        # while computing acceleration values, we have to select the nan frames
        # (i.e., frames dismissed during the previous velocity value computation) and
        # exclude these
        nan_idxs = np.arange(len(rotation_data))[rotation_data.isna().any(axis=1)]
        rot = Rotation.from_quat(data[joint_rotation_names].fillna(0.25))
        delta_rot = rot[:-step_size].inv() * rot[step_size:]
        velocities[joint_rotation_names].iloc[step_size:] = delta_rot.as_quat()

    invalid_frames = np.concatenate(
        [
            nan_idxs,
            nan_idxs + step_size,
        ]
    )

    velocities.values[invalid_frames, :] = np.nan
    velocities = velocities.add_prefix("delta_")
    return velocities


def to_velocity(data: pd.DataFrame, inplace=False) -> pd.DataFrame:
    """
    Calculates velocities from position and/or rotation data.

    :param data: A DataFrame or Series containing position and/or rotation data.
    :param inplace: If True, the velocities are calculated in-place (default is False).
    :return: A DataFrame containing the calculated velocities.
    """

    velocities = data if inplace else data.copy()

    position_columns = [c for c in data.columns if "_pos_" in c]
    rotation_columns = [c for c in data.columns if "_rot_" in c]

    compute_velocities_simple(data[position_columns], inplace=True)
    compute_velocities_quats(data[rotation_columns], inplace=True)

    return velocities

motion_learning_toolbox/to_velocity.py
cschell/Motion-Learning-Toolbox
#!/usr/bin/env python

from setuptools import find_packages, setup

setup(
    name="Motion Learning Toolbox",
    version="0.0.2",
    description="Motion Learning Toolbox",
    author="Christian Rack",
    author_email="mail@chrisrack.de",
    url="",
    install_requires=[
        "numpy",
        "pandas",
        "scipy",
        "quaternionic",
    ],
    packages=find_packages(),
)
setup.py
cschell/Motion-Learning-Toolbox
import pandas as pd
import numpy as np
from motion_learning_toolbox import canonicalize_quaternions


def test_canonicalize_quaternions():
    # Load test data
    test_df = pd.read_csv("test_data.csv")

    # The joint names for which quaternion data exists in test_df
    joint_names = ["hmd", "left_hand", "right_hand"]

    # Apply the function
    canonical_df = canonicalize_quaternions(test_df, joint_names)

    # Assumption 1: w-component should be non-negative
    for joint in joint_names:
        w_column = f"{joint}_rot_w"
        assert all(canonical_df[w_column] >= 0)

    # Assumption 2: Quaternions should be normalized
    for joint in joint_names:
        quat_cols = [f"{joint}_rot_{xyzw}" for xyzw in "wxyz"]
        norms = np.linalg.norm(canonical_df[quat_cols].values, axis=1)
        assert all(np.isclose(norms, 1, atol=1e-6))

    # Assumption 3: Function should not alter unrelated columns
    unrelated_columns = [col for col in test_df.columns if all(joint not in col for joint in joint_names)]
    assert test_df[unrelated_columns].equals(canonical_df[unrelated_columns])

tests/test_canonicalize_quaternions.py
cschell/Motion-Learning-Toolbox
import pandas as pd
import numpy as np
from motion_learning_toolbox import fix_controller_mapping
import pytest


@pytest.mark.parametrize("switched", [False, True])
def test_fix_controller_mapping(switched: bool):
    np.random.seed(42)
    n_samples = 1000
    hmd_positions_dict = {
        "hmd_pos_x": np.random.normal(loc=0, scale=0.5, size=n_samples),
        "hmd_pos_y": np.random.normal(loc=20, scale=3, size=n_samples),
        "hmd_pos_z": np.random.normal(loc=3, scale=1, size=n_samples),
        "left_hand_pos_x": np.random.normal(
            loc=-1 * -((switched - 0.5) * 2), scale=3, size=n_samples
        ),
        "left_hand_pos_y": np.random.normal(loc=20, scale=3, size=n_samples),
        "left_hand_pos_z": np.random.normal(loc=3, scale=1, size=n_samples),
        "right_hand_pos_x": np.random.normal(
            loc=1 * -((switched - 0.5) * 2), scale=3, size=n_samples
        ),
        "right_hand_pos_y": np.random.normal(loc=20, scale=3, size=n_samples),
        "right_hand_pos_z": np.random.normal(loc=3, scale=1, size=n_samples),
    }

    hmd_positions = pd.DataFrame(hmd_positions_dict)

    # Define the coordinate system for transformation
    coordinate_system = {"forward": "z", "right": "x", "up": "y"}

    fixed_positions = fix_controller_mapping(
        hmd_positions,
        left_controller_name="left_hand",
        right_controller_name="right_hand",
        coordinate_system=coordinate_system,
        is_br=True,
        inplace=False,
    )

    if switched:
        rename_mapping = {
            "left_hand_pos_x": "right_hand_pos_x",
            "left_hand_pos_y": "right_hand_pos_y",
            "left_hand_pos_z": "right_hand_pos_z",
            "right_hand_pos_x": "left_hand_pos_x",
            "right_hand_pos_y": "left_hand_pos_y",
            "right_hand_pos_z": "left_hand_pos_z",
        }
    else:
        rename_mapping = {}

    assert np.all(
        hmd_positions
        == fixed_positions.rename(columns=rename_mapping)[hmd_positions.columns]
    )

tests/test_controller_mapping.py
cschell/Motion-Learning-Toolbox
import pandas as pd
import numpy as np
from motion_learning_toolbox import resample


def test_resample():
    # Create a test DataFrame with appropriate structure
    test_df = pd.read_csv("test_data.csv")
    test_df.index = pd.to_timedelta(test_df.timestamp, unit="ms")
    joint_names = ["hmd", "left_hand", "right_hand"]  # Replace with your joint names

    # Test various target_fps values
    target_fps_values = [30, 60, 120]

    for target_fps in target_fps_values:
        resampled_data = resample(test_df, target_fps, joint_names)

        # Assertions for resampled_data
        assert len(resampled_data) == np.ceil(test_df.index[-1].total_seconds() * target_fps)

tests/test_resample.py
cschell/Motion-Learning-Toolbox
from dataclasses import dataclass
import pandas as pd
import numpy as np

from motion_learning_toolbox import to_body_relative

from scipy.spatial.transform import Rotation


def test_to_body_relative():
    # Load the test data
    test_df = pd.read_csv("test_data.csv")
    test_df.index = pd.to_timedelta(test_df.timestamp, unit="ms")
    target_joints = ["left_hand", "right_hand"]

    # Define the coordinate system for transformation
    coordinate_system = {"forward": "z", "right": "x", "up": "y"}

    # Specify the reference joint
    reference_joint = "hmd"

    # Perform the transformation
    transformed_data = to_body_relative(
        test_df, target_joints, coordinate_system, reference_joint
    )

    # Assertions for transformed_data
    assert len(transformed_data) == len(
        test_df
    )  # Ensure the number of samples remains the same
    assert transformed_data.isna().any().sum() == 0


def test_br_rotation_of_rotations_and_positions():
    left_hand_position = [123, 23, 1]
    hmd_position = [123, 23, 0]
    right_hand_position = [123, 23, -1]

    left_hand_orientation = Rotation.from_euler("xyz", [0, 90, 30], degrees=True).as_quat()
    right_hand_orientation = Rotation.from_euler("xyz", [0, 90, 40], degrees=True).as_quat()
    hmd_orientation = Rotation.from_euler("xyz", [0, 90, 50], degrees=True).as_quat()

    test_data = [
        [*left_hand_position, *right_hand_position, *hmd_position, *left_hand_orientation, *right_hand_orientation, *hmd_orientation]
    ]
    joints = ["left_hand", "right_hand", "hmd"]

    test_df = pd.DataFrame(
        test_data,
        columns=[f"{joint}_pos_{xyz}" for joint in joints for xyz in "xyz"] + [f"{joint}_rot_{xyzw}" for joint in joints for xyzw in "xyzw"],
    )

    transformed_data = to_body_relative(
        test_df,
        reference_joint="hmd",
        target_joints=["left_hand", "right_hand"],
        coordinate_system={"forward": "z", "right": "x", "up": "y"},
    )

    expected_left_hand_orientation = Rotation.from_euler("xyz", [-30, 0, 0], degrees=True).as_quat()
    expected_right_hand_orientation = Rotation.from_euler("xyz", [-40, 0, 0], degrees=True).as_quat()
    assert np.allclose(
        transformed_data[["left_hand_rot_x", "left_hand_rot_y", "left_hand_rot_z", "left_hand_rot_w",
                          "right_hand_rot_x", "right_hand_rot_y", "right_hand_rot_z", "right_hand_rot_w"]], 
        [[*expected_left_hand_orientation, *expected_right_hand_orientation]])

    expected_left_hand_position = [-1, 0, 0]
    expected_right_hand_position = [1, 0,  0]
    assert np.allclose(
        transformed_data[["left_hand_pos_x", "left_hand_pos_y", "left_hand_pos_z", "right_hand_pos_x", "right_hand_pos_y", "right_hand_pos_z"]], 
        [[*expected_left_hand_position, *expected_right_hand_position]])

tests/test_to_body_relative.py
cschell/Motion-Learning-Toolbox
import pandas as pd
import numpy as np
from scipy.spatial.transform import Rotation as R
from motion_learning_toolbox import compute_velocities_simple, compute_velocities_quats


def test_compute_velocities_simple():
    # Load test data
    test_df = pd.read_csv("test_data.csv")[["hmd_pos_x", "hmd_pos_y", "hmd_pos_z"]]

    # Apply the function
    velocities_df = compute_velocities_simple(test_df)

    # Assumption 1: Output DataFrame has the same number of rows as the input
    assert len(velocities_df) == len(test_df)
    
    # Assumption 2: Output DataFrame has columns prefixed with "delta_"
    for col in test_df.columns:
        assert f"delta_{col}" in velocities_df.columns

    # Assumption 3: First row contains NaNs
    assert velocities_df.iloc[0].isna().all()

    # Assumption 4: Adding the velocity to the previous frame should return the next frame's value
    assert np.allclose(test_df.to_numpy()[:-1] + velocities_df.to_numpy()[1:], test_df.iloc[1:].to_numpy())


def test_compute_velocities_quats():
    # Load test data
    test_df = pd.read_csv("test_data.csv")[["hmd_rot_x", "hmd_rot_y", "hmd_rot_z", "hmd_rot_w"]]

    # Apply the function
    velocities_df = compute_velocities_quats(test_df)

    # Assumption 1: Output DataFrame has the same number of rows as input
    assert len(velocities_df) == len(test_df)

    # Assumption 2: Output DataFrame has columns prefixed with "delta_"
    for col in test_df.columns:
        assert f"delta_{col}" in velocities_df.columns

    # Assumption 3: Rows with original NaN quaternion values should have NaN in the output
    nan_rows = test_df.isna().any(axis=1)
    assert velocities_df.loc[nan_rows].isna().all().all()
    
    # Assumption 4: Rotating a frame by the following ang. velocity should yield the following frame's routation
    expected_rotations = (R.from_quat(test_df[:-1]) * R.from_quat(velocities_df[1:])).as_quat()
    actual_rotations = R.from_quat(test_df.iloc[1:]).as_quat()

    assert np.allclose(expected_rotations, actual_rotations)
tests/test_to_velocity.py
cschell/Motion-Learning-Toolbox