lass RRTStar3D:
def __init__(self, start, goal, builds, bounds,
max_iter=RRT_MAX_ITER, step_size=RRT_STEP,
neighbor_radius=RRT_NEIGHBOR_RADIUS):
self.start = np.array(start)
self.goal = np.array(goal)
# Pre-calculate builds with safety height buffer
self.builds_with_safety = builds.copy()
self.builds_with_safety[:, 4] -= SAFE_HEIGHT # Decrease zmin
self.builds_with_safety[:, 5] += SAFE_HEIGHT # Increase zmax
# Ensure zmin is not negative if SAFE_HEIGHT is large
self.builds_with_safety[:, 4] = np.maximum(0, self.builds_with_safety[:, 4])
self.bounds = np.array(bounds) # Ensure bounds is numpy array
self.max_iter = max_iter
self.step_size = step_size
self.neighbor_radius = neighbor_radius
self.nodes = [self.start]
self.parent = {tuple(self.start): None}
self.cost = {tuple(self.start): 0.0}
# Initialize KDTree with the start node
self.kdtree = cKDTree(np.array([self.start])) # Ensure it's a 2D array
def sample(self):
# Biased sampling towards goal occasionally
if np.random.rand() < 0.1: # 10% chance to sample goal
return self.goal
# Sample within bounds
return np.random.uniform(self.bounds[:, 0], self.bounds[:, 1])
def nearest(self, q):
_, idx = self.kdtree.query(q)
# Handle case where KDTree might have only one node initially
if isinstance(idx, (int, np.integer)):
return self.nodes[idx]
else: # Should not happen if tree has >= 1 node, but safety check
return self.nodes[0]
def steer(self, q_near, q_rand):
delta = q_rand - q_near
dist = np.linalg.norm(delta)
if dist == 0: # Avoid division by zero
return q_near
ratio = self.step_size / dist
if ratio >= 1.0:
return q_rand
return q_near + delta * ratio
def near_neighbors(self, q_new):
# Ensure nodes list is not empty before querying
if not self.nodes:
return []
# Ensure kdtree has points before querying
if self.kdtree.n == 0:
return []
# Use query_ball_point which is efficient for radius searches
indices = self.kdtree.query_ball_point(q_new, self.neighbor_radius)
# Filter out the index of q_new itself if it's already in nodes (might happen during rewiring)
q_new_tuple = tuple(q_new)
neighbors = []
for i in indices:
# Check bounds and ensure it's not the node itself if already added
# This check might be redundant if q_new isn't added before calling this
if i < len(self.nodes): # Ensure index is valid
node = self.nodes[i]
if tuple(node) != q_new_tuple:
neighbors.append(node)
return neighbors
def plan(self):
for i in range(self.max_iter):
q_rand = self.sample()
# Check if nodes list is empty (shouldn't happen after init)
if not self.nodes:
print("Warning: Node list empty during planning.")
continue # Or handle appropriately
q_near = self.nearest(q_rand)
q_new = self.steer(q_near, q_rand)
# Check collision for the new segment using the pre-calculated safe builds
if check_segment_collision(q_near, q_new, self.builds_with_safety) > 0:
continue
# If collision-free, add the node and update KD-Tree periodically
q_new_tuple = tuple(q_new)
q_near_tuple = tuple(q_near)
# Choose parent with minimum cost among neighbors
min_cost = self.cost[q_near_tuple] + np.linalg.norm(q_new - q_near)
best_parent_node = q_near
neighbors = self.near_neighbors(q_new) # Find neighbors first
for q_neighbor in neighbors:
q_neighbor_tuple = tuple(q_neighbor)
# Check connectivity collision
if check_segment_collision(q_neighbor, q_new, self.builds_with_safety) == 0:
new_cost = self.cost[q_neighbor_tuple] + np.linalg.norm(q_new - q_neighbor)
if new_cost < min_cost:
min_cost = new_cost
best_parent_node = q_neighbor
# Add the new node with the best parent found
self.nodes.append(q_new)
q_best_parent_tuple = tuple(best_parent_node)
self.parent[q_new_tuple] = q_best_parent_tuple
self.cost[q_new_tuple] = min_cost
# Rebuild KDTree periodically
if len(self.nodes) % KD_REBUILD_EVERY == 0 or i == self.max_iter - 1:
# Important: Ensure nodes is a list of arrays before creating KDTree
if self.nodes: # Check if nodes is not empty
self.kdtree = cKDTree(np.array(self.nodes))
# Rewire neighbors to go through q_new if it provides a shorter path
for q_neighbor in neighbors:
q_neighbor_tuple = tuple(q_neighbor)
# Check if rewiring through q_new is shorter and collision-free
cost_via_new = min_cost + np.linalg.norm(q_neighbor - q_new)
if cost_via_new < self.cost[q_neighbor_tuple]:
if check_segment_collision(q_new, q_neighbor, self.builds_with_safety) == 0:
self.parent[q_neighbor_tuple] = q_new_tuple
self.cost[q_neighbor_tuple] = cost_via_new
# Check if goal is reached
if np.linalg.norm(q_new - self.goal) < self.step_size:
# Check final segment collision
if check_segment_collision(q_new, self.goal, self.builds_with_safety) == 0:
goal_tuple = tuple(self.goal)
self.nodes.append(self.goal) # Add goal node
self.parent[goal_tuple] = q_new_tuple
self.cost[goal_tuple] = min_cost + np.linalg.norm(self.goal - q_new)
print(f"RRT*: Goal reached at iteration {i+1}")
# Rebuild KDTree one last time if goal is reached
self.kdtree = cKDTree(np.array(self.nodes))
break # Exit planning loop
else: # Loop finished without reaching goal condition
print(f"RRT*: Max iterations ({self.max_iter}) reached. Connecting nearest node to goal.")
# Find node closest to goal among existing nodes
if not self.nodes:
print("Error: No nodes generated by RRT*.")
return None # Or raise error
nodes_arr = np.array(self.nodes)
distances_to_goal = np.linalg.norm(nodes_arr - self.goal, axis=1)
nearest_node_idx = np.argmin(distances_to_goal)
q_final_near = self.nodes[nearest_node_idx]
q_final_near_tuple = tuple(q_final_near)
goal_tuple = tuple(self.goal)
# Try connecting nearest found node to goal
if check_segment_collision(q_final_near, self.goal, self.builds_with_safety) == 0:
self.nodes.append(self.goal)
self.parent[goal_tuple] = q_final_near_tuple
self.cost[goal_tuple] = self.cost[q_final_near_tuple] + np.linalg.norm(self.goal - q_final_near)
print("RRT*: Connected nearest node to goal.")
else:
print("RRT*: Could not connect nearest node to goal collision-free. Returning path to nearest node.")
# Path will be constructed to q_final_near instead of goal
goal_tuple = q_final_near_tuple # Target for path reconstruction
# Backtrack path from goal (or nearest reachable node)
path = []
# Start backtracking from the actual last node added (goal or nearest)
curr_tuple = goal_tuple
if curr_tuple not in self.parent and curr_tuple != tuple(self.start):
print(f"Warning: Target node {curr_tuple} not found in parent dict. Path reconstruction might fail.")
# Fallback to the last added node if goal wasn't reachable/added correctly
if self.nodes:
curr_tuple = tuple(self.nodes[-1])
else: return None # No path possible
while curr_tuple is not None:
# Ensure the node corresponding to the tuple exists
# This requires searching self.nodes, which is inefficient.
# A better approach is to store nodes in the dict or use indices.
# For now, let's assume tuple keys match numpy arrays.
path.append(np.array(curr_tuple))
curr_tuple = self.parent.get(curr_tuple, None)
if not path:
print("Error: Path reconstruction failed.")
return None
if tuple(path[-1]) != tuple(self.start):
print("Warning: Path does not end at start node.")
return np.array(path[::-1]) # Reverse to get start -> goal order
# --------------------- Path Cost Function (Use safe builds) ---------------------
def path_cost(path_pts, builds_with_safety, drone_speed=DRONE_SPEED, penalty_k=PENALTY_K):
total_time = 0.0
total_penalty = 0.0
num_segments = len(path_pts) - 1
if num_segments < 1:
return 0.0 # No cost for a single point path
# Vectorized calculations where possible
p = path_pts[:-1] # Start points of segments
q = path_pts[1:] # End points of segments
segments = q - p
distances = np.linalg.norm(segments, axis=1)
# Avoid division by zero for zero-length segments
valid_segments = distances > 1e-6
if not np.any(valid_segments):
return 0.0 # Path has no length
p = p[valid_segments]
q = q[valid_segments]
segments = segments[valid_segments]
distances = distances[valid_segments]
dir_unit = segments / distances[:, np.newaxis]
# Interpolate wind at midpoints for better average (optional, could use start/end)
midpoints = p + segments / 2.0
# try:
# wind_vectors = interp(midpoints)
# except ValueError as e:
# print(f"Interpolation error: {e}")
# print(f"Midpoints shape: {midpoints.shape}")
# # Handle error, e.g., return a large cost or use zero wind
# wind_vectors = np.zeros_like(midpoints)
# Calculate ground speed component along each segment
# Ensure wind_vectors and dir_unit have compatible shapes for dot product
# np.einsum is efficient for row-wise dot products
# wind_along_path = np.einsum('ij,ij->i', wind_vectors, dir_unit)
ground_speeds = np.maximum(drone_speed , 1e-3) # Avoid zero/negative speed
# Calculate time for each segment
segment_times = distances / ground_speeds
total_time = np.sum(segment_times)
# Calculate collision penalty (iterate segments as check_segment_collision is per-segment)
for i in range(len(p)):
# Pass pre-calculated builds_with_safety
penetration = check_segment_collision(p[i], q[i], builds_with_safety)
if penetration > 0:
total_penalty += penalty_k * penetration**2 # Quadratic penalty
return total_time + total_penalty生成注释
最新发布
本文介绍了一种高效判断大整数a是否能被较小整数b整除的方法。通过将大整数逐位处理并利用模运算，避免了直接进行大整数运算可能带来的复杂度和溢出问题。
扫一扫
110
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
