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

dexcontrol/sensors/camera/luxonis_camera.py

@@ -1,169 +0,0 @@
-# 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/utils/motion_utils.py

@@ -1,199 +0,0 @@
-# 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
-
-
-import numpy as np
-import ruckig
-from jaxtyping import Float
-
-
-class ArmOfflineTrajectorySmoother:
-    """Class to generate smooth trajectories that respect joint limits."""
-
-    def __init__(
-        self,
-        dt: float = 0.01,
-        safety_factor: float = 2.0,
-    ) -> None:
-        """Initialize trajectory smoother with motion constraints.
-
-        Args:
-            dt: Control cycle time in seconds. Must be positive.
-            safety_factor: Factor to scale down motion limits for additional safety.
-                         Must be positive. Larger values mean more conservative motion.
-
-        Raises:
-            ValueError: If dt or safety_factor is not positive.
-        """
-        if dt <= 0:
-            raise ValueError("Control cycle must be positive")
-        if safety_factor <= 0:
-            raise ValueError("Safety factor must be positive")
-
-        self.arm_dof = 7  # 7 degrees of freedom for each arm
-        self.dt = dt
-
-        # Initialize Ruckig Online Trajectory Generation
-        self.otg = ruckig.Ruckig(self.arm_dof, dt)
-        self.inp = ruckig.InputParameter(self.arm_dof)
-        self.out = ruckig.OutputParameter(self.arm_dof)
-
-        # Set motion limits for each joint (in radians)
-        # Convert degrees to radians and apply safety factor
-        self.inp.max_velocity = (
-            np.deg2rad([180, 180, 220, 220, 220, 220, 220]) / safety_factor
-        )
-        self.inp.max_acceleration = (
-            np.deg2rad([600, 600, 600, 600, 600, 600, 600]) / safety_factor
-        )
-        self.inp.max_jerk = (
-            np.deg2rad([6000, 6000, 6000, 6000, 6000, 6000, 6000]) / safety_factor
-        )
-
-    def smooth_trajectory(
-        self, waypoints: Float[np.ndarray, "N 7"], trajectory_dt: float
-    ) -> tuple[Float[np.ndarray, "M 7"], Float[np.ndarray, "M 7"]]:
-        """Generate time-optimal smooth trajectory through waypoints.
-
-        Args:
-            waypoints: Array of joint positions with shape (N, 7) where N is the number
-                      of waypoints and 7 is the number of joints.
-            trajectory_dt: Desired time duration between waypoints (unit: s).
-
-        Returns:
-            A tuple containing:
-                - Position trajectory with shape (M, 7)
-                - Velocity trajectory with shape (M, 7)
-
-        Raises:
-            ValueError: If fewer than 2 waypoints are provided or if they don't match
-                       the expected 7-DOF format.
-        """
-        if len(waypoints) < 2:
-            raise ValueError("At least two waypoints are required")
-        if waypoints.shape[1] != self.arm_dof:
-            raise ValueError(f"Waypoints must have shape (N, {self.arm_dof})")
-
-        pos_traj = []
-        vel_traj = []
-        n_intermediate_points = int(trajectory_dt / self.dt)
-        self.inp.current_position = waypoints[0]
-
-        # Generate smooth trajectory segments between waypoints
-        for i in range(1, len(waypoints)):
-            self.inp.target_position = waypoints[i]
-            self.inp.target_velocity = [0.0] * self.arm_dof
-
-            for _ in range(n_intermediate_points):
-                _ = self.otg.update(self.inp, self.out)
-                pos_traj.append(self.out.new_position)
-                vel_traj.append(self.out.new_velocity)
-                self.out.pass_to_input(self.inp)
-
-        return np.array(pos_traj), np.array(vel_traj)
-
-
-class ArmOnlineTrajectorySmoother:
-    def __init__(
-        self,
-        init_qpos: np.ndarray,
-        control_cycle: float = 0.005,
-        safety_factor: float = 2.0,
-    ) -> None:
-        self.dof = len(init_qpos)
-
-        # Initialize Ruckig
-        self.otg = ruckig.Ruckig(self.dof, control_cycle)
-        self.inp = ruckig.InputParameter(self.dof)
-        self.out = ruckig.OutputParameter(self.dof)
-        self.inp.current_position = init_qpos
-
-        # Set motion limits
-        self.inp.max_velocity = (
-            np.deg2rad([180, 180, 220, 220, 220, 220, 220]) / safety_factor
-        )
-        self.inp.max_acceleration = (
-            np.deg2rad([600, 600, 600, 600, 600, 600, 600]) / safety_factor
-        )
-        self.inp.max_jerk = (
-            np.deg2rad([6000, 6000, 6000, 6000, 6000, 6000, 6000]) / safety_factor
-        )
-
-    def update(
-        self, target_position: np.ndarray | None = None
-    ) -> tuple[np.ndarray, np.ndarray]:
-        """Update trajectory with new target position."""
-        if target_position is not None:
-            self.inp.target_position = target_position
-            self.inp.target_velocity = [0.0] * self.dof
-
-        _ = self.otg.update(self.inp, self.out)
-        self.out.pass_to_input(self.inp)
-
-        return np.array(self.out.new_position), np.array(self.out.new_velocity)
-
-    def reset(self, init_qpos: np.ndarray):
-        self.inp.current_position = init_qpos
-        self.inp.current_velocity = [0.0] * self.dof
-
-
-def linear_interpolation(
-    start: Float[np.ndarray, " DoF"],
-    end: Float[np.ndarray, " DoF"],
-    steps: int,
-) -> Float[np.ndarray, "steps DoF"]:
-    """Generate linear interpolation between start and end joint positions.
-
-    Args:
-        start: Starting joint positions array of shape (DoF,)
-        end: Ending joint positions array of shape (DoF,)
-        steps: Number of interpolation steps (including start and end points)
-
-    Returns:
-        Array of interpolated joint positions with shape (steps, DoF)
-        where DoF is the degrees of freedom (matches input dimensions)
-    """
-    t = np.linspace(0, 1, steps)[:, None]
-    return (1 - t) * start + t * end
-
-
-def plan_arm_move_to_target(
-    init_qpos: Float[np.ndarray, "7"],
-    target_qpos: Float[np.ndarray, "7"],
-    max_qv: float,
-    dt: float = 0.01,
-) -> tuple[Float[np.ndarray, "steps 7"], Float[np.ndarray, "steps 7"]]:
-    """Plan a smooth arm movement from initial to target joint positions.
-
-    Args:
-        init_qpos: Initial joint positions array of shape (7,)
-        target_qpos: Target joint positions array of shape (7,)
-        max_qv: Maximum allowed joint velocity magnitude (radians/second)
-        dt: Control cycle time in seconds (default: 0.01)
-
-    Returns:
-        A tuple containing:
-            - Position trajectory with shape (steps, 7)
-            - Velocity trajectory with shape (steps, 7)
-            Both trajectories include the final target state.
-    """
-    duration = np.linalg.norm(target_qpos - init_qpos) / max_qv
-    steps = int(duration / dt)
-    trajectory = linear_interpolation(init_qpos, target_qpos, steps)
-
-    # Smooth the trajectory while respecting motion constraints
-    smoother = ArmOfflineTrajectorySmoother(dt=dt)
-    pos_traj, vel_traj = smoother.smooth_trajectory(trajectory, dt)
-
-    # Append final target state with zero velocity
-    pos_traj = np.concatenate([pos_traj, target_qpos[None, :]])
-    vel_traj = np.concatenate([vel_traj, np.zeros((1, 7))])
-
-    return pos_traj, vel_traj

