Animation: dock demo alive-head mode and documentation (workstream D)

animation/demo/README.md

@@ -0,0 +1,10 @@
+# Dock demo animation
+
+`DockMode` keeps Open Duck Mini visibly awake while it is parked.  Call
+`step(0.02)` at 50 Hz; it returns the canonical 16-joint target array.  Its ten
+leg entries are always byte-identical to the supplied docked pose.  Clips which
+mention a leg joint are rejected during registration.
+
+Run `PYTHONPATH=animation python animation/demo/run_demo.py --duration 20`.
+`--backend print` is dependency-free.  `--backend mujoco` currently validates
+that MuJoCo is importable and degrades to print output when it is absent.

animation/demo/__init__.py

@@ -0,0 +1,8 @@
+"""Procedural, dock-safe animation support for Open Duck Mini v2."""
+
+from .dock import DockMode
+from .gaze import GazeSolver, GazeTracker
+from .idle_engine import IdleEngine, IdleEngineConfig
+from .scheduler import BehaviorScheduler
+
+__all__ = ["BehaviorScheduler", "DockMode", "GazeSolver", "GazeTracker", "IdleEngine", "IdleEngineConfig"]

animation/demo/dock.py

@@ -0,0 +1,151 @@
+"""Dock demonstration mode with an unconditional final leg-position mask."""
+
+from __future__ import annotations
+
+import logging
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from duck_anim.joints import ALL_JOINTS, JOINT_INDEX, LEG_JOINTS, to_array
+from duck_anim.loader import load_clip_dir
+from .idle_engine import IdleEngine
+from .scheduler import BehaviorScheduler
+
+LOG = logging.getLogger(__name__)
+
+
+class _FallbackPlayer:
+    """Small compatibility player used only until duck_anim.player is available."""
+    def __init__(self, clip: Any, speed: float = 1.0, **_: Any) -> None:
+        self.clip, self.speed, self.time, self.finished, self.weight_scale = clip, speed, 0.0, False, 1.0
+
+    def update(self, dt: float) -> None:
+        self.time += dt * self.speed
+        self.finished = not self.clip.loop and self.time >= self.clip.duration
+
+    def sample(self) -> tuple[np.ndarray, float]:
+        output = np.zeros(len(ALL_JOINTS), dtype=np.float32)
+        if self.clip.n_frames:
+            index = min(int(self.time * self.clip.fps), self.clip.n_frames - 1)
+            for joint, value in zip(self.clip.joints, self.clip.frames[index]):
+                output[JOINT_INDEX[joint]] = value
+        return output, self.weight_scale
+
+
+class _FallbackMixer:
+    def __init__(self) -> None:
+        self.active_clips: dict[str, _FallbackPlayer] = {}
+
+    def add(self, player: _FallbackPlayer, name: str | None = None) -> str:
+        name = name or player.clip.name
+        self.active_clips[name] = player
+        return name
+
+    def update(self, dt: float) -> None:
+        for name, player in list(self.active_clips.items()):
+            player.update(dt)
+            if player.finished:
+                del self.active_clips[name]
+
+    def mix(self, base: np.ndarray) -> np.ndarray:
+        result = base.copy()
+        for player in self.active_clips.values():
+            values, weight = player.sample()
+            if player.clip.layer == "additive":
+                result += values * weight
+            else:
+                present = [JOINT_INDEX[j] for j in player.clip.joints]
+                result[present] = result[present] * (1 - weight) + values[present] * weight
+        return result
+
+
+class _FallbackLimiter:
+    def __init__(self, **_: Any) -> None:
+        pass
+
+    def apply(self, target: np.ndarray, previous_output: np.ndarray) -> np.ndarray:
+        return target.copy()
+
+
+try:
+    from duck_anim.mixer import LayeredMixer  # type: ignore
+    from duck_anim.player import AnimationPlayer  # type: ignore
+    from duck_anim.safety import JointSafetyLimiter  # type: ignore
+except ImportError:  # Workstream A may not yet be present in an isolated checkout.
+    LayeredMixer, AnimationPlayer, JointSafetyLimiter = _FallbackMixer, _FallbackPlayer, _FallbackLimiter
+
+
+class DockMode:
+    """Animation controller for a duck parked on its dock.
+
+    The final mask is intentional redundancy: no dependency or malformed clip can
+    affect the ten leg values returned by :meth:`step`.
+    """
+
+    def __init__(self, clips_dir: str | Path | None = None, docked_pose: np.ndarray | None = None, seed: int = 0) -> None:
+        self.docked_pose = np.asarray(docked_pose if docked_pose is not None else np.zeros(len(ALL_JOINTS), dtype=np.float32), dtype=np.float32).copy()
+        if self.docked_pose.shape != (len(ALL_JOINTS),):
+            raise ValueError("docked_pose must be a 16-joint array")
+        self._docked_legs = self.docked_pose[:len(LEG_JOINTS)].copy()
+        self.idle, self.scheduler, self.mixer = IdleEngine(seed=seed), BehaviorScheduler(seed=seed), LayeredMixer()
+        self.limiter = JointSafetyLimiter(dt=0.02)
+        self.previous_output = self.docked_pose.copy()
+        self.clips: dict[str, Any] = {}
+        self.rejected_clips: list[str] = []
+        if clips_dir is not None and Path(clips_dir).is_dir():
+            for clip in load_clip_dir(clips_dir).values():
+                self.register_clip(clip)
+
+    def register_clip(self, clip: Any) -> bool:
+        if any(joint in LEG_JOINTS for joint in clip.joints):
+            LOG.warning("Rejecting dock clip %s because it touches leg joints", clip.name)
+            self.rejected_clips.append(clip.name)
+            return False
+        self.clips[clip.name] = clip
+        tags = set(getattr(getattr(clip, "metadata", None), "tags", ()))
+        self.scheduler.register(clip.name, required_tags=tags)
+        return True
+
+    def set_mood(self, mood: str) -> None:
+        self.scheduler.set_mood(mood)
+
+    def trigger(self, clip_name: str) -> bool:
+        request = self.scheduler.trigger(clip_name)
+        if request is None or request[0] not in self.clips:
+            return False
+        self._play(*request)
+        return True
+
+    def _play(self, clip_name: str, speed: float) -> None:
+        player = AnimationPlayer(self.clips[clip_name], speed=speed)
+        self.mixer.add(player, name=clip_name)
+
+    def step(self, dt: float = 0.02) -> np.ndarray:
+        base = self.docked_pose.copy()
+        for joint, value in self.idle.update(dt).items():
+            base[JOINT_INDEX[joint]] = value
+        request = self.scheduler.update(dt)
+        if request is not None and request[0] in self.clips:
+            self._play(*request)
+        self.mixer.update(dt)
+        target = self.mixer.mix(base)
+        target[:len(LEG_JOINTS)] = self._docked_legs
+        limited = self.limiter.apply(target, self.previous_output)
+        # Safety limiter implementations may rate-limit all indices: mask after it too.
+        limited[:len(LEG_JOINTS)] = self._docked_legs
+        assert np.array_equal(limited[:len(LEG_JOINTS)], self._docked_legs)
+        self.previous_output = limited.copy()
+        return limited
+
+    def status(self) -> dict[str, Any]:
+        return {
+            "mode": "DEMO_DOCK",
+            "mood": self.scheduler.mood,
+            "active_clips": list(self.mixer.active_clips),
+            "registered_clips": list(self.clips),
+            "rejected_clips": list(self.rejected_clips),
+            "torque_policy": "legs may be held at low torque while docked",
+            "leg_mask_guarantee": "final output legs are byte-identical to docked_pose",
+        }

