Spaces:
Build error
Build error
File size: 2,903 Bytes
b82aa95 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 | import logging
from typing import Any, Dict, Tuple, Literal
from reachy_mini.utils import create_head_pose
from reachy_mini_conversation_app.tools.core_tools import Tool, ToolDependencies
from reachy_mini_conversation_app.dance_emotion_moves import GotoQueueMove
logger = logging.getLogger(__name__)
Direction = Literal["left", "right", "up", "down", "front"]
class MoveHead(Tool):
"""Move head in a given direction."""
name = "move_head"
description = "Move your head in a given direction: left, right, up, down or front."
parameters_schema = {
"type": "object",
"properties": {
"direction": {
"type": "string",
"enum": ["left", "right", "up", "down", "front"],
},
},
"required": ["direction"],
}
# mapping: direction -> args for create_head_pose
DELTAS: Dict[str, Tuple[int, int, int, int, int, int]] = {
"left": (0, 0, 0, 0, 0, 40),
"right": (0, 0, 0, 0, 0, -40),
"up": (0, 0, 0, 0, -30, 0),
"down": (0, 0, 0, 0, 30, 0),
"front": (0, 0, 0, 0, 0, 0),
}
async def __call__(self, deps: ToolDependencies, **kwargs: Any) -> Dict[str, Any]:
"""Move head in a given direction."""
direction_raw = kwargs.get("direction")
if not isinstance(direction_raw, str):
return {"error": "direction must be a string"}
direction: Direction = direction_raw # type: ignore[assignment]
logger.info("Tool call: move_head direction=%s", direction)
deltas = self.DELTAS.get(direction, self.DELTAS["front"])
target = create_head_pose(*deltas, degrees=True)
# Use new movement manager
try:
movement_manager = deps.movement_manager
# Get current state for interpolation
current_head_pose = deps.reachy_mini.get_current_head_pose()
_, current_antennas = deps.reachy_mini.get_current_joint_positions()
# Create goto move
goto_move = GotoQueueMove(
target_head_pose=target,
start_head_pose=current_head_pose,
target_antennas=(0, 0), # Reset antennas to default
start_antennas=(
current_antennas[0],
current_antennas[1],
), # Skip body_yaw
target_body_yaw=0, # Reset body yaw
start_body_yaw=current_antennas[0], # body_yaw is first in joint positions
duration=deps.motion_duration_s,
)
movement_manager.queue_move(goto_move)
movement_manager.set_moving_state(deps.motion_duration_s)
return {"status": f"looking {direction}"}
except Exception as e:
logger.error("move_head failed")
return {"error": f"move_head failed: {type(e).__name__}: {e}"}
|