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

dexcontrol/core/component.py

@@ -89,11 +89,11 @@ class RobotComponent:
             Parsed protobuf state message.
 
         Raises:
-            RuntimeError: If no state data is available.
+            None: If no state data is available.
         """
         state = self._subscriber.get_latest_data()
         if state is None:
-            raise RuntimeError("No state data available")
+            logger.error(f"No state data available for {self.__class__.__name__}")
         return state
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
@@ -133,6 +133,15 @@ class RobotComponent:
         if hasattr(self, "_subscriber") and self._subscriber:
             self._subscriber.shutdown()
 
+    def get_timestamp_ns(self) -> int:
+        """Get the current timestamp (in nanoseconds) of the most recent state update.
+
+        Returns:
+            The current timestamp in nanoseconds when driver updated the state.
+            We convert the time to client clock by adding the server time offset.
+        """
+        return self._get_state().timestamp_ns
+
 
 class RobotJointComponent(RobotComponent):
     """Base class for robot components with both state and control interfaces.
@@ -171,6 +180,8 @@ class RobotJointComponent(RobotComponent):
         state_message_type: type[M],
         zenoh_session: zenoh.Session,
         joint_name: list[str] | None = None,
+        joint_limit: list[list[float]] | None = None,
+        joint_vel_limit: list[float] | None = None,
         pose_pool: Mapping[str, list[float] | np.ndarray] | None = None,
     ) -> None:
         """Initializes RobotJointComponent.
@@ -190,6 +201,13 @@ class RobotJointComponent(RobotComponent):
             resolved_topic
         )
         self._joint_name: list[str] | None = joint_name
+        self._joint_limit: np.ndarray | None = (
+            np.array(joint_limit) if joint_limit else None
+        )
+        self._joint_vel_limit: np.ndarray | None = (
+            np.array(joint_vel_limit) if joint_vel_limit else None
+        )
+
         self._pose_pool: dict[str, np.ndarray] | None = (
             self._convert_pose_pool_to_arrays(pose_pool)
         )
@@ -231,6 +249,18 @@ class RobotJointComponent(RobotComponent):
             raise ValueError("Joint names not available for this component")
         return self._joint_name.copy()
 
+    @property
+    def joint_limit(self) -> np.ndarray | None:
+        """Gets the joint limits of the component."""
+        return self._joint_limit.copy() if self._joint_limit is not None else None
+
+    @property
+    def joint_vel_limit(self) -> np.ndarray | None:
+        """Gets the joint velocity limits of the component."""
+        return (
+            self._joint_vel_limit.copy() if self._joint_vel_limit is not None else None
+        )
+
     def get_predefined_pose(self, pose_name: str) -> np.ndarray:
         """Gets a predefined pose from the pose pool.
 
