dexcontrol 0.2.10__py3-none-any.whl → 0.2.12__py3-none-any.whl

dexcontrol/sensors/camera/luxonis_camera.py

@@ -0,0 +1,169 @@
+# Copyright (C) 2025 Dexmate Inc.
+#
+# This software is dual-licensed:
+#
+# 1. GNU Affero General Public License v3.0 (AGPL-3.0)
+#    See LICENSE-AGPL for details
+#
+# 2. Commercial License
+#    For commercial licensing terms, contact: contact@dexmate.ai
+
+"""Luxonis wrist camera sensor using RGB and depth Zenoh subscribers.
+
+This sensor mirrors the high-level API of other camera sensors. It subscribes to
+RGB (JPEG over Zenoh) and depth streams published by dexsensor's Luxonis camera
+pipeline and exposes a simple interface for getting images.
+"""
+
+from __future__ import annotations
+
+import logging
+from typing import Any, Dict, Optional, Union
+
+import numpy as np
+import zenoh
+
+from dexcontrol.config.sensors.cameras.luxonis_camera import LuxonisCameraConfig
+from dexcontrol.utils.subscribers.camera import (
+    DepthCameraSubscriber,
+    RGBCameraSubscriber,
+)
+
+logger = logging.getLogger(__name__)
+
+
+class LuxonisCameraSensor:
+    """RGBD camera sensor wrapper for Luxonis/OAK wrist camera.
+
+    Provides access to RGB and depth frames via dedicated Zenoh subscribers.
+    """
+
+    def __init__(
+        self,
+        configs: LuxonisCameraConfig,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        self._name = configs.name
+        self._configs = configs
+        self._zenoh_session = zenoh_session
+
+        # Support left and right RGB streams + depth, mirroring ZED structure
+        self._subscribers: Dict[str, Optional[Union[RGBCameraSubscriber, DepthCameraSubscriber]]] = {}
+
+        subscriber_config = configs.subscriber_config
+
+        try:
+            # Create subscribers for left_rgb, right_rgb, depth (configurable enables)
+            stream_defs: Dict[str, Dict[str, Any]] = {
+                "left_rgb": subscriber_config.get("left_rgb", {}),
+                "right_rgb": subscriber_config.get("right_rgb", {}),
+                "depth": subscriber_config.get("depth", {}),
+            }
+
+            for stream_name, cfg in stream_defs.items():
+                if not cfg or not cfg.get("enable", False):
+                    self._subscribers[stream_name] = None
+                    continue
+                topic = cfg.get("topic")
+                if not topic:
+                    logger.warning(f"'{self._name}': No topic configured for '{stream_name}'")
+                    self._subscribers[stream_name] = None
+                    continue
+
+                if stream_name == "depth":
+                    self._subscribers[stream_name] = DepthCameraSubscriber(
+                        topic=topic,
+                        zenoh_session=self._zenoh_session,
+                        name=f"{self._name}_{stream_name}_subscriber",
+                        enable_fps_tracking=configs.enable_fps_tracking,
+                        fps_log_interval=configs.fps_log_interval,
+                    )
+                else:
+                    self._subscribers[stream_name] = RGBCameraSubscriber(
+                        topic=topic,
+                        zenoh_session=self._zenoh_session,
+                        name=f"{self._name}_{stream_name}_subscriber",
+                        enable_fps_tracking=configs.enable_fps_tracking,
+                        fps_log_interval=configs.fps_log_interval,
+                    )
+
+        except Exception as e:
+            logger.error(f"Error creating Luxonis wrist camera subscribers: {e}")
+
+    # Lifecycle
+    def shutdown(self) -> None:
+        for sub in self._subscribers.values():
+            if sub:
+                sub.shutdown()
+        logger.info(f"'{self._name}' sensor shut down.")
+
+    # Status
+    def is_active(self) -> bool:
+        return any(sub.is_active() for sub in self._subscribers.values() if sub is not None)
+
+    def wait_for_active(self, timeout: float = 5.0, require_both: bool = False) -> bool:
+        subs = [s for s in self._subscribers.values() if s is not None]
+        if not subs:
+            return False
+        if require_both:
+            return all(s.wait_for_active(timeout) for s in subs)
+        return any(s.wait_for_active(timeout) for s in subs)
+
+    # Data access
+    def get_obs(
+        self, obs_keys: Optional[list[str]] = None, include_timestamp: bool = False
+    ) -> Dict[str, Optional[np.ndarray]]:
+        """Get latest images.
+
+        obs_keys can include any of: ["left_rgb", "right_rgb", "depth"]. If None, returns
+        all available. If include_timestamp is True and the underlying subscriber returns
+        (image, timestamp), that tuple is forwarded; otherwise only the image is returned.
+        """
+        keys_to_fetch = obs_keys or self.available_streams
+
+        out: Dict[str, Optional[np.ndarray]] = {}
+        for key in keys_to_fetch:
+            sub = self._subscribers.get(key)
+            data = sub.get_latest_data() if sub else None
+            is_tuple_or_list = isinstance(data, (tuple, list))
+            if include_timestamp:
+                if not is_tuple_or_list and data is not None:
+                    logger.warning(f"Timestamp is not available yet for {key} stream.")
+                out[key] = data
+            else:
+                out[key] = data[0] if is_tuple_or_list else data
+        return out
+
+    def get_rgb(self) -> Optional[np.ndarray]:
+        # Backward-compat: return left_rgb if available else right_rgb
+        for key in ("left_rgb", "right_rgb"):
+            sub = self._subscribers.get(key)
+            if sub:
+                data = sub.get_latest_data()
+                return data[0] if isinstance(data, (tuple, list)) else data
+        return None
+
+    def get_depth(self) -> Optional[np.ndarray]:
+        sub = self._subscribers.get("depth")
+        if not sub:
+            return None
+        data = sub.get_latest_data()
+        return data[0] if isinstance(data, (tuple, list)) else data
+
+    # Properties
+    @property
+    def fps(self) -> Dict[str, float]:
+        return {name: sub.fps for name, sub in self._subscribers.items() if sub is not None}
+
+    @property
+    def name(self) -> str:
+        return self._name
+
+    @property
+    def available_streams(self) -> list:
+        return [name for name, sub in self._subscribers.items() if sub is not None]
+
+    @property
+    def active_streams(self) -> list:
+        return [name for name, sub in self._subscribers.items() if sub and sub.is_active()]
+

dexcontrol/sensors/camera/zed_camera.py

@@ -140,9 +140,6 @@ class ZedCameraSensor:
 
     def _query_camera_info(self) -> None:
         """Query Zenoh for camera metadata if using RTC."""
-        if not self._configs.use_rtc:
-            logger.info(f"'{self._name}': Skipping camera info query in non-RTC mode.")
-            return
 
         enabled_rgb_streams = [
             s
@@ -171,7 +168,6 @@ class ZedCameraSensor:
 
             logger.info(f"'{self._name}': Querying for camera info at '{final_info_key}'.")
             self._camera_info = query_zenoh_json(self._zenoh_session, final_info_key)
-
             if self._camera_info:
                 logger.info(f"'{self._name}': Successfully received camera info.")
             else:
@@ -249,7 +245,8 @@ class ZedCameraSensor:
             return False
 
     def get_obs(
-        self, obs_keys: Optional[list[str]] = None
+        self, obs_keys: Optional[list[str]] = None,
+        include_timestamp: bool = False
     ) -> Dict[str, Optional[np.ndarray]]:
         """Get the latest observation data from specified camera streams.
 
@@ -257,17 +254,29 @@ class ZedCameraSensor:
             obs_keys: A list of stream names to retrieve data from (e.g.,
                       ['left_rgb', 'depth']). If None, retrieves data from all
                       enabled streams.
+            include_timestamp: If True, includes the timestamp in the observation data.
+                                The timestamp data is not available for RTC streams.
 
         Returns:
-            A dictionary mapping stream names to their latest image data. The
+            A dictionary mapping stream names to their latest image data with timestamp. The
             image is a numpy array (HxWxC for RGB, HxW for depth) or None if
-            no data is available for that stream.
+            no data is available for that stream. If include_timestamp is True,
+            the value in the dictionary is a tuple with the image and timestamp.
         """
         keys_to_fetch = obs_keys or self.available_streams
         obs_out = {}
         for key in keys_to_fetch:
             subscriber = self._subscribers.get(key)
-            obs_out[key] = subscriber.get_latest_data() if subscriber else None
+            data = subscriber.get_latest_data() if subscriber else None
+
+            is_tuple_or_list = isinstance(data, (tuple, list))
+
+            if include_timestamp:
+                if not is_tuple_or_list:
+                    logger.warning(f"Timestamp is not available yet for {key} stream.")
+                obs_out[key] = data
+            else:
+                obs_out[key] = data[0] if is_tuple_or_list else data
         return obs_out
 
     def get_left_rgb(self) -> Optional[np.ndarray]:

dexcontrol/sensors/imu/chassis_imu.py

@@ -76,7 +76,7 @@ class ChassisIMUSensor:
         """
         return self._subscriber.wait_for_active(timeout)
 
-    def get_obs(self, obs_keys: list[Literal['ang_vel', 'acc', 'quat']] | None = None) -> dict[str, np.ndarray]:
+    def get_obs(self, obs_keys: list[Literal['ang_vel', 'acc', 'quat']] | None = None) -> dict[Literal['ang_vel', 'acc', 'quat', 'timestamp_ns'], np.ndarray]:
         """Get observation data for the ZED IMU sensor.
 
         Args:
@@ -89,6 +89,7 @@ class ChassisIMUSensor:
             - 'ang_vel': Angular velocity from 'angular_velocity'
             - 'acc': Linear acceleration from 'acceleration'
             - 'quat': Orientation quaternion from 'orientation', in wxyz convention
+            - 'timestamp_ns': Timestamp in nanoseconds
         """
         if obs_keys is None:
             obs_keys = ['ang_vel', 'acc', 'quat']
@@ -110,6 +111,9 @@ class ChassisIMUSensor:
             else:
                 raise ValueError(f"Invalid observation key: {key}")
 
+        if hasattr(data, 'timestamp_ns'):
+            obs_out['timestamp_ns'] = data.timestamp_ns
+
         return obs_out
 
     def get_acceleration(self) -> np.ndarray:

dexcontrol/sensors/imu/zed_imu.py

@@ -75,7 +75,7 @@ class ZedIMUSensor:
 
         Args:
             obs_keys: List of observation keys to retrieve. If None, returns all available data.
-                     Valid keys: ['ang_vel', 'acc', 'quat']
+                     Valid keys: ['ang_vel', 'acc', 'quat', 'timestamp']
 
         Returns:
             Dictionary with observation data including all IMU measurements.
@@ -83,6 +83,7 @@ class ZedIMUSensor:
             - 'ang_vel': Angular velocity from 'angular_velocity'
             - 'acc': Linear acceleration from 'acceleration'
             - 'quat': Orientation quaternion from 'orientation'
+            - 'timestamp_ns': Timestamp from 'timestamp'
         """
         if obs_keys is None:
             obs_keys = ['ang_vel', 'acc', 'quat']
@@ -91,7 +92,7 @@ class ZedIMUSensor:
         if data is None:
             return None
 
-        obs_out = {}
+        obs_out = {'timestamp_ns': data['timestamp']}
 
         for key in obs_keys:
             if key == 'ang_vel':

dexcontrol/sensors/lidar/rplidar.py

@@ -87,6 +87,7 @@ class RPLidarSensor:
                 - ranges: Array of range measurements in meters
                 - angles: Array of corresponding angles in radians
                 - qualities: Array of quality values (0-255) if available, None otherwise
+                - timestamp: Timestamp in nanoseconds (int)
         """
         return self._subscriber.get_latest_data()
 

dexcontrol/sensors/manager.py

@@ -26,6 +26,7 @@ from omegaconf import DictConfig, OmegaConf
 from dexcontrol.config.sensors.vega_sensors import VegaSensorsConfig
 
 if TYPE_CHECKING:
+    from dexcontrol.sensors.camera.luxonis_camera import LuxonisCameraSensor
     from dexcontrol.sensors.camera.rgb_camera import RGBCameraSensor
     from dexcontrol.sensors.camera.zed_camera import ZedCameraSensor
     from dexcontrol.sensors.imu.chassis_imu import ChassisIMUSensor
@@ -49,6 +50,8 @@ class Sensors:
     if TYPE_CHECKING:
         # Type annotations for dynamically created sensor attributes
         head_camera: ZedCameraSensor
+        left_wrist_camera: LuxonisCameraSensor
+        right_wrist_camera: LuxonisCameraSensor
         base_left_camera: RGBCameraSensor
         base_right_camera: RGBCameraSensor
         base_front_camera: RGBCameraSensor

dexcontrol/utils/constants.py

@@ -20,3 +20,6 @@ COMM_CFG_PATH_ENV_VAR: Final[str] = "DEXMATE_COMM_CFG_PATH"
 
 # Environment variable to disable heartbeat monitoring
 DISABLE_HEARTBEAT_ENV_VAR: Final[str] = "DEXCONTROL_DISABLE_HEARTBEAT"
+
+# Environment variable to disable estop checking
+DISABLE_ESTOP_CHECKING_ENV_VAR: Final[str] = "DEXCONTROL_DISABLE_ESTOP_CHECKING"

dexcontrol/utils/error_code.py

@@ -0,0 +1,236 @@
+# Copyright (C) 2025 Dexmate Inc.
+#
+# This software is dual-licensed:
+#
+# 1. GNU Affero General Public License v3.0 (AGPL-3.0)
+#    See LICENSE-AGPL for details
+#
+# 2. Commercial License
+#    For commercial licensing terms, contact: contact@dexmate.ai
+
+"""Error code interpretation utilities for robot components."""
+
+from typing import Dict
+
+
+class ErrorCodeInterpreter:
+    """Interprets error codes for different robot components."""
+
+    # Arm error codes (left_arm and right_arm)
+    ARM_ERROR_CODES = {
+        0x00000020: "Overtemperature",
+        0x00000080: "Encoder error",
+        0x00000200: "Software error",
+        0x00000400: "Temperature sensor error",
+        0x00000800: "Position limit exceeded",
+        0x00002000: "Position tracking error exceeded",
+        0x00004000: "Current detection error",
+        0x00008000: "Brake failure",
+        0x00010000: "Position command limit exceeded",
+    }
+
+    # Head error codes
+    HEAD_ERROR_CODES = {
+        0x00: "Disabled",
+        0x04: "Enabled",
+        0x20: "Overvoltage",
+        0x24: "Undervoltage",
+    }
+
+    # Chassis wheel error codes (common)
+    CHASSIS_COMMON_CODES = {
+        0x00000000: "No error",
+        0x00000004: "Overvoltage",
+        0x00000008: "Undervoltage",
+    }
+
+    # Left drive wheel motor specific
+    CHASSIS_LEFT_DRIVE_CODES = {
+        0x00000010: "Overcurrent",
+        0x00000020: "Overload",
+        0x00000080: "Encoder deviation",
+        0x00000200: "Reference voltage error",
+        0x00000800: "Hall sensor error",
+        0x00001000: "Motor overtemperature",
+        0x00002000: "Encoder error",
+    }
+
+    # Right drive wheel motor specific
+    CHASSIS_RIGHT_DRIVE_CODES = {
+        0x00100000: "Overcurrent",
+        0x00200000: "Overload",
+        0x00800000: "Encoder deviation",
+        0x02000000: "Reference voltage error",
+        0x08000000: "Hall sensor error",
+        0x10000000: "Motor overtemperature",
+        0x20000000: "Encoder error",
+    }
+
+    # Steering wheel error codes (left and right)
+    CHASSIS_STEERING_CODES = {
+        0xFC00000C: "Undervoltage",
+        0xFC000010: "Overvoltage",
+    }
+
+    # Torso motor error codes
+    TORSO_ERROR_CODES = {
+        0x00000008: "Overvoltage",
+        0x00000010: "Undervoltage",
+        0x00000040: "Startup error",
+        0x00000080: "Speed feedback error",
+        0x00000100: "Overcurrent",
+    }
+
+    # BMS (Battery Management System) alarm codes
+    BMS_ERROR_CODES = {
+        0x0004: "Single cell overvoltage",
+        0x0008: "Single cell undervoltage",
+        0x0010: "Total overvoltage",
+        0x0020: "Total undervoltage",
+        0x0040: "High temperature",
+        0x0080: "Low temperature",
+        0x0100: "Discharge overcurrent",
+        0x0200: "Charge overcurrent",
+        # Bits 8-15 are reserved
+    }
+
+    @classmethod
+    def interpret_error(cls, component: str, error_code: int) -> str:
+        """
+        Interpret error code for a specific component.
+
+        Args:
+            component: Component name (e.g., 'left_arm', 'right_arm', 'head', etc.)
+            error_code: Error code to interpret
+
+        Returns:
+            Human-readable error description or hex code if unknown
+        """
+        component_lower = component.lower()
+
+        # Arms (left and right)
+        if "left_arm" in component_lower or "right_arm" in component_lower:
+            return cls._interpret_bitmask_errors(error_code, cls.ARM_ERROR_CODES)
+
+        # Hands (left and right) - same as arms
+        elif "left_hand" in component_lower or "right_hand" in component_lower:
+            return cls._interpret_bitmask_errors(error_code, cls.ARM_ERROR_CODES)
+
+        # Head
+        elif "head" in component_lower:
+            return cls.HEAD_ERROR_CODES.get(
+                error_code, f"Unknown error: 0x{error_code:02X}"
+            )
+
+        # Chassis (includes all wheel errors)
+        elif "chassis" in component_lower:
+            return cls._interpret_chassis_errors(error_code)
+
+        # Torso
+        elif "torso" in component_lower:
+            return cls._interpret_bitmask_errors(error_code, cls.TORSO_ERROR_CODES)
+
+        # BMS (Battery Management System)
+        elif "bms" in component_lower:
+            return cls._interpret_bitmask_errors(error_code, cls.BMS_ERROR_CODES)
+
+        else:
+            return f"Unknown component error: 0x{error_code:08X}"
+
+    @classmethod
+    def _interpret_bitmask_errors(
+        cls, error_code: int, error_dict: Dict[int, str]
+    ) -> str:
+        """
+        Interpret bitmask-style error codes.
+
+        Args:
+            error_code: Error code with multiple possible bits set
+            error_dict: Dictionary mapping bit masks to error descriptions
+
+        Returns:
+            Comma-separated list of active errors or hex code if none found
+        """
+        if error_code == 0:
+            return "No error"
+
+        errors = []
+        for mask, description in error_dict.items():
+            if error_code & mask:
+                errors.append(description)
+
+        if errors:
+            return ", ".join(errors)
+        else:
+            return f"Unknown error: 0x{error_code:08X}"
+
+    @classmethod
+    def _interpret_chassis_errors(cls, error_code: int) -> str:
+        """
+        Interpret chassis-specific error codes (includes all wheel errors).
+
+        Args:
+            error_code: Error code to interpret
+
+        Returns:
+            Human-readable error description
+        """
+        # Check steering wheel codes first (they have specific values)
+        if error_code in cls.CHASSIS_STEERING_CODES:
+            return cls.CHASSIS_STEERING_CODES[error_code]
+
+        # Check common codes
+        if error_code == 0:
+            return "No error"
+
+        errors = []
+
+        # Check common chassis errors
+        for mask, description in cls.CHASSIS_COMMON_CODES.items():
+            if mask != 0 and (error_code & mask):
+                errors.append(description)
+
+        # Check left drive wheel specific
+        for mask, description in cls.CHASSIS_LEFT_DRIVE_CODES.items():
+            if error_code & mask:
+                errors.append(f"Left drive: {description}")
+
+        # Check right drive wheel specific
+        for mask, description in cls.CHASSIS_RIGHT_DRIVE_CODES.items():
+            if error_code & mask:
+                errors.append(f"Right drive: {description}")
+
+        if errors:
+            return ", ".join(errors)
+        else:
+            return f"Unknown chassis error: 0x{error_code:08X}"
+
+
+def get_error_description(component: str, error_code: int) -> str:
+    """
+    Convenience function to get error description.
+
+    Args:
+        component: Component name
+        error_code: Error code to interpret
+
+    Returns:
+        Human-readable error description
+    """
+    return ErrorCodeInterpreter.interpret_error(component, error_code)
+
+
+def get_multiple_errors(components_errors: Dict[str, int]) -> Dict[str, str]:
+    """
+    Get error descriptions for multiple components.
+
+    Args:
+        components_errors: Dictionary mapping component names to error codes
+
+    Returns:
+        Dictionary mapping component names to error descriptions
+    """
+    return {
+        component: get_error_description(component, error_code)
+        for component, error_code in components_errors.items()
+    }

dexcontrol/utils/subscribers/lidar.py

@@ -92,6 +92,7 @@ class LidarSubscriber(BaseZenohSubscriber):
                 - ranges: Array of range measurements in meters
                 - angles: Array of corresponding angles in radians
                 - qualities: Array of quality values (0-255) if available, None otherwise
+                - timestamp: Timestamp in nanoseconds (int)
         """
         with self._data_lock:
             if self._latest_raw_data is None:

dexcontrol/utils/trajectory_utils.py

@@ -16,7 +16,7 @@ import numpy as np
 def generate_linear_trajectory(
     current_pos: np.ndarray,
     target_pos: np.ndarray,
-    max_vel: float = 0.5,
+    max_vel: float | np.ndarray = 0.5,
     control_hz: float = 100,
 ) -> tuple[np.ndarray, int]:
     """Generate a linear trajectory between current and target positions.
@@ -24,7 +24,9 @@ def generate_linear_trajectory(
     Args:
         current_pos: Current position array.
         target_pos: Target position array.
-        max_vel: Maximum velocity in units per second.
+        max_vel: Maximum velocity in units per second. Can be:
+            - float: Same velocity limit for all dimensions
+            - numpy array: Per-dimension velocity limits (same length as current_pos)
         control_hz: Control frequency in Hz.
 
     Returns:
@@ -32,9 +34,19 @@ def generate_linear_trajectory(
         - trajectory: Array of waypoints from current to target position.
         - num_steps: Number of steps in the trajectory.
     """
-    # Calculate linear interpolation between current and target positions
-    max_diff = np.max(np.abs(target_pos - current_pos))
-    num_steps = int(max_diff / max_vel * control_hz)
+    # Calculate time needed for each dimension
+    pos_diff = np.abs(target_pos - current_pos)
+
+    if isinstance(max_vel, np.ndarray):
+        # Per-dimension velocity limits - find the dimension that takes longest
+        time_needed = pos_diff / max_vel
+        max_time = np.max(time_needed)
+    else:
+        # Single velocity limit for all dimensions
+        max_diff = np.max(pos_diff)
+        max_time = max_diff / max_vel
+
+    num_steps = int(max_time * control_hz)
 
     # Ensure at least one step
     num_steps = max(1, num_steps)