154 lines
5.2 KiB
Python
154 lines
5.2 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
|
|
|
|
RECOVER_DESC = "mRecovery"
|
|
RECOVER_TOOL = TOOL_DEFAULT
|
|
|
|
known_segments = [ ("pHome", "pPark", 50), \
|
|
("pHome", "pGonio", 30), \
|
|
("pHome", "pScan", 25), \
|
|
("pHome", "pHeater", 75), \
|
|
("pHome", "pDewar", 10), \
|
|
("pHome", "pHelium", 230), \
|
|
("pGonio", "pGonioG", 10), \
|
|
("pPark", "pHeat", 40), \
|
|
("pHeater", "pHeatB", 10), \
|
|
("pPark", "pAux", 50), \
|
|
]
|
|
|
|
def get_dist_to_line(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)
|
|
return l.distance(v)
|
|
|
|
|
|
def get_dist_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)
|
|
d = l.distance(v)
|
|
|
|
pj = get_pojection_at_line(segment)
|
|
|
|
d1, d2 = v1.distance(v), v2.distance(v)
|
|
dp1, dp2 = v1.distance(pj), v2.distance(pj)
|
|
d12 = v1.distance(v2)
|
|
|
|
|
|
if (dp1 + dp2) > (d12 + tolerance):
|
|
d = max(d,min(d1,d2))
|
|
return d
|
|
|
|
def is_on_segment(segment):
|
|
tolerance = segment[2]
|
|
d = get_dist_to_segment(segment)
|
|
|
|
if d > tolerance:
|
|
#print "Current robot position " + str(p) + " not on 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_pojection_at_line(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)
|
|
return lv
|
|
|
|
|
|
def get_current_segment():
|
|
for segment in known_segments:
|
|
if is_on_segment(segment):
|
|
return segment
|
|
return None
|
|
|
|
def get_current_distance():
|
|
for segment in known_segments:
|
|
if is_on_segment(segment):
|
|
return get_dist_to_segment(segment)
|
|
return None
|
|
|
|
|
|
def move_to_segment(segment):
|
|
tolerance = segment[2]
|
|
p = robot.get_cartesian_pos()
|
|
v = Vector3D(p[0], p[1], p[2])
|
|
lv = get_pojection_at_line(segment)
|
|
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)
|
|
|
|
try:
|
|
robot.movel(d, tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)
|
|
print "Done"
|
|
except:
|
|
print sys.exc_info()[1]
|
|
|
|
#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])
|
|
#Always moving to primary point
|
|
if False : #(d2<=d1) and (d2 <= vicinity_tolerance):
|
|
print "Moving to secondary point " + str(segment[1] + " - d1=" + str(d1) + " d2=" + str(d2) )
|
|
robot.movel(segment[1], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)
|
|
else:
|
|
print "Moving to primary point " + str(segment[0] + " - d1=" + str(d1) + " d2=" + str(d2) )
|
|
robot.movel(segment[0], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)
|
|
print "Done"
|
|
#print "Recovered to point " + str(robot.get_curjoint_or_pointrent_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")
|
|
|
|
set_status("Recovering")
|
|
|
|
#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)
|
|
location = robot.get_current_point()
|
|
if location is None:
|
|
raise Exception("Robot didn't reach known point")
|
|
return "Success recovering to point: " + str(location)
|
|
#finally:
|
|
# robot.set_default_speed()
|
|
if not is_on_known_segment:
|
|
raise Exception("Robot is not in known segment")
|
|
|
|
|
|
|
|
|
|
|