109 lines
3.9 KiB
Python
109 lines
3.9 KiB
Python
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")
|
|
|
|
|
|
|
|
|
|
|