Merge branch 'main' of https://git.myitr.org/Jukebox/jukebox-software

ur5_control.py

@@ -6,6 +6,7 @@ import numpy as np
 import time
 import os
 import logging
+import yaml
 from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
 import sys
 from util import fprint
@@ -191,11 +192,6 @@ 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
 
@@ -222,8 +218,6 @@ def normalize_degree(theta):
     # Return angle
     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):
     # 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):
     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
     # 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:
+
+    if math.degrees(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 move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
+    
+    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):
+    # 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:
         rz = - math.pi / 2
     else:
         rz = math.pi / 2
 
     if flip:
-        gripperangle = -degtorad(gripperangle)
+        gripperangle = -math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery += math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
         gripperx -= (1-math.cos(gripperangle)) * limb3
         rz = math.pi / 2
         # 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:
-        gripperangle = degtorad(gripperangle)
+        gripperangle = math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery -= math.sin(gripperangle) * limb3
         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)
     
-
 def goto_holder_index(idx, z=0.05, gripperangle=35, 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)
+    
+    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)
-    return angles
+    #return angles
     #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__":
     
     #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
@@ -356,41 +418,28 @@ if __name__ == "__main__":
         0.0510]
 
     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
     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))
-    #rob.movejs(joints, acc=2, vel=2)
-    import yaml
+    # rob.movejs(joints, acc=2, vel=2, radius=0.1)
+
     with open('config.yml', 'r') as fileread:
         #global config
         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)
     #joints = []