| """Robot motion — DoA tracking + expressive actions.""" |
|
|
| import asyncio |
| import math |
| import time |
| import logging |
| import re |
|
|
| import numpy as np |
|
|
| def create_head_pose(roll=0, pitch=0, yaw=0, degrees=False): |
| if degrees: |
| roll, pitch, yaw = math.radians(roll), math.radians(pitch), math.radians(yaw) |
| cr, sr = math.cos(roll), math.sin(roll) |
| cp, sp = math.cos(pitch), math.sin(pitch) |
| cy, sy = math.cos(yaw), math.sin(yaw) |
| R = np.array([ |
| [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr, 0], |
| [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr, 0], |
| [-sp, cp*sr, cp*cr, 0], |
| [0, 0, 0, 1], |
| ], dtype=np.float64) |
| return R |
|
|
| from .config import DOA_SMOOTHING, CONTROL_LOOP_HZ |
|
|
| logger = logging.getLogger(__name__) |
|
|
| ACTION_RE = re.compile(r"\[ACTION:(\w+)\]", re.IGNORECASE) |
| SALE_RE = re.compile(r"\[SALE:([^\]]+)\]", re.IGNORECASE) |
|
|
|
|
| def parse_response(text: str) -> tuple[str, list[str], list[str]]: |
| actions = [m.group(1).lower() for m in ACTION_RE.finditer(text)] |
| sales = [m.group(1).strip() for m in SALE_RE.finditer(text)] |
| clean = ACTION_RE.sub("", text) |
| clean = SALE_RE.sub("", clean).strip() |
| return clean, actions, sales |
|
|
|
|
| class MotionController: |
| def __init__(self, mini): |
| self.mini = mini |
| self._running = False |
| self._doa_angle: float | None = None |
| self._speech_detected = False |
| self._is_speaking = False |
| self._action_queue: asyncio.Queue[str] = asyncio.Queue() |
|
|
| async def start(self): |
| self._running = True |
|
|
| async def stop(self): |
| self._running = False |
|
|
| def set_speaking(self, val: bool): |
| self._is_speaking = val |
|
|
| def trigger(self, action: str): |
| try: |
| self._action_queue.put_nowait(action) |
| except asyncio.QueueFull: |
| pass |
|
|
| async def doa_tracking_loop(self): |
| loop = asyncio.get_event_loop() |
| while self._running: |
| try: |
| result = await loop.run_in_executor(None, self.mini.media.get_DoA) |
| if result: |
| angle, speech = result |
| self._speech_detected = speech |
| if self._doa_angle is None: |
| self._doa_angle = angle |
| else: |
| self._doa_angle = DOA_SMOOTHING * angle + (1 - DOA_SMOOTHING) * self._doa_angle |
| except Exception: |
| pass |
| await asyncio.sleep(0.05) |
|
|
| async def motion_loop(self): |
| while self._running: |
| t = time.monotonic() |
|
|
| head_yaw = 0.0 |
| if self._doa_angle is not None and self._speech_detected: |
| head_yaw = max(min((math.pi / 2) - self._doa_angle, 0.7), -0.7) |
|
|
| ant_l, ant_r = 0.0, 0.0 |
| if self._is_speaking: |
| amp = math.radians(25) |
| ant_l = amp * math.sin(2 * math.pi * 1.5 * t) |
| ant_r = amp * math.sin(2 * math.pi * 1.5 * t + math.pi / 3) |
|
|
| head = create_head_pose(roll=0, pitch=0, yaw=head_yaw, degrees=False) |
| self.mini.set_target(head=head, antennas=[ant_r, ant_l]) |
| await asyncio.sleep(1.0 / CONTROL_LOOP_HZ) |
|
|
| async def action_loop(self): |
| while self._running: |
| try: |
| action = await asyncio.wait_for(self._action_queue.get(), timeout=0.5) |
| await self._execute(action) |
| except asyncio.TimeoutError: |
| continue |
|
|
| async def _execute(self, action: str): |
| m = self.mini |
| if action == "wave": |
| for _ in range(3): |
| m.goto_target(antennas=[0, math.radians(-40)], duration=0.2) |
| await asyncio.sleep(0.25) |
| m.goto_target(antennas=[math.radians(-40), 0], duration=0.2) |
| await asyncio.sleep(0.25) |
| m.goto_target(antennas=[0, 0], duration=0.3) |
|
|
| elif action == "nod": |
| for angle in [-15, 10, -10, 0]: |
| head = create_head_pose(pitch=math.radians(angle), degrees=False) |
| m.goto_target(head=head, duration=0.2) |
| await asyncio.sleep(0.25) |
|
|
| elif action == "shake": |
| for yaw in [-20, 20, -15, 0]: |
| head = create_head_pose(yaw=math.radians(yaw), degrees=False) |
| m.goto_target(head=head, duration=0.2) |
| await asyncio.sleep(0.2) |
|
|
| elif action == "happy": |
| for _ in range(3): |
| m.goto_target(antennas=[math.radians(-30), math.radians(-30)], duration=0.15) |
| await asyncio.sleep(0.2) |
| m.goto_target(antennas=[math.radians(10), math.radians(10)], duration=0.15) |
| await asyncio.sleep(0.2) |
| m.goto_target(antennas=[0, 0], duration=0.3) |
|
|
| elif action == "dance": |
| for _ in range(4): |
| m.goto_target(body_yaw=math.radians(20), antennas=[math.radians(-30), math.radians(10)], duration=0.25) |
| await asyncio.sleep(0.3) |
| m.goto_target(body_yaw=math.radians(-20), antennas=[math.radians(10), math.radians(-30)], duration=0.25) |
| await asyncio.sleep(0.3) |
| m.goto_target(body_yaw=0, antennas=[0, 0], duration=0.3) |
|
|
| elif action == "think": |
| head = create_head_pose(roll=math.radians(5), pitch=math.radians(-10), yaw=math.radians(15), degrees=False) |
| m.goto_target(head=head, antennas=[math.radians(-20), math.radians(-20)], duration=0.4) |
| await asyncio.sleep(1.5) |
| m.goto_target(head=create_head_pose(degrees=False), antennas=[0, 0], duration=0.4) |
|
|
| elif action == "greet": |
| head = create_head_pose(pitch=math.radians(-15), degrees=False) |
| m.goto_target(head=head, antennas=[math.radians(-20), math.radians(-20)], duration=0.3) |
| await asyncio.sleep(0.5) |
| m.goto_target(head=create_head_pose(degrees=False), antennas=[0, 0], duration=0.3) |
|
|
