philipp.kramer/urcontrol
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

add osc receive and send functions

Browse source
This commit is contained in:
Philipp Kramer 2024-05-09 19:17:11 +02:00
parent 0fc974b6d3
commit fe9cf333f9
1 changed files with 76 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

93
urosc.py
View file

@ -8,7 +8,8 @@ import math
import numpy as np
import curses
from rtde_receive import RTDEReceiveInterface as RTDEReceive
import rtde_receive
import rtde_control
@ -16,13 +17,30 @@ from rtde_receive import RTDEReceiveInterface as RTDEReceive
clients = []
rtde_r=None
rtde_c=None
server=None
#Looptimings
frequency_loop=500
frequency_oscsend=50
frequency_rtde=None
frequency_scr=30
frequency_scr=25
#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 toRad(d):
return d/360*2*math.pi
@ -35,15 +53,25 @@ def mapFromTo(x,a,b,c,d):
y=(x-a)/(b-a)*(d-c)+c
return y
def handle_received_osc(address, *args):
def constrain(v,_min,_max):
return min(_max,max(_min,v))
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 arg in args:
print(f"Data: {arg}")
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("/filter", handle_received_osc) # You can change '/filter' to any address pattern you expect
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)
@ -67,7 +95,9 @@ def setupClients(clientips,port):
def setupRTDE(robotip,frequency):
dt = 1 / frequency
rtde_r = RTDEReceive(robotip, frequency)
rtde_r = rtde_receive.RTDEReceiveInterface(robotip, frequency)
rtde_c = rtde_control.RTDEControlInterface(robotip)
return rtde_r
@ -109,19 +139,21 @@ def main(stdscr):
last_sendosc=0
last_receivertde=0
last_sendrtde=0
last_updatescr=0
q=[0,0,0,0,0,0]
try:
while key != ord('q'):
looptime=time.time()
looptime = time.time()
start = time.time()
#Get robot joint angled
if looptime-last_receivertde>=1.0/frequency_rtde:
if looptime-last_receivertde>=1.0/frequency_receivertde:
last_receivertde=looptime
if rtde_r is not None:
@ -141,6 +173,15 @@ def main(stdscr):
for jointi in range(6):
c.send_message("/j"+str(jointi), toDeg(q[jointi]))
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)
#Display
if looptime-last_updatescr>=1.0/frequency_scr:
@ -148,8 +189,9 @@ def main(stdscr):
key = stdscr.getch()
stdscr.clear()
stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(3))
stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(1))
stdscr.addstr(1, 0, "loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)), curses.color_pair(1))
stdscr.addstr(2, 0, "osc receive frequency="+str(round(1.0/np.mean(oscreceiveintervals),0))+" \t pos="+str(oscreceiveintervals_pos), curses.color_pair(1))
startbarsx=10
endbarsx=width-1
@ -170,11 +212,11 @@ def main(stdscr):
poschar="]"
if (len(poschar)>0):
stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
stdscr.addstr(3+j, px, poschar, curses.color_pair(1))
valuetext=str(round(toDeg(q[j]),2))
if q[j]>=0: #has no negative sign
valuetext=" "+valuetext
stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1))
stdscr.addstr(3+j, 0, valuetext, curses.color_pair(1))
# Refresh the screen
@ -187,6 +229,9 @@ def main(stdscr):
loopdurations_pos+=1
loopdurations_pos%=np.size(loopdurations)
if loopduration < loop_dt:
time.sleep(loop_dt - loopduration)
loopcounter += 1
@ -217,16 +262,27 @@ if __name__ == "__main__":
parser.add_argument(
"-f",
"--frequency",
dest="frequency",
help="the frequency at which the data is recorded (default is 500Hz)",
"-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()
frequency_rtde=args.frequency
frequency_receivertde=args.frequencyreceive
frequency_sendrtde=args.frequencysend
clientips=[args.clientip]
@ -245,4 +301,7 @@ if __name__ == "__main__":
input("Ended. Press Enter to shutdown")
server.shutdown()
server_thread.join()
if rtde_c is not None:
rtde_c.servoStop()
rtde_c.stopScript()