def cook(stir_time, batter_sit_time, cook1_time, cook2_time, use_gripper=True):
"""Main cooking program"""
#STAGE 0: ROBOT INITIALISATION_____________________________________________
print("----------------Initialising Robot-----------------\r\n")
#TODO: Check with Keiran if this define from his code is redundant
burt = 0
#Initialise robot, deciding whether or not to talk to the gripper (initilaising the robot opens the gripper, so during debugging if we have something in the gripper we dont want released, use use_gripper == False)
if use_gripper == True:
burt = kgr.kg_robot(port=30010,
db_host="169.254.150.100",
ee_port="COM3")
elif use_gripper == False:
burt = kgr.kg_robot(port=30010, db_host="169.254.150.100")
print("----------------Robot Initialised-----------------\r\n\r\n")
start_time = time.time()
#Define which cooking stages we want to execute
stage1 = True
stage2 = True
stage3 = True
stage4 = True
stage5 = True
#STAGE 1: POUR INGREDIENTS_________________________________________________
if stage1 == True:
pour_ingredients(burt)
#STAGE 2: WHISK BOOGALOO___________________________________________________
if stage2 == True:
whisk(burt, stir_time, batter_sit_time, start_time)
#STAGE 3: POUR BATTER______________________________________________________
if stage3 == True:
pour_batter(burt, cook1_time, start_time)
#STAGE 4: FLIP PANCAKES____________________________________________________
if stage4 == True:
flip_pancakes(burt, cook2_time, start_time)
#STAGE 5: REMOVE PANCAKES__________________________________________________
if stage5 == True:
remove_pancakes(burt, start_time)
#Reset connection with UR5 to avoid issue where gripper does not accept commands when this code is run a second time
burt.ee.reset_output_buffer()
burt.ee.close()
print("reset")
burt.close()
def main():
print("------------Configuring Burt-------------\r\n")
burt = kgr.kg_robot(port=30010,db_host="192.168.1.10")
#burt = kgr.kg_robot(port=30010,ee_port="COM32",db_host="192.168.1.51")
print("----------------Hi Burt!-----------------\r\n\r\n")
try:
while 1:
ipt = input("cmd: ")
if ipt == 'close':
break
elif ipt == 'home':
burt.home()
elif ipt == 'rec':
burt.teach_mode.record()
elif ipt == 'play':
burt.teach_mode.play()
elif ipt == 'vel':
burt.speedl([0,0,-0.01,0,0,0],acc=0.1,blocking_time=5)
elif ipt == 'hi':
burt.set_digital_out(0,1)
elif ipt == 'lo':
burt.set_digital_out(0,0)
else:
var = int(input("var: "))
burt.serial_send(ipt,var,True)
finally:
print("Goodbye")
burt.close()
def main():
print("------------Configuring Burt-------------\r\n")
burt = kgr.kg_robot(host="127.0.0.1",
port=30010,
db_host="127.0.0.5",
sim_port=19997)
#burt = kgr.kg_robot(port=30010,ee_port="COM32",db_host="192.168.1.51")
print("----------------Hi Burt!-----------------\r\n\r\n")
try:
while 1:
ipt = input("cmd: ")
if ipt == 'close':
break
elif ipt == 'home':
burt.home()
# sim
elif ipt == 't':
burt.translatel_rel([0, 0, 0.1])
elif ipt == 'c':
burt.sim.check(burt.sim.sim_joints)
elif ipt == 'o':
ora.orange(burt)
else:
var = int(input("var: "))
burt.serial_send(ipt, var, True)
finally:
print("Goodbye")
burt.close()
def main():
print("------------Configuring Burt-------------\r\n")
burt = kgr.kg_robot(port=30010, db_host="192.168.1.6")
#burt = kgr.kg_robot(port=30010,ee_port="COM32",db_host="192.168.1.51")
print("----------------Hi Burt!-----------------\r\n\r\n")
try:
while 1:
ipt = input("cmd: ")
if ipt == 'close':
break
elif ipt == 'home':
burt.home()
elif ipt == 'rec':
burt.teach_mode.record()
elif ipt == 'play':
burt.teach_mode.play(name="testRecording.json")
elif ipt == 'vel':
burt.speedl([0, 0, -0.01, 0, 0, 0], acc=0.1, blocking_time=5)
elif ipt == "stop":
burt.speedl([0, 0, 0, 0, 0, 0], acc=0.1, blocking_time=5)
elif ipt == 'hi':
burt.set_digital_out(0, 1)
elif ipt == 'lo':
burt.set_digital_out(0, 0)
elif ipt == "z+":
burt.translatel_rel([0, 0, 0.01])
elif ipt == "z-":
burt.translatel_rel([0, 0, -0.01])
elif ipt == "y+":
burt.translatel_rel([0, 0.05, 0])
elif ipt == "y-":
burt.translatel_rel([0, -0.05, 0])
elif ipt == "x+":
burt.translatel_rel([0.05, 0, 0])
elif ipt == "x-":
burt.translatel_rel([-0.05, 0, 0])
elif ipt == "test":
burt.movel()
elif ipt == "myhome":
burt.movel([0.125, 0.3, 0.4, 3, 0.9, 0])
# else:
# var = int(input("var: "))
# burt.serial_send(ipt,var,True)
print(burt.getl())
finally:
print("Goodbye")
burt.close()
def main():
print("------------Configuring Burt-------------\r\n")
#burt = kgr.kg_robot()
burt = kgr.kg_robot(port=30010, db_host="192.168.1.10")
print("----------------Hi Burt!-----------------\r\n\r\n")
try:
while 1:
ipt = input("cmd: ")
if ipt == 'close':
break
elif ipt == 'home':
print(burt.home())
elif ipt == 't':
burt.teach_mode.record()
elif ipt == 'p':
burt.teach_mode.play()
elif ipt == 'test':
demand_pose = burt.getl()
toc = time.time()
#init data streaming
burt.stream_data_start(0.01)
#loop servoj and read pose
for i in range(0, 100):
demand_pose[0] += 0.001
burt.servoj(demand_pose, control_time=0.01)
current_pose = burt.read_msg()
print(time.time() - toc, ': ', current_pose)
time.sleep(0.009)
#stop data streaming
burt.stream_data_stop()
#flush recv buffer
time.sleep(0.5)
print(burt.ping())
print(burt.ping())
finally:
print("Goodbye")
burt.close()
def follow(filename):
"""Function to get UR5 to follow a recorded trajectory"""
#extract 3D pose
body_3D_pose, left_hand_3D_pose, right_hand_3D_pose = get_arm_3D_coordinates(
filename, show_each_frame=False)
#define allowable range of x,y and z values
xlim = [-0.6, 0.6]
ylim = [-0.65, -0.16]
zlim = [0.07, 1]
#define empty lists to fill with palm values
x_sec = []
y_sec = []
z_sec = []
#For all palm values
for pos in right_hand_3D_pose[HAND.PALM.value]:
#If they are not recorded as bad
if pos[3] == False:
#Add x,y and z values to list of values to plot
x_sec.append(pos[0])
y_sec.append(pos[1])
z_sec.append(pos[2])
#create 3D scatter plot of palm values
fig = plt.figure()
ax = Axes3D(fig)
ax.scatter(x_sec, y_sec, z_sec)
plt.show()
#Initialise connection with robot
print("------------Configuring Burt-------------\r\n")
burt = 0
burt = kgr.kg_robot(port=30010, db_host="169.254.150.100")
print("----------------Hi Burt!-----------------\r\n\r\n")
#Move UR5 arm to a starting position slowly
burt.movej([
np.radians(-90),
np.radians(-65),
np.radians(-115),
np.radians(-85),
np.radians(90),
np.radians(45)
],
min_time=3)
print("moved to start")
#Flag for if this is the first position on the trajectory
first_round = True
#Counter for number of the current waypoint on the trajectory
count = 0
#define a height offset to ensure that the gripper does not hit the table trying to follow the route that the palm took
height_offset = 0.09
#For every palm value
for val in right_hand_3D_pose[HAND.PALM.value]:
#offset z axis value to minimise risk of collision with table
val[2] = val[2] + height_offset
#Initialise our invalid point tracker
point_invalid = False
#Catch if point is already declared to be invalid
if val[3] == True:
print("Lost track of point", end='')
point_invalid = True
#Set point to invalid if it lies outside our allowable 3D bounds
if val[0] > xlim[1] or val[0] < xlim[0]:
point_invalid = True
print("x out of limits", end='')
if val[1] > ylim[1] or val[1] < ylim[0]:
point_invalid = True
print("y out of limits", end='')
if val[2] > zlim[1] or val[2] < zlim[0]:
point_invalid = True
print("z out of limits", end='')
#Seperate catch for if out of bounds code leads point to be declared invalid
if point_invalid == True:
print(' ', val[0], val[1], val[2])
print("Invalid point, sleeping for a frame")
point_invalid = True
#If this is the trajectory position that is valid, move to it slowly
elif first_round == True and point_invalid == False:
first_round = False
print("Move to first pos")
print("Go to ", val[0], val[1], val[2])
burt.movel([val[0], val[1], val[2], 1.156, 2.886, -0.15],
min_time=5)
count = count + 1
#Otherwise go to that trajectory in real time
else:
print("Go to ", val)
#Note controll_time is the time per point (not blocking, it seems to just slow down the action)
burt.servoj([val[0], val[1], val[2], 1.156, 2.886, -0.15],
lookahead_time=0.2,
control_time=0.1,
gain=100)
count = count + 1
#Close connection to burt
burt.close()
print(count)
Turns on electromagnet and moves near current area, then slowly
to desired location.
"""
import time
import serial
from math import pi
import numpy
import socket
import sys
sys.path.append(r"C:\Users\44772\Documents\dsh46WaterControl\ur5control")
import waypoints as wp
import kg_robot as kgr
urnie = kgr.kg_robot(port=30010, db_host="169.254.129.254")
urnie.set_tcp(wp.plunger_tcp)
#assume alignment of theta=0 and positive x axis
#convert to x, y in m
x = 0.001 * float(sys.argv[1]) * numpy.cos(float(sys.argv[2]))
y = 0.001 * float(sys.argv[1]) * numpy.sin(float(sys.argv[2]))
#convert to x_des, y_des in m
x_des = 0.001 * float(sys.argv[3]) * numpy.cos(float(sys.argv[4]))
y_des = 0.001 * float(sys.argv[3]) * numpy.sin(float(sys.argv[4]))
urnie.set_tool_digital_out(0, 1) #turn electromagnet on
centre = numpy.add(wp.reset_above_tubl, [x, y, 0, 0, 0, 0])
# if object couldn't be found, trace edge of tub
import time
import serial
import math
from math import pi
import numpy
import socket
import sys
import sched
sys.path.append("C:/Users/David/Documents/PhD/ur5control")
import waypoints as wp
import kg_robot as kgr
tstart = time.time()
urnie = kgr.kg_robot(port=30010,db_host="169.254.161.50")
urnie.set_tcp(wp.plunger_tcp)
#scheduler used to regularly pass desired positions to servoj
scheduler = sched.scheduler(time.time, time.sleep)
def schedule_it(dt, duration, callable, *args):
for i in range(int(duration/dt)):
scheduler.enter(i*dt, 1, callable, args)
#calculate starting position of motion
def starting_pos(centrepose, xamp, xphase, yamp, yphase, depth, anglex, angley):
npose = numpy.add(centrepose, [0, 0, 0.001*(30-depth), 0, 0, 0])
npose = numpy.add(npose, [0, 0, 0, anglex, angley, 0])
npose = numpy.add(npose, [xamp*math.sin(xphase), 0, 0, 0, 0, 0])
npose = numpy.add(npose, [0, yamp*math.sin(yphase), 0, 0, 0, 0])
urnie.movel(npose)