robot looks at you

Robot/UR/URModbusServer.py

@@ -0,0 +1,163 @@
+from Communication.ModbusTCP import ModbusTCP
+
+import time, math, struct
+
+# The robot controller acts as a Modbus TCP server (port 502),
+# clients can establish connections to it and send standard MODBUS requests to it.
+
+# - Note that some Modbus device manufacturers use the terms Master (client) and Slave (server).
+# Typically the external IO device is going to be a server and the robot is going to behave as the client
+# (requesting and consuming messages from the server). Master=client and Slave=server.
+# However, note that the UR controller can be both a server and a client
+
+# - Note that all values are unsigned, if you want to convert to signed integers,
+# program "if (val > 32768): val = val - 65535".
+#
+# - The MODBUS Server has 0-based addressing.
+# Be aware that some other devices are 1-based (e.g. Anybus X-gateways), then just add one to the address
+# on that device. (e.g. address 3 on the robot will be address 4 on the Anybus X-gateway)
+
+
+class URModbusServer:
+    """Give read and write access to data in the robot controller for other devices
+
+    An interface for communicating with the modbus TCP server (port 502) on the UR.
+    Defines functions for retrieving information from the controller.
+    Information will be re-requested if an error occurs.
+    All information will be formatted to human readable information.
+    """
+
+    def __init__(self, host):
+        """
+        :param host: IP address to connect with
+        """
+        self.modbusTCP = ModbusTCP(host, 502)
+
+    def get_tcp_position(self):
+        """
+        Connects with the Modbus server to requests Cartesian data of the TCP
+        :return: Readable cartesian data of TCP, vector in mm, axis in radials
+        """
+        packet = self.modbusTCP.read_holding_registers(400, quantity=6)
+
+        if packet is None:
+            time.sleep(0.5)
+            print("Modbus Error: retrying")
+            return self.get_tcp_position()
+        else:
+            x = self._format(packet[9:11]) / 10
+            y = self._format(packet[11:13]) / 10
+            z = self._format(packet[13:15]) / 10
+            rx = self._format(packet[15:17]) / 1000
+            ry = self._format(packet[17:19]) / 1000
+            rz = self._format(packet[19:21]) / 1000
+            return x, y, z, rx, ry, rz
+        
+    def get_joint_angles(self, tries=0) -> tuple:
+        """
+        Connects with the Modbus server to requests the angles of each joint, in radians
+        :return: Readable angle values of each joint in radials
+        """
+        if tries>10:
+            print("Modbus Error: Failed")
+            return 0, 0, 0, 0, 0, 0
+
+        packet = self.modbusTCP.read_holding_registers(270, quantity=6)
+        packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6)
+
+        if (packet is None) or (packet_signs is None):
+            time.sleep(0.01)
+            print(f"Modbus Error #{tries}: retrying")
+            return self.get_joint_angles(tries+1)
+        else:
+            #print(f"Packet: {packet_signs.hex()}")
+            base = self._format_sign(packet[9:11], packet_signs[9:11])
+            shoulder = self._format_sign(packet[11:13], packet_signs[11:13])
+            elbow = self._format_sign(packet[13:15], packet_signs[13:15])
+            wrist_1 = self._format_sign(packet[15:17], packet_signs[15:17])
+            wrist_2 = self._format_sign(packet[17:19], packet_signs[17:19])
+            wrist_3 = self._format_sign(packet[19:21], packet_signs[19:21])
+            
+            return base, shoulder, elbow, wrist_1, wrist_2, wrist_3
+
+    def get_joint_angles_degrees(self, tries=0):
+        angles = self.get_joint_angles(tries)
+        return tuple([round(math.degrees(angle), 3) for angle in angles])
+
+    def get_joint_speeds(self, tries=0):
+        """
+        Connects with the Modbus server to requests the speed of each joint, in radians
+        :return: Readable angle values of each joint in radials
+        """
+        if tries>10:
+            print("Modbus Error: Failed")
+            return 0, 0, 0, 0, 0, 0
+
+        packet = self.modbusTCP.read_holding_registers(280, quantity=6)
+
+        if (packet is None):
+            time.sleep(0.01)
+            print(f"Modbus Error #{tries}: retrying")
+            return self.get_joint_speeds(tries+1)
+        else:
+            #print(f"Packet: {packet_signs.hex()}")
+            base = self._format(packet[9:11]) / 1000
+            shoulder = self._format(packet[11:13]) / 1000
+            elbow = self._format(packet[13:15]) / 1000
+            wrist_1 = self._format(packet[15:17]) / 1000
+            wrist_2 = self._format(packet[17:19]) / 1000
+            wrist_3 = self._format(packet[19:21]) / 1000
+            
+            return base, shoulder, elbow, wrist_1, wrist_2, wrist_3
+
+    @staticmethod
+    def _format(d):
+        """Formats signed integers to unsigned float
+
+        :param d: signed integer to format
+        :return: unsigned float
+        """
+        d = d.hex()
+        d_i = int(d, 16)
+        d_f = 0
+
+        if d_i < 32768:
+            d_f = float(d_i)
+        if d_i > 32767:
+            d_i = 65535 - d_i
+            d_f = float(d_i) * -1
+
+        return d_f
+    
+    @staticmethod
+    def _format_2(d):
+        """Formats signed integers to signed float
+
+        :param d: signed integer to format
+        :return: signed float
+        """
+        d = d.hex()
+        d_i = int(d, 16)
+        d_f = float(d_i)
+        return d_f
+    
+    @staticmethod
+    def _format_sign(d, sign):
+        """
+
+        :param d:
+        :param sign: 
+        :return: 
+        """
+        sign = sign.hex()
+        sign = int(sign, 16)
+
+        d = d.hex()
+        d_i = int(d, 16)
+        d_f = float(d_i) / 1000
+
+        if sign == 0:
+            return d_f
+        else:
+            d_f = round(d_f - math.radians(360), 3)
+            return d_f

