Add xs_robot get joint state methods

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py

@@ -762,6 +762,54 @@ def capture_joint_positions(self) -> None:
             )
         self._update_Tsb()
 
+    def get_joint_positions(self) -> List[float]:
+        """
+        Get the joint positions.
+
+        :return: list of joint positions [rad]
+        """
+        self.core.get_node().logdebug('Getting joint states')
+        with self.core.js_mutex:
+            return [
+                self.core.joint_states.position[self.core.js_index_map[name]]
+                for name in self.group_info.joint_names
+            ]
+
+    def get_joint_velocities(self) -> List[float]:
+        """
+        Get the joint velocities.
+
+        :return: list of joint velocities [rad/s]
+        """
+        self.core.get_node().logdebug('Getting joint velocities')
+        with self.core.js_mutex:
+            return [
+                self.core.joint_states.velocity[self.core.js_index_map[name]]
+                for name in self.group_info.joint_names
+            ]
+
+    def get_joint_efforts(self) -> List[float]:
+        """
+        Get the joint efforts.
+
+        :return: list of joint efforts [Nm or load % depending on the motor joint]
+        """
+        self.core.get_node().logdebug('Getting joint efforts')
+        with self.core.js_mutex:
+            return [
+                self.core.joint_states.effort[self.core.js_index_map[name]]
+                for name in self.group_info.joint_names
+            ]
+
+    def get_number_of_joints(self) -> int:
+        """
+        Get the number of joints in the arm group.
+
+        :return: number of joints
+        """
+        self.core.get_node().logdebug('Getting number of joints')
+        return self.group_info.num_joints
+
     def _update_Tsb(self) -> None:
         """Update transform between the space and body frame from the current joint commands."""
         self.core.get_node().logdebug('Updating T_sb')

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/gripper.py

@@ -264,3 +264,43 @@ def grasp(self, delay: float = 1.0) -> None:
         :param delay: (optional) number of seconds to delay before returning control to the user
         """
         self.gripper_controller(-self.gripper_value, delay)
+
+    def get_gripper_position(self) -> float:
+        """
+        Get the gripper joint position.
+
+        :return: position of the gripper joint [rad]
+        """
+        self.core.get_node().logdebug('Getting gripper joint position')
+        with self.core.js_mutex:
+            return self.core.joint_states.position[self.core.js_index_map[self.gripper_name]]
+
+    def get_gripper_velocity(self) -> float:
+        """
+        Get the gripper joint velocity.
+
+        :return: velocity of the gripper joint [rad/s]
+        """
+        self.core.get_node().logdebug('Getting gripper joint velocity')
+        with self.core.js_mutex:
+            return self.core.joint_states.velocity[self.core.js_index_map[self.gripper_name]]
+
+    def get_gripper_effort(self) -> float:
+        """
+        Get the gripper joint effort.
+
+        :return: effort of the gripper joint [Nm or load % depending on the motor joint]
+        """
+        self.core.get_node().logdebug('Getting gripper joint effort')
+        with self.core.js_mutex:
+            return self.core.joint_states.effort[self.core.js_index_map[self.gripper_name]]
+
+    def get_finger_position(self) -> float:
+        """
+        Get the gripper's left finger position.
+
+        :return: position of the gripper's left finger joint [m]
+        """
+        self.core.get_node().logdebug('Getting gripper finger joint position')
+        with self.core.js_mutex:
+            return self.core.joint_states.position[self.left_finger_index]