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

dexcontrol/core/chassis.py

@@ -17,15 +17,90 @@ Zenoh communication. It handles steering and wheel velocity control.
 import time
 
 import numpy as np
-import zenoh
+from dexcomm.serialization.protobuf import control_msg_pb2
+from dexcomm.utils import RateLimiter
+from jaxtyping import Float
 
 from dexcontrol.config.core import ChassisConfig
 from dexcontrol.core.component import RobotJointComponent
-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,
+    ) -> None:
+        """Initialize the hand controller.
+
+        Args:
+            configs: Hand configuration parameters containing communication topics
+                and predefined hand positions.
+        """
+        super().__init__(
+            state_sub_topic=configs.steer_state_sub_topic,
+            control_pub_topic=configs.steer_control_pub_topic,
+            state_message_type=control_msg_pb2.MotorStateWithCurrent,
+            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 = control_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,
+    ) -> None:
+        """Initialize the base controller.
+
+        Args:
+            configs: Base configuration parameters containing communication topics.
+        """
+        super().__init__(
+            state_sub_topic=configs.drive_state_sub_topic,
+            control_pub_topic=configs.drive_control_pub_topic,
+            state_message_type=control_msg_pb2.MotorStateWithCurrent,
+            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 = control_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
@@ -44,21 +119,15 @@ class Chassis(RobotJointComponent):
     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.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)
+        self.chassis_drive = ChassisDrive(configs)
+
         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 +165,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 +176,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 +186,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 +434,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 +460,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 +568,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 +594,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 +650,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

@@ -8,24 +8,25 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-"""Base module for robot components with Zenoh communication.
+"""Base module for robot components using DexComm communication.
 
-This module provides base classes for robot components that communicate via Zenoh.
-It includes RobotComponent for state-only components and RobotJointComponent for
-components that also support control commands.
+This module provides base classes for robot components that use DexComm's
+Raw API for communication. It includes RobotComponent for state-only components
+and RobotJointComponent for components that also support control commands.
 """
 
 import time
 from typing import Any, Final, Mapping, TypeVar
 
 import numpy as np
-import zenoh
+from dexcomm import Publisher, Subscriber
+from dexcomm.serialization import deserialize_protobuf, serialize_protobuf
 from google.protobuf.message import Message
 from jaxtyping import Float
 from loguru import logger
 
+from dexcontrol.utils.comm_helper import get_zenoh_config_path
 from dexcontrol.utils.os_utils import resolve_key_name
-from dexcontrol.utils.subscribers import ProtobufZenohSubscriber
 
 # Type variable for Message subclasses
 M = TypeVar("M", bound=Message)
@@ -47,41 +48,55 @@ class RobotComponent:
     def __init__(
         self,
         state_sub_topic: str,
-        zenoh_session: zenoh.Session,
         state_message_type: type[M],
     ) -> None:
         """Initializes RobotComponent.
 
         Args:
             state_sub_topic: Topic to subscribe to for state updates.
