script/data/get_samples_info.py

@@ -0,0 +1,4 @@
+
+
+
+set_return(get_samples_info(as_json=False))

script/data/samples.py

@@ -0,0 +1,23 @@
+import json
+import org.python.core.PyDictionary as PyDictionary
+
+
+samples_info = []
+
+def set_samples_info(info):
+    global samples_info
+    if (is_string(info)):
+        info = json.loads(info)
+    if not is_list(info):
+        raise Exception("Sample info must be a list (given object type is " + str(type(info)) + ")") 
+    #for sample in info:
+    #    if not (type(sample) is PyDictionary):
+    #        raise Exception("Sample info element must be a dictionary (given object type is " + str(type(sample)) + ")") 
+    samples_info = info
+    return samples_info
+
+
+
+def get_samples_info(as_json=True):
+    global sample_info
+    return json.dumps(samples_info) if as_json else samples_info

script/data/set_samples_info.py

@@ -0,0 +1,4 @@
+
+
+
+set_samples_info(args[0])

script/devices/Hexiposi.py

@@ -127,7 +127,7 @@ class Hexiposi(DiscretePositionerBase):
             
         
  
-#http://129.129.110.83:8002/hexiposi/get
+#http://myriotell:8002/hexiposi/get
 dev = Hexiposi("hexiposi", "myriotell:8002/hexiposi")
 
 add_device(dev, True)

script/devices/RobotSC.py

@@ -210,13 +210,19 @@ class RobotSC(RobotTCP):
         robot.waitState(State.Ready, 1000) #robot.state.assertReady()
 
 if simulation:
-    #add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
-    add_device(RobotSC("robot","129.129.110.81:1000"),force = True)
+    add_device(RobotSC("robot","localhost:1000"),force = True)
 else:
-    add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) 
+    #add_device(RobotSC("robot", "129.129.243.120:1000"), force = True) 
+    #add_device(RobotSC("robot", "pcp068129.psi.ch:1000"), force = True) 
+    #add_device(RobotSC("robot", "saresb-cons-06.psi.ch:1000"), force = True) 
+    add_device(RobotSC("robot", "129.129.243.90:1000"), force = True) 
+     
 
-#robot.set_monitor_speed(20)
 robot.set_default_desc(DESC_DEFAULT)
+robot.default_speed = 20
+robot.set_tool(TOOL_DEFAULT)
+robot.setPolling(DEFAULT_ROBOT_POLLING)          
+   
 
 
 class jf1(ReadonlyRegisterBase):

script/devices/RobotTCP.py

@@ -45,7 +45,8 @@ class RobotTCP(TcpDevice, Stoppable):
          self.frame = FRAME_DEFAULT
          self.polling_interval = 0.01
          self.reset = True           
-         self.default_tolerance = 10         
+         self.default_tolerance = 10    
+         self.default_speed = 100     
 
          self.task_start_retries = 3
          self.exception_on_task_start_failure = True  #Tasks may start and be finished when checked
@@ -68,8 +69,11 @@ class RobotTCP(TcpDevice, Stoppable):
         return self.tool
 
 
-    def assert_tool(self, tool):
-        if self.tool != tool:
+    def assert_tool(self, tool=None):
+        if tool  is None: 
+            if self.tool is None:
+                 raise Exception("Tool is undefined")    
+        elif self.tool != tool:
             raise Exception("Invalid tool: " + self.tool)        
 
     def set_default_desc(self,default_desc):
@@ -213,11 +217,13 @@ class RobotTCP(TcpDevice, Stoppable):
 
     def get_tool_trsf(self, name=None):
         if name is None: 
+            self.assert_tool()
             name = self.tool
         return self.get_trsf(name+".trsf")
 
     def set_tool_trsf(self, trsf, name=None):
         if name is None: 
+            self.assert_tool()
             name = self.tool
         self.set_trsf(trsf, name+".trsf")       
 