dexcontrol/utils/rate_limiter.py

@@ -1,172 +0,0 @@
-# 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
-
-"""Rate limiter utility for maintaining consistent execution rates."""
-
-import sys
-import time
-from typing import Final
-
-from loguru import logger
-
-
-class RateLimiter:
-    """Class for limiting execution rate to a target frequency.
-
-    This class provides rate limiting functionality by sleeping between iterations to
-    maintain a desired execution frequency. It also tracks statistics about the achieved
-    rate and missed deadlines.
-
-    Attributes:
-        period_sec: Time period between iterations in seconds.
-        target_rate_hz: Desired execution rate in Hz.
-        window_size: Size of moving average window for rate calculations.
-        last_time_sec: Timestamp of the last iteration.
-        next_time_sec: Scheduled timestamp for the next iteration.
-        start_time_sec: Timestamp when the rate limiter was initialized or reset.
-        duration_buffer: List of recent iteration durations for rate calculation.
-        missed_deadlines: Counter for iterations that missed their scheduled time.
-        iterations: Total number of iterations since initialization or reset.
-    """
-
-    def __init__(self, rate_hz: float, window_size: int = 50) -> None:
-        """Initializes rate limiter.
-
-        Args:
-            rate_hz: Desired rate in Hz.
-            window_size: Size of moving average window for rate calculations.
-
-        Raises:
-            ValueError: If rate_hz is not positive.
-        """
-        if rate_hz <= 0:
-            raise ValueError("Rate must be positive")
-
-        self.period_sec: Final[float] = 1.0 / rate_hz
-        self.target_rate_hz: Final[float] = rate_hz
-        self.window_size: Final[int] = max(1, window_size)
-
-        # Initialize timing variables
-        now_sec = time.monotonic()
-        self.last_time_sec: float = now_sec
-        self.next_time_sec: float = now_sec + self.period_sec
-        self.start_time_sec: float = now_sec
-
-        # Initialize statistics
-        self.duration_buffer: list[float] = []
-        self.missed_deadlines: int = 0
-        self.iterations: int = 0
-        self._MAX_ITERATIONS: Final[int] = sys.maxsize - 1000  # Leave some buffer
-
-    def sleep(self) -> None:
-        """Sleeps to maintain desired rate.
-
-        Sleeps for the appropriate duration to maintain the target rate. If the next
-        scheduled time has already passed, increments the missed deadlines counter.
-        Uses monotonic time for reliable timing regardless of system clock changes.
-        """
-        current_time_sec = time.monotonic()
-        sleep_time_sec = self.next_time_sec - current_time_sec
-
-        if sleep_time_sec > 0:
-            time.sleep(sleep_time_sec)
-        else:
-            self.missed_deadlines += 1
-
-        # Update timing and statistics
-        now_sec = time.monotonic()
-
-        # Reset iterations if approaching max value
-        if self.iterations >= self._MAX_ITERATIONS:
-            logger.warning(
-                "Iteration counter approaching max value, resetting statistics"
-            )
-            self.reset()
-            return
-
-        self.iterations += 1
-
-        if self.iterations > 1:  # Skip first iteration
-            duration_sec = now_sec - self.last_time_sec
-            if len(self.duration_buffer) >= self.window_size:
-                self.duration_buffer.pop(0)
-            self.duration_buffer.append(duration_sec)
-
-        self.last_time_sec = now_sec
-
-        # More efficient way to advance next_time_sec when multiple periods behind
-        periods_behind = max(
-            0, int((now_sec - self.next_time_sec) / self.period_sec) + 1
-        )
-        self.next_time_sec += periods_behind * self.period_sec
-
-    def get_actual_rate(self) -> float:
-        """Calculates actual achieved rate in Hz using moving average.
-
-        Returns:
-            Current execution rate based on recent iterations.
-        """
-        if not self.duration_buffer:
-            return 0.0
-        avg_duration_sec = sum(self.duration_buffer) / len(self.duration_buffer)
-        return 0.0 if avg_duration_sec <= 0 else 1.0 / avg_duration_sec
-
-    def get_average_rate(self) -> float:
-        """Calculates average rate over entire run.
-
-        Returns:
-            Average execution rate since start or last reset.
-        """
-        if self.iterations < 2:
-            return 0.0
-        total_time_sec = time.monotonic() - self.start_time_sec
-        return 0.0 if total_time_sec <= 0 else self.iterations / total_time_sec
-
-    def reset(self) -> None:
-        """Resets the rate limiter state and statistics."""
-        now_sec = time.monotonic()
-        self.last_time_sec = now_sec
-        self.next_time_sec = now_sec + self.period_sec
-        self.start_time_sec = now_sec
-        self.duration_buffer.clear()
-        self.missed_deadlines = 0
-        self.iterations = 0
-
-    def get_stats(self) -> dict[str, float | int]:
-        """Gets runtime statistics.
-
-        Returns:
-            Dictionary containing execution statistics including actual rate,
-            average rate, target rate, missed deadlines and iteration count.
-        """
-        return {
-            "actual_rate": self.get_actual_rate(),
-            "average_rate": self.get_average_rate(),
-            "target_rate": self.target_rate_hz,
-            "missed_deadlines": self.missed_deadlines,
-            "iterations": self.iterations,
-        }
-
-
-if __name__ == "__main__":
-    rate_limiter = RateLimiter(100.0)  # 100Hz
-
-    try:
-        while True:
-            rate_limiter.sleep()
-            actual_rate = rate_limiter.get_actual_rate()
-            logger.info(f"Rate: {actual_rate:.2f} Hz")
-
-    except KeyboardInterrupt:
-        stats = rate_limiter.get_stats()
-        logger.info("\nFinal stats:")
-        logger.info(f"Average rate: {stats['average_rate']:.2f} Hz")
-        logger.info(f"Final rate: {stats['actual_rate']:.2f} Hz")
-        logger.info(f"Missed deadlines: {stats['missed_deadlines']}")

