dexcontrol 0.2.1__py3-none-any.whl

dexcontrol/utils/motion_utils.py

@@ -0,0 +1,194 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+
+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/os_utils.py

@@ -0,0 +1,39 @@
+"""Operating system utility functions."""
+
+import os
+from typing import Final
+
+from loguru import logger
+
+from dexcontrol.utils.constants import ROBOT_NAME_ENV_VAR
+
+
+def resolve_key_name(key: str) -> str:
+    """Resolves a key name for zenoh topic by prepending robot name.
+
+    Args:
+        key: Original key name (e.g. 'lidar' or '/lidar')
+
+    Returns:
+        Resolved key with robot name prepended (e.g. 'robot/lidar')
+    """
+    # Get robot name from env var or use default
+    robot_name: Final[str] = os.getenv(ROBOT_NAME_ENV_VAR, "robot")
+
+    # Remove leading slash if present
+    key = key.lstrip("/")
+
+    # Combine robot name and key with single slash
+    return f"{robot_name}/{key}"
+
+
+def get_robot_model() -> str:
+    """Get the robot model from the environment variable."""
+    robot_model_abb_mapping = dict(vg="vega")
+    robot_name = os.getenv(ROBOT_NAME_ENV_VAR, "robot")
+    robot_model_abb = robot_name.split("/")[-1].split("-")[0][:2]
+    if robot_model_abb not in robot_model_abb_mapping:
+        raise ValueError(f"Unknown robot model: {robot_model_abb}")
+    model = robot_model_abb_mapping[robot_model_abb] + "-" + robot_name.split("-")[-1]
+    logger.info(f"The current robot model is: {model}")
+    return model

dexcontrol/utils/pb_utils.py

