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

Add open and close gripper functions

Browse Source
This commit is contained in:
Cole Deck 2024-04-12 22:05:34 -05:00
parent 145f51d08c
commit 939474378a
1 changed files with 39 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
ur5_control.py
View File

@ -10,7 +10,7 @@ import yaml
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import sys
from util import fprint
from pyModbusTCP.client import ModbusClient
@ -436,27 +436,49 @@ def pick_up_routine(robot, holder_index, verbose=False):
print('Pickup routine for index', holder_index)
goto_holder_index(robot, holder_index, 0.2, 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()
# Close Gripper code
time.sleep(0.5)
new_pos[2] = 0.2
rob.movel(new_pos, vel=0.1, 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)
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)
open_gripper()
# Back to safe position
# rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
def open_gripper():
print("opening")
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)
time.sleep(0.5)
c.close()
#c.close()
def close_gripper():
print("closing")
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)
time.sleep(0.5)
c.close()
#
if __name__ == "__main__":
@ -466,6 +488,15 @@ if __name__ == "__main__":
with open('config.yml', 'r') as fileread:
#global config
config = yaml.safe_load(fileread)
open_gripper()
#time.sleep(5)
#close_gripper()
#time.sleep(100)
robot = Rob(config) # robot of type Rob is the custom class above
robot.init_arm()
rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
@ -514,7 +545,7 @@ 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 [1,50]:
for i in [6,7,8]:
pick_up_routine(robot, i, verbose=True)