diff --git a/src/lerobot/teleoperators/phone/teleop_phone.py b/src/lerobot/teleoperators/phone/teleop_phone.py index f1af248e4..19eac8178 100644 --- a/src/lerobot/teleoperators/phone/teleop_phone.py +++ b/src/lerobot/teleoperators/phone/teleop_phone.py @@ -171,7 +171,13 @@ class IOSPhone(BasePhone, Teleoperator): # HEBI provides orientation in w, x, y, z format. # Scipy's Rotation expects x, y, z, w. quat_xyzw = np.concatenate((ar_quat[1:], [ar_quat[0]])) # wxyz to xyzw - rot = Rotation.from_quat(quat_xyzw) + # ARKit can emit zero/NaN quaternions before tracking is ready or on a + # dropped packet. Rotation.from_quat now rejects those; degrade the same + # way as a missing pose so teleop stays alive mid-session. + try: + rot = Rotation.from_quat(quat_xyzw) + except ValueError: + return False, None, None, None pos = ar_pos - rot.apply(self.config.camera_offset) return True, pos, rot, pose diff --git a/src/lerobot/utils/rotation.py b/src/lerobot/utils/rotation.py index 41b652947..280d33bf0 100644 --- a/src/lerobot/utils/rotation.py +++ b/src/lerobot/utils/rotation.py @@ -29,10 +29,13 @@ class Rotation: def __init__(self, quat: np.ndarray) -> None: """Initialize rotation from quaternion [x, y, z, w].""" self._quat = np.asarray(quat, dtype=float) - # Normalize quaternion + if self._quat.shape != (4,): + raise ValueError(f"Quaternion must have shape (4,), got {self._quat.shape}") + # Normalize quaternion. Reject the zero vector — it has no orientation. norm = np.linalg.norm(self._quat) - if norm > 0: - self._quat = self._quat / norm + if norm <= 0.0 or not np.isfinite(norm): + raise ValueError(f"Quaternion must be a non-zero finite vector; got {self._quat} (norm={norm})") + self._quat = self._quat / norm @classmethod def from_rotvec(cls, rotvec: np.ndarray) -> "Rotation": diff --git a/tests/utils/test_rotation.py b/tests/utils/test_rotation.py new file mode 100644 index 000000000..dc982097c --- /dev/null +++ b/tests/utils/test_rotation.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +# Copyright 2026 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. + +import numpy as np +import pytest + +from lerobot.utils.rotation import Rotation + + +def test_zero_quaternion_rejected(): + with pytest.raises(ValueError, match="non-zero"): + Rotation(np.zeros(4)) + + +def test_non_finite_quaternion_rejected(): + with pytest.raises(ValueError, match="non-zero|finite"): + Rotation(np.array([np.nan, 0.0, 0.0, 1.0])) + + +def test_wrong_shape_rejected(): + with pytest.raises(ValueError, match="shape"): + Rotation(np.array([1.0, 0.0, 0.0])) + + +def test_identity_roundtrip(): + r = Rotation.from_rotvec(np.zeros(3)) + assert np.allclose(r.as_rotvec(), 0.0) + assert np.allclose(r.as_matrix(), np.eye(3)) + + +def test_rotvec_roundtrip(): + rotvec = np.array([0.1, -0.2, 0.3]) + r = Rotation.from_rotvec(rotvec) + assert np.allclose(r.as_rotvec(), rotvec, atol=1e-6)