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

Make limb lengths and offsets global

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

20
ur5_control.py
View File

@ -14,7 +14,8 @@ from util import fprint
rob = None
offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
limb_base, limb1, limb2, limb3, limb_wrist = (0.105, .425, .39225, .1, .0997) # Limb lengths
def init(ip):
global rob
@ -225,10 +226,9 @@ def normalize_degree(theta):
def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
# Get limbs and offsets
offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
l_bs, l1, l2, l3, l_wt = (0.105, .425, .39225, .1, .0997) # Limb lengths
#l3=0.15
#l3=0.15
l_bs, l1, l2, l3, l_wt = (limb_base, limb1, limb2, limb3, limb_wrist) # Limb lengths
l3 += l3offset # add wrist offset, used for gripper angle calculations
# Calculate base angle and r relative to shoulder joint
@ -255,7 +255,7 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
base_theta = calculate_theta(x, y, l_bs)
cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
r = math.sqrt((x+cx)**2 + (y+cy)**2)
r = math.sqrt((x+offset_x+cx)**2 + (y+offset_y+cy)**2)
# Formulas to find out joint positions for (r, z)
@ -294,19 +294,19 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.2, flip=False
if flip:
gripperangle = -degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * 0.1
grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * 0.1
gripperx += (1-math.cos(gripperangle)) * limb3
else:
gripperangle = degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * 0.1
grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * 0.1
gripperx += (1-math.cos(gripperangle)) * limb3
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx)
def goto_holder_index(idx, z=0.05, gripperangle=40, flip=False):
def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
joint = config["position_map"][idx]
print("Going to cable holder index", joint["index"], "at position", joint["pos"])
angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)