controlUR5.py

# URSoftware 5.1.1

# Echo client program
import base64
import math
import socket
import time

# HOST = "169.254.52.193"    # The remote host
# HOST = "192.168.0.1"
HOST = "172.22.22.2"
PORT = 30002  # The same port as used by the server
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.connect((HOST, PORT))


def debug():
    s.send("set_digital_out(2,True)".encode() + "\n".encode())
    time.sleep(0.05)


# pose = [x,y,z,rx,ry,rz]
# acceleration and velocity have default values
# if you set a time 't', that overrides 'a' and 'v'
def movel(s, p, a=0.1, v=0.5, t=0, r=0):
    try:
        script = "movel(p[{},{},{},{},{},{}], a={}, v={}, t={}, r={})"
        script = script.format(p[0], p[1], p[2], p[3], p[4], p[5], a, v, t, r)
        s.send(bytes(script, "utf-8") + bytes("\n", "utf-8"))
        msg = s.recv(1024)
    except socket.error as socketerror:
        print("..... Some kind of error :(...")
    return msg


# angles = [base,shoulder,elbow,wrist1,wrist2,wrist3]
# angles can be given in degrees
def movej(s, angles, a=0.1, v=0.5, t=0, r=0):
    base = angles[0] * math.pi / 180  # converting to radians
    shoulder = angles[1] * math.pi / 180
    elbow = angles[2] * math.pi / 180
    w1 = angles[3] * math.pi / 180
    w2 = angles[4] * math.pi / 180
    w3 = angles[5] * math.pi / 180

    try:
        script = "movej([{},{},{},{},{},{}], a={}, v={}, t={}, r={})"
        script = script.format(base, shoulder, elbow, w1, w2, w3, a, v, t, r)
        s.send(bytes(script, "utf-8") + bytes("\n", "utf-8"))
        msg = s.recv(1024)
    except socket.error as socketerror:
        print("..... Some kind of error :(...")
    return msg


# moving to stable joint angles
# movej(s,[80.43,-40.59,61.4,-110.8,-89.77,80.45])


def initialise(s, a=0.5, v=0.5, t=0, r=0):
    x = 0  # mm
    y = -0.6
    z = 0.2
    rx = 2.221  # radians
    ry = 2.221
    rz = 0
    p = [x, y, z, rx, ry, rz]
    try:
        script = "movel(p[{},{},{},{},{},{}], a={}, v={}, t={}, r={})"
        script = script.format(p[0], p[1], p[2], p[3], p[4], p[5], a, v, t, r)
        s.send(bytes(script, "utf-8") + bytes("\n", "utf-8"))
        msg = s.recv(1024)

    except socket.error as socketerror:
        print("..... Some kind of error :(...")

    return msg


# This function doesn't work. The output doesn't make sense
def decode_position(s, msg):
    # Decode Pose or Joints from UR
    time.sleep(2)
    current_position = [0, 0, 0, 0, 0, 0]
    data_start = 0
    data_end = 0
    n = 0
    x = 0
    while x < len(msg):
        if msg[x] == "," or msg[x] == "]" or msg[x] == "e":
            data_end = x
            current_position[n] = float(msg[data_start:data_end])
            if msg[x] == "e":
                current_position[n] = current_position[n] * math.pow(
                    10, float(msg[x + 1 : x + 4])
                )
                # print "e", msg[x+1:x+4]
                # print "e", int(msg[x+1:x+4])
                if n < 5:
                    x = x + 5
                    data_start = x
                else:
                    break
            n = n + 1
        if msg[x] == "[" or msg[x] == ",":
            data_start = x + 1
        x = x + 1

    print(current_position)


msg = movel(s, [0, -1, 0.2, 2.221, 2.221, 0])
# decode_position(s, msg)
print(msg.decode("ISO-8859-1"))

# print(base64.b64decode(msg))

s.close()