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

Add missing robot pass-ins

Browse Source
This commit is contained in:
Cole Deck 2024-03-24 15:35:46 -05:00
parent 3de59f5985
commit 77fdc43fce
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ur5_control.py
View File

@ -283,7 +283,7 @@ def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_g
def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
rob = robot.robot rob = robot.robot
joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset) joints = get_joints_from_xyz_rel(robot, x, y, z, rx, ry, rz, l3offset=l3offset)
# Return current positions if coordinates don't make sense # Return current positions if coordinates don't make sense
if z<0: if z<0:
@ -303,8 +303,8 @@ def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l
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)]
def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
rob = robot.robot rob = robot.robot
start_joints = rob.getj() start_joints = rob.getj()
end_joint = get_joints_from_xyz_abs(robot, x, y, z, rx, ry, rz) end_joint = get_joints_from_xyz_abs(robot, x, y, z, rx, ry, rz)
@ -336,7 +336,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=35, gripperlength=0.20+0.0
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 + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz) return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz)
else: else:
gripperangle = math.radians(gripperangle) gripperangle = math.radians(gripperangle)
@ -384,8 +384,8 @@ def flip(robot):
safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0] safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
# Flip at safe position # 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(robot, *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 rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
def safe_move(robot, x, y, z): def safe_move(robot, x, y, z):
rob = robot.robot rob = robot.robot
@ -408,8 +408,9 @@ if __name__ == "__main__":
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)
robot = Rob(config) robot = Rob(config) # robot of type Rob is the custom class above
rob = robot.robot rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
print("Current tool pose is: ", rob.getl()) print("Current tool pose is: ", rob.getl())
move_to_home(robot) move_to_home(robot)