2023-09-14 11:48:21 +02:00
parent 8e14004e1a
commit 681e3d76bf
33 changed files with 200 additions and 87 deletions
@@ -1,50 +1,58 @@
-#Wed Feb 01 14:17:52 CET 2023
-autoSaveScanData=true
-simulation=false
-dataScanSaveOutput=false
-userAuthenticator=
-dataScanSaveScript=false
-notifiedTasks=
-parallelInitialization=false
-fdaSerialization=false
-dataTransferPath=
-saveConsoleSessionFiles=false
-hostName=
-disableEmbeddedAttributes=false
-serverPort=8080
-versionTrackingEnabled=true
-dataPath={data}/{year}_{month}/{date}/{date}_{time}_{name}
-serverEnabled=true
-depthDimension=0
-logLevel=Info
-dataLayout=default
-disableDataFileLogs=false
-sessionHandling=Off
-terminalEnabled=false
-notificationLevel=Off
-terminalPort=3579
-dataTransferUser=
-versionTrackingLogin={context}/svcusr-hlapp_robot
-noBytecodeFiles=false
-versionTrackingRemote=git@git.psi.ch\:pshell_config/bernina_robot.git
-dataScanLazyTableCreation=false
-logDaysToLive=-1
-logLevelConsole=Off
-filePermissionsConfig=Public
-scanStreamerPort=-1
-dataScanSaveSetpoints=false
-versionTrackingManual=true
-dataTransferMode=Off
-userManagement=false
-instanceName=
-dataServerPort=-1
-hideServerMessages=false
-dataScanReleaseRecords=false
-dataScanPreserveTypes=false
-dataScanFlushRecords=false
-filePermissionsLogs=Public
-logPath={logs}/{date}_{time}
-filePermissionsScripts=Public
-filePermissionsData=Default
-dataProvider=h5
-saveCommandStatistics=false
+#Mon May 15 16:26:58 CEST 2023
+autoSaveScanData=true
+dataLayout=default
+dataPath={data}/{year}_{month}/{date}/{date}_{time}_{name}
+dataProvider=h5
+dataScanFlushRecords=false
+dataScanLazyTableCreation=false
+dataScanPreserveTypes=false
+dataScanReleaseRecords=false
+dataScanSaveOutput=false
+dataScanSaveScript=false
+dataScanSaveSetpoints=false
+dataServerPort=-1
+dataTransferMode=Off
+dataTransferPath=
+dataTransferUser=
+depthDimension=0
+disableDataFileLogs=false
+disableEmbeddedAttributes=false
+fdaSerialization=false
+filePermissionsConfig=Public
+filePermissionsData=Default
+filePermissionsLogs=Public
+filePermissionsScripts=Public
+hideServerMessages=false
+hostName=
+instanceName=
+logDaysToLive=-1
+logLevel=Info
+logLevelConsole=Off
+logPath={logs}/{date}_{time}
+noBytecodeFiles=false
+notificationLevel=Off
+notifiedTasks=
+parallelInitialization=false
+saveCommandStatistics=false
+saveConsoleSessionFiles=false
+scanStreamerPort=-1
+serverEnabled=true
+serverPort=8080
+sessionHandling=Off
+simulation=false
+terminalEnabled=false
+terminalPort=3579
+userAuthenticator=
+userManagement=false
+versionTrackingEnabled=true
+versionTrackingLogin={context}/svcusr-hlapp_robot
+versionTrackingManual=true
+versionTrackingRemote=git@git.psi.ch\:pshell_config/bernina_robot.git
+xscanAppendSuffix=true
+xscanContinuousUpdate=false
+xscanCrlogicAbortable=true
+xscanCrlogicChannel=null
+xscanCrlogicIoc=null
+xscanCrlogicPrefix=null
+xscanCrlogicSimulated=false
+xscanMoveTimeout=600
@@ -1,3 +1,3 @@
-$cam_n=ch.psi.pshell.imaging.MjpegSource|http://bernina-cam-n/axis-cgi/mjpg/video.cgi reopen||-200|false
-$cam_s=ch.psi.pshell.imaging.MjpegSource|http://bernina-cam-s/axis-cgi/mjpg/video.cgi reopen||-200|
-$cam_w=ch.psi.pshell.imaging.MjpegSource|http://bernina-cam-w/axis-cgi/mjpg/video.cgi reopen||-200|
+cam_n=ch.psi.pshell.imaging.MjpegSource|http://bernina-cam-n/axis-cgi/mjpg/video.cgi reopen||-200|false
+cam_s=ch.psi.pshell.imaging.MjpegSource|http://bernina-cam-s/axis-cgi/mjpg/video.cgi reopen||-200|
+cam_w=ch.psi.pshell.imaging.MjpegSource|http://bernina-cam-w/axis-cgi/mjpg/video.cgi reopen||-200|
@@ -0,0 +1,11 @@
+#Thu Sep 14 10:26:39 CEST 2023
+description=null
+maxValue=NaN
+minValue=NaN
+offset=0.0
+precision=-1
+resolution=NaN
+rotation=false
+scale=1.0
+sign_bit=0
+unit=null
@@ -0,0 +1,11 @@
+#Thu Sep 14 10:26:39 CEST 2023
+description=null
+maxValue=NaN
+minValue=NaN
+offset=0.0
+precision=-1
+resolution=NaN
+rotation=false
+scale=1.0
+sign_bit=0
+unit=null
@@ -0,0 +1,11 @@
+#Thu Sep 14 10:26:39 CEST 2023
+description=null
+maxValue=NaN
+minValue=NaN
+offset=0.0
+precision=-1
+resolution=NaN
+rotation=false
+scale=1.0
+sign_bit=0
+unit=null
@@ -0,0 +1,11 @@
+#Thu Sep 14 10:26:39 CEST 2023
+description=null
+maxValue=NaN
+minValue=NaN
+offset=0.0
+precision=-1
+resolution=NaN
+rotation=false
+scale=1.0
+sign_bit=0
+unit=null
@@ -0,0 +1,11 @@
+#Thu Sep 14 10:26:39 CEST 2023
+description=null
+maxValue=NaN
+minValue=NaN
+offset=0.0
+precision=-1
+resolution=NaN
+rotation=false
+scale=1.0
+sign_bit=0
+unit=null
@@ -0,0 +1,11 @@
+#Thu Sep 14 10:26:39 CEST 2023
+description=null
+maxValue=NaN
+minValue=NaN
+offset=0.0
+precision=-1
+resolution=NaN
+rotation=false
+scale=1.0
+sign_bit=0
+unit=null
@@ -1,15 +1,20 @@
 KNOWN_POINTS = P_PARK, P_HOME = "park", "home"
 TASKS = MOVE_PARK, MOVE_HOME, TWEAK_X , TWEAK_Y  =  "movePark", "moveHome", "tweakX", "tweakY"

