From 99963b6968da269a5b65dc2fe53c93fbf8fb2f8f Mon Sep 17 00:00:00 2001 From: Adil Zouitine Date: Thu, 4 Sep 2025 16:26:28 +0200 Subject: [PATCH] refactor(dependencies): remove scipy dependency and introduce custom rotation utilities (#1863) - Removed the scipy dependency from the project to streamline requirements. - Added a new `rotation.py` module containing a custom `Rotation` class that replicates essential functionalities of `scipy.spatial.transform.Rotation`, allowing for rotation vector, matrix, and quaternion conversions without external dependencies. - Updated the `robot_kinematic_processor.py` to utilize the new custom rotation utilities. --- pyproject.toml | 1 - .../robot_kinematic_processor.py | 2 +- src/lerobot/utils/rotation.py | 174 ++++++++++++++++++ 3 files changed, 175 insertions(+), 2 deletions(-) create mode 100644 src/lerobot/utils/rotation.py diff --git a/pyproject.toml b/pyproject.toml index 96b4c853d..bdd634f71 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -73,7 +73,6 @@ dependencies = [ "pynput>=1.7.7", "pyserial>=3.5", "wandb>=0.20.0", - "scipy>=1.15.2", "torch>=2.2.1,<2.8.0", # TODO: Bumb dependency "torchcodec>=0.2.1,<0.6.0; sys_platform != 'win32' and (sys_platform != 'linux' or (platform_machine != 'aarch64' and platform_machine != 'arm64' and platform_machine != 'armv7l')) and (sys_platform != 'darwin' or platform_machine != 'x86_64')", # TODO: Bumb dependency diff --git a/src/lerobot/robots/so100_follower/robot_kinematic_processor.py b/src/lerobot/robots/so100_follower/robot_kinematic_processor.py index 9c272b767..b6ac6a6c0 100644 --- a/src/lerobot/robots/so100_follower/robot_kinematic_processor.py +++ b/src/lerobot/robots/so100_follower/robot_kinematic_processor.py @@ -17,7 +17,6 @@ from dataclasses import dataclass, field import numpy as np -from scipy.spatial.transform import Rotation from lerobot.configs.types import FeatureType, PolicyFeature from lerobot.constants import ACTION, OBS_STATE @@ -32,6 +31,7 @@ from lerobot.processor import ( TransitionKey, ) from lerobot.robots.robot import Robot +from lerobot.utils.rotation import Rotation @ProcessorStepRegistry.register("ee_reference_and_delta") diff --git a/src/lerobot/utils/rotation.py b/src/lerobot/utils/rotation.py new file mode 100644 index 000000000..18dc28df0 --- /dev/null +++ b/src/lerobot/utils/rotation.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Custom rotation utilities to replace scipy.spatial.transform.Rotation.""" + +import numpy as np + + +class Rotation: + """ + Custom rotation class that provides a subset of scipy.spatial.transform.Rotation functionality. + + Supports conversions between rotation vectors, rotation matrices, and quaternions. + """ + + def __init__(self, quat: np.ndarray) -> None: + """Initialize rotation from quaternion [x, y, z, w].""" + self._quat = np.asarray(quat, dtype=float) + # Normalize quaternion + norm = np.linalg.norm(self._quat) + if norm > 0: + self._quat = self._quat / norm + + @classmethod + def from_rotvec(cls, rotvec: np.ndarray) -> "Rotation": + """ + Create rotation from rotation vector using Rodrigues' formula. + + Args: + rotvec: Rotation vector [x, y, z] where magnitude is angle in radians + + Returns: + Rotation instance + """ + rotvec = np.asarray(rotvec, dtype=float) + angle = np.linalg.norm(rotvec) + + if angle < 1e-8: + # For very small angles, use identity quaternion + quat = np.array([0.0, 0.0, 0.0, 1.0]) + else: + axis = rotvec / angle + half_angle = angle / 2.0 + sin_half = np.sin(half_angle) + cos_half = np.cos(half_angle) + + # Quaternion [x, y, z, w] + quat = np.array([axis[0] * sin_half, axis[1] * sin_half, axis[2] * sin_half, cos_half]) + + return cls(quat) + + @classmethod + def from_matrix(cls, matrix: np.ndarray) -> "Rotation": + """ + Create rotation from 3x3 rotation matrix. + + Args: + matrix: 3x3 rotation matrix + + Returns: + Rotation instance + """ + matrix = np.asarray(matrix, dtype=float) + + # Shepherd's method for converting rotation matrix to quaternion + trace = np.trace(matrix) + + if trace > 0: + s = np.sqrt(trace + 1.0) * 2 # s = 4 * qw + qw = 0.25 * s + qx = (matrix[2, 1] - matrix[1, 2]) / s + qy = (matrix[0, 2] - matrix[2, 0]) / s + qz = (matrix[1, 0] - matrix[0, 1]) / s + elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]: + s = np.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2 # s = 4 * qx + qw = (matrix[2, 1] - matrix[1, 2]) / s + qx = 0.25 * s + qy = (matrix[0, 1] + matrix[1, 0]) / s + qz = (matrix[0, 2] + matrix[2, 0]) / s + elif matrix[1, 1] > matrix[2, 2]: + s = np.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2 # s = 4 * qy + qw = (matrix[0, 2] - matrix[2, 0]) / s + qx = (matrix[0, 1] + matrix[1, 0]) / s + qy = 0.25 * s + qz = (matrix[1, 2] + matrix[2, 1]) / s + else: + s = np.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2 # s = 4 * qz + qw = (matrix[1, 0] - matrix[0, 1]) / s + qx = (matrix[0, 2] + matrix[2, 0]) / s + qy = (matrix[1, 2] + matrix[2, 1]) / s + qz = 0.25 * s + + quat = np.array([qx, qy, qz, qw]) + return cls(quat) + + @classmethod + def from_quat(cls, quat: np.ndarray) -> "Rotation": + """ + Create rotation from quaternion. + + Args: + quat: Quaternion [x, y, z, w] or [w, x, y, z] (specify convention in docstring) + This implementation expects [x, y, z, w] format + + Returns: + Rotation instance + """ + return cls(quat) + + def as_matrix(self) -> np.ndarray: + """ + Convert rotation to 3x3 rotation matrix. + + Returns: + 3x3 rotation matrix + """ + qx, qy, qz, qw = self._quat + + # Compute rotation matrix from quaternion + return np.array( + [ + [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)], + [2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)], + [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)], + ], + dtype=float, + ) + + def as_rotvec(self) -> np.ndarray: + """ + Convert rotation to rotation vector. + + Returns: + Rotation vector [x, y, z] where magnitude is angle in radians + """ + qx, qy, qz, qw = self._quat + + # Ensure qw is positive for unique representation + if qw < 0: + qx, qy, qz, qw = -qx, -qy, -qz, -qw + + # Compute angle and axis + angle = 2.0 * np.arccos(np.clip(abs(qw), 0.0, 1.0)) + sin_half_angle = np.sqrt(1.0 - qw * qw) + + if sin_half_angle < 1e-8: + # For very small angles, use linearization: rotvec ≈ 2 * [qx, qy, qz] + return 2.0 * np.array([qx, qy, qz]) + + # Extract axis and scale by angle + axis = np.array([qx, qy, qz]) / sin_half_angle + return angle * axis + + def as_quat(self) -> np.ndarray: + """ + Get quaternion representation. + + Returns: + Quaternion [x, y, z, w] + """ + return self._quat.copy()