debgug

01_Code/physical_computing_interface/assembly/python-urx/ur10test.py

@@ -112,6 +112,14 @@ for node in setup["nodes"]:
     roy.set_pose(roy_trans, acc = a, vel = v)
     time.sleep(sleepTime)
 
+    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)
+
 
     
 print("")
@@ -197,6 +205,14 @@ for edge in setup["edges"]:
       #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)
+    
+    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)
 
 
 print ("roy's current pose: ", roy.getj())