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

Added tool z rotation for angled gripper

Browse Source
This commit is contained in:
Cole Deck 2024-03-17 20:07:25 -05:00
parent 83b077b4df
commit 1338c3f440
1 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ur5_control.py
View File

@ -291,19 +291,25 @@ def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset
# gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative
def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.2, flip=False):
if gripperangle < 0:
rz = - math.pi / 2
else:
rz = math.pi / 2
if flip:
gripperangle = -degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * limb3
rz = - math.pi / 2
else:
gripperangle = degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * limb3
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx)
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz)
def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
@ -382,21 +388,20 @@ if __name__ == "__main__":
#global config
config = yaml.safe_load(fileread)
rob.movej(goto_holder_index(26, 0.2, 0), acc=2, vel=2)
joints = []
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
#joints = []
for i in np.linspace(1, 85, 50):
joints.append(goto_holder_index(26, 0.2, i))
rob.movejs(joints, acc=0.4, vel=2)
#for i in np.linspace(0, 340, 340):
# joints.append(goto_holder_index(24, 0.5, i))
#rob.movejs(joints, acc=1, vel=3)
rob.movej(goto_holder_index(26, 0.2, 0), acc=2, vel=2)
rob.movej(goto_holder_index(26, 0.2, 35), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -10), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -15), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -20), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -28), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -35), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -40), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -45), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -50), acc=2, vel=2)
#rob.movej(goto_holder_index(24, 0.2, 60, flip=True), acc=2, vel=2)
#time.sleep(2)
#rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)