animation/demo/gaze.py

@@ -0,0 +1,56 @@
+"""Kinematically modest head gaze control."""
+
+from __future__ import annotations
+
+import math
+
+from duck_anim.joints import JOINT_LIMITS
+
+
+def _clamp(name: str, value: float) -> float:
+    low, high = JOINT_LIMITS[name]
+    return max(low, min(high, value))
+
+
+class GazeSolver:
+    def __init__(self, neck_ratio: float = 0.4, roll_velocity_gain: float = 0.08) -> None:
+        if not 0 <= neck_ratio <= 1:
+            raise ValueError("neck_ratio must be in [0, 1]")
+        self.neck_ratio = neck_ratio
+        self.roll_velocity_gain = roll_velocity_gain
+        self._previous_yaw = 0.0
+
+    def solve(self, azimuth: float, elevation: float) -> dict[str, float]:
+        yaw = _clamp("head_yaw", azimuth)
+        neck_pitch = _clamp("neck_pitch", elevation * self.neck_ratio)
+        head_pitch = _clamp("head_pitch", elevation * (1.0 - self.neck_ratio))
+        roll = _clamp("head_roll", -self.roll_velocity_gain * (yaw - self._previous_yaw))
+        self._previous_yaw = yaw
+        return {"neck_pitch": neck_pitch, "head_pitch": head_pitch, "head_yaw": yaw, "head_roll": roll}
+
+
+class GazeTracker:
+    """Critically damped target tracker with an explicit speed ceiling."""
+
+    def __init__(self, solver: GazeSolver | None = None, max_angular_velocity: float = 1.5, smoothing_time: float = 0.16) -> None:
+        self.solver = solver or GazeSolver()
+        self.max_angular_velocity, self.smoothing_time = max_angular_velocity, smoothing_time
+        self.azimuth = self.elevation = self.target_azimuth = self.target_elevation = 0.0
+        self._az_velocity = self._el_velocity = 0.0
+
+    def look_at(self, azimuth: float, elevation: float) -> None:
+        self.target_azimuth, self.target_elevation = azimuth, elevation
+
+    def _advance(self, current: float, target: float, velocity: float, dt: float) -> tuple[float, float]:
+        omega = 2.0 / max(self.smoothing_time, 1e-6)
+        acceleration = omega * omega * (target - current) - 2.0 * omega * velocity
+        velocity = max(-self.max_angular_velocity, min(self.max_angular_velocity, velocity + acceleration * dt))
+        delta = max(-self.max_angular_velocity * dt, min(self.max_angular_velocity * dt, velocity * dt))
+        return current + delta, velocity
+
+    def update(self, dt: float) -> dict[str, float]:
+        if dt <= 0:
+            raise ValueError("dt must be positive")
+        self.azimuth, self._az_velocity = self._advance(self.azimuth, self.target_azimuth, self._az_velocity, dt)
+        self.elevation, self._el_velocity = self._advance(self.elevation, self.target_elevation, self._el_velocity, dt)
+        return self.solver.solve(self.azimuth, self.elevation)

