API Reference

This section contains the automatically generated documentation for the robot_workspace package.

Core Modules

robot_workspace.objects.object

Classes

Object

Bases: ObjectAPI

Class representing an object detected in a robot's workspace.

Each object is characterized by its bounding box (in both pixel and world coordinates), segmentation mask (if available), size, and additional metadata. This class also provides methods to calculate dimensions, orientation, and other properties of the object.

Attributes:

Name	Type	Description
_label	str	Label identifying the object (e.g., "pencil").
_workspace	Workspace	Workspace in which the object resides.
_verbose	bool	Whether verbose logging is enabled.
_logger	Logger	Logger instance.
_original_mask_8u	ndarray	Original segmentation mask.
_u_rel_min	float	Minimum U relative coordinate.
_v_rel_min	float	Minimum V relative coordinate.
_u_rel_max	float	Maximum U relative coordinate.
_v_rel_max	float	Maximum V relative coordinate.
_width	float	Width in relative coordinates.
_height	float	Height in relative coordinates.
_gripper_rotation	float	Suggested orientation for the robot gripper.
_size_m2	float	Area of the object in square meters.
_pose_center	PoseObjectPNP	Pose of the geometric center.
_u_rel_o	float	U relative coordinate of the center.
_v_rel_o	float	V relative coordinate of the center.
_pose_com	PoseObjectPNP	Pose of the center of mass.
_u_rel_com	float	U relative coordinate of the COM.
_v_rel_com	float	V relative coordinate of the COM.

Source code in robot_workspace/objects/object.py

