diff --git a/orca_core/base_hand.py b/orca_core/base_hand.py index a7e04c6..105d4f0 100644 --- a/orca_core/base_hand.py +++ b/orca_core/base_hand.py @@ -8,9 +8,11 @@ import time from abc import ABC, abstractmethod +from collections.abc import Mapping, Sequence import numpy as np +from .demo_presets import DEMO_POSE_FRACTIONS, DEMO_SEQUENCES from .hand_config import BaseHandConfig from .joint_position import OrcaJointPositions @@ -61,9 +63,8 @@ def __init__( ) self.config.validate() OrcaJointPositions.register_joint_names(self.config.joint_ids) - + self.recorded_positions: dict[str, OrcaJointPositions] = {} - @abstractmethod def _get_joint_positions(self) -> OrcaJointPositions: @@ -77,27 +78,44 @@ def _set_joint_positions(self, joint_pos: OrcaJointPositions) -> bool: def _coerce_joint_positions( self, - joint_pos: OrcaJointPositions | dict[str, float | None] | np.ndarray, + joint_pos: OrcaJointPositions | Mapping[str, float | None] | np.ndarray, ) -> OrcaJointPositions: if isinstance(joint_pos, OrcaJointPositions): return joint_pos - if isinstance(joint_pos, dict): - return OrcaJointPositions.from_dict(joint_pos) + if isinstance(joint_pos, Mapping): + return OrcaJointPositions.from_dict(dict(joint_pos)) if isinstance(joint_pos, np.ndarray): return OrcaJointPositions.from_ndarray(joint_pos, joint_ids=self.config.joint_ids) raise TypeError( - "joint_pos must be an OrcaJointPositions instance, a dict, or a 1-D numpy array." + "joint_pos must be an OrcaJointPositions instance, a mapping, or a 1-D numpy array." ) - + + def pose_from_fractions( + self, + fractions: Mapping[str, float], + ) -> OrcaJointPositions: + """Build a pose by specifying joints as fractions of their ROM, read from config.""" + pose_dict = {joint: 0.0 for joint in self.config.joint_ids} + pose_dict.update(self.config.neutral_position) + + for joint, fraction in fractions.items(): + if joint not in self.config.joint_roms_dict: + continue + + joint_min, joint_max = self.config.joint_roms_dict[joint] + pose_dict[joint] = joint_min + float(fraction) * (joint_max - joint_min) + + return self.config.clamp_joint_positions(OrcaJointPositions.from_dict(pose_dict)) + def set_joint_positions( self, - joint_pos: OrcaJointPositions | dict[str, float | None] | np.ndarray, + joint_pos: OrcaJointPositions | Mapping[str, float | None] | np.ndarray, num_steps: int = 1, step_size: float = 1e-2, -): + ): """Command the hand to a target joint configuration. Positions are clamped to configured ROM bounds before being sent to @@ -146,19 +164,68 @@ def _linear_waypoints_to( }) ) return out - + def register_position(self, name: str, joint_pos: OrcaJointPositions): - self.recorded_positions[name] = joint_pos - + self.recorded_positions[name] = self.config.clamp_joint_positions(joint_pos) + def remove_position(self, name: str): try: del self.recorded_positions[name] except KeyError: pass # position was not among recorded positions - + def set_named_position(self, name: str, num_steps: int = 1, step_size: float = 1.0): self.set_joint_positions(self.recorded_positions[name], num_steps=num_steps, step_size=step_size) + def play_named_positions( + self, + names: Sequence[str], + cycles: int = 1, + num_steps: int = 1, + step_size: float = 1.0, + return_to_neutral: bool = False, + neutral_num_steps: int | None = None, + neutral_step_size: float | None = None, + ) -> None: + if cycles < 0: + raise ValueError("cycles must be non-negative.") + + for _ in range(cycles): + for name in names: + self.set_named_position(name, num_steps=num_steps, step_size=step_size) + if return_to_neutral: + self.set_neutral_position( + num_steps=num_steps if neutral_num_steps is None else neutral_num_steps, + step_size=step_size if neutral_step_size is None else neutral_step_size, + ) + + def run_demo( + self, + demo_name: str = "main", + cycles: int = 1, + num_steps: int = NUM_STEPS, + step_size: float = STEP_SIZE, + return_to_neutral: bool = True, + ) -> tuple[str, ...]: + if demo_name not in DEMO_POSE_FRACTIONS or demo_name not in DEMO_SEQUENCES: + available = ", ".join(sorted(DEMO_SEQUENCES)) + raise ValueError(f"Unknown demo '{demo_name}'. Available demos: {available}.") + + poses = { + name: self.pose_from_fractions(fractions) + for name, fractions in DEMO_POSE_FRACTIONS[demo_name].items() + } + sequence = DEMO_SEQUENCES[demo_name] + + for _ in range(cycles): + for name in sequence: + self.set_joint_positions(poses[name], num_steps=num_steps, step_size=step_size) + + if return_to_neutral: + self.set_neutral_position(num_steps=num_steps, step_size=step_size) + + return sequence + def set_neutral_position(self, num_steps: int = NUM_STEPS, step_size: float = STEP_SIZE): """Move hand to neutral position.""" self.set_joint_positions( diff --git a/orca_core/demo_presets.py b/orca_core/demo_presets.py new file mode 100644 index 0000000..6bbefa1 --- /dev/null +++ b/orca_core/demo_presets.py @@ -0,0 +1,99 @@ +from typing import Final + + +DEMO_POSE_FRACTIONS: Final[dict[str, dict[str, dict[str, float]]]] = { + "main": { + "open_hand": { + "thumb_cmc": 0.70, + "thumb_abd": 0.80, + "thumb_mcp": 0.85, + "thumb_dip": 0.75, + "index_abd": 0.10, + "middle_abd": 0.50, + "ring_abd": 0.70, + "pinky_abd": 0.85, + "index_mcp": 0.15, + "middle_mcp": 0.15, + "ring_mcp": 0.15, + "pinky_mcp": 0.15, + "index_pip": 0.10, + "middle_pip": 0.10, + "ring_pip": 0.10, + "pinky_pip": 0.10, + "wrist": 0.30, + }, + "power_grasp": { + "thumb_cmc": 0.35, + "thumb_abd": 0.55, + "thumb_mcp": 0.20, + "thumb_dip": 0.85, + "index_mcp": 0.85, + "middle_mcp": 0.85, + "ring_mcp": 0.85, + "pinky_mcp": 0.85, + "index_pip": 0.90, + "middle_pip": 0.90, + "ring_pip": 0.90, + "pinky_pip": 0.90, + "wrist": 0.55, + }, + "pinch": { + "thumb_cmc": 0.45, + "thumb_abd": 0.65, + "thumb_mcp": 0.40, + "thumb_dip": 0.75, + "index_abd": 0.30, + "index_mcp": 0.70, + "index_pip": 0.75, + "middle_mcp": 0.30, + "middle_pip": 0.20, + "ring_mcp": 0.20, + "ring_pip": 0.15, + "pinky_mcp": 0.20, + "pinky_pip": 0.15, + "wrist": 0.45, + }, + }, + "abduction": { + "fan_out": { + "thumb_abd": 0.85, + "index_abd": 0.10, + "middle_abd": 0.50, + "ring_abd": 0.80, + "pinky_abd": 0.90, + "wrist": 0.40, + }, + "fan_in": { + "thumb_abd": 0.25, + "index_abd": 0.85, + "middle_abd": 0.50, + "ring_abd": 0.20, + "pinky_abd": 0.15, + "wrist": 0.55, + }, + "spread_grasp": { + "thumb_cmc": 0.55, + "thumb_abd": 0.70, + "thumb_mcp": 0.45, + "thumb_dip": 0.65, + "index_abd": 0.15, + "middle_abd": 0.50, + "ring_abd": 0.80, + "pinky_abd": 0.90, + "index_mcp": 0.65, + "middle_mcp": 0.65, + "ring_mcp": 0.65, + "pinky_mcp": 0.65, + "index_pip": 0.70, + "middle_pip": 0.70, + "ring_pip": 0.70, + "pinky_pip": 0.70, + }, + }, +} + + +DEMO_SEQUENCES: Final[dict[str, tuple[str, ...]]] = { + "main": ("open_hand", "power_grasp", "pinch"), + "abduction": ("fan_out", "fan_in", "spread_grasp"), +} diff --git a/scripts/main_demo.py b/scripts/main_demo.py index b3e6d29..09fb8fa 100644 --- a/scripts/main_demo.py +++ b/scripts/main_demo.py @@ -1,19 +1,6 @@ - - import argparse from common import add_hand_arguments, connect_hand, create_hand, shutdown_hand -from orca_core import OrcaJointPositions - - -def pose_from_fractions(hand, fractions: dict[str, float]) -> OrcaJointPositions: - pose = dict(hand.config.neutral_position) - for joint, fraction in fractions.items(): - if joint not in hand.config.joint_roms_dict: - continue - joint_min, joint_max = hand.config.joint_roms_dict[joint] - pose[joint] = joint_min + fraction * (joint_max - joint_min) - return OrcaJointPositions.from_dict(pose) def main() -> int: @@ -29,79 +16,14 @@ def main() -> int: hand = create_hand(args.config_path, use_mock=args.mock) try: connect_hand(hand) - hand.init_joints(force_calibrate=args.mock) - - demo_poses = { - "open_hand": pose_from_fractions( - hand, - { - "thumb_cmc": 0.70, - "thumb_abd": 0.80, - "thumb_mcp": 0.85, - "thumb_dip": 0.75, - "index_abd": 0.10, - "middle_abd": 0.50, - "ring_abd": 0.70, - "pinky_abd": 0.85, - "index_mcp": 0.15, - "middle_mcp": 0.15, - "ring_mcp": 0.15, - "pinky_mcp": 0.15, - "index_pip": 0.10, - "middle_pip": 0.10, - "ring_pip": 0.10, - "pinky_pip": 0.10, - "wrist": 0.30, - }, - ), - "power_grasp": pose_from_fractions( - hand, - { - "thumb_cmc": 0.35, - "thumb_abd": 0.55, - "thumb_mcp": 0.20, - "thumb_dip": 0.85, - "index_mcp": 0.85, - "middle_mcp": 0.85, - "ring_mcp": 0.85, - "pinky_mcp": 0.85, - "index_pip": 0.90, - "middle_pip": 0.90, - "ring_pip": 0.90, - "pinky_pip": 0.90, - "wrist": 0.55, - }, - ), - "pinch": pose_from_fractions( - hand, - { - "thumb_cmc": 0.45, - "thumb_abd": 0.65, - "thumb_mcp": 0.40, - "thumb_dip": 0.75, - "index_abd": 0.30, - "index_mcp": 0.70, - "index_pip": 0.75, - "middle_mcp": 0.30, - "middle_pip": 0.20, - "ring_mcp": 0.20, - "ring_pip": 0.15, - "pinky_mcp": 0.20, - "pinky_pip": 0.15, - "wrist": 0.45, - }, - ), - } - - for name, pose in demo_poses.items(): - hand.register_position(name, pose) + hand.init_joints(False) print("Cycling through open_hand -> power_grasp -> pinch -> neutral") - for _ in range(args.cycles): - for name in ("open_hand", "power_grasp", "pinch"): - print(f"Moving to {name}") - hand.set_named_position(name, num_steps=args.num_steps, step_size=args.step_size) - hand.set_neutral_position(num_steps=args.num_steps, step_size=args.step_size) + hand.run_demo( + "main", + cycles=args.cycles, + num_steps=250 + ) return 0 except KeyboardInterrupt: diff --git a/scripts/main_demo_abduction.py b/scripts/main_demo_abduction.py index 6461734..3fd58d2 100644 --- a/scripts/main_demo_abduction.py +++ b/scripts/main_demo_abduction.py @@ -1,19 +1,6 @@ - - import argparse from common import add_hand_arguments, connect_hand, create_hand, shutdown_hand -from orca_core import OrcaJointPositions - - -def pose_from_fractions(hand, fractions: dict[str, float]) -> OrcaJointPositions: - pose = dict(hand.config.neutral_position) - for joint, fraction in fractions.items(): - if joint not in hand.config.joint_roms_dict: - continue - joint_min, joint_max = hand.config.joint_roms_dict[joint] - pose[joint] = joint_min + fraction * (joint_max - joint_min) - return OrcaJointPositions.from_dict(pose) def main() -> int: @@ -31,61 +18,13 @@ def main() -> int: connect_hand(hand) hand.init_joints(force_calibrate=args.mock) - demo_poses = { - "fan_out": pose_from_fractions( - hand, - { - "thumb_abd": 0.85, - "index_abd": 0.10, - "middle_abd": 0.50, - "ring_abd": 0.80, - "pinky_abd": 0.90, - "wrist": 0.40, - }, - ), - "fan_in": pose_from_fractions( - hand, - { - "thumb_abd": 0.25, - "index_abd": 0.85, - "middle_abd": 0.50, - "ring_abd": 0.20, - "pinky_abd": 0.15, - "wrist": 0.55, - }, - ), - "spread_grasp": pose_from_fractions( - hand, - { - "thumb_cmc": 0.55, - "thumb_abd": 0.70, - "thumb_mcp": 0.45, - "thumb_dip": 0.65, - "index_abd": 0.15, - "middle_abd": 0.50, - "ring_abd": 0.80, - "pinky_abd": 0.90, - "index_mcp": 0.65, - "middle_mcp": 0.65, - "ring_mcp": 0.65, - "pinky_mcp": 0.65, - "index_pip": 0.70, - "middle_pip": 0.70, - "ring_pip": 0.70, - "pinky_pip": 0.70, - }, - ), - } - - for name, pose in demo_poses.items(): - hand.register_position(name, pose) - print("Cycling through fan_out -> fan_in -> spread_grasp -> neutral") - for _ in range(args.cycles): - for name in ("fan_out", "fan_in", "spread_grasp"): - print(f"Moving to {name}") - hand.set_named_position(name, num_steps=args.num_steps, step_size=args.step_size) - hand.set_neutral_position(num_steps=args.num_steps, step_size=args.step_size) + hand.run_demo( + "abduction", + cycles=args.cycles, + num_steps=args.num_steps, + step_size=args.step_size, + ) return 0 except KeyboardInterrupt: print("\nDemo interrupted.") diff --git a/tests/test_base_hand_contract.py b/tests/test_base_hand_contract.py index d20d3ab..dcca4fe 100644 --- a/tests/test_base_hand_contract.py +++ b/tests/test_base_hand_contract.py @@ -134,6 +134,37 @@ def test_set_neutral_position_uses_configured_neutral_pose(hand): } +def test_pose_from_fractions_defaults_to_neutral_for_unspecified_joints(hand): + pose = hand.pose_from_fractions({"joint_a": 0.75, "joint_c": 0.25}) + assert pose.as_dict() == { + "joint_a": 0.5, + "joint_b": 1.25, + "joint_c": -0.25, + } + + +def test_play_named_positions_reuses_registered_poses(hand): + positions = { + "pose_one": OrcaJointPositions.from_dict({"joint_a": -0.5, "joint_b": 0.5, "joint_c": 0.0}), + "pose_two": OrcaJointPositions.from_dict({"joint_a": 1.0, "joint_b": 2.0, "joint_c": -0.5}), + } + for name, joint_pos in positions.items(): + hand.register_position(name, joint_pos) + + hand.play_named_positions( + list(positions.keys()), + cycles=2, + num_steps=1, + step_size=0.0, + ) + + assert hand.get_joint_position().as_dict() == { + "joint_a": 1.0, + "joint_b": 2.0, + "joint_c": -0.5, + } + + def test_set_neutral_position_without_configured_neutral_pose_is_noop(tmp_path): config_path = tmp_path / "config.yaml" with open(config_path, "w", encoding="utf-8") as file: diff --git a/tests/test_core.py b/tests/test_core.py index 2add347..1d4d5db 100644 --- a/tests/test_core.py +++ b/tests/test_core.py @@ -21,8 +21,11 @@ def test_orca_hand_exposes_refactored_methods(mock_config_dir): "calibrate", "tension", "jitter", + "pose_from_fractions", "register_position", "set_named_position", + "play_named_positions", + "run_demo", "get_joint_position", ]: assert hasattr(hand, method_name), f"Missing hardware method: {method_name}" @@ -49,3 +52,14 @@ def test_mock_workflow_smoke(initialized_mock_hand): actual = hand.get_joint_position().as_dict() for joint, expected in pose.as_dict().items(): assert actual[joint] == pytest.approx(expected, abs=1e-6) + + +def test_builtin_demo_positions_can_run_on_mock_hand(initialized_mock_hand): + hand = initialized_mock_hand + + sequence = hand.run_demo("main", cycles=1, num_steps=1, step_size=0.0) + assert sequence == ("open_hand", "power_grasp", "pinch") + + actual = hand.get_joint_position().as_dict() + for joint, expected in hand.config.neutral_position.items(): + assert actual[joint] == pytest.approx(expected)