animation/demo/idle_engine.py

@@ -0,0 +1,60 @@
+"""Always-on dock-safe head and antenna motion."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+import random
+
+from duck_anim.joints import ANTENNA_JOINTS, HEAD_JOINTS
+from .gaze import GazeTracker
+from .noise import NoiseChannel, SmoothNoise
+
+
+@dataclass
+class IdleEngineConfig:
+    breath_amplitude: float = math.radians(2.0)
+    breath_frequency: float = 0.25
+    gaze_cone: float = math.radians(12.0)
+    gaze_hold_range: tuple[float, float] = (0.35, 1.2)
+    antenna_amplitude: float = math.radians(7.0)
+
+
+class IdleEngine:
+    def __init__(self, config: IdleEngineConfig | None = None, seed: int = 0) -> None:
+        self.config, self.rng, self.time = config or IdleEngineConfig(), random.Random(seed), 0.0
+        self.tracker = GazeTracker()
+        self.next_gaze = 0.0
+        self.next_flick = 0.0
+        self.flick = 0.0
+        self.noise = {
+            "head_yaw": NoiseChannel(SmoothNoise(seed + 1, 0.12), math.radians(3)),
+            "head_pitch": NoiseChannel(SmoothNoise(seed + 2, 0.18), math.radians(1.5)),
+            "head_roll": NoiseChannel(SmoothNoise(seed + 3, 0.2), math.radians(2)),
+            "left_antenna": NoiseChannel(SmoothNoise(seed + 4, 0.10), self.config.antenna_amplitude),
+            "right_antenna": NoiseChannel(SmoothNoise(seed + 5, 0.13), self.config.antenna_amplitude),
+        }
+
+    def update(self, dt: float) -> dict[str, float]:
+        if dt <= 0:
+            raise ValueError("dt must be positive")
+        self.time += dt
+        if self.time >= self.next_gaze:
+            self.tracker.look_at(self.rng.uniform(-self.config.gaze_cone, self.config.gaze_cone), self.rng.uniform(-self.config.gaze_cone, self.config.gaze_cone))
+            self.next_gaze = self.time + self.rng.uniform(*self.config.gaze_hold_range)
+        if self.time >= self.next_flick:
+            self.flick = self.rng.choice((-1.0, 1.0)) * math.radians(10)
+            self.next_flick = self.time + self.rng.uniform(2.0, 6.0)
+        self.flick *= math.exp(-dt * 12.0)
+        gaze = self.tracker.update(dt)
+        breath = self.config.breath_amplitude * math.sin(2 * math.pi * self.config.breath_frequency * self.time)
+        output = {
+            "neck_pitch": gaze["neck_pitch"] + breath * 0.45,
+            "head_pitch": gaze["head_pitch"] + breath * 0.55 + self.noise["head_pitch"].sample(self.time),
+            "head_yaw": gaze["head_yaw"] + self.noise["head_yaw"].sample(self.time),
+            "head_roll": gaze["head_roll"] + self.noise["head_roll"].sample(self.time),
+            "left_antenna": self.noise["left_antenna"].sample(self.time) + self.flick,
+            "right_antenna": self.noise["right_antenna"].sample(self.time) - self.flick * 0.65,
+        }
+        assert set(output).issubset(set(HEAD_JOINTS + ANTENNA_JOINTS))
+        return output

animation/demo/noise.py

@@ -0,0 +1,45 @@
+"""Smooth deterministic value noise for small, organic joint motion."""
+
+from __future__ import annotations
+
+import math
+
+
+class SmoothNoise:
+    """Seeded continuous one-dimensional value noise."""
+
+    def __init__(self, seed: int, frequency: float, octaves: int = 2, persistence: float = 0.5) -> None:
+        if frequency <= 0 or octaves < 1 or not 0 < persistence <= 1:
+            raise ValueError("frequency > 0, octaves >= 1, and persistence in (0, 1] are required")
+        self.seed, self.frequency, self.octaves, self.persistence = seed, frequency, octaves, persistence
+
+    def _lattice(self, index: int, octave: int) -> float:
+        # Integer hashing avoids mutable RNG state and makes sample order irrelevant.
+        value = (index * 0x9E3779B1 + self.seed * 0x85EBCA77 + octave * 0xC2B2AE3D) & 0xFFFFFFFF
+        value ^= value >> 16
+        value = (value * 0x7FEB352D) & 0xFFFFFFFF
+        value ^= value >> 15
+        return (value / 0x7FFFFFFF) - 1.0
+
+    def sample(self, t: float) -> float:
+        total = weight = 0.0
+        for octave in range(self.octaves):
+            x = t * self.frequency * (2**octave)
+            left = math.floor(x)
+            fraction = x - left
+            smooth = fraction * fraction * (3.0 - 2.0 * fraction)
+            value = self._lattice(left, octave) * (1 - smooth) + self._lattice(left + 1, octave) * smooth
+            amplitude = self.persistence**octave
+            total += value * amplitude
+            weight += amplitude
+        return total / weight
+
+
+class NoiseChannel:
+    """A noise source expressed directly as a joint-angle offset."""
+
+    def __init__(self, noise: SmoothNoise, amplitude: float, offset: float = 0.0) -> None:
+        self.noise, self.amplitude, self.offset = noise, amplitude, offset
+
+    def sample(self, t: float) -> float:
+        return self.offset + self.amplitude * self.noise.sample(t)

animation/demo/run_demo.py

@@ -0,0 +1,42 @@
+"""Run the dock animation demo at the robot's 50 Hz control rate."""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+import sys
+import time
+
+if __package__ is None:
+    sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from demo.dock import DockMode
+from duck_anim.joints import ANTENNA_JOINTS, HEAD_JOINTS, JOINT_INDEX
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--backend", choices=("print", "mujoco"), default="print")
+    parser.add_argument("--seed", type=int, default=0)
+    parser.add_argument("--mood", default="curious")
+    parser.add_argument("--duration", type=float, default=20.0)
+    parser.add_argument("--clips", default=str(Path(__file__).resolve().parents[1] / "clips"))
+    args = parser.parse_args()
+    if args.backend == "mujoco":
+        try:
+            import mujoco  # noqa: F401
+        except ImportError:
+            print("MuJoCo is not installed; falling back to print backend.", file=sys.stderr)
+    dock = DockMode(args.clips, seed=args.seed)
+    dock.set_mood(args.mood)
+    steps = max(1, round(args.duration * 50))
+    for step in range(steps):
+        target = dock.step(0.02)
+        if step % 10 == 0:
+            values = ", ".join(f"{name}={target[JOINT_INDEX[name]]:+.3f}" for name in HEAD_JOINTS + ANTENNA_JOINTS)
+            print(f"{step / 50:5.2f}s {values}")
+        time.sleep(0.02)
+
+
+if __name__ == "__main__":
+    main()