Add more routines

ur5_control.py

@@ -5,13 +5,14 @@ import math
 import numpy as np
 import time
 import os
-import logging
+#import logging
 import yaml
-from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
 import sys
 from util import fprint
 from pyModbusTCP.client import ModbusClient
 
+import subprocess
+from util import win32
 
 
 
@@ -33,37 +34,76 @@ class Rob():
         self.limb_wrist = limbs["limb_wrist"]
         #self.init_arm()
 
-    def init_arm(self):
+def ping(host):
-        #sys.stdout = Logger()
+        #Returns True if host (str) responds to a ping request.
-        fprint("Starting UR5 power up...")
 
-        # power up robot here
+        # Option for the number of packets as a function of
+        if win32:
+            param1 = '-n'
+            param2 = '-w'
+            param3 = '250'
+        else:
+            param1 = '-c'
+            param2 = '-W'
+            param3 = '0.25'
 
-        # wait for power up (this function runs async)
+        # Building the command. Ex: "ping -c 1 google.com"
+        command = ['ping', param1, '1', param2, param3, host]
 
+        return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0
 
-        # trigger auto-initialize
+def powerup_arm(robot):
+    #sys.stdout = Logger()
+    fprint("Starting UR5 power up...")
+    # power up robot here via PCB
 
-        # wait for auto-initialize
+    #
-        ip = self.ip
+    # wait for power up (this function runs async)
-        # init urx
+
+    while not ping(robot.ip):
+        time.sleep(0.5)
+    # trigger auto-initialize
+    fprint("Arm online. Waiting for calibration.")
+    # wait for auto-initialize
+def connect(robot):
+    if robot.robot is None:
+        newrobot = Rob(robot.config)
+        robot = newrobot
+        ip = robot.ip
         fprint("Connecting to arm at " + ip)
         trying = True
         while trying:
             try:
-                self.robot = urx.Robot(ip)
+                robot.robot = urx.Robot(ip)
                 trying = False
             except:
                 time.sleep(1)
-
         # Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
-        self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0))
+        robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0))
-
         # Set weight
-        self.robot.set_payload(2, (0, 0, 0.1))
+        robot.robot.set_payload(2, (0, 0, 0.1))
-        #rob.set_payload(2, (0, 0, 0.1))
+    return robot
-        time.sleep(0.2)
+def init_arm(robot):
-        fprint("UR5 ready.")
+    robot = connect(robot)
+    
+    # init urx
+    
+    #rob.set_payload(2, (0, 0, 0.1))
+    time.sleep(0.2)
+    fprint("UR5 ready.")
+    #return robot.robot
+
+    # setup - in case of fail. open gripper, move up, then go home.
+    rob = robot.robot
+    open_gripper()
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    new_pos[2] += 0.025
+    rob.movel(new_pos, vel=0.05, acc=1)
+
+    move_to_home(robot, speed=0.5)
+
+    return True
 
 def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None):
     rob = robot.robot
@@ -205,7 +245,7 @@ def move_to_polar(robot, start_pos, end_pos):
 
     return rx_intermediate
 
-def move_to_home(robot):
+def move_to_home(robot, speed=2):
     rob = robot.robot
 
     # Home position in degrees
@@ -217,7 +257,25 @@ def move_to_home(robot):
                 1.675098295930943]
 
     # Move robot
-    rob.movej(home_pos, acc=2, vel=2)
+    rob.movej(home_pos, acc=2, vel=speed)
+    return True
+
+def move_to_packup(robot, speed=0.25):
+    rob = robot.robot
+    
+    # known good starting point to reach store position
+    goto_holder_index(robot, 12, 0.3, flip=False, use_closest_path=False)
+
+    # Home position in degrees
+    store_pos = [-1.5708, 
+                -1.3, 
+                2.362, 
+                0.7056, 
+                -1.425, 
+                1.5708]
+    # Move robot
+    rob.movej(store_pos, acc=0.1, vel=speed)
+    return True
 
 def normalize_degree(theta):
     # Normalizes degree theta from -1.5pi to 1.5pi
@@ -346,7 +404,7 @@ def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
 
     rob.movejs(joints, acc=2, vel=2, radius=0.1)
 