@@ -275,9 +281,12 @@ class RobotTCP(TcpDevice, Stoppable):
         if (ret==-1): raise Exception("The robot is not in remote working mode")
         if (ret==-2): raise Exception("The monitor speed is under the control of the operator")
         if (ret==-3): raise Exception("The specified speed is not supported")
+
+    def set_default_speed(self):
+        set_monitor_speed(self.default_speed)
     
     def is_calibrated(self):
-        return self.eval_bool("isCalibrated()")
+        return self.eval_bool("isCalibrated()") 
 
     def save_program(self):
         ret = self.execute('save',  timeout=5000)        
@@ -367,6 +376,10 @@ class RobotTCP(TcpDevice, Stoppable):
     def movej(self, joint_or_point, tool=None, desc=None, sync=False):
         if desc is None: desc = self.default_desc
         if tool is None: tool = self.tool
+        #If joint_or_point is a list assumes ir is a point
+        if not is_string(point):
+            robot.set_pnt(joint_or_point , "tcp_p")   
+            joint_or_point =  "tcp_p"         
         ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
         if sync:
             self.wait_end_of_move()        
@@ -375,6 +388,9 @@ class RobotTCP(TcpDevice, Stoppable):
     def movel(self, point, tool=None, desc=None, sync=False):
         if desc is None: desc = self.default_desc
         if tool is None: tool = self.tool
+        if not is_string(point):
+            robot.set_pnt(point , "tcp_p")   
+            point =  "tcp_p"        
         ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
         if sync:
             self.wait_end_of_move()        
@@ -588,13 +604,15 @@ class RobotTCP(TcpDevice, Stoppable):
     def get_flange_pos(self):            
         return self.eval_pnt("jointToPoint(" +FLANGE + ", " + self.frame + ", herej())")
 
-    def get_cartesian_pos(self):            
+    def get_cartesian_pos(self): 
+        self.assert_tool()           
         return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
         #self.set_jnt(self.herej())
         #return self.joint_to_point(tool, frame)        
 
     def get_cartesian_destination(self):
-         return self.here(self.tool, self.frame)
+        self.assert_tool()
+        return self.here(self.tool, self.frame)
 
     def get_distance_to_pnt(self, name):   
         #self.here(self.tool, self.frame) #???
@@ -672,6 +690,7 @@ class RobotTCP(TcpDevice, Stoppable):
                     m.initialize()                
     #Alignment
     def align(self, point = None, frame = FRAME_DEFAULT, desc = None):
+        self.assert_tool()
         if desc is None: desc = self.default_desc
         if point is None:
             self.get_cartesian_pos()

script/local.py

@@ -5,6 +5,7 @@ import traceback
 from ch.psi.pshell.serial import TcpDevice
 from ch.psi.pshell.modbus import ModbusTCP
 import ch.psi.mxsc.Controller as Controller
+import ch.psi.pshell.core.Nameable as Nameable
 
 
 run("setup/Layout")
@@ -29,6 +30,7 @@ add_device(img.getCamera(), force = True)
 #  Utility modules
 ###################################################################################################
 
+run("data/samples")
 run("motion/tools")
 run("motion/mount")
 run("motion/unmount")
@@ -45,6 +47,7 @@ run("motion/move_scanner")
 run("motion/dry")
 run("motion/homing_hexiposi")
 run("motion/robot_recover")
+run("motion/recover")
 run("imgproc/Utils")
 run("tools/Math")
 
@@ -81,10 +84,6 @@ def check_puck_detection():
 def stop_puck_detection():
     run("tools/StopPuckDetection")
 
-DEWAR_LEVEL_RT = 5.0
-
-def is_room_temp():
-    return dewar_level.read() <= DEWAR_LEVEL_RT
 
 
 ###################################################################################################
@@ -111,7 +110,7 @@ except:
 
 try:
     robot.setPolling(DEFAULT_ROBOT_POLLING)          
-    robot.set_tool(TOOL_CALIBRATION)
+    robot.set_tool(TOOL_DEFAULT)
     robot.set_motors_enabled(True)
     robot.set_joint_motors_enabled(True)
 except:
