diff --git a/ur5_control.py b/ur5_control.py
index d655f70..7f9ec8a 100755
--- a/ur5_control.py
+++ b/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):
- #sys.stdout = Logger()
- fprint("Starting UR5 power up...")
+def ping(host):
+ #Returns True if host (str) responds to a ping request.
- # 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
- # init urx
+ #
+ # wait for power up (this function runs async)
+
+ 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))
- #rob.set_payload(2, (0, 0, 0.1))
- time.sleep(0.2)
- fprint("UR5 ready.")
+ robot.robot.set_payload(2, (0, 0, 0.1))
+ return robot
+def init_arm(robot):
+ 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=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
+ 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=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)
- 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)
+
+ #move_to_home(robot, speed=4)
+
+ # 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()
- #time.sleep(5)
+ #open_gripper()
+ #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]:
- pick_up_routine(robot, i, verbose=True)
+ #for i in [6,7,8]:
+ #pick_up_routine(robot, i, verbose=True)