remove np.matrix

Author: AtsushiSakai

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -172,17 +172,17 @@ def main():
     # goal position [x(m), y(m)]
     goal = np.array([10, 10])
     # obstacles [x(m) y(m), ....]
-    ob = np.matrix([[-1, -1],
-                    [0, 2],
-                    [4.0, 2.0],
-                    [5.0, 4.0],
-                    [5.0, 5.0],
-                    [5.0, 6.0],
-                    [5.0, 9.0],
-                    [8.0, 9.0],
-                    [7.0, 9.0],
-                    [12.0, 12.0]
-                    ])
+    ob = np.array([[-1, -1],
+                   [0, 2],
+                   [4.0, 2.0],
+                   [5.0, 4.0],
+                   [5.0, 5.0],
+                   [5.0, 6.0],
+                   [5.0, 9.0],
+                   [8.0, 9.0],
+                   [7.0, 9.0],
+                   [12.0, 12.0]
+                   ])
 
     u = np.array([0.0, 0.0])
     config = Config()

PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py

@@ -106,14 +106,14 @@ def is_collision(sx, sy, gx, gy, rr, okdtree):
     nstep = round(d / D)
 
     for i in range(nstep):
-        idxs, dist = okdtree.search(np.matrix([x, y]).T)
+        idxs, dist = okdtree.search(np.array([x, y]).reshape(2, 1))
         if dist[0] <= rr:
             return True  # collision
         x += D * math.cos(yaw)
         y += D * math.sin(yaw)
 
     # goal point check
-    idxs, dist = okdtree.search(np.matrix([gx, gy]).T)
+    idxs, dist = okdtree.search(np.array([gx, gy]).reshape(2, 1))
     if dist[0] <= rr:
         return True  # collision
 
@@ -137,8 +137,8 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
     for (i, ix, iy) in zip(range(nsample), sample_x, sample_y):
 
         index, dists = skdtree.search(
-            np.matrix([ix, iy]).T, k=nsample)
-        inds = index[0][0]
+            np.array([ix, iy]).reshape(2, 1), k=nsample)
+        inds = index[0]
         edge_id = []
         #  print(index)
 
@@ -252,7 +252,7 @@ def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree):
         tx = (random.random() - minx) * (maxx - minx)
         ty = (random.random() - miny) * (maxy - miny)
 
-        index, dist = obkdtree.search(np.matrix([tx, ty]).T)
+        index, dist = obkdtree.search(np.array([tx, ty]).reshape(2, 1))
 
         if dist[0] >= rr:
             sample_x.append(tx)

PathPlanning/StateLatticePlanner/state_lattice_planner.py

@@ -54,8 +54,8 @@ def generate_path(target_states, k0):
             state[0], state[1], state[2], lookup_table)
 
         target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
-        init_p = np.matrix(
-            [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
+        init_p = np.array(
+            [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1)
 
         x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)