From 3cf4d3d4877c2bff6c18cc813d380f3355d1b14c Mon Sep 17 00:00:00 2001 From: Julius Schlapbach <80708107+sjschlapbach@users.noreply.github.com> Date: Thu, 14 Mar 2024 03:00:55 +0100 Subject: [PATCH] fix: ensure that dynamic line collision checking uses subsampling for correct results (#43) * fix: ensure that dynamic line collision checking uses subsampling for correct results * chore: extend test suite --- src/algorithms/replanning_rrt.py | 38 ++------------ src/algorithms/rrt.py | 12 +++-- src/envs/environment_instance.py | 58 +++++++++++++++++++-- src/test/environment_instance_test.py | 73 +++++++++++++++++++++++++++ 4 files changed, 139 insertions(+), 42 deletions(-) diff --git a/src/algorithms/replanning_rrt.py b/src/algorithms/replanning_rrt.py index 2ee7c1a..40d59c1 100644 --- a/src/algorithms/replanning_rrt.py +++ b/src/algorithms/replanning_rrt.py @@ -86,8 +86,8 @@ def run( # traverse path and check if recomputation is required along each edge with respect to the dynamic obstacles if not quiet: print("Validating path...") - collision_free, save_idx, save_time = rrt.validate_path( - path=sol_path, start_time=query_time + collision_free, save_idx, save_idx_time, last_save, last_time = ( + rrt.validate_path(path=sol_path, start_time=query_time, stepsize=stepsize) ) if collision_free: @@ -104,6 +104,8 @@ def run( print( "Path is not collision free, with first collision at edge with starting point: ", rrt.tree[sol_path[save_idx]]["position"], + "at time:", + save_idx_time, ) # add all points up to the collision point to the final path (coordinates) @@ -111,41 +113,11 @@ def run( rrt.tree[sol_path[i]]["position"] for i in range(1, save_idx + 1) ] - # follow the next edge with fixed size steps and save the last collision-free point - save_node = new_path[-1] - next_node = rrt.tree[sol_path[save_idx + 1]]["position"] - delta_distance = next_node.distance(save_node) - num_steps = int(delta_distance / stepsize) - x_step = (next_node.x - save_node.x) / num_steps - y_step = (next_node.y - save_node.y) / num_steps - - # track the position and time of the last node, which is not in collision - last_save = None - last_time = save_time - - # iterate over the edge and check for the last save point on it - for i in range(1, num_steps): - sample = ShapelyPoint( - save_node.x + i * x_step, save_node.y + i * y_step - ) - sample_time = last_time + stepsize - - # check if the sample is in collision - collision_free = self.env.static_collision_free( - point=sample, query_time=sample_time - ) - - if collision_free: - last_save = sample - last_time = sample_time - else: - break - if last_save is None: if (save_node.x, save_node.y) != start: # if the last save node is not the start node, replan from the last save point last_save = save_node - last_time = save_time + last_time = save_idx_time elif in_recursion: # to skip issues where RRT fails due an obstacle popping up around a point diff --git a/src/algorithms/rrt.py b/src/algorithms/rrt.py index 6bb1bcc..7c822f8 100644 --- a/src/algorithms/rrt.py +++ b/src/algorithms/rrt.py @@ -195,7 +195,9 @@ def rrt_find_path(self): return path[::-1] - def validate_path(self, path: List[int], start_time: float = 0.0): + def validate_path( + self, path: List[int], start_time: float = 0.0, stepsize: float = 1 + ): """ Validates the given path for collision with dynamics obstacles. @@ -219,16 +221,16 @@ def validate_path(self, path: List[int], start_time: float = 0.0): edge = ShapelyLine([(parent.x, parent.y), (child.x, child.y)]) # check for dynamic collisions - collision_free = self.env.dynamic_collision_free_ln( - edge, Interval(time, edge_end_time, closed="both") + collision_free, last_save, last_time = self.env.dynamic_collision_free_ln( + edge, Interval(time, edge_end_time, closed="both"), stepsize ) if not collision_free: - return False, i, time + return False, i, time, last_save, last_time time = edge_end_time - return True, None, None + return True, None, None, None, None def __find_closest_neighbor( self, candidate: ShapelyPoint, rewiring: bool diff --git a/src/envs/environment_instance.py b/src/envs/environment_instance.py index ad7b41c..f3f5d9d 100644 --- a/src/envs/environment_instance.py +++ b/src/envs/environment_instance.py @@ -627,7 +627,7 @@ def collision_free_intervals_ln( return False, False, intervals def dynamic_collision_free_ln( - self, line: ShapelyLine, query_interval: Interval + self, line: ShapelyLine, query_interval: Interval, stepsize: float = 1 ) -> bool: """ Check if a given line is in collision with any dynamic obstacle in the environment. @@ -649,7 +649,7 @@ def dynamic_collision_free_ln( # if no dynamic obstacles were found in the considered cells, return True if len(dynamic_ids) == 0: - return True + return True, None, None # if dynamic obstacles were found, check if the line is in collision with any of them for key in dynamic_ids: @@ -657,9 +657,59 @@ def dynamic_collision_free_ln( if obstacle.is_active(query_interval=query_interval): if obstacle.check_collision(shape=line): - return False + # The line overlaps in time and space with obstacle -> POSSIBLE COLLISION + # --> verify collision with subsampling + + # keep track of last save subsample and time + last_save = None + last_save_time = query_interval.left + + # extract start and end points from line + start = ShapelyPoint(line.coords[0][0], line.coords[0][1]) + end = ShapelyPoint(line.coords[1][0], line.coords[1][1]) + + # apply subsampling to check for collision + true_positive = False + delta_distance = end.distance(start) + num_steps = int(delta_distance / stepsize) + x_step = (end.x - start.x) / num_steps + y_step = (end.y - start.y) / num_steps + + for i in range(1, num_steps): + sample = ShapelyPoint( + start.x + i * x_step, start.y + i * y_step + ) + sample_time = last_save_time + stepsize - return True + # check if the sample is in collision + collision_free = self.static_collision_free( + point=sample, query_time=sample_time + ) + + if collision_free: + last_save = sample + last_save_time = sample_time + else: + true_positive = True + break + + # edge is in collision at some intermediary position + if true_positive: + return False, last_save, last_save_time + + else: + # check if the end of the edge is in collision + collision_free = self.static_collision_free( + point=end, query_time=query_interval.right + ) + if not collision_free: + return False, end, query_interval.right + else: + print( + "Detected false positive in dynamic edge collision check with subsampling." + ) + + return True, None, None def get_static_obs_free_volume(self): """ diff --git a/src/test/environment_instance_test.py b/src/test/environment_instance_test.py index d1511fd..8695a35 100644 --- a/src/test/environment_instance_test.py +++ b/src/test/environment_instance_test.py @@ -1796,3 +1796,76 @@ def test_free_space_computation(self): assert ( abs(free_space - (10000 - math.pi - 20 - math.pi - 100 - 40 - math.pi)) < 1 ) + + def test_dynamic_collision_free_line(self): + env_base = Environment() + obstacle = Polygon( + geometry=ShapelyPolygon([(10, 10), (20, 10), (20, 20), (10, 20)]), + radius=0.0, + time_interval=Interval(20, 30, closed="both"), + ) + env_base.add_obstacles([obstacle]) + env = EnvironmentInstance( + environment=env_base, + query_interval=Interval(0, 100, closed="both"), + scenario_range_x=(0, 100), + scenario_range_y=(0, 100), + resolution=10, + ) + + non_colliding_line = ShapelyLine([(0, 0), (5, 5)]) + line = ShapelyLine([(5, 15), (25, 15)]) + stepsize = 0.1 + + # Test 1: line is not in collision with obstacle at all + collision_free1, last_save, last_time = env.dynamic_collision_free_ln( + line=non_colliding_line, + query_interval=Interval(20, 30), + stepsize=stepsize, + ) + assert collision_free1 == True + assert last_save is None + assert last_time is None + + collision_free2, last_save, last_time = env.dynamic_collision_free_ln( + line=non_colliding_line, + query_interval=Interval(0, 20), + stepsize=stepsize, + ) + assert collision_free2 == True + assert last_save is None + assert last_time is None + + # Test 2: line with spatial intersection, but no overlap in temporal domain + collision_free3, last_save, last_time = env.dynamic_collision_free_ln( + line=line, query_interval=Interval(-10, 10), stepsize=stepsize + ) + assert collision_free3 == True + assert last_save is None + assert last_time is None + + # Test 3: line with spatial and temporal intersection, but only enters obstacle when inactive + collision_free4, last_save, last_time = env.dynamic_collision_free_ln( + line=line, query_interval=Interval(28, 48), stepsize=stepsize + ) + assert collision_free4 == True + assert last_save is None + assert last_time is None + + # Test 4: line with spatial and temporal intersection, and enters obstacle when active + collision_free5, last_save, last_time = env.dynamic_collision_free_ln( + line=line, query_interval=Interval(20, 40), stepsize=stepsize + ) + assert collision_free5 == False + assert last_save is not None + assert last_save == ShapelyPoint(9.9, 15) + assert last_time is not None + assert (last_time - 24.9) < 1e-9 + + # Test 5: line with spatial and temporal intersection, and exits obstacle before active + collision_free6, last_save, last_time = env.dynamic_collision_free_ln( + line=line, query_interval=Interval(2, 22), stepsize=stepsize + ) + assert collision_free6 == True + assert last_save is None + assert last_time is None