fix: ur.robot_state (#10)

- replace broken command with RTDE integration (robot status and safety checks)

revert logger send.

Author: MGross21

armctl/universal_robots/protocols/config.xml

@@ -3,5 +3,7 @@
     <recipe key="out">
         <field name="actual_TCP_pose" type="VECTOR6D" />
         <field name="actual_q" type="VECTOR6D" />
+        <field name="robot_status_bits" type="UINT32" />
+        <field name="safety_status_bits" type="UINT32" />
     </recipe>
 </rtde_config>

armctl/universal_robots/protocols/rtde.py

@@ -1,7 +1,10 @@
 from pathlib import Path
+from typing import NewType
 import rtde.rtde as rtde
 import rtde.rtde_config as rtde_config
 
+UINT32 = NewType("UINT32", int)
+
 
 class RTDE:
     def __init__(self, ip: str):
@@ -27,3 +30,49 @@ def joint_angles(self) -> list[float]:
     def tcp_pose(self) -> list[float]:
         """Return TCP pose [x, y, z, rx, ry, rz]."""
         return list(self._get_data().actual_TCP_pose)
+
+    def robot_status(self) -> dict[str, bool]:
+        """Return robot status.
+
+        Robot status bits (UINT32):
+        - **`Bit 0`**: Is power on
+        - **`Bit 1`**: Is program running
+        - **`Bit 2`**: Is teach button pressed
+        - **`Bit 3`**: Is power button pressed
+
+        Safety status bits (UINT32):
+        - **`Bit 0`**: Is normal mode
+        - **`Bit 1`**: Is reduced mode
+        - **`Bit 2`**: Is protective stop
+        - **`Bit 3`**: Is recovery mode
+        - **`Bit 4`**: Is safeguard stopped
+        - **`Bit 5`**: Is system emergency stopped
+        - **`Bit 6`**: Is robot emergency stopped
+        - **`Bit 7`**: Is Emergency Stopped
+        - **`Bit 8`**: Is violation
+        - **`Bit 9`**: Is Fault
+        - **`Bit 10`**: Is stopped due to safety
+        """
+        data = self._get_data()
+        rsb: UINT32 = data.robot_status_bits
+        ssb: UINT32 = data.safety_status_bits
+
+        return {
+            # Robot status bits
+            "Power On": bool(rsb & 1),
+            "Program Running": bool(rsb & 2),
+            "Teach Button": bool(rsb & 4),
+            "Power Button": bool(rsb & 8),
+            # Safety status bits
+            "Normal Mode": bool(ssb & 1),
+            "Reduced Mode": bool(ssb & 2),
+            "Protective Stop": bool(ssb & 4),
+            "Recovery Mode": bool(ssb & 8),
+            "Safeguard Stopped": bool(ssb & 16),
+            "System Emergency Stopped": bool(ssb & 32),
+            "Robot Emergency Stopped": bool(ssb & 64),
+            "Emergency Stopped": bool(ssb & 128),
+            "Violation": bool(ssb & 256),
+            "Fault": bool(ssb & 512),
+            "Stopped Due to Safety": bool(ssb & 1024),
+        }

armctl/universal_robots/universal_robots.py

@@ -184,5 +184,13 @@ def stop_motion(self) -> None:
             "stopj({})\n".format(deceleration), suppress_output=True
         )
 
-    def get_robot_state(self):
-        return self.send_command("get_robot_status()\n")
+    def get_robot_state(self) -> dict[str, bool]:
+        status = self.rtde.robot_status()
+
+        key_out = ["Power On", "Program Running", "Emergency Stopped", "Stopped Due to Safety"]
+        logger.receive(
+            "Received response: " +
+            ", ".join(f"{k}: {status[k]}" for k in key_out) +
+            " ..."
+        )
+        return status