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

Re-merge calculate_theta into get_joints_from_xyz_rel

Browse Source
This commit is contained in:
Cole Deck 2024-03-17 19:53:49 -05:00
parent 2f28a01b7c
commit f16242f5be
1 changed files with 24 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ur5_control.py
View File

@ -221,8 +221,18 @@ def normalize_degree(theta):
# Return angle
return normalized_theta
# Calculate base angle and r relative to shoulder joint
def calculate_theta(x, y, a):
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 += l3offset # add wrist offset, used for gripper angle calculations
# Calculate base angle and r relative to shoulder joint
def calculate_theta(x, y, a):
# Calculate if we need the + or - in our equations
if (x>-a and y>=0) or (x>a and y<0):
flip = 1
@ -243,14 +253,6 @@ def calculate_theta(x, y, a):
theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
return 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 += l3offset
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)