diff --git a/Surgery_MustRead b/Surgery_MustRead new file mode 100644 index 00000000..c0cbd3a2 --- /dev/null +++ b/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 diff --git a/example/wrapper/xarm6/Xarm-Python-Sugery-Robot.py b/example/wrapper/xarm6/Xarm-Python-Sugery-Robot.py new file mode 100644 index 00000000..613a2e59 --- /dev/null +++ b/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()