-DESCS = DESC_FAST,DESC_SLOW, = "mFast", "mSlow"
+DESCS = DESC_FAST,DESC_SLOW, = "mNomSpeed", "mNomSpeed"
 DESC_DEFAULT = DESCS[0]

-TOOLS = TOOL_DET = "tDet"
-TOOL_DEFAULT = TOOLS[0]
+FLANGE = "flange"
+
+TOOLS = ["t_Detector",]
+TOOL_DEFAULT = TOOL_DET =  TOOLS[0]
+
+FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"]
+FRAME_DEFAULT = FRAMES[0]

 DEFAULT_ROBOT_POLLING = 500
 TASK_WAIT_ROBOT_POLLING = 50
-DEFAULT_SPEED=20
+DEFAULT_SPEED=100

 run("devices/RobotTCP")

@@ -17,7 +22,7 @@ run("devices/RobotTCP")
 simulation = True


-class RobotSC(RobotTCP):    
+class RobotBernina(RobotTCP):    
    def __init__(self, name, server, timeout = 1000, retries = 1):
        RobotTCP.__init__(self, name, server, timeout, retries)   
        self.set_tasks(TASKS)
@@ -25,9 +30,30 @@ class RobotSC(RobotTCP):
        self.setPolling(DEFAULT_ROBOT_POLLING)          
        self.last_command_timestamp = None
        self.last_command_position = None
-        self.setSimulated() # TODO: Remove me  
-       
+        #self.setSimulated() # TODO: Remove me  

+    def move_spherical(self, r=None, gamma=None, delta=None):
+        #def movel(self, point, tool=None, desc=None, sync=False)
+        #def movec(self, point_interm, point_target, tool=None, desc=None, sync=False)
+    
+        if r is not None:
+            #separate in changing angle (movec) and changing r (movel)
+            print("not implemented")
+        else:
+            x,y,z,rx,ry,rz = self.get_cartesian_pos()
+            r = np.sqrt(x**2+y**2+z**2)
+            z = r*np.sin(np.deg2rad(90-delta))*np.cos(np.deg2rad(gamma))
+            x = r*np.sin(np.deg2rad(90-delta))*np.sin(np.deg2rad(gamma))
+            y = r*np.cos(np.deg2rad(90-delta))
+            ry = gamma
+            rx = -delta
+            # set_pnt A
+            # set_pnt B
+            #eval(movectl
+            movec
+            
+            
+    
    def move_home(self):
        if not self.is_in_point(P_HOME):
            self.start_task(MOVE_HOME)  
@@ -109,10 +135,11 @@ class RobotSC(RobotTCP):
        self.waitState(State.Ready, 1000) #robot.state.assertReady()

 if simulation:
-    add_device(RobotSC("robot","localhost:1000"),force = True)
+    add_device(RobotBernina("robot","localhost:1234"),force = True)
 else:
-    add_device(RobotSC("robot", "TellRobot6S:1000"), force = True) 
-     
+    add_device(RobotBernina("robot", "129.129.243.105:1234"), force = True)
+
+time.sleep(0.1)

 #robot.latency = 0.005   
 robot.set_default_desc(DESC_DEFAULT)
