def saoma():
global grapstate
pose0 = s.getPose()
bRet = np.isclose(0.0, pose0.r, rtol=1e-03, atol=1e-03, equal_nan=False)
if not bRet:
print("no 0.0")
raise Exception('r no 0.0')
#return
s.moveAxis(s.axis_z, -0.02)
s.moveXY(cameraposition[0], cameraposition[1], True)
#time.sleep(0.1)
s.moveAxis(s.axis_z, -7)
capturephoto()
time.sleep(2)
def pickdown400():
pose0=s.getPose()
bRet= np.isclose(-90.0, pose0.r, rtol=1e-03, atol=1e-03, equal_nan=False)
if not bRet:
print("no -90.0")
raise Exception('r no 90')
#return
s.moveAxis(s.axis_z,-0.02)
s.moveXY(mediaposition[0],mediaposition[1],True)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-111)
# time.sleep(0.1)
s.controlGripperGrab(False,0.1)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-0.02)
def jiajufang400():
pose0=s.getPose()
bRet= np.isclose(0.0, pose0.r, rtol=1e-03, atol=1e-03, equal_nan=False)
if not bRet:
print("no 0.0")
raise Exception('r no 0.0')
#return
s.moveAxis(s.axis_z,-0.02)
s.moveXY(controlbox1position[0],controlbox1position[1],True)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-105)
# time.sleep(0.1)
s.controlGripperGrab(False,0.1)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-0.02)
def jiajujia400():
global grapstate
pose0=s.getPose()
bRet= np.isclose(0.0, pose0.r, rtol=1e-03, atol=1e-03, equal_nan=False)
if not bRet:
print("no 0.0")
raise Exception('r no 0.0')
#return
if graspstate!='release':
print("no release")
raise EXception('graspstate no release')
#return
s.moveAxis(s.axis_z,-0.02)
s.moveXY(controlbox2position[0],controlbox2position[1],True)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-105)
# time.sleep(0.1)
s.controlGripperGrab(True,0.1)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-0.02)
def jia400():
#s.speed(50)
global grapstate
#global s
pose0=s.getPose()
bRet= np.isclose(-90.0, pose0.r, rtol=1e-03, atol=1e-03, equal_nan=False)
if not bRet:
print("no -90.0")
raise Exception('r no 90')
#return
if graspstate!='release':
raise Exception('graspstate no release')
#return
s.moveAxis(s.axis_z,-0.02)
s.moveXY(mediaposition[0],mediaposition[1],True)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-111)
# time.sleep(0.1)
s.controlGripperGrab(True,0.1)
# time.sleep(0.1)
s.moveAxis(s.axis_z,-0.02)