class Object(ObjectAPI):
    """
    Class representing an object detected in a robot's workspace.

    Each object is characterized by its bounding box (in both pixel and world coordinates),
    segmentation mask (if available), size, and additional metadata. This class also provides
    methods to calculate dimensions, orientation, and other properties of the object.

    Attributes:
        _label (str): Label identifying the object (e.g., "pencil").
        _workspace (Workspace): Workspace in which the object resides.
        _verbose (bool): Whether verbose logging is enabled.
        _logger (logging.Logger): Logger instance.
        _original_mask_8u (np.ndarray, optional): Original segmentation mask.
        _u_rel_min (float): Minimum U relative coordinate.
        _v_rel_min (float): Minimum V relative coordinate.
        _u_rel_max (float): Maximum U relative coordinate.
        _v_rel_max (float): Maximum V relative coordinate.
        _width (float): Width in relative coordinates.
        _height (float): Height in relative coordinates.
        _gripper_rotation (float): Suggested orientation for the robot gripper.
        _size_m2 (float): Area of the object in square meters.
        _pose_center (PoseObjectPNP): Pose of the geometric center.
        _u_rel_o (float): U relative coordinate of the center.
        _v_rel_o (float): V relative coordinate of the center.
        _pose_com (PoseObjectPNP): Pose of the center of mass.
        _u_rel_com (float): U relative coordinate of the COM.
        _v_rel_com (float): V relative coordinate of the COM.
    """

    # *** CONSTRUCTORS ***
    @log_start_end_cls()
    def __init__(
        self,
        label: str,
        u_min: int,
        v_min: int,
        u_max: int,
        v_max: int,
        mask_8u: np.ndarray | None,
        workspace: Workspace,
        verbose: bool = False,
    ) -> None:
        """
        Initializes an Object instance.

        Args:
            label (str): Label of the object (e.g., "chocolate bar").
            u_min (int): Upper-left corner u-coordinate of the bounding box (in pixels).
            v_min (int): Upper-left corner v-coordinate of the bounding box (in pixels).
            u_max (int): Lower-right corner u-coordinate of the bounding box (in pixels).
            v_max (int): Lower-right corner v-coordinate of the bounding box (in pixels).
            mask_8u (np.ndarray, optional): Segmentation mask of the object (8-bit, uint8).
            workspace (Workspace): Workspace instance where the object is located.
            verbose (bool): If True, enables verbose logging.

        Raises:
            ValueError: If the provided segmentation mask is not 8-bit unsigned,
                or if the workspace has no image shape.
        """
        super().__init__()

        self._label = label
        self._workspace = workspace
        self._verbose = verbose
        self._logger = logging.getLogger("robot_workspace")

        if self._workspace.img_shape() is None:
            raise ValueError("Object has no image shape. This probably means that the Niryo Workspace was not detected.")

        # Store original mask for rotation
        self._original_mask_8u = mask_8u.copy() if mask_8u is not None else None

        # Initialize the object with the common initialization logic
        self._init_object_properties(u_min, v_min, u_max, v_max, mask_8u)

    def _init_object_properties(self, u_min: int, v_min: int, u_max: int, v_max: int, mask_8u: np.ndarray | None) -> None:
        """
        Common initialization logic for object properties.

        Args:
            u_min (int): Upper-left corner u-coordinate (pixels).
            v_min (int): Upper-left corner v-coordinate (pixels).
            u_max (int): Lower-right corner u-coordinate (pixels).
            v_max (int): Lower-right corner v-coordinate (pixels).
            mask_8u (np.ndarray, optional): Segmentation mask (8-bit, uint8).
        """
        self._u_rel_min, self._v_rel_min = self._calc_rel_coordinates(u_min, v_min)
        self._u_rel_max, self._v_rel_max = self._calc_rel_coordinates(u_max, v_max)

        self._calc_width_height_m()

        self._width = self._u_rel_max - self._u_rel_min
        self._height = self._v_rel_max - self._v_rel_min

        if mask_8u is not None:
            self._calc_largest_contour(mask_8u)

            gripper_rotation, center = self._calc_gripper_orientation_from_segmentation_mask()
            width, height = self._rotated_bounding_box()

            u_0 = center[0]
            v_0 = center[1]

            self._gripper_rotation = gripper_rotation

            self._update_width_height(width, height)

            cm = Object._calculate_center_of_mass(mask_8u)
            if cm is not None:
                cx, cy = cm
            else:
                cx, cy = u_0, v_0
        else:
            u_0 = int(u_min + (u_max - u_min) / 2)
            v_0 = int(v_min + (v_max - v_min) / 2)
            cx = u_0
            cy = v_0
            self._gripper_rotation = 0.0

        self._calc_size()

        self._pose_center, self._u_rel_o, self._v_rel_o = self._calc_pose_from_uv_coords(u_0, v_0)
        self._pose_com, self._u_rel_com, self._v_rel_com = self._calc_pose_from_uv_coords(int(cx), int(cy))

    def __str__(self) -> str:
        position = f"x = {self._pose_com.x:.2f}, y = {self._pose_com.y:.2f}, z = {self._pose_com.z:.2f}"
        orientation = f"roll = {self._pose_com.roll:.3f}, pitch = {self._pose_com.pitch:.3f}, yaw = {self._pose_com.yaw:.3f}"
        return self.label() + "\n" + position + "\n" + orientation

    def __repr__(self) -> str:
        return self.__str__()

    # *** PUBLIC SET methods ***

    def set_position(self, xy_coordinate: list[float]) -> None:
        """
        Legacy method kept for backwards compatibility.

        Args:
            xy_coordinate (list[float]): [x, y] world coordinates in meters.
        """
        # Create a pose with the new x, y but keeping old z and orientation
        new_pose = self._pose_com.copy_with_offsets(
            x_offset=xy_coordinate[0] - self._pose_com.x, y_offset=xy_coordinate[1] - self._pose_com.y
        )
        self.set_pose_com(new_pose)

    def set_pose_com(self, pose_com: PoseObjectPNP) -> None:
        """
        Updates the object's center of mass pose and recalculates all dependent properties.

        Handles both position changes and orientation changes (rotation around z-axis).

        Args:
            pose_com (PoseObjectPNP): New pose for the object's center of mass.
        """
        if self.verbose():
            self._logger.debug(f"set_pose_com: {self._pose_com} → {pose_com}")

        # 1. Calculate transformations
        rotation_delta = pose_com.yaw - self._gripper_rotation
        translation = self._calculate_translation(pose_com)

        # 2. Transform bounding box
        new_u_min, new_v_min, new_u_max, new_v_max = self._apply_pose_transform_to_bbox(rotation_delta, translation)

        # 3. Transform mask if present
        transformed_mask = self._apply_pose_transform_to_mask(rotation_delta, translation)

        # 4. Reinitialize with new geometry
        self._init_object_properties(new_u_min, new_v_min, new_u_max, new_v_max, transformed_mask)

        # 5. Override calculated pose with exact provided pose
        self._pose_com = pose_com
        self._gripper_rotation = pose_com.yaw

    def _calculate_translation(self, target_pose: PoseObjectPNP) -> tuple[int, int]:
        """
        Calculate pixel translation needed to reach target pose.

        Args:
            target_pose (PoseObjectPNP): The target pose in world coordinates.

        Returns:
            tuple[int, int]: (translation_u, translation_v) in pixels.
        """
        img_width, img_height, _ = self._workspace.img_shape()

        # Calculate old center in pixel coordinates
        old_center_u = int(self._u_rel_com * img_width)
        old_center_v = int(self._v_rel_com * img_height)

        # Calculate new center in pixel coordinates
        new_center_u_rel, new_center_v_rel = self._world_to_rel_coordinates(target_pose)
        new_center_u = int(new_center_u_rel * img_width)
        new_center_v = int(new_center_v_rel * img_height)

        translation_u = new_center_u - old_center_u
        translation_v = new_center_v - old_center_v

        return translation_u, translation_v

    def _apply_pose_transform_to_bbox(self, rotation_delta: float, translation: tuple[int, int]) -> tuple[int, int, int, int]:
        """
        Apply rotation and translation to bounding box.

        Args:
            rotation_delta (float): Rotation change in radians.
            translation (tuple[int, int]): (delta_u, delta_v) in pixels.

        Returns:
            tuple[int, int, int, int]: New (u_min, v_min, u_max, v_max).
        """
        img_width, img_height, _ = self._workspace.img_shape()

        # Calculate old center in pixel coordinates
        old_center_u = int(self._u_rel_com * img_width)
        old_center_v = int(self._v_rel_com * img_height)

        # Calculate old bounding box corners in pixel coordinates
        old_u_min = int(self._u_rel_min * img_width)
        old_v_min = int(self._v_rel_min * img_height)
        old_u_max = int(self._u_rel_max * img_width)
        old_v_max = int(self._v_rel_max * img_height)

        # Rotate bounding box corners around the old center
        rotated_corners = self._rotate_bounding_box(
            old_u_min, old_v_min, old_u_max, old_v_max, old_center_u, old_center_v, rotation_delta
        )

        # Find new axis-aligned bounding box from rotated corners
        new_u_min = int(min(corner[0] for corner in rotated_corners))
        new_v_min = int(min(corner[1] for corner in rotated_corners))
        new_u_max = int(max(corner[0] for corner in rotated_corners))
        new_v_max = int(max(corner[1] for corner in rotated_corners))

        # Apply translation
        translation_u, translation_v = translation
        new_u_min += translation_u
        new_v_min += translation_v
        new_u_max += translation_u
        new_v_max += translation_v

        # Clamp to image boundaries
        new_u_min = max(0, min(new_u_min, img_width - 1))
        new_v_min = max(0, min(new_v_min, img_height - 1))
        new_u_max = max(0, min(new_u_max, img_width - 1))
        new_v_max = max(0, min(new_v_max, img_height - 1))

        return new_u_min, new_v_min, new_u_max, new_v_max

    def _apply_pose_transform_to_mask(self, rotation_delta: float, translation: tuple[int, int]) -> np.ndarray | None:
        """
        Apply rotation and translation to segmentation mask.

        Args:
            rotation_delta (float): Rotation change in radians.
            translation (tuple[int, int]): (delta_u, delta_v) in pixels.

        Returns:
            np.ndarray | None: Transformed mask, or None if no mask exists.
        """
        if self._original_mask_8u is None:
            return None

        img_width, img_height, _ = self._workspace.img_shape()
        old_center_u = int(self._u_rel_com * img_width)
        old_center_v = int(self._v_rel_com * img_height)

        # Rotate mask
        rotated_mask = self._rotate_mask(self._original_mask_8u, rotation_delta, old_center_u, old_center_v)

        # Update the original mask with the rotated version for future rotations
        self._original_mask_8u = rotated_mask.copy()

        # Translate mask
        translation_u, translation_v = translation
        translated_mask = self._translate_mask(rotated_mask, translation_u, translation_v)

        return translated_mask

    # *** PUBLIC GET methods ***

    def get_workspace_id(self) -> str:
        """
        Returns the ID of the workspace containing the object.

        Returns:
            str: Workspace ID.
        """
        return self.workspace().id()

    # *** PUBLIC methods ***

    def as_string_for_llm(self) -> str:
        """
        Formats object details as a string for use with language models.

        Returns:
            str: String containing object information.
        """
        return f"""- '{self.label()}' at world coordinates [{self.x_com():.2f}, {self.y_com():.2f}] with a width of {
        self.width_m():.2f} meters, a height of {self.height_m():.2f} meters and a size of {
        self.size_m2()*10000:.2f} square centimeters."""

    def as_string_for_llm_lbl(self) -> str:
        """
        Formats object details in a line-by-line style, optimized for language model usage.

        Returns:
            str: Detailed object information string.
        """
        return f"""- '{self.label()}' at world coordinates [{self.x_com():.2f}, {self.y_com():.2f}] with
    - width: {self.width_m():.2f} meters,
    - height: {self.height_m():.2f} meters and
    - size: {self.size_m2() * 10000:.2f} square centimeters."""

    def as_string_for_chat_window(self) -> str:
        """
        Formats object details for display in a chat interface.

        Returns:
            str: Chat-friendly object description.
        """
        return f"""Detected a new object: {self.label()} at world coordinate ({self.x_com():.2f}, {
        self.y_com():.2f}) with orientation {self.gripper_rotation():.1f} rad and size {
        self.width_m():.2f} m x {self.height_m():.2f} m."""

    # *** NEUE METHODEN FÜR JSON-SERIALISIERUNG ***

    def to_dict(self) -> dict[str, Any]:
        """
        Converts the Object instance to a dictionary that can be JSON serialized.

        Returns:
            dict[str, Any]: Dictionary representation of the object.
        """
        return {
            "id": self.generate_object_id(),
            "label": self._label,
            "workspace_id": self.get_workspace_id(),
            "timestamp": time.time(),
            "position": {
                "center_of_mass": {
                    "x": float(self.x_com()),
                    "y": float(self.y_com()),
                    "z": float(self._pose_com.z) if hasattr(self._pose_com, "z") else 0.0,
                },
                "center": {
                    "x": float(self.x_center()),
                    "y": float(self.y_center()),
                    "z": float(self._pose_center.z) if hasattr(self._pose_center, "z") else 0.0,
                },
            },
            "image_coordinates": {
                "center_rel": {"u": float(self._u_rel_o), "v": float(self._v_rel_o)},
                "center_of_mass_rel": {"u": float(self._u_rel_com), "v": float(self._v_rel_com)},
                "bounding_box_rel": {
                    "u_min": float(self._u_rel_min),
                    "v_min": float(self._v_rel_min),
                    "u_max": float(self._u_rel_max),
                    "v_max": float(self._v_rel_max),
                },
            },
            "dimensions": {
                "width_m": float(self._width_m),
                "height_m": float(self._height_m),
                "size_m2": float(self._size_m2),
            },
            "gripper_rotation": float(self._gripper_rotation),
            "confidence": getattr(self, "_confidence", 1.0),
            "class_id": getattr(self, "_class_id", 0),
        }

    def to_json(self) -> str:
        """
        Converts the Object instance to a JSON string.

        Returns:
            str: JSON representation.
        """
        return json.dumps(self.to_dict(), indent=2)

    def generate_object_id(self) -> str:
        """
        Generates a unique ID for the object based on its properties.

        Returns:
            str: Unique object identifier.
        """
        id_string = f"{self._label}_{self.x_com():.3f}_{self.y_com():.3f}_{time.time()}"
        return hashlib.md5(id_string.encode(), usedforsecurity=False).hexdigest()[:8]

    @staticmethod
    def _deserialize_mask(mask_data: str, shape: tuple | list, dtype: str = "uint8") -> np.ndarray:
        """
        Deserialize base64 string back to numpy mask.

        Args:
            mask_data (str): Base64 encoded mask string.
            shape (tuple | list): Original shape of the mask (height, width).
            dtype (str): Data type of the mask (default: 'uint8').

        Returns:
            np.ndarray: Reconstructed mask array.

        Raises:
            ValueError: If mask_data is invalid or shape doesn't match.
        """
        if isinstance(shape, list):
            shape = tuple(shape)

        try:
            mask_bytes = base64.b64decode(mask_data.encode("utf-8"))

            # Validate size matches shape
            dtype_obj = np.dtype(dtype)
            expected_size = int(np.prod(shape)) * dtype_obj.itemsize

            if len(mask_bytes) != expected_size:
                raise ValueError(
                    f"Mask data size mismatch: expected {expected_size} bytes "
                    f"for shape {shape} with dtype {dtype}, got {len(mask_bytes)} bytes"
                )

            mask = np.frombuffer(mask_bytes, dtype=dtype_obj)
            mask = mask.reshape(shape)
            return mask
        except Exception as e:
            raise ValueError(f"Failed to deserialize mask: {e}") from e

    @classmethod
    def from_dict(cls, data: dict[str, Any], workspace: Workspace) -> Object | None:
        """
        Creates an Object instance from a dictionary.

        Args:
            data (dict[str, Any]): Dictionary containing object data.
            workspace (Workspace): Workspace instance.

        Returns:
            Object | None: Reconstructed object instance or None if fails.
        """
        logger = logging.getLogger("robot_workspace")
        try:
            # Check if we have the new format (from to_dict) or old format (with bbox key)
            if "bbox" in data:
                # Old format - has bbox directly
                bbox = data["bbox"]
                u_min = bbox["x_min"]
                v_min = bbox["y_min"]
                u_max = bbox["x_max"]
                v_max = bbox["y_max"]
            elif "image_coordinates" in data and "bounding_box_rel" in data["image_coordinates"]:
                # New format - convert from relative coordinates
                bbox_rel = data["image_coordinates"]["bounding_box_rel"]
                img_shape = workspace.img_shape()
                if img_shape is None:
                    raise ValueError("Workspace image shape is None")

                u_min = int(bbox_rel["u_min"] * img_shape[0])
                v_min = int(bbox_rel["v_min"] * img_shape[1])
                u_max = int(bbox_rel["u_max"] * img_shape[0])
                v_max = int(bbox_rel["v_max"] * img_shape[1])
            else:
                raise KeyError("Missing bounding box information in dictionary")

            # Handle mask if present
            mask_8u = None
            if data.get("has_mask", False) and "mask_data" in data and "mask_shape" in data:
                mask_8u = cls._deserialize_mask(data["mask_data"], data["mask_shape"], data.get("mask_dtype", "uint8"))

            # Create object
            obj = Object(
                label=data["label"],
                u_min=u_min,
                v_min=v_min,
                u_max=u_max,
                v_max=v_max,
                mask_8u=mask_8u,
                workspace=workspace,
            )

            # Restore additional properties if needed
            if "confidence" in data:
                obj._confidence = data["confidence"]
            if "class_id" in data:
                obj._class_id = data["class_id"]

            return obj

        except Exception as e:
            logger.error(f"Error reconstructing object from dict: {e}")
            return None

    @classmethod
    def from_json(cls, json_str: str, workspace: Workspace) -> Object | None:
        """
        Creates an Object instance from a JSON string.

        Args:
            json_str (str): JSON string containing object data.
            workspace (Workspace): Workspace instance.

        Returns:
            Object | None: Reconstructed object instance or None if fails.
        """
        logger = logging.getLogger("robot_workspace")
        try:
            data = json.loads(json_str)
            return cls.from_dict(data, workspace)
        except Exception as e:
            logger.error(f"Error parsing JSON: {e}")
            return None

    # *** PUBLIC STATIC/CLASS GET methods ***

    @staticmethod
    def calc_width_height(pose_ul: PoseObjectPNP, pose_lr: PoseObjectPNP) -> tuple[float, float]:
        """
        Calculates the width and the height between two PoseObjects in meters.

        Args:
            pose_ul (PoseObjectPNP): PoseObject in the upper left corner.
            pose_lr (PoseObjectPNP): PoseObject in the lower right corner.

        Returns:
            tuple[float, float]: (width, height) in meters.
        """
        width_m = pose_ul.y - pose_lr.y
        height_m = pose_ul.x - pose_lr.x

        return width_m, height_m

    # *** PRIVATE methods ***

    def _world_to_rel_coordinates(self, pose: PoseObjectPNP) -> tuple[float, float]:
        """
        Converts world coordinates to relative image coordinates.

        Args:
            pose (PoseObjectPNP): Pose in world coordinates.

        Returns:
            tuple[float, float]: (u_rel, v_rel) normalized [0, 1].
        """
        # Get workspace corners in world coordinates
        ul = self._workspace.xy_ul_wc()
        lr = self._workspace.xy_lr_wc()

        if ul is None or lr is None:
            return 0.5, 0.5

        x_range = lr.x - ul.x
        u_rel = (pose.x - ul.x) / x_range if abs(x_range) > 1e-06 else 0.5

        y_range = ul.y - lr.y
        v_rel = (ul.y - pose.y) / y_range if abs(y_range) > 1e-06 else 0.5

        u_rel = max(0.0, min(1.0, u_rel))
        v_rel = max(0.0, min(1.0, v_rel))

        return float(u_rel), float(v_rel)

    def _rotate_bounding_box(
        self, u_min: int, v_min: int, u_max: int, v_max: int, center_u: int, center_v: int, angle: float
    ) -> list[tuple[int, int]]:
        """
        Rotates the four corners of a bounding box around a center point.

        Args:
            u_min, v_min (int): Top-left corner.
            u_max, v_max (int): Bottom-right corner.
            center_u, center_v (int): Center of rotation.
            angle (float): Rotation angle in radians (counter-clockwise).

        Returns:
            list[tuple[int, int]]: Four rotated corner coordinates.
        """
        corners = [
            (u_min, v_min),
            (u_max, v_min),
            (u_max, v_max),
            (u_min, v_max),
        ]

        rotated_corners = []
        cos_angle = math.cos(angle)
        sin_angle = math.sin(angle)

        for u, v in corners:
            u_translated = u - center_u
            v_translated = v - center_v

            u_rotated = u_translated * cos_angle - v_translated * sin_angle
            v_rotated = u_translated * sin_angle + v_translated * cos_angle

            u_final = int(u_rotated + center_u)
            v_final = int(v_rotated + center_v)

            rotated_corners.append((u_final, v_final))

        return rotated_corners

    def _rotate_mask(self, mask: np.ndarray, angle: float, center_u: int, center_v: int) -> np.ndarray | None:
        """
        Rotates a segmentation mask around a center point.

        Args:
            mask (np.ndarray): Original mask.
            angle (float): Rotation angle in radians.
            center_u, center_v (int): Center of rotation.

        Returns:
            np.ndarray | None: Rotated mask.
        """
        if mask is None:
            return None

        angle_degrees = -math.degrees(angle)

        height, width = mask.shape[:2]
        rotation_matrix = cv2.getRotationMatrix2D((float(center_u), float(center_v)), angle_degrees, 1.0)

        rotated_mask = cv2.warpAffine(mask, rotation_matrix, (width, height), flags=cv2.INTER_NEAREST)

        return rotated_mask

    def _translate_mask(self, mask: np.ndarray, delta_u: int, delta_v: int) -> np.ndarray | None:
        """
        Translates a segmentation mask by a given offset.

        Args:
            mask (np.ndarray): Original mask.
            delta_u, delta_v (int): Translation offsets.

        Returns:
            np.ndarray | None: Translated mask.
        """
        if mask is None:
            return None

        height, width = mask.shape[:2]
        translation_matrix = np.float32([[1, 0, delta_u], [0, 1, delta_v]])

        translated_mask = cv2.warpAffine(mask, translation_matrix, (width, height), flags=cv2.INTER_NEAREST)

        return translated_mask

    def _calc_size(self) -> None:
        """Calculates the object's area in square meters."""
        area = self._calculate_largest_contour_area()

        if area == 0:
            self._size_m2 = self._width_m * self._height_m
        else:
            ratio_w, ratio_h = self._calc_size_of_pixel_in_m()
            img_shape = self._workspace.img_shape()
            if img_shape:
                width, height, _nchannels = img_shape
                area_img = width * height
                self._size_m2 = float(area) / float(area_img) * ratio_w * ratio_h
            else:
                self._size_m2 = 0.0

    def _calc_size_of_pixel_in_m(self) -> tuple[float, float]:
        """
        Computes the physical size of the workspace in meters.

        Returns:
            tuple[float, float]: (width_m, height_m).
        """
        if self._width == 0 or self._height == 0:
            return 0.0, 0.0
        ratio_w = float(self._width_m / self._width)
        ratio_h = float(self._height_m / self._height)

        if self.verbose():
            self._logger.debug(f"Pixel size ratios - width: {ratio_w}, height: {ratio_h}")

        return ratio_w, ratio_h

    def _update_width_height(self, width: int, height: int) -> None:
        """
        Updates the object's width and height in meters.

        Args:
            width (int): Width in pixels.
            height (int): Height in pixels.
        """
        ratio_w, ratio_h = self._calc_size_of_pixel_in_m()

        self._width, self._height = self._calc_rel_coordinates(width, height)

        self._width_m = self._width * ratio_w
        self._height_m = self._height * ratio_h

    def _calc_pose_from_uv_coords(self, u: int, v: int) -> tuple[PoseObjectPNP, float, float]:
        """
        Calculates the object's pose based on pixel coordinates.

        Args:
            u (int): Pixel u-coordinate.
            v (int): Pixel v-coordinate.

        Returns:
            tuple[PoseObjectPNP, float, float]: (pose, u_rel, v_rel).
        """
        u_rel, v_rel = self._calc_rel_coordinates(u, v)

        pose = self._workspace.transform_camera2world_coords(self._workspace.id(), u_rel, v_rel, self._gripper_rotation)

        return pose, u_rel, v_rel

    @log_start_end_cls()
    def _calc_width_height_m(self) -> None:
        """Calculates physical width and height in meters."""
        if self.verbose():
            print(self._label, self._u_rel_min)

        pose_min = self._workspace.transform_camera2world_coords(self._workspace.id(), self._u_rel_min, self._v_rel_min)
        pose_max = self._workspace.transform_camera2world_coords(self._workspace.id(), self._u_rel_max, self._v_rel_max)

        if self.verbose():
            print(pose_min, pose_max)

        self._width_m, self._height_m = Object.calc_width_height(pose_min, pose_max)

    def _calc_rel_coordinates(self, u: int, v: int) -> tuple[float, float]:
        """
        Converts pixel coordinates to relative coordinates (0-1).

        Args:
            u (int): Pixel u-coordinate.
            v (int): Pixel v-coordinate.

        Returns:
            tuple[float, float]: (u_rel, v_rel).

        Raises:
            ValueError: If workspace image shape is invalid.
        """
        img_workspace_shape = self._workspace.img_shape()
        if img_workspace_shape is None:
            return 0.5, 0.5

        if img_workspace_shape[0] == 0 or img_workspace_shape[1] == 0:
            raise ValueError(
                f"Invalid workspace image shape: {img_workspace_shape}. "
                "Call workspace.set_img_shape() before creating objects."
            )

        u_rel = float(u / img_workspace_shape[0])
        v_rel = float(v / img_workspace_shape[1])

        return u_rel, v_rel

    @log_start_end_cls()
    def _calc_largest_contour(self, mask_8u: np.ndarray) -> None:
        """
        Determine the largest contour in a 2D segmentation mask.

        Args:
            mask_8u (np.ndarray): 8-bit unsigned integer array.

        Raises:
            ValueError: If mask_8u is not uint8.
        """
        if mask_8u.dtype != np.uint8:
            raise ValueError("Input mask must be an 8-bit unsigned integer array.")

        contours, _ = cv2.findContours(mask_8u, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if not contours:
            self._largest_contour = None
            return

        self._largest_contour = max(contours, key=cv2.contourArea)

    def _calculate_largest_contour_area(self) -> int:
        """
        Computes the area of the largest contour.

        Returns:
            int: Area in pixels.
        """
        if self._largest_contour is None or len(self._largest_contour) == 0:
            return 0

        largest_area = cv2.contourArea(self._largest_contour)

        return int(largest_area)

    def _rotated_bounding_box(self) -> tuple[int, int]:
        """
        Computes the dimensions of the rotated bounding box.

        Returns:
            tuple[int, int]: (width, height) in pixels.
        """
        _center, (width, height), _theta = self._get_params_of_min_area_rect()

        if width == 0 and height == 0:
            return width, height

        if self._width > self._height:
            return max(width, height), min(width, height)
        else:
            return min(width, height), max(width, height)

    def _get_params_of_min_area_rect(self) -> tuple[tuple[int, int], tuple[int, int], float]:
        """
        Calculate parameters of the minimum area rectangle.

        Returns:
            tuple: (center, dimensions, theta).
        """
        if self._largest_contour is None or len(self._largest_contour) == 0:
            return (0, 0), (0, 0), 0
        else:
            rect = cv2.minAreaRect(self._largest_contour)
            center, (width, height), theta = rect
            return (int(center[0]), int(center[1])), (int(width), int(height)), theta

    def _calc_gripper_orientation_from_segmentation_mask(self) -> tuple[float, tuple[int, int]]:
        """
        Determines the optimal orientation for the robot gripper.

        Returns:
            tuple[float, tuple[int, int]]: (rotation_radians, center_pixels).
        """
        gripper_rotation = 0.0

        center, (width, height), theta = self._get_params_of_min_area_rect()

        if not (width == 0 and height == 0):
            theta_rad = math.radians(theta)
            yaw_rel = theta_rad + math.pi / 2 if width < height else theta_rad
            yaw_rel += math.pi / 2
            yaw_rel = yaw_rel % (2 * math.pi)
            gripper_rotation = yaw_rel

        return gripper_rotation, center

    # *** PRIVATE STATIC/CLASS methods ***

    @staticmethod
    def _calculate_center_of_mass(mask_8u: np.ndarray) -> tuple[float, float] | None:
        """
        Calculates the center of mass of an object in a mask.

        Args:
            mask_8u (np.ndarray): 2D mask (uint8).

        Returns:
            tuple[float, float] | None: (cx, cy) in pixels, or None.
        """
        binary_mask = (mask_8u > 0).astype(np.uint8)
        non_zero_indices = np.nonzero(binary_mask)

        if len(non_zero_indices[0]) == 0:
            return None

        cx = np.mean(non_zero_indices[1])
        cy = np.mean(non_zero_indices[0])

        return float(cx), float(cy)

    # *** PUBLIC properties ***

    def label(self) -> str:
        """Object label."""
        return self._label

    def uv_rel_o(self) -> tuple[float, float]:
        """Object center in relative coordinates."""
        return self._u_rel_o, self._v_rel_o

    def u_rel_o(self) -> float:
        """Object center u-coordinate."""
        return self._u_rel_o

    def v_rel_o(self) -> float:
        """Object center v-coordinate."""
        return self._v_rel_o

    def pose_center(self) -> PoseObjectPNP:
        """Geometric center pose."""
        if self._pose_center is None:
            raise ValueError("Pose center not initialized")
        return self._pose_center

    def x_center(self) -> float:
        """Geometric center x (meters)."""
        return self.pose_center().x

    def y_center(self) -> float:
        """Geometric center y (meters)."""
        return self.pose_center().y

    def xy_center(self) -> tuple[float, float]:
        """Geometric center (x, y) (meters)."""
        pc = self.pose_center()
        return pc.x, pc.y

    def pose_com(self) -> PoseObjectPNP:
        """Center of mass pose."""
        if self._pose_com is None:
            raise ValueError("Pose CoM not initialized")
        return self._pose_com

    def x_com(self) -> float:
        """Center of mass x (meters)."""
        return self.pose_com().x

    def y_com(self) -> float:
        """Center of mass y (meters)."""
        return self.pose_com().y

    def xy_com(self) -> tuple[float, float]:
        """Center of mass (x, y) (meters)."""
        pcom = self.pose_com()
        return pcom.x, pcom.y

    def coordinate(self) -> list[float]:
        """Center of mass (x, y) (meters)."""
        pcom = self.pose_com()
        return [pcom.x, pcom.y]

    def shape_m(self) -> tuple[float, float]:
        """Width and height in meters."""
        return self._width_m, self._height_m

    def width_m(self) -> float:
        """Width in meters."""
        return self._width_m

    def height_m(self) -> float:
        """Height in meters."""
        return self._height_m

    def size_m2(self) -> float:
        """Area in square meters."""
        return self._size_m2

    def largest_contour(self) -> np.ndarray | None:
        """Largest contour of mask."""
        return self._largest_contour

    def gripper_rotation(self) -> float:
        """Gripper rotation (radians)."""
        return self._gripper_rotation

    def workspace(self) -> Workspace:
        """Associated workspace."""
        return self._workspace

    def verbose(self) -> bool:
        """Verbose mode status."""
        return self._verbose

    # *** PRIVATE variables ***

    _label: str = ""
    _u_rel_o: float = 0.0
    _v_rel_o: float = 0.0
    _pose_center: PoseObjectPNP | None = None
    _u_rel_com: float = 0.0
    _v_rel_com: float = 0.0
    _pose_com: PoseObjectPNP | None = None
    _u_rel_min: float = 0.0
    _v_rel_min: float = 0.0
    _u_rel_max: float = 0.0
    _v_rel_max: float = 0.0
    _width: float = 0.0
    _height: float = 0.0
    _width_m: float = 0.0
    _height_m: float = 0.0
    _depth_m: float = 0.0
    _size_m2: float = 0.0
    _largest_contour: np.ndarray | None = None
    _gripper_rotation: float = 0.0
    _workspace: Workspace = None
    _original_mask_8u: np.ndarray | None = None
    _verbose: bool = False
    _confidence: float = 1.0
    _class_id: int = 0

Functions

__init__(label, u_min, v_min, u_max, v_max, mask_8u, workspace, verbose=False)

Initializes an Object instance.

Parameters:

Name	Type	Description	Default
label	str	Label of the object (e.g., "chocolate bar").	required
u_min	int	Upper-left corner u-coordinate of the bounding box (in pixels).	required
v_min	int	Upper-left corner v-coordinate of the bounding box (in pixels).	required
u_max	int	Lower-right corner u-coordinate of the bounding box (in pixels).	required
v_max	int	Lower-right corner v-coordinate of the bounding box (in pixels).	required
mask_8u	ndarray	Segmentation mask of the object (8-bit, uint8).	required
workspace	Workspace	Workspace instance where the object is located.	required
verbose	bool	If True, enables verbose logging.	False

Raises:

Type	Description
ValueError	If the provided segmentation mask is not 8-bit unsigned, or if the workspace has no image shape.

Source code in robot_workspace/objects/object.py

@log_start_end_cls()
def __init__(
    self,
    label: str,
    u_min: int,
    v_min: int,
    u_max: int,
    v_max: int,
    mask_8u: np.ndarray | None,
    workspace: Workspace,
    verbose: bool = False,
) -> None:
    """
    Initializes an Object instance.

    Args:
        label (str): Label of the object (e.g., "chocolate bar").
        u_min (int): Upper-left corner u-coordinate of the bounding box (in pixels).
        v_min (int): Upper-left corner v-coordinate of the bounding box (in pixels).
        u_max (int): Lower-right corner u-coordinate of the bounding box (in pixels).
        v_max (int): Lower-right corner v-coordinate of the bounding box (in pixels).
        mask_8u (np.ndarray, optional): Segmentation mask of the object (8-bit, uint8).
        workspace (Workspace): Workspace instance where the object is located.
        verbose (bool): If True, enables verbose logging.

    Raises:
        ValueError: If the provided segmentation mask is not 8-bit unsigned,
            or if the workspace has no image shape.
    """
    super().__init__()

    self._label = label
    self._workspace = workspace
    self._verbose = verbose
    self._logger = logging.getLogger("robot_workspace")

    if self._workspace.img_shape() is None:
        raise ValueError("Object has no image shape. This probably means that the Niryo Workspace was not detected.")

    # Store original mask for rotation
    self._original_mask_8u = mask_8u.copy() if mask_8u is not None else None

    # Initialize the object with the common initialization logic
    self._init_object_properties(u_min, v_min, u_max, v_max, mask_8u)

as_string_for_chat_window()

Formats object details for display in a chat interface.

Returns:

Name	Type	Description
str	str	Chat-friendly object description.

Source code in robot_workspace/objects/object.py

def as_string_for_chat_window(self) -> str:
    """
    Formats object details for display in a chat interface.

    Returns:
        str: Chat-friendly object description.
    """
    return f"""Detected a new object: {self.label()} at world coordinate ({self.x_com():.2f}, {
    self.y_com():.2f}) with orientation {self.gripper_rotation():.1f} rad and size {
    self.width_m():.2f} m x {self.height_m():.2f} m."""

as_string_for_llm()

Formats object details as a string for use with language models.

Returns:

Name	Type	Description
str	str	String containing object information.

Source code in robot_workspace/objects/object.py

def as_string_for_llm(self) -> str:
    """
    Formats object details as a string for use with language models.

    Returns:
        str: String containing object information.
    """
    return f"""- '{self.label()}' at world coordinates [{self.x_com():.2f}, {self.y_com():.2f}] with a width of {
    self.width_m():.2f} meters, a height of {self.height_m():.2f} meters and a size of {
    self.size_m2()*10000:.2f} square centimeters."""

as_string_for_llm_lbl()

Formats object details in a line-by-line style, optimized for language model usage.

Returns:

Name	Type	Description
str	str	Detailed object information string.

Source code in robot_workspace/objects/object.py

def as_string_for_llm_lbl(self) -> str:
    """
    Formats object details in a line-by-line style, optimized for language model usage.

    Returns:
        str: Detailed object information string.
    """
    return f"""- '{self.label()}' at world coordinates [{self.x_com():.2f}, {self.y_com():.2f}] with
- width: {self.width_m():.2f} meters,
- height: {self.height_m():.2f} meters and
- size: {self.size_m2() * 10000:.2f} square centimeters."""

calc_width_height(pose_ul, pose_lr) staticmethod

Calculates the width and the height between two PoseObjects in meters.

Parameters:

Name	Type	Description	Default
pose_ul	PoseObjectPNP	PoseObject in the upper left corner.	required
pose_lr	PoseObjectPNP	PoseObject in the lower right corner.	required

Returns:

Type	Description
tuple[float, float]	tuple[float, float]: (width, height) in meters.

Source code in robot_workspace/objects/object.py

@staticmethod
def calc_width_height(pose_ul: PoseObjectPNP, pose_lr: PoseObjectPNP) -> tuple[float, float]:
    """
    Calculates the width and the height between two PoseObjects in meters.

    Args:
        pose_ul (PoseObjectPNP): PoseObject in the upper left corner.
        pose_lr (PoseObjectPNP): PoseObject in the lower right corner.

    Returns:
        tuple[float, float]: (width, height) in meters.
    """
    width_m = pose_ul.y - pose_lr.y
    height_m = pose_ul.x - pose_lr.x

    return width_m, height_m

coordinate()

Center of mass (x, y) (meters).

Source code in robot_workspace/objects/object.py

def coordinate(self) -> list[float]:
    """Center of mass (x, y) (meters)."""
    pcom = self.pose_com()
    return [pcom.x, pcom.y]

from_dict(data, workspace) classmethod

Creates an Object instance from a dictionary.

Parameters:

Name	Type	Description	Default
data	dict[str, Any]	Dictionary containing object data.	required
workspace	Workspace	Workspace instance.	required

Returns:

Type	Description
Object | None	Object | None: Reconstructed object instance or None if fails.

Source code in robot_workspace/objects/object.py

@classmethod
def from_dict(cls, data: dict[str, Any], workspace: Workspace) -> Object | None:
    """
    Creates an Object instance from a dictionary.

    Args:
        data (dict[str, Any]): Dictionary containing object data.
        workspace (Workspace): Workspace instance.

    Returns:
        Object | None: Reconstructed object instance or None if fails.
    """
    logger = logging.getLogger("robot_workspace")
    try:
        # Check if we have the new format (from to_dict) or old format (with bbox key)
        if "bbox" in data:
            # Old format - has bbox directly
            bbox = data["bbox"]
            u_min = bbox["x_min"]
            v_min = bbox["y_min"]
            u_max = bbox["x_max"]
            v_max = bbox["y_max"]
        elif "image_coordinates" in data and "bounding_box_rel" in data["image_coordinates"]:
            # New format - convert from relative coordinates
            bbox_rel = data["image_coordinates"]["bounding_box_rel"]
            img_shape = workspace.img_shape()
            if img_shape is None:
                raise ValueError("Workspace image shape is None")

            u_min = int(bbox_rel["u_min"] * img_shape[0])
            v_min = int(bbox_rel["v_min"] * img_shape[1])
            u_max = int(bbox_rel["u_max"] * img_shape[0])
            v_max = int(bbox_rel["v_max"] * img_shape[1])
        else:
            raise KeyError("Missing bounding box information in dictionary")

        # Handle mask if present
        mask_8u = None
        if data.get("has_mask", False) and "mask_data" in data and "mask_shape" in data:
            mask_8u = cls._deserialize_mask(data["mask_data"], data["mask_shape"], data.get("mask_dtype", "uint8"))

        # Create object
        obj = Object(
            label=data["label"],
            u_min=u_min,
            v_min=v_min,
            u_max=u_max,
            v_max=v_max,
            mask_8u=mask_8u,
            workspace=workspace,
        )

        # Restore additional properties if needed
        if "confidence" in data:
            obj._confidence = data["confidence"]
        if "class_id" in data:
            obj._class_id = data["class_id"]

        return obj

    except Exception as e:
        logger.error(f"Error reconstructing object from dict: {e}")
        return None

from_json(json_str, workspace) classmethod

Creates an Object instance from a JSON string.

Parameters:

Name	Type	Description	Default
json_str	str	JSON string containing object data.	required
workspace	Workspace	Workspace instance.	required

Returns:

Type	Description
Object | None	Object | None: Reconstructed object instance or None if fails.

Source code in robot_workspace/objects/object.py

@classmethod
def from_json(cls, json_str: str, workspace: Workspace) -> Object | None:
    """
    Creates an Object instance from a JSON string.

    Args:
        json_str (str): JSON string containing object data.
        workspace (Workspace): Workspace instance.

    Returns:
        Object | None: Reconstructed object instance or None if fails.
    """
    logger = logging.getLogger("robot_workspace")
    try:
        data = json.loads(json_str)
        return cls.from_dict(data, workspace)
    except Exception as e:
        logger.error(f"Error parsing JSON: {e}")
        return None

generate_object_id()

Generates a unique ID for the object based on its properties.

Returns:

Name	Type	Description
str	str	Unique object identifier.

Source code in robot_workspace/objects/object.py

def generate_object_id(self) -> str:
    """
    Generates a unique ID for the object based on its properties.

    Returns:
        str: Unique object identifier.
    """
    id_string = f"{self._label}_{self.x_com():.3f}_{self.y_com():.3f}_{time.time()}"
    return hashlib.md5(id_string.encode(), usedforsecurity=False).hexdigest()[:8]

get_workspace_id()

Returns the ID of the workspace containing the object.

Returns:

Name	Type	Description
str	str	Workspace ID.

Source code in robot_workspace/objects/object.py

def get_workspace_id(self) -> str:
    """
    Returns the ID of the workspace containing the object.

    Returns:
        str: Workspace ID.
    """
    return self.workspace().id()

gripper_rotation()

Gripper rotation (radians).

Source code in robot_workspace/objects/object.py

def gripper_rotation(self) -> float:
    """Gripper rotation (radians)."""
    return self._gripper_rotation

height_m()

Height in meters.

Source code in robot_workspace/objects/object.py

def height_m(self) -> float:
    """Height in meters."""
    return self._height_m

label()

Object label.

Source code in robot_workspace/objects/object.py

def label(self) -> str:
    """Object label."""
    return self._label

largest_contour()

Largest contour of mask.

Source code in robot_workspace/objects/object.py

def largest_contour(self) -> np.ndarray | None:
    """Largest contour of mask."""
    return self._largest_contour

pose_center()

Geometric center pose.

Source code in robot_workspace/objects/object.py

def pose_center(self) -> PoseObjectPNP:
    """Geometric center pose."""
    if self._pose_center is None:
        raise ValueError("Pose center not initialized")
    return self._pose_center

pose_com()

Center of mass pose.

Source code in robot_workspace/objects/object.py

def pose_com(self) -> PoseObjectPNP:
    """Center of mass pose."""
    if self._pose_com is None:
        raise ValueError("Pose CoM not initialized")
    return self._pose_com

set_pose_com(pose_com)

Updates the object's center of mass pose and recalculates all dependent properties.

Handles both position changes and orientation changes (rotation around z-axis).

Parameters:

Name	Type	Description	Default
pose_com	PoseObjectPNP	New pose for the object's center of mass.	required

Source code in robot_workspace/objects/object.py

def set_pose_com(self, pose_com: PoseObjectPNP) -> None:
    """
    Updates the object's center of mass pose and recalculates all dependent properties.

    Handles both position changes and orientation changes (rotation around z-axis).

    Args:
        pose_com (PoseObjectPNP): New pose for the object's center of mass.
    """
    if self.verbose():
        self._logger.debug(f"set_pose_com: {self._pose_com} → {pose_com}")

    # 1. Calculate transformations
    rotation_delta = pose_com.yaw - self._gripper_rotation
    translation = self._calculate_translation(pose_com)

    # 2. Transform bounding box
    new_u_min, new_v_min, new_u_max, new_v_max = self._apply_pose_transform_to_bbox(rotation_delta, translation)

    # 3. Transform mask if present
    transformed_mask = self._apply_pose_transform_to_mask(rotation_delta, translation)

    # 4. Reinitialize with new geometry
    self._init_object_properties(new_u_min, new_v_min, new_u_max, new_v_max, transformed_mask)

    # 5. Override calculated pose with exact provided pose
    self._pose_com = pose_com
    self._gripper_rotation = pose_com.yaw

set_position(xy_coordinate)

Legacy method kept for backwards compatibility.

Parameters:

Name	Type	Description	Default
xy_coordinate	list[float]	[x, y] world coordinates in meters.	required

Source code in robot_workspace/objects/object.py

def set_position(self, xy_coordinate: list[float]) -> None:
    """
    Legacy method kept for backwards compatibility.

    Args:
        xy_coordinate (list[float]): [x, y] world coordinates in meters.
    """
    # Create a pose with the new x, y but keeping old z and orientation
    new_pose = self._pose_com.copy_with_offsets(
        x_offset=xy_coordinate[0] - self._pose_com.x, y_offset=xy_coordinate[1] - self._pose_com.y
    )
    self.set_pose_com(new_pose)

shape_m()

Width and height in meters.

Source code in robot_workspace/objects/object.py

def shape_m(self) -> tuple[float, float]:
    """Width and height in meters."""
    return self._width_m, self._height_m

size_m2()

Area in square meters.

Source code in robot_workspace/objects/object.py

def size_m2(self) -> float:
    """Area in square meters."""
    return self._size_m2

to_dict()

Converts the Object instance to a dictionary that can be JSON serialized.

Returns:

Type	Description
dict[str, Any]	dict[str, Any]: Dictionary representation of the object.

Source code in robot_workspace/objects/object.py

def to_dict(self) -> dict[str, Any]:
    """
    Converts the Object instance to a dictionary that can be JSON serialized.

    Returns:
        dict[str, Any]: Dictionary representation of the object.
    """
    return {
        "id": self.generate_object_id(),
        "label": self._label,
        "workspace_id": self.get_workspace_id(),
        "timestamp": time.time(),
        "position": {
            "center_of_mass": {
                "x": float(self.x_com()),
                "y": float(self.y_com()),
                "z": float(self._pose_com.z) if hasattr(self._pose_com, "z") else 0.0,
            },
            "center": {
                "x": float(self.x_center()),
                "y": float(self.y_center()),
                "z": float(self._pose_center.z) if hasattr(self._pose_center, "z") else 0.0,
            },
        },
        "image_coordinates": {
            "center_rel": {"u": float(self._u_rel_o), "v": float(self._v_rel_o)},
            "center_of_mass_rel": {"u": float(self._u_rel_com), "v": float(self._v_rel_com)},
            "bounding_box_rel": {
                "u_min": float(self._u_rel_min),
                "v_min": float(self._v_rel_min),
                "u_max": float(self._u_rel_max),
                "v_max": float(self._v_rel_max),
            },
        },
        "dimensions": {
            "width_m": float(self._width_m),
            "height_m": float(self._height_m),
            "size_m2": float(self._size_m2),
        },
        "gripper_rotation": float(self._gripper_rotation),
        "confidence": getattr(self, "_confidence", 1.0),
        "class_id": getattr(self, "_class_id", 0),
    }

to_json()

Converts the Object instance to a JSON string.

Returns:

Name	Type	Description
str	str	JSON representation.

Source code in robot_workspace/objects/object.py

def to_json(self) -> str:
    """
    Converts the Object instance to a JSON string.

    Returns:
        str: JSON representation.
    """
    return json.dumps(self.to_dict(), indent=2)

u_rel_o()

Object center u-coordinate.

Source code in robot_workspace/objects/object.py

def u_rel_o(self) -> float:
    """Object center u-coordinate."""
    return self._u_rel_o

uv_rel_o()

Object center in relative coordinates.

Source code in robot_workspace/objects/object.py

def uv_rel_o(self) -> tuple[float, float]:
    """Object center in relative coordinates."""
    return self._u_rel_o, self._v_rel_o

v_rel_o()

Object center v-coordinate.

Source code in robot_workspace/objects/object.py

def v_rel_o(self) -> float:
    """Object center v-coordinate."""
    return self._v_rel_o

verbose()

Verbose mode status.

Source code in robot_workspace/objects/object.py

def verbose(self) -> bool:
    """Verbose mode status."""
    return self._verbose

width_m()

Width in meters.

Source code in robot_workspace/objects/object.py

def width_m(self) -> float:
    """Width in meters."""
    return self._width_m

workspace()

Associated workspace.

Source code in robot_workspace/objects/object.py

def workspace(self) -> Workspace:
    """Associated workspace."""
    return self._workspace

x_center()

Geometric center x (meters).

Source code in robot_workspace/objects/object.py

def x_center(self) -> float:
    """Geometric center x (meters)."""
    return self.pose_center().x

x_com()

Center of mass x (meters).

Source code in robot_workspace/objects/object.py

def x_com(self) -> float:
    """Center of mass x (meters)."""
    return self.pose_com().x

xy_center()

Geometric center (x, y) (meters).

Source code in robot_workspace/objects/object.py

def xy_center(self) -> tuple[float, float]:
    """Geometric center (x, y) (meters)."""
    pc = self.pose_center()
    return pc.x, pc.y

xy_com()

Center of mass (x, y) (meters).

Source code in robot_workspace/objects/object.py

def xy_com(self) -> tuple[float, float]:
    """Center of mass (x, y) (meters)."""
    pcom = self.pose_com()
    return pcom.x, pcom.y

y_center()

Geometric center y (meters).

Source code in robot_workspace/objects/object.py

def y_center(self) -> float:
    """Geometric center y (meters)."""
    return self.pose_center().y

y_com()

Center of mass y (meters).

Source code in robot_workspace/objects/object.py

def y_com(self) -> float:
    """Center of mass y (meters)."""
    return self.pose_com().y

Functions

robot_workspace.objects.objects

Classes

Objects

Bases: list[Object]

A class representing a list of Object instances.

Objects are typically stored in a workspace and represent physical entities detected by vision. This class provides several spatial query methods to find objects based on coordinates, labels, or size.

Source code in robot_workspace/objects/objects.py

class Objects(list[Object]):
    """
    A class representing a list of Object instances.

    Objects are typically stored in a workspace and represent physical entities detected by vision.
    This class provides several spatial query methods to find objects based on coordinates, labels, or size.
    """

    # *** CONSTRUCTORS ***
    def __init__(self, iterable: Iterable[Object] | None = None, verbose: bool = False) -> None:
        """
        Initializes the Objects instance.

        Args:
            iterable (Iterable[Object], optional): An iterable of Object instances. Defaults to an empty list.
            verbose (bool): If True, enables verbose logging.
        """
        if iterable is None:
            iterable = []
        super().__init__(iterable)

        self._verbose = verbose

    # *** PUBLIC SET methods ***

    # *** PUBLIC GET methods ***

    # *** PUBLIC methods ***

    @log_start_end_cls()
    def get_detected_object(
        self, coordinate: list[float], label: str | None = None, serializable: bool = False
    ) -> Object | dict[str, Any] | None:
        """
        Retrieves a detected object at or near a specified world coordinate, optionally filtering by label.

        Checks for objects that are within a 2-centimeter radius of the specified coordinate.
        If multiple objects meet the criteria, the first one found is returned.

        Args:
            coordinate (list[float]): A 2D coordinate in world units [x, y].
            label (str, optional): An optional filter for the object's label.
            serializable (bool): If True, returns a dictionary representation instead of an Object instance.

        Returns:
            Object | dict[str, Any] | None: The first object detected near the given coordinate, or None if not found.
        """
        detected_objects = self.get_detected_objects(Location.CLOSE_TO, coordinate, label)

        if detected_objects:
            res: Object | dict[str, Any] = detected_objects[0].to_dict() if serializable else detected_objects[0]
            return res
        else:
            return None

    def get_detected_objects(
        self,
        location: Location | str = Location.NONE,
        coordinate: list[float] | None = None,
        label: str | None = None,
    ) -> Objects:
        """
        Returns a list of objects filtered by spatial location, coordinate, and label.

        Args:
            location (Location | str): Spatial filter. Values can be "left next to", "right next to",
                "above", "below", "close to", or Location enum equivalents.
            coordinate (list[float], optional): (x, y) coordinate in meters used for spatial filtering.
                Required if 'location' is not NONE.
            label (str, optional): Filter by object label (substring match).

        Returns:
            Objects: A collection of filtered objects.

        Raises:
            ValueError: If coordinate is missing but required for the specified location filter.
        """
        detected_objects = self

        if label is not None:
            detected_objects = Objects(obj for obj in self if label in obj.label())

        location = Location.convert_str2location(location)

        if location is Location.NONE:
            return detected_objects

        if coordinate is None:
            raise ValueError(f"Coordinate must be provided for location filter: {location}")

        if location == Location.LEFT_NEXT_TO:
            return Objects(obj for obj in detected_objects if obj.y_com() > coordinate[1])
        elif location == Location.RIGHT_NEXT_TO:
            return Objects(obj for obj in detected_objects if obj.y_com() < coordinate[1])
        elif location == Location.ABOVE:
            return Objects(obj for obj in detected_objects if obj.x_com() > coordinate[0])
        elif location == Location.BELOW:
            return Objects(obj for obj in detected_objects if obj.x_com() < coordinate[0])
        elif location == Location.CLOSE_TO:
            return Objects(
                obj
                for obj in detected_objects
                if np.sqrt((obj.x_com() - coordinate[0]) ** 2 + (obj.y_com() - coordinate[1]) ** 2) <= 0.02
            )
        else:
            print("Error in get_detected_objects: Unknown Location:", location)
            return Objects()

    def get_detected_objects_serializable(
        self,
        location: Location | str = Location.NONE,
        coordinate: list[float] | None = None,
        label: str | None = None,
    ) -> list[dict[str, Any]]:
        """
        Similar to get_detected_objects but returns a list of dictionaries.

        Args:
            location (Location | str): Spatial filter.
            coordinate (list[float], optional): Reference (x, y) coordinate.
            label (str, optional): Filter by object label.

        Returns:
            list[dict[str, Any]]: List of dictionary representations of the filtered objects.
        """
        detected_objects = self.get_detected_objects(location, coordinate, label)

        objects = Objects.objects_to_dict_list(detected_objects)

        return objects

    def get_nearest_detected_object(self, coordinate: list[float], label: str | None = None) -> tuple[Object | None, float]:
        """
        Finds the object nearest to a specified coordinate.

        Args:
            coordinate (list[float]): Target (x, y) coordinate.
            label (str, optional): If specified, only consider objects with this label.

        Returns:
            tuple[Object | None, float]: (nearest_object, distance_in_meters).
        """
        nearest_object = None
        min_distance = float("inf")

        for obj in self:
            if label is None or obj.label() == label:
                # Calculate Euclidean distance
                distance = math.sqrt((obj.x_com() - coordinate[0]) ** 2 + (obj.y_com() - coordinate[1]) ** 2)
                # print(distance, min_distance)
                if distance < min_distance:
                    min_distance = distance
                    nearest_object = obj

        return nearest_object, min_distance

    def get_detected_objects_as_comma_separated_string(self) -> str:
        """
        Returns the labels of all detected objects as a comma-separated string.

        Returns:
            str: Comma-separated labels.
        """
        return f"""{', '.join(f"'{item.label()}'" for item in self)}"""

    def get_largest_detected_object(self, serializable: bool = False) -> tuple[Object, float] | tuple[dict[str, Any], float]:
        """
        Identifies the largest object by area.

        Args:
            serializable (bool): If True, returns a dictionary instead of an Object.

        Returns:
            tuple: (largest_object, area_m2).
        """
        largest_object = max(self, key=lambda obj: obj.size_m2())

        size = largest_object.size_m2()

        if serializable:
            return largest_object.to_dict(), size
        else:
            return largest_object, size

    def get_smallest_detected_object(self, serializable: bool = False) -> tuple[Object, float] | tuple[dict[str, Any], float]:
        """
        Identifies the smallest object by area.

        Args:
            serializable (bool): If True, returns a dictionary instead of an Object.

        Returns:
            tuple: (smallest_object, area_m2).
        """
        smallest_object = min(self, key=lambda obj: obj.size_m2())

        size = smallest_object.size_m2()

        if serializable:
            return smallest_object.to_dict(), size
        else:
            return smallest_object, size

    def get_detected_objects_sorted(
        self, ascending: bool = True, serializable: bool = False
    ) -> Objects | list[dict[str, Any]]:
        """
        Returns objects sorted by their size.

        Args:
            ascending (bool): Sorting order. Defaults to True.
            serializable (bool): If True, returns a list of dictionaries.

        Returns:
            Objects | list[dict[str, Any]]: Sorted collection of objects.
        """
        sorted_objs = Objects(sorted(self, key=lambda obj: obj.size_m2(), reverse=not ascending))

        if serializable:
            return Objects.objects_to_dict_list(sorted_objs)
        else:
            return sorted_objs

    # *** PUBLIC methods ***

    # *** PUBLIC STATIC/CLASS GET methods ***

    # *** HELPER METHODE FÜR REDIS PUBLISHER ***

    @staticmethod
    def objects_to_dict_list(objects: list[Object]) -> list[dict[str, Any]]:
        """
        Converts a list of Object instances to a list of dictionaries.

        Args:
            objects (list[Object]): List of Object instances.

        Returns:
            list[dict[str, Any]]: List of dictionary representations.
        """
        return [obj.to_dict() for obj in objects]

    @staticmethod
    def dict_list_to_objects(dict_list: list[dict[str, Any]], workspace: Workspace) -> Objects:
        """
        Reconstructs an Objects collection from a list of dictionaries.

        Args:
            dict_list (list[dict[str, Any]]): List of object dictionaries.
            workspace (Workspace): The workspace to associate with the objects.

        Returns:
            Objects: A collection of reconstructed objects.
        """
        objects = Objects()
        for obj_dict in dict_list:
            obj = Object.from_dict(obj_dict, workspace)
            if obj is not None:
                objects.append(obj)
        return objects

    # *** PRIVATE methods ***

    # *** PUBLIC properties ***

    def verbose(self) -> bool:
        """
        Returns whether verbose logging is enabled.

        Returns:
            bool: True if verbose, else False.
        """
        return self._verbose

    # *** PRIVATE variables ***

    _verbose = False

Functions

__init__(iterable=None, verbose=False)

Initializes the Objects instance.

Parameters:

Name	Type	Description	Default
iterable	Iterable[Object]	An iterable of Object instances. Defaults to an empty list.	None
verbose	bool	If True, enables verbose logging.	False

Source code in robot_workspace/objects/objects.py

def __init__(self, iterable: Iterable[Object] | None = None, verbose: bool = False) -> None:
    """
    Initializes the Objects instance.

    Args:
        iterable (Iterable[Object], optional): An iterable of Object instances. Defaults to an empty list.
        verbose (bool): If True, enables verbose logging.
    """
    if iterable is None:
        iterable = []
    super().__init__(iterable)

    self._verbose = verbose

dict_list_to_objects(dict_list, workspace) staticmethod

Reconstructs an Objects collection from a list of dictionaries.

Parameters:

Name	Type	Description	Default
dict_list	list[dict[str, Any]]	List of object dictionaries.	required
workspace	Workspace	The workspace to associate with the objects.	required

Returns:

Name	Type	Description
Objects	Objects	A collection of reconstructed objects.

Source code in robot_workspace/objects/objects.py

@staticmethod
def dict_list_to_objects(dict_list: list[dict[str, Any]], workspace: Workspace) -> Objects:
    """
    Reconstructs an Objects collection from a list of dictionaries.

    Args:
        dict_list (list[dict[str, Any]]): List of object dictionaries.
        workspace (Workspace): The workspace to associate with the objects.

    Returns:
        Objects: A collection of reconstructed objects.
    """
    objects = Objects()
    for obj_dict in dict_list:
        obj = Object.from_dict(obj_dict, workspace)
        if obj is not None:
            objects.append(obj)
    return objects

get_detected_object(coordinate, label=None, serializable=False)

Retrieves a detected object at or near a specified world coordinate, optionally filtering by label.

Checks for objects that are within a 2-centimeter radius of the specified coordinate. If multiple objects meet the criteria, the first one found is returned.

Parameters:

Name	Type	Description	Default
coordinate	list[float]	A 2D coordinate in world units [x, y].	required
label	str	An optional filter for the object's label.	None
serializable	bool	If True, returns a dictionary representation instead of an Object instance.	False

Returns:

Type	Description
Object | dict[str, Any] | None	Object | dict[str, Any] | None: The first object detected near the given coordinate, or None if not found.

Source code in robot_workspace/objects/objects.py

@log_start_end_cls()
def get_detected_object(
    self, coordinate: list[float], label: str | None = None, serializable: bool = False
) -> Object | dict[str, Any] | None:
    """
    Retrieves a detected object at or near a specified world coordinate, optionally filtering by label.

    Checks for objects that are within a 2-centimeter radius of the specified coordinate.
    If multiple objects meet the criteria, the first one found is returned.

    Args:
        coordinate (list[float]): A 2D coordinate in world units [x, y].
        label (str, optional): An optional filter for the object's label.
        serializable (bool): If True, returns a dictionary representation instead of an Object instance.

    Returns:
        Object | dict[str, Any] | None: The first object detected near the given coordinate, or None if not found.
    """
    detected_objects = self.get_detected_objects(Location.CLOSE_TO, coordinate, label)

    if detected_objects:
        res: Object | dict[str, Any] = detected_objects[0].to_dict() if serializable else detected_objects[0]
        return res
    else:
        return None

get_detected_objects(location=Location.NONE, coordinate=None, label=None)

Returns a list of objects filtered by spatial location, coordinate, and label.

Parameters:

Name	Type	Description	Default
location	Location | str	Spatial filter. Values can be "left next to", "right next to", "above", "below", "close to", or Location enum equivalents.	NONE
coordinate	list[float]	(x, y) coordinate in meters used for spatial filtering. Required if 'location' is not NONE.	None
label	str	Filter by object label (substring match).	None

Returns:

Name	Type	Description
Objects	Objects	A collection of filtered objects.

Raises:

Type	Description
ValueError	If coordinate is missing but required for the specified location filter.

Source code in robot_workspace/objects/objects.py

def get_detected_objects(
    self,
    location: Location | str = Location.NONE,
    coordinate: list[float] | None = None,
    label: str | None = None,
) -> Objects:
    """
    Returns a list of objects filtered by spatial location, coordinate, and label.

    Args:
        location (Location | str): Spatial filter. Values can be "left next to", "right next to",
            "above", "below", "close to", or Location enum equivalents.
        coordinate (list[float], optional): (x, y) coordinate in meters used for spatial filtering.
            Required if 'location' is not NONE.
        label (str, optional): Filter by object label (substring match).

    Returns:
        Objects: A collection of filtered objects.

    Raises:
        ValueError: If coordinate is missing but required for the specified location filter.
    """
    detected_objects = self

    if label is not None:
        detected_objects = Objects(obj for obj in self if label in obj.label())

    location = Location.convert_str2location(location)

    if location is Location.NONE:
        return detected_objects

    if coordinate is None:
        raise ValueError(f"Coordinate must be provided for location filter: {location}")

    if location == Location.LEFT_NEXT_TO:
        return Objects(obj for obj in detected_objects if obj.y_com() > coordinate[1])
    elif location == Location.RIGHT_NEXT_TO:
        return Objects(obj for obj in detected_objects if obj.y_com() < coordinate[1])
    elif location == Location.ABOVE:
        return Objects(obj for obj in detected_objects if obj.x_com() > coordinate[0])
    elif location == Location.BELOW:
        return Objects(obj for obj in detected_objects if obj.x_com() < coordinate[0])
    elif location == Location.CLOSE_TO:
        return Objects(
            obj
            for obj in detected_objects
            if np.sqrt((obj.x_com() - coordinate[0]) ** 2 + (obj.y_com() - coordinate[1]) ** 2) <= 0.02
        )
    else:
        print("Error in get_detected_objects: Unknown Location:", location)
        return Objects()

get_detected_objects_as_comma_separated_string()

Returns the labels of all detected objects as a comma-separated string.

Returns:

Name	Type	Description
str	str	Comma-separated labels.

Source code in robot_workspace/objects/objects.py

def get_detected_objects_as_comma_separated_string(self) -> str:
    """
    Returns the labels of all detected objects as a comma-separated string.

    Returns:
        str: Comma-separated labels.
    """
    return f"""{', '.join(f"'{item.label()}'" for item in self)}"""

get_detected_objects_serializable(location=Location.NONE, coordinate=None, label=None)

Similar to get_detected_objects but returns a list of dictionaries.

Parameters:

Name	Type	Description	Default
location	Location | str	Spatial filter.	NONE
coordinate	list[float]	Reference (x, y) coordinate.	None
label	str	Filter by object label.	None

Returns:

Type	Description
list[dict[str, Any]]	list[dict[str, Any]]: List of dictionary representations of the filtered objects.

Source code in robot_workspace/objects/objects.py

def get_detected_objects_serializable(
    self,
    location: Location | str = Location.NONE,
    coordinate: list[float] | None = None,
    label: str | None = None,
) -> list[dict[str, Any]]:
    """
    Similar to get_detected_objects but returns a list of dictionaries.

    Args:
        location (Location | str): Spatial filter.
        coordinate (list[float], optional): Reference (x, y) coordinate.
        label (str, optional): Filter by object label.

    Returns:
        list[dict[str, Any]]: List of dictionary representations of the filtered objects.
    """
    detected_objects = self.get_detected_objects(location, coordinate, label)

    objects = Objects.objects_to_dict_list(detected_objects)

    return objects

get_detected_objects_sorted(ascending=True, serializable=False)

Returns objects sorted by their size.

Parameters:

Name	Type	Description	Default
ascending	bool	Sorting order. Defaults to True.	True
serializable	bool	If True, returns a list of dictionaries.	False

Returns:

Type	Description
Objects | list[dict[str, Any]]	Objects | list[dict[str, Any]]: Sorted collection of objects.

Source code in robot_workspace/objects/objects.py

def get_detected_objects_sorted(
    self, ascending: bool = True, serializable: bool = False
) -> Objects | list[dict[str, Any]]:
    """
    Returns objects sorted by their size.

    Args:
        ascending (bool): Sorting order. Defaults to True.
        serializable (bool): If True, returns a list of dictionaries.

    Returns:
        Objects | list[dict[str, Any]]: Sorted collection of objects.
    """
    sorted_objs = Objects(sorted(self, key=lambda obj: obj.size_m2(), reverse=not ascending))

    if serializable:
        return Objects.objects_to_dict_list(sorted_objs)
    else:
        return sorted_objs

get_largest_detected_object(serializable=False)

Identifies the largest object by area.

Parameters:

Name	Type	Description	Default
serializable	bool	If True, returns a dictionary instead of an Object.	False

Returns:

Name	Type	Description
tuple	tuple[Object, float] | tuple[dict[str, Any], float]	(largest_object, area_m2).

Source code in robot_workspace/objects/objects.py

def get_largest_detected_object(self, serializable: bool = False) -> tuple[Object, float] | tuple[dict[str, Any], float]:
    """
    Identifies the largest object by area.

    Args:
        serializable (bool): If True, returns a dictionary instead of an Object.

    Returns:
        tuple: (largest_object, area_m2).
    """
    largest_object = max(self, key=lambda obj: obj.size_m2())

    size = largest_object.size_m2()

    if serializable:
        return largest_object.to_dict(), size
    else:
        return largest_object, size

get_nearest_detected_object(coordinate, label=None)

Finds the object nearest to a specified coordinate.

Parameters:

Name	Type	Description	Default
coordinate	list[float]	Target (x, y) coordinate.	required
label	str	If specified, only consider objects with this label.	None

Returns:

Type	Description
tuple[Object | None, float]	tuple[Object | None, float]: (nearest_object, distance_in_meters).

Source code in robot_workspace/objects/objects.py

def get_nearest_detected_object(self, coordinate: list[float], label: str | None = None) -> tuple[Object | None, float]:
    """
    Finds the object nearest to a specified coordinate.

    Args:
        coordinate (list[float]): Target (x, y) coordinate.
        label (str, optional): If specified, only consider objects with this label.

    Returns:
        tuple[Object | None, float]: (nearest_object, distance_in_meters).
    """
    nearest_object = None
    min_distance = float("inf")

    for obj in self:
        if label is None or obj.label() == label:
            # Calculate Euclidean distance
            distance = math.sqrt((obj.x_com() - coordinate[0]) ** 2 + (obj.y_com() - coordinate[1]) ** 2)
            # print(distance, min_distance)
            if distance < min_distance:
                min_distance = distance
                nearest_object = obj

    return nearest_object, min_distance

get_smallest_detected_object(serializable=False)

Identifies the smallest object by area.

Parameters:

Name	Type	Description	Default
serializable	bool	If True, returns a dictionary instead of an Object.	False

Returns:

Name	Type	Description
tuple	tuple[Object, float] | tuple[dict[str, Any], float]	(smallest_object, area_m2).

Source code in robot_workspace/objects/objects.py

def get_smallest_detected_object(self, serializable: bool = False) -> tuple[Object, float] | tuple[dict[str, Any], float]:
    """
    Identifies the smallest object by area.

    Args:
        serializable (bool): If True, returns a dictionary instead of an Object.

    Returns:
        tuple: (smallest_object, area_m2).
    """
    smallest_object = min(self, key=lambda obj: obj.size_m2())

    size = smallest_object.size_m2()

    if serializable:
        return smallest_object.to_dict(), size
    else:
        return smallest_object, size

objects_to_dict_list(objects) staticmethod

Converts a list of Object instances to a list of dictionaries.

Parameters:

Name	Type	Description	Default
objects	list[Object]	List of Object instances.	required

Returns:

Type	Description
list[dict[str, Any]]	list[dict[str, Any]]: List of dictionary representations.

Source code in robot_workspace/objects/objects.py

@staticmethod
def objects_to_dict_list(objects: list[Object]) -> list[dict[str, Any]]:
    """
    Converts a list of Object instances to a list of dictionaries.

    Args:
        objects (list[Object]): List of Object instances.

    Returns:
        list[dict[str, Any]]: List of dictionary representations.
    """
    return [obj.to_dict() for obj in objects]

verbose()

Returns whether verbose logging is enabled.

Returns:

Name	Type	Description
bool	bool	True if verbose, else False.

Source code in robot_workspace/objects/objects.py

def verbose(self) -> bool:
    """
    Returns whether verbose logging is enabled.

    Returns:
        bool: True if verbose, else False.
    """
    return self._verbose

Functions

robot_workspace.objects.pose_object

Classes

PoseObjectPNP

Pose object which stores x, y, z, roll, pitch & yaw parameters.

Attributes:

Name	Type	Description
x	float	X coordinate in meters.
y	float	Y coordinate in meters.
z	float	Z coordinate in meters.
roll	float	Roll orientation in radians.
pitch	float	Pitch orientation in radians.
yaw	float	Yaw orientation in radians.

Source code in robot_workspace/objects/pose_object.py

class PoseObjectPNP:
    """
    Pose object which stores x, y, z, roll, pitch & yaw parameters.

    Attributes:
        x (float): X coordinate in meters.
        y (float): Y coordinate in meters.
        z (float): Z coordinate in meters.
        roll (float): Roll orientation in radians.
        pitch (float): Pitch orientation in radians.
        yaw (float): Yaw orientation in radians.
    """

    # *** CONSTRUCTORS ***
    def __init__(
        self,
        x: float = 0.0,
        y: float = 0.0,
        z: float = 0.0,
        roll: float = 0.0,
        pitch: float = 0.0,
        yaw: float = 0.0,
    ) -> None:
        """
        Initializes a PoseObjectPNP instance.

        Args:
            x (float): X coordinate in meters.
            y (float): Y coordinate in meters.
            z (float): Z coordinate in meters.
            roll (float): Roll orientation in radians.
            pitch (float): Pitch orientation in radians.
            yaw (float): Yaw orientation in radians.
        """
        # X (meter)
        self.x = float(x)
        # Y (meter)
        self.y = float(y)
        # Z (meter)
        self.z = float(z)
        # Roll (radian)
        self.roll = float(roll)
        # Pitch (radian)
        self.pitch = float(pitch)
        # Yaw (radian)
        self.yaw = float(yaw)

    def __str__(self) -> str:
        position = f"x = {self.x:.4f}, y = {self.y:.4f}, z = {self.z:.4f}"
        orientation = f"roll = {self.roll:.3f}, pitch = {self.pitch:.3f}, yaw = {self.yaw:.3f}"
        return position + "\n" + orientation

    def __repr__(self) -> str:
        return self.__str__()

    def __add__(self, other: PoseObjectPNP) -> PoseObjectPNP:
        x = self.x + other.x
        y = self.y + other.y
        z = self.z + other.z
        roll = self.roll + other.roll
        pitch = self.pitch + other.pitch
        yaw = self.yaw + other.yaw
        return PoseObjectPNP(x, y, z, roll, pitch, yaw)

    def __sub__(self, other: PoseObjectPNP) -> PoseObjectPNP:
        x = self.x - other.x
        y = self.y - other.y
        z = self.z - other.z
        roll = self.roll - other.roll
        pitch = self.pitch - other.pitch
        yaw = self.yaw - other.yaw
        return PoseObjectPNP(x, y, z, roll, pitch, yaw)

    def __eq__(self, other: object) -> bool:
        if not isinstance(other, PoseObjectPNP):
            return NotImplemented
        return (
            self.x == other.x
            and self.y == other.y
            and self.z == other.z
            and self.roll == other.roll
            and self.pitch == other.pitch
            and self.yaw == other.yaw
        )

    @log_start_end_cls()
    def approx_eq(self, other: PoseObjectPNP, eps_position: float = 0.1, eps_orientation: float = 0.1) -> bool:
        """
        Determines if two poses are approximately the same, accounting for angle periodicity.

        Args:
            other (PoseObjectPNP): Other pose object to compare with.
            eps_position (float): Tolerance for position differences (in meters).
            eps_orientation (float): Tolerance for orientation differences (in radians).

        Returns:
            bool: True if poses are approximately the same, False otherwise.
        """
        # Calculate position differences
        delta_position = math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2 + (self.z - other.z) ** 2)

        # Calculate orientation differences
        delta_roll = abs(PoseObjectPNP._angular_difference(self.roll, other.roll))
        delta_pitch = abs(PoseObjectPNP._angular_difference(self.pitch, other.pitch))
        delta_yaw = abs(PoseObjectPNP._angular_difference(self.yaw, other.yaw))

        # Check if both position and orientation differences are within tolerances
        return (
            delta_position < eps_position
            and delta_roll < eps_orientation
            and delta_pitch < eps_orientation
            and delta_yaw < eps_orientation
        )

    def approx_eq_xyz(self, other: PoseObjectPNP, eps: float = 0.1) -> bool:
        """
        Compares the (x, y, z) coordinates of this pose object with another pose object.

        Args:
            other (PoseObjectPNP): The pose object to compare with.
            eps (float): The allowable deviation for equality in each coordinate (default: 0.1).

        Returns:
            bool: True if the difference in x, y, and z coordinates between the two poses
                is less than the specified tolerance (eps), otherwise False.
        """
        return bool(abs(self.x - other.x) < eps and abs(self.y - other.y) < eps and abs(self.z - other.z) < eps)

    def copy_with_offsets(
        self,
        x_offset: float = 0.0,
        y_offset: float = 0.0,
        z_offset: float = 0.0,
        roll_offset: float = 0.0,
        pitch_offset: float = 0.0,
        yaw_offset: float = 0.0,
    ) -> PoseObjectPNP:
        """
        Creates a new pose object by applying offsets to its position and orientation.

        Args:
            x_offset (float): Offset to apply to the x-coordinate (default: 0.0).
            y_offset (float): Offset to apply to the y-coordinate (default: 0.0).
            z_offset (float): Offset to apply to the z-coordinate (default: 0.0).
            roll_offset (float): Offset to apply to the roll orientation (default: 0.0).
            pitch_offset (float): Offset to apply to the pitch orientation (default: 0.0).
            yaw_offset (float): Offset to apply to the yaw orientation (default: 0.0).

        Returns:
            PoseObjectPNP: A new pose object with the offsets applied.
        """
        return PoseObjectPNP(
            self.x + x_offset,
            self.y + y_offset,
            self.z + z_offset,
            self.roll + roll_offset,
            self.pitch + pitch_offset,
            self.yaw + yaw_offset,
        )

    def to_list(self) -> list[float]:
        """
        Return a list [x, y, z, roll, pitch, yaw] corresponding to the pose's parameters.

        Returns:
            list[float]: A list of the pose's parameters.
        """
        list_pos = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
        return list(map(float, list_pos))

    def to_transformation_matrix(self) -> np.ndarray:
        """
        Converts the pose into a 4x4 transformation matrix.

        The transformation matrix represents the pose of an object in a
        3D coordinate system. It combines translation and rotation into
        a single homogeneous transformation.

        Returns:
            np.ndarray: A 4x4 transformation matrix of type float, where:
                - The upper-left 3x3 submatrix represents the rotation.
                - The last column represents the translation.
                - The bottom row is [0, 0, 0, 1] for homogeneity.
        """
        # Compute the rotation matrix from roll, pitch, yaw
        rx = np.array([[1, 0, 0], [0, np.cos(self.roll), -np.sin(self.roll)], [0, np.sin(self.roll), np.cos(self.roll)]])

        ry = np.array([[np.cos(self.pitch), 0, np.sin(self.pitch)], [0, 1, 0], [-np.sin(self.pitch), 0, np.cos(self.pitch)]])

        rz = np.array([[np.cos(self.yaw), -np.sin(self.yaw), 0], [np.sin(self.yaw), np.cos(self.yaw), 0], [0, 0, 1]])

        # Combined rotation matrix (Rz * Ry * Rx)
        rotation_matrix = rz @ ry @ rx

        # Translation vector
        translation_vector = np.array([self.x, self.y, self.z])

        # Construct the 4x4 transformation matrix
        transformation_matrix = np.eye(4)  # Start with an identity matrix
        transformation_matrix[:3, :3] = rotation_matrix
        transformation_matrix[:3, 3] = translation_vector

        return transformation_matrix

    @property
    def quaternion(self) -> list[float]:
        """
        Return the quaternion in a list [qx, qy, qz, qw].

        Returns:
            list[float]: Quaternion [qx, qy, qz, qw].
        """
        return self.euler_to_quaternion(self.roll, self.pitch, self.yaw)

    @property
    def quaternion_pose(self) -> list[float]:
        """
        Return the position and the quaternion in a list [x, y, z, qx, qy, qz, qw].

        Returns:
            list[float]: Position [x, y, z] + quaternion [qx, qy, qz, qw].
        """
        return [self.x, self.y, self.z, *list(self.euler_to_quaternion(self.roll, self.pitch, self.yaw))]

    @staticmethod
    def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> list[float]:
        """
        Convert euler angles to quaternion.

        Args:
            roll (float): Roll in radians.
            pitch (float): Pitch in radians.
            yaw (float): Yaw in radians.

        Returns:
            list[float]: Quaternion in a list [qx, qy, qz, qw].
        """
        qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
        qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
        qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
        qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)

        return [float(qx), float(qy), float(qz), float(qw)]

    @staticmethod
    def quaternion_to_euler_angle(qx: float, qy: float, qz: float, qw: float) -> tuple[float, float, float]:
        """
        Convert quaternion to euler angles.

        Args:
            qx (float): Quaternion x.
            qy (float): Quaternion y.
            qz (float): Quaternion z.
            qw (float): Quaternion w.

        Returns:
            tuple[float, float, float]: Euler angles (roll, pitch, yaw) in radians.
        """
        ysqr = qy * qy

        t0 = +2.0 * (qw * qx + qy * qz)
        t1 = +1.0 - 2.0 * (qx * qx + ysqr)
        roll = np.arctan2(t0, t1)

        t2 = +2.0 * (qw * qy - qz * qx)

        t2 = np.clip(t2, a_min=-1.0, a_max=1.0)
        pitch = np.arcsin(t2)

        t3 = +2.0 * (qw * qz + qx * qy)
        t4 = +1.0 - 2.0 * (ysqr + qz * qz)
        yaw = np.arctan2(t3, t4)

        return float(roll), float(pitch), float(yaw)

    # *** PUBLIC GET methods ***

    # *** PUBLIC methods ***

    # *** PUBLIC STATIC/CLASS GET methods ***

    @staticmethod
    def convert_niryo_pose_object2pose_object(pose_object: Any) -> PoseObjectPNP:
        """
        Converts a PoseObject from Niryo class to a PoseObjectPNP object.

        Args:
            pose_object (Any): PoseObject from Niryo.

        Returns:
            PoseObjectPNP: The converted pose object.
        """
        return PoseObjectPNP(
            float(pose_object.x),
            float(pose_object.y),
            float(pose_object.z),
            float(pose_object.roll),
            float(pose_object.pitch),
            float(pose_object.yaw),
        )

    @staticmethod
    def convert_pose_object2niryo_pose_object(pose_object: PoseObjectPNP) -> Any:
        """
        Convert a PoseObjectPNP to a PoseObject from Niryo Robot.

        Args:
            pose_object (PoseObjectPNP): The pose object to convert.

        Returns:
            Any: Pose object as defined by Niryo.
        """
        return PoseObject(pose_object.x, pose_object.y, pose_object.z, pose_object.roll, pose_object.pitch, pose_object.yaw)

    # *** PRIVATE methods ***

    # *** PRIVATE STATIC/CLASS methods ***

    @staticmethod
    def _normalize_angle(angle: float) -> float:
        """
        Normalize an angle to the range [-π, π].

        Args:
            angle (float): Angle in radians.

        Returns:
            float: Normalized angle.
        """
        return (angle + math.pi) % (2 * math.pi) - math.pi

    @staticmethod
    def _angular_difference(angle1: float, angle2: float) -> float:
        """
        Calculate the smallest difference between two angles.

        Args:
            angle1 (float): Angle 1 in radians.
            angle2 (float): Angle 2 in radians.

        Returns:
            float: Angular difference.
        """
        return PoseObjectPNP._normalize_angle(angle1 - angle2)

    # *** PUBLIC properties ***

    def xy_coordinate(self) -> list[float]:
        """
        Returns the (x, y) coordinates of the pose.

        Returns:
            list[float]: [x, y] coordinates.
        """
        return [self.x, self.y]

