upgrade numpy to 1.25.0 and fix bugs (AtsushiSakai#861)

Author: AtsushiSakai

AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py

@@ -8,7 +8,6 @@
 import numpy as np
 from Quadrotor import Quadrotor
 from TrajectoryGenerator import TrajectoryGenerator
-from mpl_toolkits.mplot3d import Axes3D
 
 show_animation = True
 
@@ -128,7 +127,7 @@ def calculate_position(c, t):
     Calculates a position given a set of quintic coefficients and a time.
 
     Args
-        c: List of coefficients generated by a quintic polynomial 
+        c: List of coefficients generated by a quintic polynomial
             trajectory generator.
         t: Time at which to calculate the position
 
@@ -143,7 +142,7 @@ def calculate_velocity(c, t):
     Calculates a velocity given a set of quintic coefficients and a time.
 
     Args
-        c: List of coefficients generated by a quintic polynomial 
+        c: List of coefficients generated by a quintic polynomial
             trajectory generator.
         t: Time at which to calculate the velocity
 
@@ -158,7 +157,7 @@ def calculate_acceleration(c, t):
     Calculates an acceleration given a set of quintic coefficients and a time.
 
     Args
-        c: List of coefficients generated by a quintic polynomial 
+        c: List of coefficients generated by a quintic polynomial
             trajectory generator.
         t: Time at which to calculate the acceleration
 
@@ -168,7 +167,7 @@ def calculate_acceleration(c, t):
     return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3]
 
 
-def rotation_matrix(roll, pitch, yaw):
+def rotation_matrix(roll_array, pitch_array, yaw):
     """
     Calculates the ZYX rotation matrix.
 
@@ -180,6 +179,8 @@ def rotation_matrix(roll, pitch, yaw):
     Returns
         3x3 rotation matrix as NumPy array
     """
+    roll = roll_array[0]
+    pitch = pitch_array[0]
     return np.array(
         [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll)],
          [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
@@ -190,7 +191,7 @@ def rotation_matrix(roll, pitch, yaw):
 
 def main():
     """
-    Calculates the x, y, z coefficients for the four segments 
+    Calculates the x, y, z coefficients for the four segments
     of the trajectory
     """
     x_coeffs = [[], [], [], []]

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -14,9 +14,10 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+import math
 
 
-# Similation parameters
+# Simulation parameters
 Kp = 15
 dt = 0.01
 
@@ -50,15 +51,15 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
             else:
                 theta2_goal = np.arccos(
                     (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
-            tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+            tmp = math.atan2(l2 * np.sin(theta2_goal),
                                 (l1 + l2 * np.cos(theta2_goal)))
-            theta1_goal = np.math.atan2(y, x) - tmp
+            theta1_goal = math.atan2(y, x) - tmp
 
             if theta1_goal < 0:
                 theta2_goal = -theta2_goal
-                tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+                tmp = math.atan2(l2 * np.sin(theta2_goal),
                                     (l1 + l2 * np.cos(theta2_goal)))
-                theta1_goal = np.math.atan2(y, x) - tmp
+                theta1_goal = math.atan2(y, x) - tmp
 
             theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
             theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt

Control/inverted_pendulum/inverted_pendulum_lqr_control.py

@@ -50,14 +50,14 @@ def main():
 
         if show_animation:
             plt.clf()
-            px = float(x[0])
-            theta = float(x[2])
+            px = float(x[0, 0])
+            theta = float(x[2, 0])
             plot_cart(px, theta)
             plt.xlim([-5.0, 2.0])
             plt.pause(0.001)
 
     print("Finish")
-    print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]")
+    print(f"x={float(x[0, 0]):.2f} [m] , theta={math.degrees(x[2, 0]):.2f} [deg]")
     if show_animation:
         plt.show()
 

Control/inverted_pendulum/inverted_pendulum_mpc_control.py

@@ -55,14 +55,14 @@ def main():
 
         if show_animation:
             plt.clf()
-            px = float(x[0])
-            theta = float(x[2])
+            px = float(x[0, 0])
+            theta = float(x[2, 0])
             plot_cart(px, theta)
             plt.xlim([-5.0, 2.0])
             plt.pause(0.001)
 
     print("Finish")
-    print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]")
+    print(f"x={float(x[0, 0]):.2f} [m] , theta={math.degrees(x[2, 0]):.2f} [deg]")
     if show_animation:
         plt.show()
 

Localization/histogram_filter/histogram_filter.py

@@ -15,6 +15,7 @@
 import math
 
 import matplotlib.pyplot as plt
+import matplotlib as mpl
 import numpy as np
 from scipy.ndimage import gaussian_filter
 from scipy.stats import norm
@@ -114,7 +115,7 @@ def motion_model(x, u):
 def draw_heat_map(data, mx, my):
     max_value = max([max(i_data) for i_data in data])
     plt.grid(False)
-    plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
+    plt.pcolor(mx, my, data, vmax=max_value, cmap=mpl.colormaps["Blues"])
     plt.axis("equal")
 
 
@@ -194,7 +195,7 @@ def motion_update(grid_map, u, yaw):
     y_shift = grid_map.dy // grid_map.xy_resolution
 
     if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0:  # map should be shifted
-        grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
+        grid_map = map_shift(grid_map, int(x_shift[0]), int(y_shift[0]))
         grid_map.dx -= x_shift * grid_map.xy_resolution
         grid_map.dy -= y_shift * grid_map.xy_resolution
 

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -69,8 +69,8 @@ def motion_model(x, u):
                   [0, 0, 1.0, 0],
                   [0, 0, 0, 0]])
 
