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")
+ 
+
+
+
+