dexcontrol 0.2.1__py3-none-any.whl

dexcontrol/__init__.py

@@ -0,0 +1,45 @@
+# 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].
+
+"""DexControl: Robot Control Interface Library.
+
+This package provides interfaces for controlling and monitoring robot systems.
+It serves as the primary API for interacting with Dexmate robots.
+"""
+
+from __future__ import annotations
+
+import os
+from pathlib import Path
+from typing import Final
+
+# DO NOT REMOVE this following import, it is needed for hydra to find the config
+import dexcontrol.config  # pylint: disable=unused-import
+from dexcontrol.robot import Robot
+from dexcontrol.utils.constants import COMM_CFG_PATH_ENV_VAR
+
+# Package-level constants
+LIB_PATH: Final[Path] = Path(__file__).resolve().parent
+CFG_PATH: Final[Path] = LIB_PATH / "config"
+
+
+def get_comm_cfg_path() -> Path:
+    default_path = list(
+        Path("~/.dexmate/comm/zenoh/").expanduser().glob("**/zenoh_peer_config.json5")
+    )
+    if len(default_path) == 0:
+        raise FileNotFoundError(
+            "No zenoh_peer_config.json5 file found in ~/.dexmate/comm/zenoh/"
+        )
+    return default_path[0]
+
+
+COMM_CFG_PATH: Final[Path] = Path(
+    os.getenv(COMM_CFG_PATH_ENV_VAR, get_comm_cfg_path())
+).expanduser()
+
+ROBOT_CFG_PATH: Final[Path] = CFG_PATH
+
+__all__ = ["Robot", "LIB_PATH", "CFG_PATH", "COMM_CFG_PATH", "ROBOT_CFG_PATH"]

dexcontrol/apps/dualsense_teleop_base.py

