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

small dimensions fixes

Browse Source
This commit is contained in:
BlueOceanWave 2024-04-12 20:28:31 -05:00
parent 6d6c2030a9
commit ff2269193b
2 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
config.yml
View File

@ -11,7 +11,7 @@ arm:
offset_y: 0
offset_z: 0.14
limbs:
limb_base: 0.105
limb_base: 0.11
limb1: 0.425
limb2: 0.39225
limb3: 0.1

31
ur5_control.py
View File

@ -376,13 +376,13 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.0
return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz, use_closest_path=use_closest_path)
def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, verbose=False):
def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_closest_path=True, verbose=False):
joint = robot.config["position_map"][idx]
if verbose:
print("Going to cable holder index", joint["index"], "at position", joint["pos"])
safe_move(robot, joint["pos"][1]/1000, joint["pos"][0]/1000, z)
safe_move(robot, joint["pos"][0]/1000, joint["pos"][1]/1000, z, use_closest_path=use_closest_path)
#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
@ -418,7 +418,7 @@ def flip(robot):
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
def safe_move(robot, x, y, z):
def safe_move(robot, x, y, z, use_closest_path=True):
rob = robot.robot
flip_radius = 0.22 # Min radius on which to flip
r = math.sqrt(x**2 + y**2) # Get position radius
@ -427,7 +427,7 @@ def safe_move(robot, x, y, z):
if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)):
flip(robot)
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot)), vel=2, acc=2)
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
def pick_up_routine(robot, holder_index, verbose=False):
rob = robot.robot
@ -435,27 +435,27 @@ def pick_up_routine(robot, holder_index, verbose=False):
if verbose:
print('Pickup routine for index', holder_index)
goto_holder_index(robot, holder_index, 0.2)
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0
rob.movel(new_pos, vel=0.3, acc=1)
new_pos[2] = 0.005
rob.movel(new_pos, vel=0.1, acc=1)
# goto_holder_index(robot, holder_index, 0.0)
# Close Gripper code
time.sleep(0.5)
new_pos[2] = 0.2
rob.movel(new_pos, vel=0.3, acc=1)
rob.movel(new_pos, vel=0.1, acc=1)
was_flipped = is_flipped(robot)
# Tray position 1
rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
time.sleep(0.5)
# # Tray position 1
# rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
# rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
# time.sleep(0.5)
# Back to safe position
rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
# rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
if __name__ == "__main__":
@ -511,7 +511,10 @@ if __name__ == "__main__":
# move_arc(0, 0.3, 0.1)
# move_arc(0, -0.3, 0.3)
for i in range(20, 25):
# rob.movej(get_joints_from_xyz_abs(robot, 0.2, 0, 0.05), vel=0.5, acc=2)
# goto_holder_index(robot, 27, 0.05)
for i in [1,50]:
pick_up_routine(robot, i, verbose=True)