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

dexcontrol/__init__.py

@@ -30,9 +30,10 @@ from dexcontrol.robot import Robot
 from dexcontrol.utils.constants import COMM_CFG_PATH_ENV_VAR
 
 # Package-level constants
+__version__: str = "0.3.0"  # Current library version
 LIB_PATH: Final[Path] = Path(__file__).resolve().parent
 CFG_PATH: Final[Path] = LIB_PATH / "config"
-MIN_SOC_SOFTWARE_VERSION: int = 233
+MIN_SOC_SOFTWARE_VERSION: int = 286
 
 logger.configure(handlers=[{"sink": RichHandler(markup=True), "format": "{message}"}])
 

dexcontrol/config/core/chassis.py

@@ -14,14 +14,19 @@ from dataclasses import dataclass, field
 @dataclass
 class ChassisConfig:
     _target_: str = "dexcontrol.core.chassis.Chassis"
-    control_pub_topic: str = "control/chassis"
-    state_sub_topic: str = "state/chassis"
+    steer_control_pub_topic: str = "control/chassis/steer"
+    steer_state_sub_topic: str = "state/chassis/steer"
+    drive_control_pub_topic: str = "control/chassis/drive"
+    drive_state_sub_topic: str = "state/chassis/drive"
     dof: int = 2
     center_to_wheel_axis_dist: float = (
         0.219  # the distance between base center and wheel axis in m
     )
     wheels_dist: float = 0.45  # the distance between two wheels in m (0.41 for vega-rc2, 0.45 for vega-1)
-    joint_name: list[str] = field(
-        default_factory=lambda: ["L_wheel_j1", "L_wheel_j2", "R_wheel_j1", "R_wheel_j2"]
+    steer_joint_name: list[str] = field(
+        default_factory=lambda: ["L_wheel_j1", "R_wheel_j1"]
+    )
+    drive_joint_name: list[str] = field(
+        default_factory=lambda: ["L_wheel_j2", "R_wheel_j2"]
     )
     max_vel: float = 0.8

dexcontrol/config/core/hand.py

@@ -16,6 +16,7 @@ class HandConfig:
     _target_: str = "dexcontrol.core.hand.HandF5D6"
     state_sub_topic: str = "state/hand/right"
     control_pub_topic: str = "control/hand/right"
+    touch_sensor_sub_topic: str = "state/hand/right/touch"  # Only for V2 hand
     dof: int = 6
     joint_name: list[str] = field(
         default_factory=lambda: [

dexcontrol/config/vega.py

@@ -77,6 +77,7 @@ class VegaConfig:
         default_factory=lambda: HandConfig(
             state_sub_topic="state/hand/left",
             control_pub_topic="control/hand/left",
+            touch_sensor_sub_topic="state/hand/left/touch",
             joint_name=[
                 "L_th_j1",
                 "L_ff_j1",
@@ -91,6 +92,7 @@ class VegaConfig:
         default_factory=lambda: HandConfig(
             state_sub_topic="state/hand/right",
             control_pub_topic="control/hand/right",
+            touch_sensor_sub_topic="state/hand/right/touch",
             joint_name=[
                 "R_th_j1",
                 "R_ff_j1",
@@ -111,8 +113,9 @@ class VegaConfig:
     heartbeat: HeartbeatConfig = field(default_factory=HeartbeatConfig)
 
     # Queries
-    version_info_name: str = "info/version"
+    version_info_name: str = "info/versions"  # Updated to use JSON interface
     status_info_name: str = "info/status"
+    hand_info_query_name: str = "info/hand_type"
     reboot_query_name: str = "system/reboot"
     clear_error_query_name: str = "system/clear_error"
     led_query_name: str = "system/led"

dexcontrol/core/arm.py

@@ -55,7 +55,7 @@ class Arm(RobotJointComponent):
         super().__init__(
             state_sub_topic=configs.state_sub_topic,
             control_pub_topic=configs.control_pub_topic,
-            state_message_type=dexcontrol_msg_pb2.ArmState,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
             zenoh_session=zenoh_session,
             joint_name=configs.joint_name,
             joint_limit=configs.joint_limit
@@ -97,6 +97,9 @@ class Arm(RobotJointComponent):
     def set_mode(self, mode: Literal["position", "disable"]) -> None:
         """Sets the operating mode of the arm.
 
+        .. deprecated::
+            Use set_modes() instead for setting arm modes.
+
         Args:
             mode: Operating mode for the arm. Must be either "position" or "disable".
                 "position": Enable position control
@@ -105,17 +108,35 @@ class Arm(RobotJointComponent):
         Raises:
             ValueError: If an invalid mode is specified.
         """
+        logger.warning("arm.set_mode() is deprecated, use set_modes() instead")
+        self.set_modes([mode] * 7)
+
+    def set_modes(self, modes: list[Literal["position", "disable", "release"]]) -> None:
+        """Sets the operating modes of the arm.
+
+        Args:
+            modes: List of operating modes for the arm. Each mode must be either "position", "disable", or "current".
+
+        Raises:
+            ValueError: If any mode in the list is invalid.
+        """
         mode_map = {
             "position": dexcontrol_query_pb2.SetArmMode.Mode.POSITION,
             "disable": dexcontrol_query_pb2.SetArmMode.Mode.DISABLE,
+            "release": dexcontrol_query_pb2.SetArmMode.Mode.CURRENT,
         }
 
-        if mode not in mode_map:
-            raise ValueError(
-                f"Invalid mode: {mode}. Must be one of {list(mode_map.keys())}"
-            )
+        for mode in modes:
+            if mode not in mode_map:
+                raise ValueError(
+                    f"Invalid mode: {mode}. Must be one of {list(mode_map.keys())}"
+                )
+
+        if len(modes) != 7:
+            raise ValueError("Arm modes length must match arm DoF (7).")
 
-        query_msg = dexcontrol_query_pb2.SetArmMode(mode=mode_map[mode])
+        converted_modes = [mode_map[mode] for mode in modes]
+        query_msg = dexcontrol_query_pb2.SetArmMode(modes=converted_modes)
         self.mode_querier.get(payload=query_msg.SerializeToString())
 
     def _send_position_command(self, joint_pos: np.ndarray) -> None:
@@ -124,8 +145,8 @@ class Arm(RobotJointComponent):
         Args:
             joint_pos: Joint positions as numpy array.
         """
-        control_msg = dexcontrol_msg_pb2.ArmCommand()
-        control_msg.joint_pos.extend(joint_pos.tolist())
+        control_msg = dexcontrol_msg_pb2.MotorPosVelCurrentCommand()
+        control_msg.pos.extend(joint_pos.tolist())
         self._publish_control(control_msg)
 
     def set_joint_pos(
@@ -287,10 +308,9 @@ class Arm(RobotJointComponent):
             joint_vel, clip_value=self._joint_vel_limit
         )
 
-        control_msg = dexcontrol_msg_pb2.ArmCommand(
-            command_type=dexcontrol_msg_pb2.ArmCommand.CommandType.VELOCITY_FEEDFORWARD,
-            joint_pos=list(target_pos),
-            joint_vel=list(target_vel),
+        control_msg = dexcontrol_msg_pb2.MotorPosVelCurrentCommand(
+            pos=list(target_pos),
+            vel=list(target_vel),
         )
         self._publish_control(control_msg)
 

dexcontrol/core/chassis.py

@@ -18,6 +18,7 @@ import time
 
 import numpy as np
 import zenoh
+from jaxtyping import Float
 
 from dexcontrol.config.core import ChassisConfig
 from dexcontrol.core.component import RobotJointComponent
@@ -25,7 +26,88 @@ from dexcontrol.proto import dexcontrol_msg_pb2
 from dexcontrol.utils.rate_limiter import RateLimiter
 
 
-class Chassis(RobotJointComponent):
+class ChassisSteer(RobotJointComponent):
+    """Robot hand control class.
+
+    This class provides methods to control a robotic hand by publishing commands and
+    receiving state information through Zenoh communication.
+    """
+
+    def __init__(
+        self,
+        configs: ChassisConfig,
+        zenoh_session: zenoh.Session,
+    ) -> 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.steer_state_sub_topic,
+            control_pub_topic=configs.steer_control_pub_topic,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
+            zenoh_session=zenoh_session,
+            joint_name=configs.steer_joint_name,
+        )
+
+    def _send_position_command(
+        self, joint_pos: Float[np.ndarray, " 2"] | list[float]
+    ) -> None:
+        """Send joint position control commands to the hand.
+
+        Args:
+            joint_pos: Joint positions as list or numpy array.
+        """
+        control_msg = dexcontrol_msg_pb2.MotorPosVelCommand()
+        joint_pos_array = self._convert_joint_cmd_to_array(joint_pos)
+        control_msg.pos.extend(joint_pos_array.tolist())
+        self._publish_control(control_msg)
+
+
+class ChassisDrive(RobotJointComponent):
+    """Robot base control class.
+
+    This class provides methods to control a robot's wheeled base by publishing commands
+    and receiving state information through Zenoh communication.
+    """
+
+    def __init__(
+        self,
+        configs: ChassisConfig,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the base controller.
+
+        Args:
+            configs: Base configuration parameters containing communication topics.
+            zenoh_session: Active Zenoh communication session for message passing.
+        """
+        super().__init__(
+            state_sub_topic=configs.drive_state_sub_topic,
+            control_pub_topic=configs.drive_control_pub_topic,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
+            zenoh_session=zenoh_session,
+            joint_name=configs.drive_joint_name,
+        )
+
+    def _send_position_command(
+        self, joint_pos: Float[np.ndarray, " 2"] | list[float]
+    ) -> None:
+        raise NotImplementedError("ChassisDrive does not support position control.")
+
+    def _send_velocity_command(
+        self, joint_vel: Float[np.ndarray, " 2"] | list[float]
+    ) -> None:
+        control_msg = dexcontrol_msg_pb2.MotorVelCommand()
+        joint_vel_array = self._convert_joint_cmd_to_array(joint_vel)
+        control_msg.vel.extend(joint_vel_array.tolist())
+        self._publish_control(control_msg)
+
+
+class Chassis:
     """Robot base control class.
 
     This class provides methods to control a robot's wheeled base by publishing commands
@@ -52,13 +134,9 @@ class Chassis(RobotJointComponent):
             configs: Base configuration parameters containing 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.ChassisState,
-            zenoh_session=zenoh_session,
-            joint_name=configs.joint_name,
-        )
+        self.chassis_steer = ChassisSteer(configs, zenoh_session)
+        self.chassis_drive = ChassisDrive(configs, zenoh_session)
+
         self.max_vel = configs.max_vel
         self._center_to_wheel_axis_dist = configs.center_to_wheel_axis_dist
         self._wheels_dist = configs.wheels_dist
@@ -96,7 +174,8 @@ class Chassis(RobotJointComponent):
     def shutdown(self) -> None:
         """Shutdown the base by stopping all motion."""
         self.stop()
-        super().shutdown()
+        self.chassis_steer.shutdown()
+        self.chassis_drive.shutdown()
 
     @property
     def steering_angle(self) -> np.ndarray:
@@ -106,8 +185,7 @@ class Chassis(RobotJointComponent):
             Numpy array of shape (2,) containing current steering angles in radians.
             Index 0 is left wheel, index 1 is right wheel.
         """
-        state = self._get_state()
-        return np.array([state.left.steering_pos, state.right.steering_pos])
+        return self.chassis_steer.get_joint_pos()
 
     @property
     def wheel_velocity(self) -> np.ndarray:
@@ -117,8 +195,17 @@ class Chassis(RobotJointComponent):
             Numpy array of shape (2,) containing current wheel velocities in m/s.
             Index 0 is left wheel, index 1 is right wheel.
         """
-        state = self._get_state()
-        return np.array([state.left.wheel_vel, state.right.wheel_vel])
+        return self.chassis_drive.get_joint_vel()
+
+    @property
+    def wheel_encoder_pos(self) -> np.ndarray:
+        """Get current wheel encoder positions.
+
+        Returns:
+            Numpy array of shape (2,) containing current wheel encoder positions in radians.
+            Index 0 is left wheel, index 1 is right wheel.
+        """
+        return self.chassis_drive.get_joint_pos()
 
     def set_velocity(
         self,
@@ -356,24 +443,23 @@ class Chassis(RobotJointComponent):
         Args:
             joint_id: Optional ID(s) of specific joints to query.
                 0: left steering position
-                1: left wheel position
-                2: right steering position
+                1: right steering position
+                2: left wheel position
                 3: right wheel position
 
         Returns:
             Array of joint positions in the order:
-            [left_steering_pos, left_wheel_pos, right_steering_pos, right_wheel_pos]
+            [left_steering_pos, right_steering_pos, left_wheel_pos, right_wheel_pos]
         """
-        state = self._get_state()
-        joint_pos = np.array(
-            [
-                state.left.steering_pos,
-                state.left.wheel_pos,
-                state.right.steering_pos,
-                state.right.wheel_pos,
-            ]
-        )
-        return self._extract_joint_info(joint_pos, joint_id=joint_id)
+        steer_pos = self.chassis_steer.get_joint_pos()
+        drive_pos = self.chassis_drive.get_joint_pos()
+        joint_pos = np.concatenate([steer_pos, drive_pos])
+        return RobotJointComponent._extract_joint_info(joint_pos, joint_id=joint_id)
+
+    @property
+    def joint_name(self) -> list[str]:
+        """Get the joint names of the chassis."""
+        return self.chassis_steer.joint_name + self.chassis_drive.joint_name
 
     def get_joint_pos_dict(
         self, joint_id: list[int] | int | None = None
@@ -383,58 +469,22 @@ class Chassis(RobotJointComponent):
         Args:
             joint_id: Optional ID(s) of specific joints to query.
                 0: left steering position
-                1: left wheel position
-                2: right steering position
+                1: right steering position
+                2: left wheel position
                 3: right wheel position
 
         Returns:
             Dictionary mapping joint names to position values.
         """
-        values = self.get_joint_pos(joint_id)
-        return self._convert_to_dict(values, joint_id)
-
-    def get_joint_state(self, joint_id: list[int] | int | None = None) -> np.ndarray:
-        """Get current wheel states (left and right wheel).
-
-        Args:
-            joint_id: Optional ID(s) of specific joints to query.
-                0: left wheel
-                1: right wheel
-
-        Returns:
-            Numpy array containing:
-                [:, 0]: Current steering positions in radians
-                [:, 1]: Current wheel positions in radians
-                [:, 2]: Current wheel velocities in m/s
-        """
-        state = self._get_state()
-        wheel_states = np.array(
-            [
-                [state.left.steering_pos, state.left.wheel_pos, state.left.wheel_vel],
-                [
-                    state.right.steering_pos,
-                    state.right.wheel_pos,
-                    state.right.wheel_vel,
-                ],
-            ]
-        )
-        return self._extract_joint_info(wheel_states, joint_id=joint_id)
-
-    def get_joint_state_dict(
-        self, joint_id: list[int] | int | None = None
-    ) -> dict[str, np.ndarray]:
-        """Get current wheel states as a dictionary.
-
-        Args:
-            joint_id: Optional ID(s) of specific joints to query.
-                0: left wheel
-                1: right wheel
-
-        Returns:
-            Dictionary mapping joint names to arrays of [steering_pos, wheel_pos, wheel_vel]
-        """
-        values = self.get_joint_state(joint_id)
-        return self._convert_to_dict(values, joint_id)
+        steer_pos = self.chassis_steer.get_joint_pos_dict()
+        drive_pos = self.chassis_drive.get_joint_pos_dict()
+        if joint_id is None:
+            return {**steer_pos, **drive_pos}
+        else:
+            joint_names = self.joint_name
+            if isinstance(joint_id, int):
+                joint_id = [joint_id]
+            return {joint_names[i]: steer_pos[joint_names[i]] for i in joint_id}
 
     def set_motion_state(
         self,
@@ -527,10 +577,19 @@ class Chassis(RobotJointComponent):
         # Set default control frequency if not provided
         control_hz = wait_kwargs.get("control_hz", 50)
         rate_limiter = RateLimiter(control_hz)
-        start_time = time.time()
 
-        while time.time() - start_time < wait_time:
+        now_sec = time.monotonic()
+        duration_sec = max(wait_time - 1.0, 0.0)
+        end_time_sec = now_sec + duration_sec
+
+        while True:
+            # Ensure command is sent at least once
             self._send_single_command(steering_angle, wheel_velocity)
+
+            # Exit if we've reached the allotted duration
+            if time.monotonic() >= end_time_sec:
+                break
+
             rate_limiter.sleep()
 
     def _send_single_command(
@@ -544,24 +603,16 @@ class Chassis(RobotJointComponent):
             steering_angle: Target steering angles.
             wheel_velocity: Target wheel velocities.
         """
-        control_msg = dexcontrol_msg_pb2.ChassisCommand()
-        state = self._get_state()
-
-        # Set steering positions and wheel velocities
-        control_msg.left.steering_pos = _get_value(
-            steering_angle, 0, "L_wheel_j1", state.left.steering_pos
-        )
-        control_msg.right.steering_pos = _get_value(
-            steering_angle, 1, "R_wheel_j1", state.right.steering_pos
-        )
-        control_msg.left.wheel_vel = _get_value(
-            wheel_velocity, 0, "L_wheel_j2", state.left.wheel_vel
-        )
-        control_msg.right.wheel_vel = _get_value(
-            wheel_velocity, 1, "R_wheel_j2", state.right.wheel_vel
-        )
+        if isinstance(steering_angle, float):
+            steering_angle = np.array([steering_angle, steering_angle])
+            self.chassis_steer._send_position_command(steering_angle)
+        elif isinstance(steering_angle, np.ndarray):
+            self.chassis_steer._send_position_command(steering_angle)
 
-        self._publish_control(control_msg)
+        if isinstance(wheel_velocity, float):
+            wheel_velocity = np.array([wheel_velocity, wheel_velocity])
+        elif isinstance(wheel_velocity, np.ndarray):
+            self.chassis_drive._send_velocity_command(wheel_velocity)
 
     def _compute_wheel_control(
         self, wheel_velocity: np.ndarray, current_angle: float
@@ -608,26 +659,6 @@ class Chassis(RobotJointComponent):
 
         return steering_angle, wheel_speed
 
-
-def _get_value(
-    data: float | np.ndarray | dict[str, float],
-    idx: int,
-    key: str,
-    fallback: float,
-) -> float:
-    """Helper function to extract values with fallback to current state.
-
-    Args:
-        data: Input scalar, array or dictionary containing values.
-        idx: Index to access if data is array.
-        key: Key to access if data is dictionary.
-        fallback: Default value if key/index not found.
-
-    Returns:
-        Extracted float value.
-    """
-    if isinstance(data, dict):
-        return float(data.get(key, fallback))
-    if isinstance(data, (int, float)):
-        return float(data)
-    return float(data[idx])
+    def is_active(self) -> bool:
+        """Check if the chassis is active."""
+        return self.chassis_steer.is_active() and self.chassis_drive.is_active()

dexcontrol/core/component.py

@@ -311,9 +311,9 @@ class RobotJointComponent(RobotComponent):
             ValueError: If joint positions are not available for this component.
         """
         state = self._get_state()
-        if not hasattr(state, "joint_pos"):
+        if not hasattr(state, "pos"):
             raise ValueError("Joint positions are not available for this component.")
-        joint_pos = np.array(state.joint_pos, dtype=np.float32)
+        joint_pos = np.array(state.pos, dtype=np.float32)
         return self._extract_joint_info(joint_pos, joint_id=joint_id)
 
     def get_joint_pos_dict(
@@ -349,9 +349,9 @@ class RobotJointComponent(RobotComponent):
             ValueError: If joint velocities are not available for this component.
         """
         state = self._get_state()
-        if not hasattr(state, "joint_vel"):
+        if not hasattr(state, "vel"):
             raise ValueError("Joint velocities are not available for this component.")
-        joint_vel = np.array(state.joint_vel, dtype=np.float32)
+        joint_vel = np.array(state.vel, dtype=np.float32)
         return self._extract_joint_info(joint_vel, joint_id=joint_id)
 
     def get_joint_vel_dict(
@@ -386,11 +386,31 @@ class RobotJointComponent(RobotComponent):
             ValueError: If joint currents are not available for this component.
         """
         state = self._get_state()
-        if not hasattr(state, "joint_cur"):
+        if not hasattr(state, "cur"):
             raise ValueError("Joint currents are not available for this component.")
-        joint_cur = np.array(state.joint_cur, dtype=np.float32)
+        joint_cur = np.array(state.cur, dtype=np.float32)
         return self._extract_joint_info(joint_cur, joint_id=joint_id)
 
+    def get_joint_torque(
+        self, joint_id: list[int] | int | None = None
+    ) -> Float[np.ndarray, " N"]:
+        """Gets the torque of all joints in the component.
+
+        Args:
+            joint_id: Optional ID(s) of specific joints to query.
+
+        Returns:
+            Array of joint torques in component-specific units (Nm).
+
+        Raises:
+            ValueError: If joint torques are not available for this component.
+        """
+        state = self._get_state()
+        if not hasattr(state, "torque"):
+            raise ValueError("Joint torques are not available for this component.")
+        joint_torque = np.array(state.torque, dtype=np.float32)
+        return self._extract_joint_info(joint_torque, joint_id=joint_id)
+
     def get_joint_current_dict(
         self, joint_id: list[int] | int | None = None
     ) -> dict[str, float]:
@@ -421,9 +441,9 @@ class RobotJointComponent(RobotComponent):
             ValueError: If joint error codes are not available for this component.
         """
         state = self._get_state()
-        if not hasattr(state, "joint_err"):
+        if not hasattr(state, "error"):
             raise ValueError("Joint error codes are not available for this component.")
-        joint_err = np.array(state.joint_err, dtype=np.uint32)
+        joint_err = np.array(state.error, dtype=np.uint32)
         return self._extract_joint_info(joint_err, joint_id=joint_id)
 
     def get_joint_err_dict(
@@ -457,22 +477,27 @@ class RobotJointComponent(RobotComponent):
             ValueError: If joint positions or velocities are not available.
         """
         state = self._get_state()
-        if not hasattr(state, "joint_pos") or not hasattr(state, "joint_vel"):
+        if not hasattr(state, "pos") or not hasattr(state, "vel"):
             raise ValueError(
                 "Joint positions or velocities are not available for this component."
             )
 
         # Create initial state array with positions and velocities
-        joint_pos = np.array(state.joint_pos, dtype=np.float32)
-        joint_vel = np.array(state.joint_vel, dtype=np.float32)
+        joint_pos = np.array(state.pos, dtype=np.float32)
+        joint_vel = np.array(state.vel, dtype=np.float32)
 
-        if hasattr(state, "joint_cur"):
+        if hasattr(state, "cur"):
             # If currents are available, include them
-            joint_cur = np.array(state.joint_cur, dtype=np.float32)
+            joint_cur = np.array(state.cur, dtype=np.float32)
             joint_state = np.stack([joint_pos, joint_vel, joint_cur], axis=1)
+        elif hasattr(state, "torque"):
+            # If torques are available, include them
+            joint_torque = np.array(state.torque, dtype=np.float32)
+            joint_state = np.stack([joint_pos, joint_vel, joint_torque], axis=1)
         else:
-            # Otherwise just include positions and velocities
-            joint_state = np.stack([joint_pos, joint_vel], axis=1)
+            raise ValueError(
+                f"Either current or torque should be available for this {self.__class__.__name__}."
+            )
 
         return self._extract_joint_info(joint_state, joint_id=joint_id)
 
@@ -543,8 +568,9 @@ class RobotJointComponent(RobotComponent):
         joint_cmd = self._convert_joint_cmd_to_array(joint_cmd)
         return self.get_joint_pos() + joint_cmd
 
+    @staticmethod
     def _extract_joint_info(
-        self, joint_info: np.ndarray, joint_id: list[int] | int | None = None
+        joint_info: np.ndarray, joint_id: list[int] | int | None = None
     ) -> np.ndarray:
         """Extract the joint information of the component as a numpy array.