@@ -466,7 +496,7 @@ class RobotJointComponent(RobotComponent):
     def _convert_joint_cmd_to_array(
         self,
         joint_cmd: Float[np.ndarray, " N"] | list[float] | dict[str, float],
-        clip_value: float | None = None,
+        clip_value: float | np.ndarray | None = None,
     ) -> np.ndarray:
         """Convert joint command to numpy array format.
 
@@ -475,7 +505,9 @@ class RobotJointComponent(RobotComponent):
                 - List of joint values [j1, j2, ..., jN]
                 - Numpy array with shape (N,)
                 - Dictionary mapping joint names to values
-            clip_value: Optional value to clip the output array between [-clip_value, clip_value].
+            clip_value: Optional value to clip the output array. Can be:
+                - float: symmetric clipping between [-clip_value, clip_value]
+                - numpy array: element-wise clipping between [-clip_value, clip_value]
 
         Returns:
             Joint command as numpy array.
@@ -634,6 +666,8 @@ class RobotJointComponent(RobotComponent):
         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.
 
@@ -645,6 +679,8 @@ class RobotJointComponent(RobotComponent):
             relative: If True, the joint positions are relative to the current position.
             wait_time: Time to wait after sending command in seconds.
             wait_kwargs: Optional parameters for trajectory generation.
+            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.
@@ -656,9 +692,49 @@ class RobotJointComponent(RobotComponent):
         if isinstance(joint_pos, (list, dict)):
             joint_pos = self._convert_joint_cmd_to_array(joint_pos)
 
+        if self._joint_limit is not None:
+            joint_pos = np.clip(
+                joint_pos, self._joint_limit[:, 0], self._joint_limit[:, 1]
+            )
+
         self._send_position_command(joint_pos)
 
         if wait_time > 0.0:
+            self._wait_for_position(
+                joint_pos, wait_time, exit_on_reach, exit_on_reach_kwargs
+            )
+
+    def _wait_for_position(
+        self,
+        joint_pos: Float[np.ndarray, " N"] | list[float] | dict[str, float],
+        wait_time: float,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
+    ) -> None:
+        """Wait for a specified time with optional early exit when position is reached.
+
+        Args:
+            joint_pos: Target joint positions to check against.
+            wait_time: Maximum time to wait in seconds.
+            exit_on_reach: If True, exit early when joint positions are reached.
+            exit_on_reach_kwargs: Optional parameters for position checking.
+        """
+        if exit_on_reach:
+            # Set default tolerance if not provided
+            exit_on_reach_kwargs = exit_on_reach_kwargs or {}
+            exit_on_reach_kwargs.setdefault("tolerance", 0.05)
+
+            # Convert to expected format for is_joint_pos_reached
+            if isinstance(joint_pos, list):
+                joint_pos = np.array(joint_pos, dtype=np.float32)
+
+            # Wait until position is reached or timeout
+            start_time = time.time()
+            while time.time() - start_time < wait_time:
+                if self.is_joint_pos_reached(joint_pos, **exit_on_reach_kwargs):
+                    break
+                time.sleep(0.01)
+        else:
             time.sleep(wait_time)
 
     def _send_position_command(self, joint_pos: Float[np.ndarray, " N"]) -> None:
@@ -679,12 +755,16 @@ class RobotJointComponent(RobotComponent):
         self,
         pose_name: str,
         wait_time: float = 3.0,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, float] | None = None,
     ) -> None:
         """Move the component to a predefined pose.
 
         Args:
             pose_name: Name of the pose to move to.
             wait_time: Time to wait for the component to reach the pose.
+            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 pose pool is not available or if an invalid pose name is provided.
@@ -696,7 +776,12 @@ class RobotJointComponent(RobotComponent):
                 f"Invalid pose name: {pose_name}. Available poses: {list(self._pose_pool.keys())}"
             )
         pose = self._pose_pool[pose_name]
