Select Git revision
-
Dean Camera authoredDean Camera authored
ur10test.py 6.96 KiB
import sys
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import time
import math3d as m3d
import json
# roy = nodes
# siegfried = struts
print("")
print("")
print('Hello from ur10test.py!')
print("")
with open('../assembly/python-urx/setup.json') as f:
data = json.load(f)
# print(data)
# set values
setup=data["setup"]
globalSetup=data["setup"]["parameters"]
reference_roy=globalSetup["nodePickupRobotHome"]
reference_siegfried=globalSetup["edgePickupRobotHome"]
strut90_siegfried_j=globalSetup["edge90PickupRobotHome"]
latticePitch=globalSetup["latticePitch"]
sleepTime=globalSetup["sleep"]
sleepTime1=globalSetup["sleep1"]
# reference locations are 40 mm above pickup points
reference_roy_j = (reference_roy["x"], reference_roy["y"], reference_roy["z"], reference_roy["rx"], reference_roy["ry"], reference_roy["rz"])
reference_siegfried_j = (reference_siegfried["x"], reference_siegfried["y"], reference_siegfried["z"], reference_siegfried["rx"], reference_siegfried["ry"], reference_siegfried["rz"])
strut90_siegfried_j = (strut90_siegfried_j["x"], strut90_siegfried_j["y"], strut90_siegfried_j["z"], strut90_siegfried_j["rx"], strut90_siegfried_j["ry"], strut90_siegfried_j["rz"])
roy = urx.Robot("192.168.1.52")
roy.set_tcp((0, 0, 0.1, 0, 0, 0))
roy.set_payload(2, (0, 0, 0.1))
siegfried = urx.Robot("192.168.1.53")
siegfried.set_tcp((0, 0, 0.1, 0, 0, 0))
siegfried.set_payload(2, (0, 0, 0.1))
time.sleep(0.2) #leave some time to robot to process the setup commands
# placement location reference (top view)
# [S1] [S2]
#
#[S3] [N1] [S4] [N2] [S5]
#
# [S6] [S7]
for node in setup["nodes"]:
print("Build Node:")
print(node)
v = globalSetup["v"]
a = globalSetup["a"]
# move to reference locations and get current transformation matrices
roy.movej(reference_roy_j, acc = a, vel = v)
roy_trans = roy.get_pose()
time.sleep(sleepTime)
v = globalSetup["v1"]
a = globalSetup["a1"]
# move to pickup locations from reference
roy_trans.pos.z -= globalSetup["nodeRelativeDistance"]["z"]
roy.set_pose(roy_trans, acc = a, vel = v)
time.sleep(sleepTime)
# close and open grippers
roy.send_program('set_tool_digital_out(0, True)') #close node tool
time.sleep(sleepTime1)
# roy.send_program('set_tool_digital_out(0, False)') #open node tool
# time.sleep(sleepTime1)
# move to pickup locations from reference
roy_trans.pos.z += globalSetup["nodeRelativeDistance"]["z"]
roy.set_pose(roy_trans, acc = a, vel = v)
time.sleep(sleepTime)
# move roy (nodes) to programming location from reference, drops to program, then lifts to reference height
roy_trans.pos.x += globalSetup["programmingRelativeDistance"]["x"]
roy_trans.pos.y += globalSetup["programmingRelativeDistance"]["y"]
roy.set_pose(roy_trans, acc = a, vel = v)
roy_trans.pos.z -= globalSetup["programmingRelativeDistance"]["z"]
roy.set_pose(roy_trans, acc = a, vel = v)
time.sleep(1) # programming time!
roy_trans.pos.z += globalSetup["programmingRelativeDistance"]["z"]
roy.set_pose(roy_trans, acc = a, vel = v)
# move roy (nodes) to node from reference location
roy_trans.pos.x += ((globalSetup["nodeRelativeDistance"]["x"])-globalSetup["programmingRelativeDistance"]["x"])+node["x"]*latticePitch["x"]
roy_trans.pos.y += ((globalSetup["nodeRelativeDistance"]["y"])-globalSetup["programmingRelativeDistance"]["y"])+node["y"]*latticePitch["y"]
roy.set_pose(roy_trans, acc = a, vel = v)
roy_trans.pos.z -= (globalSetup["nodeRelativeDistance"]["z"])-node["z"]*latticePitch["z"]
roy.set_pose(roy_trans, acc = a, vel = v)
roy.send_program('set_tool_digital_out(0, False)') #open node tool
time.sleep(sleepTime1)
roy_trans.pos.z += (globalSetup["nodeRelativeDistance"]["z"])
roy.set_pose(roy_trans, acc = a, vel = v)
time.sleep(sleepTime)
print("")
for edge in setup["edges"]:
print("Build Edge:")
print(edge)
v = globalSetup["v"]
a = globalSetup["a"]
# move to reference locations and get current transformation matrices
siegfried.movej(reference_siegfried_j, acc = a, vel = v)
siegfried_trans = siegfried.get_pose()
time.sleep(sleepTime)
v = globalSetup["v1"]
a = globalSetup["a1"]
# move to pickup locations from reference
siegfried_trans.pos.z -= globalSetup["nodeRelativeDistance"]["z"]
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
time.sleep(sleepTime)
# close and open grippers
siegfried.send_program('set_tool_digital_out(0, True)') #close strut tool
time.sleep(sleepTime1)
# siegfried.send_program('set_tool_digital_out(0, False)') #open strut tool
# time.sleep(sleepTime1)
# move to pickup locations from reference
siegfried_trans.pos.z += globalSetup["nodeRelativeDistance"]["z"]
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
time.sleep(sleepTime)
# placement location reference (top view)
# [S1] [S2]
#
#[S3] [N1] [S4] [N2] [S5]
#
# [S6] [S7]
#move siegfried (struts) to S1, then to build level 0.5, from reference location
if edge["rz"]>0.1 :
print("not rotated")
siegfried_trans.pos.x += ((globalSetup["edgeRelativeDistance"]["y"]))+edge["y"]*latticePitch["y"]
siegfried_trans.pos.y += ((globalSetup["edgeRelativeDistance"]["x"]))+edge["x"]*latticePitch["x"]
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
siegfried_trans.pos.z -= (globalSetup["edgeRelativeDistance"]["z"])-edge["z"]*latticePitch["z"]
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
siegfried.send_program('set_tool_digital_out(0, False)') #open strut tool
time.sleep(sleepTime1)
siegfried_trans.pos.z += (globalSetup["nodeRelativeDistance"]["z"])
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
else:
print("rotated")
v = globalSetup["v"]
a = globalSetup["a"]
#move siegfried (struts) to S4, then to build level 0.5, from reference location
#uses movej for the rotation and to get close to the build plate, to avoid sketchiness
siegfried.movej(strut90_siegfried_j, acc = a, vel = v)
siegfried_trans = siegfried.get_pose()
time.sleep(sleepTime)
v = globalSetup["v1"]
a = globalSetup["a1"]
siegfried_trans.pos.y += latticePitch["y"]*(edge["y"]+0.5)
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
siegfried_trans.pos.z -= (globalSetup["edgeRelativeDistance"]["z"]-0.008)
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
siegfried.send_program('set_tool_digital_out(0, False)') #open strut tool
time.sleep(sleepTime1)
#use movej to go back to origin point after moving back up
siegfried_trans.pos.z += (globalSetup["nodeRelativeDistance"]["z"])
siegfried.set_pose(siegfried_trans, acc = a, vel = v)
print ("roy's current pose: ", roy.getj())
print ("siegfried's current pose: ", siegfried.getj())