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

Add flip around mode to fit edge slots

Browse Source
This commit is contained in:
Cole Deck 2024-03-17 20:48:26 -05:00
parent 1338c3f440
commit 2ab1d0dbb3
1 changed files with 26 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ur5_control.py
View File

@ -290,18 +290,21 @@ def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset
# gripper angle: from vertical # gripper angle: from vertical
# gripper length: from joint to start of grip # gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative # 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): def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
if gripperangle < 0: if gripperangle < 0:
rz = - math.pi / 2 rz = - math.pi / 2
else: else:
rz = math.pi / 2 rz = math.pi / 2
if flip: if flip:
gripperangle = -degtorad(gripperangle) gripperangle = -degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * limb3 grippery += math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
gripperx += (1-math.cos(gripperangle)) * limb3 gripperx -= (1-math.cos(gripperangle)) * limb3
rz = - math.pi / 2 rz = - math.pi / 2
# flip the whole wrist
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + degtorad(180), l3offset=-gripperx, ry=math.pi/2, rz=rz)
else: else:
gripperangle = degtorad(gripperangle) gripperangle = degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength grippery = gripperlength - math.cos(gripperangle) * gripperlength
@ -309,7 +312,7 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.2, flip=False
gripperx = math.sin(gripperangle) * gripperlength gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * limb3 gripperx += (1-math.cos(gripperangle)) * limb3
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz) 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): def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
@ -394,15 +397,24 @@ if __name__ == "__main__":
#for i in np.linspace(0, 340, 340): #for i in np.linspace(0, 340, 340):
# joints.append(goto_holder_index(24, 0.5, i)) # joints.append(goto_holder_index(24, 0.5, i))
#rob.movejs(joints, acc=1, vel=3) #rob.movejs(joints, acc=1, vel=3)
angle = 30
rob.movej(goto_holder_index(26, 0.2, 0), acc=2, vel=2) rob.movej(goto_holder_index(26, 0.2, angle), acc=2, vel=2)
rob.movej(goto_holder_index(26, 0.2, 35), acc=2, vel=2) rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -10), acc=2, vel=2) rob.movej(goto_holder_index(38, 0.2, angle), 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(25, 0.2, angle, flip=True), 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.3, angle, flip=True), acc=2, vel=2)
#rob.movej(goto_holder_index(24, 0.2, 60, flip=True), acc=2, vel=2) rob.movej(goto_holder_index(24, 0.2, angle, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.0, angle, flip=True), acc=2, vel=2)
time.sleep(1)
rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(25, 0.1, angle), acc=2, vel=2)
rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(53, 0.1, angle, flip=True), acc=2, vel=2)
#time.sleep(2) #time.sleep(2)
#rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2) #rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)
#time.sleep(10) #time.sleep(10)