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

dexcontrol/core/hand.py

@@ -14,17 +14,24 @@ This module provides the Hand class for controlling a robotic hand through Zenoh
 communication. It handles joint position control and state monitoring.
 """
 
-from typing import Any
+from enum import Enum
+from typing import Any, cast
 
 import numpy as np
 import zenoh
 from jaxtyping import Float
 
 from dexcontrol.config.core.hand import HandConfig
-from dexcontrol.core.component import RobotJointComponent
+from dexcontrol.core.component import RobotComponent, RobotJointComponent
 from dexcontrol.proto import dexcontrol_msg_pb2
 
 
+class HandType(Enum):
+    UNKNOWN = "UNKNOWN"
+    HandF5D6_V1 = "HandF5D6_V1"
+    HandF5D6_V2 = "HandF5D6_V2"
+
+
 class Hand(RobotJointComponent):
     """Robot hand control class.
 
@@ -36,6 +43,7 @@ class Hand(RobotJointComponent):
         self,
         configs: HandConfig,
         zenoh_session: zenoh.Session,
+        hand_type: HandType = HandType.HandF5D6_V1,
     ) -> None:
         """Initialize the hand controller.
 
@@ -47,7 +55,7 @@ class Hand(RobotJointComponent):
         super().__init__(
             state_sub_topic=configs.state_sub_topic,
             control_pub_topic=configs.control_pub_topic,
-            state_message_type=dexcontrol_msg_pb2.HandState,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
             zenoh_session=zenoh_session,
             joint_name=configs.joint_name,
         )
@@ -64,45 +72,11 @@ class Hand(RobotJointComponent):
         Args:
             joint_pos: Joint positions as list or numpy array.
         """
-        control_msg = dexcontrol_msg_pb2.HandCommand()
+        control_msg = dexcontrol_msg_pb2.MotorPosCommand()
         joint_pos_array = self._convert_joint_cmd_to_array(joint_pos)
-        control_msg.joint_pos.extend(joint_pos_array.tolist())
+        control_msg.pos.extend(joint_pos_array.tolist())
         self._publish_control(control_msg)
 
