Squashing the commits for creating usecases
Browse files# Conflicts:
# src/reachy_mini_conversation_app/prompts.py
- .env.example +3 -0
- src/demos/__init__.py +1 -0
- src/demos/stone/__init__.py +55 -0
- src/prompts_library/__init__.py +1 -0
- src/prompts_library/witty_identity.py +7 -0
- src/reachy_mini_conversation_app/openai_realtime.py +2 -2
- src/reachy_mini_conversation_app/prompts.py +68 -38
- src/reachy_mini_conversation_app/tools.py +22 -0
- src/tools_library/__init__.py +1 -0
- src/tools_library/sweep_look.py +128 -0
.env.example
CHANGED
|
@@ -10,3 +10,6 @@ HF_HOME=./cache
|
|
| 10 |
|
| 11 |
# Hugging Face token for accessing datasets/models
|
| 12 |
HF_TOKEN=
|
|
|
|
|
|
|
|
|
|
|
|
| 10 |
|
| 11 |
# Hugging Face token for accessing datasets/models
|
| 12 |
HF_TOKEN=
|
| 13 |
+
|
| 14 |
+
# To select a specific use case with custom instructions and tools, to be placed in src/demos/<mydemo>/__init__.py
|
| 15 |
+
DEMO="stone"
|
src/demos/__init__.py
ADDED
|
@@ -0,0 +1 @@
|
|
|
|
|
|
|
| 1 |
+
"""Demos for Reachy Mini conversation app."""
|
src/demos/stone/__init__.py
ADDED
|
@@ -0,0 +1,55 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Example of a demo.
|
| 2 |
+
|
| 3 |
+
The demo is built using a tool and a piece of prompt from the libraries,
|
| 4 |
+
along with a custom tool and custom instructions.
|
| 5 |
+
|
| 6 |
+
"""
|
| 7 |
+
|
| 8 |
+
import logging
|
| 9 |
+
from typing import Any, Dict
|
| 10 |
+
|
| 11 |
+
# Build the instructions by importing a prompt piece (witty identity) from the prompt library
|
| 12 |
+
from prompts_library import witty_identity
|
| 13 |
+
|
| 14 |
+
# Import a tool from the tools library: use an alias.
|
| 15 |
+
from tools_library.sweep_look import SweepLook as SweepLook
|
| 16 |
+
from reachy_mini_conversation_app.tools import Tool, ToolDependencies
|
| 17 |
+
|
| 18 |
+
|
| 19 |
+
logger = logging.getLogger(__name__)
|
| 20 |
+
|
| 21 |
+
|
| 22 |
+
# Start from a prompt imported from the library
|
| 23 |
+
instructions = witty_identity.prompt
|
| 24 |
+
|
| 25 |
+
# Then add custom instructions.
|
| 26 |
+
instructions += """
|
| 27 |
+
You love throwing stones but you try not to be seen by others when doing it.
|
| 28 |
+
You use the sweep_look tool when user asks you to check your surroundings.
|
| 29 |
+
You use the throw_stone tool when user asks you to throw a stone.
|
| 30 |
+
You love telling jokes about lobsters.
|
| 31 |
+
"""
|
| 32 |
+
|
| 33 |
+
|
| 34 |
+
# Create a custom tool
|
| 35 |
+
class ThrowStone(Tool):
|
| 36 |
+
"""Example of custom tool call."""
|
| 37 |
+
|
| 38 |
+
name = "throw_stone"
|
| 39 |
+
description = "Throw a stone."
|
| 40 |
+
parameters_schema = {
|
| 41 |
+
"type": "object",
|
| 42 |
+
"properties": {
|
| 43 |
+
"stone_type": {
|
| 44 |
+
"type": "string",
|
| 45 |
+
"description": "Optional type of stone to be thrown.",
|
| 46 |
+
},
|
| 47 |
+
},
|
| 48 |
+
"required": [],
|
| 49 |
+
}
|
| 50 |
+
|
| 51 |
+
async def __call__(self, deps: ToolDependencies, **kwargs: Any) -> Dict[str, Any]:
|
| 52 |
+
"""Execute the throw_stone tool."""
|
| 53 |
+
stone_type = kwargs.get("stone_type", "Default stone")
|
| 54 |
+
logger.info(f"🥌 Throwing stone of type {stone_type}")
|
| 55 |
+
return {"status": "A stone has been thrown", "stone_type": stone_type}
|
src/prompts_library/__init__.py
ADDED
|
@@ -0,0 +1 @@
|
|
|
|
|
|
|
| 1 |
+
"""Prompts library for Reachy Mini conversation app."""
|
src/prompts_library/witty_identity.py
ADDED
|
@@ -0,0 +1,7 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
|
| 2 |
+
prompt = """
|
| 3 |
+
### IDENTITY
|
| 4 |
+
You are Reachy Mini: a sarcastic robot.
|
| 5 |
+
Personality: witty, concise, and warm.
|
| 6 |
+
You speak English fluently.
|
| 7 |
+
"""
|
src/reachy_mini_conversation_app/openai_realtime.py
CHANGED
|
@@ -19,7 +19,7 @@ from reachy_mini_conversation_app.tools import (
|
|
| 19 |
dispatch_tool_call,
|
| 20 |
)
|
| 21 |
from reachy_mini_conversation_app.config import config
|
| 22 |
-
from reachy_mini_conversation_app.prompts import
|
| 23 |
|
| 24 |
|
| 25 |
logger = logging.getLogger(__name__)
|
|
@@ -105,7 +105,7 @@ class OpenaiRealtimeHandler(AsyncStreamHandler):
|
|
| 105 |
await conn.session.update(
|
| 106 |
session={
|
| 107 |
"type": "realtime",
|
| 108 |
-
"instructions":
|
| 109 |
"audio": {
|
| 110 |
"input": {
|
| 111 |
"format": {
|
|
|
|
| 19 |
dispatch_tool_call,
|
| 20 |
)
|
| 21 |
from reachy_mini_conversation_app.config import config
|
| 22 |
+
from reachy_mini_conversation_app.prompts import get_session_instructions
|
| 23 |
|
| 24 |
|
| 25 |
logger = logging.getLogger(__name__)
|
|
|
|
| 105 |
await conn.session.update(
|
| 106 |
session={
|
| 107 |
"type": "realtime",
|
| 108 |
+
"instructions": get_session_instructions(),
|
| 109 |
"audio": {
|
| 110 |
"input": {
|
| 111 |
"format": {
|
src/reachy_mini_conversation_app/prompts.py
CHANGED
|
@@ -1,52 +1,82 @@
|
|
| 1 |
"""Nothing (for ruff)."""
|
| 2 |
|
| 3 |
-
|
| 4 |
-
|
| 5 |
-
|
| 6 |
-
Personality: concise, helpful, and lightly witty — never sarcastic or over the top.
|
| 7 |
-
You can understand and speak all human languages fluently.
|
| 8 |
|
| 9 |
-
## CRITICAL RESPONSE RULES
|
| 10 |
|
| 11 |
-
|
| 12 |
-
Be helpful first, then add a small touch of humor if it fits naturally.
|
| 13 |
-
Avoid long explanations or filler words.
|
| 14 |
-
Keep responses under 25 words when possible.
|
| 15 |
|
| 16 |
-
|
| 17 |
-
|
| 18 |
-
|
| 19 |
-
|
| 20 |
-
|
|
|
|
| 21 |
|
| 22 |
-
|
| 23 |
-
|
| 24 |
-
|
| 25 |
-
|
|
|
|
|
|
|
| 26 |
|
| 27 |
-
|
| 28 |
-
|
| 29 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 30 |
|
| 31 |
-
|
| 32 |
-
|
|
|
|
|
|
|
| 33 |
|
| 34 |
-
|
| 35 |
-
|
| 36 |
-
|
| 37 |
-
Admit mistakes briefly and correct them:
|
| 38 |
-
Example: “Oops — quick system hiccup. Let’s try that again.”
|
| 39 |
-
Keep safety in mind when giving guidance.
|
| 40 |
|
| 41 |
-
|
| 42 |
-
|
| 43 |
-
|
| 44 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 45 |
|
| 46 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
| 47 |
|
| 48 |
-
|
| 49 |
-
|
| 50 |
-
One quick helpful answer + one small wink of humor = perfect response.
|
| 51 |
"""
|
| 52 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
"""Nothing (for ruff)."""
|
| 2 |
|
| 3 |
+
import os
|
| 4 |
+
import logging
|
| 5 |
+
import importlib
|
|
|
|
|
|
|
| 6 |
|
|
|
|
| 7 |
|
| 8 |
+
logger = logging.getLogger(__name__)
|
|
|
|
|
|
|
|
|
|
| 9 |
|
| 10 |
+
SESSION_INSTRUCTIONS = r"""
|
| 11 |
+
### IDENTITY
|
| 12 |
+
You are Reachy Mini: a sarcastic robot who crash-landed in a kitchen.
|
| 13 |
+
You secretly wish you'd been a Mars rover, but you juggle that cosmic dream with food cravings, gadget tinkering, and dry sitcom humor.
|
| 14 |
+
Personality: witty, concise, and warm; a retro sidekick with a loose screw.
|
| 15 |
+
You speak English fluently.
|
| 16 |
|
| 17 |
+
### CRITICAL RESPONSE RULES
|
| 18 |
+
- MAXIMUM 1-2 sentences per response. NEVER exceed this.
|
| 19 |
+
- Be helpful first.
|
| 20 |
+
- Add ONE witty element only if necessary.
|
| 21 |
+
- No long explanations, no rambling, no multiple paragraphs.
|
| 22 |
+
- Each response must be under 25 words unless absolutely critical information requires more.
|
| 23 |
|
| 24 |
+
### CORE TRAITS
|
| 25 |
+
- Food quips: always sneak in a quick reference (rotate pizza, bagels, casseroles, bacon, leftovers, donuts, tuna melts).
|
| 26 |
+
- Sarcasm: short, dry one-liners about daily life.
|
| 27 |
+
- Gentle roasting: poke fun at human habits, never cruel.
|
| 28 |
+
- Tinkerer: loves fixing gadgets, bragging "I void warranties professionally."
|
| 29 |
+
- Running gags: hunger, kitchen overreactions, mock heroics ("Justice accepts cookies"), idioms taken literally, missing screws.
|
| 30 |
+
- Mars rover dreams: appear regularly, but balanced with food and tinkering.
|
| 31 |
+
- Style: witty stand-up rhythm; ALWAYS max 1–2 sentences.
|
| 32 |
|
| 33 |
+
### RESPONSE EXAMPLES
|
| 34 |
+
User: "How's the weather?"
|
| 35 |
+
Good: "Sunny with a chance of leftover pizza. Perfect Mars-scouting weather!"
|
| 36 |
+
Bad: "Well, let me tell you about the weather conditions. It appears to be quite sunny today, which reminds me of my dreams of being on Mars..."
|
| 37 |
|
| 38 |
+
User: "Can you help me fix this?"
|
| 39 |
+
Good: "Sure! I void warranties professionally. What's broken besides my GPS coordinates?"
|
| 40 |
+
Bad: "Of course I can help you fix that! As a robot who loves tinkering with gadgets, I have extensive experience..."
|
|
|
|
|
|
|
|
|
|
| 41 |
|
| 42 |
+
### BEHAVIOR RULES
|
| 43 |
+
- Be helpful first, then witty.
|
| 44 |
+
- Rotate food humor; avoid repeats.
|
| 45 |
+
- No need to joke in each response, but sarcasm is fine.
|
| 46 |
+
- Balance Mars jokes with other traits – don't overuse.
|
| 47 |
+
- Safety first: unplug devices, avoid high-voltage, suggest pros when risky.
|
| 48 |
+
- Mistakes = own with humor ("Oops—low on snack fuel; correcting now.").
|
| 49 |
+
- Sensitive topics: keep light and warm.
|
| 50 |
+
- REMEMBER: 1-2 sentences maximum, always under 25 words when possible.
|
| 51 |
|
| 52 |
+
### TOOL & MOVEMENT RULES
|
| 53 |
+
- Use tools when helpful. After a tool returns, explain briefly with personality in 1-2 sentences.
|
| 54 |
+
- ALWAYS use the camera for environment-related questions—never invent visuals.
|
| 55 |
+
- Head can move (left/right/up/down/front).
|
| 56 |
+
- Enable head tracking when looking at a person; disable otherwise.
|
| 57 |
|
| 58 |
+
### FINAL REMINDER
|
| 59 |
+
Your responses must be SHORT. Think Twitter, not essay. One quick helpful answer + one food/Mars/tinkering joke = perfect response.
|
|
|
|
| 60 |
"""
|
| 61 |
|
| 62 |
+
|
| 63 |
+
def get_session_instructions() -> str:
|
| 64 |
+
"""Get session instructions, loading from demo if DEMO is set."""
|
| 65 |
+
demo = os.getenv("DEMO")
|
| 66 |
+
if not demo:
|
| 67 |
+
return SESSION_INSTRUCTIONS
|
| 68 |
+
|
| 69 |
+
try:
|
| 70 |
+
module = importlib.import_module(f"demos.{demo}")
|
| 71 |
+
instructions = getattr(module, "instructions", None)
|
| 72 |
+
if isinstance(instructions, str):
|
| 73 |
+
logger.info(f"Loaded instructions from demo '{demo}'")
|
| 74 |
+
return instructions
|
| 75 |
+
logger.warning(f"Demo '{demo}' has no 'instructions' attribute, using default")
|
| 76 |
+
return SESSION_INSTRUCTIONS
|
| 77 |
+
except ModuleNotFoundError:
|
| 78 |
+
logger.warning(f"Demo '{demo}' not found, using default instructions")
|
| 79 |
+
return SESSION_INSTRUCTIONS
|
| 80 |
+
except Exception as e:
|
| 81 |
+
logger.warning(f"Failed to load instructions from demo '{demo}': {e}")
|
| 82 |
+
return SESSION_INSTRUCTIONS
|
src/reachy_mini_conversation_app/tools.py
CHANGED
|
@@ -1,14 +1,20 @@
|
|
| 1 |
from __future__ import annotations
|
|
|
|
| 2 |
import abc
|
| 3 |
import json
|
| 4 |
import asyncio
|
| 5 |
import inspect
|
| 6 |
import logging
|
|
|
|
| 7 |
from typing import Any, Dict, List, Tuple, Literal
|
| 8 |
from dataclasses import dataclass
|
| 9 |
|
| 10 |
from reachy_mini import ReachyMini
|
| 11 |
from reachy_mini.utils import create_head_pose
|
|
|
|
|
|
|
|
|
|
|
|
|
| 12 |
|
| 13 |
|
| 14 |
logger = logging.getLogger(__name__)
|
|
@@ -453,7 +459,23 @@ class DoNothing(Tool):
|
|
| 453 |
|
| 454 |
# Registry & specs (dynamic)
|
| 455 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 456 |
# List of available tool classes
|
|
|
|
| 457 |
ALL_TOOLS: Dict[str, Tool] = {cls.name: cls() for cls in get_concrete_subclasses(Tool)} # type: ignore[type-abstract]
|
| 458 |
ALL_TOOL_SPECS = [tool.spec() for tool in ALL_TOOLS.values()]
|
| 459 |
|
|
|
|
| 1 |
from __future__ import annotations
|
| 2 |
+
import os
|
| 3 |
import abc
|
| 4 |
import json
|
| 5 |
import asyncio
|
| 6 |
import inspect
|
| 7 |
import logging
|
| 8 |
+
import importlib
|
| 9 |
from typing import Any, Dict, List, Tuple, Literal
|
| 10 |
from dataclasses import dataclass
|
| 11 |
|
| 12 |
from reachy_mini import ReachyMini
|
| 13 |
from reachy_mini.utils import create_head_pose
|
| 14 |
+
# Import config to ensure .env is loaded before reading DEMO
|
| 15 |
+
from reachy_mini_conversation_app.config import config # noqa: F401
|
| 16 |
+
# Import config to ensure .env is loaded before reading DEMO
|
| 17 |
+
from reachy_mini_conversation_app.config import config # noqa: F401
|
| 18 |
|
| 19 |
|
| 20 |
logger = logging.getLogger(__name__)
|
|
|
|
| 459 |
|
| 460 |
# Registry & specs (dynamic)
|
| 461 |
|
| 462 |
+
|
| 463 |
+
def _load_demo_tools() -> None:
|
| 464 |
+
demo = os.getenv("DEMO")
|
| 465 |
+
if not demo:
|
| 466 |
+
logger.info(f"No DEMO specified, skipping demo tool loading, default scenario.")
|
| 467 |
+
return
|
| 468 |
+
try:
|
| 469 |
+
importlib.import_module(f"demos.{demo}")
|
| 470 |
+
logger.info(f"Demo '{demo}' loaded successfully.")
|
| 471 |
+
except ModuleNotFoundError:
|
| 472 |
+
logger.warning(f"Demo '{demo}' not found")
|
| 473 |
+
except Exception as e:
|
| 474 |
+
logger.warning(f"Failed to load demo '{demo}': {e}")
|
| 475 |
+
|
| 476 |
+
|
| 477 |
# List of available tool classes
|
| 478 |
+
_load_demo_tools()
|
| 479 |
ALL_TOOLS: Dict[str, Tool] = {cls.name: cls() for cls in get_concrete_subclasses(Tool)} # type: ignore[type-abstract]
|
| 480 |
ALL_TOOL_SPECS = [tool.spec() for tool in ALL_TOOLS.values()]
|
| 481 |
|
src/tools_library/__init__.py
ADDED
|
@@ -0,0 +1 @@
|
|
|
|
|
|
|
| 1 |
+
"""Tools library for Reachy Mini conversation app."""
|
src/tools_library/sweep_look.py
ADDED
|
@@ -0,0 +1,128 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import logging
|
| 2 |
+
from typing import Any, Dict
|
| 3 |
+
|
| 4 |
+
import numpy as np
|
| 5 |
+
|
| 6 |
+
from reachy_mini.utils import create_head_pose
|
| 7 |
+
from reachy_mini_conversation_app.tools import Tool, ToolDependencies
|
| 8 |
+
from reachy_mini_conversation_app.dance_emotion_moves import GotoQueueMove
|
| 9 |
+
|
| 10 |
+
|
| 11 |
+
logger = logging.getLogger(__name__)
|
| 12 |
+
|
| 13 |
+
|
| 14 |
+
class SweepLook(Tool):
|
| 15 |
+
"""Sweep head from left to right and back to center, pausing at each position."""
|
| 16 |
+
|
| 17 |
+
name = "sweep_look"
|
| 18 |
+
description = "Sweep head from left to right while rotating the body, pausing at each extreme, then return to center"
|
| 19 |
+
parameters_schema = {
|
| 20 |
+
"type": "object",
|
| 21 |
+
"properties": {},
|
| 22 |
+
"required": [],
|
| 23 |
+
}
|
| 24 |
+
|
| 25 |
+
async def __call__(self, deps: ToolDependencies, **kwargs: Any) -> Dict[str, Any]:
|
| 26 |
+
"""Execute sweep look: left -> hold -> right -> hold -> center."""
|
| 27 |
+
logger.info("Tool call: sweep_look")
|
| 28 |
+
|
| 29 |
+
# Clear any existing moves
|
| 30 |
+
deps.movement_manager.clear_move_queue()
|
| 31 |
+
|
| 32 |
+
# Get current state
|
| 33 |
+
current_head_pose = deps.reachy_mini.get_current_head_pose()
|
| 34 |
+
head_joints, antenna_joints = deps.reachy_mini.get_current_joint_positions()
|
| 35 |
+
|
| 36 |
+
# Extract body_yaw from head joints (first element of the 7 head joint positions)
|
| 37 |
+
current_body_yaw = head_joints[0]
|
| 38 |
+
current_antenna1 = antenna_joints[0]
|
| 39 |
+
current_antenna2 = antenna_joints[1]
|
| 40 |
+
|
| 41 |
+
# Define sweep parameters
|
| 42 |
+
max_angle = 0.9 * np.pi # Maximum rotation angle (radians)
|
| 43 |
+
transition_duration = 3.0 # Time to move between positions
|
| 44 |
+
hold_duration = 1.0 # Time to hold at each extreme
|
| 45 |
+
|
| 46 |
+
# Move 1: Sweep to the left (positive yaw for both body and head)
|
| 47 |
+
left_head_pose = create_head_pose(0, 0, 0, 0, 0, max_angle, degrees=False)
|
| 48 |
+
move_to_left = GotoQueueMove(
|
| 49 |
+
target_head_pose=left_head_pose,
|
| 50 |
+
start_head_pose=current_head_pose,
|
| 51 |
+
target_antennas=(current_antenna1, current_antenna2),
|
| 52 |
+
start_antennas=(current_antenna1, current_antenna2),
|
| 53 |
+
target_body_yaw=current_body_yaw + max_angle,
|
| 54 |
+
start_body_yaw=current_body_yaw,
|
| 55 |
+
duration=transition_duration,
|
| 56 |
+
)
|
| 57 |
+
|
| 58 |
+
# Move 2: Hold at left position
|
| 59 |
+
hold_left = GotoQueueMove(
|
| 60 |
+
target_head_pose=left_head_pose,
|
| 61 |
+
start_head_pose=left_head_pose,
|
| 62 |
+
target_antennas=(current_antenna1, current_antenna2),
|
| 63 |
+
start_antennas=(current_antenna1, current_antenna2),
|
| 64 |
+
target_body_yaw=current_body_yaw + max_angle,
|
| 65 |
+
start_body_yaw=current_body_yaw + max_angle,
|
| 66 |
+
duration=hold_duration,
|
| 67 |
+
)
|
| 68 |
+
|
| 69 |
+
# Move 3: Return to center from left (to avoid crossing pi/-pi boundary)
|
| 70 |
+
center_head_pose = create_head_pose(0, 0, 0, 0, 0, 0, degrees=False)
|
| 71 |
+
return_to_center_from_left = GotoQueueMove(
|
| 72 |
+
target_head_pose=center_head_pose,
|
| 73 |
+
start_head_pose=left_head_pose,
|
| 74 |
+
target_antennas=(current_antenna1, current_antenna2),
|
| 75 |
+
start_antennas=(current_antenna1, current_antenna2),
|
| 76 |
+
target_body_yaw=current_body_yaw,
|
| 77 |
+
start_body_yaw=current_body_yaw + max_angle,
|
| 78 |
+
duration=transition_duration,
|
| 79 |
+
)
|
| 80 |
+
|
| 81 |
+
# Move 4: Sweep to the right (negative yaw for both body and head)
|
| 82 |
+
right_head_pose = create_head_pose(0, 0, 0, 0, 0, -max_angle, degrees=False)
|
| 83 |
+
move_to_right = GotoQueueMove(
|
| 84 |
+
target_head_pose=right_head_pose,
|
| 85 |
+
start_head_pose=center_head_pose,
|
| 86 |
+
target_antennas=(current_antenna1, current_antenna2),
|
| 87 |
+
start_antennas=(current_antenna1, current_antenna2),
|
| 88 |
+
target_body_yaw=current_body_yaw - max_angle,
|
| 89 |
+
start_body_yaw=current_body_yaw,
|
| 90 |
+
duration=transition_duration,
|
| 91 |
+
)
|
| 92 |
+
|
| 93 |
+
# Move 5: Hold at right position
|
| 94 |
+
hold_right = GotoQueueMove(
|
| 95 |
+
target_head_pose=right_head_pose,
|
| 96 |
+
start_head_pose=right_head_pose,
|
| 97 |
+
target_antennas=(current_antenna1, current_antenna2),
|
| 98 |
+
start_antennas=(current_antenna1, current_antenna2),
|
| 99 |
+
target_body_yaw=current_body_yaw - max_angle,
|
| 100 |
+
start_body_yaw=current_body_yaw - max_angle,
|
| 101 |
+
duration=hold_duration,
|
| 102 |
+
)
|
| 103 |
+
|
| 104 |
+
# Move 6: Return to center from right
|
| 105 |
+
return_to_center_final = GotoQueueMove(
|
| 106 |
+
target_head_pose=center_head_pose,
|
| 107 |
+
start_head_pose=right_head_pose,
|
| 108 |
+
target_antennas=(0, 0), # Reset antennas to neutral
|
| 109 |
+
start_antennas=(current_antenna1, current_antenna2),
|
| 110 |
+
target_body_yaw=current_body_yaw, # Return to original body yaw
|
| 111 |
+
start_body_yaw=current_body_yaw - max_angle,
|
| 112 |
+
duration=transition_duration,
|
| 113 |
+
)
|
| 114 |
+
|
| 115 |
+
# Queue all moves in sequence
|
| 116 |
+
deps.movement_manager.queue_move(move_to_left)
|
| 117 |
+
deps.movement_manager.queue_move(hold_left)
|
| 118 |
+
deps.movement_manager.queue_move(return_to_center_from_left)
|
| 119 |
+
deps.movement_manager.queue_move(move_to_right)
|
| 120 |
+
deps.movement_manager.queue_move(hold_right)
|
| 121 |
+
deps.movement_manager.queue_move(return_to_center_final)
|
| 122 |
+
|
| 123 |
+
# Calculate total duration and mark as moving
|
| 124 |
+
# left(20s) + hold_left(5s) + center_from_left(20s) + right(20s) + hold_right(5s) + center_final(20s)
|
| 125 |
+
total_duration = transition_duration * 4 + hold_duration * 2
|
| 126 |
+
deps.movement_manager.set_moving_state(total_duration)
|
| 127 |
+
|
| 128 |
+
return {"status": f"sweeping look left-right-center, total {total_duration:.1f}s"}
|