ur10test.py

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())