-    def set_joint_pos(
-        self,
-        joint_pos: Float[np.ndarray, " N"] | list[float] | dict[str, float],
-        relative: bool = False,
-        wait_time: float = 0.0,
-        wait_kwargs: dict[str, float] | None = None,
-        exit_on_reach: bool = False,
-        exit_on_reach_kwargs: dict[str, Any] | None = None,
-    ) -> None:
-        """Send joint position control commands to the hand.
-
-        Args:
-            joint_pos: Joint positions as either:
-                - List of joint values [j1, j2, ..., jN]
-                - Numpy array with shape (N,)
-                - Dictionary mapping joint names to position values
-            relative: If True, the joint positions are relative to the current position.
-            wait_time: Time to wait after setting the joint positions.
-            wait_kwargs: Optional parameters for trajectory generation (not used in Hand).
-            exit_on_reach: If True, the function will exit when the joint positions are reached.
-            exit_on_reach_kwargs: Optional parameters for exit when the joint positions are reached.
-
-        Raises:
-            ValueError: If joint_pos dictionary contains invalid joint names.
-        """
-        super().set_joint_pos(
-            joint_pos,
-            relative,
-            wait_time,
-            wait_kwargs,
-            exit_on_reach,
-            exit_on_reach_kwargs,
-        )
-
     def open_hand(
         self,
         wait_time: float = 0.0,
@@ -152,6 +126,37 @@ class HandF5D6(Hand):
     the F5D6 hand model.
     """
 
+    def __init__(
+        self,
+        configs: HandConfig,
+        zenoh_session: zenoh.Session,
+        hand_type: HandType = HandType.HandF5D6_V1,
+    ) -> None:
+        super().__init__(configs, zenoh_session)
+
+        # Initialize touch sensor for F5D6_V2 hands
+        self._hand_type = hand_type
+        if self._hand_type == HandType.HandF5D6_V2:
+            self._touch_sensor = HandF5D6TouchSensor(
+                configs.touch_sensor_sub_topic, zenoh_session
+            )
+        elif self._hand_type == HandType.HandF5D6_V1:
+            self._touch_sensor = None
+        else:
+            raise ValueError(f"Invalid hand type: {self._hand_type}")
+
+    def get_finger_tip_force(self) -> Float[np.ndarray, "5"]:
+        """Get the force at the finger tips.
+
+        Returns:
+            Array of force values at the finger tips.
+        """
+        if self._touch_sensor is None:
+            raise ValueError(
+                f"Touch sensor not available for this hand type: {self._hand_type}"
+            )
+        return self._touch_sensor.get_fingertip_touch_net_force()
+
     def close_hand(
         self,
         wait_time: float = 0.0,
@@ -241,3 +246,33 @@ class HandF5D6(Hand):
             0
         ] * (1 - ratio)
         return intermediate_pos
+
+
+class HandF5D6TouchSensor(RobotComponent):
+    """Wrench sensor reader for the robot arm.
+
+    This class provides methods to read wrench sensor data through Zenoh communication.
+    """
+
+    def __init__(self, state_sub_topic: str, zenoh_session: zenoh.Session) -> None:
+        """Initialize the wrench sensor reader.
+
+        Args:
+            state_sub_topic: Topic to subscribe to for wrench sensor data.
+            zenoh_session: Active Zenoh communication session for message passing.
+        """
+        super().__init__(
+            state_sub_topic=state_sub_topic,
+            zenoh_session=zenoh_session,
+            state_message_type=dexcontrol_msg_pb2.HandTouchSensorState,
+        )
+
+    def get_fingertip_touch_net_force(self) -> Float[np.ndarray, "5"]:
+        """Get the complete wrench sensor state.
+
+        Returns:
+            Dictionary containing wrench values and button states.
+        """
+        state = self._get_state()
+        hand_touch_state = cast(dexcontrol_msg_pb2.HandTouchSensorState, state)
+        return np.array(hand_touch_state.force)

dexcontrol/core/head.py

@@ -55,7 +55,7 @@ class Head(RobotJointComponent):
         super().__init__(
             state_sub_topic=configs.state_sub_topic,
             control_pub_topic=configs.control_pub_topic,
-            state_message_type=dexcontrol_msg_pb2.HeadState,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithTorque,
             zenoh_session=zenoh_session,
             joint_name=configs.joint_name,
             joint_limit=configs.joint_limit
@@ -164,9 +164,9 @@ class Head(RobotJointComponent):
             )
 
         # Create and send control message
-        control_msg = dexcontrol_msg_pb2.HeadCommand()
-        control_msg.joint_pos.extend(joint_pos.tolist())
-        control_msg.joint_vel.extend(joint_vel.tolist())
+        control_msg = dexcontrol_msg_pb2.MotorPosVelCommand()
+        control_msg.pos.extend(joint_pos.tolist())
+        control_msg.vel.extend(joint_vel.tolist())
         self._publish_control(control_msg)
 
         # Wait if specified
@@ -202,7 +202,8 @@ class Head(RobotJointComponent):
 
         for reply in replies:
             if reply.ok is not None and reply.ok.payload is not None:
-                logger.info(reply.ok.payload.to_string())
+                # TODO: handle the reply message of head mode
+                pass
         time.sleep(0.5)
 
     def get_joint_limit(self) -> Float[np.ndarray, "3 2"] | None:

dexcontrol/core/misc.py

@@ -11,13 +11,14 @@
 """Miscellaneous robot components module.
 
 This module provides classes for various auxiliary robot components such as Battery,
-EStop (emergency stop), and UltraSonicSensor.
+EStop (emergency stop), ServerLogSubscriber, and UltraSonicSensor.
 """
 
+import json
 import os
 import threading
 import time
-from typing import TypeVar
+from typing import Any, TypeVar, cast
 
 import zenoh
 from google.protobuf.message import Message
@@ -133,7 +134,7 @@ class Battery(RobotComponent):
         power = state.current * state.voltage
         power_style = self._get_power_style(power)
         table.add_row(
-            "Power",
+            "Power Consumption",
             f"[{power_style}]{power:.2f}W[/] ([blue]{state.current:.2f}A[/] "
             f"× [blue]{state.voltage:.2f}V[/])",
         )
@@ -281,25 +282,40 @@ class EStop(RobotComponent):
                 - software_estop_enabled: Software EStop enabled
         """
         state = self._get_state()
+        state = cast(dexcontrol_msg_pb2.EStopState, state)
         if state is None:
             return {
                 "button_pressed": False,
                 "software_estop_enabled": False,
             }
+        button_pressed = (
+            state.left_button_pressed
+            or state.right_button_pressed
+            or state.waist_button_pressed
+            or state.wireless_button_pressed
+        )
         return {
-            "button_pressed": state.button_pressed,
+            "button_pressed": button_pressed,
             "software_estop_enabled": state.software_estop_enabled,
         }
 
     def is_button_pressed(self) -> bool:
         """Checks if the EStop button is pressed."""
         state = self._get_state()
-        return state.button_pressed if state is not None else False
+        state = cast(dexcontrol_msg_pb2.EStopState, state)
+        button_pressed = (
+            state.left_button_pressed
+            or state.right_button_pressed
+            or state.waist_button_pressed
+            or state.wireless_button_pressed
+        )
+        return button_pressed
 
     def is_software_estop_enabled(self) -> bool:
         """Checks if the software EStop is enabled."""
         state = self._get_state()
-        return state.software_estop_enabled if state is not None else False
+        state = cast(dexcontrol_msg_pb2.EStopState, state)
+        return state.software_estop_enabled
 
     def activate(self) -> None:
         """Activates the software emergency stop (E-Stop)."""
@@ -325,27 +341,19 @@ class EStop(RobotComponent):
 
     def show(self) -> None:
         """Displays the current EStop status as a formatted table with color indicators."""
-        state = self._get_state()
-
         table = Table(title="E-Stop Status")
         table.add_column("Parameter", style="cyan")
         table.add_column("Value")
 
-        if state is None:
-            table.add_row("Status", "[red]No E-Stop data available[/]")
-            console = Console()
-            console.print(table)
-            return
-
-        button_style = "bold red" if state.button_pressed else "bold dark_green"
-        table.add_row("Button Pressed", f"[{button_style}]{state.button_pressed}[/]")
+        button_pressed = self.is_button_pressed()
+        button_style = "bold red" if button_pressed else "bold dark_green"
+        table.add_row("Button Pressed", f"[{button_style}]{button_pressed}[/]")
 
-        software_style = (
-            "bold red" if state.software_estop_enabled else "bold dark_green"
-        )
+        if_software_estop_enabled = self.is_software_estop_enabled()
+        software_style = "bold red" if if_software_estop_enabled else "bold dark_green"
         table.add_row(
             "Software E-Stop Enabled",
-            f"[{software_style}]{state.software_estop_enabled}[/]",
+            f"[{software_style}]{if_software_estop_enabled}[/]",
         )
 
         console = Console()
@@ -651,9 +659,9 @@ class Heartbeat:
         last_value = status["last_value"]
         if last_value is not None:
             uptime_str = self._format_uptime(last_value)
-            table.add_row("Robot server uptime", f"[blue]{uptime_str}[/]")
+            table.add_row("Robot Driver Uptime", f"[blue]{uptime_str}[/]")
         else:
-            table.add_row("Robot server uptime", "[red]No data[/]")
+            table.add_row("Robot Driver Uptime", "[red]No data[/]")
 
         # Time since last heartbeat
         time_since = status["time_since_last"]
@@ -671,3 +679,145 @@ class Heartbeat:
 
         console = Console()
         console.print(table)
+
+
+class ServerLogSubscriber:
+    """Server log subscriber that monitors and displays server log messages.
+
+    This class subscribes to the "logs" topic and handles incoming log messages
+    from the robot server. It provides formatted display of server logs with
+    proper error handling and validation.
+
+    The server sends log information via the "logs" topic as JSON with format:
+    {"timestamp": "ISO8601", "message": "text", "source": "robot_server"}
+
+    Attributes:
+        _zenoh_session: Zenoh session for communication.
+        _log_subscriber: Zenoh subscriber for log messages.
+    """
+
+    def __init__(self, zenoh_session: zenoh.Session) -> None:
+        """Initialize the ServerLogSubscriber.
+
+        Args:
+            zenoh_session: Active Zenoh session for communication.
+        """
+        self._zenoh_session = zenoh_session
+        self._log_subscriber = None
+        self._initialize()
+
+    def _initialize(self) -> None:
+        """Initialize the log subscriber with error handling."""
+
+        def log_handler(sample):
+            """Handle incoming log messages from the server."""
+            if not self._is_valid_log_sample(sample):
+                return
+
+            try:
+                log_data = self._parse_log_payload(sample.payload)
+                if log_data:
+                    self._display_server_log(log_data)
+            except Exception as e:
+                logger.warning(f"Failed to process server log: {e}")
+
+        try:
+            # Subscribe to server logs topic
+            self._log_subscriber = self._zenoh_session.declare_subscriber(
+                "logs", log_handler
+            )
+            logger.debug("Server log subscriber initialized")
+        except Exception as e:
+            logger.warning(f"Failed to initialize server log subscriber: {e}")
+            self._log_subscriber = None
+
+    def _is_valid_log_sample(self, sample) -> bool:
+        """Check if log sample is valid.
+
+        Args:
+            sample: Zenoh sample to validate.
+
+        Returns:
+            True if sample is valid, False otherwise.
+        """
+        if sample is None or sample.payload is None:
+            logger.debug("Received empty log sample")
+            return False
+        return True
+
+    def _parse_log_payload(self, payload) -> dict[str, str] | None:
+        """Parse log payload and return structured data.
+
+        Args:
+            payload: Raw payload from Zenoh sample.
+
+        Returns:
+            Parsed log data as dictionary or None if parsing fails.
+        """
+        try:
+            payload_str = payload.to_bytes().decode("utf-8")
+            if not payload_str.strip():
+                logger.debug("Received empty log payload")
+                return None
+
+            log_data = json.loads(payload_str)
+
+            if not isinstance(log_data, dict):
+                logger.warning(
+                    f"Invalid log data format: expected dict, got {type(log_data)}"
+                )
+                return None
+
+            return log_data
+        except (json.JSONDecodeError, UnicodeDecodeError) as e:
+            logger.warning(f"Failed to parse log payload: {e}")
+            return None
+
+    def _display_server_log(self, log_data: dict[str, str]) -> None:
+        """Display formatted server log message.
+
+        Args:
+            log_data: Parsed log data dictionary.
+        """
+        # Extract log information with safe defaults
+        timestamp = log_data.get("timestamp", "")
+        message = log_data.get("message", "")
+        source = log_data.get("source", "unknown")
+
+        # Validate critical fields
+        if not message:
+            logger.debug("Received log with empty message")
+            return
+
+        # Log the server message with clear identification
+        logger.info(f"[SERVER_LOG] [{timestamp}] [{source}] {message}")
+
+    def is_active(self) -> bool:
+        """Check if the log subscriber is active.
+
+        Returns:
+            True if subscriber is active, False otherwise.
+        """
+        return self._log_subscriber is not None
+
+    def shutdown(self) -> None:
+        """Clean up the log subscriber and release resources."""
+        if self._log_subscriber is not None:
+            try:
+                self._log_subscriber.undeclare()
+                self._log_subscriber = None
+            except Exception as e:
+                logger.error(f"Error cleaning up log subscriber: {e}")
+
+    def get_status(self) -> dict[str, Any]:
+        """Get the current status of the log subscriber.
+
+        Returns:
+            Dictionary containing status information:
+                - is_active: Whether the subscriber is active
+                - topic: The topic being subscribed to
+        """
+        return {
+            "is_active": self.is_active(),
+            "topic": "logs",
+        }