From bec0c6376367ae3fd80bdd59f42ffe07aecd4e83 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Sun, 17 Mar 2024 16:13:53 -0500 Subject: [PATCH] Fix ur5_control bugs, fully working IK!! Thanks Nadeem --- ur5_control.py | 39 +++++++++++++++++++++++++++++++++------ 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/ur5_control.py b/ur5_control.py index 0fdecee..b205d00 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -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)): # Get limbs and offsets 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 # 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 base_theta = calculate_theta(x, y, l_bs) - cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(l_bs) - r = math.sqrt((x-cx)**2 + (y-cy)**2) + cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta) + r = math.sqrt((x+cx)**2 + (y+cy)**2) # 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 base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]] - + rz = base # Return result return base, shoulder, elbow, wrist1, ry, rz @@ -321,9 +321,36 @@ if __name__ == "__main__": - angles = get_joints_from_xyz_abs(-0.7, 0, 0) - rob.movej(angles, acc=2, vel=2) + # angles = get_joints_from_xyz_abs(-0.2, 0, 0) + # 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 = [] # for i in np.linspace(-0.3, -0.7, 50): # joints.append(get_joints_from_xyz_abs(i, 0, 0))