@@ -143,6 +142,36 @@ except:
 
 #gripper_cam.paused = True
 
+
+
+###################################################################################################
+#  Device monitoring
+###################################################################################################
+
+DEWAR_LEVEL_RT = 5.0
+is_room_temperature = False
+
+def is_room_temp():
+    return is_room_temperature
+
+
+class DewarLevelListener (DeviceListener):
+    def onValueChanged(self, device, value, former):
+        global is_room_temperature
+        if value is not None:
+            is_room_temperature =  value <= DEWAR_LEVEL_RT
+dewar_level_listener = DewarLevelListener()
+
+for l in dewar_level.listeners:
+    #if isinstance(l, DewarLevelListener):    #Class changes...
+    if Nameable.getShortClassName(l.getClass()) == "DewarLevelListener":
+        dewar_level.removeListener(l)
+        
+dewar_level.addListener(dewar_level_listener)
+dewar_level_listener.onValueChanged(dewar_level, dewar_level.take(), None)
+
+
+
 ###################################################################################################
 #  Global variables
 ###################################################################################################

script/motion/recover.py

@@ -1,5 +1,109 @@
-def recover():
-    """
-    """
-    print "recover"
+import org.apache.commons.math3.geometry.euclidean.threed.Segment as Segment
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D as Vector3D
+import org.apache.commons.math3.geometry.euclidean.threed.Line as Line3D
+
+
+known_segments = [ ("pHome",         "pPark",         10), \
+                   ("pHome",         "pDewarHome",    10), \
+                   ("pHome",         "pGonioHome",    10), \
+                   ("pHome",         "pScanHome",     10), \
+                   ("pHome",         "pHeaterHome",   10), \
+                   ("pDewarHome",    "pDewarWait",    10), \
+                   ("pGonioHome",    "pGonioGet",     10), \
+                   ("pHeaterHome",   "pHeater",       10), \
+                   ("pHeater",       "pHeaterBottom", 10), \
+                 ]
+
+
+def is_on_segment(segment):
+    tolerance = segment[2]
+    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
+    p =  robot.get_cartesian_pos()
+    v = Vector3D(p[0], p[1], p[2])
+    v1  = Vector3D(p1[0], p1[1], p1[2])
+    v2  = Vector3D(p2[0], p2[1], p2[2])
+    l = Line3D(v1, v2, 0.01)
+
+    #Check if on segment
+    d = l.distance(v)
+    if d > tolerance:
+        #print "Current robot position " + str(p) + " not on segment " + str(segment) + " - distance=" + str(d)
+        return False
+   
+    # Check if inside segment
+    d1, d2 = v1.distance(v), v2.distance(v)
+    if d1>d2: 
+        d1, d2 = d2, d1
+    d1, d2 = d1, d2
+    if d<(d1-tolerance) or d>(d2+tolerance):
+         #print "Current robot position " + str(p) + " not on segment " + str((d1, d2)) +  " of segment " + str(segment) + " - distance=" + str(d)
+         return False
+    #print "Current robot position " + str(p) + " on segment " + str(segment) + " - distance=" + str(d)
+    return True
+
+def get_current_segment():
+    for segment in known_segments:
+        if is_on_segment(segment):    
+            return segment    
+    return None
+
+def move_to_segment(segment):
+    tolerance = segment[2]
+    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
+    p =  robot.get_cartesian_pos()
+    v = Vector3D(p[0], p[1], p[2])
+    v1  = Vector3D(p1[0], p1[1], p1[2])
+    v2  = Vector3D(p2[0], p2[1], p2[2])
+    l = Line3D(v1, v2, 0.01)
     
