griip-sdk-dev 0.4.0.dev24238__py3-none-any.whl

griip_sdk/orchestration/execution/three_finger_executor.py

@@ -0,0 +1,144 @@
+"""Three-finger grasp executor (stub).
+
+A single executor class handles both external and internal grasp modes,
+selected by the ``mode`` field on the parameters dataclass. External and
+internal are exact inverses for a three-finger gripper: external opens
+to engage, internal closes to engage.
+
+End-to-end support is out of scope for the initial refactor; the stub
+exists so the proto ``oneof`` is exercised and the architecture is
+provably general.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional
+
+from griip_sdk.hardware.gripper import IO3FingerGripper, ThreeFingerGripper
+from griip_sdk.orchestration.execution.grasp_executor import GraspExecutor, GraspMode
+
+PREP_FOR_APPROACH_NARROW_IO_STATE = 0
+
+
+@dataclass(frozen=True)
+class ThreeFingerExecutorParameters:
+    """SDK-side per-grasp execution parameters for the three-finger executor."""
+
+    mode: GraspMode
+    grip_force: int
+
+
+@dataclass
+class IOThreeFingerParameters:
+    """IO based three-finger grasp execution parameters (stub)."""
+
+    mode: GraspMode
+    prep_io_state: int
+    engaged_io_state: int
+
+
+class ThreeFingerExecutor(GraspExecutor):
+    """Three-finger grasp executor.
+
+    External: open fingers around object, then close to engage.
+    Internal: close fingers inside cavity, then open to engage.
+    """
+
+    def __init__(self, gripper: ThreeFingerGripper, parameters: ThreeFingerExecutorParameters) -> None:
+        super().__init__(gripper)
+        self._params = parameters
+
+    @property
+    def gripper(self) -> ThreeFingerGripper:
+        return self._gripper  # type: ignore[return-value]
+
+    @property
+    def parameters(self) -> ThreeFingerExecutorParameters:
+        return self._params
+
+    def prepare_to_grasp(self, block: bool = True) -> None:
+        if self._params.mode is GraspMode.EXTERNAL:
+            self.gripper.open(block=block)
+        else:
+            self.gripper.close(force_value=self._params.grip_force, block=block)
+
+    def grasp_object(self, block: bool = True, timeout: Optional[float] = None) -> None:
+        if self._params.mode is GraspMode.EXTERNAL:
+            self.gripper.close(force_value=self._params.grip_force, block=block)
+        else:
+            self.gripper.open(block=block)
+
+    def release_object(self, block: bool = True) -> None:
+        if self._params.mode is GraspMode.EXTERNAL:
+            self.gripper.open(block=block)
+        else:
+            self.gripper.close(force_value=self._params.grip_force, block=block)
+
+    def is_object_engaged(self) -> bool:
+        raise NotImplementedError("Not implemented, will be implemented in the next pr")
+
+
+class IOThreeFingerExecutor(GraspExecutor):
+    """IO-based three-finger grasp executor.
+
+    External: open fingers around object, then close to engage.
+    Internal: close fingers inside cavity, then open to engage.
+    """
+
+    def __init__(self, gripper: IO3FingerGripper, parameters: IOThreeFingerParameters) -> None:
+        super().__init__(gripper)
+        self._params = parameters
+
+    @property
+    def gripper(self) -> IO3FingerGripper:
+        return self._gripper  # type: ignore[return-value]
+
+    @property
+    def parameters(self) -> IOThreeFingerParameters:
+        return self._params
+
+    def prepare_to_approach(self, block: bool = False) -> None:
+        """Pre-position the fingers to a safe narrow width while the arm travels to prep."""
+        narrow_target = min(PREP_FOR_APPROACH_NARROW_IO_STATE, max(self._params.prep_io_state, 0))
+        self.gripper.move_gripper(io_state=narrow_target, block=block)
+
+    def prepare_for_plunge(self, block: bool = True) -> None:
+        """Close the fingers fully so the gripper can plunge through a narrow aperture."""
+        self.gripper.close(block=block)
+
+    def prepare_to_grasp(self, block: bool = True) -> None:
+        self.gripper.move_gripper(io_state=self._params.prep_io_state, block=block)
+
+    def grasp_object(self, block: bool = True, timeout: Optional[float] = None) -> None:
+        self.gripper.move_gripper(io_state=self._params.engaged_io_state, block=block)
+
+    def release_object(self, block: bool = True) -> None:
+        if self._params.mode is GraspMode.EXTERNAL:
+            self.gripper.open_gripper(block=block)
+        else:
+            self.gripper.close_gripper(block=block)
+
+    def is_object_engaged(self) -> bool:
+        if self._params.mode is GraspMode.EXTERNAL:
+            return self._external_is_object_engaged()
+        else:
+            return self._internal_is_object_engaged()
+
+    def return_to_idle(self, block: bool = True) -> None:
+        self.gripper.release(block=block)
+        self.gripper.close(block=block)
+
+    def _external_is_object_engaged(self) -> bool:
+        raise NotImplementedError("Not implemented")
+
+    def _internal_is_object_engaged(self) -> bool:
+        io_sensor_state = self.gripper.get_io_sensor()
+        if io_sensor_state == 1:
+            return True
+
+        self.gripper.move_gripper(io_state=1, block=True, timeout=0.1)
+        io_sensor_state = self.gripper.get_io_sensor()
+        if io_sensor_state == 1:
+            return True
+        return False

griip_sdk/orchestration/grasp_provider.py

@@ -0,0 +1,258 @@
+"""Grasp targets for an app that moves the robot itself.
+
+The SDK takes and stores pictures, runs perception on a stored picture when
+asked, and keeps one queue of ranked grasp poses. It never commands motion:
+the app parks the robot before ``capture`` and executes the pose it pops.
+
+One perception run at a time, on the same pipeline the pick managers use.
+The results of a run replace the queue, so pop before starting the next run.
+"""
+
+from __future__ import annotations
+
+import logging
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Any, Optional, cast
+
+import grpc
+from spatialmath import SE3
+
+from griip_sdk.cell.end_effector_collision_model import EndEffectorCollisionModel
+from griip_sdk.hardware.camera import CameraManager, GrabResult
+from griip_sdk.hardware.robot_client import VrmcsRobotClient
+from griip_sdk.orchestration.perception_inputs import build_generate_picks_request
+from griip_sdk.orchestration.proto_conversions import bin_wall_to_proto, bin_walls_to_proto
+from griip_sdk.orchestration.types import CaptureInfo, GraspTarget
+
+logger = logging.getLogger(__name__)
+
+_RUN_JOIN_TIMEOUT_SECONDS = 5.0
+
+
+@dataclass
+class _StoredCapture:
+    tag: str
+    bin_number: Optional[int]
+    grab_result: GrabResult
+    tcp_pose: SE3
+    joint_angles: list[float]
+    state: str = "captured"
+    detail: Optional[str] = None
+    targets: list[GraspTarget] = field(default_factory=list)
+
+    def info(self) -> CaptureInfo:
+        return CaptureInfo(
+            tag=self.tag,
+            bin_number=self.bin_number,
+            image_id=self.grab_result.image_id,
+            capture_timestamp_ns=self.grab_result.timestamp_nanoseconds,
+            state=self.state,
+            detail=self.detail,
+        )
+
+
+class GraspProvider:
+    """Store pictures, process them on demand, pop grasp poses from one queue."""
+
+    def __init__(
+        self,
+        *,
+        picking_stub: object,
+        camera_manager: CameraManager,
+        robot_client: VrmcsRobotClient,
+        tcp_to_camera: SE3,
+        tcp_to_flange: SE3,
+        camera_intrinsics_flat: list[float],
+        roi: list[int],
+        max_detections: int,
+        max_picks_per_object: int,
+        min_depth_for_perception_mm: float,
+        end_effector_collision_model: EndEffectorCollisionModel,
+        gripper_open_width_m: Optional[float],
+        gripper_closed_width_m: float,
+        all_bin_walls: dict[int, list[dict[str, Any]]],
+        all_bin_floors: dict[int, dict[str, Any]],
+    ) -> None:
+        self._picking_stub = picking_stub
+        self._camera_manager = camera_manager
+        self._robot_client = robot_client
+        self._tcp_to_camera = tcp_to_camera
+        self._tcp_to_flange = tcp_to_flange
+        self._camera_intrinsics_flat = camera_intrinsics_flat
+        self._roi = list(roi)
+        self._max_detections = max_detections
+        self._max_picks_per_object = max_picks_per_object
+        self._min_depth_for_perception_mm = min_depth_for_perception_mm
+        self._end_effector_collision_model = end_effector_collision_model
+        self._gripper_open_width_m = gripper_open_width_m
+        self._gripper_closed_width_m = gripper_closed_width_m
+        self._all_bin_walls = all_bin_walls
+        self._all_bin_floors = all_bin_floors
+
+        self._captures: dict[str, _StoredCapture] = {}
+        self._queue: list[GraspTarget] = []
+        self._runs_in_flight = 0
+        self._threads: list[threading.Thread] = []
+        self._closed = False
+        self._state = threading.Condition()
+        # One GeneratePicks stream at a time, the same constraint PickGenerator has.
+        self._stream_lock = threading.Lock()
+
+    @property
+    def bin_numbers(self) -> list[int]:
+        return sorted(self._all_bin_walls)
+
+    # ------------------------------------------------------------------
+    # Picture store
+    # ------------------------------------------------------------------
+
+    def capture(self, tag: str, *, bin_number: Optional[int] = None) -> CaptureInfo:
+        """Take a picture at the robot's current pose and store it under ``tag``.
+
+        Never moves the robot; park it first. Replaces any picture stored
+        under the same tag. Does not start processing.
+        """
+        if bin_number is not None and bin_number not in self._all_bin_walls:
+            raise ValueError(f"bin_number {bin_number} is not in runtime.bins.available ({self.bin_numbers})")
+        joint_angles = list(self._robot_client.get_joint_angles())
+        tcp_pose = self._robot_client.get_TCP_pose()
+        grab_result = self._camera_manager.grab()
+        stored = _StoredCapture(
+            tag=tag,
+            bin_number=bin_number,
+            grab_result=grab_result,
+            tcp_pose=tcp_pose,
+            joint_angles=joint_angles,
+        )
+        with self._state:
+            self._captures[tag] = stored
+        logger.info("Stored picture %r (bin=%s, image_id=%s)", tag, bin_number, grab_result.image_id)
+        return stored.info()
+
+    def captures(self) -> list[CaptureInfo]:
+        with self._state:
+            return [stored.info() for stored in self._captures.values()]
+
+    def delete(self, tag: str) -> None:
+        with self._state:
+            self._captures.pop(tag, None)
+
+    # ------------------------------------------------------------------
+    # Processing and the queue
+    # ------------------------------------------------------------------
+
+    def process(self, tag: str) -> None:
+        """Run perception on the stored picture ``tag`` in the background.
+
+        When the run finishes its poses replace the queue. A picture replaced
+        or deleted while its run is in flight is dropped on completion.
+        """
+        with self._state:
+            if self._closed:
+                raise RuntimeError("GraspProvider is closed")
+            stored = self._captures.get(tag)
+            if stored is None:
+                raise ValueError(f"no stored picture under tag {tag!r}")
+            stored.state = "processing"
+            stored.detail = None
+            self._runs_in_flight += 1
+            thread = threading.Thread(target=self._run, args=(stored,), name=f"grasp-provider-{tag}", daemon=True)
+            self._threads.append(thread)
+        thread.start()
+
+    def next_target(self, timeout: Optional[float] = None) -> Optional[GraspTarget]:
+        """Wait for in-flight runs, then pop the best pose. None when nothing is left."""
+        deadline = None if timeout is None else time.monotonic() + timeout
+        with self._state:
+            while self._runs_in_flight > 0:
+                remaining = None if deadline is None else deadline - time.monotonic()
+                if remaining is not None and remaining <= 0.0:
+                    return None
+                self._state.wait(remaining)
+            if not self._queue:
+                return None
+            return self._queue.pop(0)
+
+    def close(self) -> None:
+        with self._state:
+            self._closed = True
+            threads = list(self._threads)
+        for thread in threads:
+            thread.join(timeout=_RUN_JOIN_TIMEOUT_SECONDS)
+
+    # ------------------------------------------------------------------
+
+    def _run(self, stored: _StoredCapture) -> None:
+        ranked: list[tuple[int, float, Any]] = []
+        detail: Optional[str] = None
+        failed = False
+        try:
+            request = build_generate_picks_request(
+                stored.grab_result,
+                stored.tcp_pose,
+                stored.joint_angles,
+                tcp_to_camera=self._tcp_to_camera,
+                camera_intrinsics_flat=self._camera_intrinsics_flat,
+                min_depth_for_perception_mm=self._min_depth_for_perception_mm,
+                max_detections=self._max_detections,
+                max_picks_per_object=self._max_picks_per_object,
+                end_effector_collision_model=self._end_effector_collision_model,
+                gripper_open_width_m=self._gripper_open_width_m,
+                gripper_closed_width_m=self._gripper_closed_width_m,
+                extra_kwargs=self._request_extras(stored.bin_number),
+            )
+            with self._stream_lock:
+                for response in cast(Any, self._picking_stub).GeneratePicks(request):
+                    if response.HasField("pick_result"):
+                        pick_result = response.pick_result
+                        ranked.append((-int(pick_result.priority), float(pick_result.distance_to_home), pick_result))
+                    if response.HasField("terminal_message"):
+                        terminal = response.terminal_message
+                        if terminal.no_parts_detected:
+                            detail = "no parts detected"
+                        elif terminal.has_only_ungraspable_objects:
+                            detail = "objects detected, none graspable"
+        except grpc.RpcError as rpc_error:
+            failed = True
+            detail = f"gRPC {rpc_error.code().name}: {rpc_error.details()}"
+            logger.error("Perception failed for picture %r: %s", stored.tag, detail)
+        except Exception as run_error:
+            failed = True
+            detail = f"{type(run_error).__name__}: {run_error}"
+            logger.error("Perception failed for picture %r: %s", stored.tag, detail, exc_info=True)
+
+        ranked.sort(key=lambda entry: (entry[0], entry[1]))
+        targets = [
+            GraspTarget.from_proto(
+                pick_result,
+                capture_tag=stored.tag,
+                bin_number=stored.bin_number,
+                image_id=stored.grab_result.image_id,
+                tcp_to_flange=self._tcp_to_flange,
+            )
+            for _, _, pick_result in ranked
+        ]
+
+        with self._state:
+            self._runs_in_flight -= 1
+            if self._captures.get(stored.tag) is stored:
+                stored.state = "failed" if failed else "done"
+                stored.detail = detail
+                stored.targets = targets
+                self._queue = list(targets)
+                logger.info("Picture %r processed: %d targets%s", stored.tag, len(targets), f" ({detail})" if detail else "")
+            else:
+                logger.info("Picture %r was replaced during processing; %d targets dropped", stored.tag, len(targets))
+            self._state.notify_all()
+
+    def _request_extras(self, bin_number: Optional[int]) -> dict[str, Any]:
+        extras: dict[str, Any] = {"roi": self._roi}
+        if bin_number is None:
+            return extras
+        extras["bin_walls"] = bin_walls_to_proto(self._all_bin_walls[bin_number])
+        bin_floor = self._all_bin_floors.get(bin_number)
+        if bin_floor is not None:
+            extras["bin_floor"] = bin_wall_to_proto(bin_floor)
+        return extras

griip_sdk/orchestration/grasp_queue.py

@@ -0,0 +1,107 @@
+from __future__ import annotations
+
+import logging
+import threading
+from typing import Optional
+
+from griip_sdk.orchestration.types import PickResultAdapter
+
+logger = logging.getLogger(__name__)
+
+
+class GraspQueue:
+    """Thread-safe queue for planned grasps, gated by a generation epoch.
+
+    A grasp is only valid while the planning assumptions it was generated under
+    still hold (the captured scene and the capture pose it was planned from). The
+    epoch is a single monotonic token for those assumptions: every fresh capture
+    or invalidation advances it and clears the queue in the same locked step.
+    Producers stamp grasps with the epoch they were generated under and ``add``
+    rejects any stale epoch, so everything queued is always current.
+    """
+
+    def __init__(self) -> None:
+        self._queue: list[PickResultAdapter] = []
+        self._lock = threading.Lock()
+        self._current_epoch = 0
+        self._current_image_id: Optional[str] = None
+
+    @property
+    def current_epoch(self) -> int:
+        with self._lock:
+            return self._current_epoch
+
+    @property
+    def current_image_id(self) -> Optional[str]:
+        with self._lock:
+            return self._current_image_id
+
+    def reset(self, new_image_id: str) -> int:
+        """Open a new generation epoch for a freshly captured image.
+
+        Clears the queue and returns the epoch the producer must stamp its grasps
+        with via :meth:`add`.
+        """
+        with self._lock:
+            self._current_epoch += 1
+            self._current_image_id = new_image_id
+            self._queue.clear()
+            logger.info(f"Queue reset. Epoch {self._current_epoch}, image ID: {new_image_id}")
+            return self._current_epoch
+
+    def invalidate(self) -> None:
+        """Drop every queued grasp and advance the epoch.
+
+        Call whenever a queued grasp's planning assumptions change (capture pose,
+        active part, bin). In-flight grasps from the previous epoch are rejected on
+        ``add``, and nothing stale survives the call.
+        """
+        with self._lock:
+            dropped = len(self._queue)
+            self._current_epoch += 1
+            self._current_image_id = None
+            self._queue.clear()
+            logger.info(f"Queue invalidated. Epoch {self._current_epoch}, dropped {dropped} grasps.")
+
+    def add(self, grasp: PickResultAdapter, epoch: int) -> bool:
+        """Add a grasp produced under ``epoch`` if that epoch is still current."""
+        with self._lock:
+            if epoch != self._current_epoch:
+                logger.warning(f"GRASP REJECTED (stale epoch {epoch}, current {self._current_epoch})")
+                return False
+            self._queue.append(grasp)
+            logger.info(f"GRASP ADDED. Queue size: {len(self._queue)}, epoch: {epoch}")
+            return True
+
+    def get_next(self) -> Optional[PickResultAdapter]:
+        """Pop the next grasp, or None if the queue is empty."""
+        with self._lock:
+            if not self._queue:
+                return None
+            best_index = min(
+                range(len(self._queue)),
+                key=lambda queue_index: (
+                    -self._queue[queue_index].priority,
+                    self._queue[queue_index].distance_to_home,
+                ),
+            )
+            grasp = self._queue.pop(best_index)
+            logger.info(f"Retrieved grasp {grasp.name}. Remaining: {len(self._queue)}")
+            return grasp
+
+    def has_grasps(self) -> bool:
+        return self.grasps_count() > 0
+
+    def grasps_count(self) -> int:
+        with self._lock:
+            return len(self._queue)
+
+    def clear_for_object(self, object_index: int) -> int:
+        """Remove all grasps for a specific object from the queue."""
+        with self._lock:
+            original_count = len(self._queue)
+            self._queue = [grasp for grasp in self._queue if grasp.obj_ref.idx != object_index]
+            removed_count = original_count - len(self._queue)
+            if removed_count > 0:
+                logger.info(f"Cleared {removed_count} grasps for object {object_index}. Remaining: {len(self._queue)}")
+            return removed_count

griip_sdk/orchestration/part_setup.py

@@ -0,0 +1,52 @@
+"""Per-part resolution and Initialize plumbing. SDK-internal free functions.
+
+Validates a ``part_id`` against the cell config and (re-)sends the per-part
+Initialize request. Used by both the manager's ``set_part`` and the cell
+builders, so it lives outside the manager class.
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+from typing import Any
+
+from griip_sdk.cell.builders import get_effective_tool_config
+from griip_sdk.cell.initialize import build_initialize_request, send_initialize
+
+
+def resolve_part_inputs(cell_config: dict[str, Any], part_id: str) -> tuple[dict[str, Any], dict[str, Any], dict[str, Any]]:
+    """Validate ``part_id`` against the cell config and return the part,
+    effective tool, and gripper config blocks. Raises ``ValueError`` if
+    ``part_id`` isn't defined. SDK-internal.
+    """
+    parts = cell_config.get("parts", {})
+    if part_id not in parts:
+        raise ValueError(f"Part '{part_id}' not defined in cell config. Known parts: {sorted(parts.keys())}")
+    part_config = parts[part_id]
+    tool_config = get_effective_tool_config(cell_config, part_config)
+    gripper_config = tool_config.get("gripper", {})
+    return part_config, tool_config, gripper_config
+
+
+def send_initialize_for_part(
+    admin_stub: Any,
+    cell_config: dict[str, Any],
+    part_config: dict[str, Any],
+    part_id: str,
+    base_path: Path,
+    *,
+    camera_intrinsics_flat: list[float],
+    image_width: int,
+    image_height: int,
+) -> None:
+    """Send the per-part Initialize request over the supplied admin stub. SDK-internal."""
+    initialize_request = build_initialize_request(
+        cell_config=cell_config,
+        part_config=part_config,
+        part_id=part_id,
+        base_path=base_path,
+        camera_intrinsics_flat=camera_intrinsics_flat,
+        image_width=image_width,
+        image_height=image_height,
+    )
+    send_initialize(admin_stub, initialize_request)