Add recorded demos

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(

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"),
+}

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: