diff --git a/README.md b/README.md index 906b11b..6959e6e 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,23 @@ somehand assets download --only mjcf mediapipe somehand webcam ``` +Use a MANUS ROS 2 glove topic with the safe viewer backend: + +```bash +source /opt/ros/humble/setup.bash +source ~/manus_ros2_ws/install/local_setup.bash + +somehand manus-ros2 \ + --topic /manus_glove_0 \ + --hand left \ + --backend viewer \ + --config configs/retargeting/left/revo2_left.yaml +``` + +The topic number does not define left or right. The adapter also validates the +`side` field inside every `ManusGlove` message. Real hardware output remains an +explicit `--backend real` choice. + ## API Quick Start See [API Usage](docs/en/api.md) for stable imports, one-step retargeting, and session orchestration. diff --git a/src/somehand/application/manus_calibration.py b/src/somehand/application/manus_calibration.py new file mode 100644 index 0000000..ee24f3d --- /dev/null +++ b/src/somehand/application/manus_calibration.py @@ -0,0 +1,587 @@ +"""Calibrated MANUS finger-curl mapping for Revo2. + +The default retargeting engine remains unchanged. This module is enabled only +when the MANUS CLI receives an explicit calibration profile. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +import math +from pathlib import Path +from typing import Mapping + +import numpy as np + +from somehand.domain import HandFrame, RetargetingStepResult +from somehand.domain.hand_side import normalize_hand_side +from somehand.infrastructure.config_loader import load_retargeting_config + +from .engine import RetargetingEngine + + +FINGER_INDICES = { + "thumb": (1, 2, 3, 4), + "index": (5, 6, 7, 8), + "middle": (9, 10, 11, 12), + "ring": (13, 14, 15, 16), + "pinky": (17, 18, 19, 20), +} + +FINGER_NAMES = tuple(FINGER_INDICES) +FOUR_FINGER_NAMES = ("index", "middle", "ring", "pinky") + +DEFAULT_DEADBANDS = { + "thumb": 0.22, + "index": 0.08, + "middle": 0.08, + "ring": 0.08, + "pinky": 0.14, +} + + +@dataclass(frozen=True, slots=True) +class FingerCalibration: + open_deg: float + comfortable_closed_deg: float + metric: str = "current" + + @property + def span_deg(self) -> float: + return self.comfortable_closed_deg - self.open_deg + + +@dataclass(frozen=True, slots=True) +class ManusCalibrationProfile: + version: int + side: str + fingers: Mapping[str, FingerCalibration] + + +def _finite_float(value: object, *, label: str) -> float: + result = float(value) + if not math.isfinite(result): + raise ValueError(f"{label} must be finite") + return result + + +def load_manus_calibration_profile( + path: str | Path, + *, + expected_side: str | None = None, +) -> ManusCalibrationProfile: + profile_path = Path(path).expanduser().resolve() + if not profile_path.is_file(): + raise FileNotFoundError( + f"MANUS calibration file not found: {profile_path}" + ) + + data = json.loads(profile_path.read_text(encoding="utf-8")) + version = int(data.get("version", 1)) + if version != 1: + raise ValueError( + f"Unsupported MANUS calibration version: {version}" + ) + + side = normalize_hand_side(data.get("side", "")) + if expected_side is not None: + normalized_expected = normalize_hand_side(expected_side) + if side != normalized_expected: + raise ValueError( + "MANUS calibration side " + f"{side!r} does not match requested side " + f"{normalized_expected!r}" + ) + + raw_fingers = data.get("fingers") + if not isinstance(raw_fingers, dict): + raise ValueError( + "MANUS calibration must contain a 'fingers' object" + ) + + missing = [ + name for name in FINGER_NAMES + if name not in raw_fingers + ] + extra = sorted(set(raw_fingers) - set(FINGER_NAMES)) + if missing or extra: + details = [] + if missing: + details.append("missing=" + ",".join(missing)) + if extra: + details.append("extra=" + ",".join(extra)) + raise ValueError( + "Invalid MANUS calibration fingers: " + + "; ".join(details) + ) + + fingers: dict[str, FingerCalibration] = {} + for name in FINGER_NAMES: + item = raw_fingers[name] + if not isinstance(item, dict): + raise ValueError( + f"Calibration for finger {name!r} must be an object" + ) + metric = str( + item.get("metric", "current") + ).strip().lower() + + if metric not in { + "current", + "joint_sum", + }: + raise ValueError( + f"Unsupported calibration metric for " + f"{name!r}: {metric!r}" + ) + + calibration = FingerCalibration( + open_deg=_finite_float( + item["open_deg"], + label=f"{name}.open_deg", + ), + comfortable_closed_deg=_finite_float( + item["comfortable_closed_deg"], + label=f"{name}.comfortable_closed_deg", + ), + metric=metric, + ) + if calibration.span_deg <= 1e-6: + raise ValueError( + f"Calibration span for {name!r} must be positive" + ) + fingers[name] = calibration + + return ManusCalibrationProfile( + version=version, + side=side, + fingers=fingers, + ) + + +def unit(vector: np.ndarray) -> np.ndarray: + norm = float(np.linalg.norm(vector)) + if norm < 1e-12: + return np.zeros(3, dtype=np.float64) + return vector / norm + + +def _vector_angle_degrees( + first: np.ndarray, + second: np.ndarray, +) -> float: + first_unit = unit(first) + second_unit = unit(second) + + cosine = float( + np.clip( + np.dot(first_unit, second_unit), + -1.0, + 1.0, + ) + ) + + return float( + np.degrees( + np.arccos(cosine) + ) + ) + + +def finger_curl_degrees( + landmarks: np.ndarray, + indices: tuple[int, int, int, int], +) -> float: + mcp, pip, dip, tip = indices + + proximal = landmarks[pip] - landmarks[mcp] + distal = landmarks[tip] - landmarks[dip] + + return _vector_angle_degrees( + proximal, + distal, + ) + + +def finger_joint_sum_degrees( + landmarks: np.ndarray, + indices: tuple[int, int, int, int], +) -> float: + """Sum MCP, PIP and DIP flexion angles.""" + + mcp, pip, dip, tip = indices + + palm_segment = landmarks[mcp] - landmarks[0] + proximal_segment = landmarks[pip] - landmarks[mcp] + middle_segment = landmarks[dip] - landmarks[pip] + distal_segment = landmarks[tip] - landmarks[dip] + + return float( + _vector_angle_degrees( + palm_segment, + proximal_segment, + ) + + _vector_angle_degrees( + proximal_segment, + middle_segment, + ) + + _vector_angle_degrees( + middle_segment, + distal_segment, + ) + ) + + +def apply_deadband(value: float, deadband: float) -> float: + normalized = float(np.clip(value, 0.0, 1.0)) + if not 0.0 <= deadband < 1.0: + raise ValueError("deadband must be in [0, 1)") + if normalized <= deadband: + return 0.0 + return float((normalized - deadband) / (1.0 - deadband)) + + +def map_to_safe_range( + normalized: float, + bound: tuple[float | None, float | None], + safe_fraction: float, +) -> float: + low, high = bound + if low is None or high is None: + raise RuntimeError(f"Missing valid joint bound: {bound}") + if not 0.0 <= safe_fraction <= 1.0: + raise ValueError("safe_fraction must be in [0, 1]") + + low_value = float(low) + high_value = float(high) + safe_high = low_value + safe_fraction * (high_value - low_value) + clipped = float(np.clip(normalized, 0.0, 1.0)) + return low_value + clipped * (safe_high - low_value) + + +class Revo2ManusQposMapper: + """Convert calibrated finger curls into conservative Revo2 qpos.""" + + def __init__( + self, + *, + hand_model, + bounds, + profile: ManusCalibrationProfile, + side: str, + four_finger_safe_range: float = 0.65, + thumb_metacarpal_safe_range: float = 0.35, + thumb_proximal_safe_range: float = 0.55, + deadbands: Mapping[str, float] = DEFAULT_DEADBANDS, + pinky_ring_dominance_compensation: float = 0.60, + smooth_alpha: float = 0.22, + max_step_rad: float = 0.035, + ) -> None: + self.side = normalize_hand_side(side) + if profile.side != self.side: + raise ValueError( + "MANUS calibration side does not match hand model side" + ) + + self.hand_model = hand_model + self.bounds = bounds + self.profile = profile + self.four_finger_safe_range = float( + four_finger_safe_range + ) + self.thumb_metacarpal_safe_range = float( + thumb_metacarpal_safe_range + ) + self.thumb_proximal_safe_range = float( + thumb_proximal_safe_range + ) + self.deadbands = { + name: float(deadbands[name]) + for name in FINGER_NAMES + } + self.pinky_ring_dominance_compensation = float( + pinky_ring_dominance_compensation + ) + self.smooth_alpha = float(smooth_alpha) + self.max_step_rad = float(max_step_rad) + + if not 0.0 < self.smooth_alpha <= 1.0: + raise ValueError("smooth_alpha must be in (0, 1]") + if self.max_step_rad <= 0.0: + raise ValueError("max_step_rad must be > 0") + + joint_to_qpos = ( + hand_model.get_joint_name_to_qpos_index() + ) + required_names = { + "thumb_metacarpal": + f"{self.side}_thumb_metacarpal_joint", + "thumb_proximal": + f"{self.side}_thumb_proximal_joint", + "index": + f"{self.side}_index_proximal_joint", + "middle": + f"{self.side}_middle_proximal_joint", + "ring": + f"{self.side}_ring_proximal_joint", + "pinky": + f"{self.side}_pinky_proximal_joint", + } + + missing = [ + joint_name + for joint_name in required_names.values() + if joint_name not in joint_to_qpos + ] + if missing: + raise ValueError( + "Revo2 MANUS mapping is missing joints: " + + ", ".join(missing) + ) + + self.qpos_indices = { + key: int(joint_to_qpos[joint_name]) + for key, joint_name in required_names.items() + } + self.controlled_indices = [ + self.qpos_indices["thumb_metacarpal"], + self.qpos_indices["thumb_proximal"], + self.qpos_indices["index"], + self.qpos_indices["middle"], + self.qpos_indices["ring"], + self.qpos_indices["pinky"], + ] + self._previous: np.ndarray | None = None + + def reset(self) -> None: + self._previous = None + + def map( + self, + landmarks: np.ndarray, + base_qpos: np.ndarray, + ) -> tuple[np.ndarray, dict[str, float], dict[str, float]]: + points = np.asarray(landmarks, dtype=np.float64) + if points.shape != (21, 3): + raise ValueError( + f"Expected MANUS landmarks shape (21, 3), " + f"got {points.shape}" + ) + if not np.all(np.isfinite(points)): + raise ValueError( + "MANUS landmarks contain NaN or infinity" + ) + + raw_normalized: dict[str, float] = {} + for finger_name in FINGER_NAMES: + calibration = self.profile.fingers[ + finger_name + ] + + if calibration.metric == "current": + curl = finger_curl_degrees( + points, + FINGER_INDICES[finger_name], + ) + elif calibration.metric == "joint_sum": + curl = finger_joint_sum_degrees( + points, + FINGER_INDICES[finger_name], + ) + else: + raise RuntimeError( + "Unsupported MANUS finger metric: " + f"{calibration.metric!r}" + ) + + normalized = ( + (curl - calibration.open_deg) + / calibration.span_deg + ) + raw_normalized[finger_name] = float( + np.clip(normalized, 0.0, 1.0) + ) + + corrected = { + finger_name: apply_deadband( + raw_normalized[finger_name], + self.deadbands[finger_name], + ) + for finger_name in FINGER_NAMES + } + + pinky_excess = max( + 0.0, + corrected["pinky"] - corrected["ring"], + ) + corrected["ring"] = float( + np.clip( + corrected["ring"] + - self.pinky_ring_dominance_compensation + * pinky_excess, + 0.0, + 1.0, + ) + ) + + desired = np.asarray( + base_qpos, + dtype=np.float64, + ).copy() + + thumb_metacarpal = self.qpos_indices[ + "thumb_metacarpal" + ] + thumb_proximal = self.qpos_indices[ + "thumb_proximal" + ] + + desired[thumb_metacarpal] = map_to_safe_range( + corrected["thumb"], + self.bounds[thumb_metacarpal], + self.thumb_metacarpal_safe_range, + ) + desired[thumb_proximal] = map_to_safe_range( + corrected["thumb"], + self.bounds[thumb_proximal], + self.thumb_proximal_safe_range, + ) + + for finger_name in FOUR_FINGER_NAMES: + qpos_index = self.qpos_indices[finger_name] + desired[qpos_index] = map_to_safe_range( + corrected[finger_name], + self.bounds[qpos_index], + self.four_finger_safe_range, + ) + + previous = self._previous + if previous is None: + previous = desired.copy() + + filtered = previous.copy() + for qpos_index in self.controlled_indices: + smoothed_target = ( + previous[qpos_index] + + self.smooth_alpha + * ( + desired[qpos_index] + - previous[qpos_index] + ) + ) + delta = float( + np.clip( + smoothed_target - previous[qpos_index], + -self.max_step_rad, + self.max_step_rad, + ) + ) + filtered[qpos_index] = previous[qpos_index] + delta + + filtered = self.hand_model.apply_mimic_constraints( + filtered + ) + self._previous = filtered.copy() + return filtered, raw_normalized, corrected + + +class CalibratedManusRetargetingEngine(RetargetingEngine): + """Retargeting engine with explicit calibrated MANUS override.""" + + def __init__( + self, + config, + *, + calibration_path: str | Path, + input_type: str = "manus_ros2", + ) -> None: + super().__init__(config, input_type=input_type) + if not config.hand.name.startswith("revo2_"): + raise ValueError( + "Calibrated MANUS mapping currently supports " + "Revo2 configs only" + ) + + self.calibration_path = str( + Path(calibration_path).expanduser().resolve() + ) + self.calibration_profile = ( + load_manus_calibration_profile( + self.calibration_path, + expected_side=config.hand.side, + ) + ) + self._calibrated_mapper = Revo2ManusQposMapper( + hand_model=self.hand_model, + bounds=self.retargeter._bounds, + profile=self.calibration_profile, + side=config.hand.side, + ) + self._calibrated_frame_count = 0 + + @classmethod + def from_config_path( + cls, + config_path: str, + *, + calibration_path: str | Path, + input_type: str = "manus_ros2", + ) -> "CalibratedManusRetargetingEngine": + return cls( + load_retargeting_config(config_path), + calibration_path=calibration_path, + input_type=input_type, + ) + + def process( + self, + frame: HandFrame, + ) -> RetargetingStepResult: + base_result = super().process(frame) + qpos, raw, corrected = self._calibrated_mapper.map( + frame.landmarks_3d, + base_result.qpos, + ) + + self._calibrated_frame_count += 1 + frame_count = self._calibrated_frame_count + if frame_count <= 3 or frame_count % 30 == 0: + raw_display = { + name: round(value, 3) + for name, value in raw.items() + } + corrected_display = { + name: round(value, 3) + for name, value in corrected.items() + } + print( + "CALIBRATED_MANUS_TRACE " + f"frame={frame_count} " + f"raw={raw_display} " + f"corrected={corrected_display} " + f"qpos={np.round(qpos, 4).tolist()}", + flush=True, + ) + + return RetargetingStepResult( + qpos=qpos.copy(), + target_directions=base_result.target_directions, + processed_landmarks=base_result.processed_landmarks, + hand_side=base_result.hand_side, + target_qpos=base_result.target_qpos, + backend=base_result.backend, + ) + + +__all__ = [ + "CalibratedManusRetargetingEngine", + "FingerCalibration", + "ManusCalibrationProfile", + "Revo2ManusQposMapper", + "apply_deadband", + "finger_curl_degrees", + "finger_joint_sum_degrees", + "load_manus_calibration_profile", + "map_to_safe_range", +] diff --git a/src/somehand/cli/commands.py b/src/somehand/cli/commands.py index edbb2fa..9da3237 100644 --- a/src/somehand/cli/commands.py +++ b/src/somehand/cli/commands.py @@ -15,6 +15,7 @@ create_bihand_pico_source, create_bihand_recording_source, create_hc_mocap_udp_source, + create_manus_ros2_source, create_pico_source, create_recording_source, save_bihand_recording_artifact, @@ -256,6 +257,71 @@ def _run_pico(args: argparse.Namespace) -> None: _finalize_run(args, summary=summary, source=source) +def _run_manus_ros2(args: argparse.Namespace) -> None: + source, recording_controller = _wrap_source_for_interactive_recording( + _wrap_live_hand_source( + create_manus_ros2_source( + topic=args.topic, + hand_side=args.hand, + timeout=args.manus_timeout, + ), + args=args, + ), + record_output_path=args.record_output, + ) + engine = _build_engine(args, input_type="manus_ros2") + session = _build_runtime_session( + engine, + args, + visualize=True, + show_preview=False, + key_callback=None if recording_controller is None else recording_controller.handle_keypress, + ) + extra_lines = [ + f"Backend: {args.backend}", + f"Signal sampling: {source.fps} fps", + f"MANUS ROS 2 topic: {args.topic}", + f"Expected message side: {display_hand_side(args.hand)}", + "Topic numbering does not define handedness; message.side is validated.", + ] + manus_calibration = getattr( + args, + "manus_calibration", + None, + ) + if manus_calibration is not None: + extra_lines.extend( + [ + f"MANUS calibration: {manus_calibration}", + "Calibrated mapping: Revo2 viewer validation only", + "Real hand output: DISABLED", + "CAN: DISABLED", + "Modbus: DISABLED", + ] + ) + if recording_controller is not None: + extra_lines.append("Press 'r' in the terminal or robot-hand viewer to start recording.") + extra_lines.append("Press 's' in the terminal or robot-hand viewer to stop recording, save, and exit.") + _print_startup( + engine, + source_desc=source.source_desc, + tracking_desc=f"Tracking MANUS hand: {display_hand_side(args.hand)} | Source fps: {source.fps}", + extra_lines=extra_lines, + ) + if recording_controller is not None: + recording_controller.start() + try: + summary = session.run( + source, + input_type="manus_ros2", + stop_condition=None if recording_controller is None else (lambda: recording_controller.stop_requested), + ) + finally: + if recording_controller is not None: + recording_controller.close() + _finalize_run(args, summary=summary, source=source) + + def _run_hc_mocap_udp(args: argparse.Namespace) -> None: source = _wrap_source_for_recording( _wrap_live_hand_source( diff --git a/src/somehand/cli/main.py b/src/somehand/cli/main.py index 014acae..04e9615 100644 --- a/src/somehand/cli/main.py +++ b/src/somehand/cli/main.py @@ -68,6 +68,9 @@ def main(argv: list[str] | None = None) -> None: return commands._run_pico(args) return + if args.command == "manus-ros2": + commands._run_manus_ros2(args) + return if args.command == "hc-mocap": if args.hand == "both": if args.backend != "viewer": diff --git a/src/somehand/cli/parser.py b/src/somehand/cli/parser.py index ebfda17..3a15321 100644 --- a/src/somehand/cli/parser.py +++ b/src/somehand/cli/parser.py @@ -32,7 +32,11 @@ def parse_config_path(value: str) -> str: return str(resolve_config_path(value)) -def add_common_args(parser: argparse.ArgumentParser) -> None: +def add_common_args( + parser: argparse.ArgumentParser, + *, + allow_both: bool = True, +) -> None: parser.add_argument( "-c", "--config", @@ -40,13 +44,19 @@ def add_common_args(parser: argparse.ArgumentParser) -> None: default=str(DEFAULT_CONFIG_PATH), help="Path to retargeting config YAML", ) + hand_choices = ["left", "right", "both"] if allow_both else ["left", "right"] + hand_help = ( + "Hand side for the current channel, or 'both' for two-hand mode" + if allow_both + else "Expected hand side for the selected input topic" + ) parser.add_argument( "-H", "--hand", type=parse_hand_selector, - choices=["left", "right", "both"], + choices=hand_choices, default="right", - help="Hand side for the current channel, or 'both' for two-hand mode", + help=hand_help, ) parser.add_argument( "--record-output", @@ -167,6 +177,33 @@ def build_parser() -> argparse.ArgumentParser: help="Timeout in seconds while waiting for PICO Bridge hand-tracking frames", ) + manus_ros2 = subparsers.add_parser( + "manus-ros2", + help="Retarget from a MANUS glove ROS 2 topic", + ) + add_common_args(manus_ros2, allow_both=False) + add_live_sampling_args(manus_ros2) + manus_ros2.add_argument( + "--topic", + required=True, + help="MANUS ManusGlove topic, for example /manus_glove_0", + ) + manus_ros2.add_argument( + "--manus-timeout", + type=float, + default=2.0, + help="Seconds to wait for the next matching MANUS frame", + ) + manus_ros2.add_argument( + "--manus-calibration", + default=None, + help=( + "Optional calibrated MANUS finger profile JSON. " + "During validation this mode supports Revo2 with " + "--backend viewer only." + ), + ) + hc_mocap = subparsers.add_parser("hc-mocap", help="Retarget from a live hc_mocap UDP stream") add_common_args(hc_mocap) add_live_sampling_args(hc_mocap) diff --git a/src/somehand/cli/runtime.py b/src/somehand/cli/runtime.py index 06e954c..be2e920 100644 --- a/src/somehand/cli/runtime.py +++ b/src/somehand/cli/runtime.py @@ -11,6 +11,9 @@ RetargetingEngine, RetargetingSession, ) +from somehand.application.manus_calibration import ( + CalibratedManusRetargetingEngine, +) from somehand.domain import RetargetingConfig from somehand.runtime import ( AsyncBiHandLandmarkOutputSink, @@ -338,7 +341,28 @@ def _build_bihand_visual_sinks( def build_engine(args: argparse.Namespace, *, input_type: str) -> RetargetingEngine: - return RetargetingEngine.from_config_path(args.config, input_type=input_type) + calibration_path = getattr(args, "manus_calibration", None) + if calibration_path is None: + return RetargetingEngine.from_config_path( + args.config, + input_type=input_type, + ) + + if input_type != "manus_ros2": + raise ValueError( + "--manus-calibration is supported only by manus-ros2" + ) + if getattr(args, "backend", "viewer") != "viewer": + raise ValueError( + "Calibrated MANUS mode is viewer-only during validation; " + "do not use --backend real or --backend sim" + ) + + return CalibratedManusRetargetingEngine.from_config_path( + args.config, + calibration_path=calibration_path, + input_type=input_type, + ) def build_bihand_engine(args: argparse.Namespace, *, input_type: str) -> BiHandRetargetingEngine: diff --git a/src/somehand/runtime/__init__.py b/src/somehand/runtime/__init__.py index bbc3a3a..afbcb59 100644 --- a/src/somehand/runtime/__init__.py +++ b/src/somehand/runtime/__init__.py @@ -6,6 +6,12 @@ from .config_validation import validate_runtime_bihand_config, validate_runtime_retargeting_config +_MANUS_EXPORTS = { + "ManusRos2InputSource", + "create_manus_ros2_source", + "manus_message_to_hand_frame", +} + _INFRA_EXPORTS = { "AsyncBiHandLandmarkOutputSink", "AsyncLandmarkOutputSink", @@ -50,6 +56,7 @@ __all__ = sorted( _INFRA_EXPORTS + | _MANUS_EXPORTS | { "validate_runtime_bihand_config", "validate_runtime_retargeting_config", @@ -62,6 +69,9 @@ def __getattr__(name: str): return validate_runtime_bihand_config if name == "validate_runtime_retargeting_config": return validate_runtime_retargeting_config + if name in _MANUS_EXPORTS: + manus_source = import_module("somehand.runtime.manus_source") + return getattr(manus_source, name) if name in _INFRA_EXPORTS: infrastructure = import_module("somehand.infrastructure") return getattr(infrastructure, name) diff --git a/src/somehand/runtime/manus_source.py b/src/somehand/runtime/manus_source.py new file mode 100644 index 0000000..1016b6c --- /dev/null +++ b/src/somehand/runtime/manus_source.py @@ -0,0 +1,320 @@ +"""MANUS ROS 2 input adapter for somehand.""" + +from __future__ import annotations + +import time +from typing import Any + +import numpy as np + +from somehand.core import HandFrame, SourceFrame, normalize_hand_side + + +# MANUS 25-node skeleton -> MediaPipe-style 21 landmarks. +# +# Two skeleton layouts are supported. +# +# Legacy/synthetic layout: +# thumb: MCP, PIP, IP, TIP +# other fingers: MCP, PIP, IP, DIP, TIP +# +# Real MetaGlove layout observed from hardware: +# thumb: MCP, PIP, DIP, TIP +# other fingers: MCP, PIP, IP, DIP, TIP +# +# For the real MetaGlove, MANUS includes an additional metacarpal +# point between the wrist and the anatomical finger MCP landmark. +# Therefore the MediaPipe-style four points for index/middle/ring/ +# pinky are selected as PIP, IP, DIP, TIP. +LEGACY_LANDMARK_KEYS = [ + ("thumb", "mcp"), + ("thumb", "pip"), + ("thumb", "ip"), + ("thumb", "tip"), + + ("index", "mcp"), + ("index", "pip"), + ("index", "dip"), + ("index", "tip"), + + ("middle", "mcp"), + ("middle", "pip"), + ("middle", "dip"), + ("middle", "tip"), + + ("ring", "mcp"), + ("ring", "pip"), + ("ring", "dip"), + ("ring", "tip"), + + ("pinky", "mcp"), + ("pinky", "pip"), + ("pinky", "dip"), + ("pinky", "tip"), +] + + +REAL_METAGLOVE_LANDMARK_KEYS = [ + ("thumb", "mcp"), + ("thumb", "pip"), + ("thumb", "dip"), + ("thumb", "tip"), + + ("index", "pip"), + ("index", "ip"), + ("index", "dip"), + ("index", "tip"), + + ("middle", "pip"), + ("middle", "ip"), + ("middle", "dip"), + ("middle", "tip"), + + ("ring", "pip"), + ("ring", "ip"), + ("ring", "dip"), + ("ring", "tip"), + + ("pinky", "pip"), + ("pinky", "ip"), + ("pinky", "dip"), + ("pinky", "tip"), +] + + +def _normalize(value: str) -> str: + return str(value).strip().lower() + + +def manus_message_to_hand_frame(msg: Any) -> HandFrame: + """Convert a ManusGlove ROS 2 message to a somehand HandFrame.""" + + if len(msg.raw_nodes) != msg.raw_node_count: + raise ValueError( + f"raw_node_count={msg.raw_node_count}, " + f"actual={len(msg.raw_nodes)}" + ) + + nodes_by_id = { + int(node.node_id): node + for node in msg.raw_nodes + } + + if 0 not in nodes_by_id: + raise ValueError("MANUS message does not contain wrist node_id=0") + + nodes_by_type = { + ( + _normalize(node.chain_type), + _normalize(node.joint_type), + ): node + for node in msg.raw_nodes + } + + is_real_metaglove = ( + ("thumb", "dip") in nodes_by_type + and ("thumb", "ip") not in nodes_by_type + ) + + landmark_keys = ( + REAL_METAGLOVE_LANDMARK_KEYS + if is_real_metaglove + else LEGACY_LANDMARK_KEYS + ) + + ordered_nodes = [nodes_by_id[0]] + missing: list[str] = [] + + for chain_name, joint_name in landmark_keys: + node = nodes_by_type.get( + (chain_name, joint_name) + ) + + if node is None: + missing.append( + f"{chain_name}/{joint_name}" + ) + else: + ordered_nodes.append(node) + + if missing: + raise ValueError( + "Missing MANUS nodes: " + ", ".join(missing) + ) + + landmarks = np.asarray( + [ + [ + node.pose.position.x, + node.pose.position.y, + node.pose.position.z, + ] + for node in ordered_nodes + ], + dtype=np.float64, + ) + + if landmarks.shape != (21, 3): + raise ValueError( + f"Expected landmarks shape (21, 3), got {landmarks.shape}" + ) + + if not np.all(np.isfinite(landmarks)): + raise ValueError("MANUS landmarks contain NaN or infinity") + + return HandFrame( + landmarks_3d=landmarks, + landmarks_2d=None, + hand_side=normalize_hand_side(msg.side), + ) + + +class ManusRos2InputSource: + """Read MANUS glove frames from a ROS 2 topic.""" + + def __init__( + self, + *, + topic: str, + hand_side: str, + timeout: float = 2.0, + nominal_fps: int = 120, + ) -> None: + # Lazy import: users running webcam mode should not need ROS 2. + try: + import rclpy + from manus_ros2_msgs.msg import ManusGlove + from rclpy.qos import qos_profile_sensor_data + except ImportError as exc: + raise RuntimeError( + "MANUS input requires ROS 2 Humble, rclpy and " + "manus_ros2_msgs. Source the ROS environments first." + ) from exc + + self.source_desc = f"ros2://{topic}" + self.hand_side = normalize_hand_side(hand_side) + self.timeout = float(timeout) + self._fps = int(nominal_fps) + + self._rclpy = rclpy + self._latest_msg = None + self._available = True + self._received_count = 0 + self._converted_count = 0 + self._timeout_count = 0 + + self._owns_context = not rclpy.ok() + + if self._owns_context: + rclpy.init(args=None) + + self._node = rclpy.create_node( + f"somehand_manus_input_{self.hand_side}" + ) + + self._subscription = self._node.create_subscription( + ManusGlove, + topic, + self._message_callback, + qos_profile_sensor_data, + ) + + @property + def fps(self) -> int: + return self._fps + + def _message_callback(self, msg: Any) -> None: + self._received_count += 1 + + try: + message_side = normalize_hand_side(msg.side) + except ValueError: + return + + if message_side != self.hand_side: + return + + # Only preserve the latest frame to avoid queue buildup. + self._latest_msg = msg + + def is_available(self) -> bool: + return ( + self._available + and self._rclpy.ok() + ) + + def get_frame(self) -> SourceFrame: + if not self.is_available(): + raise StopIteration + + deadline = time.monotonic() + self.timeout + + while self._latest_msg is None: + if not self._rclpy.ok(): + self._available = False + raise StopIteration + + remaining = deadline - time.monotonic() + + if remaining <= 0.0: + self._timeout_count += 1 + + print( + "MANUS input timeout: " + f"no matching {self.hand_side} frame received " + f"from {self.source_desc} for " + f"{self.timeout:.3f}s; stopping session.", + flush=True, + ) + + # A live robot must never continue using the last + # command after the MANUS stream disappears. + raise StopIteration + + self._rclpy.spin_once( + self._node, + timeout_sec=min(0.05, remaining), + ) + + msg = self._latest_msg + self._latest_msg = None + + frame = manus_message_to_hand_frame(msg) + self._converted_count += 1 + + return SourceFrame(detection=frame) + + def reset(self) -> bool: + return False + + def close(self) -> None: + if not self._available: + return + + self._available = False + + try: + self._node.destroy_node() + finally: + if self._owns_context and self._rclpy.ok(): + self._rclpy.shutdown() + + def stats_snapshot(self) -> dict[str, object]: + return { + "messages_received": self._received_count, + "frames_converted": self._converted_count, + "timeouts": self._timeout_count, + } + + +def create_manus_ros2_source( + *, + topic: str, + hand_side: str, + timeout: float, +) -> ManusRos2InputSource: + return ManusRos2InputSource( + topic=topic, + hand_side=hand_side, + timeout=timeout, + ) diff --git a/tests/ros2_manus_source_probe.py b/tests/ros2_manus_source_probe.py new file mode 100644 index 0000000..6463bf1 --- /dev/null +++ b/tests/ros2_manus_source_probe.py @@ -0,0 +1,141 @@ +"""Offline ROS 2 integration probe for ManusRos2InputSource.""" + +from __future__ import annotations + +import argparse +import json +import time +from typing import Any + +import numpy as np + +from somehand.runtime.manus_source import ManusRos2InputSource + + +def _side_text(value: Any) -> str: + """Normalize a string or enum-like hand side for diagnostics.""" + raw = getattr(value, "value", value) + return str(raw).strip().lower() + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--topic", required=True) + parser.add_argument("--side", choices=("left", "right"), required=True) + parser.add_argument("--frames", type=int, default=12) + parser.add_argument("--frame-timeout", type=float, default=1.0) + parser.add_argument("--overall-timeout", type=float, default=15.0) + args = parser.parse_args() + + source = ManusRos2InputSource( + topic=args.topic, + hand_side=args.side, + timeout=args.frame_timeout, + nominal_fps=30, + ) + + landmarks_history: list[np.ndarray] = [] + observed_sides: list[str] = [] + deadline = time.monotonic() + args.overall_timeout + + try: + while ( + len(landmarks_history) < args.frames + and time.monotonic() < deadline + ): + source_frame = source.get_frame() + detection = source_frame.detection + + if detection is None: + continue + + landmarks = np.asarray( + detection.landmarks_3d, + dtype=np.float64, + ) + + if landmarks.shape != (21, 3): + raise RuntimeError( + f"Expected landmarks shape (21, 3), " + f"got {landmarks.shape}" + ) + + if not np.all(np.isfinite(landmarks)): + raise RuntimeError( + "Received landmarks containing NaN or infinity" + ) + + actual_side = _side_text(detection.hand_side) + + if actual_side != args.side: + raise RuntimeError( + f"Expected side={args.side}, got {actual_side}" + ) + + landmarks_history.append(landmarks.copy()) + observed_sides.append(actual_side) + + stats = source.stats_snapshot() + + finally: + source.close() + + if len(landmarks_history) != args.frames: + raise RuntimeError( + f"Expected {args.frames} converted frames, " + f"got {len(landmarks_history)}" + ) + + stacked = np.stack(landmarks_history, axis=0) + + motion_span = float( + np.max( + np.max(stacked, axis=0) + - np.min(stacked, axis=0) + ) + ) + + if motion_span <= 1e-8: + raise RuntimeError( + f"Synthetic sequence did not move: " + f"motion_span={motion_span}" + ) + + converted = int(stats["frames_converted"]) + received = int(stats["messages_received"]) + + if converted < args.frames: + raise RuntimeError( + f"frames_converted={converted}, expected at least {args.frames}" + ) + + if received < converted: + raise RuntimeError( + f"messages_received={received} is less than " + f"frames_converted={converted}" + ) + + result = { + "status": "PASS", + "topic": args.topic, + "expected_side": args.side, + "observed_sides": sorted(set(observed_sides)), + "requested_frames": args.frames, + "received_frames": len(landmarks_history), + "landmark_shape": list(stacked.shape[1:]), + "motion_span": motion_span, + "first_wrist": stacked[0, 0].tolist(), + "first_thumb_tip": stacked[0, 4].tolist(), + "first_index_mcp": stacked[0, 5].tolist(), + "stats": stats, + } + + print( + "PHASE1E_MANUS_SOURCE_PROBE=" + + json.dumps(result, sort_keys=True) + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/test_cli.py b/tests/test_cli.py index bea429e..a8fc610 100644 --- a/tests/test_cli.py +++ b/tests/test_cli.py @@ -649,3 +649,120 @@ def test_webcam_command_uses_default_camera(): assert args.command == "webcam" assert args.camera == 0 + + +def test_manus_ros2_command_uses_safe_defaults(): + parser = build_parser() + args = parser.parse_args( + ["manus-ros2", "--topic", "/manus_glove_1", "--hand", "right"] + ) + + assert args.command == "manus-ros2" + assert args.topic == "/manus_glove_1" + assert args.hand == "right" + assert args.backend == "viewer" + assert args.manus_timeout == 2.0 + assert args.signal_fps is None + + +def test_manus_ros2_command_requires_topic(): + parser = build_parser() + + with pytest.raises(SystemExit): + parser.parse_args(["manus-ros2", "--hand", "left"]) + + +def test_manus_ros2_command_rejects_both_hand_selector(): + parser = build_parser() + + with pytest.raises(SystemExit): + parser.parse_args( + ["manus-ros2", "--topic", "/manus_glove_0", "--hand", "both"] + ) + + +def test_manus_ros2_dispatches_to_command_handler(monkeypatch): + called = [] + + monkeypatch.setattr( + cli_module, + "_run_manus_ros2", + lambda args: called.append((args.topic, args.hand, args.backend)), + ) + + cli_main_module.main( + ["manus-ros2", "--topic", "/manus_glove_0", "--hand", "left"] + ) + + assert called == [("/manus_glove_0", "left", "viewer")] + + +def test_run_manus_ros2_builds_live_source_and_session(monkeypatch): + calls = {} + + class _FakeSource: + source_desc = "ros2:///manus_glove_0" + fps = 30 + + class _FakeSession: + def run(self, source, **kwargs): + calls["run_source"] = source + calls["run_kwargs"] = kwargs + return SimpleNamespace( + num_frames=12, + num_detected=12, + source_desc=source.source_desc, + input_type="manus_ros2", + ) + + def _fake_create_manus_ros2_source(**kwargs): + calls["source_kwargs"] = kwargs + return _FakeSource() + + def _fake_build_engine(args, **kwargs): + calls["engine_kwargs"] = kwargs + return SimpleNamespace( + describe=lambda: {"model_name": "revo2", "dof": 6, "vector_pairs": 5} + ) + + def _fake_build_runtime_session(engine, args, **kwargs): + calls["session_kwargs"] = kwargs + return _FakeSession() + + monkeypatch.setattr(cli_module, "create_manus_ros2_source", _fake_create_manus_ros2_source) + monkeypatch.setattr(cli_module, "_wrap_live_hand_source", lambda source, **kwargs: source) + monkeypatch.setattr( + cli_module, + "_wrap_source_for_interactive_recording", + lambda source, **kwargs: (source, None), + ) + monkeypatch.setattr(cli_module, "_build_engine", _fake_build_engine) + monkeypatch.setattr(cli_module, "_build_runtime_session", _fake_build_runtime_session) + monkeypatch.setattr(cli_module, "_print_startup", lambda *args, **kwargs: None) + monkeypatch.setattr(cli_module, "_finalize_run", lambda *args, **kwargs: None) + + args = SimpleNamespace( + topic="/manus_glove_0", + hand="left", + manus_timeout=1.5, + signal_fps=30, + record_output=None, + backend="viewer", + config="unused.yaml", + ) + + cli_module._run_manus_ros2(args) + + assert calls["source_kwargs"] == { + "topic": "/manus_glove_0", + "hand_side": "left", + "timeout": 1.5, + } + assert calls["engine_kwargs"] == {"input_type": "manus_ros2"} + assert calls["session_kwargs"] == { + "visualize": True, + "show_preview": False, + "key_callback": None, + } + assert calls["run_kwargs"]["input_type"] == "manus_ros2" + assert calls["run_kwargs"]["stop_condition"] is None diff --git a/tests/test_lazy_imports.py b/tests/test_lazy_imports.py index bfd5cef..791301b 100644 --- a/tests/test_lazy_imports.py +++ b/tests/test_lazy_imports.py @@ -39,3 +39,28 @@ def test_pico_input_import_does_not_import_cv2(): def test_pico_source_adapter_import_does_not_import_cv2(): _assert_import_does_not_import_cv2("somehand.runtime.source_adapters") + + +def test_manus_source_import_does_not_import_rclpy(): + env = dict(os.environ) + env["PYTHONPATH"] = str(_SRC) + script = """ +import builtins +import importlib + +original_import = builtins.__import__ + +def guarded_import(name, globals=None, locals=None, fromlist=(), level=0): + if name == "rclpy" or name.startswith("rclpy."): + raise AssertionError("manus_source should import rclpy lazily") + return original_import(name, globals, locals, fromlist, level) + +builtins.__import__ = guarded_import +importlib.import_module("somehand.runtime.manus_source") +""" + subprocess.run( + [sys.executable, "-c", script], + cwd=_REPO_ROOT, + env=env, + check=True, + ) diff --git a/tests/test_manus_calibration.py b/tests/test_manus_calibration.py new file mode 100644 index 0000000..1de6d9a --- /dev/null +++ b/tests/test_manus_calibration.py @@ -0,0 +1,166 @@ +"""Tests for calibrated MANUS -> Revo2 mapping.""" + +from __future__ import annotations + +import json + +import numpy as np +import pytest + +from somehand.application.manus_calibration import ( + FINGER_NAMES, + ManusCalibrationProfile, + Revo2ManusQposMapper, + apply_deadband, + load_manus_calibration_profile, +) + + +def _profile(side: str = "right") -> ManusCalibrationProfile: + from somehand.application.manus_calibration import ( + FingerCalibration, + ) + + return ManusCalibrationProfile( + version=1, + side=side, + fingers={ + name: FingerCalibration( + open_deg=0.0, + comfortable_closed_deg=90.0, + ) + for name in FINGER_NAMES + }, + ) + + +def _closed_landmarks() -> np.ndarray: + landmarks = np.zeros((21, 3), dtype=np.float64) + bases = (1, 5, 9, 13, 17) + for base in bases: + landmarks[base] = [0.0, 0.0, 0.0] + landmarks[base + 1] = [1.0, 0.0, 0.0] + landmarks[base + 2] = [1.0, 0.0, 0.0] + landmarks[base + 3] = [1.0, 1.0, 0.0] + return landmarks + + +class _FakeHandModel: + def get_joint_name_to_qpos_index(self): + return { + "right_thumb_metacarpal_joint": 0, + "right_thumb_proximal_joint": 1, + "right_index_proximal_joint": 3, + "right_middle_proximal_joint": 5, + "right_ring_proximal_joint": 7, + "right_pinky_proximal_joint": 9, + } + + def apply_mimic_constraints(self, qpos): + result = np.asarray(qpos, dtype=np.float64).copy() + result[2] = result[1] + result[4] = result[3] + result[6] = result[5] + result[8] = result[7] + result[10] = result[9] + return result + + +def test_load_profile_validates_side(tmp_path): + path = tmp_path / "calibration.json" + path.write_text( + json.dumps( + { + "version": 1, + "side": "Right", + "fingers": { + name: { + "open_deg": 0.0, + "comfortable_closed_deg": 90.0, + } + for name in FINGER_NAMES + }, + } + ), + encoding="utf-8", + ) + + profile = load_manus_calibration_profile( + path, + expected_side="right", + ) + assert profile.side == "right" + + with pytest.raises(ValueError, match="does not match"): + load_manus_calibration_profile( + path, + expected_side="left", + ) + + +def test_load_profile_rejects_non_positive_span(tmp_path): + path = tmp_path / "calibration.json" + path.write_text( + json.dumps( + { + "version": 1, + "side": "right", + "fingers": { + name: { + "open_deg": 10.0, + "comfortable_closed_deg": 10.0, + } + for name in FINGER_NAMES + }, + } + ), + encoding="utf-8", + ) + + with pytest.raises(ValueError, match="span"): + load_manus_calibration_profile(path) + + +def test_apply_deadband(): + assert apply_deadband(0.05, 0.08) == 0.0 + assert apply_deadband(1.0, 0.08) == pytest.approx(1.0) + assert apply_deadband(0.54, 0.08) == pytest.approx(0.5) + + +def test_mapper_applies_safe_ranges_and_mimic_constraints(): + mapper = Revo2ManusQposMapper( + hand_model=_FakeHandModel(), + bounds=[(0.0, 1.0)] * 11, + profile=_profile(), + side="right", + ) + + qpos, raw, corrected = mapper.map( + _closed_landmarks(), + np.zeros(11, dtype=np.float64), + ) + + assert raw == pytest.approx( + {name: 1.0 for name in FINGER_NAMES} + ) + assert corrected == pytest.approx( + {name: 1.0 for name in FINGER_NAMES} + ) + + assert qpos[0] == pytest.approx(0.35) + assert qpos[1] == pytest.approx(0.55) + assert qpos[2] == pytest.approx(0.55) + + for source, mimic in ((3, 4), (5, 6), (7, 8), (9, 10)): + assert qpos[source] == pytest.approx(0.65) + assert qpos[mimic] == pytest.approx(0.65) + + +def test_mapper_rejects_wrong_side(): + with pytest.raises(ValueError, match="side"): + Revo2ManusQposMapper( + hand_model=_FakeHandModel(), + bounds=[(0.0, 1.0)] * 11, + profile=_profile(side="left"), + side="right", + ) diff --git a/tests/test_manus_pinky_metric.py b/tests/test_manus_pinky_metric.py new file mode 100644 index 0000000..0863abb --- /dev/null +++ b/tests/test_manus_pinky_metric.py @@ -0,0 +1,79 @@ +"""Tests for per-finger MANUS curl metrics.""" + +from __future__ import annotations + +import json + +import numpy as np + +from somehand.application.manus_calibration import ( + FINGER_NAMES, + finger_curl_degrees, + finger_joint_sum_degrees, + load_manus_calibration_profile, +) + + +def test_joint_sum_detects_mcp_only_flexion(): + landmarks = np.zeros( + (21, 3), + dtype=np.float64, + ) + + # Wrist -> MCP points along +X. + landmarks[0] = [0.0, 0.0, 0.0] + landmarks[17] = [1.0, 0.0, 0.0] + + # The entire pinky points along +Y. This represents an + # MCP-only bend: internal segments remain parallel. + landmarks[18] = [1.0, 1.0, 0.0] + landmarks[19] = [1.0, 2.0, 0.0] + landmarks[20] = [1.0, 3.0, 0.0] + + indices = (17, 18, 19, 20) + + assert finger_curl_degrees( + landmarks, + indices, + ) == 0.0 + + assert finger_joint_sum_degrees( + landmarks, + indices, + ) == 90.0 + + +def test_profile_supports_per_finger_metric(tmp_path): + path = tmp_path / "profile.json" + + fingers = { + name: { + "open_deg": 0.0, + "comfortable_closed_deg": 90.0, + "metric": ( + "joint_sum" + if name == "pinky" + else "current" + ), + } + for name in FINGER_NAMES + } + + path.write_text( + json.dumps( + { + "version": 1, + "side": "right", + "fingers": fingers, + } + ), + encoding="utf-8", + ) + + profile = load_manus_calibration_profile( + path, + expected_side="right", + ) + + assert profile.fingers["ring"].metric == "current" + assert profile.fingers["pinky"].metric == "joint_sum" diff --git a/tests/test_manus_source.py b/tests/test_manus_source.py new file mode 100644 index 0000000..15327e9 --- /dev/null +++ b/tests/test_manus_source.py @@ -0,0 +1,202 @@ +"""Unit tests for the MANUS ROS 2 input conversion.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import numpy as np +import pytest + +from somehand.runtime.manus_source import ManusRos2InputSource, manus_message_to_hand_frame + + +MANUS_SPECS = [ + ("Hand", "Invalid"), + ("Thumb", "MCP"), + ("Thumb", "PIP"), + ("Thumb", "IP"), + ("Thumb", "TIP"), + ("Index", "MCP"), + ("Index", "PIP"), + ("Index", "IP"), + ("Index", "DIP"), + ("Index", "TIP"), + ("Middle", "MCP"), + ("Middle", "PIP"), + ("Middle", "IP"), + ("Middle", "DIP"), + ("Middle", "TIP"), + ("Ring", "MCP"), + ("Ring", "PIP"), + ("Ring", "IP"), + ("Ring", "DIP"), + ("Ring", "TIP"), + ("Pinky", "MCP"), + ("Pinky", "PIP"), + ("Pinky", "IP"), + ("Pinky", "DIP"), + ("Pinky", "TIP"), +] + +EXPECTED_NODE_IDS = [ + 0, + 1, 2, 3, 4, + 5, 6, 8, 9, + 10, 11, 13, 14, + 15, 16, 18, 19, + 20, 21, 23, 24, +] + + +def _node(node_id: int, chain: str, joint: str): + return SimpleNamespace( + node_id=node_id, + chain_type=chain, + joint_type=joint, + pose=SimpleNamespace( + position=SimpleNamespace( + x=float(node_id), + y=float(node_id) + 0.1, + z=float(node_id) + 0.2, + ) + ), + ) + + +def _message(side: str = "Right"): + nodes = [ + _node(node_id, chain, joint) + for node_id, (chain, joint) in enumerate(MANUS_SPECS) + ] + + return SimpleNamespace( + side=side, + raw_nodes=nodes, + raw_node_count=len(nodes), + ) + + +def test_complete_manus_message_maps_to_media_pipe_order(): + frame = manus_message_to_hand_frame(_message()) + + assert frame.landmarks_3d.shape == (21, 3) + assert np.all(np.isfinite(frame.landmarks_3d)) + + np.testing.assert_allclose( + frame.landmarks_3d[:, 0], + np.asarray(EXPECTED_NODE_IDS, dtype=np.float64), + ) + + +def test_real_thumb_dip_is_accepted_as_thumb_ip_landmark(): + msg = _message() + + # Real MetaGlove messages use Thumb/DIP where the synthetic fixture + # historically used Thumb/IP. + msg.raw_nodes[3].joint_type = "DIP" + + frame = manus_message_to_hand_frame(msg) + + np.testing.assert_allclose( + frame.landmarks_3d[3], + np.asarray([3.0, 3.1, 3.2]), + ) + + +def test_raw_node_count_mismatch_is_rejected(): + msg = _message() + msg.raw_node_count += 1 + + with pytest.raises(ValueError, match="raw_node_count"): + manus_message_to_hand_frame(msg) + + +def test_nan_landmark_is_rejected(): + msg = _message() + msg.raw_nodes[8].pose.position.x = float("nan") + + with pytest.raises(ValueError, match="NaN or infinity"): + manus_message_to_hand_frame(msg) + + +def test_real_metaglove_shifts_non_thumb_landmarks(): + msg = _message() + + # Real MetaGlove uses Thumb/DIP instead of the legacy Thumb/IP. + msg.raw_nodes[3].joint_type = "DIP" + + frame = manus_message_to_hand_frame(msg) + + def node_position(node_index): + position = msg.raw_nodes[node_index].pose.position + return np.asarray( + [ + position.x, + position.y, + position.z, + ], + dtype=np.float64, + ) + + # Real MetaGlove non-thumb layout: + # MCP, PIP, IP, DIP, TIP + # + # MediaPipe output: + # MCP, PIP, DIP, TIP + # + # Therefore the first MANUS MCP point is omitted. + expected_raw_indices = [ + 6, 7, 8, 9, # index + 11, 12, 13, 14, # middle + 16, 17, 18, 19, # ring + 21, 22, 23, 24, # pinky + ] + + for output_index, raw_index in enumerate( + expected_raw_indices, + start=5, + ): + np.testing.assert_allclose( + frame.landmarks_3d[output_index], + node_position(raw_index), + ) + + + +class _FakeRclpy: + def ok(self) -> bool: + return True + + def spin_once( + self, + node, + *, + timeout_sec: float, + ) -> None: + return None + + +def test_manus_source_timeout_raises_stop_iteration( + capsys, +): + source = ManusRos2InputSource.__new__( + ManusRos2InputSource + ) + + source.source_desc = "ros2:///manus_glove_0" + source.hand_side = "right" + source.timeout = 0.0 + source._rclpy = _FakeRclpy() + source._node = object() + source._latest_msg = None + source._available = True + source._timeout_count = 0 + + with pytest.raises(StopIteration): + source.get_frame() + + captured = capsys.readouterr() + + assert "MANUS input timeout" in captured.out + assert source._timeout_count == 1 + assert source._available is True diff --git a/tests/test_runtime_imports.py b/tests/test_runtime_imports.py index 2177f3e..d4119c2 100644 --- a/tests/test_runtime_imports.py +++ b/tests/test_runtime_imports.py @@ -14,6 +14,7 @@ [ "somehand.runtime.sink_outputs", "somehand.runtime.sink_rendering", + "somehand.runtime.manus_source", "somehand.runtime.source_adapters", "somehand.runtime.source_recording", "somehand.runtime.source_sampling", diff --git a/tests/test_source_sampling.py b/tests/test_source_sampling.py index 6e00fcc..5f12711 100644 --- a/tests/test_source_sampling.py +++ b/tests/test_source_sampling.py @@ -2,6 +2,7 @@ from pathlib import Path import numpy as np +import pytest sys.path.insert(0, str(Path(__file__).parent.parent / "src")) @@ -122,3 +123,39 @@ def test_fixed_rate_bihand_source_uses_requested_sample_fps(): assert frame.right is not None np.testing.assert_allclose(frame.left.landmarks_3d, 2.0) np.testing.assert_allclose(frame.right.landmarks_3d, 4.0) + + + +class _StoppingHandSource: + source_desc = "fake://stopping-hand" + + @property + def fps(self) -> int: + return 120 + + def is_available(self) -> bool: + return True + + def get_frame(self): + raise StopIteration + + def reset(self) -> bool: + return False + + def close(self) -> None: + return None + + def stats_snapshot(self): + return {} + + +def test_fixed_rate_hand_source_propagates_stop_iteration(): + source = _StoppingHandSource() + + wrapped = FixedRateHandTrackingSource( + source, + sample_fps=30, + ) + + with pytest.raises(StopIteration): + wrapped.get_frame()