Attributes

quaternion property

Return the quaternion in a list [qx, qy, qz, qw].

Returns:

Type	Description
list[float]	list[float]: Quaternion [qx, qy, qz, qw].

quaternion_pose property

Return the position and the quaternion in a list [x, y, z, qx, qy, qz, qw].

Returns:

Type	Description
list[float]	list[float]: Position [x, y, z] + quaternion [qx, qy, qz, qw].

Functions

__init__(x=0.0, y=0.0, z=0.0, roll=0.0, pitch=0.0, yaw=0.0)

Initializes a PoseObjectPNP instance.

Parameters:

Name	Type	Description	Default
x	float	X coordinate in meters.	0.0
y	float	Y coordinate in meters.	0.0
z	float	Z coordinate in meters.	0.0
roll	float	Roll orientation in radians.	0.0
pitch	float	Pitch orientation in radians.	0.0
yaw	float	Yaw orientation in radians.	0.0

Source code in robot_workspace/objects/pose_object.py

def __init__(
    self,
    x: float = 0.0,
    y: float = 0.0,
    z: float = 0.0,
    roll: float = 0.0,
    pitch: float = 0.0,
    yaw: float = 0.0,
) -> None:
    """
    Initializes a PoseObjectPNP instance.

    Args:
        x (float): X coordinate in meters.
        y (float): Y coordinate in meters.
        z (float): Z coordinate in meters.
        roll (float): Roll orientation in radians.
        pitch (float): Pitch orientation in radians.
        yaw (float): Yaw orientation in radians.
    """
    # X (meter)
    self.x = float(x)
    # Y (meter)
    self.y = float(y)
    # Z (meter)
    self.z = float(z)
    # Roll (radian)
    self.roll = float(roll)
    # Pitch (radian)
    self.pitch = float(pitch)
    # Yaw (radian)
    self.yaw = float(yaw)

