From 46aae2401fbf7d364f06f243bef3cb4f2e22b14b Mon Sep 17 00:00:00 2001 From: Yu Okamoto Date: Thu, 21 Feb 2019 22:50:50 +0900 Subject: [PATCH] fix ellisoid calculation --- PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index e8f225d7..366ec5ac 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -172,10 +172,10 @@ class BITStar(object): cBest = self.g_scores[self.goalId] # Computing the sampling space - cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) - + pow(self.start[0] - self.goal[1], 2)) / 1.5 + cMin = math.sqrt(pow(self.start[0] - self.goal[0], 2) + + pow(self.start[1] - self.goal[1], 2)) / 1.5 xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0], - [(self.goal[1] - self.start[1]) / 2.0], [0]]) + [(self.start[1] + self.goal[1]) / 2.0], [0]]) a1 = np.array([[(self.goal[0] - self.start[0]) / cMin], [(self.goal[1] - self.start[1]) / cMin], [0]]) etheta = math.atan2(a1[1], a1[0])