Robot/UR/URRobot.py

@@ -0,0 +1,160 @@
+from Communication.SocketConnection import SocketConnection
+from Robot.UR.URModbusServer import URModbusServer
+from Robot.UR.URScript import URScript
+
+
+class URRobot:
+    """
+    Interface for communicating with the UR Robot
+    SecondaryPort used for sending commands
+    ModbusServer used for retrieving info
+    """
+    def __init__(self, host):
+        self.secondaryPort = 30002
+        self.secondaryInterface = SocketConnection(host, self.secondaryPort)
+        self.secondaryInterface.connect()
+        self.URModbusServer = URModbusServer(host)
+        self.URScript = URScript()
+
+        # Max safe values of acceleration and velocity are 0.4
+        # DO NOT USE THE FOLLOWING VALUES
+        # MAX a=1.3962634015954636
+        # MAX v=1.0471975511965976
+        # These are the absolute max for 'safe' movement and should only be used
+        # if you know what your are doing, otherwise you WILL damage the UR internally
+        self.acceleration = 0.1
+        self.velocity = 0.1
+
+    def movel(self, pose, a=0.1, v=0.1, joint_p=False):
+        """Move to position (linear in tool-space)
+
+        See :class:`URScript` for detailed information
+        """
+        script = URScript.movel(pose, a, v, joint_p=joint_p).encode()
+        return self._send_script(script)
+
+    def movej(self, q, a=0.1, v=0.1, joint_p=True):
+        """Move to position (linear in joint-space)
+
+        See :class:`URScript` for detailed information
+        """
+        script = URScript.movej(q, a, v, joint_p=joint_p).encode()
+        return self._send_script(script)
+    
+    def speedj(self, qd, a=0.5, t=0):
+        """Accelerate linearly in joint space and continue with constant joint speed.
+        
+        The time t is optional; if provided the function will return after time t, regardless of the target speed has been reached.
+        If thetime t is not provided, the function will return when the target speed is reached.
+
+        See :class:`URScript` for detailed information
+        """
+        script = URScript.speedj(qd, a, t).encode()
+        return self._send_script(script)
+
+    def stopj(self, a=1.5):
+        """Stop (linear in joint space)
+
+        See :class:`URScript` for detailed information
+        """
+        script = URScript.stopj(a).encode()
+        return self._send_script(script)
+
+    def set_tcp(self, pose):
+        """Set the Tool Center Point
+
+        See :class:`URScript` for detailed information
+        """
+        script = URScript.set_tcp(pose).encode()
+        return self._send_script(script)
+
+    def get_tcp_position(self):
+        """ Get TCP position
+
+        Will return values as seen on the teaching pendant (300.0mm)
+        :return: 6 Floats - Position data of TCP (x, y, z) in mm (Rx, Ry, Rz) in radials
+        """
+        position_data = self.URModbusServer.get_tcp_position()
+        return position_data
+    
+    def get_joint_angles(self):
+        """ Get joint angles
+
+        :return: 6 Floats - Position joint angles (base, shoulder, elbow, wrist_1, wrist_2, wrist_3) in radians
+        """
+        position_data = self.URModbusServer.get_joint_angles()
+        return position_data
+    
+    def get_joint_angles_degrees(self):
+        """ Get joint angles in degrees
+
+        :return: 6 Floats - Position joint angles (base, shoulder, elbow, wrist_1, wrist_2, wrist_3) in degrees
+        """
+        position_data = self.URModbusServer.get_joint_angles_degrees()
+        return position_data
+    
+    def get_joint_speeds(self):
+        """ Get joint speeds in rads/s
+
+        :return: 6 Floats - Position joint speeds (base, shoulder, elbow, wrist_1, wrist_2, wrist_3) in rads/s
+        """
+        speed_data = self.URModbusServer.get_joint_speeds()
+        return speed_data
+
+    def set_io(self, io, value):
+        """
+        Set the specified IO
+        :param io: The IO to set as INT
+        :param value: Boolean to enable or disable IO
+        :return: Boolean to check if the command has been send
+        """
+        script = "set_digital_out({}, {})".format(io, value) + "\n"
+        script = script.encode()
+        return self._send_script(script)
+
+    def translate(self, vector, a=0.1, v=0.1):
+        """ Move TCP based on its current position
+
+        Example:
+        If the TCP position was: 0.4, 0.5, 0.5 and the vector value passed is 0.1, 0.0, 0.0.
+        It will attempt to translate the TCP to 0.5, 0.5, 0.5
+        :param vector: the X, Y, Z to translate to
+        :param a: tool acceleration [m/2^s]
+        :param v: tool speed [m/s]
+        :return: Boolean to check if the command has been send
+        """
+        tcp_pos = list(self.get_tcp_position())
+        tcp_pos[0] = tcp_pos[0] / 1000 + vector[0]
+        tcp_pos[1] = tcp_pos[1] / 1000 + vector[1]
+        tcp_pos[2] = tcp_pos[2] / 1000 + vector[2]
+        return self.movel(tcp_pos, a, v)
+
+    def _send_script(self, _script):
+        """ Send URScript to the UR controller
+
+        :param _script: formatted script to send
+        :return: Boolean to check if the script has been send
+        """
+        try:
+            self.secondaryInterface.send(_script)
+        except OSError as error:
+            print("OS error: {0}".format(error))
+            return False
+        return True
+
+    @ staticmethod
+    def format_cartesian_data(cartesian_data):
+        """
+        Formats the vector from human readable mm to robot readable mm
+        A position of 300mm becomes 0.3
+        :param cartesian_data: data to format
+        :return: formatted data
+        """
+        i = 0
+        for position in cartesian_data:
+            cartesian_data[position] /= (1000 if i < 3 else 1)
+            i += 1
+        formatted_cartesian_data = cartesian_data
+
+        return formatted_cartesian_data
+

