finish classes
This commit is contained in:
parent
bf5a5a8509
commit
1cd7514ea0
5 changed files with 173 additions and 105 deletions
65
urosc/oscreceive.py
Normal file
65
urosc/oscreceive.py
Normal file
|
|
@ -0,0 +1,65 @@
|
|||
#from helpfunctions import *
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
class OSCReceive:
|
||||
from pythonosc import dispatcher
|
||||
from pythonosc import osc_server
|
||||
|
||||
import threading
|
||||
|
||||
|
||||
|
||||
def handle_received_osc_joint(self,address, *args):
|
||||
|
||||
self.server=None
|
||||
self.server_thread=None
|
||||
|
||||
self.oscreceiveintervals[self.oscreceiveintervals_pos]=time.time()-self.last_oscreceive
|
||||
self.last_oscreceive=time.time()
|
||||
self.oscreceiveintervals_pos+=1
|
||||
self.oscreceiveintervals_pos%=np.size(self.oscreceiveintervals)
|
||||
|
||||
#print(f"Received OSC message on {address}")
|
||||
joint_q_aim=[0,0,0,0,0,0]
|
||||
try:
|
||||
for iarg, arg in enumerate(args):
|
||||
#print(f"Data: {arg}")
|
||||
joint_q_aim[iarg]=float(arg)
|
||||
except:
|
||||
print("Data not in right format")
|
||||
print("Data: "+str(args))
|
||||
else:
|
||||
if self.ur is not None:
|
||||
self.ur.setJointQAim(joint_q_aim)
|
||||
|
||||
|
||||
|
||||
def getCurrentReceiveFrequency(self):
|
||||
return 1.0/np.mean(self.oscreceiveintervals)
|
||||
|
||||
def __init__(self,ip,port,_ur=None):
|
||||
|
||||
#osc receive statistics
|
||||
self.oscreceiveintervals = np.zeros(100)
|
||||
self.oscreceiveintervals_pos=0
|
||||
self.last_oscreceive=0
|
||||
|
||||
self.ur = _ur
|
||||
|
||||
|
||||
# Set up the dispatcher for the server
|
||||
disp = self.dispatcher.Dispatcher()
|
||||
disp.map("/joint", self.handle_received_osc_joint) # You can change '/filter' to any address pattern you expect
|
||||
|
||||
# Set up OSC server
|
||||
self.server = self.osc_server.ThreadingOSCUDPServer((ip, port), disp)
|
||||
print(f"Serving on {self.server.server_address}")
|
||||
self.server_thread = self.threading.Thread(target=self.server.serve_forever)
|
||||
self.server_thread.start()
|
||||
|
||||
|
||||
|
||||
def disconnect(self):
|
||||
self.server.shutdown()
|
||||
self.server_thread.join()
|
||||
|
|
@ -10,8 +10,6 @@ class UI:
|
|||
self.debugtext=["","",""]
|
||||
self.width=0
|
||||
self.height=0
|
||||
self.joint_min=None
|
||||
self.joint_max=None
|
||||
self.joint_min=_joint_min
|
||||
self.joint_max=_joint_max
|
||||
self.frequency_scr=_frequency_scr
|
||||
|
|
|
|||
81
urosc/ur.py
Normal file
81
urosc/ur.py
Normal file
|
|
@ -0,0 +1,81 @@
|
|||
from helpfunctions import *
|
||||
import time
|
||||
|
||||
class UR:
|
||||
|
||||
import rtde_receive
|
||||
import rtde_control
|
||||
|
||||
def __init__(self,_joint_min,_joint_max,_frequency_rtde_send=500,_frequency_rtde_receive=500):
|
||||
self.frequency_rtde_send=_frequency_rtde_send
|
||||
self.frequency_rtde_receive=_frequency_rtde_receive
|
||||
|
||||
|
||||
self.last_receivertde=0
|
||||
self.last_sendrtde=0
|
||||
|
||||
self.joint_min=_joint_min
|
||||
self.joint_max=_joint_max
|
||||
|
||||
self.rtde_r=None
|
||||
self.rtde_c=None
|
||||
|
||||
|
||||
# Parameters
|
||||
self.velocity = 0.1
|
||||
self.acceleration = 0.1
|
||||
self.lookahead_time = 0.1
|
||||
self.gain = 300
|
||||
self.joint_q_aim=[0,0,0,0,0,0] #joint rotations aiming for
|
||||
self.joint_q = [0,0,0,0,0,0] #joint rotation to be send
|
||||
self.velocitylimit=[1,1,1,3,3,3] #joint velocity limits
|
||||
|
||||
|
||||
self.joint_q_received=[0,0,0,0,0,0]
|
||||
|
||||
def connect(self,robotip):
|
||||
|
||||
rtde_r = self.rtde_receive.RTDEReceiveInterface(robotip, self.frequency_rtde_receive)
|
||||
|
||||
rtde_c = self.rtde_control.RTDEControlInterface(robotip)
|
||||
|
||||
def setJointQAim(self,q):
|
||||
self.joint_q_aim=q
|
||||
|
||||
|
||||
def send(self,looptime):
|
||||
|
||||
if looptime-self.last_sendrtde<1.0/self.frequency_rtde_send:
|
||||
return
|
||||
self.last_sendrtde=looptime
|
||||
|
||||
for i in range(6): #update joint_q with max speeds
|
||||
self.joint_q[i] += constrain(self.joint_q_aim[i]-self.joint_q[i], -self.velocitylimit[i]/self.frequency_rtde_send,self.velocitylimit[i]/self.frequency_rtde_send) #constrain joint speeds
|
||||
|
||||
if self.rtde_c is not None:
|
||||
self.rtde_c.servoJ(self.joint_q, self.velocity, self.acceleration, 1.0/self.frequency_rtde_send, self.lookahead_time, self.gain)
|
||||
|
||||
def receive(self,looptime):
|
||||
|
||||
#Get robot joint angles
|
||||
if looptime-self.last_receivertde>=1.0/self.frequency_rtde_receive:
|
||||
self.last_receivertde=looptime
|
||||
|
||||
if self.rtde_r is not None:
|
||||
self.joint_q_received=self.rtde_r.getActualQ()
|
||||
else:
|
||||
_offsets=[12.2,5.3,0,6,12.4,612.91]
|
||||
_factors=[1.2,1.14,1.214,0.9921,1,1.092]
|
||||
|
||||
for _i,_offset in enumerate(_offsets): #generate mockup values
|
||||
self.joint_q_received[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(self.joint_max[_i]-self.joint_min[_i])+self.joint_min[_i]
|
||||
|
||||
def getReceivedQ(self):
|
||||
return self.joint_q_received
|
||||
|
||||
def disconnect(self):
|
||||
if self.rtde_c is not None:
|
||||
self.rtde_c.servoStop()
|
||||
self.rtde_c.stopScript()
|
||||
|
||||
|
||||
126
urosc/urosc.py
126
urosc/urosc.py
|
|
@ -1,84 +1,28 @@
|
|||
import argparse
|
||||
from pythonosc import dispatcher
|
||||
from pythonosc import osc_server
|
||||
|
||||
import threading
|
||||
|
||||
import time
|
||||
import math
|
||||
import numpy as np
|
||||
import curses
|
||||
|
||||
import rtde_receive
|
||||
import rtde_control
|
||||
|
||||
|
||||
from helpfunctions import *
|
||||
from ui import UI
|
||||
from oscsend import OSCSend
|
||||
from oscreceive import OSCReceive
|
||||
from ur import UR
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
rtde_r=None
|
||||
rtde_c=None
|
||||
server=None
|
||||
|
||||
#Looptimings
|
||||
frequency_loop=500
|
||||
frequency_rtde=None
|
||||
|
||||
|
||||
#Robot Control Parameters
|
||||
# Parameters
|
||||
velocity = 0.1
|
||||
acceleration = 0.1
|
||||
lookahead_time = 0.1
|
||||
gain = 300
|
||||
joint_q_aim=[0,0,0,0,0,0] #joint rotations aiming for
|
||||
joint_q = [0,0,0,0,0,0] #joint rotation to be send
|
||||
velocitylimit=[1,1,1,3,3,3] #joint velocity limits
|
||||
|
||||
|
||||
#osc receive statistics
|
||||
oscreceiveintervals = np.zeros(100)
|
||||
oscreceiveintervals_pos=0
|
||||
last_oscreceive=0
|
||||
|
||||
def handle_received_osc_joint(address, *args):
|
||||
oscreceiveintervals[oscreceiveintervals_pos]=time.time()-last_oscreceive
|
||||
last_oscreceive=time.time()
|
||||
oscreceiveintervals_pos+=1
|
||||
oscreceiveintervals_pos%=np.size(oscreceiveintervals)
|
||||
|
||||
print(f"Received OSC message on {address}")
|
||||
for iarg, arg in enumerate(args):
|
||||
#print(f"Data: {arg}")
|
||||
joint_q_aim[iarg]=float(arg)
|
||||
|
||||
|
||||
def setupOSC(ip, port):
|
||||
# Set up the dispatcher for the server
|
||||
disp = dispatcher.Dispatcher()
|
||||
disp.map("/joint", handle_received_osc_joint) # 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 setupRTDE(robotip,frequency):
|
||||
dt = 1 / frequency
|
||||
rtde_r = rtde_receive.RTDEReceiveInterface(robotip, frequency)
|
||||
|
||||
rtde_c = rtde_control.RTDEControlInterface(robotip)
|
||||
|
||||
return rtde_r
|
||||
|
||||
|
||||
joint_min=[toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(0)]
|
||||
|
|
@ -86,8 +30,10 @@ joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)]
|
|||
|
||||
|
||||
ui = UI(joint_min,joint_max)
|
||||
oscsend = OSCSend()
|
||||
|
||||
oscsend = OSCSend()
|
||||
oscreceive = None
|
||||
ur = UR(joint_min,joint_max)
|
||||
|
||||
|
||||
def main(stdscr):
|
||||
|
|
@ -103,12 +49,8 @@ def main(stdscr):
|
|||
loopcounter = 0
|
||||
loopdurations = np.zeros(100)
|
||||
loopdurations_pos=0
|
||||
last_receivertde=0
|
||||
last_sendrtde=0
|
||||
|
||||
|
||||
q=[0,0,0,0,0,0]
|
||||
|
||||
|
||||
try:
|
||||
while ui.getKey() != ord('q'):
|
||||
|
|
@ -116,36 +58,16 @@ def main(stdscr):
|
|||
|
||||
start = time.time()
|
||||
|
||||
|
||||
#Get robot joint angled
|
||||
if looptime-last_receivertde>=1.0/frequency_receivertde:
|
||||
last_receivertde=looptime
|
||||
|
||||
if rtde_r is not None:
|
||||
q=rtde_r.getActualQ()
|
||||
else:
|
||||
_offsets=[12.2,5.3,0,6,12.4,612.91]
|
||||
_factors=[1.2,1.14,1.214,0.9921,1,1.092]
|
||||
|
||||
for _i,_offset in enumerate(_offsets):
|
||||
q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i]
|
||||
|
||||
ur.receive(looptime)
|
||||
|
||||
#send data to all osc receivers
|
||||
oscsend.send(looptime,q)
|
||||
|
||||
if looptime-last_sendrtde>=1.0/frequency_sendrtde:
|
||||
last_sendrtde=looptime
|
||||
|
||||
for i in range(6): #update joint_q with max speeds
|
||||
joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -velocitylimit[i]/frequency_sendrtde,velocitylimit[i]/frequency_sendrtde) #constrain joint speeds
|
||||
|
||||
if rtde_c is not None:
|
||||
rtde_c.servoJ(joint_q, velocity, acceleration, 1.0/frequency_sendrtde, lookahead_time, gain)
|
||||
oscsend.send(looptime,ur.getReceivedQ())
|
||||
|
||||
if ur is not None:
|
||||
ur.send(looptime)
|
||||
|
||||
#Display
|
||||
ui.update(q,looptime)
|
||||
ui.update(ur.getReceivedQ(),looptime)
|
||||
|
||||
#Looptiming
|
||||
end = time.time()
|
||||
|
|
@ -156,7 +78,8 @@ def main(stdscr):
|
|||
|
||||
ui.setDebugtext(0,"loopcounter="+str(loopcounter))
|
||||
ui.setDebugtext(1,"loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)))
|
||||
ui.setDebugtext(2,"osc receive frequency="+str(round(1.0/np.mean(oscreceiveintervals),0))+" \t pos="+str(oscreceiveintervals_pos))
|
||||
if oscreceive is not None:
|
||||
ui.setDebugtext(2,"osc receive frequency="+str(round(oscreceive.getCurrentReceiveFrequency(),0)))
|
||||
|
||||
|
||||
|
||||
|
|
@ -190,6 +113,7 @@ if __name__ == "__main__":
|
|||
parser.add_argument("--test", action='store_true', default=False, help="Disable network and robot communication")
|
||||
|
||||
|
||||
'''
|
||||
parser.add_argument(
|
||||
"-fs",
|
||||
"--frequencysend",
|
||||
|
|
@ -207,29 +131,29 @@ if __name__ == "__main__":
|
|||
type=float,
|
||||
default=500.0,
|
||||
metavar="<frequency>")
|
||||
'''
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
frequency_receivertde=args.frequencyreceive
|
||||
frequency_sendrtde=args.frequencysend
|
||||
|
||||
|
||||
|
||||
|
||||
if args.test is not True:
|
||||
server,server_thread=setupOSC(args.listenip, args.listenport)
|
||||
ur.connect(args.robotip)
|
||||
|
||||
oscreceive = OSCReceive(args.listenip, args.listenport,ur)
|
||||
|
||||
oscsend.setupReceivers([args.clientip],args.port)
|
||||
|
||||
rtde_r=setupRTDE(args.robotip,frequency_rtde)
|
||||
|
||||
|
||||
|
||||
curses.wrapper(main)
|
||||
|
||||
if server is not None:
|
||||
input("Ended. Press Enter to shutdown")
|
||||
server.shutdown()
|
||||
server_thread.join()
|
||||
if rtde_c is not None:
|
||||
rtde_c.servoStop()
|
||||
rtde_c.stopScript()
|
||||
|
||||
if oscreceive is not None:
|
||||
oscreceive.disconnect()
|
||||
if ur is not None:
|
||||
ur.disconnect()
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue