Tune littlehand behaviors and DSPy signature

dora_voice_control/dora_voice_control/robots/littlehand/behavior.py

@@ -2,16 +2,20 @@
 
 from __future__ import annotations
 
-from typing import Callable
+import os
+from typing import Callable, Optional
 
-from ...core.behavior import ActionContext, RobotBehavior
+from ...core.behavior import ActionContext, RobotBehavior, _log
 from .actions import LITTLEHAND_ACTIONS
+from .signature import LittlehandSignature
 
+_XY_MATCH_RADIUS_MM = float(os.getenv("BAJAR_XY_RADIUS_MM", "40.0"))
 
 class LittlehandBehavior(RobotBehavior):
     """Littlehand behavior using the default pick-and-place actions."""
 
     ACTIONS = LITTLEHAND_ACTIONS
+    CommandSignature = LittlehandSignature
 
     def action_handlers(self) -> dict[str, Callable[[ActionContext], bool]]:
         return {
@@ -24,13 +28,25 @@ class LittlehandBehavior(RobotBehavior):
         }
 
     def action_subir(self, ctx: ActionContext) -> bool:
-        """Move up by step_mm."""
+        """Move to the home height (initial Z)."""
-        target_z = ctx.pose[2] + self.config.step_mm
+        if ctx.pose is None:
+            return False
+        target_z = ctx.home_pose.get("z", ctx.pose[2])
         return self._queue_move(ctx, ctx.pose[0], ctx.pose[1], target_z)
 
     def action_bajar(self, ctx: ActionContext) -> bool:
-        """Move down by step_mm."""
+        """Move down by step_mm or to top of object under the tool."""
+        target = self._find_object_under_pose(ctx)
+        if target is not None:
+            target_z = target.position_mm[2] + ctx.config.tcp_offset_mm
+            _log(
+                f"bajar: using object '{target.object_type}' color={target.color} "
+                f"obj_z={target.position_mm[2]:.1f} tcp_offset={ctx.config.tcp_offset_mm:.1f} "
+                f"target_z={target_z:.1f} at pose_z={ctx.pose[2]:.1f}"
+            )
+            return self._queue_move(ctx, ctx.pose[0], ctx.pose[1], target_z)
         target_z = ctx.pose[2] - self.config.step_mm
+        _log(f"bajar: no object under pose, step to z={target_z:.1f}")
         return self._queue_move(ctx, ctx.pose[0], ctx.pose[1], target_z)
 
     def action_ir(self, ctx: ActionContext) -> bool:
@@ -56,3 +72,41 @@ class LittlehandBehavior(RobotBehavior):
         self._queue_steps(ctx, self.robot_adapter.move(ctx.home_pose))
         ctx.scene.clear_detected()
         return True
+
+    def _find_object_under_pose(self, ctx: ActionContext) -> Optional["SceneObject"]:
+        """Find the topmost object near the current pose x,y (mm)."""
+        if ctx.pose is None:
+            _log("bajar: missing pose, cannot find object under tool")
+            return None
+        px, py, pz = ctx.pose[0], ctx.pose[1], ctx.pose[2]
+        _log(
+            "bajar: current pose x={:.1f} y={:.1f} z={:.1f}".format(px, py, pz)
+        )
+        candidates = []
+        for obj in ctx.scene.query():
+            dx = px - obj.position_mm[0]
+            dy = py - obj.position_mm[1]
+            dist2 = dx * dx + dy * dy
+            if dist2 > _XY_MATCH_RADIUS_MM * _XY_MATCH_RADIUS_MM:
+                continue
+            top_surface = obj.position_mm[2] + obj.height_mm
+            candidates.append((top_surface, obj))
+            _log(
+                "bajar: near id={} type={} color={} center=({:.1f},{:.1f}) "
+                "dist_xy={:.1f} obj_z={:.1f} height={:.1f} top_z={:.1f}".format(
+                    obj.id,
+                    obj.object_type,
+                    obj.color,
+                    obj.position_mm[0],
+                    obj.position_mm[1],
+                    (dist2 ** 0.5),
+                    obj.position_mm[2],
+                    obj.height_mm,
+                    top_surface,
+                )
+            )
+        if not candidates:
+            _log(f"bajar: no objects within radius={_XY_MATCH_RADIUS_MM:.1f}mm")
+            return None
+        candidates.sort(key=lambda item: item[0], reverse=True)
+        return candidates[0][1]

dora_voice_control/dora_voice_control/robots/littlehand/signature.py

@@ -0,0 +1,27 @@
+"""DSPy signature for Littlehand command parsing."""
+
+from __future__ import annotations
+
+try:
+    import dspy
+except ImportError:  # pragma: no cover - optional dependency
+    dspy = None  # type: ignore
+
+
+if dspy is not None:
+    class LittlehandSignature(dspy.Signature):
+        """Parse Spanish voice commands for a vacuum gripper robot."""
+
+        comando = dspy.InputField(desc="Voice command in Spanish")
+        accion = dspy.OutputField(
+            desc="Action name: subir, bajar, ir, tomar, soltar, reiniciar or error"
+        )
+        objeto = dspy.OutputField(
+            desc="Object name (cubo, cilindro, estrella, caja) or 'no especificado'"
+        )
+        color = dspy.OutputField(
+            desc="Color (rojo, azul, blanco, amarillo, verde) or 'no especificado'"
+        )
+        tamano = dspy.OutputField(desc="Size or 'no especificado'")
+else:
+    LittlehandSignature = None  # type: ignore