approx_eq(other, eps_position=0.1, eps_orientation=0.1)

Determines if two poses are approximately the same, accounting for angle periodicity.

Parameters:

Name	Type	Description	Default
other	PoseObjectPNP	Other pose object to compare with.	required
eps_position	float	Tolerance for position differences (in meters).	0.1
eps_orientation	float	Tolerance for orientation differences (in radians).	0.1

Returns:

Name	Type	Description
bool	bool	True if poses are approximately the same, False otherwise.

Source code in robot_workspace/objects/pose_object.py

@log_start_end_cls()
def approx_eq(self, other: PoseObjectPNP, eps_position: float = 0.1, eps_orientation: float = 0.1) -> bool:
    """
    Determines if two poses are approximately the same, accounting for angle periodicity.

    Args:
        other (PoseObjectPNP): Other pose object to compare with.
        eps_position (float): Tolerance for position differences (in meters).
        eps_orientation (float): Tolerance for orientation differences (in radians).

    Returns:
        bool: True if poses are approximately the same, False otherwise.
    """
    # Calculate position differences
    delta_position = math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2 + (self.z - other.z) ** 2)

    # Calculate orientation differences
    delta_roll = abs(PoseObjectPNP._angular_difference(self.roll, other.roll))
    delta_pitch = abs(PoseObjectPNP._angular_difference(self.pitch, other.pitch))
    delta_yaw = abs(PoseObjectPNP._angular_difference(self.yaw, other.yaw))

    # Check if both position and orientation differences are within tolerances
    return (
        delta_position < eps_position
        and delta_roll < eps_orientation
        and delta_pitch < eps_orientation
        and delta_yaw < eps_orientation
    )

