Skip to content
Snippets Groups Projects
Select Git revision
  • ac61b81a0b20b86a4865d5e2802f704ad1075541
  • master default protected
  • LUFA-170418
  • LUFA-151115
  • LUFA-140928
  • LUFA-140302
  • LUFA-130901
  • LUFA-130901-BETA
  • LUFA-130303
  • LUFA-120730
  • LUFA-120730-BETA
  • LUFA-120219
  • LUFA-120219-BETA
  • LUFA-111009
  • LUFA-111009-BETA
  • LUFA-110528
  • LUFA-110528-BETA
17 results

Descriptors.c

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