Merge pull request AtsushiSakai#171 from yuokamoto/fix_bit_ellipse_calc

fix ellisoid calculation

Author: AtsushiSakai

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -172,10 +172,10 @@ def setup_planning(self):
         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])