diff --git a/dora_voice_control/dora_voice_control/robots/littlehand/behavior.py b/dora_voice_control/dora_voice_control/robots/littlehand/behavior.py index 50f9dcc..29179ad 100644 --- a/dora_voice_control/dora_voice_control/robots/littlehand/behavior.py +++ b/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.""" - target_z = ctx.pose[2] + self.config.step_mm + """Move to the home height (initial Z).""" + 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] diff --git a/dora_voice_control/dora_voice_control/robots/littlehand/signature.py b/dora_voice_control/dora_voice_control/robots/littlehand/signature.py new file mode 100644 index 0000000..83a073d --- /dev/null +++ b/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