@@ -0,0 +1,371 @@
+# 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].
+
+"""DualSense controller teleoperation base module.
+
+This module provides a base class for controlling robots using a DualSense controller,
+with safety features, emergency stop handling, and customizable control mappings.
+"""
+
+import threading
+from abc import ABC, abstractmethod
+from enum import Enum
+import time
+
+from dualsense_controller import DualSenseController
+from loguru import logger
+
+from dexcontrol.robot import Robot
+from dexcontrol.utils.rate_limiter import RateLimiter
+
+
+class ControlMode(Enum):
+    """Control modes for the teleop interface."""
+
+    POSITION = "position"
+    VELOCITY = "velocity"
+    TORQUE = "torque"
+
+
+class ButtonState:
+    """Class to track button state with debouncing."""
+    
+    def __init__(self, debounce_time: float = 0.05):
+        """Initialize button state.
+        
+        Args:
+            debounce_time: Time in seconds to wait before considering a state change valid.
+        """
+        self._raw_state = False
+        self._debounced_state = False
+        self._last_change_time = 0.0
+        self._debounce_time = debounce_time
+        self._lock = threading.Lock()
+    
+    def update(self, new_state: bool) -> None:
+        """Update the button state with debouncing.
+        
+        Args:
+            new_state: New raw button state.
+        """
+        with self._lock:
+            current_time = time.time()
+            if new_state != self._raw_state:
+                self._last_change_time = current_time
+                self._raw_state = new_state
+            
+            # Only update debounced state if enough time has passed
+            if current_time - self._last_change_time >= self._debounce_time:
+                self._debounced_state = self._raw_state
+    
+    @property
+    def state(self) -> bool:
+        """Get the current debounced button state."""
+        with self._lock:
+            return self._debounced_state
+
+
+class DualSenseTeleopBase(ABC):
+    """Base class for DualSense controller teleoperation.
+
+    This class provides basic functionality for controlling a robot using a DualSense
+    controller, including safety features, e-stop handling, and button/stick mappings.
+
+    Attributes:
+        is_running: Flag indicating if the control loop is active.
+        control_hz: Control loop frequency in Hz.
+        button_update_hz: Button update frequency in Hz.
+        button_debounce_time: Time in seconds to wait before considering a button state change valid.
+        dualsense: Interface to the DualSense controller.
+        button_states: Dictionary to track button states with debouncing.
+        active_buttons: Currently pressed buttons.
+        safe_pressed: Safety button (L1) state.
+        estop_on: Emergency stop state.
+        estop_lock: Thread lock for e-stop state changes.
+        bot: Robot interface instance.
+    """
+
+    def __init__(
+        self,
+        control_hz: int = 200,
+        button_update_hz: int = 20,
+        device_index: int = 0,
+        button_debounce_time: float = 0.05,
+    ) -> None:
+        """Initialize the DualSense teleoperation base.
+
+        Args:
+            control_hz: Control loop frequency in Hz. Defaults to 200.
+            button_update_hz: Button update frequency in Hz. Defaults to 20.
+            device_index: Index of the DualSense controller device. Defaults to 0.
+            button_debounce_time: Time in seconds to wait before considering a button state change valid.
+        """
+        self.is_running = True
+        self.control_hz = control_hz
+        self.button_update_hz = button_update_hz
+        self.button_debounce_time = button_debounce_time
+
+        # Thread safety
+        self.button_lock = threading.Lock()
+        self.estop_lock = threading.Lock()
+
+        # Controller setup
+        try:
+            self.dualsense = DualSenseController(
+                device_index_or_device_info=device_index
+            )
+            self.dualsense.activate()
+            logger.info(
+                f"DualSense controller activated (device index: {device_index})"
+            )
+        except Exception as e:
+            logger.error(f"Failed to initialize DualSense controller: {e}")
+            raise
+
+        # Button state tracking
+        self.button_states: dict[str, ButtonState] = {}
+        self.active_buttons: set[str] = set()
+
+        # State flags
+        self.safe_pressed = False
+        self.estop_on = False
+
+        # Control state
+        self.control_mode = ControlMode.POSITION
+        self.rate_limiter = RateLimiter(self.button_update_hz)
+
+        # Robot interface
+        try:
+            self.bot = Robot()
+            logger.info(f"Robot '{self.bot.robot_model}' initialized")
+        except Exception as e:
+            logger.error(f"Failed to initialize robot: {e}")
+            self.dualsense.deactivate()
+            raise
+
+        # Configure button mappings
+        self._setup_button_mappings()
+
+    def _setup_button_mappings(self) -> None:
+        """Set up controller button mappings and callbacks."""
+        # Safety and e-stop buttons
+        self.dualsense.btn_l1.on_down(self.safety_check)
+        self.dualsense.btn_l1.on_up(self.safety_check_release)
+        self.dualsense.btn_touchpad.on_down(self.toggle_estop)
+
+        # Standard button mappings
+        button_mapping = {
+            "btn_up": "dpad_up",
+            "btn_down": "dpad_down",
+            "btn_right": "dpad_right",
+            "btn_left": "dpad_left",
+            "btn_circle": "circle",
+            "btn_square": "square",
+            "btn_triangle": "triangle",
+            "btn_cross": "cross",
+            "btn_r1": "r1",
+            "btn_r2": "r2",
+        }
+
+        for btn_name, motion_name in button_mapping.items():
+            btn = getattr(self.dualsense, btn_name)
+            # Create button state tracker
+            self.button_states[motion_name] = ButtonState(self.button_debounce_time)
+            # Use default parameter to capture current value
+            btn.on_down(lambda m=motion_name: self._update_button_state(m, True))
+            btn.on_up(lambda m=motion_name: self._update_button_state(m, False))
+
+        # Setup additional mappings specific to the subclass
+        self._setup_additional_mappings()
+
+    def _setup_additional_mappings(self) -> None:
+        """Set up additional controller mappings specific to subclasses.
+
+        This method can be overridden by subclasses to add more button mappings.
+        """
+        pass
+
+    def _update_button_state(self, button: str, state: bool) -> None:
+        """Update the state of a button with debouncing.
+        
+        Args:
+            button: Name of the button being updated.
+            state: New raw button state.
+        """
+        if button in self.button_states:
+            self.button_states[button].update(state)
+            if state:
+                self.add_button(button)
+            else:
+                self.remove_button(button)
+
+    def add_button(self, button: str) -> None:
+        """Add a button to the set of active buttons if safety is enabled.
+
+        Args:
+            button: Name of the button being pressed.
+        """
+        if self.safe_pressed or button in ["l1", "touchpad"]:
+            with self.button_lock:
+                self.active_buttons.add(button)
+                logger.debug(f"Button added: {button}")
+
+    def remove_button(self, button: str) -> None:
+        """Remove a button from the set of active buttons.
+
+        Args:
+            button: Name of the button being released.
+        """
+        with self.button_lock:
+            self.active_buttons.discard(button)
+            logger.debug(f"Button removed: {button}")
+
+    def toggle_estop(self) -> None:
+        """Toggle the emergency stop state of the robot.
+
+        When activated, this will:
+        1. Toggle the robot's software e-stop
+        2. Stop any ongoing motion
+        3. Reset all motion commands to zero
+        """
+        try:
+            with self.estop_lock:
+                self.estop_on = not self.estop_on
+                if self.estop_on:
+                    self.stop_all_motion()
+                    self.bot.estop.activate()
+                else:
+                    self.bot.estop.deactivate()
+
+            logger.info(f"E-stop {'activated' if self.estop_on else 'deactivated'}")
+
+            # Visual feedback on controller
+            if self.estop_on:
+                self.dualsense.lightbar.set_color(255, 0, 0)  # Red for e-stop
+            else:
+                self.update_controller_feedback()
+
+        except Exception as e:
+            logger.error(f"Error toggling e-stop: {e}")
+
+    def safety_check(self) -> None:
+        """Enable safety mode (L1 button pressed).
+
+        Activates haptic feedback and enables safety-gated commands.
+        """
+        self.dualsense.left_rumble.set(50)
+        self.safe_pressed = True
+        self.dualsense.left_rumble.set(0)
+        logger.debug("Safety mode enabled")
+
+    def safety_check_release(self) -> None:
+        """Disable safety mode (L1 button released).
+
+        Deactivates haptic feedback and:
+        1. Clears all active buttons
+        2. Stops any ongoing motion
+        3. Resets all motion commands to zero
+        """
+        self.safe_pressed = False
+        self.dualsense.left_rumble.set(0)
+
+        # Clear all active buttons with lock protection
+        with self.button_lock:
+            self.active_buttons.clear()
+
+        # Stop any ongoing motion
+        self.stop_all_motion()
+        logger.debug("Safety mode disabled, motion stopped")
+
+    def get_active_buttons(self) -> set[str]:
+        """Get a copy of currently active buttons in a thread-safe way.
+
+        Returns:
+            A copy of the set of currently active buttons.
+        """
+        with self.button_lock:
+            return self.active_buttons.copy()
+
+    def update_controller_feedback(self) -> None:
+        """Update controller visual feedback based on current state.
+
+        This method can be overridden by subclasses to customize feedback.
+        """
+        # Default is white lightbar
+        self.dualsense.lightbar.set_color_white()
+
+    def run_forever(self) -> None:
+        """Run the control loop indefinitely until interrupted.
+
+        The loop runs at the specified button_update_hz frequency.
+        Handles KeyboardInterrupt for clean shutdown.
+        """
+        logger.info("Starting teleop control loop")
+        rate_limiter = RateLimiter(self.button_update_hz)
+        try:
+            while self.is_running and not self.bot.is_shutdown():
+                self.update_motion()
+                rate_limiter.sleep()
+        except KeyboardInterrupt:
+            logger.info("Received keyboard interrupt, shutting down")
+        except Exception as e:
+            logger.error(f"Error in control loop: {e}")
+            raise
+        finally:
+            self.cleanup()
+
+    def cleanup(self) -> None:
+        """Clean up resources before shutting down."""
+        self.is_running = False
+
+        # Reset controller
+        if hasattr(self, "dualsense"):
+            self.dualsense.lightbar.set_color_white()
+            self.dualsense.deactivate()
+
+        logger.info("Teleop node cleaned up and exiting")
+
+    @abstractmethod
+    def update_motion(self) -> None:
+        """Update the robot's motion based on current controller state.
+
+        This method should be implemented by derived classes to define
+        specific motion control behavior.
+        """
+        pass
+
+    @abstractmethod
+    def stop_all_motion(self) -> None:
+        """Stop all ongoing robot motion and reset motion commands.
+
+        This method should be implemented by derived classes to properly
+        handle motion stopping for their specific use case.
+        """
+        pass
+
+
+class DummyDualSenseTeleop(DualSenseTeleopBase):
+    """Simple implementation of DualSenseTeleopBase for testing purposes."""
+
+    def update_motion(self) -> None:
+        """Dummy implementation of update_motion."""
+        if self.active_buttons:
+            logger.debug(f"Active buttons: {self.active_buttons}")
+        pass
+
+    def stop_all_motion(self) -> None:
+        """Dummy implementation of stop_all_motion."""
+        logger.info("Stopping all motion (dummy implementation)")
+        pass
+
+
+def test_dummy_dualsense_teleop() -> None:
+    """Test function for the DummyDualSenseTeleop class."""
+    teleop = DummyDualSenseTeleop()
+    teleop.run_forever()
+
+
+if __name__ == "__main__":
+    test_dummy_dualsense_teleop()

