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

Add more routines

Browse Source
This commit is contained in:
Cole Deck 2024-04-18 20:33:55 -05:00
parent 939474378a
commit a698c4a753
1 changed files with 257 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

324
ur5_control.py
View File

@ -5,13 +5,14 @@ import math
import numpy as np
import time
import os
import logging
#import logging
import yaml
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import sys
from util import fprint
from pyModbusTCP.client import ModbusClient
import subprocess
from util import win32
@ -33,37 +34,76 @@ class Rob():
self.limb_wrist = limbs["limb_wrist"]
#self.init_arm()
def init_arm(self):
#sys.stdout = Logger()
fprint("Starting UR5 power up...")
def ping(host):
#Returns True if host (str) responds to a ping request.
# power up robot here
# Option for the number of packets as a function of
if win32:
param1 = '-n'
param2 = '-w'
param3 = '250'
else:
param1 = '-c'
param2 = '-W'
param3 = '0.25'
# wait for power up (this function runs async)
# Building the command. Ex: "ping -c 1 google.com"
command = ['ping', param1, '1', param2, param3, host]
return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0
# trigger auto-initialize
def powerup_arm(robot):
#sys.stdout = Logger()
fprint("Starting UR5 power up...")
# power up robot here via PCB
# wait for auto-initialize
ip = self.ip
# init urx
#
# wait for power up (this function runs async)
while not ping(robot.ip):
time.sleep(0.5)
# trigger auto-initialize
fprint("Arm online. Waiting for calibration.")
# wait for auto-initialize
def connect(robot):
if robot.robot is None:
newrobot = Rob(robot.config)
robot = newrobot
ip = robot.ip
fprint("Connecting to arm at " + ip)
trying = True
while trying:
try:
self.robot = urx.Robot(ip)
robot.robot = urx.Robot(ip)
trying = False
except:
time.sleep(1)
# Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0))
robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0))
# Set weight
self.robot.set_payload(2, (0, 0, 0.1))
#rob.set_payload(2, (0, 0, 0.1))
time.sleep(0.2)
fprint("UR5 ready.")
robot.robot.set_payload(2, (0, 0, 0.1))
return robot
def init_arm(robot):
robot = connect(robot)
# init urx
#rob.set_payload(2, (0, 0, 0.1))
time.sleep(0.2)
fprint("UR5 ready.")
#return robot.robot
# setup - in case of fail. open gripper, move up, then go home.
rob = robot.robot
open_gripper()
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] += 0.025
rob.movel(new_pos, vel=0.05, acc=1)
move_to_home(robot, speed=0.5)
return True
def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None):
rob = robot.robot
@ -205,7 +245,7 @@ def move_to_polar(robot, start_pos, end_pos):
return rx_intermediate
def move_to_home(robot):
def move_to_home(robot, speed=2):
rob = robot.robot
# Home position in degrees
@ -217,7 +257,25 @@ def move_to_home(robot):
1.675098295930943]
# Move robot
rob.movej(home_pos, acc=2, vel=2)
rob.movej(home_pos, acc=2, vel=speed)
return True
def move_to_packup(robot, speed=0.25):
rob = robot.robot
# known good starting point to reach store position
goto_holder_index(robot, 12, 0.3, flip=False, use_closest_path=False)
# Home position in degrees
store_pos = [-1.5708,
-1.3,
2.362,
0.7056,
-1.425,
1.5708]
# Move robot
rob.movej(store_pos, acc=0.1, vel=speed)
return True
def normalize_degree(theta):
# Normalizes degree theta from -1.5pi to 1.5pi
@ -346,7 +404,7 @@ def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
rob.movejs(joints, acc=2, vel=2, radius=0.1)
def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True):
def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True, rzoffset=0):
# gripper angle: from vertical
# gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative
@ -365,7 +423,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.0
gripperx -= (1-math.cos(gripperangle)) * limb3
rz = math.pi / 2
# flip the whole wrist
return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz, use_closest_path=use_closest_path)
return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path)
else:
gripperangle = math.radians(gripperangle)
@ -387,6 +445,7 @@ def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_close
#rob.movej(angles, acc=2, vel=2)
#return angles
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
return True
def is_flipped(robot):
rob = robot.robot
@ -415,8 +474,9 @@ def flip(robot):
safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
# Flip at safe position
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=4, acc=3) # Move to safe position
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=4, acc=3) # Flip gripper
return True
def safe_move(robot, x, y, z, use_closest_path=True):
rob = robot.robot
@ -427,15 +487,17 @@ def safe_move(robot, x, y, z, use_closest_path=True):
if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)):
flip(robot)
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=4, acc=3)
return True
def pick_up_routine(robot, holder_index, verbose=False):
def pick_up_routine(robot, holder_index, slot=None, verbose=False):
robot = connect(robot)
rob = robot.robot
if verbose:
print('Pickup routine for index', holder_index)
fprint('Pickup routine for index' + str(holder_index))
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
open_gripper()
curr_pos = rob.getl()
@ -448,22 +510,153 @@ def pick_up_routine(robot, holder_index, verbose=False):
new_pos[2] = 0.2
rob.movel(new_pos, vel=0.1, acc=1)
rob.movel(new_pos, vel=2, acc=1)
was_flipped = is_flipped(robot)
# # Tray position 1
rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
time.sleep(0.5)
#move_to_home(robot, speed=4)
# starting pos
angles = (-47.04,-115.81,138.33,-58.11,-56.55,-68.03)
angles = [x*math.pi/180 for x in angles]
rob.movej(angles,vel=1,acc=0.5)
angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39)
angles = [x*math.pi/180 for x in angles]
rob.movej(angles,vel=1,acc=0.5)
if slot is None:
slot = 0
if slot == 0:
angles = (7.09,-64.83,147.87,-188.00,-29.84,17.15)
angles = [x*math.pi/180 for x in angles]
if slot == 1:
angles = (8.43,-64.63,154.74,-197.30,-30.21,19.72)
angles = [x*math.pi/180 for x in angles]
if slot == 2:
angles = (10.39,-70.86,133.95,7.83,30.91,-159.00)
angles = [x*math.pi/180 for x in angles]
if slot == 3:
angles = (13.31,-76.23,140.78,1.90,31.98,-153.76)
angles = [x*math.pi/180 for x in angles]
rob.movej(angles,vel=1,acc=0.5)
open_gripper()
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] += 0.04
rob.movel(new_pos, vel=0.1, acc=1)
angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39)
angles = [x*math.pi/180 for x in angles]
rob.movej(angles,vel=1,acc=0.5)
move_to_home(robot, speed=3)
# # Tray position 1
# if slot is None:
# rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
# rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
# open_gripper()
# else:
# xoffset = 0.051 * slot
# rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
# rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=60, flip=was_flipped, use_closest_path=False,rzoffset=math.pi), vel=4, acc=3)
# #rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0, math.pi/2, 0.1), vel=4, acc=3)
# open_gripper()
# Back to safe position
rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
#time.sleep(10)
#rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=4, acc=3)
return True
def return_routine(robot, slot, holder_index=None, verbose=False):
robot = connect(robot)
rob = robot.robot
open_gripper()
was_flipped = is_flipped(robot)
if slot is None:
rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
close_gripper()
else:
xoffset = 0.051 * slot
rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
close_gripper()
if holder_index is not None:
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0.015
rob.movel(new_pos, vel=0.1, acc=1)
open_gripper()
new_pos[2] = 0.1
rob.movel(new_pos, vel=2, acc=1)
return True
else:
# go to camera
rob.movej(offset_gripper_angle(robot, 0.35, -0.35, 0.3, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
return True
def to_camera(robot, holder_index, verbose=False):
robot = connect(robot)
fprint("Bringing tube at " + str(holder_index) + " to camera")
rob = robot.robot
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
open_gripper()
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0.005
rob.movel(new_pos, vel=0.1, acc=1)
# goto_holder_index(robot, holder_index, 0.0)
close_gripper()
new_pos[2] = 0.2
rob.movel(new_pos, vel=2, acc=1)
was_flipped = is_flipped(robot)
# start inwards
rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
# go to camera
rob.movej(offset_gripper_angle(robot, 0.3, -0.2, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
return True
def return_camera(robot, holder_index, verbose=False):
robot = connect(robot)
rob = robot.robot
# move inwards first
rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0.015
rob.movel(new_pos, vel=0.1, acc=1)
open_gripper()
new_pos[2] = 0.1
rob.movel(new_pos, vel=2, acc=1)
def open_gripper():
print("opening")
fprint("Opening gripper")
c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
c.write_single_register(0, 0b0)
c.write_single_register(435, 0b1)
c.write_single_register(0, 0b0)
c.write_single_register(435, 0b1)
time.sleep(0.5)
@ -472,8 +665,11 @@ def open_gripper():
#c.close()
def close_gripper():
print("closing")
fprint("Closing gripper")
c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
c.write_single_register(435, 0b0)
c.write_single_register(0, 0b1)
c.write_single_register(435, 0b0)
c.write_single_register(0, 0b1)
time.sleep(0.5)
@ -489,44 +685,34 @@ if __name__ == "__main__":
#global config
config = yaml.safe_load(fileread)
open_gripper()
#time.sleep(5)
#open_gripper()
#time.sleep(1)
#close_gripper()
#time.sleep(100)
robot = Rob(config) # robot of type Rob is the custom class above
robot.init_arm()
#powerup_arm(robot)
robot = connect(robot)
init_arm(robot)
rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
move_to_home(robot, speed=4)
#time.sleep(3)
#move_to_packup(robot)
pick_up_routine(robot, 0, 0, True)
#move_to_home(robot)
#return_routine(robot, 0, 0, True)
time.sleep(100)
print("Current tool pose is: ", rob.getl())
move_to_home(robot)
home_pose = [-0.4999999077032916,
-0.2000072960336574,
0.40002172976662786,
0,
-3.14152741295329,
math.radians(62)]
# time.sleep(.5)
p1 = [0,
0.6,
.4,
0.2226,
3.1126,
0.0510]
p2 = [0.171,
-0.115,
0.2,
0.2226,
3.1126,
0.0510]
curr_pos = rob.getl()
@ -545,8 +731,8 @@ if __name__ == "__main__":
# rob.movej(get_joints_from_xyz_abs(robot, 0.2, 0, 0.05), vel=0.5, acc=2)
# goto_holder_index(robot, 27, 0.05)
for i in [6,7,8]:
pick_up_routine(robot, i, verbose=True)
#for i in [6,7,8]:
#pick_up_routine(robot, i, verbose=True)