Seems hava bug in method lazy_shortening of class RRTStarBidirectionalHeuristic

[liuchong1995]

# update best path
# remove cost of removed edges
pre_best = self.c_best
self.c_best -= sum(dist_between_points(i, j) for i, j in pairwise(self.sigma_best[a:b]))
# add cost of new edge
self.c_best += + dist_between_points(self.sigma_best[a], self.sigma_best[b])
self.sigma_best = self.sigma_best[:min(a, b) + 1] + self.sigma_best[max(a, b):]
after_best = self.c_best
print(self.samples_taken,"samples ","pre_best: ", pre_best, "after_best: ", after_best)

I add two variable to remember the change of c_best, run the example rrt_star_bid_h_2d.py(max_samples = 50000,prc = 0.000001) the console is

1136 samples pre_best: 147.27631712157728 after_best: 150.50517143061194 1580 samples pre_best: 149.07440848778623 after_best: 154.02473881731657 1591 samples pre_best: 152.70569271151646 after_best: 189.95360817018766 1601 samples pre_best: 149.32839308025953 after_best: 179.4453476769691 1609 samples pre_best: 172.77377293324594 after_best: 157.15884990689494 1627 samples pre_best: 148.31354996340158 after_best: 156.29333137162521 1637 samples pre_best: 149.1706237177986 after_best: 157.16829854933493 1657 samples pre_best: 153.34796461046608 after_best: 157.16829854933496 1666 samples pre_best: 153.3652822500094 after_best: 160.96965032326972 1687 samples pre_best: 155.06727461460594 after_best: 169.25245716730342 1692 samples pre_best: 159.71872887272414 after_best: 170.4275243089187 1696 samples pre_best: 151.96171927099167 after_best: 172.5873966049496 1715 samples pre_best: 147.9352498164779 after_best: 154.97443333935067 1733 samples pre_best: 148.0548974739475 after_best: 173.80074256390108 1743 samples pre_best: 149.32378668988508 after_best: 175.93740885996755 1755 samples pre_best: 153.97781044999525 after_best: 181.29535020507882 1765 samples pre_best: 155.80810660614787 after_best: 170.51843700102742 1769 samples pre_best: 165.69613793043368 after_best: 194.97927005774272 1783 samples pre_best: 151.91759841366624 after_best: 193.3618164841871 1799 samples pre_best: 155.70062223710895 after_best: 163.16083759986492 1807 samples pre_best: 153.56712099976983 after_best: 169.4573163110155 1811 samples pre_best: 169.4573163110155 after_best: 196.4927441024516 1817 samples pre_best: 151.44461546378926 after_best: 175.0240219131137 1822 samples pre_best: 149.1612152734508 after_best: 164.85613888193072 1830 samples pre_best: 153.9478548782316 after_best: 160.52198975701043 1837 samples pre_best: 160.41089651882388 after_best: 168.35113349301423 2236 samples pre_best: 146.62926170454017 after_best: 147.5816293148402 2241 samples pre_best: 147.5816293148402 after_best: 151.61023624966396 2242 samples pre_best: 151.61023624966396 after_best: 181.43622010703797 2251 samples pre_best: 153.70750118031822 after_best: 157.54004003665247 2253 samples pre_best: 157.54004003665247 after_best: 173.14227684364138 2265 samples pre_best: 157.36418886806683 after_best: 164.9897074568459 2269 samples pre_best: 151.27365231989575 after_best: 192.9479331301609 2271 samples pre_best: 179.03234723755835 after_best: 218.14467572605895 2277 samples pre_best: 150.44856477666883 after_best: 156.97356214791188 2279 samples pre_best: 156.97356214791188 after_best: 164.83801818148063 2326 samples pre_best: 149.6099219232762 after_best: 152.5874562397474 2329 samples pre_best: 152.5874562397474 after_best: 160.25403684005317 2333 samples pre_best: 153.45287773460592 after_best: 173.800502332282 2335 samples pre_best: 154.17718899425404 after_best: 171.6316206505236 2338 samples pre_best: 156.06523698537885 after_best: 171.77863115575138 2343 samples pre_best: 151.45986597871934 after_best: 178.91763239481583 2349 samples pre_best: 149.15797474873287 after_best: 159.58070000333905 2363 samples pre_best: 150.0154718759291 after_best: 158.02998062964014 2381 samples pre_best: 149.48485494540273 after_best: 177.11044834512631 2390 samples pre_best: 148.91429948696754 after_best: 155.8776982411878 2392 samples pre_best: 148.9332211525741 after_best: 177.04876948395608 2398 samples pre_best: 148.64593590835386 after_best: 168.47199424897258 2400 samples pre_best: 151.1349936614904 after_best: 159.0976186154945 2414 samples pre_best: 146.35960502533163 after_best: 155.29782622292154 2426 samples pre_best: 150.37464467196168 after_best: 155.7942227186323 2434 samples pre_best: 155.7942227186323 after_best: 168.68684880982022 2441 samples pre_best: 161.68427836558953 after_best: 171.05784697968141 2462 samples pre_best: 153.34056678632658 after_best: 169.65327609241263 2481 samples pre_best: 148.80447276481226 after_best: 168.45850094303756 2483 samples pre_best: 165.63047407729917 after_best: 179.523988133413 2487 samples pre_best: 150.7347010169954 after_best: 154.5681529983975 2509 samples pre_best: 150.48920306172397 after_best: 159.06384380869687 2512 samples pre_best: 159.06384380869687 after_best: 188.34102476107265 2521 samples pre_best: 148.49010659871817 after_best: 173.6172654252876 2530 samples pre_best: 150.50728730026722 after_best: 172.52243182850617 2532 samples pre_best: 150.96133314918342 after_best: 163.27333650603805 2537 samples pre_best: 150.43652531756268 after_best: 158.2451879629066 2554 samples pre_best: 149.87327040803999 after_best: 171.53825345780712 2557 samples pre_best: 151.06725426003794 after_best: 183.82313642937237 2561 samples pre_best: 153.951208335046 after_best: 164.0427601623763 2564 samples pre_best: 153.08933898574304 after_best: 177.95379691685451 2566 samples pre_best: 172.40827627791012 after_best: 180.22835519065126 2568 samples pre_best: 149.86725589376144 after_best: 180.32719821066428 2580 samples pre_best: 150.0030732580511 after_best: 158.0030711255912 2581 samples pre_best: 158.0030711255912 after_best: 185.17292871463349 2583 samples pre_best: 149.36097389846253 after_best: 167.694582078274 2586 samples pre_best: 154.2788957293173 after_best: 170.06069615064087 2595 samples pre_best: 150.56956518196557 after_best: 157.09456255320862 2604 samples pre_best: 149.69724066197475 after_best: 165.08631184523537 2607 samples pre_best: 154.07481982880125 after_best: 166.29742055517153 2615 samples pre_best: 147.37582381850086 after_best: 165.31709364168796 2623 samples pre_best: 150.11558067197655 after_best: 158.07202869108107 2625 samples pre_best: 152.56589213641718 after_best: 175.31563415470316 2629 samples pre_best: 149.87620185906923 after_best: 149.87705787991501 2652 samples pre_best: 149.87705787991501 after_best: 160.1865682242192 2675 samples pre_best: 147.57654441702468 after_best: 182.97273703360898 2677 samples pre_best: 149.74642256792947 after_best: 160.85757727280804 2690 samples pre_best: 149.03731687964074 after_best: 156.14409697415604 2695 samples pre_best: 152.71344491143924 after_best: 173.46244316056683 2702 samples pre_best: 161.63372931567147 after_best: 173.60477336036297 2709 samples pre_best: 148.55702642327205 after_best: 169.3085830810782 2725 samples pre_best: 148.2630011567049 after_best: 152.69770135960783 2735 samples pre_best: 148.18791139146055 after_best: 154.30061538386755 2772 samples pre_best: 147.7942484395685 after_best: 149.84915605788547 2788 samples pre_best: 149.84915605788547 after_best: 179.05605538348172 ...

seems the method does not shortening the current best path?

[SZanlongo]

This seems to be an off-by-one bug from me trying to be clever in #21, Should be taken care of now.

[SZanlongo]

Duplicate of #19.