Fixed #28: Joints in joints_set_point, joints_current, current_joints…

cri_lib/cri_controller.py

@@ -121,9 +121,13 @@ def connect(
             # Start sending ALIVEJOG message
             self.jog_thread.start()
 
+            # Send hello message, this lets the robot control know who we are and what our time is (for logging / troubleshooting)
             hello_msg = f'INFO Hello "{application_name}" {application_version} {datetime.now(timezone.utc).strftime(format="%Y-%m-%dT%H:%M:%S")}'
-
             self._send_command(hello_msg)
+
+            # Request the axis count, this is needed for interpreting some messages
+            self._send_command("CONFIG GetAxes")
+
             logger.debug("Connected to %s:%d", host, port)
             return True
 

cri_lib/cri_protocol_parser.py

@@ -32,6 +32,7 @@ def __init__(self, robot_state: RobotState, robot_state_lock: Lock):
         self.robot_state_lock = robot_state_lock
         self.file_list: list = []
         self.file_list_lock: Lock = threading.Lock()
+        self.robot_joint_count = 0
 
     def parse_message(
         self, message: str
@@ -122,6 +123,11 @@ def _parse_status(self, parameters: list[str]) -> None:
         """
         segment_start_idx = 0
 
+        r_cnt = self.robot_state.robot_axes_count
+        e_cnt = self.robot_state.external_axes_count
+        t_cnt = self.robot_state.tool_axes_count
+        p_cnt = self.robot_state.platform_axes_count
+
         while segment_start_idx < len(parameters):
             match parameters[segment_start_idx]:
                 case "MODE":
@@ -132,18 +138,48 @@ def _parse_status(self, parameters: list[str]) -> None:
                     segment_start_idx += 2
 
                 case "POSJOINTSETPOINT":
-                    joints = []
+                    joints = [0.0] * 16  # array of 16 elements
                     for i in range(16):
-                        joints.append(float(parameters[segment_start_idx + 1 + i]))
+                        # external axes follow immediately after the last robot axis
+                        # therefore we need to reorder the entries
+                        if i < self.robot_state.robot_axes_count:  # robot axes
+                            joints[i] = float(parameters[segment_start_idx + 1 + i])
+                        elif i < r_cnt + e_cnt:  # external axes
+                            joints[6 + i - r_cnt] = float(
+                                parameters[segment_start_idx + 1 + i]
+                            )
+                        elif i < r_cnt + e_cnt + t_cnt:  # tool axes
+                            joints[9 + i - r_cnt - e_cnt] = float(
+                                parameters[segment_start_idx + 1 + i]
+                            )
+                        elif i < r_cnt + e_cnt + t_cnt + p_cnt:  # platform axes
+                            joints[12 + i - r_cnt - e_cnt - t_cnt] = float(
+                                parameters[segment_start_idx + 1 + i]
+                            )
 
                     with self.robot_state_lock:
                         self.robot_state.joints_set_point = JointsState(*joints)
                     segment_start_idx += 17
 
                 case "POSJOINTCURRENT":
-                    joints = []
+                    joints = [0.0] * 16  # array of 16 elements
                     for i in range(16):
-                        joints.append(float(parameters[segment_start_idx + 1 + i]))
+                        # external axes follow immediately after the last robot axis
+                        # therefore we need to reorder the entries
+                        if i < self.robot_state.robot_axes_count:  # robot axes
+                            joints[i] = float(parameters[segment_start_idx + 1 + i])
+                        elif i < r_cnt + e_cnt:  # external axes
+                            joints[6 + i - r_cnt] = float(
+                                parameters[segment_start_idx + 1 + i]
+                            )
+                        elif i < r_cnt + e_cnt + t_cnt:  # tool axes
+                            joints[9 + i - r_cnt - e_cnt] = float(
+                                parameters[segment_start_idx + 1 + i]
+                            )
+                        elif i < r_cnt + e_cnt + t_cnt + p_cnt:  # platform axes
+                            joints[12 + i - r_cnt - e_cnt - t_cnt] = float(
+                                parameters[segment_start_idx + 1 + i]
+                            )
 
                     with self.robot_state_lock:
                         self.robot_state.joints_current = JointsState(*joints)
@@ -212,29 +248,55 @@ def _parse_status(self, parameters: list[str]) -> None:
                     segment_start_idx += 2
 
                 case "CURRENTJOINTS":
-                    currents = []
+                    currents = [0.0] * 16  # array of 16 elements
                     for i in range(16):
-                        currents.append(
-                            float(int(parameters[segment_start_idx + 1 + i])) / 1000
-                        )
+                        # external axes follow immediately after the last robot axis
+                        # therefore we need to reorder the entries
+                        if i < self.robot_state.robot_axes_count:  # robot axes
+                            currents[i] = (
+                                float(parameters[segment_start_idx + 1 + i]) / 1000
+                            )
+                        elif i < r_cnt + e_cnt:  # external axes
+                            currents[6 + i - r_cnt] = (
+                                float(parameters[segment_start_idx + 1 + i]) / 1000
+                            )
+                        elif i < r_cnt + e_cnt + t_cnt:  # tool axes
+                            currents[9 + i - r_cnt - e_cnt] = (
+                                float(parameters[segment_start_idx + 1 + i]) / 1000
+                            )
+                        elif i < r_cnt + e_cnt + t_cnt + p_cnt:  # platform axes
+                            currents[12 + i - r_cnt - e_cnt - t_cnt] = (
+                                float(parameters[segment_start_idx + 1 + i]) / 1000
+                            )
 
                     with self.robot_state_lock:
                         self.robot_state.current_joints = currents
                     segment_start_idx += 17
 
                 case "ERROR":
-                    errors = []
+                    errors = [ErrorStates()] * 16
                     self.robot_state.combined_axes_error = parameters[
                         segment_start_idx + 1
                     ]
-                    for axis_idx in range(16):
-                        value_int = int(
-                            parameters[segment_start_idx + 2 + axis_idx], base=10
-                        )
+                    for i in range(16):
+                        value_int = int(parameters[segment_start_idx + 2 + i], base=10)
                         error_bits = []
-                        for i in range(8):
-                            error_bits.append(value_int & (1 << i) != 0)
-                        errors.append(ErrorStates(*error_bits))
+                        for j in range(8):
+                            error_bits.append(value_int & (1 << j) != 0)
+
+                        # external axes follow immediately after the last robot axis
+                        # therefore we need to reorder the entries
+                        if i < self.robot_state.robot_axes_count:  # robot axes
+                            errors[i] = ErrorStates(*error_bits)
+                        elif i < r_cnt + e_cnt:  # external axes
+                            errors[6 + i - r_cnt] = ErrorStates(*error_bits)
+                        elif i < r_cnt + e_cnt + t_cnt:  # tool axes
+                            errors[9 + i - r_cnt - e_cnt] = ErrorStates(*error_bits)
+                        elif i < r_cnt + e_cnt + t_cnt + p_cnt:  # platform axes
+                            errors[12 + i - r_cnt - e_cnt - t_cnt] = ErrorStates(
+                                *error_bits
+                            )
+
                     with self.robot_state_lock:
                         self.robot_state.error_states = errors
                     segment_start_idx += 18
@@ -532,6 +594,42 @@ def _parse_config(self, parameters: Sequence[str]) -> None:
             with self.robot_state_lock:
                 self.robot_state.project_file = parameters[1]
 
+        if parameters[0] == "Axes":
+            with self.robot_state_lock:
+                self.robot_state.robot_axes_count = 0
+                self.robot_state.external_axes_count = 0
+                self.robot_state.tool_axes_count = 0
+                self.robot_state.platform_axes_count = 0
+
+                # Count axes of each type
+                # Each axis description follows this format: A1 canid posmin posmax velmax
+                # Where A is the axis type and 1 is the index within that type
+                for param in parameters[1:]:
+                    if (
+                        param.startswith("A")
+                        and len(param) == 2
+                        and param[1].isnumeric()
+                    ):
+                        self.robot_state.robot_axes_count += 1
+                    if (
+                        param.startswith("E")
+                        and len(param) == 2
+                        and param[1].isnumeric()
+                    ):
+                        self.robot_state.external_axes_count += 1
+                    if (
+                        param.startswith("T")
+                        and len(param) == 2
+                        and param[1].isnumeric()
+                    ):
+                        self.robot_state.tool_axes_count += 1
+                    if (
+                        param.startswith("P")
+                        and len(param) == 2
+                        and param[1].isnumeric()
+                    ):
+                        self.robot_state.platform_axes_count += 1
+
     def _parse_cmderror(self, parameters: Sequence[str]) -> dict[str, str]:
         """Parses a CMDERROR message to notify calling function
 

cri_lib/robot_state.py

@@ -197,11 +197,23 @@ class RobotState:
     mode: RobotMode = RobotMode.JOINT
     """Robot execution mode"""
 
+    robot_axes_count: int = 0
+    """Number of robot axes"""
+    external_axes_count: int = 0
+    """Number of external axes"""
+    tool_axes_count: int = 0
+    """Number of tool axes"""
+    platform_axes_count: int = 0
+    """Number of mobile platform axes"""
+
     joints_set_point: JointsState = field(default_factory=JointsState)
-    """Set point of robot joints"""
+    """Target positions of the axes. This position is calculated by the kinematics."""
 
     joints_current: JointsState = field(default_factory=JointsState)
-    """Actual values of robot joints"""
+    """
+    Actual hardware positions of the axes. Generally use joints_set_point as
+    base values for moving the robot to avoid creating a control loop!
+    """
 
     position_robot: RobotCartesianPosition = field(
         default_factory=RobotCartesianPosition

examples/README.md

@@ -0,0 +1,7 @@
+# Examples
+This directory contains examples on how to use the CRI Python API.
+
+To run an example copy it to the parent directory, then call it as follows:
+```sh
+python3 ./NameOfExampleFile.py
+```

examples/get_joint_position.py

@@ -0,0 +1,37 @@
+import logging
+from time import sleep
+
+from cri_lib import CRIController
+
+# 🔹 Configure logging
+logging.basicConfig(
+    level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
+)
+logger = logging.getLogger(__name__)
+
+# CRIController is the main interface for controlling the iRC
+controller = CRIController()
+
+# Connect to default iRC IP
+# controller.connect("192.168.3.11")
+if not controller.connect("127.0.0.1", 3921):
+    logger.error("Unable to connect to iRC! Ensure the simulator is running.")
+    quit()
+
+# Acquire active control.
+# not necessary for this example
+# controller.set_active_control(True)
+
+# 10 times in 500ms intervals
+for i in range(0, 10):
+    sleep(0.5)
+    j = controller.robot_state.joints_set_point
+    logger.info(
+        f"Axis positions: A1={j.A1} A2={j.A2} A3={j.A3} A4={j.A4} A5={j.A5} A6={j.A6} E1={j.E1} E2={j.E2} E3={j.E3} T1={j.G1} T2={j.G2} T3={j.G3} P1={j.P1} P2={j.P2} P3={j.P3} P4={j.P4}"
+    )
+
+# Disable motors and disconnect
+logger.info("Disconnecting...")
+controller.close()
+
+logger.info("Script execution completed successfully.")