approx_eq_xyz(other, eps=0.1)

Compares the (x, y, z) coordinates of this pose object with another pose object.

Parameters:

Name	Type	Description	Default
other	PoseObjectPNP	The pose object to compare with.	required
eps	float	The allowable deviation for equality in each coordinate (default: 0.1).	0.1

Returns:

Name	Type	Description
bool	bool	True if the difference in x, y, and z coordinates between the two poses is less than the specified tolerance (eps), otherwise False.

Source code in robot_workspace/objects/pose_object.py

def approx_eq_xyz(self, other: PoseObjectPNP, eps: float = 0.1) -> bool:
    """
    Compares the (x, y, z) coordinates of this pose object with another pose object.

    Args:
        other (PoseObjectPNP): The pose object to compare with.
        eps (float): The allowable deviation for equality in each coordinate (default: 0.1).

    Returns:
        bool: True if the difference in x, y, and z coordinates between the two poses
            is less than the specified tolerance (eps), otherwise False.
    """
    return bool(abs(self.x - other.x) < eps and abs(self.y - other.y) < eps and abs(self.z - other.z) < eps)

convert_niryo_pose_object2pose_object(pose_object) staticmethod

Converts a PoseObject from Niryo class to a PoseObjectPNP object.

Parameters:

Name	Type	Description	Default
pose_object	Any	PoseObject from Niryo.	required

