171 lines
4.3 KiB
Python
171 lines
4.3 KiB
Python
import argparse
|
|
|
|
|
|
import time
|
|
import math
|
|
import numpy as np
|
|
import curses
|
|
|
|
|
|
|
|
from helpfunctions import *
|
|
from ui import UI
|
|
from oscsend import OSCSend
|
|
from oscreceive import OSCReceive
|
|
from ur import UR
|
|
|
|
|
|
#Looptimings
|
|
frequency_loop=500
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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)]
|
|
|
|
|
|
ui = UI(joint_min,joint_max)
|
|
|
|
oscsend = OSCSend()
|
|
oscreceive = None
|
|
ur = UR(joint_min,joint_max)
|
|
|
|
|
|
def main(stdscr):
|
|
|
|
ui.begin(stdscr)
|
|
|
|
|
|
loop_dt = 1 / frequency_loop
|
|
|
|
|
|
|
|
|
|
loopcounter = 0
|
|
loopdurations = np.zeros(100)
|
|
loopdurations_pos=0
|
|
|
|
|
|
keyDown=False
|
|
|
|
try:
|
|
while ui.getKey() != ord('q'):
|
|
looptime = time.time()
|
|
|
|
start = time.time()
|
|
|
|
ur.receiveJoints(looptime)
|
|
#ur.receiveStatus(looptime)
|
|
|
|
#send data to all osc receivers
|
|
oscsend.send(looptime,ur.getReceivedQ(),ur.getReceivedTCPPose())
|
|
|
|
if ur is not None:
|
|
ur.send(looptime)
|
|
|
|
#Display
|
|
ui.update(ur.getReceivedQ(),looptime)
|
|
|
|
#Looptiming
|
|
end = time.time()
|
|
loopduration = end - start
|
|
loopdurations[loopdurations_pos]=loopduration
|
|
loopdurations_pos+=1
|
|
loopdurations_pos%=np.size(loopdurations)
|
|
|
|
ui.setDebugtext(0,"loopcounter="+str(loopcounter)+"\tControlEnabled="+str(ur.isControlEnabled()))
|
|
ui.setDebugtext(1,"loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)))
|
|
if oscreceive is not None:
|
|
ui.setDebugtext(2,"osc receive frequency="+str(round(oscreceive.getCurrentReceiveFrequency(),0)))
|
|
#ui.setDebugtext(3,"invkin"+str(ur.getForwardKinematics()))
|
|
tcp=ur.getReceivedTCPPose()
|
|
tcp_deg=[tcp[0],tcp[1],tcp[2],toDeg(tcp[3]),toDeg(tcp[4]),toDeg(tcp[5])]
|
|
#ui.setDebugtext(3,"osc test:"+str(oscreceive.getDebug())+"\t tcp_deg="+str(tcp_deg))
|
|
ui.setDebugtext(3,"osc test:"+str(oscreceive.getDebug()))
|
|
|
|
if ui.getKey() == ord('e') and keyDown==False:
|
|
keyDown=True
|
|
ur.enableControl()
|
|
if ui.getKey() == ord('d') and keyDown==False:
|
|
keyDown=True
|
|
ur.disableControl()
|
|
if ui.getKey()==-1:
|
|
keyDown=False
|
|
|
|
|
|
|
|
|
|
if loopduration < loop_dt:
|
|
time.sleep(loop_dt - loopduration)
|
|
loopcounter += 1
|
|
|
|
except (KeyboardInterrupt):
|
|
print("\nKeyboardInterrupt")
|
|
except Exception as e:
|
|
pass
|
|
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("--oscsendip",
|
|
default="127.0.0.1", help="The ip to connect to")
|
|
parser.add_argument("--oscsendport",
|
|
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("--test", action='store_true', default=False, help="Disable network and robot communication")
|
|
|
|
|
|
'''
|
|
parser.add_argument(
|
|
"-fs",
|
|
"--frequencysend",
|
|
dest="frequencysend",
|
|
help="the frequency at which the robot control data is send",
|
|
type=float,
|
|
default=500.0,
|
|
metavar="<frequency>")
|
|
|
|
parser.add_argument(
|
|
"-fr",
|
|
"--frequencyreceive",
|
|
dest="frequencyreceive",
|
|
help="the frequency at which the robot actualq data is received",
|
|
type=float,
|
|
default=500.0,
|
|
metavar="<frequency>")
|
|
'''
|
|
|
|
args = parser.parse_args()
|
|
|
|
|
|
|
|
if args.test is not True:
|
|
ur.connect(args.robotip)
|
|
|
|
oscreceive = OSCReceive(args.listenip, args.listenport,oscsend,ur)
|
|
|
|
oscsend.setupReceivers([args.oscsendip],args.oscsendport)
|
|
|
|
|
|
|
|
curses.wrapper(main)
|
|
|
|
if oscreceive is not None:
|
|
oscreceive.disconnect()
|
|
if ur is not None:
|
|
ur.disconnect()
|
|
|