diff --git a/.env.example b/.env.example index 52e2aef..3fa94ad 100644 --- a/.env.example +++ b/.env.example @@ -1,9 +1,9 @@ # Copy this file to '.env' and set the values. -# One of 'aalto', 'tubingen', 'usp_coil', 'usp_neurosoft', 'default' +# One of 'aalto', 'tubingen', 'usp_coil', 'usp_neurosoft', 'mpi', 'default' SITE= -# One of 'elfin', 'elfin_new_api', 'dobot', 'ur', 'test' +# One of 'elfin', 'elfin_new_api', 'dobot', 'ur', 'omron', 'test' ROBOT= # Either 'true' or 'false' diff --git a/README.md b/README.md index e398714..ce1076c 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,28 @@ Robotized transcranial magnetic stimulation controlled by InVesalius Navigator ( - Edit `.env` and set the robot type, site, and other configuration parameters. +## Omron TM5X-700 + +The Omron driver uses two interfaces on the TMFlow controller: + +- **Modbus TCP** (port 502) for the pose, TCP speed, project state and emergency state. +- **TM script socket** (port 5890) for linear, circular and dynamic motion commands. + +On the robot, enable the Modbus slave and a listen node on port 5890. Then set: + +```bash +SITE=mpi +ROBOT=omron +``` + +`SITE=mpi` applies the 180 degree rz offset used by the MPI CBS cell. For another +cell, add a matching entry to `SITE_CONFIG` in `robot/constants.py`. + +The robot IP is sent by InVesalius when you press **Connect to robot**. Install +the dependencies with `pip install -r requirements.txt`; this includes +`pymodbus`. Force and pressure sensing are provided by the separate sensor +stack, not by the Omron driver. + # Running The first step is to run the relay_server.py script with an argument to set the server port diff --git a/environment.yml b/environment.yml index f04ab09..fb9c05c 100644 --- a/environment.yml +++ b/environment.yml @@ -14,6 +14,7 @@ dependencies: - pynput - pyserial - python-dotenv + - pymodbus>=3.6,<4 - python-socketio==5.11.3 - uvicorn[standard]==0.30.1 - websocket-client diff --git a/requirements.txt b/requirements.txt index 324ac06..bdbc546 100644 --- a/requirements.txt +++ b/requirements.txt @@ -10,3 +10,4 @@ pynput matplotlib==3.9.0 scipy==1.14.0 python-dotenv +pymodbus>=3.6,<4 diff --git a/robot/constants.py b/robot/constants.py index c74ab65..f9279ba 100644 --- a/robot/constants.py +++ b/robot/constants.py @@ -30,6 +30,12 @@ "ry_offset": 0, "rz_offset": 0, }, + # Omron TM5X-700 at MPI CBS: the coil needs a 180 degree rz offset. + "mpi": { + "rx_offset": 0, + "ry_offset": 0, + "rz_offset": 180, + }, "default": { "rx_offset": 0, "ry_offset": 0, @@ -138,6 +144,30 @@ "force_sensor_threshold": 10, "force_sensor_scale_threshold": 30, }, + "omron": { + # General + # The MPI cell uses a 1200 mm working-space limit. + "working_space_radius": 1200, + # The main loop polls several Modbus registers; do not spin flat out. + "sleep": 0.02, + # Head motion + "head_velocity_threshold": 60, + # Tuning motion + "distance_threshold_for_tuning": 30, + "angular_distance_threshold_for_tuning": 5, + # Arc motion + "distance_threshold_for_arc_motion": 80, + "angular_distance_threshold_for_arc_motion": 30, + "arc_bezier_curve_step": None, + "threshold_to_update_arc_motion_target": 40, + "versor_scale_factor": 50, + "middle_arc_scale_factor": 1.5, + # Robot tracker registration + "transformation_matrix_threshold": 1, + # Force and torque sensor + "force_sensor_threshold": 10, + "force_sensor_scale_threshold": 30, + }, } # Publisher messages from invesalius to any robot diff --git a/robot/control/robot_control.py b/robot/control/robot_control.py index 18f8615..8943cfd 100644 --- a/robot/control/robot_control.py +++ b/robot/control/robot_control.py @@ -551,6 +551,13 @@ def connect_to_robot(self, robot_ip): robot = ur.UniversalRobot(robot_ip) success = robot.connect() + elif robot_type == "omron": + # Imported lazily so the other robots do not require pymodbus. + import robot.robots.omron.omron as omron + + robot = omron.Omron(robot_ip) + success = robot.connect() + elif robot_type == "test": # TODO: Add 'test' robot here. pass diff --git a/robot/robots/omron/omron.py b/robot/robots/omron/omron.py new file mode 100644 index 0000000..45a708c --- /dev/null +++ b/robot/robots/omron/omron.py @@ -0,0 +1,436 @@ +"""Omron TM5X-700 robot driver. + +Pose and status are read over Modbus TCP. Motion commands go over the TMFlow +script socket on port 5890. Set ``ROBOT=omron`` and a matching site entry in +``.env`` before connecting. +""" + +import time +from socket import AF_INET, SOCK_STREAM, socket + +from pymodbus.client import ModbusTcpClient + +from robot.robots.robot import Robot + + +class Omron(Robot): + """Interface for the Omron TM5 cobot.""" + + PORT = 5890 + RESPONSE_LENGTH = 1024 + DEFAULT_SCRIPT_ID = 0 + + # Modbus input-register addresses used by the TMFlow project. + modbus_addresses = { + 'start' : 7104, + 'stop' : 7105, + 'get_x' : 7001, + 'get_y' : 7003, + 'get_z' : 7005, + 'get_rx' : 7007, + 'get_ry' : 7009, + 'get_rz' : 7011, + 'get_tcp_speed' : 7871, + 'get_auto_manual_mode' : 7102, + 'project_running' : 7202, + 'emergency_stopped' : 7208 + } + + modbus_fcs = { + 'start' : 4, + 'stop' : 4, + 'get_x' : 4, + 'get_y' : 4, + 'get_z' : 4, + 'get_rx' : 4, + 'get_ry' : 4, + 'get_rz' : 4, + 'get_tcp_speed' : 4, + 'get_auto_manual_mode' : 4, + 'project_running' : 2, + 'emergency_stopped' : 2 + } + + def __init__(self, ip, config=None): + """ + Initialize the robot connection. + + :param ip: The IP address of the robot. + :param config: Unused; kept for the Robot interface. + """ + self.ip = ip + self.connected = False + self.modbus = ModbusTcpClient(ip) + self.current_move_tag = 1 + self.last_target = None + self.free_drive = False + + def connect(self): + """ + Connect to the robot. + + :return: True if successful, otherwise False. + """ + if self.free_drive: + print("Cannot connect, free drive enabled!") + return False + + if self.connected: + print("Already connected to the robot") + return True + + if not self.modbus.connect(): + print("Could not connect to the robot's Modbus server.") + return False + + try: + if self.__read_modbus_int16('get_auto_manual_mode') == 2: + print('Robot is in manual mode. Set robot to auto mode to connect. Waiting...') + # Wait at most 30 s: without a timeout this loop hung forever if + # the robot stayed in manual mode (and loops forever on a None + # reply from a failed Modbus read). + waited = 0.0 + mode = self.__read_modbus_int16('get_auto_manual_mode') + while mode != 1 and waited < 30.0: + time.sleep(0.05) + waited += 0.05 + mode = self.__read_modbus_int16('get_auto_manual_mode') + if mode != 1: + print("Robot did not switch to auto mode within 30 s -- aborting connect.") + return False + + result = self.modbus.write_coil(self.modbus_addresses['stop'], True) + if not result.isError(): + time.sleep(1) + result = self.modbus.write_coil(self.modbus_addresses['start'], True) + if not result.isError(): + self.socket = socket(AF_INET, SOCK_STREAM) + self.socket.setblocking(True) + self.socket.settimeout(None) + self.socket.connect((self.ip, self.PORT)) + self.connected = True + except Exception as e: + print(f"Failed to connect to the robot:\n{e}") + + return self.connected + + def disconnect(self): + """ + Disconnects from the robot. + + :return: True if successful, otherwise False. + """ + success = False + + if not self.connected: + print("Not connected to the robot, therefore cannot disconnect") + return success + + try: + self.socket.close() + time.sleep(0.5) + self.modbus.write_coil(self.modbus_addresses['stop'], True) + self.connected = False + self.modbus.close() + success = True + except: + print("Failed to disconnect from the robot") + + return success + + def is_connected(self): + if self.get_error_state() is not None: + self.connected = False + return self.connected + + def get_pose(self): + """ + Get the current pose of the robot. + + :return: ``(success, pose)``. The pose is a list of 6 values: + [x, y, z, rx, ry, rz], where x, y, z are in mm and rx, ry, rz are + Euler angles in degrees (sxyz convention). + """ + # Read the six pose values from the Modbus input registers. + coordinates = [self.__read_modbus_float32(addrs) for addrs in ['get_x', 'get_y', 'get_z', 'get_rx', 'get_ry', 'get_rz', ]] + if any(c is None for c in coordinates): + # A Modbus read failed -- report a failed update instead of a + # coordinate list containing None (which would crash the caller). + return False, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + return True, coordinates + + def is_moving(self): + """ + Return True if the robot is currently moving. + + :return: True if moving, False if stopped, None if the speed is + unavailable. + """ + speed = self.__read_modbus_float32('get_tcp_speed') + if speed is None: + return None + # The state machine uses this as a plain bool: a tuple would be + # truthy and leave the state machine in MOVING. + return speed > 0.8 + + def get_error_state(self): + project_running = self.__read_modbus_int16('project_running') + emergency_stopped = self.__read_modbus_int16('emergency_stopped') + if project_running is None or emergency_stopped is None: + # Modbus read failed -- report the error state as not available + # instead of misreading None as 0 ("Socket Broken"). + return None + if project_running == 0: + return "Socket Broken" + elif emergency_stopped == 1: + return "Emergency Stopped" + return None + + def is_error_state(self): + """ + Return True if the robot is in an error state, False if it is not in an error state, and None if the + information is not available. + + :return: True if in error state, False if not, and None if the information is not available. + """ + + return self.get_error_state() is not None + + #NOTE: blending not used, set as 0, can be changed later + def move_linear(self, target, speed_ratio): + """ + Move the robot in a linear path to the target in robot's base coordinate system with a given speed. + + If the robot is already moving, this method should stop the current movement and start the new one. + + :param target: The target position in a linear path, as a list of 6 values: [x, y, z, rx, ry, rz]. + [x, y, z] define the position in mm, and [rx, ry, rz] define the Euler angles in degrees, + using the 'sxyz' convention. + :param speed_ratio: The speed of the movement (as a proportion of the maximum speed; 0-1). + """ + print(f"move_linear called with speed_ratio = {speed_ratio}") + command = f'Line("CPP", {target[0]}, {target[1]}, {target[2]}, {target[3]}, {target[4]}, {target[5]}, {int(speed_ratio * 100)}, 0, 100, true)' + success, _ = self.__format_and_send_request(command, 'TMSCT', append_tag=True, verbose = False) + return success + + def move_circular(self, start_position, waypoint, target, speed_ratio): + """ + Move the robot in a circular path through a specified waypoint to the target in robot's base + coordinate system with a given speed. + + If the robot is already moving, this method should stop the current movement and start the new one. + + :param start_position: The starting pose of the robot, as a list of 6 values: [x, y, z, rx, ry, rz]. + The position [x, y, z] is in mm and the Euler angles [rx, ry, rz] are in degrees, using the 'sxyz' convention. + :param waypoint: The intermediate waypoint, as a list of 6 values: [x, y, z, rx, ry, rz]. + The position [x, y, z] is in mm and the Euler angles [rx, ry, rz] are in degrees, using the 'sxyz' convention. + :param target: The target pose, as a list of 6 values: [x, y, z, rx, ry, rz]. + The position [x, y, z] is in mm and the Euler angles [rx, ry, rz] are in degrees, using the 'sxyz' convention. + :param speed_ratio: The speed of the movement (as a proportion of the maximum speed; 0-1). + """ + + command = f'Circle("CPP", {waypoint[0]}, {waypoint[1]}, {waypoint[2]}, {waypoint[3]}, {waypoint[4]}, {waypoint[5]},' + \ + f'{target[0]}, {target[1]}, {target[2]}, {target[3]}, {target[4]}, {target[5]},' + \ + f'{int(speed_ratio * 100)}, 200, 0, {0}, false)' + success, _ = self.__format_and_send_request(command, 'TMSCT', append_tag = True, verbose = False) + return success + + def dynamic_motion(self, target, speed_ratio): + """ + Change the robot movement dynamically/immediately in a linear path to the target in robot's base coordinate system + with a given speed. + + :param target: The target position in a linear path, as a list of 6 values: [x, y, z, rx, ry, rz]. + [x, y, z] define the position in mm, and [rx, ry, rz] define the Euler angles in degrees, + using the 'sxyz' convention. + :param speed_ratio: The speed of the movement (as a proprecvortion of the maximum speed; 0-1). + """ + return self.move_linear(target=target, speed_ratio=speed_ratio) + + def stop_robot(self): + """ + Stop the robot's movement. + + :return: True if the stop command was successful, False otherwise. + """ + command = 'StopAndClearBuffer()' + # NOTE: __format_and_send_request returns a (success, raw_response) + # tuple -- without unpacking, `if success:` at the call site is always + # True and failed stops are reported as successful. + success, _ = self.__format_and_send_request(command, 'TMSCT', verbose=False) + return success + + def enable_free_drive(self): + """ + free drive requires the robot to be in manual mode, so this disconnects, + allowing for the mode to be switched physically via the robot stick/remote + """ + print('Enabling free drive...') + self.disconnect() + self.free_drive = True + print('Free drive enabled - Robot Controller frozen\n(Switch to Manual mode to move the robot)') + + def disable_free_drive(self): + """ reconnects to the robot/project after free drive has been completed """ + print('Exiting free drive...') + self.free_drive = False + self.connect() + print('Free drive disabled - Robot Controller resumed') + + def close(self): + """ + Perform necessary cleanup and disconnection procedures for the robot. + """ + success, _ = self.stop_robot() + self.disconnect() + self.modbus.close() + + return success + + # We do not use this but the base abstract class demands we have it implemented. + def read_force_sensor(self): + pass + + # We do not use this but the base abstract class demands we have it implemented. + def initialize(self): + pass + + def clean_errors(self): + """Omron errors are cleared on the robot teach pendant.""" + return True + + def __read_modbus_float32(self, command): + fc = self.modbus_fcs[command] + address = self.modbus_addresses[command] + val = None + # Read 2 input registers (32-bit float = 2 x 16-bit registers) + if fc == 1: + print("Error: Parsing for function code 1 not implemented") + elif fc == 2: + print("Error: Parsing for function code 2 not implemented") + elif fc == 3: + print("Error: Parsing for function code 3 not implemented") + elif fc == 4: + result = self.modbus.read_input_registers(address, count=2) + if result is None or result.isError(): + # Any Modbus timeout / error returns None-ish replies; using + # result.registers raised AttributeError every loop cycle. + print(f"W: Modbus read failed for '{command}'.") + return None + val = self.modbus.convert_from_registers(result.registers, data_type=self.modbus.DATATYPE.FLOAT32, word_order="big") + else: + result = None + print(f"Function code '{fc}' does not exist!") + + return val + + def __read_modbus_int16(self, command): + fc = self.modbus_fcs[command] + address = self.modbus_addresses[command] + result = None + if fc == 1: + print("Error: Parsing for function code 1 not implemented") + elif fc == 2: + result = self.modbus.read_discrete_inputs(address, count=1) + if result is None or result.isError(): + print(f"W: Modbus read failed for '{command}'.") + return None + return result.bits[0] + elif fc == 3: + print("Error: Parsing for function code 3 not implemented") + elif fc == 4: + result = self.modbus.read_input_registers(address, count=1) + if result is None or result.isError(): + print(f"W: Modbus read failed for '{command}'.") + return None + else: + print(f"Function code '{fc}' does not exist!") + + if result is None: + return None + return self.modbus.convert_from_registers( + result.registers, data_type=self.modbus.DATATYPE.INT16, + word_order="big") + + def __calc_checksum(self, data): + """ calculates the checksum, used as part of the request to the Omron PC used by __format_and_send_request. Explained in the TM robot script protocol documentation """ + # calculated according to xor + checksum = 0 + for char in data: + checksum ^= ord(char) + + return checksum + + def __format_and_send_request(self, command, header, append_tag=False, expect_response=False, response_format_check_func=None, verbose=False): + """Send a command over the TM script socket.""" + success = True + raw_response = None + + if header == "TMSCT": + # The script id is required by the protocol; this driver uses 0. + length = len(f"{self.DEFAULT_SCRIPT_ID},{command}") + # checksum is structured as * followed by the hexadecimal of the checksum excluding "0x". + checksum = self.__calc_checksum(f"{header},{length},{self.DEFAULT_SCRIPT_ID},{command},") + checksum_formatted = "*{:02X}".format(checksum) + full_message = f"${header},{length},{self.DEFAULT_SCRIPT_ID},{command},{checksum_formatted}\r\n" + else: + # The script id is required by the protocol; this driver uses 1. + length = len(f"01,{command}") + # checksum is structured as * followed by the hexadecimal of the checksum excluding "0x". + checksum = self.__calc_checksum(f"{header},{length},01,{command},") + checksum_formatted = "*{:02X}".format(checksum) + full_message = f"${header},{length},01,{command},{checksum_formatted}\r\n" + + if verbose: + print(f"Sending request: '{command}' formatted as '{full_message}'") + + # This makes socket.recv send a BlockingIOError if no message is present in the buffer, instead of freezing the program to wait for a message. + self.socket.setblocking(False) + + # Clearing buffer. This is primarily needed due to the robot sending a "script compilation success" message to our socket for every command. + if verbose: + print("Clearing socket recieve buffer...") + try: + while True: + m = self.socket.recv(self.RESPONSE_LENGTH) + if not m: + break + # To ensure we do not overload CPU + time.sleep(0.001) + except BlockingIOError: + pass + + try: + self.socket.setblocking(True) + self.socket.settimeout(2.0) + + # Send the request to the robot. + if verbose: + print('Sending formatted command...') + + self.socket.sendall(full_message.encode('utf-8')) + if expect_response: + if verbose: + print('Receiving inputs...') + + while not raw_response: + m = self.socket.recv(self.RESPONSE_LENGTH) + packets = [segment for segment in m.split(b'\r\n') if segment] + for p in packets: + if response_format_check_func(p): + raw_response = p + + if m == b'': + self.connected = False + return False, None + + time.sleep(0.01) + + if verbose: + print(f'Data response received: "{raw_response}"') + except (BrokenPipeError, ConnectionResetError, TimeoutError) as e: + print("Robot connection error: {}".format(e)) + self.connected = False + return False, None + + return success, raw_response