Jukebox/jukebox-software
10
0
Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Add real-time UR5 position updates (hopefully)

Browse Source
This commit is contained in:
Cole Deck
2024-05-09 18:18:55 -05:00
parent f416e25c1c
commit be51423c89
7 changed files with 109 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
ur5_control.py
View File

@@ -80,7 +80,7 @@ def connect(robot):
while trying and count < 10:
count += 1
try:
robot.robot = urx.Robot(ip)
robot.robot = urx.Robot(ip, use_rt=True)
robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0))
# Set weight
robot.robot.set_payload(2, (0, 0, 0.1))
@@ -704,12 +704,36 @@ def close_gripper():
def get_position_thread(robot, pos_updates):
robot = connect(robot)
rob = robot.robot
oldvals = rob.getl_rt()
deltavals = [0,0,0]
import uptime
t = 0.01
count = 0
while True:
start = uptime.uptime()
if pos_updates.qsize() < 2:
vals = rob.getl()
pos_updates.put(tuple(vals))
#print("Adding position to queue")
time.sleep(0.01)
vals = rob.getl_rt()
if vals != oldvals:
pos_updates.put(tuple(oldvals))
#time.sleep(0.01)
# deltavals = list()
# deltavals.append(vals[0]-oldvals[0])
# deltavals.append(vals[1]-oldvals[1])
# deltavals.append(vals[2]-oldvals[2])
# count = 0
oldvals = vals
# else:
# count += 0.2
# if count < 1:
# tmpvals = vals
# tmpvals[0] = oldvals[0] + deltavals[0]*count
# tmpvals[1] = oldvals[1] + deltavals[1]*count
# tmpvals[2] = oldvals[2] + deltavals[2]*count
# pos_updates.put(tuple(tmpvals))
while start + t > uptime.uptime():
time.sleep(0.0001)
if __name__ == "__main__":