Returns:

Name	Type	Description
PoseObjectPNP	PoseObjectPNP	The converted pose object.

Source code in robot_workspace/objects/pose_object.py

@staticmethod
def convert_niryo_pose_object2pose_object(pose_object: Any) -> PoseObjectPNP:
    """
    Converts a PoseObject from Niryo class to a PoseObjectPNP object.

    Args:
        pose_object (Any): PoseObject from Niryo.

    Returns:
        PoseObjectPNP: The converted pose object.
    """
    return PoseObjectPNP(
        float(pose_object.x),
        float(pose_object.y),
        float(pose_object.z),
        float(pose_object.roll),
        float(pose_object.pitch),
        float(pose_object.yaw),
    )

convert_pose_object2niryo_pose_object(pose_object) staticmethod

Convert a PoseObjectPNP to a PoseObject from Niryo Robot.

Parameters:

Name	Type	Description	Default
pose_object	PoseObjectPNP	The pose object to convert.	required

Returns:

Name	Type	Description
Any	Any	Pose object as defined by Niryo.

Source code in robot_workspace/objects/pose_object.py

@staticmethod
def convert_pose_object2niryo_pose_object(pose_object: PoseObjectPNP) -> Any:
    """
    Convert a PoseObjectPNP to a PoseObject from Niryo Robot.

    Args:
        pose_object (PoseObjectPNP): The pose object to convert.

    Returns:
        Any: Pose object as defined by Niryo.
    """
    return PoseObject(pose_object.x, pose_object.y, pose_object.z, pose_object.roll, pose_object.pitch, pose_object.yaw)

copy_with_offsets(x_offset=0.0, y_offset=0.0, z_offset=0.0, roll_offset=0.0, pitch_offset=0.0, yaw_offset=0.0)

Creates a new pose object by applying offsets to its position and orientation.

Parameters:

Name	Type	Description	Default
x_offset	float	Offset to apply to the x-coordinate (default: 0.0).	0.0
y_offset	float	Offset to apply to the y-coordinate (default: 0.0).	0.0
z_offset	float	Offset to apply to the z-coordinate (default: 0.0).	0.0
roll_offset	float	Offset to apply to the roll orientation (default: 0.0).	0.0
pitch_offset	float	Offset to apply to the pitch orientation (default: 0.0).	0.0
yaw_offset	float	Offset to apply to the yaw orientation (default: 0.0).	0.0

Returns:

Name	Type	Description
PoseObjectPNP	PoseObjectPNP	A new pose object with the offsets applied.

Source code in robot_workspace/objects/pose_object.py

def copy_with_offsets(
    self,
    x_offset: float = 0.0,
    y_offset: float = 0.0,
    z_offset: float = 0.0,
    roll_offset: float = 0.0,
    pitch_offset: float = 0.0,
    yaw_offset: float = 0.0,
) -> PoseObjectPNP:
    """
    Creates a new pose object by applying offsets to its position and orientation.

    Args:
        x_offset (float): Offset to apply to the x-coordinate (default: 0.0).
        y_offset (float): Offset to apply to the y-coordinate (default: 0.0).
        z_offset (float): Offset to apply to the z-coordinate (default: 0.0).
        roll_offset (float): Offset to apply to the roll orientation (default: 0.0).
        pitch_offset (float): Offset to apply to the pitch orientation (default: 0.0).
        yaw_offset (float): Offset to apply to the yaw orientation (default: 0.0).

    Returns:
        PoseObjectPNP: A new pose object with the offsets applied.
    """
    return PoseObjectPNP(
        self.x + x_offset,
        self.y + y_offset,
        self.z + z_offset,
        self.roll + roll_offset,
        self.pitch + pitch_offset,
        self.yaw + yaw_offset,
    )

euler_to_quaternion(roll, pitch, yaw) staticmethod

Convert euler angles to quaternion.

Parameters:

Name	Type	Description	Default
roll	float	Roll in radians.	required
pitch	float	Pitch in radians.	required
yaw	float	Yaw in radians.	required

Returns:

Type	Description
list[float]	list[float]: Quaternion in a list [qx, qy, qz, qw].

Source code in robot_workspace/objects/pose_object.py

@staticmethod
def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> list[float]:
    """
    Convert euler angles to quaternion.

    Args:
        roll (float): Roll in radians.
        pitch (float): Pitch in radians.
        yaw (float): Yaw in radians.

    Returns:
        list[float]: Quaternion in a list [qx, qy, qz, qw].
    """
    qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
    qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
    qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
    qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)

    return [float(qx), float(qy), float(qz), float(qw)]

quaternion_to_euler_angle(qx, qy, qz, qw) staticmethod

Convert quaternion to euler angles.

Parameters:

Name	Type	Description	Default
qx	float	Quaternion x.	required
qy	float	Quaternion y.	required
qz	float	Quaternion z.	required
qw	float	Quaternion w.	required

Returns:

Type	Description
tuple[float, float, float]	tuple[float, float, float]: Euler angles (roll, pitch, yaw) in radians.

Source code in robot_workspace/objects/pose_object.py

@staticmethod
def quaternion_to_euler_angle(qx: float, qy: float, qz: float, qw: float) -> tuple[float, float, float]:
    """
    Convert quaternion to euler angles.

    Args:
        qx (float): Quaternion x.
        qy (float): Quaternion y.
        qz (float): Quaternion z.
        qw (float): Quaternion w.

    Returns:
        tuple[float, float, float]: Euler angles (roll, pitch, yaw) in radians.
    """
    ysqr = qy * qy

    t0 = +2.0 * (qw * qx + qy * qz)
    t1 = +1.0 - 2.0 * (qx * qx + ysqr)
    roll = np.arctan2(t0, t1)

    t2 = +2.0 * (qw * qy - qz * qx)

    t2 = np.clip(t2, a_min=-1.0, a_max=1.0)
    pitch = np.arcsin(t2)

    t3 = +2.0 * (qw * qz + qx * qy)
    t4 = +1.0 - 2.0 * (ysqr + qz * qz)
    yaw = np.arctan2(t3, t4)

    return float(roll), float(pitch), float(yaw)

to_list()

Return a list [x, y, z, roll, pitch, yaw] corresponding to the pose's parameters.

Returns:

Type	Description
list[float]	list[float]: A list of the pose's parameters.

Source code in robot_workspace/objects/pose_object.py

def to_list(self) -> list[float]:
    """
    Return a list [x, y, z, roll, pitch, yaw] corresponding to the pose's parameters.

    Returns:
        list[float]: A list of the pose's parameters.
    """
    list_pos = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
    return list(map(float, list_pos))

to_transformation_matrix()

Converts the pose into a 4x4 transformation matrix.

The transformation matrix represents the pose of an object in a 3D coordinate system. It combines translation and rotation into a single homogeneous transformation.

Returns:

Type	Description
ndarray	np.ndarray: A 4x4 transformation matrix of type float, where: - The upper-left 3x3 submatrix represents the rotation. - The last column represents the translation. - The bottom row is [0, 0, 0, 1] for homogeneity.

Source code in robot_workspace/objects/pose_object.py

def to_transformation_matrix(self) -> np.ndarray:
    """
    Converts the pose into a 4x4 transformation matrix.

    The transformation matrix represents the pose of an object in a
    3D coordinate system. It combines translation and rotation into
    a single homogeneous transformation.

    Returns:
        np.ndarray: A 4x4 transformation matrix of type float, where:
            - The upper-left 3x3 submatrix represents the rotation.
            - The last column represents the translation.
            - The bottom row is [0, 0, 0, 1] for homogeneity.
    """
    # Compute the rotation matrix from roll, pitch, yaw
    rx = np.array([[1, 0, 0], [0, np.cos(self.roll), -np.sin(self.roll)], [0, np.sin(self.roll), np.cos(self.roll)]])

    ry = np.array([[np.cos(self.pitch), 0, np.sin(self.pitch)], [0, 1, 0], [-np.sin(self.pitch), 0, np.cos(self.pitch)]])

    rz = np.array([[np.cos(self.yaw), -np.sin(self.yaw), 0], [np.sin(self.yaw), np.cos(self.yaw), 0], [0, 0, 1]])

    # Combined rotation matrix (Rz * Ry * Rx)
    rotation_matrix = rz @ ry @ rx

    # Translation vector
    translation_vector = np.array([self.x, self.y, self.z])

    # Construct the 4x4 transformation matrix
    transformation_matrix = np.eye(4)  # Start with an identity matrix
    transformation_matrix[:3, :3] = rotation_matrix
    transformation_matrix[:3, 3] = translation_vector

    return transformation_matrix

xy_coordinate()

Returns the (x, y) coordinates of the pose.

Returns:

Type	Description
list[float]	list[float]: [x, y] coordinates.

Source code in robot_workspace/objects/pose_object.py

def xy_coordinate(self) -> list[float]:
    """
    Returns the (x, y) coordinates of the pose.

    Returns:
        list[float]: [x, y] coordinates.
    """
    return [self.x, self.y]

Functions

robot_workspace.workspaces.workspace

Classes

Workspace

Bases: ABC

Abstract base class representing a robotic workspace.

A workspace defines a region where a robot can pick and place objects. It handles coordinate transformations between camera images and world coordinates.

Attributes:

Name	Type	Description
_id	str	Unique identifier for the workspace.
_verbose	bool	If True, enables verbose logging.
_logger	Logger	Logger instance.
_observation_pose	PoseObjectPNP	Optimal pose for the camera to observe the workspace.
_xy_ul_wc	PoseObjectPNP	Upper-left corner in world coordinates.
_xy_ll_wc	PoseObjectPNP	Lower-left corner in world coordinates.
_xy_ur_wc	PoseObjectPNP	Upper-right corner in world coordinates.
_xy_lr_wc	PoseObjectPNP	Lower-right corner in world coordinates.
_xy_center_wc	PoseObjectPNP	Center of the workspace in world coordinates.
_width_m	float	Width of the workspace in meters.
_height_m	float	Height of the workspace in meters.
_img_shape	tuple[int, int, int]	Shape of the camera image (height, width, channels).

Source code in robot_workspace/workspaces/workspace.py

