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

Add basic kinematics for gripper angle

Browse Source
This commit is contained in:
Cole Deck 2024-03-17 19:50:04 -05:00
parent fb85a56d47
commit 2f28a01b7c
1 changed files with 101 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
ur5_control.py
View File

@ -190,6 +190,11 @@ def move_to_polar(start_pos, end_pos):
return rx_intermediate
def degtorad(angle):
return angle/180.0 * math.pi
def radtodeg(angle):
return angle*180.0 / math.pi
def move_to_home():
global rob
@ -216,37 +221,35 @@ def normalize_degree(theta):
# Return angle
return normalized_theta
# 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
elif (x<-a and y>=0) or (x<a and y<0):
flip = -1
else:
# Critical section (x=a, or x=-a). Infinite slope
# Return 0 or 180 depending on sign
return math.atan2(y, 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)):
# Calculate tangent line y = mx + b
m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
b = flip * a * math.sqrt(1+m*m)
# Calculate equivalent tangent point on circle
cx = (-flip*m*b)/(1+m*m)
cy = m*cx + flip*b
# Calculate base angle, make angle negative if flip=1
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, .09, .0997) # Limb lengths
l_bs, l1, l2, l3, l_wt = (0.105, .425, .39225, .1, .0997) # Limb lengths
#l3=0.15
# 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
elif (x<-a and y>=0) or (x<a and y<0):
flip = -1
else:
# Critical section (x=a, or x=-a). Infinite slope
# Return 0 or 180 depending on sign
return math.atan2(y, 0)
# Calculate tangent line y = mx + b
m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
b = flip * a * math.sqrt(1+m*m)
# Calculate equivalent tangent point on circle
cx = (-flip*m*b)/(1+m*m)
cy = m*cx + flip*b
# Calculate base angle, make angle negative if flip=1
theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
return theta
l3 += l3offset
base_theta = calculate_theta(x, y, l_bs)
cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
@ -264,28 +267,58 @@ 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
wrist1 += rx
# Return result
return base, shoulder, elbow, wrist1, ry, rz
def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz)
def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
# Joint offsets
# Base, Shoulder, Elbow, Wrist
inverse = [1, -1, 1, 1, 1, 1]
offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
if radtodeg(joints[1]) > 137:
print("CRASH! Shoulder at", joints[1] * 180/math.pi)
#else:
#print("Shoulder at", joints[1] * 180/math.pi)
# Return adjusted joint positions
return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
# gripper angle: from vertical
# gripper length: from joint to start of grip
# 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):
if flip:
gripperangle = -degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * 0.1
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * 0.1
else:
gripperangle = degtorad(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * 0.1
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * 0.1
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):
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)
#rob.movej(angles, acc=2, vel=2)
return angles
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
if __name__ == "__main__":
#rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
#rob.movel((x, y, z, rx, ry, rz), a, v)
init("192.168.1.145")
print("Current tool pose is: ", rob.getl())
move_to_home()
#move_to_home()
home_pose = [-0.4999999077032916,
-0.2000072960336574,
@ -338,21 +371,49 @@ if __name__ == "__main__":
# 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)
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)
rob.movej(goto_holder_index(26, 0.2, 0), acc=2, vel=2)
joints = []
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"][1]/1000, joint["pos"][0]/1000, 0)
joints.append(angles)
rob.movejs(joints, acc=2, vel=2)
for i in np.linspace(1, 85, 50):
joints.append(goto_holder_index(26, 0.2, i))
rob.movejs(joints, acc=0.4, vel=2)
rob.movej(goto_holder_index(24, 0.2, -10), 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(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.2, -40), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -45), acc=2, vel=2)
rob.movej(goto_holder_index(24, 0.2, -50), acc=2, vel=2)
#rob.movej(goto_holder_index(24, 0.2, 60, flip=True), acc=2, vel=2)
#time.sleep(2)
#rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)
#time.sleep(10)
# time.sleep(4)
# goto_holder_index(26, 0.1, 20)
# time.sleep(4)
# goto_holder_index(26, 0.1, 30)
# time.sleep(4)
# goto_holder_index(26, 0.1, 40)
# for joint in config["position_map"]:
#joint = config["position_map"][26]
#print("Going to cable holder index", joint["index"], "at position", joint["pos"])
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )# rx=math.pi / 5)
#joints.append(angles)
#rob.movej(angles, acc=2, vel=2)
#time.sleep(10)
#rob.movejs(joints, 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))