From 30116ef0946609290586b53298a83e951f601d48 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Thu, 13 Feb 2025 21:16:58 +0800 Subject: [PATCH 01/11] feat: add ElasticBands --- PathPlanning/ElasticBands/elastic_bands.py | 262 +++++++++++++++++++++ PathPlanning/ElasticBands/obstacles.npy | Bin 0 -> 2000128 bytes 2 files changed, 262 insertions(+) create mode 100644 PathPlanning/ElasticBands/elastic_bands.py create mode 100644 PathPlanning/ElasticBands/obstacles.npy diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py new file mode 100644 index 0000000000..3890c86537 --- /dev/null +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -0,0 +1,262 @@ +""" +Elastic Bands + +author: Wang Zheng (@Aglargil) + +Ref: + +- [Elastic Bands: Connecting Path Planning and Control] +(http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf) +""" + +import numpy as np +import sys +import pathlib +import matplotlib.pyplot as plt +from matplotlib.patches import Circle + +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) + +from Mapping.DistanceMap.distance_map import compute_sdf + +# Elastic Bands Params +MAX_BUBBLE_RADIUS = 100 +MIN_BUBBLE_RADIUS = 10 +RHO0 = 20.0 # Maximum distance for applying repulsive force +KC = 0.05 # Contraction force gain +KR = -0.1 # Repulsive force gain +LAMBDA = 0.7 # Overlap constraint factor +STEP_SIZE = 3.0 # Step size for calculating gradient + +# Visualization Params +ENABLE_PLOT = True +ENABLE_INTERACTIVE = False +MAX_ITER = 50 + + +class Bubble: + def __init__(self, position, radius): + self.pos = np.array(position) # Bubble center coordinates [x, y] + self.radius = radius # Safety distance radius ρ(b) + if self.radius > MAX_BUBBLE_RADIUS: + self.radius = MAX_BUBBLE_RADIUS + if self.radius < MIN_BUBBLE_RADIUS: + self.radius = MIN_BUBBLE_RADIUS + + +class ElasticBands: + def __init__(self, initial_path, obstacles, rho0=RHO0, kc=0.05, kr=-0.1): + self.distance_map = compute_sdf(obstacles) + self.bubbles = [ + Bubble(p, self.compute_rho(p)) for p in initial_path + ] # Initialize bubble chain + self.kc = kc # Contraction force gain + self.kr = kr # Repulsive force gain + self.rho0 = rho0 # Maximum distance for applying repulsive force + + def compute_rho(self, position): + """Compute the distance field value at the position""" + return self.distance_map[int(position[0]), int(position[1])] + + def contraction_force(self, i): + """Calculate internal contraction force for the i-th bubble""" + if i == 0 or i == len(self.bubbles) - 1: + return np.zeros(2) + + prev = self.bubbles[i - 1].pos + next_ = self.bubbles[i + 1].pos + current = self.bubbles[i].pos + + # f_c = kc * ( (prev-current)/|prev-current| + (next-current)/|next-current| ) + dir_prev = (prev - current) / (np.linalg.norm(prev - current) + 1e-6) + dir_next = (next_ - current) / (np.linalg.norm(next_ - current) + 1e-6) + return self.kc * (dir_prev + dir_next) + + def external_force(self, i): + """Calculate external repulsive force for the i-th bubble""" + h = STEP_SIZE # Step size + b = self.bubbles[i].pos + rho = self.bubbles[i].radius + + if rho >= self.rho0: + return np.zeros(2) + + # Finite difference approximation of the gradient ∂ρ/∂b + dx = np.array([h, 0]) + dy = np.array([0, h]) + grad_x = (self.compute_rho(b - dx) - self.compute_rho(b + dx)) / (2 * h) + grad_y = (self.compute_rho(b - dy) - self.compute_rho(b + dy)) / (2 * h) + grad = np.array([grad_x, grad_y]) + + return self.kr * (self.rho0 - rho) * grad + + def update_bubbles(self): + """Update bubble positions""" + new_bubbles = [] + for i in range(len(self.bubbles)): + if i == 0 or i == len(self.bubbles) - 1: + new_bubbles.append(self.bubbles[i]) # Fixed start and end points + continue + + f_total = self.contraction_force(i) + self.external_force(i) + alpha = self.bubbles[i].radius # Adaptive step size + new_pos = self.bubbles[i].pos + alpha * f_total + new_pos = np.clip(new_pos, 0, 499) + new_radius = self.compute_rho(new_pos) + + # Update bubble and maintain overlap constraint + new_bubble = Bubble(new_pos, new_radius) + new_bubbles.append(new_bubble) + + self.bubbles = new_bubbles + self._maintain_overlap() + + def _maintain_overlap(self): + """Maintain bubble chain continuity (simplified insertion/deletion mechanism)""" + # Insert bubbles + i = 0 + while i < len(self.bubbles) - 1: + bi, bj = self.bubbles[i], self.bubbles[i + 1] + dist = np.linalg.norm(bi.pos - bj.pos) + if dist > LAMBDA * (bi.radius + bj.radius): + new_pos = (bi.pos + bj.pos) / 2 + rho = self.compute_rho( + new_pos + ) # Calculate new radius using environment model + self.bubbles.insert(i + 1, Bubble(new_pos, rho)) + i += 2 # Skip the processed region + else: + i += 1 + + # Delete redundant bubbles + i = 1 + while i < len(self.bubbles) - 1: + prev = self.bubbles[i - 1] + next_ = self.bubbles[i + 1] + dist = np.linalg.norm(prev.pos - next_.pos) + if dist <= LAMBDA * (prev.radius + next_.radius): + del self.bubbles[i] # Delete if redundant + else: + i += 1 + + +class ElasticBandsVisualizer: + def __init__(self): + self.obstacles = np.zeros((500, 500)) + self.start_point = None + self.end_point = None + self.elastic_band = None + + if ENABLE_PLOT: + self.fig, self.ax = plt.subplots(figsize=(8, 8)) + # Set the display range of the graph + self.ax.set_xlim(0, 500) + self.ax.set_ylim(0, 500) + + if ENABLE_INTERACTIVE: + self.path_points = [] # Add a list to store path points + # Connect mouse events + self.fig.canvas.mpl_connect("button_press_event", self.on_click) + else: + self.path_points = [ + [30, 136], + [61, 214], + [77, 256], + [77, 309], + [53, 366], + [41, 422], + [51, 453], + [110, 471], + [184, 437], + [257, 388], + [343, 353], + [402, 331], + [476, 273], + [456, 206], + [430, 160], + [402, 107], + ] + self.obstacles = np.load(pathlib.Path(__file__).parent / "obstacles.npy") + self.plan_path() + + self.plot_background() + + def plot_background(self): + """Plot the background grid""" + if not ENABLE_PLOT: + return + + self.ax.cla() + self.ax.set_xlim(0, 500) + self.ax.set_ylim(0, 500) + self.ax.grid(True) + if self.path_points: + self.ax.plot( + [p[0] for p in self.path_points], + [p[1] for p in self.path_points], + "yo", + markersize=8, + ) + + self.ax.imshow(self.obstacles.T, origin="lower", cmap="binary", alpha=0.3) + if self.elastic_band is not None: + path = [b.pos.tolist() for b in self.elastic_band.bubbles] + path = np.array(path) + self.ax.plot(path[:, 0], path[:, 1], "b-", linewidth=2, label="path") + + for bubble in self.elastic_band.bubbles: + circle = Circle( + bubble.pos, bubble.radius, fill=False, color="g", alpha=0.3 + ) + self.ax.add_patch(circle) + self.ax.plot(bubble.pos[0], bubble.pos[1], "bo", markersize=10) + + self.ax.legend() + plt.draw() + plt.pause(0.01) + + def on_click(self, event): + """Handle mouse click events""" + if event.inaxes != self.ax: + return + + x, y = int(event.xdata), int(event.ydata) + + if event.button == 1: # Left click to add obstacles + size = 30 # Side length of the square + half_size = size // 2 + + # Ensure not out of the map boundary + x_start = max(0, x - half_size) + x_end = min(self.obstacles.shape[0], x + half_size) + y_start = max(0, y - half_size) + y_end = min(self.obstacles.shape[1], y + half_size) + + # Set the square area as obstacles (value set to 1) + self.obstacles[x_start:x_end, y_start:y_end] = 1 + + elif event.button == 3: # Right click to add path points + self.path_points.append([x, y]) + + elif event.button == 2: # Middle click to end path input and start planning + if len(self.path_points) >= 2: + self.plan_path() + + self.plot_background() + + def plan_path(self): + """Plan the path""" + + initial_path = self.path_points + # Create an elastic band object and optimize + self.elastic_band = ElasticBands(initial_path, self.obstacles) + for _ in range(MAX_ITER): + self.elastic_band.update_bubbles() + self.path_points = [b.pos for b in self.elastic_band.bubbles] + self.plot_background() + + +if __name__ == "__main__": + ElasticBandsVisualizer() + if ENABLE_PLOT: + plt.show() diff --git a/PathPlanning/ElasticBands/obstacles.npy b/PathPlanning/ElasticBands/obstacles.npy new file mode 100644 index 0000000000000000000000000000000000000000..c8b50d3cbbc6118659c8ed72a414199cf570c5f1 GIT binary patch literal 2000128 zcmeI#v92UJ6$aoT7$No0xtF3a`$DdF2^#8v<{pF8;{_8K^{P)d|@4ox^`R||J{qF6%-+%wxckkc6 z`~HVd|M~RcUw`}I({~?l*Z=V0pPxT|x&HYdAO8LE%m4rCH{X2o{_U5KU%!9*)7z*1 z`{f(|5g1- z?Xxqh=?D-YK!5-N0t5&UAV7dXlfY4K-Q|80n!yAJ5FkK+009C72oNAZptHa(`TJ;} zomovsfB*pk1PBlyK!5-N0tA`_j&iH}!8w~l(;*v7fB*pk1PBlyK!5-N0@Dl3mOIrG zEpy~BePbz*009C72oNAZfB*pk1eyena-jRcIh#Y%AsbA9009C72oNAZfB*pk(+kX& zJJk~{bL236V=0gT0RjXF5FkK+009C7ngot=p!>l&n?utf8%%%z0RjXF5FkK+009Ei z3(S@~)e|jq0>DUbjG0t5&UAV7cs0RjY?1dei``@uPzL(?G}On?9Z0t5&UAV7cs z0Rqhev*keb#1YQs(A-!aAV7cs0RjXF5FkK+0D&ff*>a$I;`nBBXlg2h2@oJafB*pk z1PBlyK%iM*wj8LQIKtT+nj6al1PBlyK!5-N0t5&UAkZW*TMkrD9N%mXO-*Gm0RjXF z5FkK+009C72s8`KmIKujM>v~9b7Ogc009C72oNAZfB*pk1eye9%Yo{NZY&QFAV7cs0RjXF5FkJxslaS`Og+&^9Fo3J z$|gX7009C72oNAZfWVUia$vx5nA73Ow)PkS0t5&UAV7cs0RjZl3CN{!WE|#nNcTc1 zm;eC+1PBlyK!5-N0#6Far2)rbPKPJk+G7L=5FkK+009C72oOjoAeY9GahTH~-3z5) z0t5&UAV7cs0RjXFJSiZT1{{Yu9iD7!j}ah1fB*pk1PBlyKp>rfTpCBlVNQp1FO-4_ z5FkK+009C72oNCfq<~x+a2)1zc(ScMMt}eT0t5&UAV7csfph|LX&f1cIUUlyPzokM zfB*pk1PBlyK!CuL0&;19k5U;+dP z5FkK+009C72s|kumj)b%IUSyCYmX5iK!5-N0t5&UAV46UfLt0!#$ir}bT5>G2@oJa zfB*pk1PBly@T7oT8gLxuba=9@Jw|{40RjXF5FkK+0D*J@a%mjd?hff1SHT1b5FkK+ z009C72oP9IVE0^V|BE%W?GPY9fB*pk1PBlyKp?4rd>RM0yF=3URW<TiA9hPZm8zDe|009C72oNAZfIw1#-E*n^Cv9A1 z6Cgl<009C72oNAZU@-ytG!ArkhsE03b_ft4K!5-N0t5&UAdpUA_grfK=~`C71PBly zK!5-N0t5&USW-Yfjbq*2VaevUDFOrt5FkK+009C72qY5NJ(t>lqGnYp0RjXF5FkK+ z009C72pkH?xj~fG;qZVBAV7cs0RjXF5FkK+0D%+&S#znnQ?!>N2@oJafB*pk1PBly zK%i1U&JCz%9V(d(BS3%v0RjXF5FkK+009CI1)j~No_omZCIJEj2oNAZfB*pk1PBml z64)(gH{lpefB*pk1PBlyK!5-N0t8MHkk3zowhRFR1PBlyK!5-N0t5&UcpxC3KfrN| z009C72oNAZfB*pk1PGiYAfKNEZ5aXt2oNAZfB*pk1PBly@IXL5e}Ll_0RjXF5FkK+ z009C72oN|)Kt4YS+A;(P5FkK+009C72oNAZ;DLaA{s6}<0t5&UAV7cs0RjXF5Fl`p zfP8)uv}FhoAV7cs0RjXF5FkK+zyksK`~i+z1PBlyK!5-N0t5&UAVANe`2!rc2oNAZfB*pk1PBlyK!CtW0`mDu(3T-UfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBl~Md0UeW0_t4o`Px-0t5&UAV7cs0RjXF5co=9m)v>Z=T`s+7w*sP z^7nwlZ2|-c5FkK+009C72oM+~uuJZ|@3WQh{@gBqTYZctK!5-N0t5&UAV7csfj$De z@e_MTwCqRGz0RjXF z5FkK+0D(RNyX4OMK3mo9&+YQJ)yH@O1PBlyK!5-N0t5&U=p(R8?!51_Rqg)VE`M8n zj3+>V009C72oNAZfB=C$0=wkS`#xLM?$7P=x7EjZ0t5&UAV7cs0RjXF5a=VYOYXey zvsLZ>+%A7xeT*kSfB*pk1PBlyK!5;&J_5Vs&ig)F)$Y&j^0(E;cmf0n5FkK+009C7 z2oUHauuJZ|@3U3y{@gBqTYZctK!5-N0t5&UAV7csfj$Dec zSqKmyK!5-N0t5&UAVA9U1k;n1PBlyK!5-N0t5&UI1*^hYsQD@E)GZi<30fb1PBlyK!5-N0t5&=7T6^} z+Gq5^D2K;g<}LvO1PBlyK!5-N0t5&g35?2X<_-B>9FF?TeF6js5FkK+009C72oQKI zuuFcl&*+0u4v)LcT>=CM5FkK+009C72oN|D7?sz|8}hq29QB#|1PBlyK!5-N0t5&U zAn;gVm;7j-(Fda(9(S3$1PBlyK!5-N0t5&UAaEowDzBM0NEEV5FkK+009C7 z2oNAZ;IY6i`O!Y34@NmW?lN}?5FkK+009C72oNAZ;7DLpUNdjV@8WROXYLaqK!5-N z0t5&UAV7e?V}V`rqkTpnjBhQRW-6cSP009C72oNAZAhST$obqhlrL%f=KF@_H zhjScC{p&gb0t5&UAV7cs0Rs62cFR?z6Wh(fexnnzIh6X>bpiwk5FkK+009C7@(X0k zC$FuybYica*YhFEVXQ-`e_ba)fB*pk1PBlyKp?-sZh5M7V!JumZ*)R7hf@E#PJjRb z0t5&UAV7dXet~THQ|5FkK+009C72;>*oEl-tBY&Qq{ zjZVnsQ0iaT2@oJafB*pk1PBnwFOV&ts5g3!!=4WLzj5q<009C72oNAZfB=D#fSeaH z95~zrX77VIC7%Qc5FkK+009C72&^bDdmd3=bR7o{D}Mjj69EDQ2oNAZfB*pkF9LF3 zSa9HQ6PUdZ;*@+6AV7cs0RjXF5FoIk!0dTMebIFsIIQ^nV^0JK5FkK+009C72)qc$ zePO|Y!%blJK8REDNq_(W0t5&UAV7e?iUPCe5%opaap17x_m4dhAV7cs0RjXF5Fqd( zAoqm@2M#xZ+4~?)$tM8<1PBlyK!5-N0xJs4o=4OdUB`jLir+u>M1TMR0t5&UAV7e? zi-6o0792R-1ZMApI3=G12oNAZfB*pk1PH7sFnbWLWbcDGMV|x+5FkK+009C72xJtxO4jI3H)J%W?0RjXF z5FkK+z>9!f7Zw~i+yt`sL7bvb0t5&UAV7cs0RjXv3S`eC>W{ADz#-%JkD3V(AV7cs z0RjXF5O@)g>%xKqhnqn5K8REFNq|5`f$ZNut@UR-BQ+BsK!5;&ECQ|nE*l>w%Ynm9 zpjRJ+rcVL{G79wi9>_9J#xqhg0RjXF5Xd5s<@bO(qoX*i>5%37LY)K%d@Zos_wv^Q zt`HzVfB*pky##jqU17h`IUMG4==DCEhX8@|1$O&BJzv<01PBlyKwuw%-F{ctZ}c{Y zxg7TS#WLytWTQ(I-yQp_x3X%ff)s|zo+V-v8&WXfB*pk1lAJR?f1r6r^oem z()?cM=V<~n3hee?ue#qEdrM6O2oNAZU=@K?e?OG=b=J9+)?f8{Z`W0wO8e^VXFdWm z3Y7Za^*F684m0+ang|deK!Ct10$F|!^r~}}b6b5Mz0Nz!Jgc9l{SmkcWO*Oe8U0#v z(9Hx05FkK+z&L@@zrU+qA17(%o&u}(Uuj=G?aW1BR)JFgyB;T&#bMU|QWpUN1PBly zK!Cs)fh@mMd(}Bc(X0gK66p25m?zHlDu=ncN;L!s5FkK+009D{1+MztdDXh3J{)d`Fc z*zLVub-yEe!<+;N5FkK+009E?3at9OwzRK#PeNq``U#Z!-}Nxg;?S=n%tU|y0RjXF z5FkK+009D52xQ5h>WuCvIb6{@)+9iH009C72oNAZfB=Dh0;Rm?dKhPM=+_ZuB0zuu z0RjXF5FkK+0D&t6vgA*7Mt77PuIL?W5+Fc;009C72oNAZfIvTiQr>etjI%iO>j*Ou zAV7cs0RjXF5FkK+z!d^n@~1kZJ4y~$^o}(N5FkK+009C72oNAZpr1e~@3|hvSseOx zgqa8sAV7cs0RjXF5FkL{3V|&7Q=QQrC5J0|$C?BP5FkK+009C72oNC9PoR|dTo2aKNO(G%7rK!5-N0t5&UAV7csf%64+&AnOn zeSSw+kpKY#1PBlyK!5-N0t8kP$dZ3&uXClIuonUZ2oNAZfB*pk1PBl~Utsp!o29<< zPtJ-22oNAZfB*pk1PBlyu#!NQ{5yM{EA@oE5FkK+009C72oNAZfWY|zv*+F{^__om zRwO`x009C72oNAZfB=D&1hVAc+3Q@XC+vj)0RjXF5FkK+009C7&KH$*8xi`x^rIX82f3G@A z4z3d*K!5-N0t5&UAV7e?H3GfzUY2=E%312~RcFb;bpiwk5FkK+009C72oShNpjY0@ zGEYf4OZ~m-EIGJNfB*pk1PBlyK!5-N0@n!i%6nPnDJf^EzgL|l2iFM@AV7cs0RjXF z5FkL{8i8JUFUve7c4&gX;tc5FkK+009C72oP9JpjVDEPjp5v zhf<&Qnm@}tB?s3D5FkK+009C72oNB!nn0GEwp*R0KI2f5ah(7G0t5&UAV7cs0Rp)N zcFTMA8~vKaA@|#*dIAIp5FkK+009C72-FK?$!Y40Gu_RhzLSk4K!5-N0t5&UAV7dX zZh_tMp8ZC@W^u^2EJK0DA1PBlyK!5-N z0t5);7T7KC*>Ch~7Khw#m+A=+AV7cs0RjXF5Fk)5kR_+7GtP83hx$%7k^lh$1PBly zK!5-N0=Wft%X{`4{hGxg_uHj<0t5&UAV7cs0RjXF)C*+EY3htK-OZuClZ_-mfB*pk z1PBlyK!8AQf!*?+{YJlLamf94sh$7<0t5&UAV7cs0Rr^`S#p{><4kvRsPAMW2@oJa zfB*pk1PBlykXvB4yl21BuUQ;&zg?;)K!5-N0t5&UAV7dXy+D?nrp`Fi-5lyW*+>Ee z2oNAZfB*pk1PJ67*e&nbZ}e*xhum+M>Io1aK!5-N0t5&UAW$!mC8wz~&U812`c5{I z009C72oNAZfB*pkxdnF1d-fasn#Cda+ogH}1PBlyK!5-N0t5)u3uMV@>Wnkp&7r=N zjU+&T009C72oNAZfIx16-SVFOM!#lp$o+Pyo&W&?1PBlyK!5-N0`&q}a+*5hOm}mr z?_?th5FkK+009C72oNBUTVS`mXTQ;}SsZe|U8*NQfB*pk1PBlyK!8BKK$e`Q&N$Or zhx!gMk^lh$1PBlyK!5-N0t99dsLgxE#c5@6n4!1SK!5-N0t5&UAV7cs0Rr^`S@Nbj z<4kKE>N~(l0t5&UAV7cs0RjXF5ST%rHt!i1rlrSwAP`%1B@g-lP009C72oNAZfB*pk z1PIg%td_fL_fs!qBmn{h2oNAZfB*pk1PBlqFHoCjR~a{6)CvR$5FkK+009C72oNAZ zpj}{<++Dkmb|Fs?AV7cs0RjXF5FkK+0D7YH(t~V1PBlyK!5-N0t5&UAV8p9V3pim zyN`AuPY@tLfB*pk1PBlyK!5;&@dCBEc9n7CMXf-9009C72oNAZfB*pk1lk2w$=$X4 zXczJX0RjXF5FkK+009C72oM-AP@8L488=?k3IqrcAV7cs0RjXF5FkLHU0{{mUAvEV zAx{t>K!5-N0t5&UAV7csf$;*hxptLt<3+7NfB*pk1PBlyK!5-N0tDIxR>|G9`)C*P z1OWmB2oNAZfB*pk1PBlqFHoCnR~a{6)CvR$5FkK+009C72oNAZpj}{<++Dkmb|Fs? zAV7cs0RjXF5FkK+0D7YH(t~V1PBlyK!5-N0t5&UAV8p9V3pimyN`AuPY@tLfB*pk z1PBlyK!5;&@dCBEc9n7CMXf-9009C72oNAZfB*pk1lk2w$=$X4XczJX0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U KAV7dX2>c(B2Lwm} literal 0 HcmV?d00001 From 082bf538a436668f20b0b2f6a62ef1ad82521c5d Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Thu, 13 Feb 2025 23:25:16 +0800 Subject: [PATCH 02/11] feat: Elastic Bands update --- PathPlanning/ElasticBands/elastic_bands.py | 25 ++++----------------- PathPlanning/ElasticBands/obstacles.npy | Bin 2000128 -> 2000128 bytes PathPlanning/ElasticBands/points.npy | Bin 0 -> 224 bytes 3 files changed, 4 insertions(+), 21 deletions(-) create mode 100644 PathPlanning/ElasticBands/points.npy diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index 3890c86537..2456c2bab2 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -53,6 +53,7 @@ def __init__(self, initial_path, obstacles, rho0=RHO0, kc=0.05, kr=-0.1): self.kc = kc # Contraction force gain self.kr = kr # Repulsive force gain self.rho0 = rho0 # Maximum distance for applying repulsive force + self._maintain_overlap() def compute_rho(self, position): """Compute the distance field value at the position""" @@ -143,8 +144,7 @@ def _maintain_overlap(self): class ElasticBandsVisualizer: def __init__(self): self.obstacles = np.zeros((500, 500)) - self.start_point = None - self.end_point = None + self.path_points = [] self.elastic_band = None if ENABLE_PLOT: @@ -158,24 +158,7 @@ def __init__(self): # Connect mouse events self.fig.canvas.mpl_connect("button_press_event", self.on_click) else: - self.path_points = [ - [30, 136], - [61, 214], - [77, 256], - [77, 309], - [53, 366], - [41, 422], - [51, 453], - [110, 471], - [184, 437], - [257, 388], - [343, 353], - [402, 331], - [476, 273], - [456, 206], - [430, 160], - [402, 107], - ] + self.path_points = np.load(pathlib.Path(__file__).parent / "points.npy") self.obstacles = np.load(pathlib.Path(__file__).parent / "obstacles.npy") self.plan_path() @@ -257,6 +240,6 @@ def plan_path(self): if __name__ == "__main__": - ElasticBandsVisualizer() + _ = ElasticBandsVisualizer() if ENABLE_PLOT: plt.show() diff --git a/PathPlanning/ElasticBands/obstacles.npy b/PathPlanning/ElasticBands/obstacles.npy index c8b50d3cbbc6118659c8ed72a414199cf570c5f1..d8e66d2041626f35ac85d8b6d5e0525d2635f1c4 100644 GIT binary patch delta 2584 zcmZqZs%ikD7RDB)EzEt7x62i>%wXJ}_mr7~j|8@yH~)jx+ks?(_4aiI0s_n+6DIKB zV=I8wG=S9*&(;8|uK??%7W)AA_H)*P7nrB_Jz*9AIeb)rsPHhDvT=d{2Po_u}22z&Q4n=FdfhaWug@Vpe$1W$}$>Yqbk5g4HfnVaFY7~ zb~7n#P+{f(@_zx;015?wWeV}NDyg;bU{gPCYHgPW05Rb4mjD0& delta 2614 zcmZqZs%ikD7RDB)EzEt7Cr~f9@usIq`+w-WyUJf*ecl*3{)&)#MhzSfz4sa0600+llWHX990K4q~ zIQr4pz~tosPF@M%qy}dL(_w=hC^a_NwF5~Y76tOe9I_fGv`%1boxrqp0&~Od?R8}w z4SXO{fpGxXz`z#(2mS=G2a(wdVDSd9ICa?(U^6@Bfih_VDDi*b-kv8X{ey8kpCLGl z(O3kSLn@d-mRmqAAC>|UAkSUk29*a8-wtkp1I(c0AHjieuK>scR1to`1S+sk0CR6S zM+Q5Z4QP@;g)^8yPPxI1q6y?Ua1I4#@IfVu%eAQDpbCKt+2J%10M(8+z_}X~*aN{{ zz_=V}FCQrCa4|tmfe3I}?Ex;gDP#l7_80ow|83#ekPT{TYyc;Z540V%< o-(cMS&qCk?I1Nz921e5gzUjaq{Q-(G6egrv#ZTg_bWdFZ0L2^sW&i*H diff --git a/PathPlanning/ElasticBands/points.npy b/PathPlanning/ElasticBands/points.npy new file mode 100644 index 0000000000000000000000000000000000000000..6a23860434af27b15058fd1d5e20eb5954f23772 GIT binary patch literal 224 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlWC!@qoAIaUsO_*m=~X4l#&V(cT3DEP6dh= zXCxM+0{I$dItoUbItsN4WCJdF1_%g)(vnd6AR~lv2FgDU<)4T0Uqk6_Q1N$A{%R Date: Fri, 14 Feb 2025 11:45:01 +0800 Subject: [PATCH 03/11] feat: ElasticBands update --- Mapping/DistanceMap/distance_map.py | 39 ++++++++++++++++++++++ PathPlanning/ElasticBands/elastic_bands.py | 34 ++++++++++++++----- 2 files changed, 65 insertions(+), 8 deletions(-) diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py index 54c98c6a75..af271d057f 100644 --- a/Mapping/DistanceMap/distance_map.py +++ b/Mapping/DistanceMap/distance_map.py @@ -11,11 +11,50 @@ import numpy as np import matplotlib.pyplot as plt +import scipy INF = 1e20 ENABLE_PLOT = True +def compute_sdf_scipy(obstacles): + """ + Compute the signed distance field (SDF) from a boolean field using scipy. + + Parameters + ---------- + obstacles : array_like + A 2D boolean array where '1' represents obstacles and '0' represents free space. + + Returns + ------- + array_like + A 2D array representing the signed distance field, where positive values indicate distance + to the nearest obstacle, and negative values indicate distance to the nearest free space. + """ + # distance_transform_edt use '0' as obstacles, so we need to convert the obstacles to '0' + a = scipy.ndimage.distance_transform_edt(obstacles == 0) + b = scipy.ndimage.distance_transform_edt(obstacles == 1) + return a - b + + +def compute_udf_scipy(obstacles): + """ + Compute the unsigned distance field (UDF) from a boolean field using scipy. + + Parameters + ---------- + obstacles : array_like + A 2D boolean array where '1' represents obstacles and '0' represents free space. + + Returns + ------- + array_like + A 2D array of distances from the nearest obstacle, with the same dimensions as `bool_field`. + """ + return scipy.ndimage.distance_transform_edt(obstacles == 0) + + def compute_sdf(obstacles): """ Compute the signed distance field (SDF) from a boolean field. diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index 2456c2bab2..cc1e5e798b 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -17,7 +17,7 @@ sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) -from Mapping.DistanceMap.distance_map import compute_sdf +from Mapping.DistanceMap.distance_map import compute_sdf_scipy # Elastic Bands Params MAX_BUBBLE_RADIUS = 100 @@ -45,8 +45,8 @@ def __init__(self, position, radius): class ElasticBands: - def __init__(self, initial_path, obstacles, rho0=RHO0, kc=0.05, kr=-0.1): - self.distance_map = compute_sdf(obstacles) + def __init__(self, initial_path, obstacles, rho0=RHO0, kc=KC, kr=KR): + self.distance_map = compute_sdf_scipy(obstacles) self.bubbles = [ Bubble(p, self.compute_rho(p)) for p in initial_path ] # Initialize bubble chain @@ -73,7 +73,7 @@ def contraction_force(self, i): dir_next = (next_ - current) / (np.linalg.norm(next_ - current) + 1e-6) return self.kc * (dir_prev + dir_next) - def external_force(self, i): + def repulsive_force(self, i): """Calculate external repulsive force for the i-th bubble""" h = STEP_SIZE # Step size b = self.bubbles[i].pos @@ -99,7 +99,7 @@ def update_bubbles(self): new_bubbles.append(self.bubbles[i]) # Fixed start and end points continue - f_total = self.contraction_force(i) + self.external_force(i) + f_total = self.contraction_force(i) + self.repulsive_force(i) alpha = self.bubbles[i].radius # Adaptive step size new_pos = self.bubbles[i].pos + alpha * f_total new_pos = np.clip(new_pos, 0, 499) @@ -146,10 +146,11 @@ def __init__(self): self.obstacles = np.zeros((500, 500)) self.path_points = [] self.elastic_band = None + self.running = True if ENABLE_PLOT: self.fig, self.ax = plt.subplots(figsize=(8, 8)) - # Set the display range of the graph + self.fig.canvas.mpl_connect("close_event", self.on_close) self.ax.set_xlim(0, 500) self.ax.set_ylim(0, 500) @@ -164,15 +165,32 @@ def __init__(self): self.plot_background() + def on_close(self, event): + """Handle window close event""" + self.running = False + plt.close("all") # Close all figure windows + def plot_background(self): """Plot the background grid""" - if not ENABLE_PLOT: + if not ENABLE_PLOT or not self.running: return self.ax.cla() self.ax.set_xlim(0, 500) self.ax.set_ylim(0, 500) self.ax.grid(True) + + if ENABLE_INTERACTIVE: + self.ax.set_title( + "Elastic Bands Path Planning\n" + "Left click: Add obstacles\n" + "Right click: Add path points\n" + "Middle click: Start planning", + pad=20, + ) + else: + self.ax.set_title("Elastic Bands Path Planning", pad=20) + if self.path_points: self.ax.plot( [p[0] for p in self.path_points], @@ -242,4 +260,4 @@ def plan_path(self): if __name__ == "__main__": _ = ElasticBandsVisualizer() if ENABLE_PLOT: - plt.show() + plt.show(block=True) From e7847907d214483a73dd7b2d9d01895c3ff2eab1 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Fri, 14 Feb 2025 12:07:41 +0800 Subject: [PATCH 04/11] feat: ElasticBands add test --- tests/test_elastic_bands.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 tests/test_elastic_bands.py diff --git a/tests/test_elastic_bands.py b/tests/test_elastic_bands.py new file mode 100644 index 0000000000..b1ef363d18 --- /dev/null +++ b/tests/test_elastic_bands.py @@ -0,0 +1,14 @@ +import conftest +import numpy as np +from PathPlanning.ElasticBands.elastic_bands import ElasticBands + + +def test_1(): + path = np.load("PathPlanning/ElasticBands/points.npy") + obstacles = np.load("PathPlanning/ElasticBands/obstacles.npy") + elastic_bands = ElasticBands(path, obstacles) + elastic_bands.update_bubbles() + + +if __name__ == "__main__": + conftest.run_this_test(__file__) From 51eab04ec7a9baa9a709be766f4a0d6368fe1638 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Fri, 14 Feb 2025 12:27:22 +0800 Subject: [PATCH 05/11] feat: ElasticBands reduce occupation --- PathPlanning/ElasticBands/elastic_bands.py | 41 ++++++++++++------ PathPlanning/ElasticBands/obstacles.npy | Bin 2000128 -> 384 bytes .../ElasticBands/{points.npy => path.npy} | Bin 224 -> 224 bytes 3 files changed, 28 insertions(+), 13 deletions(-) rename PathPlanning/ElasticBands/{points.npy => path.npy} (57%) diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index cc1e5e798b..8d4f856679 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -31,6 +31,7 @@ # Visualization Params ENABLE_PLOT = True ENABLE_INTERACTIVE = False +ENABLE_SAVE_DATA = False MAX_ITER = 50 @@ -144,6 +145,7 @@ def _maintain_overlap(self): class ElasticBandsVisualizer: def __init__(self): self.obstacles = np.zeros((500, 500)) + self.obstacles_points = [] self.path_points = [] self.elastic_band = None self.running = True @@ -159,8 +161,12 @@ def __init__(self): # Connect mouse events self.fig.canvas.mpl_connect("button_press_event", self.on_click) else: - self.path_points = np.load(pathlib.Path(__file__).parent / "points.npy") - self.obstacles = np.load(pathlib.Path(__file__).parent / "obstacles.npy") + self.path_points = np.load(pathlib.Path(__file__).parent / "path.npy") + self.obstacles_points = np.load( + pathlib.Path(__file__).parent / "obstacles.npy" + ) + for x, y in self.obstacles_points: + self.add_obstacle(x, y) self.plan_path() self.plot_background() @@ -216,6 +222,16 @@ def plot_background(self): plt.draw() plt.pause(0.01) + def add_obstacle(self, x, y): + """Add an obstacle at the given coordinates""" + size = 30 # Side length of the square + half_size = size // 2 + x_start = max(0, x - half_size) + x_end = min(self.obstacles.shape[0], x + half_size) + y_start = max(0, y - half_size) + y_end = min(self.obstacles.shape[1], y + half_size) + self.obstacles[x_start:x_end, y_start:y_end] = 1 + def on_click(self, event): """Handle mouse click events""" if event.inaxes != self.ax: @@ -224,23 +240,22 @@ def on_click(self, event): x, y = int(event.xdata), int(event.ydata) if event.button == 1: # Left click to add obstacles - size = 30 # Side length of the square - half_size = size // 2 - - # Ensure not out of the map boundary - x_start = max(0, x - half_size) - x_end = min(self.obstacles.shape[0], x + half_size) - y_start = max(0, y - half_size) - y_end = min(self.obstacles.shape[1], y + half_size) - - # Set the square area as obstacles (value set to 1) - self.obstacles[x_start:x_end, y_start:y_end] = 1 + self.add_obstacle(x, y) + self.obstacles_points.append([x, y]) elif event.button == 3: # Right click to add path points self.path_points.append([x, y]) elif event.button == 2: # Middle click to end path input and start planning if len(self.path_points) >= 2: + if ENABLE_SAVE_DATA: + np.save( + pathlib.Path(__file__).parent / "path.npy", self.path_points + ) + np.save( + pathlib.Path(__file__).parent / "obstacles.npy", + self.obstacles_points, + ) self.plan_path() self.plot_background() diff --git a/PathPlanning/ElasticBands/obstacles.npy b/PathPlanning/ElasticBands/obstacles.npy index d8e66d2041626f35ac85d8b6d5e0525d2635f1c4..af4376afcf0e987bbb62c4a80c7afac3d221961d 100644 GIT binary patch literal 384 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlWC!@qoAIaUsO_*m=~X4l#&V(cT3DEP6dh= zXCxM+0{I$-W;zN+nmP)#3giN=d=|8_M@a;}<~r z%~1IiD1RoD9|z^LL+NCwxEhq72<3}I`IS(ULI^1x9f*d43PmIl1p>4|G>E6*Me_(`bTqnR^mle{ z?i_pU^QG|B%+A_ApO1@Q_P@XT!*758$Dh3W*Smjz`0DF#|NQa8uik(7^*6u#@Y(wh z-~9dKKRO~i z;WdYnmq*bA2oNAZfB*pk1PBngP(UsXOb)L(T-cJPB0zuu0RjXF5FkK+KuH0)G>)Ca zYYrtZkD>_>AV7cs0RjXF5Fl`&fLt1w9A0y{uq91JfB*pk1PBlyK!5;&k^*vR96N{C z97BAJ3tQ4u1PBlyK!5-N0t5&UC@CP9#<6pF&7tJw zQ8WPp1PBlyK!5-N0t7A;kV^xT!)p!~wxp>D5FkK+009C72oNCflz==JTpTz&b#Z)5 zfB*pk1PBlyK!5;&iv{G;0Oi2p;;Usk0t5&UAV7cs0RjXFJS8BP1{VhoPhA`z6Cgl< z009C72oNAZ;9>!}G(b6U*aVJ!5GqCj1PBlyK!5-N0t5);3mo%^=P>8MA^-MSfdByl z1PBlyK!5-N0uKQ>G%Pr9*aVJ!5GqCj1PBlyK!5-N0t5);3mo%^=P>8MA^-MSfdByl z1PBlyK!5-N0uKQ>G%Pr9*aVJ!5GqCj1PBlyK!5-N0t5&=1dh4EbJ%d;unA=KLFgI@ z5FkK+009C72oNCf5Xj0O<_%*G95#VtAB2jL009C72oNAZfB*pk4}oKD@EkTAIBWu0 zeGs}v0t5&UAV7cs0RjXFJOr}xhk3)81BXrE*ax9vBtU=w0RjXF5FkK+z(e4e8$5>% z2M(J+Rv(0}kpKY#1PBlyK!5-N0uO<#{9)cO=D=YSIQBuP7zq#{K!5-N0t5&UAn*`4 z<_6DU!-2ylkktpFYa~E`009C72oNAZU}1M!GXgjkktpFYa~E`009C72oNAZU}1M!GXgjkktpF zYa~E`009C72oNAZU|!E!GXgjkktpFYa~E`009C72oNAZU`BzgoM7JQ1`ZrHfnI$O+C~Bd z2oNAZfB*pk1ZEWIl^d)R-NAvwCXm$!p=%^SfB*pk1PBlyKww6Jtejxp=mriPHi2G! z5ZXop1PBlyK!5-N0t99h=#?9+6Wzgq!zPf`2cc^uK!5-N0t5&UAV6S7fvlWh-slDn z95#VoeGuA40t5&UAV7cs0RjYO6zG*3tP|bAfx{+{)d!(#BtU=w0RjXF5FkLHqCi&8 zFmH4d2M(LS***v@BLM;g2oNAZfB*pk6$Q@b2H!<@aNw{BWc5Ml8VL{}K!5-N0t5&U zs3?$?Gt3*^#DT*maJCOZ%SeC#0RjXF5FkK+Kt+MGxxshQ9UM4p0$F_!x<HoNAZ zfB*pk1S$$-+svaOeT=XclrW$kOP z+b;nE1PBlyK!5-N0t5)$Lm(^v%`)$P3bX9@s(tQt`z1ht009C72oNAZfB=De2wau- zW}Rn0g<1FiWB0w+?Uw)n0t5&UAV7cs0RjZ>A@F1K-70@?KZRB5Ia|kGw_gGT2oNAZ zfB*pk1PBnghrro9x5{@d^;POSTSv>mdjbRq5FkK+009C72oM+}a5m4a@?A@LmHN)s z(Q@#f009C72oNAZfB*pk1jY!Q&2y`K*HT`kzO!|-9K0t$fB*pk1PBlyK!5;&F#>1v z+$!I-lvkN{IU%fWjB1PBlyK!5-N0t5&U7$a~t&#m%ZOL>+0&eqX#@SXqx0t5&UAV7cs z0RjZZ2%ODxt9;i|UZuXXb+jD3CqRGz0RjXF5FkK+0D&%<8VAV7cs0RjXF5FkK+z#RpuC{QJr?{(hs z4D5*j0RjXF5FkK+009C72<*AF%C8eAK!5-N0t5&UAV7cs0RndvsFKU~I`4P}_C$aH z0RjXF5FkK+009C7_S{qK4yyIEh69EDQ2oNAZ zfB*pk1PJ5{^v<8wi!;q~$nO{{5FkK+009C72oNAZfIx;oR!%f;oK|m#j2WuE`?U2#4<|IIX z009C72oNAZfWV3Zz4My&qKm3HtaxYCM1TMR0t5&UAV7csfee8vIZ>U_J-r<=de)o- z2oNAZfB*pk1PBmVQJ{BTvtD#j6^9k?jG71#AV7cs0RjXF5Fn5tP$eg-GrFg@Lq^Y< zlK=q%1PBlyK!5-N0xJsi&TH0-E~?_N;+;_w0RjXF5FkK+009C7G6bsRM0G~@^mfST zS#uH~K!5-N0t5&UAV6S6f!=w|deKEy99FzDY9c^@009C72oNAZfIx;om7J)~=$_sV z89i%G0t5&UAV7cs0RjXFtSHbsuURj;sEWgicScPF2oNAZfB*pk1PBnw5U7$9)fwH> z+aaT8%}Ia&0RjXF5FkK+0D%<+dgnFkMHf|ZSnFtowv*si~fB*pk1PBlyK!CuC0=@H^^`eWaIIMVQ)I@*)0RjXF5FkK+0D%mF zDmhV|(LKE#GJ4jW1PBlyK!5-N0t5&USW%#NUb9|wQ5A<3?~IxV5FkK+009C72oNBU zAy6eJsx!K$w?jtHnv(zl0t5&UAV7cs0Rk%u^v-M6i!Q3-u;QIj69EDQ2oNAZfB*pk z1TqAwqQrhc382O)kJ^*0RjXF z5FkK+0D+kWM(0H9NB8u0n7L=|od5v>1PBlyK!5-N0+|B6^O^PHG^;vf_OiJN5FkK+ z009C72oNAJvq06HsP5?6-VQV06?-Q@fB*pk1PBlyK!8A|K<|8Jy*SOP4w=1dZUO`d z5FkK+009C72+S-{H7BY&y0*8&%y-4!2@oJafB*pk1PBlykSWkRpII+Xv#LX8FPobH z0RjXF5FkK+009Cs3slXC>W;4M?J)CQv3CLl2oNAZfB*pk1PEjb^v-A2i_@&?klD-T zCP07y0RjXF5FkK+z{~BtU=w0RjXF5FkK+K&F6P8oC_DI%M{^ zxd{*;K!5-N0t5&UATWb~TpFjuVXVUpcf}qF5FkK+009C72oNBUDIk}IE{CxWnLTc9 z0t5&UAV7cs0RjXF%pf3_#wl?a>oCJzu}1;~2oNAZfB*pk1PEjb$fcpnVXQ-DkDHqS z0RjXF5FkK+009Cs2*{;zN*u;I%y3uikpKY#1PBlyK!5-N0+|AGY3Ono>yX*w<|aUZ z009C72oNAZfWQm_a%r3rhp`Sb+!cEyK!5-N0t5&UAV7dXrhr@;x*WzjWcIkZ2@oJa zfB*pk1PBlyFoS?x8mGiztiud<#U2R|AV7cs0RjXF5Fn5#AeV+Nhp`TsJ#KCS1PBly zK!5-N0t5)mARw27VtV3pxo0|Xu0t5&U zAV7cs0Rl4!$fa>g9L74#a98Y+009C72oNAZfB*pknF4ZY=yDkAklEwrCP07y0RjXF z5FkK+zzhO%X`B*=u?{ob6?-H=fB*pk1PBlyK!8A|K($=jvaIHCw|ZOW;ynQZ1PBly zK!5-N0tBiE+%2bN@27RH+3T7=SIfbB0t5&UAV7cs0RjXF5Fjv8pp|1+eLvFL z+5`v?AV7cs0RjXF5FkLHC9rA^Z`IXu@SXqx0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7Y6*PzMbum6x7O*ZmH+_) z1PBlyK!5-N0t99esFKU?R_84JVV?vD5FkK+009C72oNC9N8oO`waR|_oUT;}5FkK+ z009C72oNAZU>1QYdH8O1&e9+DNq_(W0t5&UAV7cs0Rnvl?v`7t?5EG^T7>`s0t5&U zAV7cs0RjYO5vY=f?^fq5{b8R32oNAZfB*pk1PBly&`02IxwXoE`kby+2oNAZfB*pk z1PBlyKwuVuDtY*BbO9{Qz9c|^009C72oNAZfB=F10&;7ZS8?dyVOAtSfB*pk1PBlyK!5;&GXhm|sXEVe z!>7Ng=J;t5Ul1TbfB*pk1PBlyK;RmIYI&=Ay{~pnHNRKA{Cxrh2oNAZfB*pk1PGiJ zsFt^?*L$`vTHjUkYdLsNfB*pk1PBlyK!5;&s|Bj%t?Ko@+CSC&UiI?#2@oJafB*pk z1PBlya8{sN-l|^j*}iCfSIw{G;5`8X1PBlyK!5-N0tBuWsFt_X8y&>qoWs@k&3pt1 z5FkK+009C72oPuq$Y~+Nfx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oQJ($f04ufx{+n?1NA-5+Fc;009C72oNAZAYb5^M?8l)2M+nS&k6(x z5FkK+009C72oRW0KwgeB;wo9AV7cs z0RjXF5SULuE{!we(AQzUn_|ZV2oNAZfB*pk1PBnw6_88AmqTBN+^#k|0RjXF5FkK+ z009C7<`a-hgeXr0zo^RF^b%gK8J1PBly zK!5-N0t5(D6<8(j-KCCJAKhhtciBhF$$J6>2oNAZfB*pk1PD|WxJzDJWgo3RTBW|T zb+jD3CqRGz0RjXF5FkK+0D-p!&gL!Oh2N(f-d2A1?o)qnwLfn;cu#-;0RjXF5FkK+ z009DT3#^t4N7wVVvV7ih@tyzy0t5&UAV7cs0RjXS;N6Pk;ac z0t5&UAV7cs0RnFed@5)8^Kd)s@V2slKX18sPk;ac0t5&UAV7cs0Rka#Hh22&oL8%_ zs`<4XyeB|_009C72oNAZfB=EF1*+x7?0Vl;_V4E{7w-uWAV7cs0RjXF5FkJx1hVsL zwfS3p#i1qRJplp)2oNAZfB*pk1PE*b)pBEYy{)dwUiWVEw;a4DK!5-N0t5&UAV7cs zfwu+jmg{EU&)drN{y#{|!FvJ(2oNAZfB*pk1PBlqBQSg3es=#Y^bw<{kN2# zooDv%TMph6AV7cs0RjXF5FkK+z!-tqbLO-AZz(@J&+Ol~9K0t$fB*pk1PBlyK!5;& zF#@ya%xCxCQhs)x*}rc&cu#-;0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&U zAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C7 z2oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N z0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72wWlX EKat1^r~m)} diff --git a/PathPlanning/ElasticBands/points.npy b/PathPlanning/ElasticBands/path.npy similarity index 57% rename from PathPlanning/ElasticBands/points.npy rename to PathPlanning/ElasticBands/path.npy index 6a23860434af27b15058fd1d5e20eb5954f23772..be7c253d6507299a2ef2f2ea901472838c89e76f 100644 GIT binary patch delta 103 xcmaFB_<(UjLxLOw1Xx08H7LEE5yH3(<*$VDZ$tT-P`V5%t`6mQK>4ju8URQF2$lc< delta 103 ycmaFB_<(UjLxMa51cX6pNhp1g5yChF Date: Fri, 14 Feb 2025 14:18:49 +0800 Subject: [PATCH 06/11] fix: ElasticBands test --- tests/test_elastic_bands.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/tests/test_elastic_bands.py b/tests/test_elastic_bands.py index b1ef363d18..ad4e13af1a 100644 --- a/tests/test_elastic_bands.py +++ b/tests/test_elastic_bands.py @@ -4,8 +4,17 @@ def test_1(): - path = np.load("PathPlanning/ElasticBands/points.npy") - obstacles = np.load("PathPlanning/ElasticBands/obstacles.npy") + path = np.load("PathPlanning/ElasticBands/path.npy") + obstacles_points = np.load("PathPlanning/ElasticBands/obstacles.npy") + obstacles = np.zeros((500, 500)) + for x, y in obstacles_points: + size = 30 # Side length of the square + half_size = size // 2 + x_start = max(0, x - half_size) + x_end = min(obstacles.shape[0], x + half_size) + y_start = max(0, y - half_size) + y_end = min(obstacles.shape[1], y + half_size) + obstacles[x_start:x_end, y_start:y_end] = 1 elastic_bands = ElasticBands(path, obstacles) elastic_bands.update_bubbles() From 859d4936ac40d6738dbeadc5bd0419ceef1dbcf7 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Fri, 14 Feb 2025 15:14:38 +0800 Subject: [PATCH 07/11] feat: ElasticBands remove tangential component --- PathPlanning/ElasticBands/elastic_bands.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index 8d4f856679..cc10888cf5 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -30,7 +30,7 @@ # Visualization Params ENABLE_PLOT = True -ENABLE_INTERACTIVE = False +ENABLE_INTERACTIVE = True ENABLE_SAVE_DATA = False MAX_ITER = 50 @@ -101,8 +101,13 @@ def update_bubbles(self): continue f_total = self.contraction_force(i) + self.repulsive_force(i) + v = self.bubbles[i - 1].pos - self.bubbles[i + 1].pos + + # Remove tangential component + f_star = f_total - f_total * v * v / (np.linalg.norm(v) ** 2 + 1e-6) + alpha = self.bubbles[i].radius # Adaptive step size - new_pos = self.bubbles[i].pos + alpha * f_total + new_pos = self.bubbles[i].pos + alpha * f_star new_pos = np.clip(new_pos, 0, 499) new_radius = self.compute_rho(new_pos) From 0de8fe26cfa08468a57838fecb878aa5df10dc09 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Sun, 16 Feb 2025 01:09:18 +0800 Subject: [PATCH 08/11] feat: Elastic Bands update --- PathPlanning/ElasticBands/elastic_bands.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index cc10888cf5..c079e5f0ab 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -30,7 +30,7 @@ # Visualization Params ENABLE_PLOT = True -ENABLE_INTERACTIVE = True +ENABLE_INTERACTIVE = False ENABLE_SAVE_DATA = False MAX_ITER = 50 @@ -210,7 +210,8 @@ def plot_background(self): markersize=8, ) - self.ax.imshow(self.obstacles.T, origin="lower", cmap="binary", alpha=0.3) + self.ax.imshow(self.obstacles.T, origin="lower", cmap="binary", alpha=0.8) + self.ax.plot([], [], color="black", label="obstacles") if self.elastic_band is not None: path = [b.pos.tolist() for b in self.elastic_band.bubbles] path = np.array(path) @@ -222,8 +223,9 @@ def plot_background(self): ) self.ax.add_patch(circle) self.ax.plot(bubble.pos[0], bubble.pos[1], "bo", markersize=10) + self.ax.plot([], [], color="green", label="bubbles") - self.ax.legend() + self.ax.legend(loc="upper right") plt.draw() plt.pause(0.01) From 845a3738096f1f98b90602de75bea5588ba41c2f Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Sun, 16 Feb 2025 01:26:52 +0800 Subject: [PATCH 09/11] feat: Elastic Bands doc --- .../elastic_bands/elastic_bands_main.rst | 10 ++++++++++ docs/modules/5_path_planning/path_planning_main.rst | 1 + 2 files changed, 11 insertions(+) create mode 100644 docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst new file mode 100644 index 0000000000..95bf7ce506 --- /dev/null +++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst @@ -0,0 +1,10 @@ +Elastic Bands +------------- + +This is a path planning with Elastic Bands. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif + +Ref: + +- `Elastic Bands: Connecting Path Planning and Control `__ diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst index 4960330b3e..65fbfdbc3d 100644 --- a/docs/modules/5_path_planning/path_planning_main.rst +++ b/docs/modules/5_path_planning/path_planning_main.rst @@ -31,3 +31,4 @@ Path planning is the ability of a robot to search feasible and efficient path to hybridastar/hybridastar frenet_frame_path/frenet_frame_path coverage_path/coverage_path + elastic_bands/elastic_bands \ No newline at end of file From 782bc486ef37ea82f3beb7e9ea6bb7c1a6d83d5d Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Sun, 16 Feb 2025 22:00:41 +0800 Subject: [PATCH 10/11] feat: Elastic Bands update --- Mapping/DistanceMap/distance_map.py | 12 ++++ PathPlanning/ElasticBands/elastic_bands.py | 19 ++++-- .../elastic_bands/elastic_bands_main.rst | 63 +++++++++++++++++++ 3 files changed, 90 insertions(+), 4 deletions(-) diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py index af271d057f..0dcc7380c5 100644 --- a/Mapping/DistanceMap/distance_map.py +++ b/Mapping/DistanceMap/distance_map.py @@ -20,6 +20,12 @@ def compute_sdf_scipy(obstacles): """ Compute the signed distance field (SDF) from a boolean field using scipy. + This function has the same functionality as compute_sdf. + However, by using scipy.ndimage.distance_transform_edt, it can compute much faster. + + Example: 500×500 map + • compute_sdf: 3 sec + • compute_sdf_scipy: 0.05 sec Parameters ---------- @@ -41,6 +47,12 @@ def compute_sdf_scipy(obstacles): def compute_udf_scipy(obstacles): """ Compute the unsigned distance field (UDF) from a boolean field using scipy. + This function has the same functionality as compute_udf. + However, by using scipy.ndimage.distance_transform_edt, it can compute much faster. + + Example: 500×500 map + • compute_udf: 1.5 sec + • compute_udf_scipy: 0.02 sec Parameters ---------- diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index c079e5f0ab..0b6b2eb6d1 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -46,7 +46,16 @@ def __init__(self, position, radius): class ElasticBands: - def __init__(self, initial_path, obstacles, rho0=RHO0, kc=KC, kr=KR): + def __init__( + self, + initial_path, + obstacles, + rho0=RHO0, + kc=KC, + kr=KR, + lambda_=LAMBDA, + step_size=STEP_SIZE, + ): self.distance_map = compute_sdf_scipy(obstacles) self.bubbles = [ Bubble(p, self.compute_rho(p)) for p in initial_path @@ -54,6 +63,8 @@ def __init__(self, initial_path, obstacles, rho0=RHO0, kc=KC, kr=KR): self.kc = kc # Contraction force gain self.kr = kr # Repulsive force gain self.rho0 = rho0 # Maximum distance for applying repulsive force + self.lambda_ = lambda_ # Overlap constraint factor + self.step_size = step_size # Step size for calculating gradient self._maintain_overlap() def compute_rho(self, position): @@ -76,7 +87,7 @@ def contraction_force(self, i): def repulsive_force(self, i): """Calculate external repulsive force for the i-th bubble""" - h = STEP_SIZE # Step size + h = self.step_size # Step size b = self.bubbles[i].pos rho = self.bubbles[i].radius @@ -125,7 +136,7 @@ def _maintain_overlap(self): while i < len(self.bubbles) - 1: bi, bj = self.bubbles[i], self.bubbles[i + 1] dist = np.linalg.norm(bi.pos - bj.pos) - if dist > LAMBDA * (bi.radius + bj.radius): + if dist > self.lambda_ * (bi.radius + bj.radius): new_pos = (bi.pos + bj.pos) / 2 rho = self.compute_rho( new_pos @@ -141,7 +152,7 @@ def _maintain_overlap(self): prev = self.bubbles[i - 1] next_ = self.bubbles[i + 1] dist = np.linalg.norm(prev.pos - next_.pos) - if dist <= LAMBDA * (prev.radius + next_.radius): + if dist <= self.lambda_ * (prev.radius + next_.radius): del self.bubbles[i] # Delete if redundant else: i += 1 diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst index 95bf7ce506..139996f291 100644 --- a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst +++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst @@ -5,6 +5,69 @@ This is a path planning with Elastic Bands. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif + +Core Concept +~~~~~~~~~~~~ +- **Elastic Band**: A dynamically deformable collision-free path initialized by a global planner. +- **Objective**: + + * Shorten and smooth the path. + * Maximize obstacle clearance. + * Maintain global path connectivity. + +Bubble Representation +~~~~~~~~~~~~~~~~~~~ +- **Definition**: A local free-space region around configuration :math:`b`: + + .. math:: + B(b) = \{ q: \|q - b\| < \rho(b) \}, + + where :math:`\rho(b)` is the radius of the bubble. + + +Force-Based Deformation +~~~~~~~~~~~~~~~~~~~~~~~ +The elastic band deforms under artificial forces: + +Internal Contraction Force +++++++++++++++++++++++++++ +- **Purpose**: Reduces path slack and length. +- **Formula**: For node :math:`b_i`: + + .. math:: + f_c(b_i) = k_c \left( \frac{b_{i-1} - b_i}{\|b_{i-1} - b_i\|} + \frac{b_{i+1} - b_i}{\|b_{i+1} - b_i\|} \right) + + where :math:`k_c` is the contraction gain. + +External Repulsion Force ++++++++++++++++++++++++++ +- **Purpose**: Pushes the path away from obstacles. +- **Formula**: For node :math:`b_i`: + + .. math:: + f_r(b_i) = \begin{cases} + k_r (\rho_0 - \rho(b_i)) \nabla \rho(b_i) & \text{if } \rho(b_i) < \rho_0, \\ + 0 & \text{otherwise}. + \end{cases} + + where :math:`k_r` is the repulsion gain, :math:`\rho_0` is the maximum distance for applying repulsion force, and :math:`\nabla \rho(b_i)` is approximated via finite differences: + + .. math:: + \frac{\partial \rho}{\partial x} \approx \frac{\rho(b_i + h) - \rho(b_i - h)}{2h}. + +Dynamic Path Maintenance +~~~~~~~~~~~~~~~~~~~~~~~ +1. **Node Update**: + + .. math:: + b_i^{\text{new}} = b_i^{\text{old}} + \alpha (f_c + f_r), + + where :math:`\alpha` is a step-size parameter, which often proportional to :math:`\rho(b_i^{\text{old}})` + +2. **Overlap Enforcement**: +- Insert new nodes if adjacent nodes are too far apart +- Remove redundant nodes if adjacent nodes are too close + Ref: - `Elastic Bands: Connecting Path Planning and Control `__ From 5b31739329290741cac470aaa5fec77fc23ad05e Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Mon, 17 Feb 2025 18:29:45 +0800 Subject: [PATCH 11/11] feat: ElasticBands update --- PathPlanning/ElasticBands/elastic_bands.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index 0b6b2eb6d1..785f822d14 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -30,7 +30,11 @@ # Visualization Params ENABLE_PLOT = True +# ENABLE_INTERACTIVE is True allows user to add obstacles by left clicking +# and add path points by right clicking and start planning by middle clicking ENABLE_INTERACTIVE = False +# ENABLE_SAVE_DATA is True allows saving the path and obstacles which added +# by user in interactive mode to file ENABLE_SAVE_DATA = False MAX_ITER = 50