import urx
import math3d as m3d
from scipy.optimize import fsolve
import math
import numpy as np
import time
import os
#import logging
import yaml
import sys
from util import fprint
from pyModbusTCP.client import ModbusClient
from multiprocessing import Queue
import subprocess
from util import win32
class Rob():
robot = None
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
#
def __init__(self, config):
self.config = config
armc = config["arm"]
self.ip = armc["ip"]
tool = armc["tool"]
limbs = armc["limbs"]
self.offset_x, self.offset_y, self.offset_z = (tool["offset_x"], tool["offset_y"], tool["offset_z"])
self.limb_base = limbs["limb_base"]
self.limb1 = limbs["limb1"]
self.limb2 = limbs["limb2"]
self.limb3 = limbs["limb3"]
self.limb_wrist = limbs["limb_wrist"]
#self.init_arm()
def ping(host):
#Returns True if host (str) responds to a ping request.
# 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'
# 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
def powerup_arm(robot):
#sys.stdout = Logger()
fprint("Starting UR5 power up...")
# power up robot here via PCB
#
# wait for power up (this function runs async)
count = 0
while not ping(robot.ip) and count == 10:
time.sleep(0.5)
count += 1
# 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
count = 0
while trying and count < 10:
count += 1
try:
robot.robot = urx.Robot(ip, use_rt=False)
robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0))
# Set weight
robot.robot.set_payload(2, (0, 0, 0.1))
trying = False
except:
time.sleep(0.5)
# Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
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)
curr_j = rob.getj()
curr_j[3] -= 0.2 # radians
rob.movej(curr_j, vel=0.2, 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
new_orientation = m3d.Transform()
new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis
new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis
new_orientation.orient.rotate_zb(zb) # Replace rz with the desired rotation around Z-axis
# Get the current pose
trans = rob.getl()
# Apply the new orientation while keeping the current position
new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
new_trans.pos.x = x
new_trans.pos.y = y
new_trans.pos.z = z
#rob.speedj(0.2, 0.5, 99999)
rob.set_pose(new_trans, acc=2, vel=2, command="movej", threshold=threshold) # apply the new pose
def set_pos_rel_rot_abs(robot, x, y, z, xb, yb, zb):
rob = robot.robot
new_orientation = m3d.Transform()
new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis
new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis
new_orientation.orient.rotate_zb(zb) # Replace rz with the desired rotation around Z-axis
# Get the current pose
trans = rob.getl()
# Apply the new orientation while keeping the current position
new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
new_trans.pos.x += x
new_trans.pos.y += y
new_trans.pos.z += z
#rob.speedj(0.2, 0.5, 99999)
rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej") # apply the new pose
def set_pos_abs_rot_rel(robot, x, y, z, xb, yb, zb):
rob = robot.robot
new_orientation = m3d.Transform()
new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis
new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis
new_orientation.orient.rotate_zb(zb) # Replace rz with the desired rotation around Z-axis
# Get the current pose
trans = rob.getl()
# Apply the new orientation while keeping the current position
new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
new_trans.pos.x = x
new_trans.pos.y = y
new_trans.pos.z = z
#rob.speedj(0.2, 0.5, 99999)
rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej") # apply the new pose
def is_safe_move(start_pose, end_pose, r=0.25):
start_x, start_y = (start_pose[0], start_pose[1])
end_x, end_y = (end_pose[0], end_pose[1])
try:
m = (end_y-start_y)/(end_x-start_x)
b = start_y - m*start_x
# print('m = y/x =', m)
# print('b =', b)
except:
m = (end_x-start_x)/(end_y-start_y)
b = start_x - m*start_y
# print('m = x/y =', m)
# print('b =', b)
return r**2 - b**2 + m**2 * r**2 < 0
def cartesian_to_polar(x, y):
r = np.sqrt(x**2 + y**2)
theta = np.arctan2(y, x)
return r, theta
def polar_to_cartesian(r, theta):
x = r * np.cos(theta)
y = r * np.sin(theta)
return x, y
def move_to_polar(robot, start_pos, end_pos):
rob = robot.robot
# Convert to polar coordinates
start_r, start_theta = cartesian_to_polar(start_pos[0], start_pos[1])
end_r, end_theta = cartesian_to_polar(end_pos[0], end_pos[1])
# Interpolate for xy (spiral arc)
n_points = 30
r_intermediate = np.linspace(start_r, end_r, n_points)
theta_intermediate = np.linspace(start_theta, end_theta, n_points)
# Interpolate for z (height)
start_z = start_pos[2]
end_z = end_pos[2]
z_intermediate = np.linspace(start_z, end_z, n_points)
# Interpolate for rz (keep tool rotation fixed relative to robot)
curr_rot = rob.getl()
theta_delta = theta_intermediate[1]-theta_intermediate[0]
rx_intermediate = [curr_rot[5] + theta_delta*i for i in range(n_points)]
# curr_rot = rob.getj()
# start_rz = curr_rot[5]
# rot = end_theta - start_theta
# end_base_joint = curr_rot[0]-start_theta + rot
# end_rz = curr_rot[0] + rot
# # rob.movel([*polar_to_cartesian(end_r, end_theta), *rob.getl()[2:]], acc=2, vel=2)
# print('start_theta = ', math.degrees(start_theta))
# print('end_theta = ', math.degrees(curr_rot[0]-start_theta+rot))
# print('start_rz =', math.degrees(start_rz))
# print('rot =', math.degrees(rot))
# print('end_rz =', math.degrees(end_rz))
# rz_intermediate = np.linspace(start_rz, end_rz, n_points)
# Convert back to cartesian coordinates
curr_pos = rob.getl()
intermediate_points = [[*polar_to_cartesian(r, theta), z, *curr_pos[3:]]
for r, theta, z, rx in zip(r_intermediate,
theta_intermediate,
z_intermediate,
rx_intermediate)]
# Move robot
rob.movels(intermediate_points, acc=2, vel=2, radius=0.1)
return rx_intermediate
def move_to_home(robot, keep_flip=False, speed=2):
rob = robot.robot
if is_flipped(robot) and not keep_flip:
flip(robot)
# Move robot to home position
rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.25), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
return True
def move_to_packup(robot, speed=0.25):
robot = connect(robot)
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
multiplier = 1
normalized_theta = theta % (math.pi * multiplier)
# Maintain the negative sign if the original angle is negative
if theta < 0:
normalized_theta -= math.pi * multiplier
# Return angle
return normalized_theta
def get_joints_from_xyz_rel(robot, 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
#l3=0.15
l_bs, l1, l2, l3, l_wt = (robot.limb_base, robot.limb1, robot.limb2, robot.limb3, robot.limb_wrist) # Limb lengths
l3 += l3offset # add wrist offset, used for gripper angle calculations
offset_x = robot.offset_x
offset_y = robot.offset_y
offset_z = robot.offset_z
# Calculate base angle and r relative to shoulder joint
def calculate_theta(x, y, a):
# Calculate if we need the + or - in our equations
if (x>-a and y>=0) or (x>a and y<0):
flip = 1
elif (x<-a and y>=0) or (x<a and y<0):
flip = -1
else:
# Critical section (x=a, or x=-a). Infinite slope
# Return 0 or 180 depending on sign
return math.atan2(y, 0)
# Calculate tangent line y = mx + b
m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
b = flip * a * math.sqrt(1+m*m)
# Calculate equivalent tangent point on circle
cx = (-flip*m*b)/(1+m*m)
cy = m*cx + flip*b
# Calculate base angle, make angle negative if flip=1
theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
return theta
base_theta = calculate_theta(x, y, l_bs)
cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
r = math.sqrt((x+offset_x+cx)**2 + (y+offset_y+cy)**2)
# Formulas to find out joint positions for (r, z)
def inv_kin_r_z(p):
a, b, c = p
return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r
l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (l3*math.sin(a-b-c)) - (z + offset_z), # z
a-b-c) # wrist angle
# Normalize angles
base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
wrist1 += rx
# Return result
return base, shoulder, elbow, wrist1, ry, rz
def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0, use_closest_path=True):
rob = robot.robot
joints = get_joints_from_xyz_rel(robot, 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 math.degrees(joints[1]) > 137:
print("CRASH! Shoulder at", joints[1] * 180/math.pi)
#else:
#print("Shoulder at", joints[1] * 180/math.pi)
# Get adjusted joint positions
adjusted_joints = [o+j*i for j, o, i in zip(joints, offsets, inverse)]
curr_joints = rob.getj()
def get_complimentary_angle(joint_angle):
if joint_angle<0:
new_angle = joint_angle + 2*math.pi
else:
new_angle = joint_angle - 2*math.pi
if abs(new_angle) > math.radians(350):
return joint_angle
else:
return new_angle
# Use closest path (potentially going beyond 180 degrees)
if use_closest_path:
if abs(get_complimentary_angle(adjusted_joints[0])-curr_joints[0]) < abs(adjusted_joints[0]-curr_joints[0]):
adjusted_joints[0] = get_complimentary_angle(adjusted_joints[0])
# final_joint_positions = []
# for curr_joint, adjusted_joint in zip(curr_joints, adjusted_joints):
# if abs(curr_joint - adjusted_joint) < abs(curr_joint - get_complimentary_angle(adjusted_joint)):
# final_joint_positions.append(adjusted_joint)
# else:
# final_joint_positions.append(get_complimentary_angle(adjusted_joint))
# return final_joint_positions
return adjusted_joints
def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
rob = robot.robot
start_joints = rob.getj()
end_joint = get_joints_from_xyz_abs(robot, 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(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
limb3 = robot.limb3
# Determine tool rotation depending on gripper angle
if gripperangle < 0:
rz = - math.pi / 2
else:
rz = math.pi / 2
if flip:
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(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=-3*math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path)
else:
gripperangle = math.radians(gripperangle)
grippery = gripperlength - math.cos(gripperangle) * gripperlength
grippery -= math.sin(gripperangle) * limb3
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * limb3
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, 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"][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
#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
wrist1 = rob.getj()[3]
if wrist1 > 0:
return True
else:
return False
def flip(robot):
rob = robot.robot
# A list of safe positions to flip
safe_positions = [(-0.18, -0.108, 0.35),
(0.18, -0.108, 0.35)]
# 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(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
# print('flip?: ', is_flipped(robot))
return True
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
# Flip gripper if needed
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)
return True
def holder_routine(robot, pos_updates, holder_index, pick_up, verbose=False):
robot = connect(robot)
rob = robot.robot
# Don't attempt to place a tube in the camera slot
if holder_index == 49:
return
if verbose:
fprint('Pickup routine for index' + str(holder_index))
# Go to the correct holder
if pick_up:
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
else:
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
if pick_up:
open_gripper()
# Move down
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0.005
rob.movel(new_pos, vel=0.1, acc=1)
if pos_updates is not None:
pos_updates.put(1)
fprint("Triggering LED interface")
# Pick up or drop off
if pick_up:
close_gripper()
else:
open_gripper()
# Move up
new_pos[2] = 0.2
rob.movel(new_pos, vel=2, acc=1)
was_flipped = is_flipped(robot)
if pos_updates is not None:
pos_updates.put(2)
fprint("Triggering LED interface")
# goto_holder_index(robot, 25, z=0.2)
def pick_up_holder(robot, pos_updates, holder_index, verbose=False):
holder_routine(robot, pos_updates, holder_index, True, verbose=verbose)
def drop_off_holder(robot, pos_updates, holder_index, verbose=False):
holder_routine(robot, pos_updates, holder_index, False, verbose=verbose)
def tray_routine(robot, slot=0, pick_up=True):
robot = connect(robot)
rob = robot.robot
# Default to 0 if invalid value
if slot not in [0, 1, 2, 3]:
slot = 0
slot_prepositions = [(-9.93, -112.67, 144.02, -116.69, -54.13, -10.29),
(-12.35, -124.95, 148.61, -107.27, -54.36, -13.26),
(-16.45, -96.97, 137.85, 58.39, -305.08, 161.75),
(-16.66, -97.28, 138.16, 58.54, -305.05, 161.50)]
# Initial position depending on slot and robot orientation
if slot in [0, 1]:
if is_flipped(robot):
flip(robot)
else:
move_to_home(robot, keep_flip=True)
else:
goto_holder_index(robot, 25, z=0.3)
# Align robot to the slot
if slot in [2,3]:
angles = [(-2.77, -99.64, 131.02, 67.67, 70.04-360, 153.03),
slot_prepositions[slot]]
else:
angles = [(-58, -114.45, 100.52, -45.24, -96.95, 120),
(-39.98, -124.92, 132.28, -61.56, -55.60, -50.77),
slot_prepositions[slot]]
angles = [[x*math.pi/180 for x in move] for move in angles]
rob.movejs(angles,vel=2,acc=1)
# Positions for each slot
slot_distance = .052
slot_height = -.015-.0095+0.007 # add 7mm for shim
first_slot = -0.3084+0.01+0.003 # add 3mm for tray adjust
slot_position = [
[first_slot, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
[first_slot+slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
[first_slot+2*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
[first_slot+3*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
]
if pick_up:
open_gripper()
rob.movel(slot_position[slot], vel=0.2, acc=1)
# Place/Grab the tube
if pick_up:
close_gripper()
else:
open_gripper()
# Move back
tilt = 0.3
curr_pos = rob.getl()
new_pos = curr_pos
if slot==3:
new_pos[0] -= 0.05 #x
new_pos[1] += 0.15 #y
new_pos[2] = 0.09 #z
new_pos[3] += tilt
new_pos[4] += tilt
new_pos[5] += tilt
rob.movel(new_pos, vel=0.2, acc=1)
# Go home to safe position
move_to_home(robot, speed=1, keep_flip=True)
def pick_up_tray(robot, slot=0):
tray_routine(robot, slot, True)
def drop_off_tray(robot, slot=0):
tray_routine(robot, slot, False)
def return_routine(robot, slot, holder_index=None, verbose=False):
# OLD UNUSED
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 goto_camera(robot, pos_updates):
robot = connect(robot)
goto_holder_index(robot, 49, 0.2)
def tray_to_camera(robot, pos_updates, slot):
pick_up_tray(robot, slot)
goto_camera(robot, pos_updates)
def holder_to_tray(robot, pos_updates, holder_index, slot):
pick_up_holder(robot, pos_updates, holder_index)
drop_off_tray(robot, slot)
def holder_to_camera(robot, pos_updates, holder_index, verbose=False):
robot = connect(robot)
fprint("Bringing tube at " + str(holder_index) + " to camera")
rob = robot.robot
pick_up_holder(robot, pos_updates, holder_index)
goto_camera(robot, pos_updates)
def camera_to_holder(robot, pos_updates, holder_index, verbose=False):
robot = connect(robot)
rob = robot.robot
drop_off_holder(robot, pos_updates, holder_index)
# def open_gripper():
# fprint("Opening gripper")
# c = ModbusClient(host="192.168.1.21", port=502, auto_open=False, auto_close=False)
# c.open()
# while not c.is_open:
# time.sleep(0.01)
# c.write_single_register(112, 0b0)
# # c.write_single_register(435, 0b10000000)
# time.sleep(0.5)
# # c.write_single_register(112, 0b0)
# c.write_single_register(435, 0b10000000)
# time.sleep(0.5)
# c.close()
# #c.close()
# def close_gripper():
# fprint("Closing gripper")
# c = ModbusClient(host="192.168.1.21", port=502, auto_open=False, auto_close=False)
# c.open()
# while not c.is_open:
# time.sleep(0.01)
# c.write_single_register(435, 0b00000000)
# # c.write_single_register(112, 0b1)
# time.sleep(0.5)
# # c.write_single_register(435, 0b00000000)
# c.write_single_register(112, 0b1)
# time.sleep(0.5)
# c.close()
# time.sleep(0.2)
# #
def open_gripper():
fprint("Opening gripper")
c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
c.write_single_register(112, 0b0)
c.write_single_register(435, 0b10000000)
c.write_single_register(112, 0b0)
c.write_single_register(435, 0b10000000)
time.sleep(0.5)
c.close()
#c.close()
def close_gripper():
fprint("Closing gripper")
c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
c.write_single_register(435, 0b00000000)
c.write_single_register(112, 0b1)
c.write_single_register(435, 0b00000000)
c.write_single_register(112, 0b1)
time.sleep(0.5)
c.close()
#
def get_position_thread(robot, pos_updates):
try:
robot = connect(robot)
rob = robot.robot
oldvals = rob.getl()
deltavals = [0,0,0]
import uptime
t = 0.01
count = 0
while True:
start = uptime.uptime()
if pos_updates.qsize() < 2:
vals = rob.getl()
if vals != oldvals:
if pos_updates is not None:
pos_updates.put(tuple(oldvals))
#time.sleep(0.01)
# deltavals = list()
# deltavals.append(vals[0]-oldvals[0])
# deltavals.append(vals[1]-oldvals[1])
# deltavals.append(vals[2]-oldvals[2])
# count = 0
oldvals = vals
# else:
# count += 0.2
# if count < 1:
# tmpvals = vals
# tmpvals[0] = oldvals[0] + deltavals[0]*count
# tmpvals[1] = oldvals[1] + deltavals[1]*count
# tmpvals[2] = oldvals[2] + deltavals[2]*count
# pos_updates.put(tuple(tmpvals))
while start + t > uptime.uptime():
time.sleep(0.0001)
except:
pass
if __name__ == "__main__":
with open('config.yml', 'r') as fileread:
#global config
config = yaml.safe_load(fileread)
robot = Rob(config) # robot of type Rob is the custom class above
#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_packup(robot)
move_to_home(robot)
pick_up_holder(robot, None, 8)
#drop_off_tray(robot, 0)
# drop_off_tray(robot, 1)
# drop_off_tray(robot, 2)
# drop_off_tray(robot, 3)
# pick_up_tray(robot, 1)
# drop_off_holder(robot, 5)
# pick_up_holder(robot, 26)
# drop_off_tray(robot, 3)
# for i in range(0,54):
# pick_up_holder(robot, None, i)
# #print('Drop off', i+1)
# drop_off_tray(robot, 0)
# #input()
# # holder_to_camera(robot, 0)
# # camera_to_holder(robot, 0)
print("Current tool pose is: ", rob.getl())
curr_pos = rob.getl()
config = None
rob.stop()
os.kill(os.getpid(), 9) # dirty kill of self
sys.exit(0)