-        self.set_joint_pos(pose, wait_time=wait_time)
+        self.set_joint_pos(
+            pose,
+            wait_time=wait_time,
+            exit_on_reach=exit_on_reach,
+            exit_on_reach_kwargs=exit_on_reach_kwargs,
+        )
 
     def is_joint_pos_reached(
         self,
@@ -819,11 +904,12 @@ class RobotJointComponent(RobotComponent):
         else:
             # Check all joints, ensuring arrays are same length
             min_len = min(len(current_pos), len(target_pos))
-            return bool(
+            is_reached = bool(
                 np.all(
                     np.abs(current_pos[:min_len] - target_pos[:min_len]) <= tolerance
                 )
             )
+            return is_reached
 
     def is_pose_reached(
         self,

dexcontrol/core/hand.py

@@ -14,6 +14,8 @@ 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
+
 import numpy as np
 import zenoh
 from jaxtyping import Float
@@ -51,8 +53,8 @@ class Hand(RobotJointComponent):
         )
 
         # Store predefined hand positions as private attributes
-        self._joint_pos_open = np.array(configs.joint_pos_open, dtype=np.float32)
-        self._joint_pos_close = np.array(configs.joint_pos_close, dtype=np.float32)
+        self._joint_pos_open = np.array(configs.pose_pool["open"], dtype=np.float32)
+        self._joint_pos_close = np.array(configs.pose_pool["close"], dtype=np.float32)
 
     def _send_position_command(
         self, joint_pos: Float[np.ndarray, " N"] | list[float]
@@ -73,6 +75,8 @@ class Hand(RobotJointComponent):
         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.
 
@@ -84,27 +88,61 @@ class Hand(RobotJointComponent):
             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)
+        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) -> None:
+    def open_hand(
+        self,
+        wait_time: float = 0.0,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
+    ) -> None:
         """Open the hand to the predefined open position.
 
         Args:
             wait_time: Time to wait after opening the 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.
         """
-        self.set_joint_pos(self._joint_pos_open, wait_time=wait_time)
+        self.set_joint_pos(
+            self._joint_pos_open,
+            wait_time=wait_time,
+            exit_on_reach=exit_on_reach,
+            exit_on_reach_kwargs=exit_on_reach_kwargs,
+        )
 
-    def close_hand(self, wait_time: float = 0.0) -> None:
+    def close_hand(
+        self,
+        wait_time: float = 0.0,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
+    ) -> None:
         """Close the hand to the predefined closed position.
 
         Args:
             wait_time: Time to wait after closing the 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.
+
         """
-        self.set_joint_pos(self._joint_pos_close, wait_time=wait_time)
+        self.set_joint_pos(
+            self._joint_pos_close,
+            wait_time=wait_time,
+            exit_on_reach=exit_on_reach,
+            exit_on_reach_kwargs=exit_on_reach_kwargs,
+        )
 
 
 class HandF5D6(Hand):
@@ -114,7 +152,12 @@ class HandF5D6(Hand):
     the F5D6 hand model.
     """
 
-    def close_hand(self, wait_time: float = 0.0) -> None:
+    def close_hand(
+        self,
+        wait_time: float = 0.0,
+        exit_on_reach: bool = False,
+        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
@@ -122,13 +165,28 @@ class HandF5D6(Hand):
         # 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)
+        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)
+        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,
+        )
 
-    def open_hand(self, wait_time: float = 0.0) -> None:
+    def open_hand(
+        self,
+        wait_time: float = 0.0,
+        exit_on_reach: bool = False,
+        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
@@ -136,11 +194,21 @@ class HandF5D6(Hand):
         # 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)
+        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)
+        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,
+        )
 
     def _get_intermediate_close_position(self) -> np.ndarray:
         """Get intermediate position for closing hand.

dexcontrol/core/head.py

@@ -58,74 +58,86 @@ class Head(RobotJointComponent):
             state_message_type=dexcontrol_msg_pb2.HeadState,
             zenoh_session=zenoh_session,
             joint_name=configs.joint_name,
+            joint_limit=configs.joint_limit
+            if hasattr(configs, "joint_limit")
+            else None,
+            joint_vel_limit=configs.joint_vel_limit
+            if hasattr(configs, "joint_vel_limit")
+            else None,
             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
         )
