def calculate_fw_heading_vector( self, follow_direction ):
# get the necessary variables for the working set of sensors
# the working set is the sensors on the side we are bearing on, indexed from rearmost to foremost on the robot
# NOTE: uses preexisting knowledge of the how the sensors are stored and indexed
if follow_direction == FWDIR_LEFT:
# if we are following to the left, we bear on the righthand sensors
sensor_placements = self.proximity_sensor_placements[7:3:-1]
sensor_distances = self.supervisor.proximity_sensor_distances()[7:3:-1]
sensor_detections = self.supervisor.proximity_sensor_positive_detections()[7:3:-1]
elif follow_direction == FWDIR_RIGHT:
# if we are following to the right, we bear on the lefthand sensors
sensor_placements = self.proximity_sensor_placements[:4]
sensor_distances = self.supervisor.proximity_sensor_distances()[:4]
sensor_detections = self.supervisor.proximity_sensor_positive_detections()[:4]
else:
raise Exception( "unknown wall-following direction" )
if True not in sensor_detections:
# if there is no wall to track detected, we default to predefined reference points
# NOTE: these points are designed to turn the robot towards the bearing side, which aids with cornering behavior
# the resulting heading vector is also meant to point close to directly aft of the robot
# this helps when determining switching conditions in the supervisor state machine
p1 = [ -0.2, 0.0 ]
if follow_direction == FWDIR_LEFT: p2 = [ -0.2, -0.0001 ]
if follow_direction == FWDIR_RIGHT: p2 = [ -0.2, 0.0001 ]
else:
# sort the sensor distances along with their corresponding indices
sensor_distances, indices = zip( *sorted( zip( # this method ensures two different sensors are always used
sensor_distances, # sensor distances
[0, 1, 2, 3] # corresponding indices
) ) )
# get the smallest sensor distances and their corresponding indices
d1, d2 = sensor_distances[0:2]
i1, i2 = indices[0:2]
# calculate the vectors to the obstacle in the robot's reference frame
sensor1_pos, sensor1_theta = sensor_placements[i1].vunpack()
sensor2_pos, sensor2_theta = sensor_placements[i2].vunpack()
p1, p2 = [ d1, 0.0 ], [ d2, 0.0 ]
p1 = linalg.rotate_and_translate_vector( p1, sensor1_theta, sensor1_pos )
p2 = linalg.rotate_and_translate_vector( p2, sensor2_theta, sensor2_pos )
# ensure p2 is forward of p1
if i2 < i1: p1, p2 = p2, p1
# compute the key vectors and auxiliary data
l_wall_surface = [ p2, p1 ]
l_parallel_component = linalg.sub( p2, p1 )
l_distance_vector = linalg.sub( p1, linalg.proj( p1, l_parallel_component ) )
unit_perp = linalg.unit( l_distance_vector )
distance_desired = linalg.scale( unit_perp, self.follow_distance )
l_perpendicular_component = linalg.sub( l_distance_vector, distance_desired )
l_fw_heading_vector = linalg.add( l_parallel_component, l_perpendicular_component )
return l_fw_heading_vector, l_parallel_component, l_perpendicular_component, l_distance_vector, l_wall_surface
def calculate_fw_heading_vector( self, follow_direction ):
# get the necessary variables for the working set of sensors
# the working set is the sensors on the side we are bearing on, indexed from rearmost to foremost on the robot
# NOTE: uses preexisting knowledge of the how the sensors are stored and indexed
if follow_direction == FWDIR_LEFT:
# if we are following to the left, we bear on the righthand sensors
sensor_placements = self.proximity_sensor_placements[7:3:-1]
sensor_distances = self.supervisor.proximity_sensor_distances()[7:3:-1]
sensor_detections = self.supervisor.proximity_sensor_positive_detections()[7:3:-1]
elif follow_direction == FWDIR_RIGHT:
# if we are following to the right, we bear on the lefthand sensors
sensor_placements = self.proximity_sensor_placements[:4]
sensor_distances = self.supervisor.proximity_sensor_distances()[:4]
sensor_detections = self.supervisor.proximity_sensor_positive_detections()[:4]
else:
raise Exception( "unknown wall-following direction" )
if True not in sensor_detections:
# if there is no wall to track detected, we default to predefined reference points
# NOTE: these points are designed to turn the robot towards the bearing side, which aids with cornering behavior
# the resulting heading vector is also meant to point close to directly aft of the robot
# this helps when determining switching conditions in the supervisor state machine
p1 = [ -0.2, 0.0 ]
if follow_direction == FWDIR_LEFT: p2 = [ -0.2, -0.0001 ]
if follow_direction == FWDIR_RIGHT: p2 = [ -0.2, 0.0001 ]
else:
# sort the sensor distances along with their corresponding indices
sensor_distances, indices = zip( *sorted( zip( # this method ensures two different sensors are always used
sensor_distances, # sensor distances
[0, 1, 2, 3] # corresponding indices
) ) )
# get the smallest sensor distances and their corresponding indices
d1, d2 = sensor_distances[0:2]
i1, i2 = indices[0:2]
# calculate the vectors to the obstacle in the robot's reference frame
sensor1_pos, sensor1_theta = sensor_placements[i1].vunpack()
sensor2_pos, sensor2_theta = sensor_placements[i2].vunpack()
p1, p2 = [ d1, 0.0 ], [ d2, 0.0 ]
p1 = linalg.rotate_and_translate_vector( p1, sensor1_theta, sensor1_pos )
p2 = linalg.rotate_and_translate_vector( p2, sensor2_theta, sensor2_pos )
# ensure p2 is forward of p1
if i2 < i1: p1, p2 = p2, p1
# compute the key vectors and auxiliary data
l_wall_surface = [ p2, p1 ]
l_parallel_component = linalg.sub( p2, p1 )
l_distance_vector = linalg.sub( p1, linalg.proj( p1, l_parallel_component ) )
unit_perp = linalg.unit( l_distance_vector )
distance_desired = linalg.scale( unit_perp, self.follow_distance )
l_perpendicular_component = linalg.sub( l_distance_vector, distance_desired )
l_fw_heading_vector = linalg.add( l_parallel_component, l_perpendicular_component )
return l_fw_heading_vector, l_parallel_component, l_perpendicular_component, l_distance_vector, l_wall_surface
def convex_polygon_intersect_test(polygon1, polygon2):
# assign polygons according to which has fewest sides - we will test against the polygon with fewer sides first
if polygon1.numedges() <= polygon2.numedges():
polygonA = polygon1
polygonB = polygon2
else:
polygonA = polygon2
polygonB = polygon1
# perform Seperating Axis Test
intersect = True
edge_index = 0
edges = polygonA.edges() + polygonB.edges()
while intersect and edge_index < len(
edges
): # loop through the edges of polygonA searching for a separating axis
# get an axis normal to the current edge
edge = edges[edge_index]
edge_vector = linalg.sub(edge[1], edge[0])
projection_axis = linalg.lnormal(edge_vector)
# get the projection ranges for each polygon onto the projection axis
minA, maxA = range_project_polygon(projection_axis, polygonA)
minB, maxB = range_project_polygon(projection_axis, polygonB)
# test if projections overlap
if minA > maxB or maxA < minB:
intersect = False
edge_index += 1
return intersect
def convex_polygon_intersect_test( polygon1, polygon2 ):
# assign polygons according to which has fewest sides - we will test against the polygon with fewer sides first
if polygon1.numedges() <= polygon2.numedges():
polygonA = polygon1
polygonB = polygon2
else:
polygonA = polygon2
polygonB = polygon1
# perform Seperating Axis Test
intersect = True
edge_index = 0
edges = polygonA.edges() + polygonB.edges()
while intersect == True and edge_index < len( edges ): # loop through the edges of polygonA searching for a separating axis
# get an axis normal to the current edge
edge = edges[ edge_index ]
edge_vector = linalg.sub( edge[1], edge[0] )
projection_axis = linalg.lnormal( edge_vector )
# get the projection ranges for each polygon onto the projection axis
minA, maxA = range_project_polygon( projection_axis, polygonA )
minB, maxB = range_project_polygon( projection_axis, polygonB )
# test if projections overlap
if minA > maxB or maxA < minB:
intersect = False
edge_index += 1
return intersect
def _draw_detection_to_frame(self):
target_delta = self.proximity_sensor.target_delta
if target_delta != None:
detector_endpoints = self.proximity_sensor.detector_line.vertexes
detector_vector = linalg.sub(detector_endpoints[1], detector_endpoints[0])
target_vector = linalg.add(detector_endpoints[0], linalg.scale(detector_vector, target_delta))
self.viewer.current_frame.add_circle(pos=target_vector, radius=0.02, color="black", alpha=0.7)
def line_segment_intersection(line1, line2):
# see http://stackoverflow.com/questions/563198
nointersect_symbol = (False, None, None)
p1, r1 = line1[0], linalg.sub(line1[1], line1[0])
p2, r2 = line2[0], linalg.sub(line2[1], line2[0])
r1xr2 = float(linalg.cross(r1, r2))
if r1xr2 == 0.0: return nointersect_symbol
p2subp1 = linalg.sub(p2, p1)
d1 = linalg.cross(p2subp1, r2) / r1xr2
d2 = linalg.cross(p2subp1, r1) / r1xr2
if d1 >= 0.0 and d1 <= 1.0 and d2 >= 0.0 and d2 <= 1.0:
return True, linalg.add(p1, linalg.scale(r1, d1)), d1
else:
return nointersect_symbol
def line_segment_intersection( line1, line2 ):
# see http://stackoverflow.com/questions/563198
nointersect_symbol = ( False, None, None )
p1, r1 = line1[0], linalg.sub( line1[1], line1[0] )
p2, r2 = line2[0], linalg.sub( line2[1], line2[0] )
r1xr2 = float( linalg.cross( r1, r2 ) )
if r1xr2 == 0.0: return nointersect_symbol
p2subp1 = linalg.sub( p2, p1 )
d1 = linalg.cross( p2subp1, r2 ) / r1xr2
d2 = linalg.cross( p2subp1, r1 ) / r1xr2
if d1 >= 0.0 and d1 <= 1.0 and d2 >= 0.0 and d2 <= 1.0:
return True, linalg.add( p1, linalg.scale( r1, d1 ) ), d1
else:
return nointersect_symbol
def _draw_detection_to_frame( self ):
target_delta = self.proximity_sensor.target_delta
if target_delta != None:
detector_endpoints = self.proximity_sensor.detector_line.vertexes
detector_vector = linalg.sub( detector_endpoints[1], detector_endpoints[0] )
target_vector = linalg.add( detector_endpoints[0], linalg.scale( detector_vector, target_delta ) )
self.viewer.current_frame.add_circle( pos = target_vector,
radius = 0.02,
color = "black",
alpha = 0.7 )
def _draw_detection_to_frame(self, frame):
"""
Visualize the detection of the proximity sensor
:param frame: The frame to be used
"""
target_delta = self.proximity_sensor.target_delta
if target_delta is not None:
detector_endpoints = self.proximity_sensor.detector_line.vertexes
detector_vector = linalg.sub(detector_endpoints[1],
detector_endpoints[0])
target_vector = linalg.add(
detector_endpoints[0],
linalg.scale(detector_vector, target_delta))
frame.add_circle(pos=target_vector,
radius=0.02,
color="black",
alpha=0.7)
def _as_vector( self ): return linalg.sub( self.vertexes[1], self.vertexes[0] )
def _as_vector( self ): return linalg.sub( self.vertexes[1], self.vertexes[0] )