class Workspace(ABC):
    """
    Abstract base class representing a robotic workspace.

    A workspace defines a region where a robot can pick and place objects.
    It handles coordinate transformations between camera images and world coordinates.

    Attributes:
        _id (str): Unique identifier for the workspace.
        _verbose (bool): If True, enables verbose logging.
        _logger (logging.Logger): Logger instance.
        _observation_pose (PoseObjectPNP): Optimal pose for the camera to observe the workspace.
        _xy_ul_wc (PoseObjectPNP): Upper-left corner in world coordinates.
        _xy_ll_wc (PoseObjectPNP): Lower-left corner in world coordinates.
        _xy_ur_wc (PoseObjectPNP): Upper-right corner in world coordinates.
        _xy_lr_wc (PoseObjectPNP): Lower-right corner in world coordinates.
        _xy_center_wc (PoseObjectPNP): Center of the workspace in world coordinates.
        _width_m (float): Width of the workspace in meters.
        _height_m (float): Height of the workspace in meters.
        _img_shape (tuple[int, int, int]): Shape of the camera image (height, width, channels).
    """

    # *** CONSTRUCTORS ***
    @log_start_end_cls()
    def __init__(self, workspace_id: str, verbose: bool = False) -> None:
        """
        Initializes the workspace.

        Args:
            workspace_id (str): Unique ID of the workspace.
            verbose (bool): Whether to enable verbose logging.
        """
        self._id = workspace_id
        self._verbose = verbose
        self._logger = logging.getLogger("robot_workspace")

        self._set_observation_pose()

        self._set_4corners_of_workspace()

        self._calc_width_height()

        self._calc_center_of_workspace()

    def __str__(self) -> str:
        size = f"width = {self._width_m:.2f}, height = {self._height_m:.2f}"
        return "Workspace " + self.id() + "\n" + size + "\n" + str(self._xy_center_wc)

    def __repr__(self) -> str:
        return self.__str__()

    # *** PUBLIC GET methods ***

    @log_start_end_cls()
    def is_visible(self, camera_pose: PoseObjectPNP) -> bool:
        """
        Checks whether the workspace is visible from the given camera pose.

        Args:
            camera_pose (PoseObjectPNP): The current pose of the camera.

        Returns:
            bool: True if the workspace is considered visible, False otherwise.
        """
        if self.verbose() and self._logger:
            self._logger.debug(f"is_visible check: camera={camera_pose}, obs={self._observation_pose}")

        if self._observation_pose is None:
            return False

        return camera_pose.approx_eq_xyz(self._observation_pose)

    # *** PUBLIC methods ***

    def set_img_shape(self, img_shape: tuple[int, int, int]) -> None:
        """
        Sets the image shape for the workspace.

        Args:
            img_shape (tuple[int, int, int]): (height, width, channels).
        """
        self._img_shape = img_shape

    @abstractmethod
    def transform_camera2world_coords(self, workspace_id: str, u_rel: float, v_rel: float, yaw: float = 0.0) -> PoseObjectPNP:
        """
        Transforms relative image coordinates to world coordinates.

        Args:
            workspace_id (str): ID of the workspace.
            u_rel (float): Normalized horizontal coordinate [0, 1].
            v_rel (float): Normalized vertical coordinate [0, 1].
            yaw (float): Orientation of the object at the given point.

        Returns:
            PoseObjectPNP: Corresponding pose in world coordinates.
        """
        pass

    # *** PUBLIC STATIC/CLASS GET methods ***

    # *** PRIVATE methods ***

    @abstractmethod
    def _set_4corners_of_workspace(self) -> None:
        """Sets the four corners of the workspace in world coordinates."""
        pass

    @abstractmethod
    def _set_observation_pose(self) -> None:
        """Sets the optimal observation pose for the workspace."""
        pass

    @log_start_end_cls()
    def _calc_width_height(self) -> None:
        """Calculates the physical width and height of the workspace."""
        self._width_m, self._height_m = Object.calc_width_height(self._xy_ul_wc, self._xy_lr_wc)

    @log_start_end_cls()
    def _calc_center_of_workspace(self) -> None:
        """Calculates the center point of the workspace."""
        if self._xy_ll_wc is None or self._xy_ul_wc is None or self._xy_lr_wc is None:
            raise ValueError("Workspace corners must be set before calculating center.")

        dx = self._xy_ll_wc.x - self._xy_ul_wc.x
        dy = self._xy_ll_wc.y - self._xy_lr_wc.y

        self._xy_center_wc = self._xy_lr_wc.copy_with_offsets(-dx / 2.0, dy / 2.0)

        if self.verbose() and self._logger:
            self._logger.debug(f"_calc_center_of_workspace: {self._xy_center_wc}, {self._xy_ll_wc.x}")

    # *** PRIVATE STATIC/CLASS methods ***

    # *** PUBLIC properties ***

    def id(self) -> str:
        """Returns the workspace ID."""
        return self._id

    # def environment(self) -> "Environment":
    #     return self._environment

    def xy_ul_wc(self) -> PoseObjectPNP:
        """Returns the upper-left corner in world coordinates."""
        return self._xy_ul_wc

    def xy_ll_wc(self) -> PoseObjectPNP:
        """Returns the lower-left corner in world coordinates."""
        return self._xy_ll_wc

    def xy_ur_wc(self) -> PoseObjectPNP:
        """Returns the upper-right corner in world coordinates."""
        return self._xy_ur_wc

    def xy_lr_wc(self) -> PoseObjectPNP:
        """Returns the lower-right corner in world coordinates."""
        return self._xy_lr_wc

    def xy_center_wc(self) -> PoseObjectPNP:
        """Returns the center of the workspace in world coordinates."""
        return self._xy_center_wc

    def width_m(self) -> float:
        """Returns the width of the workspace in meters."""
        return self._width_m

    def height_m(self) -> float:
        """Returns the height of the workspace in meters."""
        return self._height_m

    def img_shape(self) -> tuple[int, int, int]:
        """Returns the image shape of the workspace."""
        return self._img_shape

    def observation_pose(self) -> PoseObjectPNP:
        """Returns the optimal observation pose."""
        return self._observation_pose

    def verbose(self) -> bool:
        """Returns whether verbose logging is enabled."""
        return self._verbose

    # *** PRIVATE variables ***

    _id: str = ""
    _xy_ul_wc: PoseObjectPNP = None
    _xy_ll_wc: PoseObjectPNP = None
    _xy_ur_wc: PoseObjectPNP = None
    _xy_lr_wc: PoseObjectPNP = None
    _xy_center_wc: PoseObjectPNP = None
    _width_m: float = 0.0
    _height_m: float = 0.0
    _img_shape: tuple[int, int, int] = None
    _observation_pose: PoseObjectPNP = None
    _verbose: bool = False
    _logger: logging.Logger = None

Functions

__init__(workspace_id, verbose=False)

Initializes the workspace.

Parameters:

Name	Type	Description	Default
workspace_id	str	Unique ID of the workspace.	required
verbose	bool	Whether to enable verbose logging.	False

Source code in robot_workspace/workspaces/workspace.py

@log_start_end_cls()
def __init__(self, workspace_id: str, verbose: bool = False) -> None:
    """
    Initializes the workspace.

    Args:
        workspace_id (str): Unique ID of the workspace.
        verbose (bool): Whether to enable verbose logging.
    """
    self._id = workspace_id
    self._verbose = verbose
    self._logger = logging.getLogger("robot_workspace")

    self._set_observation_pose()

    self._set_4corners_of_workspace()

    self._calc_width_height()

    self._calc_center_of_workspace()

height_m()

Returns the height of the workspace in meters.

Source code in robot_workspace/workspaces/workspace.py

def height_m(self) -> float:
    """Returns the height of the workspace in meters."""
    return self._height_m

id()

Returns the workspace ID.

Source code in robot_workspace/workspaces/workspace.py

def id(self) -> str:
    """Returns the workspace ID."""
    return self._id

img_shape()

Returns the image shape of the workspace.

Source code in robot_workspace/workspaces/workspace.py

def img_shape(self) -> tuple[int, int, int]:
    """Returns the image shape of the workspace."""
    return self._img_shape

is_visible(camera_pose)

Checks whether the workspace is visible from the given camera pose.

Parameters:

Name	Type	Description	Default
camera_pose	PoseObjectPNP	The current pose of the camera.	required

Returns:

Name	Type	Description
bool	bool	True if the workspace is considered visible, False otherwise.

Source code in robot_workspace/workspaces/workspace.py

@log_start_end_cls()
def is_visible(self, camera_pose: PoseObjectPNP) -> bool:
    """
    Checks whether the workspace is visible from the given camera pose.

    Args:
        camera_pose (PoseObjectPNP): The current pose of the camera.

    Returns:
        bool: True if the workspace is considered visible, False otherwise.
    """
    if self.verbose() and self._logger:
        self._logger.debug(f"is_visible check: camera={camera_pose}, obs={self._observation_pose}")

    if self._observation_pose is None:
        return False

    return camera_pose.approx_eq_xyz(self._observation_pose)

observation_pose()

Returns the optimal observation pose.

Source code in robot_workspace/workspaces/workspace.py

def observation_pose(self) -> PoseObjectPNP:
    """Returns the optimal observation pose."""
    return self._observation_pose

set_img_shape(img_shape)

Sets the image shape for the workspace.

Parameters:

Name	Type	Description	Default
img_shape	tuple[int, int, int]	(height, width, channels).	required

Source code in robot_workspace/workspaces/workspace.py

def set_img_shape(self, img_shape: tuple[int, int, int]) -> None:
    """
    Sets the image shape for the workspace.

    Args:
        img_shape (tuple[int, int, int]): (height, width, channels).
    """
    self._img_shape = img_shape

transform_camera2world_coords(workspace_id, u_rel, v_rel, yaw=0.0) abstractmethod

Transforms relative image coordinates to world coordinates.

Parameters:

Name	Type	Description	Default
workspace_id	str	ID of the workspace.	required
u_rel	float	Normalized horizontal coordinate [0, 1].	required
v_rel	float	Normalized vertical coordinate [0, 1].	required
yaw	float	Orientation of the object at the given point.	0.0

Returns:

Name	Type	Description
PoseObjectPNP	PoseObjectPNP	Corresponding pose in world coordinates.

Source code in robot_workspace/workspaces/workspace.py

@abstractmethod
def transform_camera2world_coords(self, workspace_id: str, u_rel: float, v_rel: float, yaw: float = 0.0) -> PoseObjectPNP:
    """
    Transforms relative image coordinates to world coordinates.

    Args:
        workspace_id (str): ID of the workspace.
        u_rel (float): Normalized horizontal coordinate [0, 1].
        v_rel (float): Normalized vertical coordinate [0, 1].
        yaw (float): Orientation of the object at the given point.

    Returns:
        PoseObjectPNP: Corresponding pose in world coordinates.
    """
    pass

verbose()

Returns whether verbose logging is enabled.

Source code in robot_workspace/workspaces/workspace.py

def verbose(self) -> bool:
    """Returns whether verbose logging is enabled."""
    return self._verbose

width_m()

Returns the width of the workspace in meters.

Source code in robot_workspace/workspaces/workspace.py

def width_m(self) -> float:
    """Returns the width of the workspace in meters."""
    return self._width_m

xy_center_wc()

Returns the center of the workspace in world coordinates.

Source code in robot_workspace/workspaces/workspace.py

def xy_center_wc(self) -> PoseObjectPNP:
    """Returns the center of the workspace in world coordinates."""
    return self._xy_center_wc

xy_ll_wc()

Returns the lower-left corner in world coordinates.

Source code in robot_workspace/workspaces/workspace.py

def xy_ll_wc(self) -> PoseObjectPNP:
    """Returns the lower-left corner in world coordinates."""
    return self._xy_ll_wc

xy_lr_wc()

Returns the lower-right corner in world coordinates.

Source code in robot_workspace/workspaces/workspace.py

def xy_lr_wc(self) -> PoseObjectPNP:
    """Returns the lower-right corner in world coordinates."""
    return self._xy_lr_wc

xy_ul_wc()

Returns the upper-left corner in world coordinates.

Source code in robot_workspace/workspaces/workspace.py

def xy_ul_wc(self) -> PoseObjectPNP:
    """Returns the upper-left corner in world coordinates."""
    return self._xy_ul_wc

xy_ur_wc()

Returns the upper-right corner in world coordinates.

Source code in robot_workspace/workspaces/workspace.py

def xy_ur_wc(self) -> PoseObjectPNP:
    """Returns the upper-right corner in world coordinates."""
    return self._xy_ur_wc

Functions

robot_workspace.workspaces.niryo_workspace

Classes

NiryoWorkspace

Bases: Workspace

Implementation of Workspace for the Niryo Ned2 robot.

This class provides specific coordinate transformations and poses for the Niryo Ned2 robotic arm and its mounted camera.

Source code in robot_workspace/workspaces/niryo_workspace.py

class NiryoWorkspace(Workspace):
    """
    Implementation of Workspace for the Niryo Ned2 robot.

    This class provides specific coordinate transformations and poses
    for the Niryo Ned2 robotic arm and its mounted camera.
    """

    # *** CONSTRUCTORS ***
    def __init__(
        self,
        workspace_id: str,
        environment: EnvironmentProtocol,
        verbose: bool = False,
        config: WorkspaceConfig | None = None,
    ) -> None:
        """
        Initializes the NiryoWorkspace.

        Args:
            workspace_id (str): Unique ID of the workspace.
            environment (EnvironmentProtocol): Object providing robot environment access.
            verbose (bool): Whether to enable verbose output.
            config (WorkspaceConfig, optional): Optional workspace configuration.
        """
        self._environment = environment
        self._config = config
        self._logger = logging.getLogger("robot_workspace")

        super().__init__(workspace_id, verbose)

    # *** PUBLIC GET methods ***

    # *** PUBLIC methods ***

    @classmethod
    def from_config(cls, config: WorkspaceConfig, environment: EnvironmentProtocol, verbose: bool = False) -> NiryoWorkspace:
        """
        Creates a NiryoWorkspace instance from a configuration object.

        Args:
            config (WorkspaceConfig): Configuration instance.
            environment (EnvironmentProtocol): Environment object.
            verbose (bool): Whether to enable verbose output.

        Returns:
            NiryoWorkspace: The initialized workspace instance.
        """
        workspace = cls(config.id, environment, verbose, config)

        # Override observation pose from config if available
        if config.observation_pose:
            workspace._observation_pose = PoseObjectPNP(
                x=config.observation_pose.x,
                y=config.observation_pose.y,
                z=config.observation_pose.z,
                roll=config.observation_pose.roll,
                pitch=config.observation_pose.pitch,
                yaw=config.observation_pose.yaw,
            )

        # Override image shape from config if available
        if config.image_shape:
            workspace.set_img_shape(config.image_shape)

        return workspace

    def transform_camera2world_coords(self, workspace_id: str, u_rel: float, v_rel: float, yaw: float = 0.0) -> PoseObjectPNP:
        """
        Transforms relative image coordinates to Niryo world coordinates.

        Args:
            workspace_id (str): ID of the workspace.
            u_rel (float): Normalized horizontal coordinate [0, 1].
            v_rel (float): Normalized vertical coordinate [0, 1].
            yaw (float): Orientation of the object.

        Returns:
            PoseObjectPNP: Corresponding pose in world coordinates.
        """
        if self.verbose():
            self._logger.debug(
                f"transform_camera2world_coords input - workspace_id: {workspace_id}, u_rel: {u_rel}, v_rel: {v_rel}, yaw: {yaw}"
            )

        obj_coords = self._environment.get_robot_target_pose_from_rel(workspace_id, u_rel, v_rel, yaw)

        if self.verbose():
            self._logger.debug(f"transform_camera2world_coords output: {obj_coords}")

        return obj_coords

    # *** PUBLIC STATIC/CLASS GET methods ***

    # *** PRIVATE methods ***

    @log_start_end_cls()
    def _set_4corners_of_workspace(self) -> None:
        """Sets the four corners of the workspace in world coordinates."""
        self._xy_ul_wc = self.transform_camera2world_coords(self._id, 0.0, 0.0)
        self._xy_ll_wc = self.transform_camera2world_coords(self._id, 0.0, 1.0)
        self._xy_ur_wc = self.transform_camera2world_coords(self._id, 1.0, 0.0)
        self._xy_lr_wc = self.transform_camera2world_coords(self._id, 1.0, 1.0)

        if self.verbose():
            self._logger.debug(f"Workspace corners - UL: {self._xy_ul_wc}, LL: {self._xy_ll_wc}")
            self._logger.debug(f"Workspace corners - UR: {self._xy_ur_wc}, LR: {self._xy_lr_wc}")

    @log_start_end_cls()
    def _set_observation_pose(self) -> None:
        """Sets the observation pose using configuration data."""
        if not self._config:
            raise ValueError(
                f"No configuration provided for workspace '{self._id}'. " "Initialize with config_path or from_config()."
            )

        if not self._config.observation_pose:
            raise ValueError(f"No observation pose defined in config for workspace '{self._id}'")

        self._observation_pose = PoseObjectPNP(
            x=self._config.observation_pose.x,
            y=self._config.observation_pose.y,
            z=self._config.observation_pose.z,
            roll=self._config.observation_pose.roll,
            pitch=self._config.observation_pose.pitch,
            yaw=self._config.observation_pose.yaw,
        )

    # *** PRIVATE STATIC/CLASS methods ***

    # *** PUBLIC properties ***

    def environment(self) -> EnvironmentProtocol:
        """Returns the environment associated with this workspace."""
        return self._environment

    # *** PRIVATE variables ***

    _environment: EnvironmentProtocol = None
    _logger: logging.Logger = None

Functions

__init__(workspace_id, environment, verbose=False, config=None)

Initializes the NiryoWorkspace.

Parameters:

Name	Type	Description	Default
workspace_id	str	Unique ID of the workspace.	required
environment	EnvironmentProtocol	Object providing robot environment access.	required
verbose	bool	Whether to enable verbose output.	False
config	WorkspaceConfig	Optional workspace configuration.	None

Source code in robot_workspace/workspaces/niryo_workspace.py

def __init__(
    self,
    workspace_id: str,
    environment: EnvironmentProtocol,
    verbose: bool = False,
    config: WorkspaceConfig | None = None,
) -> None:
    """
    Initializes the NiryoWorkspace.

    Args:
        workspace_id (str): Unique ID of the workspace.
        environment (EnvironmentProtocol): Object providing robot environment access.
        verbose (bool): Whether to enable verbose output.
        config (WorkspaceConfig, optional): Optional workspace configuration.
    """
    self._environment = environment
    self._config = config
    self._logger = logging.getLogger("robot_workspace")

    super().__init__(workspace_id, verbose)

environment()

Returns the environment associated with this workspace.

Source code in robot_workspace/workspaces/niryo_workspace.py

def environment(self) -> EnvironmentProtocol:
    """Returns the environment associated with this workspace."""
    return self._environment

from_config(config, environment, verbose=False) classmethod

Creates a NiryoWorkspace instance from a configuration object.

Parameters:

Name	Type	Description	Default
config	WorkspaceConfig	Configuration instance.	required
environment	EnvironmentProtocol	Environment object.	required
verbose	bool	Whether to enable verbose output.	False

Returns:

Name	Type	Description
NiryoWorkspace	NiryoWorkspace	The initialized workspace instance.

Source code in robot_workspace/workspaces/niryo_workspace.py

@classmethod
def from_config(cls, config: WorkspaceConfig, environment: EnvironmentProtocol, verbose: bool = False) -> NiryoWorkspace:
    """
    Creates a NiryoWorkspace instance from a configuration object.

    Args:
        config (WorkspaceConfig): Configuration instance.
        environment (EnvironmentProtocol): Environment object.
        verbose (bool): Whether to enable verbose output.

    Returns:
        NiryoWorkspace: The initialized workspace instance.
    """
    workspace = cls(config.id, environment, verbose, config)

    # Override observation pose from config if available
    if config.observation_pose:
        workspace._observation_pose = PoseObjectPNP(
            x=config.observation_pose.x,
            y=config.observation_pose.y,
            z=config.observation_pose.z,
            roll=config.observation_pose.roll,
            pitch=config.observation_pose.pitch,
            yaw=config.observation_pose.yaw,
        )

    # Override image shape from config if available
    if config.image_shape:
        workspace.set_img_shape(config.image_shape)

    return workspace

transform_camera2world_coords(workspace_id, u_rel, v_rel, yaw=0.0)

Transforms relative image coordinates to Niryo world coordinates.

Parameters:

Name	Type	Description	Default
workspace_id	str	ID of the workspace.	required
u_rel	float	Normalized horizontal coordinate [0, 1].	required
v_rel	float	Normalized vertical coordinate [0, 1].	required
yaw	float	Orientation of the object.	0.0

Returns:

Name	Type	Description
PoseObjectPNP	PoseObjectPNP	Corresponding pose in world coordinates.

Source code in robot_workspace/workspaces/niryo_workspace.py

def transform_camera2world_coords(self, workspace_id: str, u_rel: float, v_rel: float, yaw: float = 0.0) -> PoseObjectPNP:
    """
    Transforms relative image coordinates to Niryo world coordinates.

    Args:
        workspace_id (str): ID of the workspace.
        u_rel (float): Normalized horizontal coordinate [0, 1].
        v_rel (float): Normalized vertical coordinate [0, 1].
        yaw (float): Orientation of the object.

    Returns:
        PoseObjectPNP: Corresponding pose in world coordinates.
    """
    if self.verbose():
        self._logger.debug(
            f"transform_camera2world_coords input - workspace_id: {workspace_id}, u_rel: {u_rel}, v_rel: {v_rel}, yaw: {yaw}"
        )

    obj_coords = self._environment.get_robot_target_pose_from_rel(workspace_id, u_rel, v_rel, yaw)

    if self.verbose():
        self._logger.debug(f"transform_camera2world_coords output: {obj_coords}")

    return obj_coords

Functions

robot_workspace.workspaces.widowx_workspace

Classes

WidowXWorkspace

Bases: Workspace

Implementation of Workspace for the WidowX 250 6DOF robot.

The WidowX robot typically uses a third-person camera view rather than a gripper-mounted camera.

Source code in robot_workspace/workspaces/widowx_workspace.py

class WidowXWorkspace(Workspace):
    """
    Implementation of Workspace for the WidowX 250 6DOF robot.

    The WidowX robot typically uses a third-person camera view
    rather than a gripper-mounted camera.
    """

    # *** CONSTRUCTORS ***
    def __init__(
        self,
        workspace_id: str,
        environment: EnvironmentProtocol,
        verbose: bool = False,
        config: WorkspaceConfig | None = None,
    ) -> None:
        """
        Initializes the WidowXWorkspace.

        Args:
            workspace_id (str): Unique ID of the workspace.
            environment (EnvironmentProtocol): Object implementing EnvironmentProtocol.
            verbose (bool): Whether to enable verbose output.
            config (WorkspaceConfig, optional): Optional workspace configuration.
        """
        self._environment = environment
        self._config = config
        self._logger = logging.getLogger("robot_workspace")

        super().__init__(workspace_id, verbose)

    # *** PUBLIC GET methods ***

    # *** PUBLIC methods ***

    @classmethod
    def from_config(cls, config: WorkspaceConfig, environment: EnvironmentProtocol, verbose: bool = False) -> WidowXWorkspace:
        """
        Creates a WidowXWorkspace from a configuration object.

        Args:
            config (WorkspaceConfig): Workspace configuration.
            environment (EnvironmentProtocol): Environment object.
            verbose (bool): Whether to enable verbose output.

        Returns:
            WidowXWorkspace: The initialized workspace instance.
        """
        workspace = cls(config.id, environment, verbose, config)

        # Override observation pose from config if available
        if config.observation_pose:
            workspace._observation_pose = PoseObjectPNP(
                x=config.observation_pose.x,
                y=config.observation_pose.y,
                z=config.observation_pose.z,
                roll=config.observation_pose.roll,
                pitch=config.observation_pose.pitch,
                yaw=config.observation_pose.yaw,
            )

        # Override image shape from config if available
        if config.image_shape:
            workspace.set_img_shape(config.image_shape)

        return workspace

    def transform_camera2world_coords(self, workspace_id: str, u_rel: float, v_rel: float, yaw: float = 0.0) -> PoseObjectPNP:
        """
        Transforms relative image coordinates to WidowX world coordinates.

        Args:
            workspace_id (str): ID of the workspace.
            u_rel (float): Normalized horizontal coordinate [0, 1].
            v_rel (float): Normalized vertical coordinate [0, 1].
            yaw (float): Orientation of the object.

        Returns:
            PoseObjectPNP: Corresponding pose in world coordinates.
        """
        if self.verbose() and self._logger:
            self._logger.debug(
                f"transform_camera2world_coords input - workspace_id: {workspace_id}, u_rel: {u_rel}, v_rel: {v_rel}, yaw: {yaw}"
            )

        # Delegate to environment's transformation if available
        if hasattr(self, "_environment") and self._environment is not None:
            obj_coords = self._environment.get_robot_target_pose_from_rel(workspace_id, u_rel, v_rel, yaw)
            if self.verbose() and self._logger:
                self._logger.debug(f"transform_camera2world_coords output: {obj_coords}")
            return obj_coords

        # Fallback: Linear interpolation using workspace corners or defaults
        if self._xy_ul_wc is not None and self._xy_lr_wc is not None:
            x_min = self._xy_lr_wc.x
            x_max = self._xy_ul_wc.x
            y_min = self._xy_lr_wc.y
            y_max = self._xy_ul_wc.y

            x = x_max - u_rel * (x_max - x_min)
            y = y_max - v_rel * (y_max - y_min)
        else:
            x = 0.5 - u_rel * 0.4
            y = 0.2 - v_rel * 0.4

        obj_coords = PoseObjectPNP(x, y, 0.05, 0.0, 1.57, yaw)

        if self.verbose() and self._logger:
            self._logger.debug(f"transform_camera2world_coords output (fallback): {obj_coords}")

        return obj_coords

    # *** PUBLIC STATIC/CLASS GET methods ***

    # *** PRIVATE methods ***

    @log_start_end_cls()
    def _set_4corners_of_workspace(self) -> None:
        """Sets the four corners of the workspace using the transform method."""
        self._xy_ul_wc = self.transform_camera2world_coords(self._id, 0.0, 0.0)
        self._xy_ll_wc = self.transform_camera2world_coords(self._id, 1.0, 0.0)
        self._xy_ur_wc = self.transform_camera2world_coords(self._id, 0.0, 1.0)
        self._xy_lr_wc = self.transform_camera2world_coords(self._id, 1.0, 1.0)

        if self.verbose():
            self._logger.debug(f"Workspace corners - UL: {self._xy_ul_wc}, LL: {self._xy_ll_wc}")
            self._logger.debug(f"Workspace corners - UR: {self._xy_ur_wc}, LR: {self._xy_lr_wc}")

    @log_start_end_cls()
    def _set_observation_pose(self) -> None:
        """Sets the observation pose using configuration data."""
        if not self._config:
            raise ValueError(
                f"No configuration provided for workspace '{self._id}'. " "Initialize with config_path or from_config()."
            )

        if not self._config.observation_pose:
            raise ValueError(f"No observation pose defined in config for workspace '{self._id}'")

        self._observation_pose = PoseObjectPNP(
            x=self._config.observation_pose.x,
            y=self._config.observation_pose.y,
            z=self._config.observation_pose.z,
            roll=self._config.observation_pose.roll,
            pitch=self._config.observation_pose.pitch,
            yaw=self._config.observation_pose.yaw,
        )

    # *** PRIVATE STATIC/CLASS methods ***

    # *** PUBLIC properties ***

    def environment(self) -> EnvironmentProtocol:
        """Returns the environment associated with this workspace."""
        return self._environment

    # *** PRIVATE variables ***

    _environment: EnvironmentProtocol = None
    _logger: logging.Logger = None

Functions

__init__(workspace_id, environment, verbose=False, config=None)

Initializes the WidowXWorkspace.

Parameters:

Name	Type	Description	Default
workspace_id	str	Unique ID of the workspace.	required
environment	EnvironmentProtocol	Object implementing EnvironmentProtocol.	required
verbose	bool	Whether to enable verbose output.	False
config	WorkspaceConfig	Optional workspace configuration.	None

Source code in robot_workspace/workspaces/widowx_workspace.py

def __init__(
    self,
    workspace_id: str,
    environment: EnvironmentProtocol,
    verbose: bool = False,
    config: WorkspaceConfig | None = None,
) -> None:
    """
    Initializes the WidowXWorkspace.

    Args:
        workspace_id (str): Unique ID of the workspace.
        environment (EnvironmentProtocol): Object implementing EnvironmentProtocol.
        verbose (bool): Whether to enable verbose output.
        config (WorkspaceConfig, optional): Optional workspace configuration.
    """
    self._environment = environment
    self._config = config
    self._logger = logging.getLogger("robot_workspace")

    super().__init__(workspace_id, verbose)

environment()

Returns the environment associated with this workspace.

Source code in robot_workspace/workspaces/widowx_workspace.py

def environment(self) -> EnvironmentProtocol:
    """Returns the environment associated with this workspace."""
    return self._environment

from_config(config, environment, verbose=False) classmethod

Creates a WidowXWorkspace from a configuration object.

Parameters:

Name	Type	Description	Default
config	WorkspaceConfig	Workspace configuration.	required
environment	EnvironmentProtocol	Environment object.	required
verbose	bool	Whether to enable verbose output.	False

Returns:

Name	Type	Description
WidowXWorkspace	WidowXWorkspace	The initialized workspace instance.

Source code in robot_workspace/workspaces/widowx_workspace.py

@classmethod
def from_config(cls, config: WorkspaceConfig, environment: EnvironmentProtocol, verbose: bool = False) -> WidowXWorkspace:
    """
    Creates a WidowXWorkspace from a configuration object.

    Args:
        config (WorkspaceConfig): Workspace configuration.
        environment (EnvironmentProtocol): Environment object.
        verbose (bool): Whether to enable verbose output.

    Returns:
        WidowXWorkspace: The initialized workspace instance.
    """
    workspace = cls(config.id, environment, verbose, config)

    # Override observation pose from config if available
    if config.observation_pose:
        workspace._observation_pose = PoseObjectPNP(
            x=config.observation_pose.x,
            y=config.observation_pose.y,
            z=config.observation_pose.z,
            roll=config.observation_pose.roll,
            pitch=config.observation_pose.pitch,
            yaw=config.observation_pose.yaw,
        )

    # Override image shape from config if available
    if config.image_shape:
        workspace.set_img_shape(config.image_shape)

    return workspace

transform_camera2world_coords(workspace_id, u_rel, v_rel, yaw=0.0)

Transforms relative image coordinates to WidowX world coordinates.

Parameters:

Name	Type	Description	Default
workspace_id	str	ID of the workspace.	required
u_rel	float	Normalized horizontal coordinate [0, 1].	required
v_rel	float	Normalized vertical coordinate [0, 1].	required
yaw	float	Orientation of the object.	0.0

Returns:

Name	Type	Description
PoseObjectPNP	PoseObjectPNP	Corresponding pose in world coordinates.

Source code in robot_workspace/workspaces/widowx_workspace.py

def transform_camera2world_coords(self, workspace_id: str, u_rel: float, v_rel: float, yaw: float = 0.0) -> PoseObjectPNP:
    """
    Transforms relative image coordinates to WidowX world coordinates.

    Args:
        workspace_id (str): ID of the workspace.
        u_rel (float): Normalized horizontal coordinate [0, 1].
        v_rel (float): Normalized vertical coordinate [0, 1].
        yaw (float): Orientation of the object.

    Returns:
        PoseObjectPNP: Corresponding pose in world coordinates.
    """
    if self.verbose() and self._logger:
        self._logger.debug(
            f"transform_camera2world_coords input - workspace_id: {workspace_id}, u_rel: {u_rel}, v_rel: {v_rel}, yaw: {yaw}"
        )

    # Delegate to environment's transformation if available
    if hasattr(self, "_environment") and self._environment is not None:
        obj_coords = self._environment.get_robot_target_pose_from_rel(workspace_id, u_rel, v_rel, yaw)
        if self.verbose() and self._logger:
            self._logger.debug(f"transform_camera2world_coords output: {obj_coords}")
        return obj_coords

    # Fallback: Linear interpolation using workspace corners or defaults
    if self._xy_ul_wc is not None and self._xy_lr_wc is not None:
        x_min = self._xy_lr_wc.x
        x_max = self._xy_ul_wc.x
        y_min = self._xy_lr_wc.y
        y_max = self._xy_ul_wc.y

        x = x_max - u_rel * (x_max - x_min)
        y = y_max - v_rel * (y_max - y_min)
    else:
        x = 0.5 - u_rel * 0.4
        y = 0.2 - v_rel * 0.4

    obj_coords = PoseObjectPNP(x, y, 0.05, 0.0, 1.57, yaw)

    if self.verbose() and self._logger:
        self._logger.debug(f"transform_camera2world_coords output (fallback): {obj_coords}")

    return obj_coords

Functions