@@ -0,0 +1,103 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Utility functions for Python data structures and protobuf messages."""
+
+from enum import Enum
+from typing import Any, Literal
+
+from dexcontrol.proto import dexcontrol_query_pb2
+
+
+def software_version_to_dict(
+    version_msg: dexcontrol_query_pb2.SoftwareVersion,
+) -> dict[str, dict[Literal["hardware_version", "software_version", "main_hash"], Any]]:
+    """Convert a SoftwareVersion protobuf message to a dictionary.
+
+    Args:
+        version_msg: SoftwareVersion protobuf message.
+
+    Returns:
+        Dictionary containing version information with component names as keys.
+    """
+    return {
+        key: {
+            "hardware_version": value.hardware_version,
+            "software_version": value.software_version,
+            "main_hash": value.main_hash,
+        }
+        for key, value in version_msg.firmware_version.items()
+    }
+
+
+class ComponentStatus(Enum):
+    """Enum representing the status of a component."""
+
+    NORMAL = 0
+    NA = 1
+    ERROR = 2
+
+
+def status_to_enum(status: dexcontrol_query_pb2.ComponentStatus) -> ComponentStatus:
+    """Convert a ComponentStatus protobuf message to a ComponentStatus enum.
+
+    Args:
+        status: ComponentStatus protobuf message.
+
+    Returns:
+        ComponentStatus enum value.
+
+    Raises:
+        ValueError: If the status value is not recognized.
+    """
+    status_map = {
+        dexcontrol_query_pb2.ComponentStatus.NORMAL: ComponentStatus.NORMAL,
+        dexcontrol_query_pb2.ComponentStatus.NA: ComponentStatus.NA,
+        dexcontrol_query_pb2.ComponentStatus.ERROR: ComponentStatus.ERROR,
+    }
+
+    if status not in status_map:
+        raise ValueError(f"Invalid status: {status}")
+
+    return status_map[status]
+
+
+def status_to_dict(
+    status_msg: dexcontrol_query_pb2.ComponentStates,
+) -> dict[str, dict[str, Any]]:
+    """Convert a ComponentStates protobuf message to a dictionary.
+
+    Args:
+        status_msg: ComponentStates protobuf message.
+
+    Returns:
+        Dictionary containing status information for each component.
+    """
+    return {
+        name: {
+            "connected": state.connected,
+            "enabled": status_to_enum(state.enabled),
+            "error_state": status_to_enum(state.error_state),
+            "error_code": state.error_code,
+        }
+        for name, state in status_msg.states.items()
+    }
+
+
+def convert_enum_to_str(d: dict) -> dict:
+    """Convert enum values to their string representations in a dictionary.
+
+    Args:
+        d: Dictionary containing enum values.
+
+    Returns:
+        Dictionary with enum values converted to their string representations.
+    """
+    for key, value in d.items():
+        if isinstance(value, dict):
+            d[key] = convert_enum_to_str(value)
+        elif isinstance(value, Enum):
+            d[key] = value.name
+    return d

dexcontrol/utils/rate_limiter.py

@@ -0,0 +1,167 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""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/reset_orbbec_camera_usb.py

@@ -0,0 +1,98 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Utility for resetting Orbbec camera USB connection."""
+
+import os
+import sys
+import time
+
+import pyudev
+from loguru import logger
+
+# Orbbec vendor ID constant
+_ORBBEC_VENDOR_ID = "2bc5"
+
+
+def check_root() -> None:
+    """Verify the script is running with root privileges.
+
+    Exits the program if not running as root.
+    """
+    if os.geteuid() != 0:
+        logger.error("This script must be run as root (sudo). Exiting...")
+        logger.info("Run with: sudo $(which python) reset_orbbec_camera_usb.py")
+        sys.exit(1)
+
+
+def reset_orbbec() -> bool:
+    """Reset the USB connection for an Orbbec camera.
+
+    Simulates unplugging and replugging the device by toggling the USB
+    authorization state.
+
+    Returns:
+        bool: True if camera was found and reset successfully, False otherwise.
+    """
+    # Check for root privileges first
+    check_root()
+
+    # Initialize pyudev
+    context = pyudev.Context()
+
+    # Find Orbbec device
+    for device in context.list_devices(subsystem="usb"):
+        if device.properties.get("ID_VENDOR_ID") == _ORBBEC_VENDOR_ID:
+            return _reset_device(device)
+
+    logger.warning("Orbbec camera not found")
+    return False
+
+
+def _reset_device(device: pyudev.Device) -> bool:
+    """Reset a specific USB device.
+
+    Args:
+        device: pyudev Device object to reset
+
+    Returns:
+        bool: True if reset was successful, False otherwise
+    """
+    # Get the parent device (USB port)
+    port = device.find_parent("usb", "usb_device")
+
+    if port is None:
+        logger.warning("Could not find parent USB device")
+        return False
+
+    # Construct path to authorized file
+    path = os.path.join(port.sys_path, "authorized")
+
+    if not os.path.exists(path):
+        logger.warning(f"Authorize file not found at: {path}")
+        return False
+
+    try:
+        # Simulate unplug
+        with open(path, "w") as f:
+            f.write("0")
+        logger.info("USB device deauthorized")
+
+        # Wait a moment for the system to process
+        time.sleep(1)
+
+        # Simulate plug
+        with open(path, "w") as f:
+            f.write("1")
+
+        logger.info(f"Orbbec camera reset successfully. Path: {path}")
+        return True
+    except IOError as e:
+        logger.error(f"Failed to reset device: {e}")
+        return False
+
+
+if __name__ == "__main__":
+    reset_orbbec()

dexcontrol/utils/subscribers/__init__.py

@@ -0,0 +1,44 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Zenoh subscriber utilities for dexcontrol.
+
+This module provides a collection of subscriber classes and utilities for handling
+Zenoh communication in a flexible and reusable way.
+"""
+
+from .base import BaseZenohSubscriber, CustomDataHandler
+from .camera import DepthCameraSubscriber, RGBCameraSubscriber, RGBDCameraSubscriber
+from .decoders import (
+    DecoderFunction,
+    json_decoder,
+    protobuf_decoder,
+    raw_bytes_decoder,
+    string_decoder,
+)
+from .generic import GenericZenohSubscriber
+from .imu import IMUSubscriber
+from .lidar import LidarSubscriber
+from .protobuf import ProtobufZenohSubscriber
+
+__all__ = [
+    "BaseZenohSubscriber",
+    "CustomDataHandler",
+    "GenericZenohSubscriber",
+    "ProtobufZenohSubscriber",
+    "DecoderFunction",
+    "protobuf_decoder",
+    "raw_bytes_decoder",
+    "json_decoder",
+    "string_decoder",
+    # Camera subscribers
+    "RGBCameraSubscriber",
+    "DepthCameraSubscriber",
+    "RGBDCameraSubscriber",
+    # Lidar subscriber
+    "LidarSubscriber",
+    # IMU subscriber
+    "IMUSubscriber",
+]