+    a = l.getAbscissa(v)
+    lv = l.pointAt(a) 
+    dlv = lv.distance(v)
+    if dlv> (tolerance + 0.1):
+        raise Exception( "Error moving from " + str(p) + " to segment - distance=" + str(dlv))
+    d =  [lv.x, lv.y, lv.z, p[3], p[4], p[5]] 
+    print "Moving from " + str(p) + " to segment " + str(segment) + " - distance=" + str(dlv)  + " - dest=" + str(d)
+    robot.movel(d, tool=None, desc=DESC_SLOW, sync=True)       
+
+
+#Moves to first point of the segment ehich is safer, unless in the vicinity of the second
+def move_to_safest_point(segment, vicinity_tolerance = 100):
+    d1, d2 = robot.get_distance_to_pnt(segment[0]),  robot.get_distance_to_pnt(segment[1])
+    if (d2<=d1) and (d2 <= vicinity_tolerance):
+        robot.movel(segment[1], tool=None, desc=DESC_SLOW, sync=True) 
+    else:
+        robot.movel(segment[0], tool=None, desc=DESC_SLOW, sync=True) 
+    #print "Recovered to point " + str(robot.get_current_point())
+
+     
+def recover():
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+
+    if robot.get_current_point() is not None:
+        raise Exception("Robot is in known location")
+       
+    #Enabling
+    enable_motion()
+
+
+    is_on_known_segment = False
+    for segment in known_segments:
+        if is_on_segment(segment):
+            #try:
+            #    robot.set_monitor_speed(5)
+                is_on_known_segment = True
+                move_to_segment(segment)
+                move_to_safest_point(segment)
+                return "Success recovering to point: " +  str(robot.get_current_point())
+            #finally:     
+            #    robot.set_default_speed()
+    if not is_on_known_segment:
+        raise Exception("Robot is not in known segment")
+ 
+
+
+
+   

script/test/TestBugPcAPI2.py

@@ -1,6 +1,6 @@
 import ch.psi.pshell.imaging.MjpegSource as MjpegSource
 MjpegSource2 = get_context().pluginManager.getDynamicClass("MjpegSource2")
-add_device(MjpegSource("gripper_cam", "http://129.129.110.114/axis-cgi/mjpg/video.cgi"), True)
+add_device(MjpegSource("gripper_cam", "http://axis-accc8e9cc87b.psi.ch/axis-cgi/mjpg/video.cgi"), True)
 #gripper_cam.polling=1000
 gripper_cam.monitored = True
 show_panel(gripper_cam)

script/test/TestEuclidean.py

@@ -0,0 +1,23 @@
+import org.apache.commons.math3.geometry.euclidean.threed.Segment as Segment
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D as Vector3D
+import org.apache.commons.math3.geometry.euclidean.threed.Line as Line
+
+
+p = Vector3D(0, 0, 3)
+
+p1 = Vector3D(0,0,0)
+p2 = Vector3D(0,1,1 )
+tolerance = 0.001
+
+l = Line(p1, p2, tolerance)
+
+
+print l.distance(p)
+print p1.distance(p)
+print p2.distance(p)
+print "---"
+
+print l.getAbscissa(p)
+print l.pointAt(l.getAbscissa(p))
+
+#print l.closestPoint(Line(p, p, 0.01))

script/test/TestMicrohawk.py

@@ -2,8 +2,8 @@ import ch.psi.pshell.serial.TcpDevice as TcpDevice
 import ch.psi.pshell.serial.UdpDevice as UdpDevice
 
 
-microscan = TcpDevice("microscan", "129.129.126.200:2001")
-#microscan = UdpDevice("microscan", "129.129.126.200:2001")
+microscan = TcpDevice("microscan", "MicroHAWK38C528:2001")
+#microscan = UdpDevice("microscan", "MicroHAWK38C528:2001")
 microscan.initialize()
 
 

script/test/TestRecover.py

@@ -0,0 +1,15 @@
+print "Pos=" + str(robot.get_cartesian_pos())
+for p in robot.get_known_points():
+    print p + " = " + str(get_pnt(p))
+
+print "-------------"
+
+for segment in known_segments:
+    is_on_segment(segment)
+
+print "-------------"
+for segment in known_segments:
+    try:
+        move_to_segment(segment)
+    except:
+        print sys.exc_info()[1]