dexcontrol/utils/rtc_utils.py

@@ -1,144 +0,0 @@
-# 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
-
-"""RTC utilities for dexcontrol.
-
-This module provides utility functions for creating RTC subscribers
-that first query Zenoh for connection information.
-"""
-
-import zenoh
-from loguru import logger
-
-from dexcontrol.utils.subscribers.rtc import RTCSubscriber
-from dexcontrol.utils.zenoh_utils import query_zenoh_json
-
-
-def query_rtc_info(
-    zenoh_session: zenoh.Session,
-    info_topic: str,
-    timeout: float = 2.0,
-    max_retries: int = 1,
-    retry_delay: float = 0.5,
-) -> dict | None:
-    """Query Zenoh for RTC connection information.
-
-    Args:
-        zenoh_session: Active Zenoh session for communication.
-        info_topic: Zenoh topic to query for RTC info.
-        timeout: Maximum time to wait for a response in seconds.
-        max_retries: Maximum number of retry attempts.
-        retry_delay: Initial delay between retries (doubles each retry).
-
-    Returns:
-        Dictionary containing host and port information if successful, None otherwise.
-    """
-
-    # Use the general Zenoh query function
-    info = query_zenoh_json(
-        zenoh_session=zenoh_session,
-        topic=info_topic,
-        timeout=timeout,
-        max_retries=max_retries,
-        retry_delay=retry_delay,
-    )
-
-    return info
-
-
-def create_rtc_subscriber_from_zenoh(
-    zenoh_session: zenoh.Session,
-    info_topic: str,
-    name: str = "rtc_subscriber",
-    enable_fps_tracking: bool = True,
-    fps_log_interval: int = 100,
-    query_timeout: float = 2.0,
-    max_retries: int = 1,
-) -> RTCSubscriber | None:
-    """Create a RTC subscriber by first querying Zenoh for connection info.
-
-    Args:
-        zenoh_session: Active Zenoh session for communication.
-        info_topic: Zenoh topic to query for RTC connection info.
-        name: Name for logging purposes.
-        enable_fps_tracking: Whether to track and log FPS metrics.
-        fps_log_interval: Number of frames between FPS calculations.
-        query_timeout: Maximum time to wait for Zenoh query response.
-        max_retries: Maximum number of retry attempts for Zenoh query.
-
-    Returns:
-        RTCSubscriber instance if successful, None otherwise.
-    """
-    # Query Zenoh for RTC connection information
-    rtc_info = query_rtc_info(zenoh_session, info_topic, query_timeout, max_retries)
-
-    if rtc_info is None:
-        logger.error("Failed to get RTC connection info from Zenoh")
-        return None
-
-    url = rtc_info.get("signaling_url")
-
-    if not url:
-        logger.error(f"Invalid RTC info: url={url}")
-        return None
-
-    # Construct WebSocket URL
-    ws_url = url
-    logger.info(f"Creating RTC subscriber with URL: {ws_url}")
-
-    try:
-        # Create and return the RTC subscriber
-        subscriber = RTCSubscriber(
-            url=ws_url,
-            name=name,
-            enable_fps_tracking=enable_fps_tracking,
-            fps_log_interval=fps_log_interval,
-        )
-        return subscriber
-    except Exception as e:
-        logger.error(f"Failed to create RTC subscriber: {e}")
-        return None
-
-
-def create_rtc_subscriber_with_config(
-    zenoh_session: zenoh.Session,
-    config,
-    name: str = "rtc_subscriber",
-    enable_fps_tracking: bool = True,
-    fps_log_interval: int = 100,
-) -> RTCSubscriber | None:
-    """Create a RTC subscriber using configuration object.
-
-    Args:
-        zenoh_session: Active Zenoh session for communication.
-        config: Configuration object containing info_key.
-        name: Name for logging purposes.
-        enable_fps_tracking: Whether to track and log FPS metrics.
-        fps_log_interval: Number of frames between FPS calculations.
-    Returns:
-        RTCSubscriber instance if successful, None otherwise.
-    """
-    if "info_key" not in config:
-        logger.error("Config subscriber_config missing info_key")
-        return None
-
-    if not config["enable"]:
-        logger.info(f"Skipping {name} because it is disabled")
-        return None
-
-    info_topic = config["info_key"]
-
-    return create_rtc_subscriber_from_zenoh(
-        zenoh_session=zenoh_session,
-        info_topic=info_topic,
-        name=name,
-        enable_fps_tracking=enable_fps_tracking,
-        fps_log_interval=fps_log_interval,
-    )