github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 06:28:00 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix ellisoid calculation

Browse Source
This commit is contained in:
Yu Okamoto
2019-02-21 22:50:50 +09:00
parent 754256d15e
commit 46aae2401f
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -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])