Robot/UR/URScript.py

@@ -0,0 +1,142 @@
+# SW Version 1.8 - URScript manual:
+# When connected URScript programs or commands are sent in clear text on the socket.
+# Each line is terminated by ’\n’
+# python 3 must use bytes (b"") to send the string
+
+# When using URScript the robot moves according to the base plane.
+
+# Joint positions (q) and joint speeds (qd) are represented directly as lists of 6 Floats, one for each robot joint.
+# Tool poses (x) are represented as poses also consisting of 6 Floats.
+# In a pose, the first 3 coordinates is a position vector and the last 3 an axis-angle
+
+
+class URScript:
+    """ Defines functions for use with the URRobot.
+
+    Based on URScript manual version 1.8
+    """
+    __version__ = 1.8
+
+    @staticmethod
+    def movec(pose_via, pose_to, a=0.1, v=0.1, r=0, joint_p=False):
+        """Move Circular: Move to position (circular in tool-space)
+
+        TCP moves on the circular arc segment from current pose, through pose_via to pose_to.
+        Accelerates to and moves with constant tool speed v
+        :param pose_via: path point (note: only position is used).
+        (pose_via can also be specified as joint positions, then forward kinematics is used to
+        calculate the corresponding pose)
+        :param pose_to: target pose (pose_to can also be specified as joint positions, then
+        forward kinematics is used to calculate the corresponding pose
+        :param a: tool acceleration [m/2^s]
+        :param v: tool speed [m/s]
+        :param r: blend radius (of target pose) [m]
+        :param joint_p: if True, poses are specified as joint positions
+        :return: string containing the movec script
+        """
+        prefix = "" if joint_p else "p"
+        return"movec({}[{}, {}, {}, {}, {}, {}], {}[{}, {}, {}, {}, {}, {}], " \
+              "a={}, v={}, r={})".format(prefix, *pose_via, prefix, *pose_to, a, v, r) + "\n"
+
+    @staticmethod
+    def movej(q, a=0.1, v=0.1, t=0, r=0, joint_p=False):
+        """Move to position (linear in joint-space)
+
+        When using this command, the
+        robot must be at standstill or come from a movej or movel with a blend.
+        The speed and acceleration parameters controls the trapezoid speed
+        profile of the move. The $t$ parameters can be used in stead to set the
+        time for this move. Time setting has priority over speed and acceleration
+        settings. The blend radius can be set with the $r$ parameters, to avoid
+        the robot stopping at the point. However, if he blend region of this mover
+        overlaps with previous or following regions, this move will be skipped, and
+        an ’Overlapping Blends’ warning message will be generated.
+        :param q: joint positions (q can also be specified as a pose, the inverse
+        kinematics is used to calculate the corresponding joint positions)
+        :param a: joint acceleration of leading axis [rad/sˆ2]
+        :param v: joint speed of leading axis [rad/s]
+        :param t: time [S]
+        :param r: blend radius [m]
+        :param joint_p: if True, poses are specified as joint positions
+        :return: string containing the movej script
+        """
+        prefix = "" if joint_p else "p"
+        return "movej({}[{}, {}, {}, {}, {}, {}], a={}, v={}, t={}, r={})".format(prefix, *q, a, v, t, r) + "\n"
+
+    @staticmethod
+    def movel(pose, a=0.1, v=0.1, t=0, r=0, joint_p=False):
+        """Move to position (linear in tool-space)
+
+        :param pose: target pose (pose can also be specified as joint positions,
+        then forward kinematics is used to calculate the corresponding pose)
+        :param a: tool acceleration [m/2^s]
+        :param v: tool speed [m/s]
+        :param t: time [S]
+        :param r: blend radius [m]
+        :param joint_p: if True, poses are specified as joint positions
+        :return: string containing the movel script
+        """
+        prefix = "" if joint_p else "p"
+        return "movel({}[{}, {}, {}, {}, {}, {}], a={}, v={}, t={}, r={})".format(prefix, *pose, a, v, t, r) + "\n"
+
+    @staticmethod
+    def movep(pose, a=0.1, v=0.1, t=0, r=0, joint_p=False):
+        """Move Process
+
+        Blend circular (in tool-space) and move linear (in tool-space) to position.
+        Accelerates to and moves with constant tool speed v.
+        :param pose: target pose (pose can also be specified as joint positions,
+        then forward kinematics is used to calculate the corresponding pose
+        :param a: tool acceleration [m/2^s]
+        :param v: tool speed [m/s]
+        :param t: time [S]
+        :param r: blend radius [m]
+        :param joint_p: if True, poses are specified as joint positions
+        :return: string containing the movep script
+        """
+        prefix = "" if joint_p else "p"
+        return "movep({}[{}, {}, {}, {}, {}, {}], a={}, v={}, t={}, r={})" .format(prefix, *pose, a, v, t, r) + "\n"
+
+    @staticmethod
+    def speedj(qd, a=0.1, t=0):
+        """Accelerate linearly in joint space and continue with constant joint speed.
+
+        The time t is optional; if provided the function will return after time t, regardless of the target speed has been reached.
+        If the time t is not provided, the function will return when the target speed is reached.
+        :param qd: joint speeds [rad/s]
+        :param a: joint acceleration [rad/sˆ2]
+        :param t: time [S]. Optional. Will not consider if equals or bellow 0
+        :return: string containing the speedj script
+        """
+        speedtime = "" if t <= 0 else ", t=" + str(t)
+        return "speedj([{}, {}, {}, {}, {}, {}], a={}{})".format(*qd, a, speedtime) + "\n"
+
+    @staticmethod
+    def set_tcp(pose):
+        """Set the Tool Center Point
+
+        Sets the transformation from the output flange coordinate system to the TCP as a pose.
+        :param pose: A pose representing the offset in x, y z and the rotation of the tcp around x, y and z
+        :return: A string containing the set tcp script
+        """
+        return "set_tcp(p[{}, {}, {}, {}, {}, {}])".format(*pose) + "\n"
+
+    @staticmethod
+    def stopj(a=1.5):
+        """Stop (linear in joint space)
+
+        Decelerate joint speeds to zero
+        :param a: joint acceleration [rad/sˆ2] (of leading axis)
+        :return: String containing the stopj script
+        """
+        return "stopj({})".format(a) + "\n"
+
+    @staticmethod
+    def stopl(a=0.5):
+        """Stop (linear in tool space)
+
+        Decelerate tool speeds to zero
+        :param a: joint acceleration [rad/sˆ2] (of leading axis)
+        :return: String containing the stopj script
+        """
+        return "stopl({})".format(a) + "\n"