-def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True):
+def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True, rzoffset=0):
     # gripper angle: from vertical
     # gripper length: from joint to start of grip
     # to flip, you can use flip=True or make gripper angle negative
@@ -365,7 +423,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.0
         gripperx -= (1-math.cos(gripperangle)) * limb3
         rz = math.pi / 2
         # flip the whole wrist
-        return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz, use_closest_path=use_closest_path)
+        return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path)
 
     else:
         gripperangle = math.radians(gripperangle)
@@ -387,6 +445,7 @@ def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_close
     #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, )
+    return True
 
 def is_flipped(robot):
     rob = robot.robot
@@ -415,8 +474,9 @@ def flip(robot):
     safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
 
     # Flip at safe position
-    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=is_flipped(robot)), vel=4, acc=3) # Move to safe position
-    rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
+    rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=4, acc=3) # Flip gripper
+    return True
 
 def safe_move(robot, x, y, z, use_closest_path=True):
     rob = robot.robot
@@ -427,15 +487,17 @@ def safe_move(robot, x, y, z, use_closest_path=True):
     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), use_closest_path=use_closest_path), vel=2, acc=2)
+    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=4, acc=3)
+    return True
 
-def pick_up_routine(robot, holder_index, verbose=False):
+def pick_up_routine(robot, holder_index, slot=None, verbose=False):
+    robot = connect(robot)
     rob = robot.robot
     
     if verbose:
-        print('Pickup routine for index', holder_index)
+        fprint('Pickup routine for index' + str(holder_index))
 
-    goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
+    goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
     open_gripper()
 
     curr_pos = rob.getl()
@@ -448,22 +510,153 @@ def pick_up_routine(robot, holder_index, verbose=False):
 
 
     new_pos[2] = 0.2
-    rob.movel(new_pos, vel=0.1, acc=1)
+    rob.movel(new_pos, vel=2, 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)
+    #move_to_home(robot, speed=4)
-    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)
+    # starting pos
+    angles = (-47.04,-115.81,138.33,-58.11,-56.55,-68.03)
+    angles = [x*math.pi/180 for x in angles]
+    rob.movej(angles,vel=1,acc=0.5)
+
+    angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39)
+    angles = [x*math.pi/180 for x in angles]
+    rob.movej(angles,vel=1,acc=0.5)
+
+    if slot is None:
+        slot = 0
+    if slot == 0:
+        angles = (7.09,-64.83,147.87,-188.00,-29.84,17.15)
+        angles = [x*math.pi/180 for x in angles]
+    if slot == 1:
+        angles = (8.43,-64.63,154.74,-197.30,-30.21,19.72)
+        angles = [x*math.pi/180 for x in angles]
+    if slot == 2:
+        angles = (10.39,-70.86,133.95,7.83,30.91,-159.00)
+        angles = [x*math.pi/180 for x in angles]
+    if slot == 3:
+        angles = (13.31,-76.23,140.78,1.90,31.98,-153.76)
+        angles = [x*math.pi/180 for x in angles]
+
+    rob.movej(angles,vel=1,acc=0.5)
+
     open_gripper()
 
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    new_pos[2] += 0.04
+    rob.movel(new_pos, vel=0.1, acc=1)
+
+    angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39)
+    angles = [x*math.pi/180 for x in angles]
+    rob.movej(angles,vel=1,acc=0.5)
+    
+    move_to_home(robot, speed=3)
+    # # Tray position 1
+    # if slot is None:
+    #     rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
+    #     rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
+    #     open_gripper()
+    # else:
+    #     xoffset = 0.051 * slot
+    #     rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
+    #     rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=60, flip=was_flipped, use_closest_path=False,rzoffset=math.pi), vel=4, acc=3)
+    #     #rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0, math.pi/2, 0.1), vel=4, acc=3)
+    #     open_gripper()
+
     # Back to safe position
