dexcontrol 0.2.4__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/config/sensors/cameras/__init__.py

@@ -10,3 +10,5 @@
 
 from .rgb_camera import RGBCameraConfig
 from .zed_camera import ZedCameraConfig
+
+__all__ = ["RGBCameraConfig", "ZedCameraConfig"]

dexcontrol/config/sensors/cameras/rgb_camera.py

@@ -13,16 +13,33 @@ from dataclasses import dataclass, field
 
 @dataclass
 class RGBCameraConfig:
+    """Configuration for the RGBCameraSensor.
+
+    Attributes:
+        _target_: The target class to instantiate.
+        name: A unique name for the sensor instance.
+        enable: Whether the sensor is enabled.
+        use_rtc: If True, uses the high-performance RTCSubscriber. If False,
+                 uses the standard RGBCameraSubscriber with a Zenoh topic.
+        enable_fps_tracking: If True, tracks and logs the FPS of the stream.
+        fps_log_interval: The interval in seconds for logging FPS.
+        subscriber_config: A dictionary containing the configuration for the
+                           underlying subscriber (either RTC or standard).
+    """
+
     _target_: str = "dexcontrol.sensors.camera.rgb_camera.RGBCameraSensor"
     name: str = "rgb_camera"
+    enable: bool = False
+    use_rtc: bool = True
     enable_fps_tracking: bool = False
     fps_log_interval: int = 30
-    enable: bool = False
+
     subscriber_config: dict = field(
-        default_factory=lambda: dict(
-            rgb=dict(
-                enable=True,
-                info_key="camera/rgb/info",
-            )
-        )
+        default_factory=lambda: {
+            "rgb": {
+                "enable": True,
+                "info_key": "camera/rgb/info",
+                "topic": "camera/rgb",
+            }
+        }
     )

dexcontrol/config/sensors/cameras/zed_camera.py

@@ -13,24 +13,43 @@ from dataclasses import dataclass, field
 
 @dataclass
 class ZedCameraConfig:
+    """Configuration for the ZedCameraSensor.
+
+    Attributes:
+        _target_: The target class to instantiate.
+        name: A unique name for the sensor instance.
+        enable: Whether the sensor is enabled.
+        use_rtc: If True, RGB streams use the high-performance RTCSubscriber.
+                 If False, they use the standard RGBCameraSubscriber. Depth
+                 always uses a standard Zenoh subscriber.
+        enable_fps_tracking: If True, tracks and logs the FPS of all streams.
+        fps_log_interval: The interval in seconds for logging FPS.
+        subscriber_config: A dictionary containing the configurations for each
+                           individual stream (left_rgb, right_rgb, depth).
+    """
+
     _target_: str = "dexcontrol.sensors.camera.zed_camera.ZedCameraSensor"
     name: str = "zed_camera"
+    enable: bool = False
+    use_rtc: bool = True
     enable_fps_tracking: bool = False
     fps_log_interval: int = 30
-    enable: bool = False
+
     subscriber_config: dict = field(
-        default_factory=lambda: dict(
-            left_rgb=dict(
-                enable=True,
-                info_key="camera/head/left_rgb/info",
-            ),
-            right_rgb=dict(
-                enable=True,
-                info_key="camera/head/right_rgb/info",
-            ),
-            depth=dict(
-                enable=True,
-                topic="camera/head/depth",
-            ),
-        )
+        default_factory=lambda: {
+            "left_rgb": {
+                "enable": True,
+                "info_key": "camera/head/left_rgb/info",
+                "topic": "camera/head/left_rgb",
+            },
+            "right_rgb": {
+                "enable": True,
+                "info_key": "camera/head/right_rgb/info",
+                "topic": "camera/head/right_rgb",
+            },
+            "depth": {
+                "enable": True,
+                "topic": "camera/head/depth",
+            },
+        }
     )

dexcontrol/config/sensors/vega_sensors.py

@@ -38,6 +38,7 @@ def _make_rgb_camera(name: str) -> Callable[[], RGBCameraConfig]:
             rgb=dict(
                 enable=True,
                 info_key=f"camera/base/{name}/rgb/info",
+                topic=f"camera/base/{name}/rgb",
             )
         ),
         name=f"base_{name}_camera",

dexcontrol/config/vega.py

@@ -167,6 +167,12 @@ class Vega1Config(VegaConfig):
             wheels_dist=0.45,
         )
     )
+    head: HeadConfig = field(
+        default_factory=lambda: HeadConfig(
+            joint_limit_lower=[-1.483, -2.792, -1.378],
+            joint_limit_upper=[1.483, 2.792, 1.483],
+        )
+    )
 
 
 # Register variant configurations

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: