diff --git a/ur_py_ctl/interpreter.py b/ur_py_ctl/interpreter.py new file mode 100644 index 0000000000000000000000000000000000000000..01f57f5994e0f7ee435df8e55b95272f080a8e60 --- /dev/null +++ b/ur_py_ctl/interpreter.py @@ -0,0 +1,78 @@ +import socket + +UR_ADRESS = "marius.d.control.lth.se" +UR_PORT = 30002 +UR_INTERPRET_PORT = 30020 + +PC_ADRESS = "tatlin.control.lth.se" +PC_PORT = 31000 + +SETUP_INTERPRETER_SCRIPT = f""" +def program(): + socket_open("{PC_ADRESS}",{PC_PORT}) + thread communication_thread(): + while True: + torques=to_str(get_tcp_force()) + socket_send_string(torques) + sync() + end + return False + + end + interpreter_mode(clearQueueOnEnter = True, clearOnEnd = True) +end + +program() +""".encode() + + +def run_interpreter(): + # Setting up socket for sending and receiving. + with ( + socket.create_connection((UR_ADRESS, UR_PORT), timeout=2) as send_socket, + socket.socket(socket.AF_INET, socket.SOCK_STREAM) as recv_socket, + socket.create_connection( + (UR_ADRESS, UR_INTERPRET_PORT), timeout=2 + ) as interpret_socket, + ): + + recv_socket.bind((PC_ADRESS, PC_PORT)) + + # Skapa ett program som ställer robotarmen i interpreter_mode() + + # Öppnar socket + + # Definierar tråd för skickade av krafter + + send_socket.send(SETUP_INTERPRETER_SCRIPT) + + recv_socket.listen(1) + + _, client_address = recv_socket.accept() + + print(f"Recieved accept from: {client_address}") + + # UR5 är nu i interpreter_mode() + + # Flera kommandon separeras med mellanslag. + + interpret_socket.send( + "j_angles=get_actual_joint_positions() " + + "socket_send_string(to_str(j_angles))\n".encode() + ) + + joint_message = interpret_socket.recv(4096).decode() + + print(joint_message) + + # Avsluta interpret_mode() + + interpret_socket.send("end_interpreter()\n".encode()) + + answer1 = interpret_socket.recv(4096) + + print(answer1.decode()) + + +if __name__ == "__main__": + run_interpreter()