Master

Surgery_MustRead

@@ -0,0 +1,3 @@
+For the Xarm Sugery program go into example/wrapper, then the xarm6 file 
+
+The program is called Xarm-Python-Sugery-Robot

example/wrapper/xarm6/Xarm-Python-Sugery-Robot.py

@@ -0,0 +1,241 @@
+from cgitb import reset
+from logging import root
+import os
+import sys
+import time
+import math
+from tracemalloc import stop
+import keyboard
+import tkinter as tk  
+
+sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
+
+from xarm.wrapper import XArmAPI
+
+#Figure out how to stop it from asking for IP later
+#####################################################################################
+
+if len(sys.argv) >= 2:
+    ip = sys.argv[1]
+else:
+    try:
+        from configparser import ConfigParser
+        parser = ConfigParser()
+        parser.read('../robot.conf')
+        ip = parser.get('xArm', 'ip')
+    except:
+        # ip = input('Please input the xArm ip address:')
+        ip = "192.168.1.200"
+        if not ip:
+            print('input error, exit')
+            sys.exit(1)
+
+#####################################################################################
+
+counter = 0
+posCounter = 0
+SurgeryPos = 0
+speed = 50
+arm = XArmAPI(ip)
+arm.motion_enable(enable=True)
+arm.set_mode(0)
+arm.set_state(state=0)
+#Reset Variables:
+Rx = 71.8
+Ry = -355.8
+Rz = 320.8 #290
+Rr = -173.8
+Rp = -0.2
+RYaw = -90.9
+
+#Equation Variables
+dx = 0
+dy = 0
+dz = 0
+dR = 0.0
+dP = 0.0
+dY = 0.0
+
+#Starting Variables
+f1 = [101.576, -316.251, 209.116, -175.62754973008145, -13.643901271229808, -85.68566637447609]
+f2 = [87.847, -400.419, 208.735, -175.3408989451775, 19.076515197321733, -95.66195020751445]
+f3 = [51.469, -297.305, 189.965, -176.05898694981497, -14.620737016148349, -91.83035224835659]
+f4 = [6.408, -430.872, 163.482, -162.0227241804796, 41.34429072196313, -111.83929896147423]
+f5 = [-42.856, -302.035, 129.169, -158.26962143925462, 52.37699413766372, -168.08003399060263]
+t1 = [137.1,-357.5,213.9,170.8,8.9,-70]
+t2 = [-20.3,-398.6,160,-165.2,28.5,-127.2]
+
+zoffset = 10 # units of mm
+
+
+def confirmPosition():
+    global dx
+    global dy
+    global dz
+    global dY
+    global newPos
+    if counter == 1 :
+        newPos = arm.position
+        dx += newPos[0] - f1[0]
+        dy += newPos[1] - f1[1]
+        dz += newPos[2] - f1[2]
+        f1[0] = newPos[0]
+        #dy += newPos[1] - f1[1]
+        f1[1] = newPos[1]
+        #dz += newPos[2] - f1[2]
+        f1[2] = newPos[2]
+        f1[3] = newPos[3]
+        #dR = newPos[3] - f1[3]
+        f1[4] = newPos[4]
+        #dP = newPos[4] - f1[4]
+        f1[5] = newPos[5]
+        #dY += newPos[5] - f1[5]
+        print(newPos) 
+        #print(dx,dy,dz)
+
+    if counter == 2 :
+        newPos = arm.position
+        dx += newPos[0] - f2[0]
+        dy += newPos[1] - f2[1]
+        dz += newPos[2] - f2[2]
+        f2[0] = newPos[0]
+        f2[1] = newPos[1]
+        f2[2] = newPos[2]
+        f2[3] = newPos[3]
+        f2[4] = newPos[4]
+        f2[5] = newPos[5]
+
+        print(newPos) 
+
+    if counter == 3 :
+        newPos = arm.position
+        dx += newPos[0] - f3[0]
+        dy += newPos[1] - f3[1]
+        dz += newPos[2] - f3[2]
+        f3[0] = newPos[0]
+        f3[1] = newPos[1]
+        f3[2] = newPos[2]
+        f3[3] = newPos[3]
+        f3[4] = newPos[4]
+        f3[5] = newPos[5]
+        print(newPos) 
+
+    if counter == 4 :
+        newPos = arm.position
+        dx += newPos[0] - f4[0]
+        dy += newPos[1] - f4[1]
+        dz += newPos[2] - f4[2]
+        f4[0] = newPos[0]
+        f4[1] = newPos[1]
+        f4[2] = newPos[2]
+        f4[3] = newPos[3]
+        f4[4] = newPos[4]
+        f4[5] = newPos[5]
+        print(newPos) 
+
+    if counter == 5 :
+        newPos = arm.position
+        dx += newPos[0] - f5[0]
+        dy += newPos[1] - f5[1]
+        dz += newPos[2] - f5[2]
+        f5[0] = newPos[0]
+        f5[1] = newPos[1]
+        f5[2] = newPos[2]
+        f5[3] = newPos[3]
+        f5[4] = newPos[4]
+        f5[5] = newPos[5]
+        print(newPos) 
+
+    #displacement averages
+    dx = dx
+    dy = dy
+    dz = dz
+    print(dx,dy,dz)
+
+
+def increment_position():
+    global counter
+    counter += 1 
+    print(counter)
+
+    if counter == 1 :
+            arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+            time.sleep(1)
+            arm.set_position(f1[0],f1[1],f1[2]+zoffset,f1[3],f1[4],f1[5], speed=80, wait=True)
+            find_position = tk.Label(window, text=str("Go to the UFACTORY Studio and go into live control and turn the robot into manual mode and move it to the exact point of the feducial"), font=("Helvetica", 16)) 
+            find_position.pack(pady=5)
+    elif  counter == 2:
+            arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+            time.sleep(1)
+            arm.set_position(f2[0],f2[1],f2[2]+zoffset,f2[3],f2[4],f2[5], speed=80, wait=True)
+    elif  counter == 3:   
+            arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+            time.sleep(1)  
+            arm.set_position(f3[0],f3[1],f3[2]+zoffset,f3[3],f3[4],f3[5], speed=80, wait=True)          
+    elif  counter == 4:
+            arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+            time.sleep(1)
+            arm.set_position(f4[0],f4[1],f4[2]+zoffset,f4[3],f4[4],f4[5], speed=80, wait=True)           
+    elif  counter == 5:
+            arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+            time.sleep(1)
+            arm.set_position(f5[0],f5[1],f5[2]+zoffset,f5[3],f5[4],f5[5], speed=80, wait=True)   
+    enable() 
+
+def BeginSurgery():
+    arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+    time.sleep(1)
+    arm.set_position(t1[0]+(dx/5),t1[1]+(dy/5),t1[2]+(dz/5),t1[3],t1[4],t1[5], speed=80, wait=True)
+
+def nextSurgeryPos():
+     arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+     time.sleep(1)
+     arm.set_position(t2[0]+(dx/5),t2[1]+(dy/5),t2[2]+(dz/5),t2[3],t2[4],t2[5], speed=80, wait=True)
+
+def enable():
+    register_button.config(state=tk.NORMAL, text="Register Subject") #Enable the button and set its text
+
+def reset_position():
+    global counter
+    global zoffset
+    counter = 0 
+    zoffset = 0
+    arm.set_position(Rx, Ry, Rz, Rr, Rp, RYaw, speed=80, wait=True)
+
+def quit():
+      exit() 
+
+def getPosition(): 
+      #posCounter += 1 
+      #print("posCounter: ",posCounter)
+      display_position = tk.Label(position_window, text=str(arm.position), font=("Helvetica", 16)) 
+      display_position.pack(pady=5)
+
+
+window = tk.Tk()
+window.title("Subject Registration")
+register_button = tk.Button(window, text="Register Subject",command = increment_position) 
+register_button.pack(pady=20) 
+
+reset_button = tk.Button(window, text="Reset XArm",command=reset_position) 
+reset_button.pack(pady=40)
+
+quit_button = tk.Button(window, text="Quit Program",command=quit) 
+quit_button.pack(pady=50)
+
+position_window = tk.Toplevel(window)
+position_window.title("List of positions")
+
+position_button = tk.Button(window, text="position",command=getPosition) 
+position_button.pack(pady=70)
+
+changePos_button = tk.Button(window, text="Confirm fine tune position",command=confirmPosition) 
+changePos_button.pack(pady=72)
+
+StartSurgery_button = tk.Button(window, text="Start Surgery",command=BeginSurgery) 
+StartSurgery_button.place(x=30, y=20)  
+
+ContinueSurgery_button = tk.Button(window, text="Continue Surgery",command=nextSurgeryPos) 
+ContinueSurgery_button.place(x=30, y=100)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
+
+window.mainloop()