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

Basic move restrictions and flip routine

Browse Source
This commit is contained in:
BlueOceanWave 2024-03-23 15:47:10 -05:00
parent 218303e92b
commit 2ec7906ee4
2 changed files with 207 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
inv_kin_testing.ipynb
View File

File diff suppressed because one or more lines are too long

166
ur5_control.py
View File

@ -6,6 +6,7 @@ import numpy as np
import time import time
import os import os
import logging import logging
import yaml
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import sys import sys
from util import fprint from util import fprint
@ -191,11 +192,6 @@ def move_to_polar(start_pos, end_pos):
return rx_intermediate 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(): def move_to_home():
global rob global rob
@ -222,8 +218,6 @@ def normalize_degree(theta):
# Return angle # Return angle
return normalized_theta return normalized_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): 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 # Get limbs and offsets
@ -276,37 +270,61 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): 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) joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
# Return current positions if coordinates don't make sense
if z<0:
return rob.getj()
# Joint offsets # Joint offsets
# Base, Shoulder, Elbow, Wrist # Base, Shoulder, Elbow, Wrist
inverse = [1, -1, 1, 1, 1, 1] inverse = [1, -1, 1, 1, 1, 1]
offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0] offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
if radtodeg(joints[1]) > 137:
if math.degrees(joints[1]) > 137:
print("CRASH! Shoulder at", joints[1] * 180/math.pi) print("CRASH! Shoulder at", joints[1] * 180/math.pi)
#else: #else:
#print("Shoulder at", joints[1] * 180/math.pi) #print("Shoulder at", joints[1] * 180/math.pi)
# Return adjusted joint positions # Return adjusted joint positions
return [o+j*i for j, o, i in zip(joints, offsets, inverse)] return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
# gripper angle: from vertical def move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
# gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative global rob
start_joints = rob.getj()
end_joint = get_joints_from_xyz_abs(x, y, z, rx, ry, rz)
n_points = 50
intermediate_joints = []
for i in range(0, 6):
intermediate_joints.append(np.linspace(start_joints[i], end_joint[i], n_points))
joints = [joint_position for joint_position in zip(*intermediate_joints)]
rob.movejs(joints, acc=2, vel=2, radius=0.1)
def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False): def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
# gripper angle: from vertical
# gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative
# Determine tool rotation depending on gripper angle
if gripperangle < 0: if gripperangle < 0:
rz = - math.pi / 2 rz = - math.pi / 2
else: else:
rz = math.pi / 2 rz = math.pi / 2
if flip: if flip:
gripperangle = -degtorad(gripperangle) gripperangle = -math.radians(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery += math.sin(gripperangle) * limb3 grippery += math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2 gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
gripperx -= (1-math.cos(gripperangle)) * limb3 gripperx -= (1-math.cos(gripperangle)) * limb3
rz = - math.pi / 2 rz = - math.pi / 2
# flip the whole wrist # flip the whole wrist
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + degtorad(180), l3offset=-gripperx, ry=math.pi/2, rz=rz) return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz)
else: else:
gripperangle = degtorad(gripperangle) gripperangle = math.radians(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * limb3 grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength gripperx = math.sin(gripperangle) * gripperlength
@ -314,15 +332,59 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, fli
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz) return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz)
def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False): def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
joint = config["position_map"][idx] joint = config["position_map"][idx]
print("Going to cable holder index", joint["index"], "at position", joint["pos"]) 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)
safe_move(joint["pos"][1]/1000, joint["pos"][0]/1000, z)
#angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
#rob.movej(angles, acc=2, vel=2) #rob.movej(angles, acc=2, vel=2)
return angles #return angles
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, ) #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
def is_flipped():
global rob
wrist2 = rob.getj()[4]
if wrist2>0:
return True
else:
return False
def flip():
global rob
# A list of safe positions to flip
safe_positions = [(-0.205, -0.108, 0.3),
(0.205, -0.108, 0.3)]
# Find the closest safe position
curr_pos = rob.getl()[:3]
def dist_from_robot(pos):
x, y, z = pos
rx, ry, rz = curr_pos
return math.sqrt((rx-x)**2+(ry-y)**2+(rz-z)**2)
pos_dist_pairs = zip(safe_positions, [dist_from_robot(pos) for pos in safe_positions])
safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
# Flip at safe position
rob.movej(offset_gripper_angle(*safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position
rob.movej(offset_gripper_angle(*safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
def safe_move(x, y, z):
flip_radius = 0.17 # Min radius on which to flip
r = math.sqrt(x**2 + y**2) # Get position radius
# Flip gripper if needed
if (r <= flip_radius and is_flipped()) or (r > flip_radius and not is_flipped()):
flip()
global rob
rob.movej(offset_gripper_angle(x, y, z, flip=is_flipped()), vel=2, acc=2)
if __name__ == "__main__": if __name__ == "__main__":
#rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2) #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
@ -356,59 +418,55 @@ if __name__ == "__main__":
0.0510] 0.0510]
curr_pos = rob.getl() curr_pos = rob.getl()
# up/down,
# tool rotation
# tool angle (shouldn't need)
# rob.set_pos(p1[0:3], acc=0.5, vel=0.5)
# set_pos_abs(*home_pose)
# 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 config = None
joints = [] joints = []
for i in np.linspace(-0.2, -0.7, 50): for i in np.linspace(-0.2, -0.7, 10):
joints.append(get_joints_from_xyz_abs(i, 0, 0)) joints.append(get_joints_from_xyz_abs(i, 0, 0))
#rob.movejs(joints, acc=2, vel=2) # rob.movejs(joints, acc=2, vel=2, radius=0.1)
import yaml
with open('config.yml', 'r') as fileread: with open('config.yml', 'r') as fileread:
#global config #global config
config = yaml.safe_load(fileread) config = yaml.safe_load(fileread)
# move_arc(0, 0.3, 0.1)
# move_arc(0, -0.3, 0.3)
# for i in range(20):
# goto_holder_index(i, 0.1)
flip()
flip()
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2) #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
#joints = [] #joints = []
#for i in np.linspace(0, 340, 340): #for i in np.linspace(0, 340, 340):
# joints.append(goto_holder_index(24, 0.5, i)) # joints.append(goto_holder_index(24, 0.5, i))
#rob.movejs(joints, acc=1, vel=3) #rob.movejs(joints, acc=1, vel=3)
angle = 30 # angle = 30
rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2) # rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2)
#rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2) # # rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2)
#rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2) # rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2)
rob.movej(goto_holder_index(26, 0.1, angle, flip=True), acc=2, vel=2) # rob.movej(goto_holder_index(26, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(offset_gripper_angle(-0.3, -0.3, 0.4, flip=True), acc=2, vel=2)
rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2) # safe_move(0.1, 0.1, 0.1)
rob.movej(goto_holder_index(24, 0.0, angle, flip=True), acc=2, vel=2) # safe_move(-0.3, -0.3, 0.1)
time.sleep(1)
rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2) # flip()
rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2) # flip()
rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(24, 0.0, angle, flip=True), acc=2, vel=2)
# time.sleep(1)
# rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
# rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2) # rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2) # rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2) # rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2)