From 1399af5fbe02b1120a77622314b95565b9c1080c Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Thu, 9 May 2024 17:12:12 +0200 Subject: [PATCH] implement osc rtde output --- requirements.txt | 4 +- tests/rtdetest1.py | 7 +- tests/servoj_circle.py | 4 +- urosc.py | 208 +++++++++++++++++++++++++++++++++++++++++ 4 files changed, 217 insertions(+), 6 deletions(-) create mode 100644 urosc.py diff --git a/requirements.txt b/requirements.txt index ec5b0a8..7d06f53 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,5 @@ ur_rtde numpy -windows-curses \ No newline at end of file +windows-curses +python-osc +netifaces \ No newline at end of file diff --git a/tests/rtdetest1.py b/tests/rtdetest1.py index c18da1e..0645f47 100644 --- a/tests/rtdetest1.py +++ b/tests/rtdetest1.py @@ -5,7 +5,7 @@ import math import rtde_control import rtde_receive -rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101") +rtde_r = rtde_receive.RTDEReceiveInterface("192.168.2.165") @@ -21,11 +21,12 @@ c=0 scale=0.2 -rtde_c = rtde_control.RTDEControlInterface("192.168.1.101") +rtde_c = rtde_control.RTDEControlInterface("192.168.2.165") #rtde_c.moveJ([-0.10, -0.5, 0.20, -0.001, 3.12, 0.04], 0.1, 0.1) #rtde_c.moveJ([-1.54, -1.83, -2.28, -0.59, 1.60, 0.023], 0.1, 0.1) rtde_c.moveJ([2.74, -1.45, -1.63, -1.63, 1.57, 4.31],0.1,0.1) -rtde_c.moveJ(rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]), 0.1,0.1) +rtde_c.moveJ([2.74, -1.27, -1.31, -1.68, 1.58, 4.31],0.1,0.1) +#rtde_c.moveJ(rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]), 0.1,0.1) print("gefahren") diff --git a/tests/servoj_circle.py b/tests/servoj_circle.py index e8c3cd0..11beec4 100644 --- a/tests/servoj_circle.py +++ b/tests/servoj_circle.py @@ -2,7 +2,7 @@ import rtde_control import time import math -rtde_c = rtde_control.RTDEControlInterface("192.168.1.101") +rtde_c = rtde_control.RTDEControlInterface("192.168.2.164") # Parameters velocity = 0.1 #0.5 @@ -47,7 +47,7 @@ for i in range(frequency*duration): cx=i/(frequency*duration) * 2 *3.1415 * 1 cx%= (2*3.1415) - joint_q=rtde_c.getInverseKinematics([xcenter+math.sin(cx)*0.2,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]) + joint_q=rtde_c.getInverseKinematics([xcenter+math.sin(cx)*scale,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]) #print(joint_q) rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) diff --git a/urosc.py b/urosc.py new file mode 100644 index 0000000..dbfcb0b --- /dev/null +++ b/urosc.py @@ -0,0 +1,208 @@ +import argparse +from pythonosc import dispatcher +from pythonosc import osc_server +from pythonosc import udp_client +import threading +import time +import math + +import curses + +from rtde_receive import RTDEReceiveInterface as RTDEReceive + + + +clients=None +rtde_r=None + + + + +def toRad(d): + return d/360*2*math.pi + +def toDeg(r): + return r*360/2/math.pi + + +def mapFromTo(x,a,b,c,d): + y=(x-a)/(b-a)*(d-c)+c + return y + +def handle_received_osc(address, *args): + print(f"Received OSC message on {address}") + for arg in args: + print(f"Data: {arg}") + +def setupOSC(ip, port): + # Set up the dispatcher for the server + disp = dispatcher.Dispatcher() + disp.map("/filter", handle_received_osc) # You can change '/filter' to any address pattern you expect + + # Set up OSC server + server = osc_server.ThreadingOSCUDPServer((ip, port), disp) + print(f"Serving on {server.server_address}") + server_thread = threading.Thread(target=server.serve_forever) + server_thread.start() + + return server,server_thread + +def setupClients(clientips,port): + clients = [] + + # Set up OSC client + for cip in clientips: + print("connecting to "+str(cip)) + client = udp_client.SimpleUDPClient(cip, port) + clients.append(client) + + return clients + + +def setupRTDE(robotip,frequency): + dt = 1 / frequency + rtde_r = RTDEReceive(robotip, frequency) + + return rtde_r + + +joint_min=[toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(0)] +joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)] + + + + +def main(stdscr): + """Main entry point allowing external calls + + Args: + args ([str]): command line parameter list + """ + + # Clear and refresh the screen for a blank canvas + stdscr.clear() + stdscr.refresh() + + curses.cbreak() + stdscr.keypad(1) + stdscr.nodelay(1) + + + # Start colors in curses + curses.start_color() + curses.init_pair(1, curses.COLOR_CYAN, curses.COLOR_BLACK) + curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK) + curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_WHITE) + + + height, width = stdscr.getmaxyx() + + dt = 1 / args.frequency + + + + key = '' + i = 0 + try: + while key != ord('q'): + key = stdscr.getch() + stdscr.clear() + start = time.time() + + q=rtde_r.getActualQ() + for c in clients: + + + for jointi in range(6): + c.send_message("/j"+str(jointi), toDeg(q[jointi])) + #print("sent "+str(jointi)+" = "+str(q[jointi])) + #time.sleep(0.1) + #print(q) + #sys.stdout.write("\r") + #sys.stdout.write("{:3d} samples.".format(i)) + #sys.stdout.flush() + + stdscr.addstr(0, 0, "test "+str(i), curses.color_pair(1)) + + + for j in range(6): + posx=mapFromTo(q[j],joint_min[j],joint_max[j],0,width) #scale to screen width + for px in range(width): + diff=abs(posx-px) + poschar='' + if diff<3: + poschar='.' + if diff<2: + poschar='i' + if diff<0.5: + poschar='|' + + if (len(poschar)>0): + stdscr.addstr(2+j, px, poschar, curses.color_pair(1)) + valuetext=str(round(q[j],2)) + if q[j]>=0: #has no negative sign + valuetext=" "+valuetext + stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1)) + + + # Refresh the screen + stdscr.refresh() + + + end = time.time() + duration = end - start + + if duration < dt: + time.sleep(dt - duration) + i += 1 + + except KeyboardInterrupt: + + print("\nData recording stopped.") + curses.endwin() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--listenip", + default="127.0.0.1", help="The ip to listen on") + parser.add_argument("--listenport", + type=int, default=5005, help="The port to listen on") + + parser.add_argument("--clientip", + default="127.0.0.1", help="The ip to connect to") + parser.add_argument("--port", + type=int, default=5005, help="The port to connect to") + + + parser.add_argument("--robotip", + default="127.0.0.1", help="The ip to connect to") + + parser.add_argument( + "-f", + "--frequency", + dest="frequency", + help="the frequency at which the data is recorded (default is 500Hz)", + type=float, + default=500.0, + metavar="") + args = parser.parse_args() + + clientips=[args.clientip] + + server,server_thread=setupOSC(args.listenip, args.listenport) + + clients=setupClients(clientips,args.port) + + rtde_r=setupRTDE(args.robotip,args.frequency) + + + curses.wrapper(main) + + + input("Ended. Press Enter to shutdown") + + + server.shutdown() + server_thread.join() +