dexcontrol 0.2.12__py3-none-any.whl → 0.3.1__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 loguru import logger
 
 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.
 
@@ -35,20 +42,18 @@ class Hand(RobotJointComponent):
     def __init__(
         self,
         configs: HandConfig,
-        zenoh_session: zenoh.Session,
+        hand_type: HandType = HandType.HandF5D6_V1,
     ) -> None:
         """Initialize the hand controller.
 
         Args:
             configs: Hand configuration parameters containing communication topics
                 and predefined hand positions.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
         super().__init__(
             state_sub_topic=configs.state_sub_topic,
             control_pub_topic=configs.control_pub_topic,
-            state_message_type=dexcontrol_msg_pb2.HandState,
-            zenoh_session=zenoh_session,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
             joint_name=configs.joint_name,
         )
 
@@ -64,45 +69,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 +123,34 @@ class HandF5D6(Hand):
     the F5D6 hand model.
     """
 
+    def __init__(
+        self,
+        configs: HandConfig,
+        hand_type: HandType = HandType.HandF5D6_V1,
+    ) -> None:
+        super().__init__(configs)
+
+        # 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)
+        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,
@@ -159,27 +158,30 @@ class HandF5D6(Hand):
         exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Close the hand fully using a two-step approach for better control."""
-        if self.is_joint_pos_reached(self._joint_pos_close, tolerance=0.1):
-            return
-
-        # First step: Move to intermediate position
-        intermediate_pos = self._get_intermediate_close_position()
-        first_step_wait_time = 0.8
-        self.set_joint_pos(
-            intermediate_pos,
-            wait_time=first_step_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
-
-        # Second step: Move to final closed position
-        remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
-        self.set_joint_pos(
-            self._joint_pos_close,
-            wait_time=remaining_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
+        try:
+            if self.is_joint_pos_reached(self._joint_pos_close, tolerance=0.1):
+                return
+
+            # First step: Move to intermediate position
+            intermediate_pos = self._get_intermediate_close_position()
+            first_step_wait_time = 0.8
+            self.set_joint_pos(
+                intermediate_pos,
+                wait_time=first_step_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
+
+            # Second step: Move to final closed position
+            remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
+            self.set_joint_pos(
+                self._joint_pos_close,
+                wait_time=remaining_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
+        except Exception as e:
+            logger.warning(f"Failed to close hand: {e}")
 
     def open_hand(
         self,
@@ -188,27 +190,30 @@ class HandF5D6(Hand):
         exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Open the hand fully using a two-step approach for better control."""
-        if self.is_joint_pos_reached(self._joint_pos_open, tolerance=0.1):
-            return
-
-        # First step: Move to intermediate position
-        intermediate_pos = self._get_intermediate_open_position()
-        first_step_wait_time = 0.3
-        self.set_joint_pos(
-            intermediate_pos,
-            wait_time=first_step_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
-
-        # Second step: Move to final open position
-        remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
-        self.set_joint_pos(
-            self._joint_pos_open,
-            wait_time=remaining_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
+        try:
+            if self.is_joint_pos_reached(self._joint_pos_open, tolerance=0.1):
+                return
+
+            # First step: Move to intermediate position
+            intermediate_pos = self._get_intermediate_open_position()
+            first_step_wait_time = 0.3
+            self.set_joint_pos(
+                intermediate_pos,
+                wait_time=first_step_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
+
+            # Second step: Move to final open position
+            remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
+            self.set_joint_pos(
+                self._joint_pos_open,
+                wait_time=remaining_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
+        except Exception as e:
+            logger.warning(f"Failed to open hand: {e}")
 
     def _get_intermediate_close_position(self) -> np.ndarray:
         """Get intermediate position for closing hand.
@@ -241,3 +246,31 @@ 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) -> None:
+        """Initialize the wrench sensor reader.
+
+        Args:
+            state_sub_topic: Topic to subscribe to for wrench sensor data.
+        """
+        super().__init__(
+            state_sub_topic=state_sub_topic,
+            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

@@ -15,17 +15,16 @@ communication. It handles joint position and velocity control, mode setting, and
 state monitoring.
 """
 
-import time
-from typing import Final, Literal
+from typing import Literal
 
 import numpy as np
-import zenoh
+from dexcomm import call_service
 from jaxtyping import Float
-from loguru import logger
 
 from dexcontrol.config.core import HeadConfig
 from dexcontrol.core.component import RobotJointComponent
 from dexcontrol.proto import dexcontrol_msg_pb2, dexcontrol_query_pb2
+from dexcontrol.utils.comm_helper import get_zenoh_config_path
 from dexcontrol.utils.os_utils import resolve_key_name
 
 
@@ -44,19 +43,16 @@ class Head(RobotJointComponent):
     def __init__(
         self,
         configs: HeadConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the head controller.
 
         Args:
             configs: Configuration parameters for the head including communication topics.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
         super().__init__(
             state_sub_topic=configs.state_sub_topic,
             control_pub_topic=configs.control_pub_topic,
-            state_message_type=dexcontrol_msg_pb2.HeadState,
-            zenoh_session=zenoh_session,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithTorque,
             joint_name=configs.joint_name,
             joint_limit=configs.joint_limit
             if hasattr(configs, "joint_limit")
@@ -67,9 +63,8 @@ class Head(RobotJointComponent):
             pose_pool=configs.pose_pool,
         )
 
-        self.mode_querier: Final[zenoh.Querier] = zenoh_session.declare_querier(
-            resolve_key_name(configs.set_mode_query), timeout=2.0
-        )
+        # Store the query topic for later use with DexComm
+        self._mode_query_topic = resolve_key_name(configs.set_mode_query)
         assert self._joint_vel_limit is not None, "joint_vel_limit is not set"
 
     def set_joint_pos(
@@ -164,9 +159,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
@@ -198,12 +193,15 @@ class Head(RobotJointComponent):
 
         query_msg = dexcontrol_query_pb2.SetHeadMode()
         query_msg.mode = mode_map[mode]
-        replies = self.mode_querier.get(payload=query_msg.SerializeToString())
 
-        for reply in replies:
-            if reply.ok is not None and reply.ok.payload is not None:
-                logger.info(reply.ok.payload.to_string())
-        time.sleep(0.5)
+        call_service(
+            self._mode_query_topic,
+            request=query_msg,
+            timeout=1.0,
+            config=get_zenoh_config_path(),
+            request_serializer=lambda x: x.SerializeToString(),
+            response_deserializer=None,
+        )
 
     def get_joint_limit(self) -> Float[np.ndarray, "3 2"] | None:
         """Get the joint limits of the head.
@@ -224,16 +222,7 @@ class Head(RobotJointComponent):
         """Clean up Zenoh resources for the head component."""
         self.stop()
         super().shutdown()
-        try:
-            if hasattr(self, "mode_querier") and self.mode_querier:
-                self.mode_querier.undeclare()
-        except Exception as e:
-            # Don't log "Undeclared querier" errors as warnings - they're expected during shutdown
-            error_msg = str(e).lower()
-            if not ("undeclared" in error_msg or "closed" in error_msg):
-                logger.warning(
-                    f"Error undeclaring mode querier for {self.__class__.__name__}: {e}"
-                )
+        # No need to undeclare queriers when using DexComm
 
     def _process_joint_velocities(
         self,