-    B = np.array([[DT * math.cos(x[2]), 0],
-                  [DT * math.sin(x[2]), 0],
+    B = np.array([[DT * math.cos(x[2, 0]), 0],
+                  [DT * math.sin(x[2, 0]), 0],
                   [0.0, DT],
                   [1.0, 0.0]])
 

Mapping/circle_fitting/circle_fitting.py

@@ -40,9 +40,9 @@ def circle_fitting(x, y):
 
     T = np.linalg.inv(F).dot(G)
 
-    cxe = float(T[0] / -2)
-    cye = float(T[1] / -2)
-    re = math.sqrt(cxe**2 + cye**2 - T[2])
+    cxe = float(T[0, 0] / -2)
+    cye = float(T[1, 0] / -2)
+    re = math.sqrt(cxe**2 + cye**2 - T[2, 0])
 
     error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
 

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -23,7 +23,7 @@
 show_animation = True
 
 
-class RTree(object):
+class RTree:
     # Class to represent the explicit tree created
     # while sampling through the state space
 
@@ -132,7 +132,7 @@ def node_id_to_real_world_coord(self, nid):
             self.node_id_to_grid_coord(nid))
 
 
-class BITStar(object):
+class BITStar:
 
     def __init__(self, start, goal,
                  obstacleList, randArea, eta=2.0,
@@ -189,7 +189,7 @@ def setup_planning(self):
                             [(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])
+        eTheta = math.atan2(a1[1, 0], a1[0, 0])
         # first column of identity matrix transposed
         id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
         M = np.dot(a1, id1_t)

PathPlanning/InformedRRTStar/informed_rrt_star.py

@@ -58,7 +58,7 @@ def informed_rrt_star_search(self, animation=True):
         a1 = np.array([[(self.goal.x - self.start.x) / c_min],
                        [(self.goal.y - self.start.y) / c_min], [0]])
 
-        e_theta = math.atan2(a1[1], a1[0])
+        e_theta = math.atan2(a1[1, 0], a1[0, 0])
         # first column of identity matrix transposed
         id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
         m = a1 @ id1_t
@@ -136,7 +136,7 @@ def choose_parent(self, new_node, near_inds):
 
     def find_near_nodes(self, new_node):
         n_node = len(self.node_list)
-        r = 50.0 * math.sqrt((math.log(n_node) / n_node))
+        r = 50.0 * math.sqrt(math.log(n_node) / n_node)
         d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for
                   node in self.node_list]
         near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]

PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py

@@ -81,7 +81,7 @@ def generate_lookup_table():
         if x is not None:
             print("find good path")
             lookup_table.append(
-                [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
+                [x[-1], y[-1], yaw[-1], float(p[0, 0]), float(p[1, 0]), float(p[2, 0])])
 
     print("finish lookup table generation")