-        self.default_vel: Final[float] = configs.default_vel
-        self.max_vel: Final[float] = configs.max_vel
-        assert self.max_vel > self.default_vel, (
-            "max_vel must be greater than default_vel"
-        )
-        self._joint_limit: Float[np.ndarray, "3 2"] = np.stack(
-            [configs.joint_limit_lower, configs.joint_limit_upper], axis=1
-        )
+        assert self._joint_vel_limit is not None, "joint_vel_limit is not set"
 
     def set_joint_pos(
         self,
-        joint_pos: Float[np.ndarray, "2"] | list[float] | dict[str, float],
+        joint_pos: Float[np.ndarray, "3"] | 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, float] | None = None,
     ) -> None:
         """Send joint position control commands to the head.
 
         Args:
             joint_pos: Joint positions as either:
                 - List of joint values [j1, j2]
-                - Numpy array with shape (2,), in radians
+                - Numpy array with shape (3,), in radians
                 - 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 sending command in seconds. If 0, returns
                 immediately after sending command.
             wait_kwargs: Optional parameters for trajectory generation (not used in Head).
+            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.
         """
         self.set_joint_pos_vel(
-            joint_pos, joint_vel=None, relative=relative, wait_time=wait_time
+            joint_pos,
+            joint_vel=None,
+            relative=relative,
+            wait_time=wait_time,
+            exit_on_reach=exit_on_reach,
+            exit_on_reach_kwargs=exit_on_reach_kwargs,
         )
 
     def set_joint_pos_vel(
         self,
-        joint_pos: Float[np.ndarray, "2"] | list[float] | dict[str, float],
-        joint_vel: Float[np.ndarray, "2"]
+        joint_pos: Float[np.ndarray, "3"] | list[float] | dict[str, float],
+        joint_vel: Float[np.ndarray, "3"]
         | list[float]
         | dict[str, float]
         | float
         | None = None,
         relative: bool = False,
         wait_time: float = 0.0,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, float] | None = None,
     ) -> None:
         """Send control commands to the head.
 
         Args:
             joint_pos: Joint positions as either:
                 - List of joint values [j1, j2]
-                - Numpy array with shape (2,), in radians
+                - Numpy array with shape (3,), in radians
                 - Dictionary mapping joint names to position values
             joint_vel: Optional joint velocities as either:
                 - List of joint values [v1, v2]
-                - Numpy array with shape (2,), in rad/s
+                - Numpy array with shape (3,), in rad/s
                 - Dictionary mapping joint names to velocity values
                 - Single float value to be applied to all joints
                 If None, velocities are calculated based on default velocity setting.
             relative: If True, the joint positions are relative to the current position.
             wait_time: Time to wait after sending command in seconds. If 0, returns
                 immediately after sending command.
+            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 wait_time is negative or joint_pos dictionary contains
@@ -142,6 +154,15 @@ class Head(RobotJointComponent):
         joint_pos = self._convert_joint_cmd_to_array(joint_pos)
         joint_vel = self._process_joint_velocities(joint_vel, joint_pos)
 
+        if self._joint_limit is not None:
+            joint_pos = np.clip(
+                joint_pos, self._joint_limit[:, 0], self._joint_limit[:, 1]
+            )
+        if self._joint_vel_limit is not None:
+            joint_vel = np.clip(
+                joint_vel, -self._joint_vel_limit, self._joint_vel_limit
+            )
+
         # Create and send control message
         control_msg = dexcontrol_msg_pb2.HeadCommand()
         control_msg.joint_pos.extend(joint_pos.tolist())
@@ -149,8 +170,12 @@ class Head(RobotJointComponent):
         self._publish_control(control_msg)
 
         # Wait if specified
-        if wait_time > 0.0:
-            time.sleep(wait_time)
+        self._wait_for_position(
+            joint_pos=joint_pos,
+            wait_time=wait_time,
+            exit_on_reach=exit_on_reach,
+            exit_on_reach_kwargs=exit_on_reach_kwargs,
+        )
 
     def set_mode(self, mode: Literal["enable", "disable"]) -> None:
         """Set the operating mode of the head.
@@ -180,12 +205,12 @@ class Head(RobotJointComponent):
                 logger.info(reply.ok.payload.to_string())
         time.sleep(0.5)
 
-    def get_joint_limit(self) -> Float[np.ndarray, "3 2"]:
+    def get_joint_limit(self) -> Float[np.ndarray, "3 2"] | None:
         """Get the joint limits of the head.
 
         Returns:
             Array of joint limits with shape (3, 2), where the first column contains
-            lower limits and the second column contains upper limits.
+            lower limits and the second column contains upper limits, or None if not configured.
         """
         return self._joint_limit
 
@@ -212,7 +237,7 @@ class Head(RobotJointComponent):
 
     def _process_joint_velocities(
         self,
-        joint_vel: Float[np.ndarray, "2"]
+        joint_vel: Float[np.ndarray, "3"]
         | list[float]
         | dict[str, float]
         | float
@@ -234,14 +259,18 @@ class Head(RobotJointComponent):
             motion_norm = np.linalg.norm(joint_motion)
 
             if motion_norm < 1e-6:  # Avoid division by zero
-                return np.zeros(2, dtype=np.float32)
+                return np.zeros(3, dtype=np.float32)
 
-            # Scale velocities by default velocity
-            return (joint_motion / motion_norm) * self.default_vel
+            default_vel = (
+                2.5 if self._joint_vel_limit is None else np.min(self._joint_vel_limit)
+            )
+            return (joint_motion / motion_norm) * default_vel
 
         if isinstance(joint_vel, (int, float)):
             # Single value - apply to all joints
-            return np.full(2, joint_vel, dtype=np.float32)
+            return np.full(3, joint_vel, dtype=np.float32)
 
-        # Convert to array and clip to max velocity
-        return self._convert_joint_cmd_to_array(joint_vel, clip_value=self.max_vel)
+        # Convert to array and clip to velocity limits
+        return self._convert_joint_cmd_to_array(
+            joint_vel, clip_value=self._joint_vel_limit
+        )