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

dexcontrol/config/core/arm.py

@@ -21,6 +21,8 @@ class ArmConfig:
     control_pub_topic: str = "control/arm/right"
     set_mode_query: str = "mode/arm/right"
     dof: int = 7
+    default_max_vel: float = 2.0  # do not go over 3.0
+    default_control_hz: int = 100
     joint_name: list[str] = field(
         default_factory=lambda: [f"R_arm_j{i + 1}" for i in range(7)]
     )

dexcontrol/config/core/hand.py

@@ -28,20 +28,9 @@ class HandConfig:
         ]
     )
 
-    # 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.1834, 0.2891, 0.2801, 0.284, 0.2811, -0.0158]
-    )
-    joint_pos_close: list[float] = field(
-        default_factory=lambda: [
-            -0.3468,
-            -1.0946,
-            -1.0844,
-            -1.0154,
-            -1.0118,
-            1.6,
-        ]
+    pose_pool: dict[str, list[float]] = field(
+        default_factory=lambda: {
+            "open": [0.1834, 0.2891, 0.2801, 0.284, 0.2811, -0.0158],
+            "close": [-0.3468, -1.0946, -1.0844, -1.0154, -1.0118, 1.6],
+        }
     )

dexcontrol/core/arm.py

@@ -15,7 +15,7 @@ communication and the ArmWrenchSensor class for reading wrench sensor data.
 """
 
 import time
-from typing import Final, Literal
+from typing import Any, Final, Literal
 
 import numpy as np
 import zenoh
@@ -72,6 +72,15 @@ class Arm(RobotJointComponent):
                 configs.wrench_sub_topic, zenoh_session
             )
 
+        self._default_max_vel = configs.default_max_vel
+        self._default_control_hz = configs.default_control_hz
+        if self._default_max_vel > 3.0:
+            logger.warning(
+                f"Max velocity is set to {self._default_max_vel}, which is greater than 3.0. This is not recommended."
+            )
+            self._default_max_vel = 3.0
+            logger.warning("Max velocity is clamped to 3.0")
+
     def set_mode(self, mode: Literal["position", "disable"]) -> None:
         """Sets the operating mode of the arm.
 
@@ -112,6 +121,8 @@ class Arm(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:
         """Controls the arm in joint position mode.
 
@@ -130,7 +141,8 @@ class Arm(RobotJointComponent):
                 wait_time > 0). Supported keys:
                 - control_hz: Control frequency in Hz (default: 100).
                 - max_vel: Maximum velocity in rad/s (default: 0.5).
-
+            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.
         """
@@ -142,7 +154,13 @@ class Arm(RobotJointComponent):
         )
 
         if wait_time > 0.0:
-            self._execute_trajectory_motion(resolved_joint_pos, wait_time, wait_kwargs)
+            self._execute_trajectory_motion(
+                resolved_joint_pos,
+                wait_time,
+                wait_kwargs,
+                exit_on_reach,
+                exit_on_reach_kwargs,
+            )
         else:
             # Convert to array format
             if isinstance(resolved_joint_pos, (list, dict)):
@@ -156,6 +174,8 @@ class Arm(RobotJointComponent):
         joint_pos: Float[np.ndarray, " N"] | list[float] | dict[str, float],
         wait_time: float,
         wait_kwargs: dict[str, float],
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Execute trajectory-based motion to target position.
 
@@ -163,10 +183,14 @@ class Arm(RobotJointComponent):
             joint_pos: Target joint positions as list, numpy array, or dictionary.
             wait_time: Total time for the motion.
             wait_kwargs: 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.
         """
         # Set default parameters
-        control_hz = wait_kwargs.get("control_hz", 100)
-        max_vel = wait_kwargs.get("max_vel", 0.5)
+        control_hz = wait_kwargs.get("control_hz", self._default_control_hz)
+        max_vel = wait_kwargs.get("max_vel", self._default_max_vel)
+        exit_on_reach_kwargs = exit_on_reach_kwargs or {}
+        exit_on_reach_kwargs.setdefault("tolerance", 0.05)
 
         # Create rate limiter and get current position
         rate_limiter = RateLimiter(control_hz)
@@ -186,7 +210,6 @@ class Arm(RobotJointComponent):
         trajectory, _ = generate_linear_trajectory(
             current_joint_pos, target_pos, max_vel, control_hz
         )
-
         # Execute trajectory with time limit
         start_time = time.time()
         for pos in trajectory:
@@ -199,6 +222,10 @@ class Arm(RobotJointComponent):
         while time.time() - start_time < wait_time:
             self._send_position_command(target_pos)
             rate_limiter.sleep()
+            if exit_on_reach and self.is_joint_pos_reached(
+                target_pos, **exit_on_reach_kwargs
+            ):
+                break
 
     def set_joint_pos_vel(
         self,

dexcontrol/core/component.py

@@ -634,6 +634,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 +647,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.
@@ -659,6 +663,41 @@ class RobotJointComponent(RobotComponent):
         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 +718,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 +739,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 +867,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

@@ -79,6 +79,8 @@ class Head(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, float] | None = None,
     ) -> None:
         """Send joint position control commands to the head.
 
@@ -91,12 +93,19 @@ class Head(RobotJointComponent):
             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(
@@ -109,6 +118,8 @@ class Head(RobotJointComponent):
         | 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.
 
@@ -126,6 +137,8 @@ class Head(RobotJointComponent):
             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
@@ -149,8 +162,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.

dexcontrol/core/torso.py

@@ -14,8 +14,6 @@ This module provides the Torso class for controlling a robot torso through Zenoh
 communication. It handles joint position and velocity control and state monitoring.
 """
 
-import time
-
 import numpy as np
 import zenoh
 from jaxtyping import Float
@@ -69,6 +67,8 @@ class Torso(RobotJointComponent):
         | 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 torso.
 
@@ -86,6 +86,8 @@ class Torso(RobotJointComponent):
             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
@@ -109,8 +111,12 @@ class Torso(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_joint_pos(
         self,
@@ -118,6 +124,8 @@ class Torso(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, float] | None = None,
     ) -> None:
         """Send joint position control commands to the torso.
 
@@ -130,12 +138,19 @@ class Torso(RobotJointComponent):
             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 Torso).
+            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 stop(self) -> None:

dexcontrol/robot.py

@@ -739,14 +739,18 @@ class Robot:
 
     def get_joint_pos_dict(
         self,
-        component: Literal["left_arm", "right_arm", "torso", "head"]
-        | list[Literal["left_arm", "right_arm", "torso", "head"]],
+        component: Literal[
+            "left_arm", "right_arm", "torso", "head", "left_hand", "right_hand"
+        ]
+        | list[
+            Literal["left_arm", "right_arm", "torso", "head", "left_hand", "right_hand"]
+        ],
     ) -> dict[str, float]:
         """Get the joint positions of one or more robot components.
 
         Args:
             component: Component name or list of component names to get joint positions for.
-                Valid components are "left_arm", "right_arm", "torso", and "head".
+                Valid components are "left_arm", "right_arm", "torso", "head", "left_hand", and "right_hand".
 
         Returns:
             Dictionary mapping joint names to joint positions.
@@ -761,6 +765,8 @@ class Robot:
             "right_arm": self.right_arm,
             "torso": self.torso,
             "head": self.head,
+            "left_hand": self.left_hand,
+            "right_hand": self.right_hand,
         }
 
         try:
@@ -826,6 +832,8 @@ class Robot:
         relative: bool = False,
         wait_time: float = 0.0,
         wait_kwargs: dict[str, Any] | None = None,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Set the joint positions of the robot.
 
@@ -836,7 +844,9 @@ class Robot:
             wait_time: Time to wait for movement completion in seconds.
             wait_kwargs: Additional parameters for trajectory generation.
                 control_hz: Control frequency in Hz (default: 100).
-                max_vel: Maximum velocity for trajectory (default: 0.5).
+                max_vel: Maximum velocity for trajectory (default: 3.).
+            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 any component name is invalid.
@@ -846,6 +856,7 @@ class Robot:
             wait_kwargs = {}
 
         try:
+            start_time = time.time()
             component_map = self._get_component_map()
 
             # Validate component names
@@ -871,11 +882,69 @@ class Robot:
                     relative,
                     wait_time,
                     wait_kwargs,
+                    exit_on_reach=exit_on_reach,
+                    exit_on_reach_kwargs=exit_on_reach_kwargs,
                 )
+            remaining_time = wait_time - (time.time() - start_time)
+            if remaining_time <= 0:
+                return
+
+            self._wait_for_multi_component_positions(
+                component_map,
+                pv_components,
+                joint_pos,
+                start_time,
+                wait_time,
+                exit_on_reach,
+                exit_on_reach_kwargs,
+            )
 
         except Exception as e:
             raise RuntimeError(f"Failed to set joint positions: {e}") from e
 
+    def _wait_for_multi_component_positions(
+        self,
+        component_map: dict[str, Any],
+        components: list[str],
+        joint_pos: dict[str, list[float] | np.ndarray],
+        start_time: float,
+        wait_time: float,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
+    ) -> None:
+        """Wait for multiple components to reach target positions.
+
+        Args:
+            component_map: Mapping of component names to component instances.
+            components: List of component names to check.
+            joint_pos: Target joint positions for each component.
+            start_time: Time when the operation started.
+            wait_time: Maximum time to wait.
+            exit_on_reach: If True, exit early when all positions are reached.
+            exit_on_reach_kwargs: Optional parameters for position checking.
+        """
+        sleep_interval = 0.01  # Use consistent sleep interval
+
+        if exit_on_reach:
+            if components:
+                # Set default tolerance if not provided
+                exit_on_reach_kwargs = exit_on_reach_kwargs or {}
+
+                # Wait until all positions are reached or timeout
+                while time.time() - start_time < wait_time:
+                    if all(
+                        component_map[c].is_joint_pos_reached(
+                            joint_pos[c], **exit_on_reach_kwargs
+                        )
+                        for c in components
+                    ):
+                        break
+                    time.sleep(sleep_interval)
+        else:
+            # Simple wait without position checking
+            while time.time() - start_time < wait_time:
+                time.sleep(sleep_interval)
+
     def compensate_torso_pitch(self, joint_pos: np.ndarray, part: str) -> np.ndarray:
         """Compensate for torso pitch in joint positions.
 
@@ -1092,6 +1161,8 @@ class Robot:
         relative: bool,
         wait_time: float,
         wait_kwargs: dict[str, Any],
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Set non-PV components with smooth trajectory over wait_time.
 
@@ -1102,9 +1173,11 @@ class Robot:
             relative: Whether positions are relative.
             wait_time: Time to wait for movement completion.
             wait_kwargs: Additional trajectory parameters.
+            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.
         """
-        control_hz = wait_kwargs.get("control_hz", 100)
-        max_vel = wait_kwargs.get("max_vel", 0.5)
+        control_hz = wait_kwargs.get("control_hz", self.left_arm._default_control_hz)
+        max_vel = wait_kwargs.get("max_vel", self.left_arm._default_max_vel)
 
         # Generate trajectories for smooth motion during wait_time
         rate_limiter = RateLimiter(control_hz)
@@ -1136,5 +1209,12 @@ class Robot:
                 break
 
         # Wait for any remaining time
-        while time.time() - start_time < wait_time:
-            rate_limiter.sleep()
+        self._wait_for_multi_component_positions(
+            component_map,
+            non_pv_components,
+            joint_pos,
+            start_time,
+            wait_time,
+            exit_on_reach,
+            exit_on_reach_kwargs,
+        )

dexcontrol/sensors/camera/rgb_camera.py

@@ -138,3 +138,39 @@ class RGBCameraSensor:
             Sensor name string.
         """
         return self._name
+
+    @property
+    def height(self) -> int:
+        """Get the height of the camera image.
+
+        Returns:
+            Height of the camera image.
+        """
+        image = self.get_obs()
+        if image is None:
+            return 0
+        return image.shape[0]
+
+    @property
+    def width(self) -> int:
+        """Get the width of the camera image.
+
+        Returns:
+            Width of the camera image.
+        """
+        image = self.get_obs()
+        if image is None:
+            return 0
+        return image.shape[1]
+
+    @property
+    def resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera image.
+
+        Returns:
+            Resolution of the camera image.
+        """
+        image = self.get_obs()
+        if image is None:
+            return 0, 0
+        return image.shape[0], image.shape[1]

dexcontrol/sensors/camera/zed_camera.py

@@ -356,3 +356,34 @@ class ZedCameraSensor:
             Camera info dictionary if available, None otherwise.
         """
         return self._camera_info
+
+    @property
+    def height(self) -> dict[str, int]:
+        """Get the height of the camera image.
+
+        Returns:
+            Height of the camera image.
+        """
+        images = self.get_obs()
+        return {name: image.shape[0] if image is not None else 0 for name, image in images.items()}
+
+    @property
+    def width(self) -> dict[str, int]:
+        """Get the width of the camera image.
+
+        Returns:
+            Width of the camera image.
+        """
+        images = self.get_obs()
+        return {name: image.shape[1] if image is not None else 0 for name, image in images.items()}
+
+
+    @property
+    def resolution(self) -> dict[str, tuple[int, int]]:
+        """Get the resolution of the camera image.
+
+        Returns:
+            Resolution of the camera image.
+        """
+        images = self.get_obs()
+        return {name: (image.shape[0], image.shape[1]) if image is not None else (0, 0) for name, image in images.items()}

{dexcontrol-0.2.7.dist-info → dexcontrol-0.2.10.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: dexcontrol
-Version: 0.2.7
+Version: 0.2.10
 Summary: A Python library of Sensing and Control for Dexmate's Robot
 Project-URL: Repository, https://github.com/dexmate-ai/dexcontrol
 Author-email: Dexmate <contact@dexmate.ai>
@@ -200,7 +200,7 @@ Classifier: Programming Language :: Python :: 3.11
 Classifier: Programming Language :: Python :: 3.12
 Classifier: Programming Language :: Python :: 3.13
 Classifier: Typing :: Typed
-Requires-Python: <3.13,>=3.10
+Requires-Python: <3.14,>=3.10
 Requires-Dist: eclipse-zenoh>=1.2.0
 Requires-Dist: hydra-core==1.3.2
 Requires-Dist: jaxtyping>=0.2.38
@@ -225,7 +225,7 @@ Requires-Dist: sphinx-autodoc-typehints>=1.24.0; extra == 'docs'
 Requires-Dist: sphinx-rtd-theme>=1.3.0; extra == 'docs'
 Requires-Dist: sphinx>=7.0.0; extra == 'docs'
 Provides-Extra: example
-Requires-Dist: dexmotion>=0.2.0; extra == 'example'
+Requires-Dist: dexmotion>=0.2.1; extra == 'example'
 Requires-Dist: dualsense-controller>=0.3.1; extra == 'example'
 Requires-Dist: matplotlib>=3.8.0; extra == 'example'
 Requires-Dist: pytransform3d==3.13.0; extra == 'example'

{dexcontrol-0.2.7.dist-info → dexcontrol-0.2.10.dist-info}/RECORD

@@ -1,12 +1,12 @@
 dexcontrol/__init__.py,sha256=1UGkcmMHphLJM7MYIfSuWfSameV1XLBDVjEazl5hb1I,1656
-dexcontrol/robot.py,sha256=Kyp8VXcDM6PORlvWBQqH4JR-4Uu6ttn652gNX7t5fg8,41686
+dexcontrol/robot.py,sha256=vFjrSHEJQaivieeDdRAfeUq39qhWOTmUN-2-pYnjWLE,45025
 dexcontrol/apps/dualsense_teleop_base.py,sha256=Dw1z-2HA5D7DPKutZxlOdXsN9vpk4gS6XzJsL5ZQLM0,12702
 dexcontrol/config/__init__.py,sha256=UVLNpzGD14e8g68rUZFXTh0B7FRx6uS0Eg_MecjinYM,520
 dexcontrol/config/vega.py,sha256=4RrP0V5bmH0OUM3V0gND6cpRuMqoziLTQdKiRey1_ag,7192
 dexcontrol/config/core/__init__.py,sha256=Ym2R1hr1iMKQuXcg16BpZfQtTb0hQ5Q7smUIMlwKfho,637
-dexcontrol/config/core/arm.py,sha256=bRemtNO_eQVUgcojw1ItwWiqOZVOaaIq72G5-QqFu-I,1183
+dexcontrol/config/core/arm.py,sha256=G7k39vLx_PkctfZIaTMIAiJscpDAMo9B-ZLuy7xRsPo,1272
 dexcontrol/config/core/chassis.py,sha256=163hO4lVVaW7r9dvNleH0cdDds_GfO15EepnjloeT9U,868
-dexcontrol/config/core/hand.py,sha256=MONnh9rUmfDqUl3NMoJ6rOy8hZKvD5iv1egkYdIl1pQ,1272
+dexcontrol/config/core/hand.py,sha256=gPIsYgm1wP_L4BWwVmnG8UGxM_a5J_HJDkIamhgl2n4,954
 dexcontrol/config/core/head.py,sha256=1eqsDPGBG8r8MocJwUMnDAQnOD152sl7P2k6Lg_ZKOA,1124
 dexcontrol/config/core/misc.py,sha256=KQstrVKFL-vdoaHVdj-VGwMrrhzm4pVz2g4A2etnav4,1169
 dexcontrol/config/core/torso.py,sha256=6B_-8LhT4rJ4K_oF9sjroOwIIj_JwlNS2JC8LmCCrSA,1456
@@ -23,13 +23,13 @@ dexcontrol/config/sensors/lidar/rplidar.py,sha256=ybuT_f1ADWF3oGH1gi6D2F80TbJEm4
 dexcontrol/config/sensors/ultrasonic/__init__.py,sha256=-q83RhIMZJGVFVPYaA4hOugoG6wZw8EL6wJg7-HTSxU,294
 dexcontrol/config/sensors/ultrasonic/ultrasonic.py,sha256=7b4dm1QOhy5_5RFVpY-frXZyDzqok0K1u7ed9gf3PL0,552
 dexcontrol/core/__init__.py,sha256=bYPMLxbbn5QeuPyA6OPGDS2JTYpnVvaZJT8PeILFjQY,252
-dexcontrol/core/arm.py,sha256=jeBpPILV_-n6r-tqFUUjQPSog1Jxd9HKki572S6e8qk,12467
+dexcontrol/core/arm.py,sha256=6acUfzOTGEzPrdKDFcdMGgf5ovYNLP14PwGYkSnSpLM,13917
 dexcontrol/core/chassis.py,sha256=tB-tsx6MjKJDOtQxBf7PIX0L1JwgnsXUzmNPC1VKsQU,23165
-dexcontrol/core/component.py,sha256=KRlf6XR5vvki1N4EorIJzvc15_O8UZMHWa4khHTPDVk,31060
-dexcontrol/core/hand.py,sha256=swfWWM9h6o27i-41R-fw_JLfOzBHtpNIJDI1IgQPGIE,6541
-dexcontrol/core/head.py,sha256=V1ySWoFSPGCX65tbP-UHWiXgiJEtnEdiWLUDtvl-Sns,9227
+dexcontrol/core/component.py,sha256=Z1-Q80yPJJ3njp45jrd51mEWKUtGgqq3oLYDTI4mBcw,33279
+dexcontrol/core/hand.py,sha256=wd__YB5wcM-LlksEm3PatXhy665zLnu9nV_9x85WxIA,8649
+dexcontrol/core/head.py,sha256=_COrqMmGzefK8lK38i6J3FqvcaeTAsY6Ss2rkHf1yoE,10097
 dexcontrol/core/misc.py,sha256=m7WtziXGyhM7z27MjxiIQfTMu0_z30ReLkUCHzaRk9E,22547
-dexcontrol/core/torso.py,sha256=N5XE_lY-i78iMFEjIxMJ9I6KWK4YgHv6VqVUKEWLrcg,7402
+dexcontrol/core/torso.py,sha256=xJc8tTC5vbvT-pXz6fHgZEa17YNF-V8f9pHoEsUzn6s,8259
 dexcontrol/proto/dexcontrol_msg_pb2.py,sha256=iRvfKLjV7eRxb9jOB_KrCsd77GbgzByThW_JIKd_ibM,5691
 dexcontrol/proto/dexcontrol_msg_pb2.pyi,sha256=_vzo6hozKpC5jzdKUOTH81VXCNYO1awcgTFhVNo91N0,9371
 dexcontrol/proto/dexcontrol_query_pb2.py,sha256=Aa98bPmOuImBheN6fv4Ysy1Bmk5oX2tGatqcykDUz0E,5328
@@ -38,8 +38,8 @@ dexcontrol/sensors/__init__.py,sha256=Dp06cuO_3xC6i4u5rHqfK5NqlIC5kaCue_bAtTC6JE
 dexcontrol/sensors/manager.py,sha256=pBseqoAcrYdbBbSpFGChh_ROQlxnKDcolcKzKCEjpwM,6853
 dexcontrol/sensors/ultrasonic.py,sha256=WAHvHh64iQ0HfqVf-Oo0Rg8R32Cdk5d8k8kDSwa3Xrc,3258
 dexcontrol/sensors/camera/__init__.py,sha256=Vwe98I4Lvdv3F2UslOzKkeUkt5Rl2jSqbKlU6gIBeF0,609
-dexcontrol/sensors/camera/rgb_camera.py,sha256=ZyWS2FkEF7MYg5rlG_YXd8VpbBu--Y-1Rlb6uzYHNNo,5045
-dexcontrol/sensors/camera/zed_camera.py,sha256=7gVoRhwynCUjyTOtUBZCiNZS3MhXoQYBDrr_fGlho4g,14007
+dexcontrol/sensors/camera/rgb_camera.py,sha256=UaEk8aszHeBRg5xMQlHxZk2KMP7wU3m6UnE_qgh6V4Q,5892
+dexcontrol/sensors/camera/zed_camera.py,sha256=OrmyZmamM2tpZD4Wu66MJUjvX4zZPIWsl0DZZHhAloE,14964
 dexcontrol/sensors/imu/__init__.py,sha256=bBC7_NSLJ5qLMvUYu2-9yXKO2bRpQLC0HyywBwnbM0A,768
 dexcontrol/sensors/imu/chassis_imu.py,sha256=9gziCrtFKfWmeiDdbgD4aIkkdtjJahbckayYj_Lv5r4,4858
 dexcontrol/sensors/imu/zed_imu.py,sha256=VB0TfQYIqKNEPGPCbdAO9YtshWs4nE7y0y9FnWU-Mko,5148
@@ -66,7 +66,7 @@ dexcontrol/utils/subscribers/imu.py,sha256=Us4ZWzLfqujg16jvrNIoHcvUyxbRCv6mZlaAi
 dexcontrol/utils/subscribers/lidar.py,sha256=ZIUpzfb-n5zLnCfZBDxJMMI4FKmlwdOYHQzpVlfIGQw,5631
 dexcontrol/utils/subscribers/protobuf.py,sha256=gtE2b9ZtR2UXftKA5nX7bvTLkj8AeXDYZMqe4B3t5BQ,3696
 dexcontrol/utils/subscribers/rtc.py,sha256=mxD-IIeQwluvq0_D63lQJJEXrJsIc6yxX7uD0PDh08k,11350
-dexcontrol-0.2.7.dist-info/METADATA,sha256=9V7o2gUcFDBux04BFEqFSmYs0MCA8Pxqbvara3g9Fe8,36790
-dexcontrol-0.2.7.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
-dexcontrol-0.2.7.dist-info/licenses/LICENSE,sha256=0J2KCMNNnW5WZPK5x8xUiCxApBf7h83693ggSJYiue0,31745
-dexcontrol-0.2.7.dist-info/RECORD,,
+dexcontrol-0.2.10.dist-info/METADATA,sha256=VNSJzSGYSXAhUaKM3oj8Y7qy8hH1tkX4QAEGLV-mDDY,36791
+dexcontrol-0.2.10.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
+dexcontrol-0.2.10.dist-info/licenses/LICENSE,sha256=0J2KCMNNnW5WZPK5x8xUiCxApBf7h83693ggSJYiue0,31745
+dexcontrol-0.2.10.dist-info/RECORD,,