AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

remove np.matrix

Browse Source
This commit is contained in:
Atsushi Sakai
2018-12-15 18:01:43 +09:00
parent eaa9b4d025
commit bfc785c998
1 changed files with 8 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -174,13 +174,13 @@ class BITStar(object):
# 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
xCenter = np.matrix([[(self.start[0] + self.goal[0]) / 2.0],
[(self.goal[1] - self.start[1]) / 2.0], [0]])
a1 = np.matrix([[(self.goal[0] - self.start[0]) / cMin],
[(self.goal[1] - self.start[1]) / cMin], [0]])
xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0],
[(self.goal[1] - self.start[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])
# first column of idenity matrix transposed
id1_t = np.matrix([1.0, 0.0, 0.0])
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = np.dot(a1, id1_t)
U, S, Vh = np.linalg.svd(M, 1, 1)
C = np.dot(np.dot(U, np.diag(
@@ -530,7 +530,6 @@ class BITStar(object):
def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
samples=None, start=None, end=None, tree=None):
print("Plotting Graph")
plt.clf()
for rnd in samples:
if rnd is not None:
@@ -561,9 +560,9 @@ class BITStar(object):
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
R = np.matrix([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R * np.matrix([x, y])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R @ np.array([x, y])
px = np.array(fx[0, :] + cx).flatten()
py = np.array(fx[1, :] + cy).flatten()
plt.plot(cx, cy, "xc")