implement enable disable control via keyboard
This commit is contained in:
parent
1cd7514ea0
commit
7899b73f40
3 changed files with 93 additions and 38 deletions
|
|
@ -1,4 +1,4 @@
|
|||
#from helpfunctions import *
|
||||
from helpfunctions import *
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
|
|
@ -10,7 +10,8 @@ class OSCReceive:
|
|||
|
||||
|
||||
|
||||
def handle_received_osc_joint(self,address, *args):
|
||||
def handle_received_osc_joint(self,address, *args):
|
||||
|
||||
|
||||
self.server=None
|
||||
self.server_thread=None
|
||||
|
|
@ -25,7 +26,7 @@ class OSCReceive:
|
|||
try:
|
||||
for iarg, arg in enumerate(args):
|
||||
#print(f"Data: {arg}")
|
||||
joint_q_aim[iarg]=float(arg)
|
||||
joint_q_aim[iarg]=toRad(float(arg))
|
||||
except:
|
||||
print("Data not in right format")
|
||||
print("Data: "+str(args))
|
||||
|
|
@ -36,7 +37,11 @@ class OSCReceive:
|
|||
|
||||
|
||||
def getCurrentReceiveFrequency(self):
|
||||
return 1.0/np.mean(self.oscreceiveintervals)
|
||||
_mean=np.mean(self.oscreceiveintervals)
|
||||
if _mean>0:
|
||||
return 1.0/_mean
|
||||
else:
|
||||
return 0
|
||||
|
||||
def __init__(self,ip,port,_ur=None):
|
||||
|
||||
|
|
@ -50,7 +55,7 @@ class OSCReceive:
|
|||
|
||||
# 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
|
||||
disp.map("/joints", 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)
|
||||
|
|
@ -61,5 +66,7 @@ class OSCReceive:
|
|||
|
||||
|
||||
def disconnect(self):
|
||||
self.server.shutdown()
|
||||
self.server_thread.join()
|
||||
if self.server is not None:
|
||||
self.server.shutdown()
|
||||
if self.server_thread is not None:
|
||||
self.server_thread.join()
|
||||
|
|
|
|||
77
urosc/ur.py
77
urosc/ur.py
|
|
@ -10,6 +10,7 @@ class UR:
|
|||
self.frequency_rtde_send=_frequency_rtde_send
|
||||
self.frequency_rtde_receive=_frequency_rtde_receive
|
||||
|
||||
self.controlEnabled=False
|
||||
|
||||
self.last_receivertde=0
|
||||
self.last_sendrtde=0
|
||||
|
|
@ -27,48 +28,85 @@ class UR:
|
|||
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 = [0,0,0,0,0,0] #joint rotation last sent
|
||||
#self.velocitylimit=[1,1,1,3,3,3] #joint velocity limits
|
||||
self.velocitylimit=[0.2,0.2,0.2,0.2,0.2,0.2] #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)
|
||||
self.rtde_r = self.rtde_receive.RTDEReceiveInterface(robotip, self.frequency_rtde_receive)
|
||||
|
||||
rtde_c = self.rtde_control.RTDEControlInterface(robotip)
|
||||
self.rtde_c = self.rtde_control.RTDEControlInterface(robotip)
|
||||
|
||||
def setJointQAim(self,q):
|
||||
self.joint_q_aim=q
|
||||
|
||||
def isJointsWithinSafetyLimits(self,q):
|
||||
return self.rtde_c.isJointsWithinSafetyLimits(q)
|
||||
|
||||
def enableControl(self):
|
||||
if time.time()-self.last_receivertde<0.1: #if received position is recent
|
||||
self.joint_q=self.joint_q_received #update current position to received position
|
||||
self.joint_q_aim=self.joint_q_received
|
||||
self.controlEnabled=True
|
||||
def disableControl(self):
|
||||
self.controlEnabled=False
|
||||
|
||||
def isControlEnabled(self):
|
||||
return self.controlEnabled
|
||||
|
||||
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.controlEnabled:
|
||||
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)
|
||||
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):
|
||||
def receiveJoints(self,looptime):
|
||||
|
||||
#Get robot joint angles
|
||||
if looptime-self.last_receivertde>=1.0/self.frequency_rtde_receive:
|
||||
self.last_receivertde=looptime
|
||||
if looptime-self.last_receivertde<1.0/self.frequency_rtde_receive:
|
||||
return
|
||||
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]
|
||||
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 receiveStatus(self,looptime):
|
||||
|
||||
if looptime-self.last_receivertde<1.0/self.frequency_rtde_receive:
|
||||
return
|
||||
self.last_receivertde=looptime
|
||||
|
||||
self.status_connected=self.rtde_r.isConnected()
|
||||
self.status_timestamp=self.rtde_r.getTimestamp()
|
||||
self.status_protectivestopped=self.rtde_r.isProtectiveStopped()
|
||||
self.status_emergencystopped=self.rtde_r.isEmergencyStopped()
|
||||
self.status_robotstatus=self.rtde_c.getRobotStatus()
|
||||
'''
|
||||
self.status_=self.rtde_r.()
|
||||
self.status_=self.rtde_r.()
|
||||
self.status_=self.rtde_r.()
|
||||
self.status_=self.rtde_r.()
|
||||
self.status_=self.rtde_r.()
|
||||
self.status_=self.rtde_r.()
|
||||
self.status_=self.rtde_r.()
|
||||
'''
|
||||
|
||||
def getReceivedQ(self):
|
||||
return self.joint_q_received
|
||||
|
|
@ -78,4 +116,5 @@ class UR:
|
|||
self.rtde_c.servoStop()
|
||||
self.rtde_c.stopScript()
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@ from oscreceive import OSCReceive
|
|||
from ur import UR
|
||||
|
||||
|
||||
|
||||
#Looptimings
|
||||
frequency_loop=500
|
||||
|
||||
|
|
@ -50,7 +49,8 @@ def main(stdscr):
|
|||
loopdurations = np.zeros(100)
|
||||
loopdurations_pos=0
|
||||
|
||||
|
||||
|
||||
keyDown=False
|
||||
|
||||
try:
|
||||
while ui.getKey() != ord('q'):
|
||||
|
|
@ -58,7 +58,8 @@ def main(stdscr):
|
|||
|
||||
start = time.time()
|
||||
|
||||
ur.receive(looptime)
|
||||
ur.receiveJoints(looptime)
|
||||
ur.receiveStatus(looptime)
|
||||
|
||||
#send data to all osc receivers
|
||||
oscsend.send(looptime,ur.getReceivedQ())
|
||||
|
|
@ -76,11 +77,20 @@ def main(stdscr):
|
|||
loopdurations_pos+=1
|
||||
loopdurations_pos%=np.size(loopdurations)
|
||||
|
||||
ui.setDebugtext(0,"loopcounter="+str(loopcounter))
|
||||
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)))
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -88,9 +98,10 @@ def main(stdscr):
|
|||
time.sleep(loop_dt - loopduration)
|
||||
loopcounter += 1
|
||||
|
||||
except KeyboardInterrupt:
|
||||
|
||||
except (KeyboardInterrupt):
|
||||
print("\nKeyboardInterrupt")
|
||||
except Exception as e:
|
||||
pass
|
||||
curses.endwin()
|
||||
|
||||
|
||||
|
|
@ -101,9 +112,9 @@ if __name__ == "__main__":
|
|||
parser.add_argument("--listenport",
|
||||
type=int, default=5005, help="The port to listen on")
|
||||
|
||||
parser.add_argument("--clientip",
|
||||
parser.add_argument("--oscsendip",
|
||||
default="127.0.0.1", help="The ip to connect to")
|
||||
parser.add_argument("--port",
|
||||
parser.add_argument("--oscsendport",
|
||||
type=int, default=5005, help="The port to connect to")
|
||||
|
||||
|
||||
|
|
@ -144,14 +155,12 @@ if __name__ == "__main__":
|
|||
|
||||
oscreceive = OSCReceive(args.listenip, args.listenport,ur)
|
||||
|
||||
oscsend.setupReceivers([args.clientip],args.port)
|
||||
oscsend.setupReceivers([args.oscsendip],args.oscsendport)
|
||||
|
||||
|
||||
|
||||
|
||||
curses.wrapper(main)
|
||||
|
||||
|
||||
if oscreceive is not None:
|
||||
oscreceive.disconnect()
|
||||
if ur is not None:
|
||||
|
|
|
|||
Loading…
Reference in a new issue