diff --git a/cri_lib/cri_controller.py b/cri_lib/cri_controller.py index 7a76a1f..d5f70fa 100644 --- a/cri_lib/cri_controller.py +++ b/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 diff --git a/cri_lib/cri_protocol_parser.py b/cri_lib/cri_protocol_parser.py index c2e94e9..23f025f 100644 --- a/cri_lib/cri_protocol_parser.py +++ b/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 diff --git a/cri_lib/robot_state.py b/cri_lib/robot_state.py index 6d9052b..53d025e 100644 --- a/cri_lib/robot_state.py +++ b/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 diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 0000000..fdb115e --- /dev/null +++ b/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 +``` diff --git a/examples/get_joint_position.py b/examples/get_joint_position.py new file mode 100644 index 0000000..709eff5 --- /dev/null +++ b/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.") diff --git a/tests/test_cri_parser.py b/tests/test_cri_parser.py index 58f9b7c..98962be 100644 --- a/tests/test_cri_parser.py +++ b/tests/test_cri_parser.py @@ -34,6 +34,8 @@ def robot_state_equal(actual: RobotState, expected: RobotState) -> bool: def test_parse_state(): + """This tests parsing the STATUS message with the assumption that all possible axes are present""" + test_message = """ CRISTART 1234 STATUS MODE joint POSJOINTSETPOINT 1.00 2.00 3.00 4.00 5.00 6.00 7.00 8.00 9.00 10.00 11.00 12.00 13.00 14.00 15.00 16.00 @@ -55,6 +57,10 @@ def test_parse_state(): """ robot_state_correct = RobotState() + robot_state_correct.robot_axes_count = 6 + robot_state_correct.external_axes_count = 3 + robot_state_correct.tool_axes_count = 3 + robot_state_correct.platform_axes_count = 4 robot_state_correct.category_time_ns["STATUS"] = 0 robot_state_correct.mode = RobotMode.JOINT robot_state_correct.joints_set_point = JointsState( @@ -145,11 +151,234 @@ def test_parse_state(): robot_state_correct.combined_axes_error = "no_error" controller = CRIController() + controller.parser.robot_state.robot_axes_count = 6 + controller.parser.robot_state.external_axes_count = 3 + controller.parser.robot_state.tool_axes_count = 3 + controller.parser.robot_state.platform_axes_count = 4 controller._parse_message(test_message) assert robot_state_equal(controller.robot_state, robot_state_correct) +def test_parse_state2(): + """This tests parsing the STATUS message with missing axes""" + + test_message = """ +CRISTART 1234 STATUS MODE joint +POSJOINTSETPOINT 1.00 2.00 3.00 4.00 5.00 6.00 7.00 8.00 9.00 10.00 11.00 12.00 13.00 14.00 15.00 16.00 +POSJOINTCURRENT 1.00 2.00 3.00 4.00 5.00 6.00 7.00 8.00 9.00 10.00 11.00 12.00 13.00 14.00 15.00 16.00 +POSCARTROBOT 10.0 20.0 30.0 0.00 90.00 0.00 +POSCARTPLATFORM 10.0 20.0 180.00 +OVERRIDE 80.0 +DIN 0000000000000FF00 DOUT 0000000000000FF00 +ESTOP 3 SUPPLY 23000 CURRENTALL 2600 +CURRENTJOINTS 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 +ERROR no_error 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 +KINSTATE 30 +OPMODE -1 +CARTSPEED 123.4 +GSIG 00ff00ff00ff +FRAMEROBOT MyFrame 1.0 2.0 3.0 4.0 5.0 6.0 +UNKNOWNSEGMENT 1 2 3 4 +CRIEND + """ + + robot_state_correct = RobotState() + robot_state_correct.robot_axes_count = 3 + robot_state_correct.external_axes_count = 2 + robot_state_correct.tool_axes_count = 1 + robot_state_correct.platform_axes_count = 2 + robot_state_correct.category_time_ns["STATUS"] = 0 + robot_state_correct.mode = RobotMode.JOINT + robot_state_correct.joints_set_point = JointsState( + 1.00, + 2.00, + 3.00, + 0.00, + 0.00, + 0.00, + 4.00, + 5.00, + 0.00, + 6.00, + 0.00, + 0.00, + 7.00, + 8.00, + 0.00, + 0.00, + ) + robot_state_correct.joints_current = JointsState( + 1.00, + 2.00, + 3.00, + 0.00, + 0.00, + 0.00, + 4.00, + 5.00, + 0.00, + 6.00, + 0.00, + 0.00, + 7.00, + 8.00, + 0.00, + 0.00, + ) + es0 = ErrorStates() + es255 = ErrorStates(True, True, True, True, True, True, True, True) + robot_state_correct.position_robot = RobotCartesianPosition( + 10.0, 20.0, 30.0, 0.0, 90.0, 0.00 + ) + robot_state_correct.position_platform = PlatformCartesianPosition(10.0, 20.0, 180.0) + robot_state_correct.override = 80.0 + robot_state_correct.din = [False] * 64 + robot_state_correct.din[8:16] = [True] * 8 + robot_state_correct.dout = [False] * 64 + robot_state_correct.dout[8:16] = [True] * 8 + robot_state_correct.emergency_stop_ok = True + robot_state_correct.main_relay = True + robot_state_correct.supply_voltage = 23.0 + robot_state_correct.current_total = 2.6 + robot_state_correct.current_joints = [i * 10 for i in range(1, 17)] + # robot_state_correct.error_states = [ErrorStates(*([True] * 8))] * 16 + robot_state_correct.error_states = [ + es255, + es255, + es255, + es0, + es0, + es0, + es255, + es255, + es0, + es255, + es0, + es0, + es255, + es255, + es0, + es0, + ] + robot_state_correct.kinematics_state = KinematicsState(30) + robot_state_correct.operation_mode = OperationMode(-1) + robot_state_correct.cart_speed_mm_per_s = 123.4 + robot_state_correct.global_signals = ( + [True] * 8 + + [False] * 8 + + [True] * 8 + + [False] * 8 + + [True] * 8 + + [False] * 8 + + [False] * 80 + ) + robot_state_correct.frame_name = "MyFrame" + robot_state_correct.frame_position_current = RobotCartesianPosition( + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 + ) + robot_state_correct.current_joints = [ + 0.01, + 0.02, + 0.03, + 0.00, + 0.00, + 0.0, + 0.04, + 0.05, + 0.00, + 0.06, + 0.00, + 0.00, + 0.07, + 0.08, + 0.00, + 0.00, + ] + robot_state_correct.combined_axes_error = "no_error" + + controller = CRIController() + # controller.parser.robot_state.robot_axes_count = 3 + # controller.parser.robot_state.external_axes_count = 2 + # controller.parser.robot_state.tool_axes_count = 1 + # controller.parser.robot_state.platform_axes_count = 2 + # Read axis counts from config message + controller._parse_message( + "CRISTART 1234 CONFIG Axes A1 1 2 3 4 A2 1 2 3 4 A3 1 2 3 4 E1 1 2 3 4 E2 1 2 3 4 T1 1 2 3 4 P1 1 2 3 4 P2 1 2 3 4" + ) + controller._parse_message(test_message) + + # ignore the category time + robot_state_correct.category_time_ns = controller.robot_state.category_time_ns + print(controller.robot_state) + print(robot_state_correct) + assert robot_state_equal(controller.robot_state, robot_state_correct) + + +def test_parse_config_axes1(): + """Tests parsing CONFIG Axis with all possible axes""" + + test_message = "CRISTART 1234 CONFIG Axes A1 1 2 3 4 A2 1 2 3 4 A3 1 2 3 4 A4 1 2 3 4 A5 1 2 3 4 A6 1 2 3 4 E1 1 2 3 4 E2 1 2 3 4 E3 1 2 3 4 T1 1 2 3 4 T2 1 2 3 4 T3 1 2 3 4 P1 1 2 3 4 P2 1 2 3 4 P3 1 2 3 4 P4 1 2 3 4" + + robot_state_correct = RobotState() + robot_state_correct.robot_axes_count = 6 + robot_state_correct.external_axes_count = 3 + robot_state_correct.tool_axes_count = 3 + robot_state_correct.platform_axes_count = 4 + + controller = CRIController() + controller._parse_message(test_message) + + assert ( + controller.robot_state.robot_axes_count == robot_state_correct.robot_axes_count + ) + assert ( + controller.robot_state.external_axes_count + == robot_state_correct.external_axes_count + ) + assert controller.robot_state.tool_axes_count == robot_state_correct.tool_axes_count + assert ( + controller.robot_state.platform_axes_count + == robot_state_correct.platform_axes_count + ) + + # ignore the category time + robot_state_correct.category_time_ns = controller.robot_state.category_time_ns + assert robot_state_equal(controller.robot_state, robot_state_correct) + + +def test_parse_config_axes2(): + """Tests parsing CONFIG Axis with missing axes""" + + test_message = "CRISTART 1234 CONFIG Axes A1 1 2 3 4 A2 1 2 3 4 A3 1 2 3 4 E1 1 2 3 4 E2 1 2 3 4 T1 1 2 3 4 P1 1 2 3 4 P2 1 2 3 4" + + robot_state_correct = RobotState() + robot_state_correct.robot_axes_count = 3 + robot_state_correct.external_axes_count = 2 + robot_state_correct.tool_axes_count = 1 + robot_state_correct.platform_axes_count = 2 + + controller = CRIController() + controller._parse_message(test_message) + + assert ( + controller.robot_state.robot_axes_count == robot_state_correct.robot_axes_count + ) + assert ( + controller.robot_state.external_axes_count + == robot_state_correct.external_axes_count + ) + assert controller.robot_state.tool_axes_count == robot_state_correct.tool_axes_count + assert ( + controller.robot_state.platform_axes_count + == robot_state_correct.platform_axes_count + ) + + # ignore the category time + robot_state_correct.category_time_ns = controller.robot_state.category_time_ns + assert robot_state_equal(controller.robot_state, robot_state_correct) + + def test_parse_runstate_main(): test_message = ( "CRISTART 1234 RUNSTATE MAIN testmotion.xml pickpart.xml 12 3 0 2 CRIEND"