@@ -1,8 +1,5 @@
 import threading

-FRAME_DEFAULT = "world"
-FLANGE = "flange"
-
 MAX_NUMBER_PARAMETERS = 20

 run("devices/RobotMotors")
@@ -391,11 +388,11 @@ class RobotTCP(TcpDevice, Stoppable):

    def reset_motion(self, joint=None, timeout=None):
        #TODO: in new robot robot.resetMotion() is freezing controller
-        #self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))
-        if joint is None:
-            self.execute('reset', timeout=timeout)
-        else:
-            self.execute('reset', str(joint), timeout=timeout)
+        self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))
+        #if joint is None:
+        #    self.execute('reset', timeout=timeout)
+        #else:
+        #    self.execute('reset', str(joint), timeout=timeout)

    def is_empty(self):
        self.empty = self.eval_bool("isEmpty()")
@@ -433,8 +430,8 @@ class RobotTCP(TcpDevice, Stoppable):
            joint_or_point =  "tcp_p"         
                    
        #TODO: in new robot movel and movej is freezing controller
-        #ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
-        ret = int(self.execute('movej',joint_or_point, tool, desc))
+        ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
+        #ret = int(self.execute('movej',joint_or_point, tool, desc))
        
        if sync:
            self.wait_end_of_move()    
@@ -451,8 +448,8 @@ class RobotTCP(TcpDevice, Stoppable):
            robot.set_pnt(point , "tcp_p")   
            point =  "tcp_p"        
        #TODO: in new robot movel and movej is freezing controller
-        #ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
-        ret = int(self.execute('movel',point, tool, desc))
+        ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
+        #ret = int(self.execute('movel',point, tool, desc))
        
        if sync:
            self.wait_end_of_move() 
@@ -467,8 +464,8 @@ class RobotTCP(TcpDevice, Stoppable):
        if tool is None: tool = self.tool
        
        #TODO: in new robot  movel and movej is freezing controller
-        #ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
-        ret = int(self.execute('movec', point_interm, point_target, tool, desc))
+        ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
+        #ret = int(self.execute('movec', point_interm, point_target, tool, desc))
                
        if sync:
            self.wait_end_of_move()  
@@ -580,8 +577,9 @@ class RobotTCP(TcpDevice, Stoppable):

        #TODO: in new robot exec taskCreate is freezing controller
        #REMOVE if bug is fixed
-        self.execute('task_create',name, str(priority), program, *args)
-        #self.evaluate('taskCreate "' + name + '", ' + str(priority) + ', ' + cmd)        
+        #self.execute('task_create',name, str(priority), program, *args)
+        print 'taskCreate "' + name + '", ' + str(priority) + ', ' + cmd
+        self.evaluate('taskCreate "' + name + '", ' + str(priority) + ', ' + cmd)        
        if self.isSimulated():
            self.simulated_point = ""                         
        
@@ -728,15 +726,17 @@ class RobotTCP(TcpDevice, Stoppable):
            frame = self.frame
         #Do not work        
        #self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
-        #return self.get_pnt()
-        a = self.execute('get_pos', tool, frame)
-        ret = []
-        for i in range(6): ret.append(float(a[i]))
-        return ret   
+        self.evaluate("tcp_j=herej()")
+        self.evaluate("tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
+        return self.get_pnt()
+        #a = self.execute('get_pos', tool, frame)
+        #ret = []
+        #for i in range(6): ret.append(float(a[i]))
+        #return ret   


    def get_flange_pos(self, frame=None):            
-        return get_cartesian_pos(FLANGE, frame)
+        return self.get_cartesian_pos(FLANGE, frame)


    def get_cartesian_destination(self, tool=None, frame=None):
@@ -770,7 +770,12 @@ class RobotTCP(TcpDevice, Stoppable):
    def get_distance_to_pnts(self, *pars):
        if self.isSimulated():
            return [self.get_distance_to_pnt(p) for p in pars]
-        ret = self.execute("dist_pnt", *pars)
+
+        if self.is_emulation():    
+            ret = [self.get_distance_to_pnt(p) for p in pars]
+        else:
+            ret = self.execute("dist_pnt", *pars) #TODO: Emulation controler is crashing        
+        
        ret = ret[0:len(pars)]
        for i in range(len(ret)):
            try:
@@ -799,7 +804,8 @@ class RobotTCP(TcpDevice, Stoppable):
        if not self.is_in_point(p, tolerance):
            raise Exception ("Not in position " + p)

-    
+    def is_emulation(self):
+        return "localhost" in robot.client.getServerAddress()
    #Cartesian peudo-motors
    def set_motors_enabled(self, value):
        if value !=self.cartesian_motors_enabled:
@@ -0,0 +1,6 @@
+robot.movej("park", sync=True)
+robot.movej("home", sync=True)
+robot.movel("park", sync=True)
+robot.movel("home", sync=True)
+robot.movec("p1","park", sync=True)
+robot.movec("p1","home", sync=True)