pshell_config/mxsc
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
gac-S_Changer
2018-08-29 10:19:38 +02:00
parent 1af729d724
commit 1229efe275
12 changed files with 233 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
script/motion/dry.py
-25
View File
@@ -1,29 +1,4 @@
def set_air_stream(state):
"""
"""
valve_1.write(state)
valve_2.write(not state)
def set_heater(state):
"""
"""
gripper_dryer.write(state)
def get_air_stream():
"""
"""
return valve_1.read()
def get_heater():
"""
"""
return gripper_dryer.read()
def dry(heat_time, speed):
"""
script/motion/recover.py
+73 -33
View File
@@ -2,20 +2,21 @@ 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", 10), \
("pHome", "pDewarHome", 10), \
("pHome", "pGonioHome", 10), \
("pHome", "pScanHome", 10), \
("pHome", "pHeaterHome", 10), \
known_segments = [ ("pHome", "pPark", 50), \
("pHome", "pDewarHome", 30), \
("pHome", "pGonioHome", 30), \
("pHome", "pScanHome", 25), \
("pHome", "pHeaterHome", 25), \
("pDewarHome", "pDewarWait", 10), \
("pGonioHome", "pGonioGet", 10), \
("pHeaterHome", "pHeater", 10), \
("pHeater", "pHeaterBottom", 10), \
]
def is_on_segment(segment):
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()
@@ -23,57 +24,93 @@ def is_on_segment(segment):
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)
#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_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]
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)
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]]
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)
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])
if (d2<=d1) and (d2 <= vicinity_tolerance):
robot.movel(segment[1], tool=None, desc=DESC_SLOW, sync=True)
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:
robot.movel(segment[0], tool=None, desc=DESC_SLOW, sync=True)
#print "Recovered to point " + str(robot.get_current_point())
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():
@@ -97,7 +134,10 @@ def recover():
is_on_known_segment = True
move_to_segment(segment)
move_to_safest_point(segment)
return "Success recovering to point: " + str(robot.get_current_point())
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: