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

Fix ur5_control bugs, fully working IK!! Thanks Nadeem

Browse Source
This commit is contained in:
Cole Deck 2024-03-17 16:13:53 -05:00
parent 4ae30b82a0
commit bec0c63763
1 changed files with 33 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ur5_control.py
View File

@ -220,7 +220,7 @@ 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)): 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)):
# Get limbs and offsets # Get limbs and offsets
offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .0997) # Limb lengths l_bs, l1, l2, l3, l_wt = (0.105, .425, .39225, .09, .0997) # Limb lengths
#l3=0.15 #l3=0.15
# Calculate base angle and r relative to shoulder joint # Calculate base angle and r relative to shoulder joint
@ -249,8 +249,8 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
return theta return theta
base_theta = calculate_theta(x, y, l_bs) base_theta = calculate_theta(x, y, l_bs)
cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(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+cx)**2 + (y+cy)**2)
# Formulas to find out joint positions for (r, z) # Formulas to find out joint positions for (r, z)
@ -264,7 +264,7 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
# Normalize angles # Normalize angles
base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]] base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
rz = base
# Return result # Return result
return base, shoulder, elbow, wrist1, ry, rz return base, shoulder, elbow, wrist1, ry, rz
@ -321,9 +321,36 @@ if __name__ == "__main__":
angles = get_joints_from_xyz_abs(-0.7, 0, 0) # angles = get_joints_from_xyz_abs(-0.2, 0, 0)
rob.movej(angles, acc=2, vel=2) # rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(-0.2, -0.2, 0)
# rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(0, -0.6, 0)
# rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(0, -0.5, 0)
# rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(0, -0.4, 0)
# rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(0, -0.3, 0)
# rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(0, -0.2, 0)
# rob.movej(angles, acc=2, vel=2)
# angles = get_joints_from_xyz_abs(0, -0.13, 0)
# rob.movej(angles, acc=2, vel=2)
config = None
# joints = []
# for i in np.linspace(-0.2, -0.7, 50):
# joints.append(get_joints_from_xyz_abs(i, 0, 0))
# rob.movejs(joints, acc=2, vel=2)
import yaml
with open('config.yml', 'r') as fileread:
#global config
config = yaml.safe_load(fileread)
for joint in config["position_map"]:
print("Going to cable holder index", joint["index"], "at position", joint["pos"])
angles = get_joints_from_xyz_abs(joint["pos"][0]/1000, joint["pos"][1]/1000, 0)
rob.movej(angles, acc=2, vel=2)
# joints = [] # joints = []
# for i in np.linspace(-0.3, -0.7, 50): # for i in np.linspace(-0.3, -0.7, 50):
# joints.append(get_joints_from_xyz_abs(i, 0, 0)) # joints.append(get_joints_from_xyz_abs(i, 0, 0))