def _detect_collisions( self ):
colliders = self.world.colliders()
solids = self.world.solids()
for collider in colliders:
polygon1 = collider.global_geometry # polygon1
for solid in solids:
if solid is not collider: # don't test an object against itself
polygon2 = solid.global_geometry # polygon2
if geometrics.check_nearness( polygon1, polygon2 ): # don't bother testing objects that are not near each other
if geometrics.convex_polygon_intersect_test( polygon1, polygon2 ):
raise CollisionException()
def __generate_rectangle_obstacles(self, world):
"""
Generate random rectangle obstacles
:param world: The world for which they are generated
:return: List of generated rectangle obstacles
"""
obs_min_dim = self.cfg["obstacle"]["rectangle"]["min_dim"]
obs_max_dim = self.cfg["obstacle"]["rectangle"]["max_dim"]
obs_max_combined_dim = self.cfg["obstacle"]["rectangle"][
"max_combined_dim"]
obs_min_count = self.cfg["obstacle"]["rectangle"]["min_count"]
obs_max_count = self.cfg["obstacle"]["rectangle"]["max_count"]
obs_min_dist = self.cfg["obstacle"]["rectangle"]["min_distance"]
obs_max_dist = self.cfg["obstacle"]["rectangle"]["max_distance"]
# generate the obstacles
obstacles = []
obs_dim_range = obs_max_dim - obs_min_dim
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange(obs_min_count, obs_max_count + 1)
test_geometries = [r.global_geometry for r in world.robots]
while len(obstacles) < num_obstacles:
# generate dimensions
width = obs_min_dim + (random() * obs_dim_range)
height = obs_min_dim + (random() * obs_dim_range)
while width + height > obs_max_combined_dim:
height = obs_min_dim + (random() * obs_dim_range)
# generate position
dist = obs_min_dist + (random() * obs_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
# generate orientation
theta = -pi + (random() * 2 * pi)
# test if the obstacle overlaps the robots or the goal
obstacle = RectangleObstacle(width, height, Pose(x, y, theta))
intersects = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test(
test_geometry, obstacle.global_geometry)
if not intersects:
obstacles.append(obstacle)
return obstacles
def _detect_collisions(self):
"""
Checks the world for any collisions between colliding objects (robots) and solid objects (obstacles)
Raises a CollisionException if a collision is detected
"""
colliders = self.world.colliders()
solids = self.world.solids()
for collider in colliders:
polygon1 = collider.global_geometry # polygon1
for solid in solids:
if solid is not collider: # don't test an object against itself
polygon2 = solid.global_geometry # polygon2
if geometrics.check_nearness(polygon1,
polygon2): # don't bother testing objects that are not near each other
if geometrics.convex_polygon_intersect_test(polygon1, polygon2):
raise CollisionException()
def __generate_random_features(self, world):
"""
Generate random features represented by octagon obstacles
:param world: The world for which they are generated
:return: List of generated features
"""
obs_radius = self.cfg["obstacle"]["feature"]["radius"]
obs_min_count = self.cfg["obstacle"]["feature"]["min_count"]
obs_max_count = self.cfg["obstacle"]["feature"]["max_count"]
obs_min_dist = self.cfg["obstacle"]["feature"]["min_distance"]
obs_max_dist = self.cfg["obstacle"]["feature"]["max_distance"]
# generate the obstacles
obstacles = []
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange(obs_min_count, obs_max_count + 1)
test_geometries = [r.global_geometry for r in world.robots]
while len(obstacles) < num_obstacles:
# generate position
dist = obs_min_dist + (random() * obs_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
# generate orientation
theta = -pi + (random() * 2 * pi)
# test if the obstacle overlaps the robots or the goal
obstacle = FeaturePoint(obs_radius, Pose(x, y, theta), 0)
intersects = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test(
test_geometry, obstacle.global_geometry)
if not intersects:
obstacles.append(obstacle)
for i, feature in enumerate(obstacles):
feature.id = i
return obstacles
def __check_obstacle_intersections(self, goal):
"""
Check for intersections between the goal and the obstacles
:param goal: The goal position
:return: Boolean value indicating if the goal is too close to an obstacle
"""
# generate a proximity test geometry for the goal
min_clearance = self.cfg["goal"]["min_clearance"]
n = 6 # goal is n sided polygon
goal_test_geometry = []
for i in range(n):
goal_test_geometry.append([
goal[0] + min_clearance * cos(i * 2 * pi / n),
goal[1] + min_clearance * sin(i * 2 * pi / n)
])
goal_test_geometry = Polygon(goal_test_geometry)
intersects = False
for obstacle in self.current_obstacles:
intersects |= geometrics.convex_polygon_intersect_test(
goal_test_geometry, obstacle.global_geometry)
return intersects
def random_map(self, world):
# OBSTACLE PARAMS
obs_min_dim = OBS_MIN_DIM
obs_max_dim = OBS_MAX_DIM
obs_max_combined_dim = OBS_MAX_COMBINED_DIM
obs_min_count = OBS_MIN_COUNT
obs_max_count = OBS_MAX_COUNT
obs_min_dist = OBS_MIN_DIST
obs_max_dist = OBS_MAX_DIST
# GOAL PARAMS
goal_min_dist = GOAL_MIN_DIST
goal_max_dist = GOAL_MAX_DIST
# BUILD RANDOM ELEMENTS
# generate the goal
goal_dist_range = goal_max_dist - goal_min_dist
dist = goal_min_dist + (random() * goal_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
goal = [x, y]
# generate a proximity test geometry for the goal
r = MIN_GOAL_CLEARANCE
n = 6
goal_test_geometry = []
for i in range(n):
goal_test_geometry.append(
[x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)])
goal_test_geometry = Polygon(goal_test_geometry)
# generate the obstacles
obstacles = []
obs_dim_range = obs_max_dim - obs_min_dim
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange(obs_min_count, obs_max_count + 1)
test_geometries = [r.global_geometry
for r in world.robots] + [goal_test_geometry]
while len(obstacles) < num_obstacles:
# generate dimensions
width = obs_min_dim + (random() * obs_dim_range)
height = obs_min_dim + (random() * obs_dim_range)
while width + height > obs_max_combined_dim:
height = obs_min_dim + (random() * obs_dim_range)
# generate position
dist = obs_min_dist + (random() * obs_dist_range)
phi = -pi + (random() * 2 * pi)
x = dist * sin(phi)
y = dist * cos(phi)
# generate orientation
theta = -pi + (random() * 2 * pi)
# test if the obstacle overlaps the robots or the goal
obstacle = RectangleObstacle(width, height, Pose(x, y, theta))
intersects = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test(
test_geometry, obstacle.global_geometry)
if not intersects:
obstacles.append(obstacle)
# update the current obstacles and goal
self.current_obstacles = obstacles
self.current_goal = goal
# apply the new obstacles and goal to the world
self.apply_to_world(world)
def random_map( self, world ):
# OBSTACLE PARAMS
obs_min_dim = OBS_MIN_DIM
obs_max_dim = OBS_MAX_DIM
obs_max_combined_dim = OBS_MAX_COMBINED_DIM
obs_min_count = OBS_MIN_COUNT
obs_max_count = OBS_MAX_COUNT
obs_min_dist = OBS_MIN_DIST
obs_max_dist = OBS_MAX_DIST
# GOAL PARAMS
goal_min_dist = GOAL_MIN_DIST
goal_max_dist = GOAL_MAX_DIST
# BUILD RANDOM ELEMENTS
# generate the goal
goal_dist_range = goal_max_dist - goal_min_dist
dist = goal_min_dist + ( random() * goal_dist_range )
phi = -pi + ( random() * 2 * pi )
x = dist * sin( phi )
y = dist * cos( phi )
goal = [ x, y ]
# generate a proximity test geometry for the goal
r = MIN_GOAL_CLEARANCE
n = 6
goal_test_geometry = []
for i in range( n ):
goal_test_geometry.append(
[ x + r*cos( i * 2*pi/n ),
y + r*sin( i * 2*pi/n ) ] )
goal_test_geometry = Polygon( goal_test_geometry )
# generate the obstacles
obstacles = []
obs_dim_range = obs_max_dim - obs_min_dim
obs_dist_range = obs_max_dist - obs_min_dist
num_obstacles = randrange( obs_min_count, obs_max_count+1 )
test_geometries = [ r.global_geometry for r in world.robots ] + [ goal_test_geometry ]
while len( obstacles ) < num_obstacles:
# generate dimensions
width = obs_min_dim + ( random() * obs_dim_range )
height = obs_min_dim + ( random() * obs_dim_range )
while width + height > obs_max_combined_dim:
height = obs_min_dim + ( random() * obs_dim_range )
# generate position
dist = obs_min_dist + ( random() * obs_dist_range )
phi = -pi + ( random() * 2 * pi )
x = dist * sin( phi )
y = dist * cos( phi )
# generate orientation
theta = -pi + ( random() * 2 * pi )
# test if the obstacle overlaps the robots or the goal
obstacle = RectangleObstacle( width, height,
Pose( x, y, theta ) )
intersects = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry )
if intersects == False: obstacles.append( obstacle )
# update the current obstacles and goal
self.current_obstacles = obstacles
self.current_goal = goal
# apply the new obstacles and goal to the world
self.apply_to_world( world )