-            zenoh_session: Active Zenoh session for communication.
             state_message_type: Protobuf message class for component state.
         """
         self._state_message_type = state_message_type
-        self._zenoh_session: Final[zenoh.Session] = zenoh_session
-        self._init_subscriber(state_sub_topic, state_message_type, zenoh_session)
+        self._latest_state: M | None = None
+        self._is_active = False
+        self._init_subscriber(state_sub_topic, state_message_type)
 
     def _init_subscriber(
         self,
         state_sub_topic: str,
         state_message_type: type[M],
-        zenoh_session: zenoh.Session,
     ) -> None:
-        """Initialize the Zenoh subscriber for state updates.
+        """Initialize the subscriber for state updates using DexComm.
 
         Args:
             state_sub_topic: Topic to subscribe to for state updates.
             state_message_type: Protobuf message class for component state.
-            zenoh_session: Active Zenoh session for communication.
-        """
-        self._subscriber = ProtobufZenohSubscriber(
-            topic=state_sub_topic,
-            zenoh_session=zenoh_session,
-            message_type=state_message_type,
-            name=f"{self.__class__.__name__}",
-            enable_fps_tracking=False,
+        """
+        # Resolve topic with robot namespace
+        full_topic = resolve_key_name(state_sub_topic)
+
+        # Create DexComm subscriber with protobuf deserialization
+        self._subscriber = Subscriber(
+            topic=full_topic,
+            callback=self._on_state_update,
+            deserializer=lambda data: deserialize_protobuf(data, state_message_type),
+            config=get_zenoh_config_path(),
+        )
+
+        logger.debug(
+            f"Created subscriber for {self.__class__.__name__} on {full_topic}"
         )
 
+    def _on_state_update(self, state: M) -> None:
+        """Handle incoming state updates.
+
+        Args:
+            state: Deserialized protobuf state message.
+        """
+        self._latest_state = state
+        if state:
+            self._is_active = True
+
     def _get_state(self) -> M:
         """Gets the current state of the component.
 
@@ -91,10 +106,9 @@ class RobotComponent:
         Raises:
             None: If no state data is available.
         """
-        state = self._subscriber.get_latest_data()
-        if state is None:
+        if self._latest_state is None:
             logger.error(f"No state data available for {self.__class__.__name__}")
-        return state
+        return self._latest_state
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
         """Waits for the component to start receiving state updates.
@@ -105,7 +119,12 @@ class RobotComponent:
         Returns:
             True if component becomes active, False if timeout is reached.
         """
-        return self._subscriber.wait_for_active(timeout)
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            if self._is_active:
+                return True
+            time.sleep(0.1)
+        return False
 
     def is_active(self) -> bool:
         """Check if component is receiving state updates.
@@ -113,7 +132,7 @@ class RobotComponent:
         Returns:
             True if component is active, False otherwise.
         """
-        return self._subscriber.is_active()
+        return self._is_active
 
     def shutdown(self) -> None:
         """Cleans up Zenoh resources."""
@@ -149,7 +168,7 @@ class RobotJointComponent(RobotComponent):
     Extends RobotComponent to add APIs for interacting with joints.
 
     Attributes:
-        _publisher: Zenoh publisher for control commands.
+        _publisher: Publisher for control commands (Zenoh or dexcomm).
         _joint_name: List of joint names for this component.
         _pose_pool: Dictionary of predefined poses for this component.
     """
@@ -178,7 +197,6 @@ class RobotJointComponent(RobotComponent):
         state_sub_topic: str,
         control_pub_topic: str,
         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,
@@ -190,16 +208,24 @@ class RobotJointComponent(RobotComponent):
             state_sub_topic: Topic to subscribe to for state updates.
             control_pub_topic: Topic to publish control commands.
             state_message_type: Protobuf message class for component state.
-            zenoh_session: Active Zenoh session for communication.
             joint_name: List of joint names for this component.
+            joint_limit: Joint position limits.
+            joint_vel_limit: Joint velocity limits.
             pose_pool: Dictionary of predefined poses for this component.
         """
-        super().__init__(state_sub_topic, zenoh_session, state_message_type)
+        super().__init__(state_sub_topic, state_message_type)
 
-        resolved_topic: Final[str] = resolve_key_name(control_pub_topic)
-        self._publisher: Final[zenoh.Publisher] = zenoh_session.declare_publisher(
-            resolved_topic
+        # Resolve topic with robot namespace
+        full_topic = resolve_key_name(control_pub_topic)
+
+        # Create DexComm publisher with protobuf serialization
+        self._publisher: Final[Publisher] = Publisher(
+            topic=full_topic,
+            serializer=serialize_protobuf,
+            config=get_zenoh_config_path(),
         )
+
+        logger.debug(f"Created publisher for {self.__class__.__name__} on {full_topic}")
         self._joint_name: list[str] | None = joint_name
         self._joint_limit: np.ndarray | None = (
             np.array(joint_limit) if joint_limit else None
@@ -218,22 +244,19 @@ class RobotJointComponent(RobotComponent):
         Args:
             control_msg: Protobuf control message to publish.
         """
-        msg_bytes = control_msg.SerializeToString()
-        self._publisher.put(msg_bytes)
+        # DexComm publisher with protobuf serializer handles this
+        self._publisher.publish(control_msg)
 
     def shutdown(self) -> None:
-        """Cleans up all Zenoh resources."""
+        """Cleans up all communication resources."""
         super().shutdown()
         try:
             if hasattr(self, "_publisher") and self._publisher:
-                self._publisher.undeclare()
+                self._publisher.shutdown()
         except Exception as e:
-            # Don't log "Undeclared publisher" errors as warnings - they're expected during shutdown
-            error_msg = str(e).lower()
-            if not ("undeclared" in error_msg or "closed" in error_msg):
-                logger.warning(
-                    f"Error undeclaring publisher for {self.__class__.__name__}: {e}"
-                )
+            logger.warning(
+                f"Error shutting down publisher for {self.__class__.__name__}: {e}"
+            )
 
     @property
     def joint_name(self) -> list[str]:
@@ -311,9 +334,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 +372,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 +409,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 +464,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 +500,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 +591,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.