dexcontrol/config/__init__.py

@@ -0,0 +1,14 @@
+# 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].
+
+from hydra.core.config_store import ConfigStore
+
+from .vega import Vega1Config, VegaConfig
+
+# Register the configs
+cs = ConfigStore.instance()
+cs.store(name="vega", node=VegaConfig)
+cs.store(name="vega_rc2", node=VegaConfig)
+cs.store(name="vega_1", node=Vega1Config)

dexcontrol/config/core/__init__.py

@@ -0,0 +1,22 @@
+# 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].
+
+from .arm import ArmConfig
+from .chassis import ChassisConfig
+from .hand import HandConfig
+from .head import HeadConfig
+from .misc import BatteryConfig, EStopConfig, HeartbeatConfig
+from .torso import TorsoConfig
+
+__all__ = [
+    "ArmConfig",
+    "ChassisConfig",
+    "HandConfig",
+    "HeadConfig",
+    "BatteryConfig",
+    "EStopConfig",
+    "HeartbeatConfig",
+    "TorsoConfig",
+]

dexcontrol/config/core/arm.py

@@ -0,0 +1,32 @@
+# 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].
+
+from dataclasses import dataclass, field
+
+import numpy as np
+
+
+@dataclass
+class ArmConfig:
+    _target_: str = "dexcontrol.core.arm.Arm"
+    state_sub_topic: str = "state/arm/right"
+    wrench_sub_topic: str = "state/wrench/right"
+    control_pub_topic: str = "control/arm/right"
+    set_mode_query: str = "mode/arm/right"
+    dof: int = 7
+    joint_name: list[str] = field(
+        default_factory=lambda: [f"R_arm_j{i + 1}" for i in range(7)]
+    )
+    joint_limit: list[list[float]] = field(
+        default_factory=lambda: [[-np.pi, np.pi] for _ in range(7)]
+    )
+    pose_pool: dict[str, list[float]] = field(
+        default_factory=lambda: {
+            "folded": [-1.57079, 0.0, 0, -3.1, 0, 0, 0.69813],
+            "folded_closed_hand": [-1.57079, 0.0, 0, -3.1, 0, 0, 0.9],
+            "L_shape": [-0.064, 0.3, 0.0, -1.556, -1.271, 0.0, 0.0],
+            "zero": [1.57079, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        }
+    )

dexcontrol/config/core/chassis.py

@@ -0,0 +1,22 @@
+# 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].
+
+from dataclasses import dataclass, field
+
+
+@dataclass
+class ChassisConfig:
+    _target_: str = "dexcontrol.core.chassis.Chassis"
+    control_pub_topic: str = "control/chassis"
+    state_sub_topic: str = "state/chassis"
+    dof: int = 2
+    center_to_wheel_axis_dist: float = (
+        0.219  # the distance between base center and wheel axis in m
+    )
+    wheels_dist: float = 0.41  # the distance between two wheels in m (0.41 for vega_rc2, 0.45 for vega_1)
+    joint_name: list[str] = field(
+        default_factory=lambda: ["L_wheel_j1", "L_wheel_j2", "R_wheel_j1", "R_wheel_j2"]
+    )
+    max_vel: float = 0.6

dexcontrol/config/core/hand.py

@@ -0,0 +1,42 @@
+# 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].
+
+from dataclasses import dataclass, field
+
+
+@dataclass
+class HandConfig:
+    _target_: str = "dexcontrol.core.hand.HandF5D6"
+    state_sub_topic: str = "state/hand/right"
+    control_pub_topic: str = "control/hand/right"
+    dof: int = 6
+    joint_name: list[str] = field(
+        default_factory=lambda: [
+            "R_th_j1",
+            "R_ff_j1",
+            "R_mf_j1",
+            "R_rf_j1",
+            "R_lf_j1",
+            "R_th_j0",
+        ]
+    )
+
+    # Not to modify the following varaibles unless you change a different hand
+    control_joint_names: list[str] = field(
+        default_factory=lambda: ["th_j1", "ff_j1", "mf_j1", "rf_j1", "lf_j1", "th_j0"]
+    )
+    joint_pos_open: list[float] = field(
+        default_factory=lambda: [0.18313, 0.29012, 0.28084, 0.28498, 0.28204, -0.034]
+    )
+    joint_pos_close: list[float] = field(
+        default_factory=lambda: [
+            -0.64862,
+            -1.17584,
+            -1.16855,
+            -1.17493,
+            -1.17277,
+            1.6,
+        ]
+    )

dexcontrol/config/core/head.py

@@ -0,0 +1,33 @@
+# 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].
+
+from dataclasses import dataclass, field
+
+
+@dataclass
+class HeadConfig:
+    _target_: str = "dexcontrol.core.head.Head"
+    state_sub_topic: str = "state/head"
+    control_pub_topic: str = "control/head"
+    set_mode_query: str = "mode/head"
+    joint_name: list[str] = field(
+        default_factory=lambda: ["head_j1", "head_j2", "head_j3"]
+    )
+    dof: int = 3
+    # Use the following varaibles if you are using vega-rc2
+    joint_limit_lower: list[float] = field(
+        default_factory=lambda: [-1.2217, -2.7925, -1.396]
+    )
+    joint_limit_upper: list[float] = field(
+        default_factory=lambda: [1.2217, 2.7925, 1.396]
+    )
+    default_vel: float = 1.57
+    max_vel: float = 2.0
+    pose_pool: dict[str, list[float]] = field(
+        default_factory=lambda: {
+            "home": [0.0, 0.0, 0.0],
+            "tucked": [0.0, 0.0, -1.37],
+        }
+    )

dexcontrol/config/core/misc.py

@@ -0,0 +1,37 @@
+# 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].
+
+"""Miscellaneous robot component configurations."""
+
+import os
+from dataclasses import dataclass
+
+from dexcontrol.utils.constants import DISABLE_HEARTBEAT_ENV_VAR
+
+
+@dataclass
+class BatteryConfig:
+    _target_: str = "dexcontrol.core.misc.Battery"
+    state_sub_topic: str = "state/bms"
+
+
+@dataclass
+class EStopConfig:
+    _target_: str = "dexcontrol.core.misc.EStop"
+    state_sub_topic: str = "state/estop"
+    estop_query_name: str = "system/estop"
+
+
+@dataclass
+class HeartbeatConfig:
+    _target_: str = "dexcontrol.core.misc.Heartbeat"
+    heartbeat_topic: str = "/heartbeat"
+    timeout_seconds: float = 1.0
+    enabled: bool = True  # Can be disabled via DEXCONTROL_DISABLE_HEARTBEAT=1
+
+    def __post_init__(self):
+        """Check environment variable to disable heartbeat monitoring."""
+        if os.getenv(DISABLE_HEARTBEAT_ENV_VAR, "0").lower() in ("1", "true", "yes"):
+            self.enabled = False

dexcontrol/config/core/torso.py

@@ -0,0 +1,36 @@
+# 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].
+
+from dataclasses import dataclass, field
+
+
+@dataclass
+class TorsoConfig:
+    _target_: str = "dexcontrol.core.torso.Torso"
+    state_sub_topic: str = "state/torso"
+    control_pub_topic: str = "control/torso"
+    dof: int = 3
+    joint_name: list[str] = field(
+        default_factory=lambda: ["torso_j1", "torso_j2", "torso_j3"]
+    )
+    default_vel: float = 0.4  # rad/s, will be used if joint_vel is not provided
+    max_vel: float = 0.6  # max velocity of torso joint, will be used to clip
+    # the joint_vel, Highly recommended to set this value
+    # not higher than 0.6 rad/s
+    pose_pool: dict[str, list[float]] = field(
+        default_factory=lambda: {
+            "home": [0.0, 0.0, 0.0],
+            "folded": [0.0, 0.0, -1.5708],
+            "crouch20_low": [0.0, 0.0, -0.35],
+            "crouch20_medium": [0.52, 1.05, 0.18],
+            "crouch20_high": [0.78, 1.57, 0.44],
+            "crouch45_low": [0.0, 0.0, -0.79],
+            "crouch45_medium": [0.52, 1.05, -0.26],
+            "crouch45_high": [0.78, 1.57, 0],
+            "crouch90_low": [0.0, 0.0, -1.57],
+            "crouch90_medium": [0.52, 1.05, -1.04],
+            "crouch90_high": [0.78, 1.57, -0.78],
+        }
+    )

dexcontrol/config/sensors/__init__.py

@@ -0,0 +1,4 @@
+# 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].

dexcontrol/config/sensors/cameras/__init__.py

@@ -0,0 +1,7 @@
+# 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].
+
+from .gemini_camera import GeminiCameraConfig
+from .rgb_camera import RGBCameraConfig