-    rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
+    #time.sleep(10)
+    #rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=4, acc=3)
+    return True
+
+def return_routine(robot, slot, holder_index=None, verbose=False):
+    robot = connect(robot)
+    rob = robot.robot
+
+    
+    open_gripper()
+    was_flipped = is_flipped(robot)
+    if slot is None:
+        rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
+        rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
+        close_gripper()
+    else:
+        xoffset = 0.051 * slot
+        rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
+        rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
+        close_gripper()
+
+
+    if holder_index is not None:
+        goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
+        curr_pos = rob.getl()
+        new_pos = curr_pos
+        new_pos[2] = 0.015
+        rob.movel(new_pos, vel=0.1, acc=1)
+        open_gripper()
+        new_pos[2] = 0.1
+        rob.movel(new_pos, vel=2, acc=1)
+        return True
+    else:
+        # go to camera
+        rob.movej(offset_gripper_angle(robot, 0.35, -0.35, 0.3, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
+        return True
+    
+    
+def to_camera(robot, holder_index, verbose=False):
+    robot = connect(robot)
+    fprint("Bringing tube at " + str(holder_index) + " to camera")
+    rob = robot.robot
+
+    goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
+
+    open_gripper()
+
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    new_pos[2] = 0.005
+    rob.movel(new_pos, vel=0.1, acc=1)
+    # goto_holder_index(robot, holder_index, 0.0)
+    close_gripper()
+    
+
+
+    new_pos[2] = 0.2
+    rob.movel(new_pos, vel=2, acc=1)
+    was_flipped = is_flipped(robot)
+
+    # start inwards
+    rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
+    
+    # go to camera
+    rob.movej(offset_gripper_angle(robot, 0.3, -0.2, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
+    return True
+
+def return_camera(robot, holder_index, verbose=False):
+    robot = connect(robot)
+    rob = robot.robot
+
+    # move inwards first
+    rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=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.015
+    rob.movel(new_pos, vel=0.1, acc=1)
+    open_gripper()
+    new_pos[2] = 0.1
+    rob.movel(new_pos, vel=2, acc=1)
+    
 
 def open_gripper():
-    print("opening")
+    fprint("Opening gripper")
     c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
     
+    c.write_single_register(0, 0b0)
+    c.write_single_register(435, 0b1)
+
     c.write_single_register(0, 0b0)
     c.write_single_register(435, 0b1)
     time.sleep(0.5)
@@ -472,8 +665,11 @@ def open_gripper():
     #c.close()
 
 def close_gripper():
-    print("closing")
+    fprint("Closing gripper")
     c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
+    c.write_single_register(435, 0b0)
+    c.write_single_register(0, 0b1)
+
     c.write_single_register(435, 0b0)
     c.write_single_register(0, 0b1)
     time.sleep(0.5)
@@ -489,44 +685,34 @@ if __name__ == "__main__":
         #global config
         config = yaml.safe_load(fileread)
     
-    open_gripper()
+    #open_gripper()
-    #time.sleep(5)
+    #time.sleep(1)
     #close_gripper()
-    
+
-    #time.sleep(100)
+
-    
-    
-    
     robot = Rob(config) # robot of type Rob is the custom class above
-    robot.init_arm()
+    #powerup_arm(robot)
+    robot = connect(robot)
+    init_arm(robot)
+    
     rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
+
+    move_to_home(robot, speed=4)
+
+    #time.sleep(3)
+    #move_to_packup(robot)
+    
+    pick_up_routine(robot, 0, 0, True)
+
+    #move_to_home(robot)
+
+    #return_routine(robot, 0, 0, True)
+    
+    
+    time.sleep(100)
     
     print("Current tool pose is: ",  rob.getl())
-    move_to_home(robot)
-
-    home_pose = [-0.4999999077032916, 
-               -0.2000072960336574, 
-               0.40002172976662786, 
-               0, 
-               -3.14152741295329, 
-               math.radians(62)]
-
-    # time.sleep(.5)
-
-    p1 = [0,
-
-        0.6,
-        .4,
-        0.2226,
-        3.1126,
-        0.0510]
     
-    p2 = [0.171,
-        -0.115,
-        0.2,
-        0.2226,
-        3.1126,
-        0.0510]
 
     curr_pos = rob.getl()
 
@@ -545,8 +731,8 @@ if __name__ == "__main__":
     # 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 [6,7,8]:
+    #for i in [6,7,8]:
-        pick_up_routine(robot, i, verbose=True)
+        #pick_up_routine(robot, i, verbose=True)